diff --git a/COPYRIGHT.txt b/COPYRIGHT.txt index 5cc6f7971b55..0498f335cf1b 100644 --- a/COPYRIGHT.txt +++ b/COPYRIGHT.txt @@ -101,6 +101,13 @@ Copyright: 2007, Starbreeze Studios 2007-2014, Juan Linietsky, Ariel Manzur License: Expat and Zlib +Files: ./modules/jolt_physics/spaces/jolt_temp_allocator.cpp +Comment: Jolt Physics +Copyright: 2021, Jorrit Rouwe + 2014-present, Godot Engine contributors + 2007-2014, Juan Linietsky, Ariel Manzur +License: Expat + Files: ./modules/lightmapper_rd/lm_compute.glsl Comment: Joint Non-Local Means (JNLM) denoiser Copyright: 2020, Manuel Prandini @@ -289,6 +296,11 @@ Comment: International Components for Unicode Copyright: 2016-2024, Unicode, Inc. License: Unicode +Files: ./thirdparty/jolt_physics/ +Comment: Jolt Physics +Copyright: 2021, Jorrit Rouwe +License: Expat + Files: ./thirdparty/jpeg-compressor/ Comment: jpeg-compressor Copyright: 2012, Rich Geldreich diff --git a/core/os/memory.h b/core/os/memory.h index 033e417cb57f..0071c3d514fe 100644 --- a/core/os/memory.h +++ b/core/os/memory.h @@ -122,10 +122,10 @@ _ALWAYS_INLINE_ T *_post_initialize(T *p_obj) { return p_obj; } -#define memnew(m_class) _post_initialize(new ("") m_class) +#define memnew(m_class) _post_initialize(::new ("") m_class) -#define memnew_allocator(m_class, m_allocator) _post_initialize(new (m_allocator::alloc) m_class) -#define memnew_placement(m_placement, m_class) _post_initialize(new (m_placement) m_class) +#define memnew_allocator(m_class, m_allocator) _post_initialize(::new (m_allocator::alloc) m_class) +#define memnew_placement(m_placement, m_class) _post_initialize(::new (m_placement) m_class) _ALWAYS_INLINE_ bool predelete_handler(void *) { return true; @@ -189,7 +189,7 @@ T *memnew_arr_template(size_t p_elements) { /* call operator new */ for (size_t i = 0; i < p_elements; i++) { - new (&elems[i]) T; + ::new (&elems[i]) T; } } diff --git a/doc/classes/ProjectSettings.xml b/doc/classes/ProjectSettings.xml index 91961fcf02b6..85f56b9aab87 100644 --- a/doc/classes/ProjectSettings.xml +++ b/doc/classes/ProjectSettings.xml @@ -2364,6 +2364,136 @@ [b]Note:[/b] This property is only read when the project starts. To change the physics FPS at runtime, set [member Engine.physics_ticks_per_second] instead. [b]Note:[/b] Only [member physics/common/max_physics_steps_per_frame] physics ticks may be simulated per rendered frame at most. If more physics ticks have to be simulated per rendered frame to keep up with rendering, the project will appear to slow down (even if [code]delta[/code] is used consistently in physics calculations). Therefore, it is recommended to also increase [member physics/common/max_physics_steps_per_frame] if increasing [member physics/common/physics_ticks_per_second] significantly above its default value. + + The maximum angle between two triangles (in a [ConcavePolygonShape3D] or [HeightMapShape3D]) within which a convex edge will be considered active or inactive. + Collisions against an inactive edge will have its normal overridden to instead be the surface normal of the triangle. This can help alleviate ghost collisions. + [b]Note:[/b] Setting this too high can result in objects not depenetrating properly. + [b]Note:[/b] This applies to all shape queries, as well as physics bodies within the simulation. + [b]Note:[/b] This does not apply when enabling Jolt's enhanced internal edge removal, which supersedes this. + + + The amount of shape margin to use for certain convex collision shapes, such as [BoxShape3D], [CylinderShape3D] and [ConvexPolygonShape3D], as a fraction of the shape's shortest axis. + [b]Note:[/b] Shape margins in Jolt do not add any extra size to the shape, instead the shape is first shrunk by the margin and then inflated by the same amount, resulting in a shape with rounded edges. This is mainly used to speed up collision detection with convex shapes. + [b]Note:[/b] Setting this too low (e.g. 0) can also negatively affect the accuracy of the collision detection with convex shapes. + + + Which of the two nodes in a single-body joint (i.e. when omitting one of the nodes) should represent the world. This can be thought of as having the omitted node be a [StaticBody3D] at [code](0, 0, 0)[/code]. + [b]Note:[/b] Godot Physics uses [code]node_b[/code] as the default. + + + The maximum angular velocity that a [RigidBody3D] can reach. + + + The maximum number of [PhysicsBody3D] to support at the same time, awake or sleeping. + [b]Note:[/b] When this limit is exceeded a warning is emitted and anything past that point is undefined behavior. + [b]Note:[/b] This limit also applies within the editor. + + + The maximum number of body pairs to allow processing of. + [b]Note:[/b] When this limit is exceeded collisions will randomly be ignored and bodies will pass through each other. + + + The maximum number of contact constraints to allow processing of. + [b]Note:[/b] When this limit is exceeded collisions will randomly be ignored and bodies will pass through each other. + + + The maximum linear velocity that a [RigidBody3D] can reach. + + + The amount of memory to pre-allocate for the stack allocator used within Jolt. + + + How wide/deep/high a [WorldBoundaryShape3D] will be. + [b]Note:[/b] Only half of this value will be used for its height. + [b]Note:[/b] Setting this too high can negatively affect the accuracy of the collision detection. + [b]Note:[/b] Collisions against the effective edges of a [WorldBoundaryShape3D] will be inconsistent. + + + The amount of depenetration per iteration when depenetrating during motion queries. + [b]Note:[/b] This applies to [method CharacterBody3D.move_and_slide], [method PhysicsBody3D.move_and_collide] and [method PhysicsServer3D.body_test_motion]. + + + The number of iterations to run when depenetrating during motion queries. + [b]Note:[/b] This applies to [method CharacterBody3D.move_and_slide], [method PhysicsBody3D.move_and_collide] and [method PhysicsServer3D.body_test_motion]. + + + Whether to use Jolt's enhanced internal edge removal during motion queries. This can help alleviate ghost collisions. + [b]Note:[/b] This applies to [method CharacterBody3D.move_and_slide], [method PhysicsBody3D.move_and_collide] and [method PhysicsServer3D.body_test_motion]. + [b]Note:[/b] This will only remove edge collisions internal to a single body, meaning edges between separate bodies can still cause ghost collisions. + + + Whether to populate the [code]face_index[/code] field in the results of [method PhysicsDirectSpaceState3D.intersect_ray], also accessed through [method RayCast3D.get_collision_face_index]. + [b]Note:[/b] Enabling this setting will increase Jolt's memory usage for [ConcavePolygonShape3D] by around 25%. + [b]Note:[/b] The face index will be left at its default value of [code]-1[/code] when this setting is disabled. + + + Whether to use Jolt's enhanced internal edge removal during shape queries. This can help alleviate ghost collisions when using shape queries for things like character movement. + [b]Note:[/b] This applies to [method PhysicsDirectSpaceState3D.cast_motion], [method PhysicsDirectSpaceState3D.collide_shape], [method PhysicsDirectSpaceState3D.get_rest_info] and [method PhysicsDirectSpaceState3D.intersect_shape]. + [b]Note:[/b] Enabling this setting can cause certain shapes to be culled from the results entirely, but you will get at least one intersection per body. + [b]Note:[/b] This will only remove edge collisions internal to a single body, meaning edges between separate bodies can still cause ghost collisions. + + + Whether [RigidBody3D] is allowed to go to sleep. + + + Whether or not [Area3D] is able to detect overlaps with static physics bodies, such as [StaticBody3D]. + [b]Note:[/b] Enabling this setting can come at a heavy CPU and memory cost if you allow many/large [Area3D] to overlap with complex static geometry, such as [ConcavePolygonShape3D] or [HeightMapShape3D]. It is strongly recommended that you set up your collision layers and masks in such a way that only a few small [Area3D] can detect static bodies. + [b]Note:[/b] This also applies to overlaps with [RigidBody3D] frozen with [constant RigidBody3D.FREEZE_MODE_STATIC]. + [b]Note:[/b] This is not needed to detect overlaps with [AnimatableBody3D], which is a kinematic body. + + + How much of the position error of a [RigidBody3D] to fix during a physics step, where 0 is none and 1 is the full amount, which affect things like how quickly bodies depenetrate. + [b]Note:[/b] Setting this too high can result in instability for [RigidBody3D]. + + + The maximum relative angle by which a body pair can move and still reuse the collision results from the previous physics step. + + + The maximum relative distance by which a body pair can move and still reuse the collision results from the previous physics step. + + + Whether the body pair contact cache is enabled, which removes the need for potentially expensive collision detection when the relative orientation between two bodies hasn't changed much. + + + The minimum velocity needed before a collision can be elastic. + + + Fraction of its inner radius a body may penetrate another body whilst using CCD. + + + Fraction of its inner radius a body must move per step to make use of CCD. + + + Whether or not a [RigidBody3D] frozen with [constant RigidBody3D.FREEZE_MODE_KINEMATIC] is able to collide with (and thus generate contacts for) other kinematic (and static) bodies. + [b]Note:[/b] This setting can come at a heavy CPU and memory cost if you allow many/large frozen kinematic bodies with a non-zero [member RigidBody3D.max_contacts_reported] to overlap with complex static geometry, such as [ConcavePolygonShape3D] or [HeightMapShape3D]. + + + How much bodies are allowed to penetrate each other. + + + Number of solver position iterations. The greater the number of iterations, the more accurate the simulation will be, at the cost of CPU performance. + + + Time in seconds a [RigidBody3D] will spend below the sleep velocity threshold before going to sleep. + + + Velocity of certain points on the bounding box of a [RigidBody3D] below which it can be put to sleep. + + + How big the points of a [SoftBody3D] are. This can prevent things like cloth from laying perfectly flush against other surfaces and cause Z-fighting. + + + Radius around physics bodies inside which speculative contact points will be detected. + [b]Note:[/b] Setting this too high will result in ghost collisions, as speculative contacts are based on the closest points during the collision detection step which may not be the actual closest points by the time the two bodies hit. + + + Whether to use Jolt's enhanced internal edge removal for [RigidBody3D]. This can help alleviate ghost collisions when (for example) a [RigidBody3D] collides with the edges of two perfectly joined [BoxShape3D]. + [b]Note:[/b] This will only remove edge collisions internal to a single body, meaning edges between separate bodies can still cause ghost collisions. + + + Number of solver velocity iterations. The greater the number of iterations, the more accurate the simulation will be, at the cost of CPU performance. + [b]Note:[/b] This needs to be at least [code]2[/code] in order for friction to work, as friction is applied using the non-penetration impulse from the previous iteration. + Maximum number of canvas item commands that can be batched into a single draw call. diff --git a/modules/jolt_physics/SCsub b/modules/jolt_physics/SCsub new file mode 100644 index 000000000000..e5bce1f17e6d --- /dev/null +++ b/modules/jolt_physics/SCsub @@ -0,0 +1,186 @@ +#!/usr/bin/env python +from misc.utility.scons_hints import * + +Import("env") +Import("env_modules") + +env_jolt = env_modules.Clone() + +# Thirdparty source files + +thirdparty_dir = "#thirdparty/jolt_physics/" +thirdparty_sources = [ + "Jolt/RegisterTypes.cpp", + "Jolt/AABBTree/AABBTreeBuilder.cpp", + "Jolt/Core/Color.cpp", + "Jolt/Core/Factory.cpp", + "Jolt/Core/IssueReporting.cpp", + "Jolt/Core/JobSystemSingleThreaded.cpp", + "Jolt/Core/JobSystemThreadPool.cpp", + "Jolt/Core/JobSystemWithBarrier.cpp", + "Jolt/Core/LinearCurve.cpp", + "Jolt/Core/Memory.cpp", + "Jolt/Core/Profiler.cpp", + "Jolt/Core/RTTI.cpp", + "Jolt/Core/Semaphore.cpp", + "Jolt/Core/StringTools.cpp", + "Jolt/Core/TickCounter.cpp", + "Jolt/Geometry/ConvexHullBuilder.cpp", + "Jolt/Geometry/ConvexHullBuilder2D.cpp", + "Jolt/Geometry/Indexify.cpp", + "Jolt/Geometry/OrientedBox.cpp", + "Jolt/Math/Vec3.cpp", + # "Jolt/ObjectStream/ObjectStream.cpp", + # "Jolt/ObjectStream/ObjectStreamBinaryIn.cpp", + # "Jolt/ObjectStream/ObjectStreamBinaryOut.cpp", + # "Jolt/ObjectStream/ObjectStreamIn.cpp", + # "Jolt/ObjectStream/ObjectStreamOut.cpp", + # "Jolt/ObjectStream/ObjectStreamTextIn.cpp", + # "Jolt/ObjectStream/ObjectStreamTextOut.cpp", + "Jolt/ObjectStream/SerializableObject.cpp", + # "Jolt/ObjectStream/TypeDeclarations.cpp", + "Jolt/Physics/DeterminismLog.cpp", + "Jolt/Physics/IslandBuilder.cpp", + "Jolt/Physics/LargeIslandSplitter.cpp", + "Jolt/Physics/PhysicsScene.cpp", + "Jolt/Physics/PhysicsSystem.cpp", + "Jolt/Physics/PhysicsUpdateContext.cpp", + "Jolt/Physics/StateRecorderImpl.cpp", + "Jolt/Physics/Body/Body.cpp", + "Jolt/Physics/Body/BodyCreationSettings.cpp", + "Jolt/Physics/Body/BodyInterface.cpp", + "Jolt/Physics/Body/BodyManager.cpp", + "Jolt/Physics/Body/MassProperties.cpp", + "Jolt/Physics/Body/MotionProperties.cpp", + "Jolt/Physics/Character/Character.cpp", + "Jolt/Physics/Character/CharacterBase.cpp", + "Jolt/Physics/Character/CharacterVirtual.cpp", + "Jolt/Physics/Collision/CastConvexVsTriangles.cpp", + "Jolt/Physics/Collision/CastSphereVsTriangles.cpp", + "Jolt/Physics/Collision/CollideConvexVsTriangles.cpp", + "Jolt/Physics/Collision/CollideSphereVsTriangles.cpp", + "Jolt/Physics/Collision/CollisionDispatch.cpp", + "Jolt/Physics/Collision/CollisionGroup.cpp", + "Jolt/Physics/Collision/EstimateCollisionResponse.cpp", + "Jolt/Physics/Collision/GroupFilter.cpp", + "Jolt/Physics/Collision/GroupFilterTable.cpp", + "Jolt/Physics/Collision/ManifoldBetweenTwoFaces.cpp", + "Jolt/Physics/Collision/NarrowPhaseQuery.cpp", + "Jolt/Physics/Collision/NarrowPhaseStats.cpp", + "Jolt/Physics/Collision/PhysicsMaterial.cpp", + "Jolt/Physics/Collision/PhysicsMaterialSimple.cpp", + "Jolt/Physics/Collision/TransformedShape.cpp", + "Jolt/Physics/Collision/BroadPhase/BroadPhase.cpp", + "Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.cpp", + "Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp", + "Jolt/Physics/Collision/BroadPhase/QuadTree.cpp", + "Jolt/Physics/Collision/Shape/BoxShape.cpp", + "Jolt/Physics/Collision/Shape/CapsuleShape.cpp", + "Jolt/Physics/Collision/Shape/CompoundShape.cpp", + "Jolt/Physics/Collision/Shape/ConvexHullShape.cpp", + "Jolt/Physics/Collision/Shape/ConvexShape.cpp", + "Jolt/Physics/Collision/Shape/CylinderShape.cpp", + "Jolt/Physics/Collision/Shape/DecoratedShape.cpp", + "Jolt/Physics/Collision/Shape/EmptyShape.cpp", + "Jolt/Physics/Collision/Shape/HeightFieldShape.cpp", + "Jolt/Physics/Collision/Shape/MeshShape.cpp", + "Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp", + "Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp", + "Jolt/Physics/Collision/Shape/PlaneShape.cpp", + "Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp", + "Jolt/Physics/Collision/Shape/ScaledShape.cpp", + "Jolt/Physics/Collision/Shape/Shape.cpp", + "Jolt/Physics/Collision/Shape/SphereShape.cpp", + "Jolt/Physics/Collision/Shape/StaticCompoundShape.cpp", + "Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp", + "Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp", + "Jolt/Physics/Collision/Shape/TriangleShape.cpp", + "Jolt/Physics/Constraints/ConeConstraint.cpp", + "Jolt/Physics/Constraints/Constraint.cpp", + "Jolt/Physics/Constraints/ConstraintManager.cpp", + "Jolt/Physics/Constraints/ContactConstraintManager.cpp", + "Jolt/Physics/Constraints/DistanceConstraint.cpp", + "Jolt/Physics/Constraints/FixedConstraint.cpp", + "Jolt/Physics/Constraints/GearConstraint.cpp", + "Jolt/Physics/Constraints/HingeConstraint.cpp", + "Jolt/Physics/Constraints/MotorSettings.cpp", + "Jolt/Physics/Constraints/PathConstraint.cpp", + "Jolt/Physics/Constraints/PathConstraintPath.cpp", + "Jolt/Physics/Constraints/PathConstraintPathHermite.cpp", + "Jolt/Physics/Constraints/PointConstraint.cpp", + "Jolt/Physics/Constraints/PulleyConstraint.cpp", + "Jolt/Physics/Constraints/RackAndPinionConstraint.cpp", + "Jolt/Physics/Constraints/SixDOFConstraint.cpp", + "Jolt/Physics/Constraints/SliderConstraint.cpp", + "Jolt/Physics/Constraints/SpringSettings.cpp", + "Jolt/Physics/Constraints/SwingTwistConstraint.cpp", + "Jolt/Physics/Constraints/TwoBodyConstraint.cpp", + "Jolt/Physics/Ragdoll/Ragdoll.cpp", + "Jolt/Physics/SoftBody/SoftBodyCreationSettings.cpp", + "Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp", + "Jolt/Physics/SoftBody/SoftBodyShape.cpp", + "Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp", + "Jolt/Physics/Vehicle/MotorcycleController.cpp", + "Jolt/Physics/Vehicle/TrackedVehicleController.cpp", + "Jolt/Physics/Vehicle/VehicleAntiRollBar.cpp", + "Jolt/Physics/Vehicle/VehicleCollisionTester.cpp", + "Jolt/Physics/Vehicle/VehicleConstraint.cpp", + "Jolt/Physics/Vehicle/VehicleController.cpp", + "Jolt/Physics/Vehicle/VehicleDifferential.cpp", + "Jolt/Physics/Vehicle/VehicleEngine.cpp", + "Jolt/Physics/Vehicle/VehicleTrack.cpp", + "Jolt/Physics/Vehicle/VehicleTransmission.cpp", + "Jolt/Physics/Vehicle/Wheel.cpp", + "Jolt/Physics/Vehicle/WheeledVehicleController.cpp", + "Jolt/Renderer/DebugRenderer.cpp", + "Jolt/Renderer/DebugRendererPlayback.cpp", + "Jolt/Renderer/DebugRendererRecorder.cpp", + "Jolt/Renderer/DebugRendererSimple.cpp", + "Jolt/Skeleton/SkeletalAnimation.cpp", + "Jolt/Skeleton/Skeleton.cpp", + "Jolt/Skeleton/SkeletonMapper.cpp", + "Jolt/Skeleton/SkeletonPose.cpp", + "Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.cpp", + "Jolt/TriangleGrouper/TriangleGrouperMorton.cpp", + "Jolt/TriangleSplitter/TriangleSplitter.cpp", + "Jolt/TriangleSplitter/TriangleSplitterBinning.cpp", + "Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.cpp", + "Jolt/TriangleSplitter/TriangleSplitterLongestAxis.cpp", + "Jolt/TriangleSplitter/TriangleSplitterMean.cpp", + "Jolt/TriangleSplitter/TriangleSplitterMorton.cpp", +] + +thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources] + +env_jolt.Prepend(CPPPATH=[thirdparty_dir]) + +if env.dev_build: + env_jolt.Append(CPPDEFINES=["JPH_ENABLE_ASSERTS"]) + +if env.editor_build: + env_jolt.Append(CPPDEFINES=["JPH_DEBUG_RENDERER"]) + +if env["precision"] == "double": + env_jolt.Append(CPPDEFINES=["JPH_DOUBLE_PRECISION"]) + +env_thirdparty = env_jolt.Clone() +env_thirdparty.disable_warnings() + +thirdparty_obj = [] +env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) +env.modules_sources += thirdparty_obj + +# Godot source files + +module_obj = [] + +env_jolt.add_source_files(module_obj, "*.cpp") +env_jolt.add_source_files(module_obj, "joints/*.cpp") +env_jolt.add_source_files(module_obj, "misc/*.cpp") +env_jolt.add_source_files(module_obj, "objects/*.cpp") +env_jolt.add_source_files(module_obj, "shapes/*.cpp") +env_jolt.add_source_files(module_obj, "spaces/*.cpp") +env.modules_sources += module_obj + +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/jolt_physics/config.py b/modules/jolt_physics/config.py new file mode 100644 index 000000000000..a42f27fbe122 --- /dev/null +++ b/modules/jolt_physics/config.py @@ -0,0 +1,6 @@ +def can_build(env, platform): + return not env["disable_3d"] + + +def configure(env): + pass diff --git a/modules/jolt_physics/joints/jolt_cone_twist_joint_3d.cpp b/modules/jolt_physics/joints/jolt_cone_twist_joint_3d.cpp new file mode 100644 index 000000000000..950a10a81e65 --- /dev/null +++ b/modules/jolt_physics/joints/jolt_cone_twist_joint_3d.cpp @@ -0,0 +1,386 @@ +/**************************************************************************/ +/* jolt_cone_twist_joint_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_cone_twist_joint_3d.h" + +#include "../misc/jolt_type_conversions.h" +#include "../objects/jolt_body_3d.h" +#include "../spaces/jolt_space_3d.h" + +#include "core/error/error_macros.h" + +#include "Jolt/Physics/Constraints/SwingTwistConstraint.h" + +namespace { + +constexpr double DEFAULT_BIAS = 0.3; +constexpr double DEFAULT_SOFTNESS = 0.8; +constexpr double DEFAULT_RELAXATION = 1.0; + +} // namespace + +JPH::Constraint *JoltConeTwistJoint3D::_build_swing_twist(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b, float p_swing_limit_span, float p_twist_limit_span) const { + JPH::SwingTwistConstraintSettings constraint_settings; + + const bool twist_span_valid = p_twist_limit_span >= 0 && p_twist_limit_span <= JPH::JPH_PI; + const bool swing_span_valid = p_swing_limit_span >= 0 && p_swing_limit_span <= JPH::JPH_PI; + + if (twist_limit_enabled && twist_span_valid) { + constraint_settings.mTwistMinAngle = -p_twist_limit_span; + constraint_settings.mTwistMaxAngle = p_twist_limit_span; + } else { + constraint_settings.mTwistMinAngle = -JPH::JPH_PI; + constraint_settings.mTwistMaxAngle = JPH::JPH_PI; + } + + if (swing_limit_enabled && swing_span_valid) { + constraint_settings.mNormalHalfConeAngle = p_swing_limit_span; + constraint_settings.mPlaneHalfConeAngle = p_swing_limit_span; + } else { + constraint_settings.mNormalHalfConeAngle = JPH::JPH_PI; + constraint_settings.mPlaneHalfConeAngle = JPH::JPH_PI; + + if (!swing_span_valid) { + constraint_settings.mTwistMinAngle = -JPH::JPH_PI; + constraint_settings.mTwistMaxAngle = JPH::JPH_PI; + } + } + + constraint_settings.mSpace = JPH::EConstraintSpace::LocalToBodyCOM; + constraint_settings.mPosition1 = to_jolt_r(p_shifted_ref_a.origin); + constraint_settings.mTwistAxis1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_X)); + constraint_settings.mPlaneAxis1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_Z)); + constraint_settings.mPosition2 = to_jolt_r(p_shifted_ref_b.origin); + constraint_settings.mTwistAxis2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_X)); + constraint_settings.mPlaneAxis2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_Z)); + constraint_settings.mSwingType = JPH::ESwingType::Pyramid; + + if (p_jolt_body_a == nullptr) { + return constraint_settings.Create(JPH::Body::sFixedToWorld, *p_jolt_body_b); + } else if (p_jolt_body_b == nullptr) { + return constraint_settings.Create(*p_jolt_body_a, JPH::Body::sFixedToWorld); + } else { + return constraint_settings.Create(*p_jolt_body_a, *p_jolt_body_b); + } +} + +void JoltConeTwistJoint3D::_update_swing_motor_state() { + if (JPH::SwingTwistConstraint *constraint = static_cast(jolt_ref.GetPtr())) { + constraint->SetSwingMotorState(swing_motor_enabled ? JPH::EMotorState::Velocity : JPH::EMotorState::Off); + } +} + +void JoltConeTwistJoint3D::_update_twist_motor_state() { + if (JPH::SwingTwistConstraint *constraint = static_cast(jolt_ref.GetPtr())) { + constraint->SetTwistMotorState(twist_motor_enabled ? JPH::EMotorState::Velocity : JPH::EMotorState::Off); + } +} + +void JoltConeTwistJoint3D::_update_motor_velocity() { + if (JPH::SwingTwistConstraint *constraint = static_cast(jolt_ref.GetPtr())) { + // We flip the direction since Jolt is CCW but Godot is CW. + constraint->SetTargetAngularVelocityCS({ (float)-twist_motor_target_speed, (float)-swing_motor_target_speed_y, (float)-swing_motor_target_speed_z }); + } +} + +void JoltConeTwistJoint3D::_update_swing_motor_limit() { + if (JPH::SwingTwistConstraint *constraint = static_cast(jolt_ref.GetPtr())) { + JPH::MotorSettings &motor_settings = constraint->GetSwingMotorSettings(); + motor_settings.mMinTorqueLimit = (float)-swing_motor_max_torque; + motor_settings.mMaxTorqueLimit = (float)swing_motor_max_torque; + } +} + +void JoltConeTwistJoint3D::_update_twist_motor_limit() { + if (JPH::SwingTwistConstraint *constraint = static_cast(jolt_ref.GetPtr())) { + JPH::MotorSettings &motor_settings = constraint->GetTwistMotorSettings(); + motor_settings.mMinTorqueLimit = (float)-twist_motor_max_torque; + motor_settings.mMaxTorqueLimit = (float)twist_motor_max_torque; + } +} + +void JoltConeTwistJoint3D::_limits_changed() { + rebuild(); + _wake_up_bodies(); +} + +void JoltConeTwistJoint3D::_swing_motor_state_changed() { + _update_swing_motor_state(); + _wake_up_bodies(); +} + +void JoltConeTwistJoint3D::_twist_motor_state_changed() { + _update_twist_motor_state(); + _wake_up_bodies(); +} + +void JoltConeTwistJoint3D::_motor_velocity_changed() { + _update_motor_velocity(); + _wake_up_bodies(); +} + +void JoltConeTwistJoint3D::_swing_motor_limit_changed() { + _update_swing_motor_limit(); + _wake_up_bodies(); +} + +void JoltConeTwistJoint3D::_twist_motor_limit_changed() { + _update_twist_motor_limit(); + _wake_up_bodies(); +} + +JoltConeTwistJoint3D::JoltConeTwistJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Transform3D &p_local_ref_a, const Transform3D &p_local_ref_b) : + JoltJoint3D(p_old_joint, p_body_a, p_body_b, p_local_ref_a, p_local_ref_b) { + rebuild(); +} + +double JoltConeTwistJoint3D::get_param(PhysicsServer3D::ConeTwistJointParam p_param) const { + switch (p_param) { + case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: { + return swing_limit_span; + } + case PhysicsServer3D::CONE_TWIST_JOINT_TWIST_SPAN: { + return twist_limit_span; + } + case PhysicsServer3D::CONE_TWIST_JOINT_BIAS: { + return DEFAULT_BIAS; + } + case PhysicsServer3D::CONE_TWIST_JOINT_SOFTNESS: { + return DEFAULT_SOFTNESS; + } + case PhysicsServer3D::CONE_TWIST_JOINT_RELAXATION: { + return DEFAULT_RELAXATION; + } + default: { + ERR_FAIL_V_MSG(0.0, vformat("Unhandled cone twist joint parameter: '%d'. This should not happen. Please report this.", p_param)); + } + } +} + +void JoltConeTwistJoint3D::set_param(PhysicsServer3D::ConeTwistJointParam p_param, double p_value) { + switch (p_param) { + case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: { + swing_limit_span = p_value; + _limits_changed(); + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_TWIST_SPAN: { + twist_limit_span = p_value; + _limits_changed(); + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_BIAS: { + if (!Math::is_equal_approx(p_value, DEFAULT_BIAS)) { + WARN_PRINT(vformat("Cone twist joint bias is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_SOFTNESS: { + if (!Math::is_equal_approx(p_value, DEFAULT_SOFTNESS)) { + WARN_PRINT(vformat("Cone twist joint softness is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_RELAXATION: { + if (!Math::is_equal_approx(p_value, DEFAULT_RELAXATION)) { + WARN_PRINT(vformat("Cone twist joint relaxation is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + default: { + ERR_FAIL_MSG(vformat("Unhandled cone twist joint parameter: '%d'. This should not happen. Please report this.", p_param)); + } break; + } +} + +double JoltConeTwistJoint3D::get_jolt_param(JoltParameter p_param) const { + switch (p_param) { + case JoltPhysicsServer3D::CONE_TWIST_JOINT_SWING_MOTOR_TARGET_VELOCITY_Y: { + return swing_motor_target_speed_y; + } + case JoltPhysicsServer3D::CONE_TWIST_JOINT_SWING_MOTOR_TARGET_VELOCITY_Z: { + return swing_motor_target_speed_z; + } + case JoltPhysicsServer3D::CONE_TWIST_JOINT_TWIST_MOTOR_TARGET_VELOCITY: { + return twist_motor_target_speed; + } + case JoltPhysicsServer3D::CONE_TWIST_JOINT_SWING_MOTOR_MAX_TORQUE: { + return swing_motor_max_torque; + } + case JoltPhysicsServer3D::CONE_TWIST_JOINT_TWIST_MOTOR_MAX_TORQUE: { + return twist_motor_max_torque; + } + default: { + ERR_FAIL_V_MSG(0.0, vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param)); + } + } +} + +void JoltConeTwistJoint3D::set_jolt_param(JoltParameter p_param, double p_value) { + switch (p_param) { + case JoltPhysicsServer3D::CONE_TWIST_JOINT_SWING_MOTOR_TARGET_VELOCITY_Y: { + swing_motor_target_speed_y = p_value; + _motor_velocity_changed(); + } break; + case JoltPhysicsServer3D::CONE_TWIST_JOINT_SWING_MOTOR_TARGET_VELOCITY_Z: { + swing_motor_target_speed_z = p_value; + _motor_velocity_changed(); + } break; + case JoltPhysicsServer3D::CONE_TWIST_JOINT_TWIST_MOTOR_TARGET_VELOCITY: { + twist_motor_target_speed = p_value; + _motor_velocity_changed(); + } break; + case JoltPhysicsServer3D::CONE_TWIST_JOINT_SWING_MOTOR_MAX_TORQUE: { + swing_motor_max_torque = p_value; + _swing_motor_limit_changed(); + } break; + case JoltPhysicsServer3D::CONE_TWIST_JOINT_TWIST_MOTOR_MAX_TORQUE: { + twist_motor_max_torque = p_value; + _twist_motor_limit_changed(); + } break; + default: { + ERR_FAIL_MSG(vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param)); + } break; + } +} + +bool JoltConeTwistJoint3D::get_jolt_flag(JoltFlag p_flag) const { + switch (p_flag) { + case JoltPhysicsServer3D::CONE_TWIST_JOINT_FLAG_USE_SWING_LIMIT: { + return swing_limit_enabled; + } + case JoltPhysicsServer3D::CONE_TWIST_JOINT_FLAG_USE_TWIST_LIMIT: { + return twist_limit_enabled; + } + case JoltPhysicsServer3D::CONE_TWIST_JOINT_FLAG_ENABLE_SWING_MOTOR: { + return swing_motor_enabled; + } + case JoltPhysicsServer3D::CONE_TWIST_JOINT_FLAG_ENABLE_TWIST_MOTOR: { + return twist_motor_enabled; + } + default: { + ERR_FAIL_V_MSG(false, vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag)); + } + } +} + +void JoltConeTwistJoint3D::set_jolt_flag(JoltFlag p_flag, bool p_enabled) { + switch (p_flag) { + case JoltPhysicsServer3D::CONE_TWIST_JOINT_FLAG_USE_SWING_LIMIT: { + swing_limit_enabled = p_enabled; + _limits_changed(); + } break; + case JoltPhysicsServer3D::CONE_TWIST_JOINT_FLAG_USE_TWIST_LIMIT: { + twist_limit_enabled = p_enabled; + _limits_changed(); + } break; + case JoltPhysicsServer3D::CONE_TWIST_JOINT_FLAG_ENABLE_SWING_MOTOR: { + swing_motor_enabled = p_enabled; + _swing_motor_state_changed(); + } break; + case JoltPhysicsServer3D::CONE_TWIST_JOINT_FLAG_ENABLE_TWIST_MOTOR: { + twist_motor_enabled = p_enabled; + _twist_motor_state_changed(); + } break; + default: { + ERR_FAIL_MSG(vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag)); + } break; + } +} + +float JoltConeTwistJoint3D::get_applied_force() const { + JPH::SwingTwistConstraint *constraint = static_cast(jolt_ref.GetPtr()); + ERR_FAIL_NULL_V(constraint, 0.0f); + + JoltSpace3D *space = get_space(); + ERR_FAIL_NULL_V(space, 0.0f); + + const float last_step = space->get_last_step(); + if (unlikely(last_step == 0.0f)) { + return 0.0f; + } + + return constraint->GetTotalLambdaPosition().Length() / last_step; +} + +float JoltConeTwistJoint3D::get_applied_torque() const { + JPH::SwingTwistConstraint *constraint = static_cast(jolt_ref.GetPtr()); + ERR_FAIL_NULL_V(constraint, 0.0f); + + JoltSpace3D *space = get_space(); + ERR_FAIL_NULL_V(space, 0.0f); + + const float last_step = space->get_last_step(); + if (unlikely(last_step == 0.0f)) { + return 0.0f; + } + + const JPH::Vec3 swing_twist_lambda = JPH::Vec3(constraint->GetTotalLambdaTwist(), constraint->GetTotalLambdaSwingY(), constraint->GetTotalLambdaSwingZ()); + + // Note that the motor lambda is in a different space than the swing twist lambda, and since the two forces can cancel each other it is + // technically incorrect to just add them. The bodies themselves have moved, so we can't transform one into the space of the other anymore. + const float total_lambda = swing_twist_lambda.Length() + constraint->GetTotalLambdaMotor().Length(); + + return total_lambda / last_step; +} + +void JoltConeTwistJoint3D::rebuild() { + destroy(); + + JoltSpace3D *space = get_space(); + + if (space == nullptr) { + return; + } + + const JPH::BodyID body_ids[2] = { + body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(), + body_b != nullptr ? body_b->get_jolt_id() : JPH::BodyID() + }; + + const JoltWritableBodies3D jolt_bodies = space->write_bodies(body_ids, 2); + + JPH::Body *jolt_body_a = static_cast(jolt_bodies[0]); + JPH::Body *jolt_body_b = static_cast(jolt_bodies[1]); + + ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr); + + Transform3D shifted_ref_a; + Transform3D shifted_ref_b; + + _shift_reference_frames(Vector3(), Vector3(), shifted_ref_a, shifted_ref_b); + + jolt_ref = _build_swing_twist(jolt_body_a, jolt_body_b, shifted_ref_a, shifted_ref_b, (float)swing_limit_span, (float)twist_limit_span); + + space->add_joint(this); + + _update_enabled(); + _update_iterations(); + _update_swing_motor_state(); + _update_twist_motor_state(); + _update_motor_velocity(); + _update_swing_motor_limit(); + _update_twist_motor_limit(); +} diff --git a/modules/jolt_physics/joints/jolt_cone_twist_joint_3d.h b/modules/jolt_physics/joints/jolt_cone_twist_joint_3d.h new file mode 100644 index 000000000000..171e8457b088 --- /dev/null +++ b/modules/jolt_physics/joints/jolt_cone_twist_joint_3d.h @@ -0,0 +1,97 @@ +/**************************************************************************/ +/* jolt_cone_twist_joint_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_CONE_TWIST_JOINT_3D_H +#define JOLT_CONE_TWIST_JOINT_3D_H + +#include "../jolt_physics_server_3d.h" +#include "jolt_joint_3d.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/Body/Body.h" + +class JoltConeTwistJoint3D final : public JoltJoint3D { + typedef PhysicsServer3D::ConeTwistJointParam Parameter; + typedef JoltPhysicsServer3D::ConeTwistJointParamJolt JoltParameter; + typedef JoltPhysicsServer3D::ConeTwistJointFlagJolt JoltFlag; + + double swing_limit_span = 0.0; + double twist_limit_span = 0.0; + + double swing_motor_target_speed_y = 0.0; + double swing_motor_target_speed_z = 0.0; + double twist_motor_target_speed = 0.0; + + double swing_motor_max_torque = FLT_MAX; + double twist_motor_max_torque = FLT_MAX; + + bool swing_limit_enabled = true; + bool twist_limit_enabled = true; + + bool swing_motor_enabled = false; + bool twist_motor_enabled = false; + + JPH::Constraint *_build_swing_twist(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b, float p_swing_limit_span, float p_twist_limit_span) const; + + void _update_swing_motor_state(); + void _update_twist_motor_state(); + void _update_motor_velocity(); + void _update_swing_motor_limit(); + void _update_twist_motor_limit(); + + void _limits_changed(); + void _swing_motor_state_changed(); + void _twist_motor_state_changed(); + void _motor_velocity_changed(); + void _swing_motor_limit_changed(); + void _twist_motor_limit_changed(); + +public: + JoltConeTwistJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Transform3D &p_local_ref_a, const Transform3D &p_local_ref_b); + + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; } + + double get_param(PhysicsServer3D::ConeTwistJointParam p_param) const; + void set_param(PhysicsServer3D::ConeTwistJointParam p_param, double p_value); + + double get_jolt_param(JoltParameter p_param) const; + void set_jolt_param(JoltParameter p_param, double p_value); + + bool get_jolt_flag(JoltFlag p_flag) const; + void set_jolt_flag(JoltFlag p_flag, bool p_enabled); + + float get_applied_force() const; + float get_applied_torque() const; + + virtual void rebuild() override; +}; + +#endif // JOLT_CONE_TWIST_JOINT_3D_H diff --git a/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.cpp b/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.cpp new file mode 100644 index 000000000000..0fa67f182b5b --- /dev/null +++ b/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.cpp @@ -0,0 +1,696 @@ +/**************************************************************************/ +/* jolt_generic_6dof_joint_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_generic_6dof_joint_3d.h" + +#include "../misc/jolt_type_conversions.h" +#include "../objects/jolt_body_3d.h" +#include "../spaces/jolt_space_3d.h" + +#include "core/error/error_macros.h" + +#include "Jolt/Physics/Constraints/SixDOFConstraint.h" + +namespace { + +constexpr double DEFAULT_LINEAR_LIMIT_SOFTNESS = 0.7; +constexpr double DEFAULT_LINEAR_RESTITUTION = 0.5; +constexpr double DEFAULT_LINEAR_DAMPING = 1.0; + +constexpr double DEFAULT_ANGULAR_LIMIT_SOFTNESS = 0.5; +constexpr double DEFAULT_ANGULAR_DAMPING = 1.0; +constexpr double DEFAULT_ANGULAR_RESTITUTION = 0.0; +constexpr double DEFAULT_ANGULAR_FORCE_LIMIT = 0.0; +constexpr double DEFAULT_ANGULAR_ERP = 0.5; + +} // namespace + +JPH::Constraint *JoltGeneric6DOFJoint3D::_build_6dof(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b) const { + JPH::SixDOFConstraintSettings constraint_settings; + + for (int axis = 0; axis < AXIS_COUNT; ++axis) { + double lower = limit_lower[axis]; + double upper = limit_upper[axis]; + + if (axis >= AXIS_ANGULAR_X && axis <= AXIS_ANGULAR_Z) { + const double temp = lower; + lower = -upper; + upper = -temp; + } + + if (!limit_enabled[axis] || lower > upper) { + constraint_settings.MakeFreeAxis((JoltAxis)axis); + } else { + constraint_settings.SetLimitedAxis((JoltAxis)axis, (float)lower, (float)upper); + } + } + + constraint_settings.mSpace = JPH::EConstraintSpace::LocalToBodyCOM; + constraint_settings.mPosition1 = to_jolt_r(p_shifted_ref_a.origin); + constraint_settings.mAxisX1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_X)); + constraint_settings.mAxisY1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_Y)); + constraint_settings.mPosition2 = to_jolt_r(p_shifted_ref_b.origin); + constraint_settings.mAxisX2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_X)); + constraint_settings.mAxisY2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_Y)); + constraint_settings.mSwingType = JPH::ESwingType::Pyramid; + + if (p_jolt_body_a == nullptr) { + return constraint_settings.Create(JPH::Body::sFixedToWorld, *p_jolt_body_b); + } else if (p_jolt_body_b == nullptr) { + return constraint_settings.Create(*p_jolt_body_a, JPH::Body::sFixedToWorld); + } else { + return constraint_settings.Create(*p_jolt_body_a, *p_jolt_body_b); + } +} + +void JoltGeneric6DOFJoint3D::_update_limit_spring_parameters(int p_axis) { + JPH::SixDOFConstraint *constraint = static_cast(jolt_ref.GetPtr()); + if (unlikely(constraint == nullptr)) { + return; + } + + JPH::SpringSettings settings = constraint->GetLimitsSpringSettings((JoltAxis)p_axis); + + settings.mMode = JPH::ESpringMode::FrequencyAndDamping; + + if (limit_spring_enabled[p_axis]) { + settings.mFrequency = (float)limit_spring_frequency[p_axis]; + settings.mDamping = (float)limit_spring_damping[p_axis]; + } else { + settings.mFrequency = 0.0f; + settings.mDamping = 0.0f; + } + + constraint->SetLimitsSpringSettings((JoltAxis)p_axis, settings); +} + +void JoltGeneric6DOFJoint3D::_update_motor_state(int p_axis) { + if (JPH::SixDOFConstraint *constraint = static_cast(jolt_ref.GetPtr())) { + if (motor_enabled[p_axis]) { + constraint->SetMotorState((JoltAxis)p_axis, JPH::EMotorState::Velocity); + } else if (spring_enabled[p_axis]) { + constraint->SetMotorState((JoltAxis)p_axis, JPH::EMotorState::Position); + } else { + constraint->SetMotorState((JoltAxis)p_axis, JPH::EMotorState::Off); + } + } +} + +void JoltGeneric6DOFJoint3D::_update_motor_velocity(int p_axis) { + JPH::SixDOFConstraint *constraint = static_cast(jolt_ref.GetPtr()); + if (unlikely(constraint == nullptr)) { + return; + } + + if (p_axis >= AXIS_LINEAR_X && p_axis <= AXIS_LINEAR_Z) { + constraint->SetTargetVelocityCS(JPH::Vec3( + (float)motor_speed[AXIS_LINEAR_X], + (float)motor_speed[AXIS_LINEAR_Y], + (float)motor_speed[AXIS_LINEAR_Z])); + } else { + // We flip the direction since Jolt is CCW but Godot is CW. + constraint->SetTargetAngularVelocityCS(JPH::Vec3( + (float)-motor_speed[AXIS_ANGULAR_X], + (float)-motor_speed[AXIS_ANGULAR_Y], + (float)-motor_speed[AXIS_ANGULAR_Z])); + } +} + +void JoltGeneric6DOFJoint3D::_update_motor_limit(int p_axis) { + JPH::SixDOFConstraint *constraint = static_cast(jolt_ref.GetPtr()); + if (unlikely(constraint == nullptr)) { + return; + } + + JPH::MotorSettings &motor_settings = constraint->GetMotorSettings((JoltAxis)p_axis); + + float limit = FLT_MAX; + + if (motor_enabled[p_axis]) { + limit = (float)motor_limit[p_axis]; + } else if (spring_enabled[p_axis]) { + limit = (float)spring_limit[p_axis]; + } + + if (p_axis >= AXIS_LINEAR_X && p_axis <= AXIS_LINEAR_Z) { + motor_settings.SetForceLimit(limit); + } else { + motor_settings.SetTorqueLimit(limit); + } +} + +void JoltGeneric6DOFJoint3D::_update_spring_parameters(int p_axis) { + JPH::SixDOFConstraint *constraint = static_cast(jolt_ref.GetPtr()); + if (unlikely(constraint == nullptr)) { + return; + } + + JPH::MotorSettings &motor_settings = constraint->GetMotorSettings((JoltAxis)p_axis); + + if (spring_use_frequency[p_axis]) { + motor_settings.mSpringSettings.mMode = JPH::ESpringMode::FrequencyAndDamping; + motor_settings.mSpringSettings.mFrequency = (float)spring_frequency[p_axis]; + } else { + motor_settings.mSpringSettings.mMode = JPH::ESpringMode::StiffnessAndDamping; + motor_settings.mSpringSettings.mStiffness = (float)spring_stiffness[p_axis]; + } + + motor_settings.mSpringSettings.mDamping = (float)spring_damping[p_axis]; +} + +void JoltGeneric6DOFJoint3D::_update_spring_equilibrium(int p_axis) { + JPH::SixDOFConstraint *constraint = static_cast(jolt_ref.GetPtr()); + if (unlikely(constraint == nullptr)) { + return; + } + + if (p_axis >= AXIS_LINEAR_X && p_axis <= AXIS_LINEAR_Z) { + const Vector3 target_position = Vector3( + (float)spring_equilibrium[AXIS_LINEAR_X], + (float)spring_equilibrium[AXIS_LINEAR_Y], + (float)spring_equilibrium[AXIS_LINEAR_Z]); + + constraint->SetTargetPositionCS(to_jolt(target_position)); + } else { + // We flip the direction since Jolt is CCW but Godot is CW. + const Basis target_orientation = Basis::from_euler( + Vector3((float)-spring_equilibrium[AXIS_ANGULAR_X], + (float)-spring_equilibrium[AXIS_ANGULAR_Y], + (float)-spring_equilibrium[AXIS_ANGULAR_Z]), + EulerOrder::ZYX); + + constraint->SetTargetOrientationCS(to_jolt(target_orientation)); + } +} + +void JoltGeneric6DOFJoint3D::_limits_changed() { + rebuild(); + _wake_up_bodies(); +} + +void JoltGeneric6DOFJoint3D::_limit_spring_parameters_changed(int p_axis) { + _update_limit_spring_parameters(p_axis); + _wake_up_bodies(); +} + +void JoltGeneric6DOFJoint3D::_motor_state_changed(int p_axis) { + _update_motor_state(p_axis); + _update_motor_limit(p_axis); + _wake_up_bodies(); +} + +void JoltGeneric6DOFJoint3D::_motor_speed_changed(int p_axis) { + _update_motor_velocity(p_axis); + _wake_up_bodies(); +} + +void JoltGeneric6DOFJoint3D::_motor_limit_changed(int p_axis) { + _update_motor_limit(p_axis); + _wake_up_bodies(); +} + +void JoltGeneric6DOFJoint3D::_spring_state_changed(int p_axis) { + _update_motor_state(p_axis); + _wake_up_bodies(); +} + +void JoltGeneric6DOFJoint3D::_spring_parameters_changed(int p_axis) { + _update_spring_parameters(p_axis); + _wake_up_bodies(); +} + +void JoltGeneric6DOFJoint3D::_spring_equilibrium_changed(int p_axis) { + _update_spring_equilibrium(p_axis); + _wake_up_bodies(); +} + +void JoltGeneric6DOFJoint3D::_spring_limit_changed(int p_axis) { + _update_motor_limit(p_axis); + _wake_up_bodies(); +} + +JoltGeneric6DOFJoint3D::JoltGeneric6DOFJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Transform3D &p_local_ref_a, const Transform3D &p_local_ref_b) : + JoltJoint3D(p_old_joint, p_body_a, p_body_b, p_local_ref_a, p_local_ref_b) { + rebuild(); +} + +double JoltGeneric6DOFJoint3D::get_param(Axis p_axis, Param p_param) const { + const int axis_lin = AXES_LINEAR + (int)p_axis; + const int axis_ang = AXES_ANGULAR + (int)p_axis; + + switch ((int)p_param) { + case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { + return limit_lower[axis_lin]; + } + case PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT: { + return limit_upper[axis_lin]; + } + case PhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: { + return DEFAULT_LINEAR_LIMIT_SOFTNESS; + } + case PhysicsServer3D::G6DOF_JOINT_LINEAR_RESTITUTION: { + return DEFAULT_LINEAR_RESTITUTION; + } + case PhysicsServer3D::G6DOF_JOINT_LINEAR_DAMPING: { + return DEFAULT_LINEAR_DAMPING; + } + case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: { + return motor_speed[axis_lin]; + } + case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: { + return motor_limit[axis_lin]; + } + case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: { + return spring_stiffness[axis_lin]; + } + case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_DAMPING: { + return spring_damping[axis_lin]; + } + case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: { + return spring_equilibrium[axis_lin]; + } + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: { + return limit_lower[axis_ang]; + } + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: { + return limit_upper[axis_ang]; + } + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: { + return DEFAULT_ANGULAR_LIMIT_SOFTNESS; + } + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_DAMPING: { + return DEFAULT_ANGULAR_DAMPING; + } + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_RESTITUTION: { + return DEFAULT_ANGULAR_RESTITUTION; + } + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: { + return DEFAULT_ANGULAR_FORCE_LIMIT; + } + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_ERP: { + return DEFAULT_ANGULAR_ERP; + } + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: { + return motor_speed[axis_ang]; + } + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: { + return motor_limit[axis_ang]; + } + case JoltPhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: { + return spring_stiffness[axis_ang]; + } + case JoltPhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: { + return spring_damping[axis_ang]; + } + case JoltPhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: { + return spring_equilibrium[axis_ang]; + } + default: { + ERR_FAIL_V_MSG(0.0, vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param)); + } + } +} + +void JoltGeneric6DOFJoint3D::set_param(Axis p_axis, Param p_param, double p_value) { + const int axis_lin = AXES_LINEAR + (int)p_axis; + const int axis_ang = AXES_ANGULAR + (int)p_axis; + + switch ((int)p_param) { + case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { + limit_lower[axis_lin] = p_value; + _limits_changed(); + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT: { + limit_upper[axis_lin] = p_value; + _limits_changed(); + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: { + if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_LIMIT_SOFTNESS)) { + WARN_PRINT(vformat("6DOF joint linear limit softness is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_RESTITUTION: { + if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_RESTITUTION)) { + WARN_PRINT(vformat("6DOF joint linear restitution is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_DAMPING: { + if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_DAMPING)) { + WARN_PRINT(vformat("6DOF joint linear damping is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: { + motor_speed[axis_lin] = p_value; + _motor_speed_changed(axis_lin); + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: { + motor_limit[axis_lin] = p_value; + _motor_limit_changed(axis_lin); + } break; + case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: { + spring_stiffness[axis_lin] = p_value; + _spring_parameters_changed(axis_lin); + } break; + case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_DAMPING: { + spring_damping[axis_lin] = p_value; + _spring_parameters_changed(axis_lin); + } break; + case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: { + spring_equilibrium[axis_lin] = p_value; + _spring_equilibrium_changed(axis_lin); + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: { + limit_lower[axis_ang] = p_value; + _limits_changed(); + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: { + limit_upper[axis_ang] = p_value; + _limits_changed(); + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: { + if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_LIMIT_SOFTNESS)) { + WARN_PRINT(vformat("6DOF joint angular limit softness is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_DAMPING: { + if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_DAMPING)) { + WARN_PRINT(vformat("6DOF joint angular damping is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_RESTITUTION: { + if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_RESTITUTION)) { + WARN_PRINT(vformat("6DOF joint angular restitution is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: { + if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_FORCE_LIMIT)) { + WARN_PRINT(vformat("6DOF joint angular force limit is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_ERP: { + if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_ERP)) { + WARN_PRINT(vformat("6DOF joint angular ERP is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: { + motor_speed[axis_ang] = p_value; + _motor_speed_changed(axis_ang); + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: { + motor_limit[axis_ang] = p_value; + _motor_limit_changed(axis_ang); + } break; + case JoltPhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: { + spring_stiffness[axis_ang] = p_value; + _spring_parameters_changed(axis_ang); + } break; + case JoltPhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: { + spring_damping[axis_ang] = p_value; + _spring_parameters_changed(axis_ang); + } break; + case JoltPhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: { + spring_equilibrium[axis_ang] = p_value; + _spring_equilibrium_changed(axis_ang); + } break; + default: { + ERR_FAIL_MSG(vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param)); + } break; + } +} + +bool JoltGeneric6DOFJoint3D::get_flag(Axis p_axis, Flag p_flag) const { + const int axis_lin = AXES_LINEAR + (int)p_axis; + const int axis_ang = AXES_ANGULAR + (int)p_axis; + + switch ((int)p_flag) { + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: { + return limit_enabled[axis_lin]; + } + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: { + return limit_enabled[axis_ang]; + } + case JoltPhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: { + return spring_enabled[axis_ang]; + } + case JoltPhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: { + return spring_enabled[axis_lin]; + } + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR: { + return motor_enabled[axis_ang]; + } + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: { + return motor_enabled[axis_lin]; + } + default: { + ERR_FAIL_V_MSG(false, vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag)); + } + } +} + +void JoltGeneric6DOFJoint3D::set_flag(Axis p_axis, Flag p_flag, bool p_enabled) { + const int axis_lin = AXES_LINEAR + (int)p_axis; + const int axis_ang = AXES_ANGULAR + (int)p_axis; + + switch ((int)p_flag) { + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: { + limit_enabled[axis_lin] = p_enabled; + _limits_changed(); + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: { + limit_enabled[axis_ang] = p_enabled; + _limits_changed(); + } break; + case JoltPhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: { + spring_enabled[axis_ang] = p_enabled; + _spring_state_changed(axis_ang); + } break; + case JoltPhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: { + spring_enabled[axis_lin] = p_enabled; + _spring_state_changed(axis_lin); + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR: { + motor_enabled[axis_ang] = p_enabled; + _motor_state_changed(axis_ang); + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: { + motor_enabled[axis_lin] = p_enabled; + _motor_state_changed(axis_lin); + } break; + default: { + ERR_FAIL_MSG(vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag)); + } break; + } +} + +double JoltGeneric6DOFJoint3D::get_jolt_param(Axis p_axis, JoltParam p_param) const { + const int axis_lin = AXES_LINEAR + (int)p_axis; + const int axis_ang = AXES_ANGULAR + (int)p_axis; + + switch ((int)p_param) { + case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_FREQUENCY: { + return spring_frequency[axis_lin]; + } + case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_MAX_FORCE: { + return spring_limit[axis_lin]; + } + case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SPRING_FREQUENCY: { + return limit_spring_frequency[axis_lin]; + } + case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SPRING_DAMPING: { + return limit_spring_damping[axis_lin]; + } + case JoltPhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_FREQUENCY: { + return spring_frequency[axis_ang]; + } + case JoltPhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_MAX_TORQUE: { + return spring_limit[axis_ang]; + } + default: { + ERR_FAIL_V_MSG(0.0, vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param)); + } + } +} + +void JoltGeneric6DOFJoint3D::set_jolt_param(Axis p_axis, JoltParam p_param, double p_value) { + const int axis_lin = AXES_LINEAR + (int)p_axis; + const int axis_ang = AXES_ANGULAR + (int)p_axis; + + switch ((int)p_param) { + case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_FREQUENCY: { + spring_frequency[axis_lin] = p_value; + _spring_parameters_changed(axis_lin); + } break; + case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_MAX_FORCE: { + spring_limit[axis_lin] = p_value; + _spring_limit_changed(axis_lin); + } break; + case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SPRING_FREQUENCY: { + limit_spring_frequency[axis_lin] = p_value; + _limit_spring_parameters_changed(axis_lin); + } break; + case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SPRING_DAMPING: { + limit_spring_damping[axis_lin] = p_value; + _limit_spring_parameters_changed(axis_lin); + } break; + case JoltPhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_FREQUENCY: { + spring_frequency[axis_ang] = p_value; + _spring_parameters_changed(axis_ang); + } break; + case JoltPhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_MAX_TORQUE: { + spring_limit[axis_ang] = p_value; + _spring_limit_changed(axis_ang); + } break; + default: { + ERR_FAIL_MSG(vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param)); + } break; + } +} + +bool JoltGeneric6DOFJoint3D::get_jolt_flag(Axis p_axis, JoltFlag p_flag) const { + const int axis_lin = AXES_LINEAR + (int)p_axis; + const int axis_ang = AXES_ANGULAR + (int)p_axis; + + switch ((int)p_flag) { + case JoltPhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT_SPRING: { + return limit_spring_enabled[axis_lin]; + } + case JoltPhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING_FREQUENCY: { + return spring_use_frequency[axis_lin]; + } + case JoltPhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING_FREQUENCY: { + return spring_use_frequency[axis_ang]; + } + default: { + ERR_FAIL_V_MSG(false, vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag)); + } + } +} + +void JoltGeneric6DOFJoint3D::set_jolt_flag(Axis p_axis, JoltFlag p_flag, bool p_enabled) { + const int axis_lin = AXES_LINEAR + (int)p_axis; + const int axis_ang = AXES_ANGULAR + (int)p_axis; + + switch ((int)p_flag) { + case JoltPhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT_SPRING: { + limit_spring_enabled[axis_lin] = p_enabled; + _limit_spring_parameters_changed(axis_lin); + } break; + case JoltPhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING_FREQUENCY: { + spring_use_frequency[axis_lin] = p_enabled; + _spring_parameters_changed(axis_lin); + } break; + case JoltPhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING_FREQUENCY: { + spring_use_frequency[axis_ang] = p_enabled; + _spring_parameters_changed(axis_ang); + } break; + default: { + ERR_FAIL_MSG(vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag)); + } break; + } +} + +float JoltGeneric6DOFJoint3D::get_applied_force() const { + JPH::SixDOFConstraint *constraint = static_cast(jolt_ref.GetPtr()); + ERR_FAIL_NULL_V(constraint, 0.0f); + + JoltSpace3D *space = get_space(); + ERR_FAIL_NULL_V(space, 0.0f); + + const float last_step = space->get_last_step(); + if (unlikely(last_step == 0.0f)) { + return 0.0f; + } + + const JPH::Vec3 total_lambda = constraint->GetTotalLambdaPosition() + constraint->GetTotalLambdaMotorTranslation(); + + return total_lambda.Length() / last_step; +} + +float JoltGeneric6DOFJoint3D::get_applied_torque() const { + JPH::SixDOFConstraint *constraint = static_cast(jolt_ref.GetPtr()); + ERR_FAIL_NULL_V(constraint, 0.0f); + + JoltSpace3D *space = get_space(); + ERR_FAIL_NULL_V(space, 0.0f); + + const float last_step = space->get_last_step(); + if (unlikely(last_step == 0.0f)) { + return 0.0f; + } + + const JPH::Vec3 total_lambda = constraint->GetTotalLambdaRotation() + constraint->GetTotalLambdaMotorRotation(); + + return total_lambda.Length() / last_step; +} + +void JoltGeneric6DOFJoint3D::rebuild() { + destroy(); + + JoltSpace3D *space = get_space(); + if (space == nullptr) { + return; + } + + const JPH::BodyID body_ids[2] = { + body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(), + body_b != nullptr ? body_b->get_jolt_id() : JPH::BodyID() + }; + + const JoltWritableBodies3D jolt_bodies = space->write_bodies(body_ids, 2); + + JPH::Body *jolt_body_a = static_cast(jolt_bodies[0]); + JPH::Body *jolt_body_b = static_cast(jolt_bodies[1]); + + ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr); + + Transform3D shifted_ref_a; + Transform3D shifted_ref_b; + + _shift_reference_frames(Vector3(), Vector3(), shifted_ref_a, shifted_ref_b); + + jolt_ref = _build_6dof(jolt_body_a, jolt_body_b, shifted_ref_a, shifted_ref_b); + + space->add_joint(this); + + _update_enabled(); + _update_iterations(); + + _update_limit_spring_parameters(AXIS_LINEAR_X); + _update_limit_spring_parameters(AXIS_LINEAR_Y); + _update_limit_spring_parameters(AXIS_LINEAR_Z); + + for (int axis = 0; axis < AXIS_COUNT; ++axis) { + _update_motor_state(axis); + _update_motor_velocity(axis); + _update_motor_limit(axis); + _update_spring_parameters(axis); + _update_spring_equilibrium(axis); + } +} diff --git a/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.h b/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.h new file mode 100644 index 000000000000..9d7204897867 --- /dev/null +++ b/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.h @@ -0,0 +1,127 @@ +/**************************************************************************/ +/* jolt_generic_6dof_joint_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_GENERIC_6DOF_JOINT_3D_H +#define JOLT_GENERIC_6DOF_JOINT_3D_H + +#include "../jolt_physics_server_3d.h" +#include "jolt_joint_3d.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/Constraints/SixDOFConstraint.h" + +class JoltGeneric6DOFJoint3D final : public JoltJoint3D { + typedef Vector3::Axis Axis; + typedef JPH::SixDOFConstraintSettings::EAxis JoltAxis; + typedef PhysicsServer3D::G6DOFJointAxisParam Param; + typedef JoltPhysicsServer3D::G6DOFJointAxisParamJolt JoltParam; + typedef PhysicsServer3D::G6DOFJointAxisFlag Flag; + typedef JoltPhysicsServer3D::G6DOFJointAxisFlagJolt JoltFlag; + + enum { + AXIS_LINEAR_X = JoltAxis::TranslationX, + AXIS_LINEAR_Y = JoltAxis::TranslationY, + AXIS_LINEAR_Z = JoltAxis::TranslationZ, + AXIS_ANGULAR_X = JoltAxis::RotationX, + AXIS_ANGULAR_Y = JoltAxis::RotationY, + AXIS_ANGULAR_Z = JoltAxis::RotationZ, + AXIS_COUNT = JoltAxis::Num, + AXES_LINEAR = AXIS_LINEAR_X, + AXES_ANGULAR = AXIS_ANGULAR_X, + }; + + double limit_lower[AXIS_COUNT] = {}; + double limit_upper[AXIS_COUNT] = {}; + + double limit_spring_frequency[AXIS_COUNT] = {}; + double limit_spring_damping[AXIS_COUNT] = {}; + + double motor_speed[AXIS_COUNT] = {}; + double motor_limit[AXIS_COUNT] = { FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX }; + + double spring_stiffness[AXIS_COUNT] = {}; + double spring_frequency[AXIS_COUNT] = {}; + double spring_damping[AXIS_COUNT] = {}; + double spring_equilibrium[AXIS_COUNT] = {}; + double spring_limit[AXIS_COUNT] = { FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX }; + + bool limit_enabled[AXIS_COUNT] = {}; + + bool limit_spring_enabled[AXIS_COUNT] = {}; + + bool motor_enabled[AXIS_COUNT] = {}; + + bool spring_enabled[AXIS_COUNT] = {}; + bool spring_use_frequency[AXIS_COUNT] = {}; + + JPH::Constraint *_build_6dof(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b) const; + + void _update_limit_spring_parameters(int p_axis); + void _update_motor_state(int p_axis); + void _update_motor_velocity(int p_axis); + void _update_motor_limit(int p_axis); + void _update_spring_parameters(int p_axis); + void _update_spring_equilibrium(int p_axis); + + void _limits_changed(); + void _limit_spring_parameters_changed(int p_axis); + void _motor_state_changed(int p_axis); + void _motor_speed_changed(int p_axis); + void _motor_limit_changed(int p_axis); + void _spring_state_changed(int p_axis); + void _spring_parameters_changed(int p_axis); + void _spring_equilibrium_changed(int p_axis); + void _spring_limit_changed(int p_axis); + +public: + JoltGeneric6DOFJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Transform3D &p_local_ref_a, const Transform3D &p_local_ref_b); + + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_6DOF; } + + double get_param(Axis p_axis, Param p_param) const; + void set_param(Axis p_axis, Param p_param, double p_value); + + bool get_flag(Axis p_axis, Flag p_flag) const; + void set_flag(Axis p_axis, Flag p_flag, bool p_enabled); + + double get_jolt_param(Axis p_axis, JoltParam p_param) const; + void set_jolt_param(Axis p_axis, JoltParam p_param, double p_value); + + bool get_jolt_flag(Axis p_axis, JoltFlag p_flag) const; + void set_jolt_flag(Axis p_axis, JoltFlag p_flag, bool p_enabled); + + float get_applied_force() const; + float get_applied_torque() const; + + virtual void rebuild() override; +}; + +#endif // JOLT_GENERIC_6DOF_JOINT_3D_H diff --git a/modules/jolt_physics/joints/jolt_hinge_joint_3d.cpp b/modules/jolt_physics/joints/jolt_hinge_joint_3d.cpp new file mode 100644 index 000000000000..ba9360b559e5 --- /dev/null +++ b/modules/jolt_physics/joints/jolt_hinge_joint_3d.cpp @@ -0,0 +1,430 @@ +/**************************************************************************/ +/* jolt_hinge_joint_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_hinge_joint_3d.h" + +#include "../misc/jolt_type_conversions.h" +#include "../objects/jolt_body_3d.h" +#include "../spaces/jolt_space_3d.h" + +#include "core/config/engine.h" +#include "core/error/error_macros.h" + +#include "Jolt/Physics/Constraints/FixedConstraint.h" +#include "Jolt/Physics/Constraints/HingeConstraint.h" + +namespace { + +constexpr double DEFAULT_BIAS = 0.3; +constexpr double DEFAULT_LIMIT_BIAS = 0.3; +constexpr double DEFAULT_SOFTNESS = 0.9; +constexpr double DEFAULT_RELAXATION = 1.0; + +double estimate_physics_step() { + Engine *engine = Engine::get_singleton(); + + const double step = 1.0 / engine->get_physics_ticks_per_second(); + const double step_scaled = step * engine->get_time_scale(); + + return step_scaled; +} + +} // namespace + +JPH::Constraint *JoltHingeJoint3D::_build_hinge(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b, float p_limit) const { + JPH::HingeConstraintSettings constraint_settings; + + constraint_settings.mSpace = JPH::EConstraintSpace::LocalToBodyCOM; + constraint_settings.mPoint1 = to_jolt_r(p_shifted_ref_a.origin); + constraint_settings.mHingeAxis1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_Z)); + constraint_settings.mNormalAxis1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_X)); + constraint_settings.mPoint2 = to_jolt_r(p_shifted_ref_b.origin); + constraint_settings.mHingeAxis2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_Z)); + constraint_settings.mNormalAxis2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_X)); + constraint_settings.mLimitsMin = -p_limit; + constraint_settings.mLimitsMax = p_limit; + + if (limit_spring_enabled) { + constraint_settings.mLimitsSpringSettings.mFrequency = (float)limit_spring_frequency; + constraint_settings.mLimitsSpringSettings.mDamping = (float)limit_spring_damping; + } + + if (p_jolt_body_a == nullptr) { + return constraint_settings.Create(JPH::Body::sFixedToWorld, *p_jolt_body_b); + } else if (p_jolt_body_b == nullptr) { + return constraint_settings.Create(*p_jolt_body_a, JPH::Body::sFixedToWorld); + } else { + return constraint_settings.Create(*p_jolt_body_a, *p_jolt_body_b); + } +} + +JPH::Constraint *JoltHingeJoint3D::_build_fixed(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b) const { + JPH::FixedConstraintSettings constraint_settings; + + constraint_settings.mSpace = JPH::EConstraintSpace::LocalToBodyCOM; + constraint_settings.mAutoDetectPoint = false; + constraint_settings.mPoint1 = to_jolt_r(p_shifted_ref_a.origin); + constraint_settings.mAxisX1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_X)); + constraint_settings.mAxisY1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_Y)); + constraint_settings.mPoint2 = to_jolt_r(p_shifted_ref_b.origin); + constraint_settings.mAxisX2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_X)); + constraint_settings.mAxisY2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_Y)); + + if (p_jolt_body_a == nullptr) { + return constraint_settings.Create(JPH::Body::sFixedToWorld, *p_jolt_body_b); + } else if (p_jolt_body_b == nullptr) { + return constraint_settings.Create(*p_jolt_body_a, JPH::Body::sFixedToWorld); + } else { + return constraint_settings.Create(*p_jolt_body_a, *p_jolt_body_b); + } +} + +void JoltHingeJoint3D::_update_motor_state() { + if (unlikely(_is_fixed())) { + return; + } + + if (JPH::HingeConstraint *constraint = static_cast(jolt_ref.GetPtr())) { + constraint->SetMotorState(motor_enabled ? JPH::EMotorState::Velocity : JPH::EMotorState::Off); + } +} + +void JoltHingeJoint3D::_update_motor_velocity() { + if (unlikely(_is_fixed())) { + return; + } + + if (JPH::HingeConstraint *constraint = static_cast(jolt_ref.GetPtr())) { + // We flip the direction since Jolt is CCW but Godot is CW. + constraint->SetTargetAngularVelocity((float)-motor_target_speed); + } +} + +void JoltHingeJoint3D::_update_motor_limit() { + if (unlikely(_is_fixed())) { + return; + } + + if (JPH::HingeConstraint *constraint = static_cast(jolt_ref.GetPtr())) { + JPH::MotorSettings &motor_settings = constraint->GetMotorSettings(); + motor_settings.mMinTorqueLimit = (float)-motor_max_torque; + motor_settings.mMaxTorqueLimit = (float)motor_max_torque; + } +} + +void JoltHingeJoint3D::_limits_changed() { + rebuild(); + _wake_up_bodies(); +} + +void JoltHingeJoint3D::_limit_spring_changed() { + rebuild(); + _wake_up_bodies(); +} + +void JoltHingeJoint3D::_motor_state_changed() { + _update_motor_state(); + _wake_up_bodies(); +} + +void JoltHingeJoint3D::_motor_speed_changed() { + _update_motor_velocity(); + _wake_up_bodies(); +} + +void JoltHingeJoint3D::_motor_limit_changed() { + _update_motor_limit(); + _wake_up_bodies(); +} + +JoltHingeJoint3D::JoltHingeJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Transform3D &p_local_ref_a, const Transform3D &p_local_ref_b) : + JoltJoint3D(p_old_joint, p_body_a, p_body_b, p_local_ref_a, p_local_ref_b) { + rebuild(); +} + +double JoltHingeJoint3D::get_param(Parameter p_param) const { + switch (p_param) { + case PhysicsServer3D::HINGE_JOINT_BIAS: { + return DEFAULT_BIAS; + } + case PhysicsServer3D::HINGE_JOINT_LIMIT_UPPER: { + return limit_upper; + } + case PhysicsServer3D::HINGE_JOINT_LIMIT_LOWER: { + return limit_lower; + } + case PhysicsServer3D::HINGE_JOINT_LIMIT_BIAS: { + return DEFAULT_LIMIT_BIAS; + } + case PhysicsServer3D::HINGE_JOINT_LIMIT_SOFTNESS: { + return DEFAULT_SOFTNESS; + } + case PhysicsServer3D::HINGE_JOINT_LIMIT_RELAXATION: { + return DEFAULT_RELAXATION; + } + case PhysicsServer3D::HINGE_JOINT_MOTOR_TARGET_VELOCITY: { + return motor_target_speed; + } + case PhysicsServer3D::HINGE_JOINT_MOTOR_MAX_IMPULSE: { + // With Godot using max impulse instead of max torque we don't have much choice but to calculate this and hope the timestep doesn't change. + return motor_max_torque * estimate_physics_step(); + } + default: { + ERR_FAIL_V_MSG(0.0, vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param)); + } + } +} + +void JoltHingeJoint3D::set_param(Parameter p_param, double p_value) { + switch (p_param) { + case PhysicsServer3D::HINGE_JOINT_BIAS: { + if (!Math::is_equal_approx(p_value, DEFAULT_BIAS)) { + WARN_PRINT(vformat("Hinge joint bias is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::HINGE_JOINT_LIMIT_UPPER: { + limit_upper = p_value; + _limits_changed(); + } break; + case PhysicsServer3D::HINGE_JOINT_LIMIT_LOWER: { + limit_lower = p_value; + _limits_changed(); + } break; + case PhysicsServer3D::HINGE_JOINT_LIMIT_BIAS: { + if (!Math::is_equal_approx(p_value, DEFAULT_LIMIT_BIAS)) { + WARN_PRINT(vformat("Hinge joint bias limit is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::HINGE_JOINT_LIMIT_SOFTNESS: { + if (!Math::is_equal_approx(p_value, DEFAULT_SOFTNESS)) { + WARN_PRINT(vformat("Hinge joint softness is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::HINGE_JOINT_LIMIT_RELAXATION: { + if (!Math::is_equal_approx(p_value, DEFAULT_RELAXATION)) { + WARN_PRINT(vformat("Hinge joint relaxation is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::HINGE_JOINT_MOTOR_TARGET_VELOCITY: { + motor_target_speed = p_value; + _motor_speed_changed(); + } break; + case PhysicsServer3D::HINGE_JOINT_MOTOR_MAX_IMPULSE: { + // With Godot using max impulse instead of max torque we don't have much choice but to calculate this and hope the timestep doesn't change. + motor_max_torque = p_value / estimate_physics_step(); + _motor_limit_changed(); + } break; + default: { + ERR_FAIL_MSG(vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param)); + } break; + } +} + +double JoltHingeJoint3D::get_jolt_param(JoltParameter p_param) const { + switch (p_param) { + case JoltPhysicsServer3D::HINGE_JOINT_LIMIT_SPRING_FREQUENCY: { + return limit_spring_frequency; + } + case JoltPhysicsServer3D::HINGE_JOINT_LIMIT_SPRING_DAMPING: { + return limit_spring_damping; + } + case JoltPhysicsServer3D::HINGE_JOINT_MOTOR_MAX_TORQUE: { + return motor_max_torque; + } + default: { + ERR_FAIL_V_MSG(0.0, vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param)); + } + } +} + +void JoltHingeJoint3D::set_jolt_param(JoltParameter p_param, double p_value) { + switch (p_param) { + case JoltPhysicsServer3D::HINGE_JOINT_LIMIT_SPRING_FREQUENCY: { + limit_spring_frequency = p_value; + _limit_spring_changed(); + } break; + case JoltPhysicsServer3D::HINGE_JOINT_LIMIT_SPRING_DAMPING: { + limit_spring_damping = p_value; + _limit_spring_changed(); + } break; + case JoltPhysicsServer3D::HINGE_JOINT_MOTOR_MAX_TORQUE: { + motor_max_torque = p_value; + _motor_limit_changed(); + } break; + default: { + ERR_FAIL_MSG(vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param)); + } break; + } +} + +bool JoltHingeJoint3D::get_flag(Flag p_flag) const { + switch (p_flag) { + case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT: { + return limits_enabled; + } + case PhysicsServer3D::HINGE_JOINT_FLAG_ENABLE_MOTOR: { + return motor_enabled; + } + default: { + ERR_FAIL_V_MSG(false, vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag)); + } + } +} + +void JoltHingeJoint3D::set_flag(Flag p_flag, bool p_enabled) { + switch (p_flag) { + case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT: { + limits_enabled = p_enabled; + _limits_changed(); + } break; + case PhysicsServer3D::HINGE_JOINT_FLAG_ENABLE_MOTOR: { + motor_enabled = p_enabled; + _motor_state_changed(); + } break; + default: { + ERR_FAIL_MSG(vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag)); + } break; + } +} + +bool JoltHingeJoint3D::get_jolt_flag(JoltFlag p_flag) const { + switch (p_flag) { + case JoltPhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT_SPRING: { + return limit_spring_enabled; + } + default: { + ERR_FAIL_V_MSG(false, vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag)); + } + } +} + +void JoltHingeJoint3D::set_jolt_flag(JoltFlag p_flag, bool p_enabled) { + switch (p_flag) { + case JoltPhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT_SPRING: { + limit_spring_enabled = p_enabled; + _limit_spring_changed(); + } break; + default: { + ERR_FAIL_MSG(vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag)); + } break; + } +} + +float JoltHingeJoint3D::get_applied_force() const { + ERR_FAIL_NULL_V(jolt_ref, 0.0f); + + JoltSpace3D *space = get_space(); + ERR_FAIL_NULL_V(space, 0.0f); + + const float last_step = space->get_last_step(); + if (unlikely(last_step == 0.0f)) { + return 0.0f; + } + + if (_is_fixed()) { + JPH::FixedConstraint *constraint = static_cast(jolt_ref.GetPtr()); + return constraint->GetTotalLambdaPosition().Length() / last_step; + } else { + JPH::HingeConstraint *constraint = static_cast(jolt_ref.GetPtr()); + const JPH::Vec3 total_lambda = JPH::Vec3(constraint->GetTotalLambdaRotation()[0], constraint->GetTotalLambdaRotation()[1], constraint->GetTotalLambdaRotationLimits() + constraint->GetTotalLambdaMotor()); + return total_lambda.Length() / last_step; + } +} + +float JoltHingeJoint3D::get_applied_torque() const { + ERR_FAIL_NULL_V(jolt_ref, 0.0f); + + JoltSpace3D *space = get_space(); + ERR_FAIL_NULL_V(space, 0.0f); + + const float last_step = space->get_last_step(); + if (unlikely(last_step == 0.0f)) { + return 0.0f; + } + + if (_is_fixed()) { + JPH::FixedConstraint *constraint = static_cast(jolt_ref.GetPtr()); + return constraint->GetTotalLambdaRotation().Length() / last_step; + } else { + JPH::HingeConstraint *constraint = static_cast(jolt_ref.GetPtr()); + return constraint->GetTotalLambdaRotation().Length() / last_step; + } +} + +void JoltHingeJoint3D::rebuild() { + destroy(); + + JoltSpace3D *space = get_space(); + + if (space == nullptr) { + return; + } + + const JPH::BodyID body_ids[2] = { + body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(), + body_b != nullptr ? body_b->get_jolt_id() : JPH::BodyID() + }; + + const JoltWritableBodies3D jolt_bodies = space->write_bodies(body_ids, 2); + + JPH::Body *jolt_body_a = static_cast(jolt_bodies[0]); + JPH::Body *jolt_body_b = static_cast(jolt_bodies[1]); + + ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr); + + float ref_shift = 0.0f; + float limit = JPH::JPH_PI; + + if (limits_enabled && limit_lower <= limit_upper) { + const double limit_midpoint = (limit_lower + limit_upper) / 2.0f; + + ref_shift = float(-limit_midpoint); + limit = float(limit_upper - limit_midpoint); + } + + Transform3D shifted_ref_a; + Transform3D shifted_ref_b; + + _shift_reference_frames(Vector3(), Vector3(0.0f, 0.0f, ref_shift), shifted_ref_a, shifted_ref_b); + + if (_is_fixed()) { + jolt_ref = _build_fixed(jolt_body_a, jolt_body_b, shifted_ref_a, shifted_ref_b); + } else { + jolt_ref = _build_hinge(jolt_body_a, jolt_body_b, shifted_ref_a, shifted_ref_b, limit); + } + + space->add_joint(this); + + _update_enabled(); + _update_iterations(); + _update_motor_state(); + _update_motor_velocity(); + _update_motor_limit(); +} diff --git a/modules/jolt_physics/joints/jolt_hinge_joint_3d.h b/modules/jolt_physics/joints/jolt_hinge_joint_3d.h new file mode 100644 index 000000000000..05ac9c3ee7a3 --- /dev/null +++ b/modules/jolt_physics/joints/jolt_hinge_joint_3d.h @@ -0,0 +1,100 @@ +/**************************************************************************/ +/* jolt_hinge_joint_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_HINGE_JOINT_3D_H +#define JOLT_HINGE_JOINT_3D_H + +#include "../jolt_physics_server_3d.h" +#include "jolt_joint_3d.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/Constraints/SliderConstraint.h" + +class JoltHingeJoint3D final : public JoltJoint3D { + typedef PhysicsServer3D::HingeJointParam Parameter; + typedef JoltPhysicsServer3D::HingeJointParamJolt JoltParameter; + typedef PhysicsServer3D::HingeJointFlag Flag; + typedef JoltPhysicsServer3D::HingeJointFlagJolt JoltFlag; + + double limit_lower = 0.0; + double limit_upper = 0.0; + + double limit_spring_frequency = 0.0; + double limit_spring_damping = 0.0; + + double motor_target_speed = 0.0f; + double motor_max_torque = FLT_MAX; + + bool limits_enabled = false; + bool limit_spring_enabled = false; + + bool motor_enabled = false; + + JPH::Constraint *_build_hinge(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b, float p_limit) const; + JPH::Constraint *_build_fixed(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b) const; + + bool _is_sprung() const { return limit_spring_enabled && limit_spring_frequency > 0.0; } + bool _is_fixed() const { return limits_enabled && limit_lower == limit_upper && !_is_sprung(); } + + void _update_motor_state(); + void _update_motor_velocity(); + void _update_motor_limit(); + + void _limits_changed(); + void _limit_spring_changed(); + void _motor_state_changed(); + void _motor_speed_changed(); + void _motor_limit_changed(); + +public: + JoltHingeJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Transform3D &p_local_ref_a, const Transform3D &p_local_ref_b); + + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_HINGE; } + + double get_param(Parameter p_param) const; + void set_param(Parameter p_param, double p_value); + + double get_jolt_param(JoltParameter p_param) const; + void set_jolt_param(JoltParameter p_param, double p_value); + + bool get_flag(Flag p_flag) const; + void set_flag(Flag p_flag, bool p_enabled); + + bool get_jolt_flag(JoltFlag p_flag) const; + void set_jolt_flag(JoltFlag p_flag, bool p_enabled); + + float get_applied_force() const; + float get_applied_torque() const; + + virtual void rebuild() override; +}; + +#endif // JOLT_HINGE_JOINT_3D_H diff --git a/modules/jolt_physics/joints/jolt_joint_3d.cpp b/modules/jolt_physics/joints/jolt_joint_3d.cpp new file mode 100644 index 000000000000..b72ed5635d28 --- /dev/null +++ b/modules/jolt_physics/joints/jolt_joint_3d.cpp @@ -0,0 +1,237 @@ +/**************************************************************************/ +/* jolt_joint_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_joint_3d.h" + +#include "../jolt_project_settings.h" +#include "../misc/jolt_type_conversions.h" +#include "../objects/jolt_body_3d.h" +#include "../spaces/jolt_space_3d.h" + +#include "core/error/error_macros.h" + +namespace { + +constexpr int DEFAULT_SOLVER_PRIORITY = 1; + +} // namespace + +void JoltJoint3D::_shift_reference_frames(const Vector3 &p_linear_shift, const Vector3 &p_angular_shift, Transform3D &r_shifted_ref_a, Transform3D &r_shifted_ref_b) { + Vector3 origin_a = local_ref_a.origin; + Vector3 origin_b = local_ref_b.origin; + + if (body_a != nullptr) { + origin_a *= body_a->get_scale(); + origin_a -= to_godot(body_a->get_jolt_shape()->GetCenterOfMass()); + } + + if (body_b != nullptr) { + origin_b *= body_b->get_scale(); + origin_b -= to_godot(body_b->get_jolt_shape()->GetCenterOfMass()); + } + + const Basis &basis_a = local_ref_a.basis; + const Basis &basis_b = local_ref_b.basis; + + const Basis shifted_basis_a = basis_a * Basis::from_euler(p_angular_shift, EulerOrder::ZYX); + const Vector3 shifted_origin_a = origin_a - basis_a.xform(p_linear_shift); + + r_shifted_ref_a = Transform3D(shifted_basis_a, shifted_origin_a); + r_shifted_ref_b = Transform3D(basis_b, origin_b); +} + +void JoltJoint3D::_wake_up_bodies() { + if (body_a != nullptr) { + body_a->wake_up(); + } + + if (body_b != nullptr) { + body_b->wake_up(); + } +} + +void JoltJoint3D::_update_enabled() { + if (jolt_ref != nullptr) { + jolt_ref->SetEnabled(enabled); + } +} + +void JoltJoint3D::_update_iterations() { + if (jolt_ref != nullptr) { + jolt_ref->SetNumVelocityStepsOverride((JPH::uint)velocity_iterations); + jolt_ref->SetNumPositionStepsOverride((JPH::uint)position_iterations); + } +} + +void JoltJoint3D::_enabled_changed() { + _update_enabled(); + _wake_up_bodies(); +} + +void JoltJoint3D::_iterations_changed() { + _update_iterations(); + _wake_up_bodies(); +} + +String JoltJoint3D::_bodies_to_string() const { + return vformat("'%s' and '%s'", body_a != nullptr ? body_a->to_string() : "", body_b != nullptr ? body_b->to_string() : ""); +} + +JoltJoint3D::JoltJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Transform3D &p_local_ref_a, const Transform3D &p_local_ref_b) : + enabled(p_old_joint.enabled), + collision_disabled(p_old_joint.collision_disabled), + body_a(p_body_a), + body_b(p_body_b), + rid(p_old_joint.rid), + local_ref_a(p_local_ref_a), + local_ref_b(p_local_ref_b) { + if (body_a != nullptr) { + body_a->add_joint(this); + } + + if (body_b != nullptr) { + body_b->add_joint(this); + } + + if (body_b == nullptr && JoltProjectSettings::use_joint_world_node_a()) { + // The joint scene nodes will, when omitting one of the two body nodes, always pass in a + // null `body_b` to indicate it being the "world node", regardless of which body node you + // leave blank. So we need to correct for that if we wish to use the arguably more intuitive + // alternative where `body_a` is the "world node" instead. + + SWAP(body_a, body_b); + SWAP(local_ref_a, local_ref_b); + } +} + +JoltJoint3D::~JoltJoint3D() { + if (body_a != nullptr) { + body_a->remove_joint(this); + } + + if (body_b != nullptr) { + body_b->remove_joint(this); + } + + destroy(); +} + +JoltSpace3D *JoltJoint3D::get_space() const { + if (body_a != nullptr && body_b != nullptr) { + JoltSpace3D *space_a = body_a->get_space(); + JoltSpace3D *space_b = body_b->get_space(); + + if (space_a == nullptr || space_b == nullptr) { + return nullptr; + } + + ERR_FAIL_COND_V_MSG(space_a != space_b, nullptr, vformat("Joint was found to connect bodies in different physics spaces. This joint will effectively be disabled. This joint connects %s.", _bodies_to_string())); + + return space_a; + } else if (body_a != nullptr) { + return body_a->get_space(); + } else if (body_b != nullptr) { + return body_b->get_space(); + } + + return nullptr; +} + +void JoltJoint3D::set_enabled(bool p_enabled) { + if (enabled == p_enabled) { + return; + } + + enabled = p_enabled; + + _enabled_changed(); +} + +int JoltJoint3D::get_solver_priority() const { + return DEFAULT_SOLVER_PRIORITY; +} + +void JoltJoint3D::set_solver_priority(int p_priority) { + if (p_priority != DEFAULT_SOLVER_PRIORITY) { + WARN_PRINT(vformat("Joint solver priority is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } +} + +void JoltJoint3D::set_solver_velocity_iterations(int p_iterations) { + if (velocity_iterations == p_iterations) { + return; + } + + velocity_iterations = p_iterations; + + _iterations_changed(); +} + +void JoltJoint3D::set_solver_position_iterations(int p_iterations) { + if (position_iterations == p_iterations) { + return; + } + + position_iterations = p_iterations; + + _iterations_changed(); +} + +void JoltJoint3D::set_collision_disabled(bool p_disabled) { + collision_disabled = p_disabled; + + if (body_a == nullptr || body_b == nullptr) { + return; + } + + PhysicsServer3D *physics_server = PhysicsServer3D::get_singleton(); + + if (collision_disabled) { + physics_server->body_add_collision_exception(body_a->get_rid(), body_b->get_rid()); + physics_server->body_add_collision_exception(body_b->get_rid(), body_a->get_rid()); + } else { + physics_server->body_remove_collision_exception(body_a->get_rid(), body_b->get_rid()); + physics_server->body_remove_collision_exception(body_b->get_rid(), body_a->get_rid()); + } +} + +void JoltJoint3D::destroy() { + if (jolt_ref == nullptr) { + return; + } + + JoltSpace3D *space = get_space(); + + if (space != nullptr) { + space->remove_joint(this); + } + + jolt_ref = nullptr; +} diff --git a/modules/jolt_physics/joints/jolt_joint_3d.h b/modules/jolt_physics/joints/jolt_joint_3d.h new file mode 100644 index 000000000000..b3f39630afa5 --- /dev/null +++ b/modules/jolt_physics/joints/jolt_joint_3d.h @@ -0,0 +1,110 @@ +/**************************************************************************/ +/* jolt_joint_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_JOINT_3D_H +#define JOLT_JOINT_3D_H + +#include "core/math/transform_3d.h" +#include "core/string/ustring.h" +#include "core/templates/rid.h" +#include "servers/physics_server_3d.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/Constraints/Constraint.h" + +class JoltBody3D; +class JoltSpace3D; + +class JoltJoint3D { +protected: + bool enabled = true; + bool collision_disabled = false; + + int velocity_iterations = 0; + int position_iterations = 0; + + JPH::Ref jolt_ref; + + JoltBody3D *body_a = nullptr; + JoltBody3D *body_b = nullptr; + + RID rid; + + Transform3D local_ref_a; + Transform3D local_ref_b; + + void _shift_reference_frames(const Vector3 &p_linear_shift, const Vector3 &p_angular_shift, Transform3D &r_shifted_ref_a, Transform3D &r_shifted_ref_b); + + void _wake_up_bodies(); + + void _update_enabled(); + void _update_iterations(); + + void _enabled_changed(); + void _iterations_changed(); + + String _bodies_to_string() const; + +public: + JoltJoint3D() = default; + JoltJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Transform3D &p_local_ref_a, const Transform3D &p_local_ref_b); + virtual ~JoltJoint3D(); + + virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_MAX; } + + RID get_rid() const { return rid; } + void set_rid(const RID &p_rid) { rid = p_rid; } + + JoltSpace3D *get_space() const; + + JPH::Constraint *get_jolt_ref() const { return jolt_ref; } + + bool is_enabled() const { return enabled; } + void set_enabled(bool p_enabled); + + int get_solver_priority() const; + void set_solver_priority(int p_priority); + + int get_solver_velocity_iterations() const { return velocity_iterations; } + void set_solver_velocity_iterations(int p_iterations); + + int get_solver_position_iterations() const { return position_iterations; } + void set_solver_position_iterations(int p_iterations); + + bool is_collision_disabled() const { return collision_disabled; } + void set_collision_disabled(bool p_disabled); + + void destroy(); + + virtual void rebuild() {} +}; + +#endif // JOLT_JOINT_3D_H diff --git a/modules/jolt_physics/joints/jolt_pin_joint_3d.cpp b/modules/jolt_physics/joints/jolt_pin_joint_3d.cpp new file mode 100644 index 000000000000..14aee380ce33 --- /dev/null +++ b/modules/jolt_physics/joints/jolt_pin_joint_3d.cpp @@ -0,0 +1,171 @@ +/**************************************************************************/ +/* jolt_pin_joint_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_pin_joint_3d.h" + +#include "../misc/jolt_type_conversions.h" +#include "../objects/jolt_body_3d.h" +#include "../spaces/jolt_space_3d.h" + +#include "core/error/error_macros.h" + +#include "Jolt/Physics/Constraints/PointConstraint.h" + +namespace { + +constexpr double DEFAULT_BIAS = 0.3; +constexpr double DEFAULT_DAMPING = 1.0; +constexpr double DEFAULT_IMPULSE_CLAMP = 0.0; + +} // namespace + +JPH::Constraint *JoltPinJoint3D::_build_pin(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b) { + JPH::PointConstraintSettings constraint_settings; + constraint_settings.mSpace = JPH::EConstraintSpace::LocalToBodyCOM; + constraint_settings.mPoint1 = to_jolt_r(p_shifted_ref_a.origin); + constraint_settings.mPoint2 = to_jolt_r(p_shifted_ref_b.origin); + + if (p_jolt_body_a == nullptr) { + return constraint_settings.Create(JPH::Body::sFixedToWorld, *p_jolt_body_b); + } else if (p_jolt_body_b == nullptr) { + return constraint_settings.Create(*p_jolt_body_a, JPH::Body::sFixedToWorld); + } else { + return constraint_settings.Create(*p_jolt_body_a, *p_jolt_body_b); + } +} + +void JoltPinJoint3D::_points_changed() { + rebuild(); + _wake_up_bodies(); +} + +JoltPinJoint3D::JoltPinJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Vector3 &p_local_a, const Vector3 &p_local_b) : + JoltJoint3D(p_old_joint, p_body_a, p_body_b, Transform3D({}, p_local_a), Transform3D({}, p_local_b)) { + rebuild(); +} + +void JoltPinJoint3D::set_local_a(const Vector3 &p_local_a) { + local_ref_a = Transform3D({}, p_local_a); + _points_changed(); +} + +void JoltPinJoint3D::set_local_b(const Vector3 &p_local_b) { + local_ref_b = Transform3D({}, p_local_b); + _points_changed(); +} + +double JoltPinJoint3D::get_param(PhysicsServer3D::PinJointParam p_param) const { + switch (p_param) { + case PhysicsServer3D::PIN_JOINT_BIAS: { + return DEFAULT_BIAS; + } + case PhysicsServer3D::PIN_JOINT_DAMPING: { + return DEFAULT_DAMPING; + } + case PhysicsServer3D::PIN_JOINT_IMPULSE_CLAMP: { + return DEFAULT_IMPULSE_CLAMP; + } + default: { + ERR_FAIL_V_MSG(0.0, vformat("Unhandled pin joint parameter: '%d'. This should not happen. Please report this.", p_param)); + } + } +} + +void JoltPinJoint3D::set_param(PhysicsServer3D::PinJointParam p_param, double p_value) { + switch (p_param) { + case PhysicsServer3D::PIN_JOINT_BIAS: { + if (!Math::is_equal_approx(p_value, DEFAULT_BIAS)) { + WARN_PRINT(vformat("Pin joint bias is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::PIN_JOINT_DAMPING: { + if (!Math::is_equal_approx(p_value, DEFAULT_DAMPING)) { + WARN_PRINT(vformat("Pin joint damping is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::PIN_JOINT_IMPULSE_CLAMP: { + if (!Math::is_equal_approx(p_value, DEFAULT_IMPULSE_CLAMP)) { + WARN_PRINT(vformat("Pin joint impulse clamp is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + default: { + ERR_FAIL_MSG(vformat("Unhandled pin joint parameter: '%d'. This should not happen. Please report this.", p_param)); + } break; + } +} + +float JoltPinJoint3D::get_applied_force() const { + JPH::PointConstraint *constraint = static_cast(jolt_ref.GetPtr()); + ERR_FAIL_NULL_V(constraint, 0.0f); + + JoltSpace3D *space = get_space(); + ERR_FAIL_NULL_V(space, 0.0f); + + const float last_step = space->get_last_step(); + if (unlikely(last_step == 0.0f)) { + return 0.0f; + } + + return constraint->GetTotalLambdaPosition().Length() / last_step; +} + +void JoltPinJoint3D::rebuild() { + destroy(); + + JoltSpace3D *space = get_space(); + + if (space == nullptr) { + return; + } + + const JPH::BodyID body_ids[2] = { + body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(), + body_b != nullptr ? body_b->get_jolt_id() : JPH::BodyID() + }; + + const JoltWritableBodies3D jolt_bodies = space->write_bodies(body_ids, 2); + + JPH::Body *jolt_body_a = static_cast(jolt_bodies[0]); + JPH::Body *jolt_body_b = static_cast(jolt_bodies[1]); + + ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr); + + Transform3D shifted_ref_a; + Transform3D shifted_ref_b; + + _shift_reference_frames(Vector3(), Vector3(), shifted_ref_a, shifted_ref_b); + + jolt_ref = _build_pin(jolt_body_a, jolt_body_b, shifted_ref_a, shifted_ref_b); + + space->add_joint(this); + + _update_enabled(); + _update_iterations(); +} diff --git a/modules/jolt_physics/joints/jolt_pin_joint_3d.h b/modules/jolt_physics/joints/jolt_pin_joint_3d.h new file mode 100644 index 000000000000..55cee1ad2d47 --- /dev/null +++ b/modules/jolt_physics/joints/jolt_pin_joint_3d.h @@ -0,0 +1,64 @@ +/**************************************************************************/ +/* jolt_pin_joint_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_PIN_JOINT_3D_H +#define JOLT_PIN_JOINT_3D_H + +#include "jolt_joint_3d.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/Constraints/SliderConstraint.h" + +class JoltPinJoint3D final : public JoltJoint3D { + static JPH::Constraint *_build_pin(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b); + + void _points_changed(); + +public: + JoltPinJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Vector3 &p_local_a, const Vector3 &p_local_b); + + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_PIN; } + + Vector3 get_local_a() const { return local_ref_a.origin; } + void set_local_a(const Vector3 &p_local_a); + + Vector3 get_local_b() const { return local_ref_b.origin; } + void set_local_b(const Vector3 &p_local_b); + + double get_param(PhysicsServer3D::PinJointParam p_param) const; + void set_param(PhysicsServer3D::PinJointParam p_param, double p_value); + + float get_applied_force() const; + + virtual void rebuild() override; +}; + +#endif // JOLT_PIN_JOINT_3D_H diff --git a/modules/jolt_physics/joints/jolt_slider_joint_3d.cpp b/modules/jolt_physics/joints/jolt_slider_joint_3d.cpp new file mode 100644 index 000000000000..5bf119fb8e50 --- /dev/null +++ b/modules/jolt_physics/joints/jolt_slider_joint_3d.cpp @@ -0,0 +1,544 @@ +/**************************************************************************/ +/* jolt_slider_joint_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_slider_joint_3d.h" + +#include "../misc/jolt_type_conversions.h" +#include "../objects/jolt_body_3d.h" +#include "../spaces/jolt_space_3d.h" + +#include "core/error/error_macros.h" + +#include "Jolt/Physics/Constraints/FixedConstraint.h" +#include "Jolt/Physics/Constraints/SliderConstraint.h" + +namespace { + +constexpr double DEFAULT_LINEAR_LIMIT_SOFTNESS = 1.0; +constexpr double DEFAULT_LINEAR_LIMIT_RESTITUTION = 0.7; +constexpr double DEFAULT_LINEAR_LIMIT_DAMPING = 1.0; + +constexpr double DEFAULT_LINEAR_MOTION_SOFTNESS = 1.0; +constexpr double DEFAULT_LINEAR_MOTION_RESTITUTION = 0.7; +constexpr double DEFAULT_LINEAR_MOTION_DAMPING = 0.0; + +constexpr double DEFAULT_LINEAR_ORTHO_SOFTNESS = 1.0; +constexpr double DEFAULT_LINEAR_ORTHO_RESTITUTION = 0.7; +constexpr double DEFAULT_LINEAR_ORTHO_DAMPING = 1.0; + +constexpr double DEFAULT_ANGULAR_LIMIT_UPPER = 0.0; +constexpr double DEFAULT_ANGULAR_LIMIT_LOWER = 0.0; +constexpr double DEFAULT_ANGULAR_LIMIT_SOFTNESS = 1.0; +constexpr double DEFAULT_ANGULAR_LIMIT_RESTITUTION = 0.7; +constexpr double DEFAULT_ANGULAR_LIMIT_DAMPING = 0.0; + +constexpr double DEFAULT_ANGULAR_MOTION_SOFTNESS = 1.0; +constexpr double DEFAULT_ANGULAR_MOTION_RESTITUTION = 0.7; +constexpr double DEFAULT_ANGULAR_MOTION_DAMPING = 1.0; + +constexpr double DEFAULT_ANGULAR_ORTHO_SOFTNESS = 1.0; +constexpr double DEFAULT_ANGULAR_ORTHO_RESTITUTION = 0.7; +constexpr double DEFAULT_ANGULAR_ORTHO_DAMPING = 1.0; + +} // namespace + +JPH::Constraint *JoltSliderJoint3D::_build_slider(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b, float p_limit) const { + JPH::SliderConstraintSettings constraint_settings; + + constraint_settings.mSpace = JPH::EConstraintSpace::LocalToBodyCOM; + constraint_settings.mAutoDetectPoint = false; + constraint_settings.mPoint1 = to_jolt_r(p_shifted_ref_a.origin); + constraint_settings.mSliderAxis1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_X)); + constraint_settings.mNormalAxis1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_Z)); + constraint_settings.mPoint2 = to_jolt_r(p_shifted_ref_b.origin); + constraint_settings.mSliderAxis2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_X)); + constraint_settings.mNormalAxis2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_Z)); + constraint_settings.mLimitsMin = -p_limit; + constraint_settings.mLimitsMax = p_limit; + + if (limit_spring_enabled) { + constraint_settings.mLimitsSpringSettings.mFrequency = (float)limit_spring_frequency; + constraint_settings.mLimitsSpringSettings.mDamping = (float)limit_spring_damping; + } + + if (p_jolt_body_a == nullptr) { + return constraint_settings.Create(JPH::Body::sFixedToWorld, *p_jolt_body_b); + } else if (p_jolt_body_b == nullptr) { + return constraint_settings.Create(*p_jolt_body_a, JPH::Body::sFixedToWorld); + } else { + return constraint_settings.Create(*p_jolt_body_a, *p_jolt_body_b); + } +} + +JPH::Constraint *JoltSliderJoint3D::_build_fixed(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b) const { + JPH::FixedConstraintSettings constraint_settings; + + constraint_settings.mSpace = JPH::EConstraintSpace::LocalToBodyCOM; + constraint_settings.mAutoDetectPoint = false; + constraint_settings.mPoint1 = to_jolt_r(p_shifted_ref_a.origin); + constraint_settings.mAxisX1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_X)); + constraint_settings.mAxisY1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_Y)); + constraint_settings.mPoint2 = to_jolt_r(p_shifted_ref_b.origin); + constraint_settings.mAxisX2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_X)); + constraint_settings.mAxisY2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_Y)); + + if (p_jolt_body_a == nullptr) { + return constraint_settings.Create(JPH::Body::sFixedToWorld, *p_jolt_body_b); + } else if (p_jolt_body_b == nullptr) { + return constraint_settings.Create(*p_jolt_body_a, JPH::Body::sFixedToWorld); + } else { + return constraint_settings.Create(*p_jolt_body_a, *p_jolt_body_b); + } +} + +void JoltSliderJoint3D::_update_motor_state() { + if (unlikely(_is_fixed())) { + return; + } + + if (JPH::SliderConstraint *constraint = static_cast(jolt_ref.GetPtr())) { + constraint->SetMotorState(motor_enabled ? JPH::EMotorState::Velocity : JPH::EMotorState::Off); + } +} + +void JoltSliderJoint3D::_update_motor_velocity() { + if (unlikely(_is_fixed())) { + return; + } + + if (JPH::SliderConstraint *constraint = static_cast(jolt_ref.GetPtr())) { + constraint->SetTargetVelocity((float)motor_target_speed); + } +} + +void JoltSliderJoint3D::_update_motor_limit() { + if (unlikely(_is_fixed())) { + return; + } + + if (JPH::SliderConstraint *constraint = static_cast(jolt_ref.GetPtr())) { + JPH::MotorSettings &motor_settings = constraint->GetMotorSettings(); + motor_settings.mMinForceLimit = (float)-motor_max_force; + motor_settings.mMaxForceLimit = (float)motor_max_force; + } +} + +void JoltSliderJoint3D::_limits_changed() { + rebuild(); + _wake_up_bodies(); +} + +void JoltSliderJoint3D::_limit_spring_changed() { + rebuild(); + _wake_up_bodies(); +} + +void JoltSliderJoint3D::_motor_state_changed() { + _update_motor_state(); + _wake_up_bodies(); +} + +void JoltSliderJoint3D::_motor_speed_changed() { + _update_motor_velocity(); + _wake_up_bodies(); +} + +void JoltSliderJoint3D::_motor_limit_changed() { + _update_motor_limit(); + _wake_up_bodies(); +} + +JoltSliderJoint3D::JoltSliderJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Transform3D &p_local_ref_a, const Transform3D &p_local_ref_b) : + JoltJoint3D(p_old_joint, p_body_a, p_body_b, p_local_ref_a, p_local_ref_b) { + rebuild(); +} + +double JoltSliderJoint3D::get_param(PhysicsServer3D::SliderJointParam p_param) const { + switch (p_param) { + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER: { + return limit_upper; + } + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_LOWER: { + return limit_lower; + } + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: { + return DEFAULT_LINEAR_LIMIT_SOFTNESS; + } + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: { + return DEFAULT_LINEAR_LIMIT_RESTITUTION; + } + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: { + return DEFAULT_LINEAR_LIMIT_DAMPING; + } + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: { + return DEFAULT_LINEAR_MOTION_SOFTNESS; + } + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: { + return DEFAULT_LINEAR_MOTION_RESTITUTION; + } + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_DAMPING: { + return DEFAULT_LINEAR_MOTION_DAMPING; + } + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: { + return DEFAULT_LINEAR_ORTHO_SOFTNESS; + } + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: { + return DEFAULT_LINEAR_ORTHO_RESTITUTION; + } + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: { + return DEFAULT_LINEAR_ORTHO_DAMPING; + } + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: { + return DEFAULT_ANGULAR_LIMIT_UPPER; + } + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: { + return DEFAULT_ANGULAR_LIMIT_LOWER; + } + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: { + return DEFAULT_ANGULAR_LIMIT_SOFTNESS; + } + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: { + return DEFAULT_ANGULAR_LIMIT_RESTITUTION; + } + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: { + return DEFAULT_ANGULAR_LIMIT_DAMPING; + } + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: { + return DEFAULT_ANGULAR_MOTION_SOFTNESS; + } + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: { + return DEFAULT_ANGULAR_MOTION_RESTITUTION; + } + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: { + return DEFAULT_ANGULAR_MOTION_DAMPING; + } + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: { + return DEFAULT_ANGULAR_ORTHO_SOFTNESS; + } + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: { + return DEFAULT_ANGULAR_ORTHO_RESTITUTION; + } + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: { + return DEFAULT_ANGULAR_ORTHO_DAMPING; + } + default: { + ERR_FAIL_V_MSG(0.0, vformat("Unhandled slider joint parameter: '%d'. This should not happen. Please report this.", p_param)); + } + } +} + +void JoltSliderJoint3D::set_param(PhysicsServer3D::SliderJointParam p_param, double p_value) { + switch (p_param) { + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER: { + limit_upper = p_value; + _limits_changed(); + } break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_LOWER: { + limit_lower = p_value; + _limits_changed(); + } break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: { + if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_LIMIT_SOFTNESS)) { + WARN_PRINT(vformat("Slider joint linear limit softness is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: { + if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_LIMIT_RESTITUTION)) { + WARN_PRINT(vformat("Slider joint linear limit restitution is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: { + if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_LIMIT_DAMPING)) { + WARN_PRINT(vformat("Slider joint linear limit damping is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: { + if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_MOTION_SOFTNESS)) { + WARN_PRINT(vformat("Slider joint linear motion softness is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: { + if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_MOTION_RESTITUTION)) { + WARN_PRINT(vformat("Slider joint linear motion restitution is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_DAMPING: { + if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_MOTION_DAMPING)) { + WARN_PRINT(vformat("Slider joint linear motion damping is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: { + if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_ORTHO_SOFTNESS)) { + WARN_PRINT(vformat("Slider joint linear ortho softness is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: { + if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_ORTHO_RESTITUTION)) { + WARN_PRINT(vformat("Slider joint linear ortho restitution is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: { + if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_ORTHO_DAMPING)) { + WARN_PRINT(vformat("Slider joint linear ortho damping is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: { + if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_LIMIT_UPPER)) { + WARN_PRINT(vformat("Slider joint angular limits are not supported when using Jolt Physics. Any such value will be ignored. Try using Generic6DOFJoint3D instead. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: { + if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_LIMIT_LOWER)) { + WARN_PRINT(vformat("Slider joint angular limits are not supported when using Jolt Physics. Any such value will be ignored. Try using Generic6DOFJoint3D instead. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: { + if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_LIMIT_SOFTNESS)) { + WARN_PRINT(vformat("Slider joint angular limit softness is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: { + if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_LIMIT_RESTITUTION)) { + WARN_PRINT(vformat("Slider joint angular limit restitution is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: { + if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_LIMIT_DAMPING)) { + WARN_PRINT(vformat("Slider joint angular limit damping is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: { + if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_MOTION_SOFTNESS)) { + WARN_PRINT(vformat("Slider joint angular motion softness is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: { + if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_MOTION_RESTITUTION)) { + WARN_PRINT(vformat("Slider joint angular motion restitution is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: { + if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_MOTION_DAMPING)) { + WARN_PRINT(vformat("Slider joint angular motion damping is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: { + if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_ORTHO_SOFTNESS)) { + WARN_PRINT(vformat("Slider joint angular ortho softness is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: { + if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_ORTHO_RESTITUTION)) { + WARN_PRINT(vformat("Slider joint angular ortho restitution is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: { + if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_ORTHO_DAMPING)) { + WARN_PRINT(vformat("Slider joint angular ortho damping is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string())); + } + } break; + default: { + ERR_FAIL_MSG(vformat("Unhandled slider joint parameter: '%d'. This should not happen. Please report this.", p_param)); + } break; + } +} + +double JoltSliderJoint3D::get_jolt_param(JoltParameter p_param) const { + switch (p_param) { + case JoltPhysicsServer3D::SLIDER_JOINT_LIMIT_SPRING_FREQUENCY: { + return limit_spring_frequency; + } + case JoltPhysicsServer3D::SLIDER_JOINT_LIMIT_SPRING_DAMPING: { + return limit_spring_damping; + } + case JoltPhysicsServer3D::SLIDER_JOINT_MOTOR_TARGET_VELOCITY: { + return motor_target_speed; + } + case JoltPhysicsServer3D::SLIDER_JOINT_MOTOR_MAX_FORCE: { + return motor_max_force; + } + default: { + ERR_FAIL_V_MSG(0.0, vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param)); + } + } +} + +void JoltSliderJoint3D::set_jolt_param(JoltParameter p_param, double p_value) { + switch (p_param) { + case JoltPhysicsServer3D::SLIDER_JOINT_LIMIT_SPRING_FREQUENCY: { + limit_spring_frequency = p_value; + _limit_spring_changed(); + } break; + case JoltPhysicsServer3D::SLIDER_JOINT_LIMIT_SPRING_DAMPING: { + limit_spring_damping = p_value; + _limit_spring_changed(); + } break; + case JoltPhysicsServer3D::SLIDER_JOINT_MOTOR_TARGET_VELOCITY: { + motor_target_speed = p_value; + _motor_speed_changed(); + } break; + case JoltPhysicsServer3D::SLIDER_JOINT_MOTOR_MAX_FORCE: { + motor_max_force = p_value; + _motor_limit_changed(); + } break; + default: { + ERR_FAIL_MSG(vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param)); + } break; + } +} + +bool JoltSliderJoint3D::get_jolt_flag(JoltFlag p_flag) const { + switch (p_flag) { + case JoltPhysicsServer3D::SLIDER_JOINT_FLAG_USE_LIMIT: { + return limits_enabled; + } + case JoltPhysicsServer3D::SLIDER_JOINT_FLAG_USE_LIMIT_SPRING: { + return limit_spring_enabled; + } + case JoltPhysicsServer3D::SLIDER_JOINT_FLAG_ENABLE_MOTOR: { + return motor_enabled; + } + default: { + ERR_FAIL_V_MSG(false, vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag)); + } + } +} + +void JoltSliderJoint3D::set_jolt_flag(JoltFlag p_flag, bool p_enabled) { + switch (p_flag) { + case JoltPhysicsServer3D::SLIDER_JOINT_FLAG_USE_LIMIT: { + limits_enabled = p_enabled; + _limits_changed(); + } break; + case JoltPhysicsServer3D::SLIDER_JOINT_FLAG_USE_LIMIT_SPRING: { + limit_spring_enabled = p_enabled; + _limit_spring_changed(); + } break; + case JoltPhysicsServer3D::SLIDER_JOINT_FLAG_ENABLE_MOTOR: { + motor_enabled = p_enabled; + _motor_state_changed(); + } break; + default: { + ERR_FAIL_MSG(vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag)); + } break; + } +} + +float JoltSliderJoint3D::get_applied_force() const { + ERR_FAIL_NULL_V(jolt_ref, 0.0f); + + JoltSpace3D *space = get_space(); + ERR_FAIL_NULL_V(space, 0.0f); + + const float last_step = space->get_last_step(); + if (unlikely(last_step == 0.0f)) { + return 0.0f; + } + + if (_is_fixed()) { + JPH::FixedConstraint *constraint = static_cast(jolt_ref.GetPtr()); + return constraint->GetTotalLambdaPosition().Length() / last_step; + } else { + JPH::SliderConstraint *constraint = static_cast(jolt_ref.GetPtr()); + const JPH::Vec3 total_lambda = JPH::Vec3(constraint->GetTotalLambdaPosition()[0], constraint->GetTotalLambdaPosition()[1], constraint->GetTotalLambdaPositionLimits() + constraint->GetTotalLambdaMotor()); + return total_lambda.Length() / last_step; + } +} + +float JoltSliderJoint3D::get_applied_torque() const { + ERR_FAIL_NULL_V(jolt_ref, 0.0f); + + JoltSpace3D *space = get_space(); + ERR_FAIL_NULL_V(space, 0.0f); + + const float last_step = space->get_last_step(); + if (unlikely(last_step == 0.0f)) { + return 0.0f; + } + + if (_is_fixed()) { + JPH::FixedConstraint *constraint = static_cast(jolt_ref.GetPtr()); + return constraint->GetTotalLambdaRotation().Length() / last_step; + } else { + JPH::SliderConstraint *constraint = static_cast(jolt_ref.GetPtr()); + return constraint->GetTotalLambdaRotation().Length() / last_step; + } +} + +void JoltSliderJoint3D::rebuild() { + destroy(); + + JoltSpace3D *space = get_space(); + + if (space == nullptr) { + return; + } + + const JPH::BodyID body_ids[2] = { + body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(), + body_b != nullptr ? body_b->get_jolt_id() : JPH::BodyID() + }; + + const JoltWritableBodies3D jolt_bodies = space->write_bodies(body_ids, 2); + + JPH::Body *jolt_body_a = static_cast(jolt_bodies[0]); + JPH::Body *jolt_body_b = static_cast(jolt_bodies[1]); + + ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr); + + float ref_shift = 0.0f; + float limit = FLT_MAX; + + if (limits_enabled && limit_lower <= limit_upper) { + const double limit_midpoint = (limit_lower + limit_upper) / 2.0f; + + ref_shift = float(-limit_midpoint); + limit = float(limit_upper - limit_midpoint); + } + + Transform3D shifted_ref_a; + Transform3D shifted_ref_b; + + _shift_reference_frames(Vector3(ref_shift, 0.0f, 0.0f), Vector3(), shifted_ref_a, shifted_ref_b); + + if (_is_fixed()) { + jolt_ref = _build_fixed(jolt_body_a, jolt_body_b, shifted_ref_a, shifted_ref_b); + } else { + jolt_ref = _build_slider(jolt_body_a, jolt_body_b, shifted_ref_a, shifted_ref_b, limit); + } + + space->add_joint(this); + + _update_enabled(); + _update_iterations(); + _update_motor_state(); + _update_motor_velocity(); + _update_motor_limit(); +} diff --git a/modules/jolt_physics/joints/jolt_slider_joint_3d.h b/modules/jolt_physics/joints/jolt_slider_joint_3d.h new file mode 100644 index 000000000000..31ed1a4c3a30 --- /dev/null +++ b/modules/jolt_physics/joints/jolt_slider_joint_3d.h @@ -0,0 +1,96 @@ +/**************************************************************************/ +/* jolt_slider_joint_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_SLIDER_JOINT_3D_H +#define JOLT_SLIDER_JOINT_3D_H + +#include "../jolt_physics_server_3d.h" +#include "jolt_joint_3d.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/Constraints/SliderConstraint.h" + +class JoltSliderJoint3D final : public JoltJoint3D { + typedef PhysicsServer3D::SliderJointParam Parameter; + typedef JoltPhysicsServer3D::SliderJointParamJolt JoltParameter; + typedef JoltPhysicsServer3D::SliderJointFlagJolt JoltFlag; + + double limit_upper = 0.0; + double limit_lower = 0.0; + + double limit_spring_frequency = 0.0; + double limit_spring_damping = 0.0; + + double motor_target_speed = 0.0f; + double motor_max_force = FLT_MAX; + + bool limits_enabled = true; + bool limit_spring_enabled = false; + + bool motor_enabled = false; + + JPH::Constraint *_build_slider(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b, float p_limit) const; + JPH::Constraint *_build_fixed(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b) const; + + bool _is_sprung() const { return limit_spring_enabled && limit_spring_frequency > 0.0; } + bool _is_fixed() const { return limits_enabled && limit_lower == limit_upper && !_is_sprung(); } + + void _update_motor_state(); + void _update_motor_velocity(); + void _update_motor_limit(); + + void _limits_changed(); + void _limit_spring_changed(); + void _motor_state_changed(); + void _motor_speed_changed(); + void _motor_limit_changed(); + +public: + JoltSliderJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Transform3D &p_local_ref_a, const Transform3D &p_local_ref_b); + + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_SLIDER; } + + double get_param(PhysicsServer3D::SliderJointParam p_param) const; + void set_param(PhysicsServer3D::SliderJointParam p_param, double p_value); + + double get_jolt_param(JoltParameter p_param) const; + void set_jolt_param(JoltParameter p_param, double p_value); + + bool get_jolt_flag(JoltFlag p_flag) const; + void set_jolt_flag(JoltFlag p_flag, bool p_enabled); + + float get_applied_force() const; + float get_applied_torque() const; + + virtual void rebuild() override; +}; + +#endif // JOLT_SLIDER_JOINT_3D_H diff --git a/modules/jolt_physics/jolt_globals.cpp b/modules/jolt_physics/jolt_globals.cpp new file mode 100644 index 000000000000..670042c9d03a --- /dev/null +++ b/modules/jolt_physics/jolt_globals.cpp @@ -0,0 +1,122 @@ +/**************************************************************************/ +/* jolt_globals.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_globals.h" + +#include "objects/jolt_group_filter.h" +#include "shapes/jolt_custom_double_sided_shape.h" +#include "shapes/jolt_custom_ray_shape.h" +#include "shapes/jolt_custom_user_data_shape.h" + +#include "core/os/memory.h" +#include "core/string/print_string.h" +#include "core/variant/variant.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/RegisterTypes.h" + +#include + +void *jolt_alloc(size_t p_size) { + return Memory::alloc_static(p_size); +} + +void *jolt_realloc(void *p_mem, size_t p_old_size, size_t p_new_size) { + return Memory::realloc_static(p_mem, p_new_size); +} + +void jolt_free(void *p_mem) { + Memory::free_static(p_mem); +} + +void *jolt_aligned_alloc(size_t p_size, size_t p_alignment) { + return Memory::alloc_aligned_static(p_size, p_alignment); +} + +void jolt_aligned_free(void *p_mem) { + Memory::free_aligned_static(p_mem); +} + +#ifdef JPH_ENABLE_ASSERTS + +void jolt_trace(const char *p_format, ...) { + va_list args; + va_start(args, p_format); + char buffer[1024] = { '\0' }; + vsnprintf(buffer, sizeof(buffer), p_format, args); + va_end(args); + print_verbose(buffer); +} + +bool jolt_assert(const char *p_expr, const char *p_msg, const char *p_file, uint32_t p_line) { + ERR_PRINT(vformat("Jolt Physics assertion '%s' failed with message '%s' at '%s:%d'", p_expr, p_msg != nullptr ? p_msg : "", p_file, p_line)); + return false; +} + +#endif + +void jolt_initialize() { + JPH::Allocate = &jolt_alloc; + JPH::Reallocate = &jolt_realloc; + JPH::Free = &jolt_free; + JPH::AlignedAllocate = &jolt_aligned_alloc; + JPH::AlignedFree = &jolt_aligned_free; + +#ifdef JPH_ENABLE_ASSERTS + JPH::Trace = &jolt_trace; + JPH::AssertFailed = &jolt_assert; +#endif + + JPH::Factory::sInstance = new JPH::Factory(); + + JPH::RegisterTypes(); + + JoltCustomRayShape::register_type(); + JoltCustomUserDataShape::register_type(); + JoltCustomDoubleSidedShape::register_type(); + + JoltGroupFilter::instance = new JoltGroupFilter(); + JoltGroupFilter::instance->SetEmbedded(); +} + +void jolt_deinitialize() { + if (JoltGroupFilter::instance != nullptr) { + delete JoltGroupFilter::instance; + JoltGroupFilter::instance = nullptr; + } + + JPH::UnregisterTypes(); + + if (JPH::Factory::sInstance != nullptr) { + delete JPH::Factory::sInstance; + JPH::Factory::sInstance = nullptr; + } +} diff --git a/modules/jolt_physics/jolt_globals.h b/modules/jolt_physics/jolt_globals.h new file mode 100644 index 000000000000..fd3c7708f019 --- /dev/null +++ b/modules/jolt_physics/jolt_globals.h @@ -0,0 +1,37 @@ +/**************************************************************************/ +/* jolt_globals.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_GLOBALS_H +#define JOLT_GLOBALS_H + +void jolt_initialize(); +void jolt_deinitialize(); + +#endif // JOLT_GLOBALS_H diff --git a/modules/jolt_physics/jolt_physics_server_3d.cpp b/modules/jolt_physics/jolt_physics_server_3d.cpp new file mode 100644 index 000000000000..234623a6bed5 --- /dev/null +++ b/modules/jolt_physics/jolt_physics_server_3d.cpp @@ -0,0 +1,1979 @@ +/**************************************************************************/ +/* jolt_physics_server_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_physics_server_3d.h" + +#include "joints/jolt_cone_twist_joint_3d.h" +#include "joints/jolt_generic_6dof_joint_3d.h" +#include "joints/jolt_hinge_joint_3d.h" +#include "joints/jolt_joint_3d.h" +#include "joints/jolt_pin_joint_3d.h" +#include "joints/jolt_slider_joint_3d.h" +#include "objects/jolt_area_3d.h" +#include "objects/jolt_body_3d.h" +#include "objects/jolt_soft_body_3d.h" +#include "servers/physics_server_3d_wrap_mt.h" +#include "shapes/jolt_box_shape_3d.h" +#include "shapes/jolt_capsule_shape_3d.h" +#include "shapes/jolt_concave_polygon_shape_3d.h" +#include "shapes/jolt_convex_polygon_shape_3d.h" +#include "shapes/jolt_cylinder_shape_3d.h" +#include "shapes/jolt_height_map_shape_3d.h" +#include "shapes/jolt_separation_ray_shape_3d.h" +#include "shapes/jolt_sphere_shape_3d.h" +#include "shapes/jolt_world_boundary_shape_3d.h" +#include "spaces/jolt_job_system.h" +#include "spaces/jolt_physics_direct_space_state_3d.h" +#include "spaces/jolt_space_3d.h" + +#include "core/error/error_macros.h" + +JoltPhysicsServer3D::JoltPhysicsServer3D(bool p_on_separate_thread) : + on_separate_thread(p_on_separate_thread) { + singleton = this; +} + +JoltPhysicsServer3D::~JoltPhysicsServer3D() { + if (singleton == this) { + singleton = nullptr; + } +} + +RID JoltPhysicsServer3D::world_boundary_shape_create() { + JoltShape3D *shape = memnew(JoltWorldBoundaryShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_rid(rid); + return rid; +} + +RID JoltPhysicsServer3D::separation_ray_shape_create() { + JoltShape3D *shape = memnew(JoltSeparationRayShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_rid(rid); + return rid; +} + +RID JoltPhysicsServer3D::sphere_shape_create() { + JoltShape3D *shape = memnew(JoltSphereShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_rid(rid); + return rid; +} + +RID JoltPhysicsServer3D::box_shape_create() { + JoltShape3D *shape = memnew(JoltBoxShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_rid(rid); + return rid; +} + +RID JoltPhysicsServer3D::capsule_shape_create() { + JoltShape3D *shape = memnew(JoltCapsuleShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_rid(rid); + return rid; +} + +RID JoltPhysicsServer3D::cylinder_shape_create() { + JoltShape3D *shape = memnew(JoltCylinderShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_rid(rid); + return rid; +} + +RID JoltPhysicsServer3D::convex_polygon_shape_create() { + JoltShape3D *shape = memnew(JoltConvexPolygonShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_rid(rid); + return rid; +} + +RID JoltPhysicsServer3D::concave_polygon_shape_create() { + JoltShape3D *shape = memnew(JoltConcavePolygonShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_rid(rid); + return rid; +} + +RID JoltPhysicsServer3D::heightmap_shape_create() { + JoltShape3D *shape = memnew(JoltHeightMapShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_rid(rid); + return rid; +} + +RID JoltPhysicsServer3D::custom_shape_create() { + ERR_FAIL_V_MSG(RID(), "Custom shapes are not supported."); +} + +void JoltPhysicsServer3D::shape_set_data(RID p_shape, const Variant &p_data) { + JoltShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL(shape); + + shape->set_data(p_data); +} + +Variant JoltPhysicsServer3D::shape_get_data(RID p_shape) const { + const JoltShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL_V(shape, Variant()); + + return shape->get_data(); +} + +void JoltPhysicsServer3D::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) { + JoltShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL(shape); + + shape->set_solver_bias((float)p_bias); +} + +PhysicsServer3D::ShapeType JoltPhysicsServer3D::shape_get_type(RID p_shape) const { + const JoltShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL_V(shape, SHAPE_CUSTOM); + + return shape->get_type(); +} + +void JoltPhysicsServer3D::shape_set_margin(RID p_shape, real_t p_margin) { + JoltShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL(shape); + + shape->set_margin((float)p_margin); +} + +real_t JoltPhysicsServer3D::shape_get_margin(RID p_shape) const { + const JoltShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL_V(shape, 0.0); + + return (real_t)shape->get_margin(); +} + +real_t JoltPhysicsServer3D::shape_get_custom_solver_bias(RID p_shape) const { + const JoltShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL_V(shape, 0.0); + + return (real_t)shape->get_solver_bias(); +} + +RID JoltPhysicsServer3D::space_create() { + JoltSpace3D *space = memnew(JoltSpace3D(job_system)); + RID rid = space_owner.make_rid(space); + space->set_rid(rid); + + const RID default_area_rid = area_create(); + JoltArea3D *default_area = area_owner.get_or_null(default_area_rid); + ERR_FAIL_NULL_V(default_area, RID()); + space->set_default_area(default_area); + default_area->set_space(space); + + return rid; +} + +void JoltPhysicsServer3D::space_set_active(RID p_space, bool p_active) { + JoltSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL(space); + + if (p_active) { + space->set_active(true); + active_spaces.insert(space); + } else { + space->set_active(false); + active_spaces.erase(space); + } +} + +bool JoltPhysicsServer3D::space_is_active(RID p_space) const { + JoltSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL_V(space, false); + + return active_spaces.has(space); +} + +void JoltPhysicsServer3D::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) { + JoltSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL(space); + + space->set_param(p_param, (double)p_value); +} + +real_t JoltPhysicsServer3D::space_get_param(RID p_space, SpaceParameter p_param) const { + const JoltSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL_V(space, 0.0); + + return (real_t)space->get_param(p_param); +} + +PhysicsDirectSpaceState3D *JoltPhysicsServer3D::space_get_direct_state(RID p_space) { + JoltSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL_V(space, nullptr); + ERR_FAIL_COND_V_MSG((on_separate_thread && !doing_sync) || space->is_stepping(), nullptr, "Space state is inaccessible right now, wait for iteration or physics process notification."); + + return space->get_direct_state(); +} + +void JoltPhysicsServer3D::space_set_debug_contacts(RID p_space, int p_max_contacts) { +#ifdef DEBUG_ENABLED + JoltSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL(space); + + space->set_max_debug_contacts(p_max_contacts); +#endif +} + +PackedVector3Array JoltPhysicsServer3D::space_get_contacts(RID p_space) const { +#ifdef DEBUG_ENABLED + JoltSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL_V(space, PackedVector3Array()); + + return space->get_debug_contacts(); +#else + return PackedVector3Array(); +#endif +} + +int JoltPhysicsServer3D::space_get_contact_count(RID p_space) const { +#ifdef DEBUG_ENABLED + JoltSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL_V(space, 0); + + return space->get_debug_contact_count(); +#else + return 0; +#endif +} + +RID JoltPhysicsServer3D::area_create() { + JoltArea3D *area = memnew(JoltArea3D); + RID rid = area_owner.make_rid(area); + area->set_rid(rid); + return rid; +} + +void JoltPhysicsServer3D::area_set_space(RID p_area, RID p_space) { + JoltArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + JoltSpace3D *space = nullptr; + + if (p_space.is_valid()) { + space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL(space); + } + + area->set_space(space); +} + +RID JoltPhysicsServer3D::area_get_space(RID p_area) const { + const JoltArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, RID()); + + const JoltSpace3D *space = area->get_space(); + + if (space == nullptr) { + return RID(); + } + + return space->get_rid(); +} + +void JoltPhysicsServer3D::area_add_shape(RID p_area, RID p_shape, const Transform3D &p_transform, bool p_disabled) { + JoltArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + JoltShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL(shape); + + area->add_shape(shape, p_transform, p_disabled); +} + +void JoltPhysicsServer3D::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) { + JoltArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + JoltShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL(shape); + + area->set_shape(p_shape_idx, shape); +} + +RID JoltPhysicsServer3D::area_get_shape(RID p_area, int p_shape_idx) const { + const JoltArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, RID()); + + const JoltShape3D *shape = area->get_shape(p_shape_idx); + ERR_FAIL_NULL_V(shape, RID()); + + return shape->get_rid(); +} + +void JoltPhysicsServer3D::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform3D &p_transform) { + JoltArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + area->set_shape_transform(p_shape_idx, p_transform); +} + +Transform3D JoltPhysicsServer3D::area_get_shape_transform(RID p_area, int p_shape_idx) const { + const JoltArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, Transform3D()); + + return area->get_shape_transform_scaled(p_shape_idx); +} + +int JoltPhysicsServer3D::area_get_shape_count(RID p_area) const { + const JoltArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, 0); + + return area->get_shape_count(); +} + +void JoltPhysicsServer3D::area_remove_shape(RID p_area, int p_shape_idx) { + JoltArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + area->remove_shape(p_shape_idx); +} + +void JoltPhysicsServer3D::area_clear_shapes(RID p_area) { + JoltArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + area->clear_shapes(); +} + +void JoltPhysicsServer3D::area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) { + JoltArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + area->set_shape_disabled(p_shape_idx, p_disabled); +} + +void JoltPhysicsServer3D::area_attach_object_instance_id(RID p_area, ObjectID p_id) { + RID area_rid = p_area; + + if (space_owner.owns(area_rid)) { + const JoltSpace3D *space = space_owner.get_or_null(area_rid); + area_rid = space->get_default_area()->get_rid(); + } + + JoltArea3D *area = area_owner.get_or_null(area_rid); + ERR_FAIL_NULL(area); + + area->set_instance_id(p_id); +} + +ObjectID JoltPhysicsServer3D::area_get_object_instance_id(RID p_area) const { + RID area_rid = p_area; + + if (space_owner.owns(area_rid)) { + const JoltSpace3D *space = space_owner.get_or_null(area_rid); + area_rid = space->get_default_area()->get_rid(); + } + + JoltArea3D *area = area_owner.get_or_null(area_rid); + ERR_FAIL_NULL_V(area, ObjectID()); + + return area->get_instance_id(); +} + +void JoltPhysicsServer3D::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) { + RID area_rid = p_area; + + if (space_owner.owns(area_rid)) { + const JoltSpace3D *space = space_owner.get_or_null(area_rid); + area_rid = space->get_default_area()->get_rid(); + } + + JoltArea3D *area = area_owner.get_or_null(area_rid); + ERR_FAIL_NULL(area); + + area->set_param(p_param, p_value); +} + +Variant JoltPhysicsServer3D::area_get_param(RID p_area, AreaParameter p_param) const { + RID area_rid = p_area; + + if (space_owner.owns(area_rid)) { + const JoltSpace3D *space = space_owner.get_or_null(area_rid); + area_rid = space->get_default_area()->get_rid(); + } + + JoltArea3D *area = area_owner.get_or_null(area_rid); + ERR_FAIL_NULL_V(area, Variant()); + + return area->get_param(p_param); +} + +void JoltPhysicsServer3D::area_set_transform(RID p_area, const Transform3D &p_transform) { + JoltArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + return area->set_transform(p_transform); +} + +Transform3D JoltPhysicsServer3D::area_get_transform(RID p_area) const { + const JoltArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, Transform3D()); + + return area->get_transform_scaled(); +} + +void JoltPhysicsServer3D::area_set_collision_mask(RID p_area, uint32_t p_mask) { + JoltArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + area->set_collision_mask(p_mask); +} + +uint32_t JoltPhysicsServer3D::area_get_collision_mask(RID p_area) const { + const JoltArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, 0); + + return area->get_collision_mask(); +} + +void JoltPhysicsServer3D::area_set_collision_layer(RID p_area, uint32_t p_layer) { + JoltArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + area->set_collision_layer(p_layer); +} + +uint32_t JoltPhysicsServer3D::area_get_collision_layer(RID p_area) const { + const JoltArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, 0); + + return area->get_collision_layer(); +} + +void JoltPhysicsServer3D::area_set_monitorable(RID p_area, bool p_monitorable) { + JoltArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + area->set_monitorable(p_monitorable); +} + +void JoltPhysicsServer3D::area_set_monitor_callback(RID p_area, const Callable &p_callback) { + JoltArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + area->set_body_monitor_callback(p_callback); +} + +void JoltPhysicsServer3D::area_set_area_monitor_callback(RID p_area, const Callable &p_callback) { + JoltArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + area->set_area_monitor_callback(p_callback); +} + +void JoltPhysicsServer3D::area_set_ray_pickable(RID p_area, bool p_enable) { + JoltArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + area->set_pickable(p_enable); +} + +RID JoltPhysicsServer3D::body_create() { + JoltBody3D *body = memnew(JoltBody3D); + RID rid = body_owner.make_rid(body); + body->set_rid(rid); + return rid; +} + +void JoltPhysicsServer3D::body_set_space(RID p_body, RID p_space) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + JoltSpace3D *space = nullptr; + + if (p_space.is_valid()) { + space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL(space); + } + + body->set_space(space); +} + +RID JoltPhysicsServer3D::body_get_space(RID p_body) const { + const JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, RID()); + + const JoltSpace3D *space = body->get_space(); + + if (space == nullptr) { + return RID(); + } + + return space->get_rid(); +} + +void JoltPhysicsServer3D::body_set_mode(RID p_body, BodyMode p_mode) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_mode(p_mode); +} + +PhysicsServer3D::BodyMode JoltPhysicsServer3D::body_get_mode(RID p_body) const { + const JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, BODY_MODE_STATIC); + + return body->get_mode(); +} + +void JoltPhysicsServer3D::body_add_shape(RID p_body, RID p_shape, const Transform3D &p_transform, bool p_disabled) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + JoltShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL(shape); + + body->add_shape(shape, p_transform, p_disabled); +} + +void JoltPhysicsServer3D::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + JoltShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL(shape); + + body->set_shape(p_shape_idx, shape); +} + +RID JoltPhysicsServer3D::body_get_shape(RID p_body, int p_shape_idx) const { + const JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, RID()); + + const JoltShape3D *shape = body->get_shape(p_shape_idx); + ERR_FAIL_NULL_V(shape, RID()); + + return shape->get_rid(); +} + +void JoltPhysicsServer3D::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform3D &p_transform) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_shape_transform(p_shape_idx, p_transform); +} + +Transform3D JoltPhysicsServer3D::body_get_shape_transform(RID p_body, int p_shape_idx) const { + const JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, Transform3D()); + + return body->get_shape_transform_scaled(p_shape_idx); +} + +int JoltPhysicsServer3D::body_get_shape_count(RID p_body) const { + const JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0); + + return body->get_shape_count(); +} + +void JoltPhysicsServer3D::body_remove_shape(RID p_body, int p_shape_idx) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->remove_shape(p_shape_idx); +} + +void JoltPhysicsServer3D::body_clear_shapes(RID p_body) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->clear_shapes(); +} + +void JoltPhysicsServer3D::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_shape_disabled(p_shape_idx, p_disabled); +} + +void JoltPhysicsServer3D::body_attach_object_instance_id(RID p_body, ObjectID p_id) { + if (JoltBody3D *body = body_owner.get_or_null(p_body)) { + body->set_instance_id(p_id); + } else if (JoltSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body)) { + soft_body->set_instance_id(p_id); + } else { + ERR_FAIL(); + } +} + +ObjectID JoltPhysicsServer3D::body_get_object_instance_id(RID p_body) const { + const JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, ObjectID()); + + return body->get_instance_id(); +} + +void JoltPhysicsServer3D::body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_ccd_enabled(p_enable); +} + +bool JoltPhysicsServer3D::body_is_continuous_collision_detection_enabled(RID p_body) const { + const JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, false); + + return body->is_ccd_enabled(); +} + +void JoltPhysicsServer3D::body_set_collision_layer(RID p_body, uint32_t p_layer) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_collision_layer(p_layer); +} + +uint32_t JoltPhysicsServer3D::body_get_collision_layer(RID p_body) const { + const JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0); + + return body->get_collision_layer(); +} + +void JoltPhysicsServer3D::body_set_collision_mask(RID p_body, uint32_t p_mask) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_collision_mask(p_mask); +} + +uint32_t JoltPhysicsServer3D::body_get_collision_mask(RID p_body) const { + const JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0); + + return body->get_collision_mask(); +} + +void JoltPhysicsServer3D::body_set_collision_priority(RID p_body, real_t p_priority) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_collision_priority((float)p_priority); +} + +real_t JoltPhysicsServer3D::body_get_collision_priority(RID p_body) const { + const JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0.0); + + return (real_t)body->get_collision_priority(); +} + +void JoltPhysicsServer3D::body_set_user_flags(RID p_body, uint32_t p_flags) { + WARN_PRINT("Body user flags are not supported. Any such value will be ignored."); +} + +uint32_t JoltPhysicsServer3D::body_get_user_flags(RID p_body) const { + return 0; +} + +void JoltPhysicsServer3D::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_param(p_param, p_value); +} + +Variant JoltPhysicsServer3D::body_get_param(RID p_body, BodyParameter p_param) const { + const JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, Variant()); + + return body->get_param(p_param); +} + +void JoltPhysicsServer3D::body_reset_mass_properties(RID p_body) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->reset_mass_properties(); +} + +void JoltPhysicsServer3D::body_set_state(RID p_body, BodyState p_state, const Variant &p_value) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_state(p_state, p_value); +} + +Variant JoltPhysicsServer3D::body_get_state(RID p_body, BodyState p_state) const { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, Variant()); + + return body->get_state(p_state); +} + +void JoltPhysicsServer3D::body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + return body->apply_central_impulse(p_impulse); +} + +void JoltPhysicsServer3D::body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + return body->apply_impulse(p_impulse, p_position); +} + +void JoltPhysicsServer3D::body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + return body->apply_torque_impulse(p_impulse); +} + +void JoltPhysicsServer3D::body_apply_central_force(RID p_body, const Vector3 &p_force) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + return body->apply_central_force(p_force); +} + +void JoltPhysicsServer3D::body_apply_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + return body->apply_force(p_force, p_position); +} + +void JoltPhysicsServer3D::body_apply_torque(RID p_body, const Vector3 &p_torque) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + return body->apply_torque(p_torque); +} + +void JoltPhysicsServer3D::body_add_constant_central_force(RID p_body, const Vector3 &p_force) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->add_constant_central_force(p_force); +} + +void JoltPhysicsServer3D::body_add_constant_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->add_constant_force(p_force, p_position); +} + +void JoltPhysicsServer3D::body_add_constant_torque(RID p_body, const Vector3 &p_torque) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->add_constant_torque(p_torque); +} + +void JoltPhysicsServer3D::body_set_constant_force(RID p_body, const Vector3 &p_force) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_constant_force(p_force); +} + +Vector3 JoltPhysicsServer3D::body_get_constant_force(RID p_body) const { + const JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, Vector3()); + + return body->get_constant_force(); +} + +void JoltPhysicsServer3D::body_set_constant_torque(RID p_body, const Vector3 &p_torque) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_constant_torque(p_torque); +} + +Vector3 JoltPhysicsServer3D::body_get_constant_torque(RID p_body) const { + const JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, Vector3()); + + return body->get_constant_torque(); +} + +void JoltPhysicsServer3D::body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_axis_velocity(p_axis_velocity); +} + +void JoltPhysicsServer3D::body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_axis_lock(p_axis, p_lock); +} + +bool JoltPhysicsServer3D::body_is_axis_locked(RID p_body, BodyAxis p_axis) const { + const JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, false); + + return body->is_axis_locked(p_axis); +} + +void JoltPhysicsServer3D::body_add_collision_exception(RID p_body, RID p_excepted_body) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->add_collision_exception(p_excepted_body); +} + +void JoltPhysicsServer3D::body_remove_collision_exception(RID p_body, RID p_excepted_body) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->remove_collision_exception(p_excepted_body); +} + +void JoltPhysicsServer3D::body_get_collision_exceptions(RID p_body, List *p_exceptions) { + const JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + for (const RID &exception : body->get_collision_exceptions()) { + p_exceptions->push_back(exception); + } +} + +void JoltPhysicsServer3D::body_set_max_contacts_reported(RID p_body, int p_amount) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + return body->set_max_contacts_reported(p_amount); +} + +int JoltPhysicsServer3D::body_get_max_contacts_reported(RID p_body) const { + const JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0); + + return body->get_max_contacts_reported(); +} + +void JoltPhysicsServer3D::body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) { + WARN_PRINT("Per-body contact depth threshold is not supported. Any such value will be ignored."); +} + +real_t JoltPhysicsServer3D::body_get_contacts_reported_depth_threshold(RID p_body) const { + return 0.0; +} + +void JoltPhysicsServer3D::body_set_omit_force_integration(RID p_body, bool p_enable) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_custom_integrator(p_enable); +} + +bool JoltPhysicsServer3D::body_is_omitting_force_integration(RID p_body) const { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, false); + + return body->has_custom_integrator(); +} + +void JoltPhysicsServer3D::body_set_state_sync_callback(RID p_body, const Callable &p_callable) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_state_sync_callback(p_callable); +} + +void JoltPhysicsServer3D::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_userdata) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_custom_integration_callback(p_callable, p_userdata); +} + +void JoltPhysicsServer3D::body_set_ray_pickable(RID p_body, bool p_enable) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_pickable(p_enable); +} + +bool JoltPhysicsServer3D::body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result) { + JoltBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, false); + + JoltSpace3D *space = body->get_space(); + ERR_FAIL_NULL_V(space, false); + + return space->get_direct_state()->body_test_motion(*body, p_parameters, r_result); +} + +PhysicsDirectBodyState3D *JoltPhysicsServer3D::body_get_direct_state(RID p_body) { + ERR_FAIL_COND_V_MSG((on_separate_thread && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); + + JoltBody3D *body = body_owner.get_or_null(p_body); + if (unlikely(body == nullptr || body->get_space() == nullptr)) { + return nullptr; + } + + ERR_FAIL_COND_V_MSG(body->get_space()->is_stepping(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); + + return body->get_direct_state(); +} + +RID JoltPhysicsServer3D::soft_body_create() { + JoltSoftBody3D *body = memnew(JoltSoftBody3D); + RID rid = soft_body_owner.make_rid(body); + body->set_rid(rid); + return rid; +} + +void JoltPhysicsServer3D::soft_body_update_rendering_server(RID p_body, PhysicsServer3DRenderingServerHandler *p_rendering_server_handler) { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + return body->update_rendering_server(p_rendering_server_handler); +} + +void JoltPhysicsServer3D::soft_body_set_space(RID p_body, RID p_space) { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + JoltSpace3D *space = nullptr; + + if (p_space.is_valid()) { + space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL(space); + } + + body->set_space(space); +} + +RID JoltPhysicsServer3D::soft_body_get_space(RID p_body) const { + const JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, RID()); + + const JoltSpace3D *space = body->get_space(); + + if (space == nullptr) { + return RID(); + } + + return space->get_rid(); +} + +void JoltPhysicsServer3D::soft_body_set_mesh(RID p_body, RID p_mesh) { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_mesh(p_mesh); +} + +AABB JoltPhysicsServer3D::soft_body_get_bounds(RID p_body) const { + const JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, AABB()); + + return body->get_bounds(); +} + +void JoltPhysicsServer3D::soft_body_set_collision_layer(RID p_body, uint32_t p_layer) { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_collision_layer(p_layer); +} + +uint32_t JoltPhysicsServer3D::soft_body_get_collision_layer(RID p_body) const { + const JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0); + + return body->get_collision_layer(); +} + +void JoltPhysicsServer3D::soft_body_set_collision_mask(RID p_body, uint32_t p_mask) { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_collision_mask(p_mask); +} + +uint32_t JoltPhysicsServer3D::soft_body_get_collision_mask(RID p_body) const { + const JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0); + + return body->get_collision_mask(); +} + +void JoltPhysicsServer3D::soft_body_add_collision_exception(RID p_body, RID p_excepted_body) { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->add_collision_exception(p_excepted_body); +} + +void JoltPhysicsServer3D::soft_body_remove_collision_exception(RID p_body, RID p_excepted_body) { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->remove_collision_exception(p_excepted_body); +} + +void JoltPhysicsServer3D::soft_body_get_collision_exceptions(RID p_body, List *p_exceptions) { + const JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + for (const RID &exception : body->get_collision_exceptions()) { + p_exceptions->push_back(exception); + } +} + +void JoltPhysicsServer3D::soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_value) { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_state(p_state, p_value); +} + +Variant JoltPhysicsServer3D::soft_body_get_state(RID p_body, BodyState p_state) const { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, Variant()); + + return body->get_state(p_state); +} + +void JoltPhysicsServer3D::soft_body_set_transform(RID p_body, const Transform3D &p_transform) { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + return body->set_transform(p_transform); +} + +void JoltPhysicsServer3D::soft_body_set_ray_pickable(RID p_body, bool p_enable) { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + return body->set_pickable(p_enable); +} + +void JoltPhysicsServer3D::soft_body_set_simulation_precision(RID p_body, int p_precision) { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + return body->set_simulation_precision(p_precision); +} + +int JoltPhysicsServer3D::soft_body_get_simulation_precision(RID p_body) const { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0); + + return body->get_simulation_precision(); +} + +void JoltPhysicsServer3D::soft_body_set_total_mass(RID p_body, real_t p_total_mass) { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + return body->set_mass((float)p_total_mass); +} + +real_t JoltPhysicsServer3D::soft_body_get_total_mass(RID p_body) const { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0.0); + + return (real_t)body->get_mass(); +} + +void JoltPhysicsServer3D::soft_body_set_linear_stiffness(RID p_body, real_t p_coefficient) { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + return body->set_stiffness_coefficient((float)p_coefficient); +} + +real_t JoltPhysicsServer3D::soft_body_get_linear_stiffness(RID p_body) const { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0.0); + + return (real_t)body->get_stiffness_coefficient(); +} + +void JoltPhysicsServer3D::soft_body_set_pressure_coefficient(RID p_body, real_t p_coefficient) { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + return body->set_pressure((float)p_coefficient); +} + +real_t JoltPhysicsServer3D::soft_body_get_pressure_coefficient(RID p_body) const { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0.0); + + return (real_t)body->get_pressure(); +} + +void JoltPhysicsServer3D::soft_body_set_damping_coefficient(RID p_body, real_t p_coefficient) { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + return body->set_linear_damping((float)p_coefficient); +} + +real_t JoltPhysicsServer3D::soft_body_get_damping_coefficient(RID p_body) const { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0.0); + + return (real_t)body->get_linear_damping(); +} + +void JoltPhysicsServer3D::soft_body_set_drag_coefficient(RID p_body, real_t p_coefficient) { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + return body->set_drag((float)p_coefficient); +} + +real_t JoltPhysicsServer3D::soft_body_get_drag_coefficient(RID p_body) const { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0.0); + + return (real_t)body->get_drag(); +} + +void JoltPhysicsServer3D::soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_vertex_position(p_point_index, p_global_position); +} + +Vector3 JoltPhysicsServer3D::soft_body_get_point_global_position(RID p_body, int p_point_index) const { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, Vector3()); + + return body->get_vertex_position(p_point_index); +} + +void JoltPhysicsServer3D::soft_body_remove_all_pinned_points(RID p_body) { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->unpin_all_vertices(); +} + +void JoltPhysicsServer3D::soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + if (p_pin) { + body->pin_vertex(p_point_index); + } else { + body->unpin_vertex(p_point_index); + } +} + +bool JoltPhysicsServer3D::soft_body_is_point_pinned(RID p_body, int p_point_index) const { + JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, false); + + return body->is_vertex_pinned(p_point_index); +} + +RID JoltPhysicsServer3D::joint_create() { + JoltJoint3D *joint = memnew(JoltJoint3D); + RID rid = joint_owner.make_rid(joint); + joint->set_rid(rid); + return rid; +} + +void JoltPhysicsServer3D::joint_clear(RID p_joint) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + if (joint->get_type() != JOINT_TYPE_MAX) { + JoltJoint3D *empty_joint = memnew(JoltJoint3D); + empty_joint->set_rid(joint->get_rid()); + + memdelete(joint); + joint = nullptr; + + joint_owner.replace(p_joint, empty_joint); + } +} + +void JoltPhysicsServer3D::joint_make_pin(RID p_joint, RID p_body_a, const Vector3 &p_local_a, RID p_body_b, const Vector3 &p_local_b) { + JoltJoint3D *old_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(old_joint); + + JoltBody3D *body_a = body_owner.get_or_null(p_body_a); + ERR_FAIL_NULL(body_a); + + JoltBody3D *body_b = body_owner.get_or_null(p_body_b); + ERR_FAIL_COND(body_a == body_b); + + JoltJoint3D *new_joint = memnew(JoltPinJoint3D(*old_joint, body_a, body_b, p_local_a, p_local_b)); + + memdelete(old_joint); + old_joint = nullptr; + + joint_owner.replace(p_joint, new_joint); +} + +void JoltPhysicsServer3D::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); + JoltPinJoint3D *pin_joint = static_cast(joint); + + pin_joint->set_param(p_param, (double)p_value); +} + +real_t JoltPhysicsServer3D::pin_joint_get_param(RID p_joint, PinJointParam p_param) const { + const JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0.0); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, 0.0); + const JoltPinJoint3D *pin_joint = static_cast(joint); + + return (real_t)pin_joint->get_param(p_param); +} + +void JoltPhysicsServer3D::pin_joint_set_local_a(RID p_joint, const Vector3 &p_local_a) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); + JoltPinJoint3D *pin_joint = static_cast(joint); + + pin_joint->set_local_a(p_local_a); +} + +Vector3 JoltPhysicsServer3D::pin_joint_get_local_a(RID p_joint) const { + const JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, Vector3()); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, Vector3()); + const JoltPinJoint3D *pin_joint = static_cast(joint); + + return pin_joint->get_local_a(); +} + +void JoltPhysicsServer3D::pin_joint_set_local_b(RID p_joint, const Vector3 &p_local_b) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); + JoltPinJoint3D *pin_joint = static_cast(joint); + + pin_joint->set_local_b(p_local_b); +} + +Vector3 JoltPhysicsServer3D::pin_joint_get_local_b(RID p_joint) const { + const JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, Vector3()); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, Vector3()); + const JoltPinJoint3D *pin_joint = static_cast(joint); + + return pin_joint->get_local_b(); +} + +void JoltPhysicsServer3D::joint_make_hinge(RID p_joint, RID p_body_a, const Transform3D &p_hinge_a, RID p_body_b, const Transform3D &p_hinge_b) { + JoltJoint3D *old_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(old_joint); + + JoltBody3D *body_a = body_owner.get_or_null(p_body_a); + ERR_FAIL_NULL(body_a); + + JoltBody3D *body_b = body_owner.get_or_null(p_body_b); + ERR_FAIL_COND(body_a == body_b); + + JoltJoint3D *new_joint = memnew(JoltHingeJoint3D(*old_joint, body_a, body_b, p_hinge_a, p_hinge_b)); + + memdelete(old_joint); + old_joint = nullptr; + + joint_owner.replace(p_joint, new_joint); +} + +void JoltPhysicsServer3D::joint_make_hinge_simple(RID p_joint, RID p_body_a, const Vector3 &p_pivot_a, const Vector3 &p_axis_a, RID p_body_b, const Vector3 &p_pivot_b, const Vector3 &p_axis_b) { + ERR_FAIL_MSG("Simple hinge joints are not supported when using Jolt Physics."); +} + +void JoltPhysicsServer3D::hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE); + JoltHingeJoint3D *hinge_joint = static_cast(joint); + + return hinge_joint->set_param(p_param, (double)p_value); +} + +real_t JoltPhysicsServer3D::hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const { + const JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0.0); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, 0.0); + const JoltHingeJoint3D *hinge_joint = static_cast(joint); + + return (real_t)hinge_joint->get_param(p_param); +} + +void JoltPhysicsServer3D::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_enabled) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE); + JoltHingeJoint3D *hinge_joint = static_cast(joint); + + return hinge_joint->set_flag(p_flag, p_enabled); +} + +bool JoltPhysicsServer3D::hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const { + const JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, false); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, false); + const JoltHingeJoint3D *hinge_joint = static_cast(joint); + + return hinge_joint->get_flag(p_flag); +} + +void JoltPhysicsServer3D::joint_make_slider(RID p_joint, RID p_body_a, const Transform3D &p_local_ref_a, RID p_body_b, const Transform3D &p_local_ref_b) { + JoltJoint3D *old_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(old_joint); + + JoltBody3D *body_a = body_owner.get_or_null(p_body_a); + ERR_FAIL_NULL(body_a); + + JoltBody3D *body_b = body_owner.get_or_null(p_body_b); + ERR_FAIL_COND(body_a == body_b); + + JoltJoint3D *new_joint = memnew(JoltSliderJoint3D(*old_joint, body_a, body_b, p_local_ref_a, p_local_ref_b)); + + memdelete(old_joint); + old_joint = nullptr; + + joint_owner.replace(p_joint, new_joint); +} + +void JoltPhysicsServer3D::slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_SLIDER); + JoltSliderJoint3D *slider_joint = static_cast(joint); + + return slider_joint->set_param(p_param, (real_t)p_value); +} + +real_t JoltPhysicsServer3D::slider_joint_get_param(RID p_joint, SliderJointParam p_param) const { + const JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0.0); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_SLIDER, 0.0); + const JoltSliderJoint3D *slider_joint = static_cast(joint); + + return slider_joint->get_param(p_param); +} + +void JoltPhysicsServer3D::joint_make_cone_twist(RID p_joint, RID p_body_a, const Transform3D &p_local_ref_a, RID p_body_b, const Transform3D &p_local_ref_b) { + JoltJoint3D *old_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(old_joint); + + JoltBody3D *body_a = body_owner.get_or_null(p_body_a); + ERR_FAIL_NULL(body_a); + + JoltBody3D *body_b = body_owner.get_or_null(p_body_b); + ERR_FAIL_COND(body_a == body_b); + + JoltJoint3D *new_joint = memnew(JoltConeTwistJoint3D(*old_joint, body_a, body_b, p_local_ref_a, p_local_ref_b)); + + memdelete(old_joint); + old_joint = nullptr; + + joint_owner.replace(p_joint, new_joint); +} + +void JoltPhysicsServer3D::cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_CONE_TWIST); + JoltConeTwistJoint3D *cone_twist_joint = static_cast(joint); + + return cone_twist_joint->set_param(p_param, (double)p_value); +} + +real_t JoltPhysicsServer3D::cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const { + const JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0.0); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0.0); + const JoltConeTwistJoint3D *cone_twist_joint = static_cast(joint); + + return (real_t)cone_twist_joint->get_param(p_param); +} + +void JoltPhysicsServer3D::joint_make_generic_6dof(RID p_joint, RID p_body_a, const Transform3D &p_local_ref_a, RID p_body_b, const Transform3D &p_local_ref_b) { + JoltJoint3D *old_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(old_joint); + + JoltBody3D *body_a = body_owner.get_or_null(p_body_a); + ERR_FAIL_NULL(body_a); + + JoltBody3D *body_b = body_owner.get_or_null(p_body_b); + ERR_FAIL_COND(body_a == body_b); + + JoltJoint3D *new_joint = memnew(JoltGeneric6DOFJoint3D(*old_joint, body_a, body_b, p_local_ref_a, p_local_ref_b)); + + memdelete(old_joint); + old_joint = nullptr; + + joint_owner.replace(p_joint, new_joint); +} + +void JoltPhysicsServer3D::generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF); + JoltGeneric6DOFJoint3D *g6dof_joint = static_cast(joint); + + return g6dof_joint->set_param(p_axis, p_param, (double)p_value); +} + +real_t JoltPhysicsServer3D::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const { + const JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0.0); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, 0.0); + const JoltGeneric6DOFJoint3D *g6dof_joint = static_cast(joint); + + return (real_t)g6dof_joint->get_param(p_axis, p_param); +} + +void JoltPhysicsServer3D::generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_enable) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF); + JoltGeneric6DOFJoint3D *g6dof_joint = static_cast(joint); + + return g6dof_joint->set_flag(p_axis, p_flag, p_enable); +} + +bool JoltPhysicsServer3D::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const { + const JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, false); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, false); + const JoltGeneric6DOFJoint3D *g6dof_joint = static_cast(joint); + + return g6dof_joint->get_flag(p_axis, p_flag); +} + +PhysicsServer3D::JointType JoltPhysicsServer3D::joint_get_type(RID p_joint) const { + const JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, JOINT_TYPE_PIN); + + return joint->get_type(); +} + +void JoltPhysicsServer3D::joint_set_solver_priority(RID p_joint, int p_priority) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + joint->set_solver_priority(p_priority); +} + +int JoltPhysicsServer3D::joint_get_solver_priority(RID p_joint) const { + const JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0); + + return joint->get_solver_priority(); +} + +void JoltPhysicsServer3D::joint_disable_collisions_between_bodies(RID p_joint, bool p_disable) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + joint->set_collision_disabled(p_disable); +} + +bool JoltPhysicsServer3D::joint_is_disabled_collisions_between_bodies(RID p_joint) const { + const JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, false); + + return joint->is_collision_disabled(); +} + +void JoltPhysicsServer3D::free(RID p_rid) { + if (JoltShape3D *shape = shape_owner.get_or_null(p_rid)) { + free_shape(shape); + } else if (JoltBody3D *body = body_owner.get_or_null(p_rid)) { + free_body(body); + } else if (JoltJoint3D *joint = joint_owner.get_or_null(p_rid)) { + free_joint(joint); + } else if (JoltArea3D *area = area_owner.get_or_null(p_rid)) { + free_area(area); + } else if (JoltSoftBody3D *soft_body = soft_body_owner.get_or_null(p_rid)) { + free_soft_body(soft_body); + } else if (JoltSpace3D *space = space_owner.get_or_null(p_rid)) { + free_space(space); + } else { + ERR_FAIL_MSG("Failed to free RID: The specified RID has no owner."); + } +} + +void JoltPhysicsServer3D::set_active(bool p_active) { + active = p_active; +} + +void JoltPhysicsServer3D::init() { + job_system = new JoltJobSystem(); +} + +void JoltPhysicsServer3D::finish() { + if (job_system != nullptr) { + delete job_system; + job_system = nullptr; + } +} + +void JoltPhysicsServer3D::step(real_t p_step) { + if (!active) { + return; + } + + for (JoltSpace3D *active_space : active_spaces) { + job_system->pre_step(); + + active_space->step((float)p_step); + + job_system->post_step(); + } +} + +void JoltPhysicsServer3D::sync() { + doing_sync = true; +} + +void JoltPhysicsServer3D::end_sync() { + doing_sync = false; +} + +void JoltPhysicsServer3D::flush_queries() { + if (!active) { + return; + } + + flushing_queries = true; + + for (JoltSpace3D *space : active_spaces) { + space->call_queries(); + } + + flushing_queries = false; + +#ifdef DEBUG_ENABLED + job_system->flush_timings(); +#endif +} + +bool JoltPhysicsServer3D::is_flushing_queries() const { + return flushing_queries; +} + +int JoltPhysicsServer3D::get_process_info(ProcessInfo p_process_info) { + return 0; +} + +void JoltPhysicsServer3D::free_space(JoltSpace3D *p_space) { + ERR_FAIL_NULL(p_space); + + free_area(p_space->get_default_area()); + space_set_active(p_space->get_rid(), false); + space_owner.free(p_space->get_rid()); + memdelete(p_space); +} + +void JoltPhysicsServer3D::free_area(JoltArea3D *p_area) { + ERR_FAIL_NULL(p_area); + + p_area->set_space(nullptr); + area_owner.free(p_area->get_rid()); + memdelete(p_area); +} + +void JoltPhysicsServer3D::free_body(JoltBody3D *p_body) { + ERR_FAIL_NULL(p_body); + + p_body->set_space(nullptr); + body_owner.free(p_body->get_rid()); + memdelete(p_body); +} + +void JoltPhysicsServer3D::free_soft_body(JoltSoftBody3D *p_body) { + ERR_FAIL_NULL(p_body); + + p_body->set_space(nullptr); + soft_body_owner.free(p_body->get_rid()); + memdelete(p_body); +} + +void JoltPhysicsServer3D::free_shape(JoltShape3D *p_shape) { + ERR_FAIL_NULL(p_shape); + + p_shape->remove_self(); + shape_owner.free(p_shape->get_rid()); + memdelete(p_shape); +} + +void JoltPhysicsServer3D::free_joint(JoltJoint3D *p_joint) { + ERR_FAIL_NULL(p_joint); + + joint_owner.free(p_joint->get_rid()); + memdelete(p_joint); +} + +#ifdef DEBUG_ENABLED + +void JoltPhysicsServer3D::dump_debug_snapshots(const String &p_dir) { + for (JoltSpace3D *space : active_spaces) { + space->dump_debug_snapshot(p_dir); + } +} + +void JoltPhysicsServer3D::space_dump_debug_snapshot(RID p_space, const String &p_dir) { + JoltSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL(space); + + space->dump_debug_snapshot(p_dir); +} + +#endif + +bool JoltPhysicsServer3D::joint_get_enabled(RID p_joint) const { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, false); + + return joint->is_enabled(); +} + +void JoltPhysicsServer3D::joint_set_enabled(RID p_joint, bool p_enabled) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + joint->set_enabled(p_enabled); +} + +int JoltPhysicsServer3D::joint_get_solver_velocity_iterations(RID p_joint) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0); + + return joint->get_solver_velocity_iterations(); +} + +void JoltPhysicsServer3D::joint_set_solver_velocity_iterations(RID p_joint, int p_value) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + return joint->set_solver_velocity_iterations(p_value); +} + +int JoltPhysicsServer3D::joint_get_solver_position_iterations(RID p_joint) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0); + + return joint->get_solver_position_iterations(); +} + +void JoltPhysicsServer3D::joint_set_solver_position_iterations(RID p_joint, int p_value) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + return joint->set_solver_position_iterations(p_value); +} + +float JoltPhysicsServer3D::pin_joint_get_applied_force(RID p_joint) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0.0f); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, 0.0); + JoltPinJoint3D *pin_joint = static_cast(joint); + + return pin_joint->get_applied_force(); +} + +double JoltPhysicsServer3D::hinge_joint_get_jolt_param(RID p_joint, HingeJointParamJolt p_param) const { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0.0); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, 0.0); + JoltHingeJoint3D *hinge_joint = static_cast(joint); + + return hinge_joint->get_jolt_param(p_param); +} + +void JoltPhysicsServer3D::hinge_joint_set_jolt_param(RID p_joint, HingeJointParamJolt p_param, double p_value) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE); + JoltHingeJoint3D *hinge_joint = static_cast(joint); + + return hinge_joint->set_jolt_param(p_param, p_value); +} + +bool JoltPhysicsServer3D::hinge_joint_get_jolt_flag(RID p_joint, HingeJointFlagJolt p_flag) const { + const JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, false); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, false); + const JoltHingeJoint3D *hinge_joint = static_cast(joint); + + return hinge_joint->get_jolt_flag(p_flag); +} + +void JoltPhysicsServer3D::hinge_joint_set_jolt_flag(RID p_joint, HingeJointFlagJolt p_flag, bool p_enabled) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE); + JoltHingeJoint3D *hinge_joint = static_cast(joint); + + return hinge_joint->set_jolt_flag(p_flag, p_enabled); +} + +float JoltPhysicsServer3D::hinge_joint_get_applied_force(RID p_joint) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0.0f); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, 0.0f); + JoltHingeJoint3D *hinge_joint = static_cast(joint); + + return hinge_joint->get_applied_force(); +} + +float JoltPhysicsServer3D::hinge_joint_get_applied_torque(RID p_joint) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0.0f); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, 0.0f); + JoltHingeJoint3D *hinge_joint = static_cast(joint); + + return hinge_joint->get_applied_torque(); +} + +double JoltPhysicsServer3D::slider_joint_get_jolt_param(RID p_joint, SliderJointParamJolt p_param) const { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0.0); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_SLIDER, 0.0); + JoltSliderJoint3D *slider_joint = static_cast(joint); + + return slider_joint->get_jolt_param(p_param); +} + +void JoltPhysicsServer3D::slider_joint_set_jolt_param(RID p_joint, SliderJointParamJolt p_param, double p_value) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_SLIDER); + JoltSliderJoint3D *slider_joint = static_cast(joint); + + return slider_joint->set_jolt_param(p_param, p_value); +} + +bool JoltPhysicsServer3D::slider_joint_get_jolt_flag(RID p_joint, SliderJointFlagJolt p_flag) const { + const JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, false); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_SLIDER, false); + const JoltSliderJoint3D *slider_joint = static_cast(joint); + + return slider_joint->get_jolt_flag(p_flag); +} + +void JoltPhysicsServer3D::slider_joint_set_jolt_flag(RID p_joint, SliderJointFlagJolt p_flag, bool p_enabled) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_SLIDER); + JoltSliderJoint3D *slider_joint = static_cast(joint); + + return slider_joint->set_jolt_flag(p_flag, p_enabled); +} + +float JoltPhysicsServer3D::slider_joint_get_applied_force(RID p_joint) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0.0f); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_SLIDER, 0.0f); + JoltSliderJoint3D *slider_joint = static_cast(joint); + + return slider_joint->get_applied_force(); +} + +float JoltPhysicsServer3D::slider_joint_get_applied_torque(RID p_joint) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0.0f); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_SLIDER, 0.0f); + JoltSliderJoint3D *slider_joint = static_cast(joint); + + return slider_joint->get_applied_torque(); +} + +double JoltPhysicsServer3D::cone_twist_joint_get_jolt_param(RID p_joint, ConeTwistJointParamJolt p_param) const { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0.0); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0.0); + JoltConeTwistJoint3D *cone_twist_joint = static_cast(joint); + + return cone_twist_joint->get_jolt_param(p_param); +} + +void JoltPhysicsServer3D::cone_twist_joint_set_jolt_param(RID p_joint, ConeTwistJointParamJolt p_param, double p_value) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_CONE_TWIST); + JoltConeTwistJoint3D *cone_twist_joint = static_cast(joint); + + return cone_twist_joint->set_jolt_param(p_param, p_value); +} + +bool JoltPhysicsServer3D::cone_twist_joint_get_jolt_flag(RID p_joint, ConeTwistJointFlagJolt p_flag) const { + const JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, false); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, false); + const JoltConeTwistJoint3D *cone_twist_joint = static_cast(joint); + + return cone_twist_joint->get_jolt_flag(p_flag); +} + +void JoltPhysicsServer3D::cone_twist_joint_set_jolt_flag(RID p_joint, ConeTwistJointFlagJolt p_flag, bool p_enabled) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_CONE_TWIST); + JoltConeTwistJoint3D *cone_twist_joint = static_cast(joint); + + return cone_twist_joint->set_jolt_flag(p_flag, p_enabled); +} + +float JoltPhysicsServer3D::cone_twist_joint_get_applied_force(RID p_joint) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0.0f); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0.0f); + JoltConeTwistJoint3D *cone_twist_joint = static_cast(joint); + + return cone_twist_joint->get_applied_force(); +} + +float JoltPhysicsServer3D::cone_twist_joint_get_applied_torque(RID p_joint) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0.0f); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0.0f); + JoltConeTwistJoint3D *cone_twist_joint = static_cast(joint); + + return cone_twist_joint->get_applied_torque(); +} + +double JoltPhysicsServer3D::generic_6dof_joint_get_jolt_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParamJolt p_param) const { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0.0); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, 0.0); + JoltGeneric6DOFJoint3D *g6dof_joint = static_cast(joint); + + return g6dof_joint->get_jolt_param(p_axis, p_param); +} + +void JoltPhysicsServer3D::generic_6dof_joint_set_jolt_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParamJolt p_param, double p_value) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF); + JoltGeneric6DOFJoint3D *g6dof_joint = static_cast(joint); + + return g6dof_joint->set_jolt_param(p_axis, p_param, p_value); +} + +bool JoltPhysicsServer3D::generic_6dof_joint_get_jolt_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlagJolt p_flag) const { + const JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, false); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, false); + const JoltGeneric6DOFJoint3D *g6dof_joint = static_cast(joint); + + return g6dof_joint->get_jolt_flag(p_axis, p_flag); +} + +void JoltPhysicsServer3D::generic_6dof_joint_set_jolt_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlagJolt p_flag, bool p_enabled) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF); + JoltGeneric6DOFJoint3D *g6dof_joint = static_cast(joint); + + return g6dof_joint->set_jolt_flag(p_axis, p_flag, p_enabled); +} + +float JoltPhysicsServer3D::generic_6dof_joint_get_applied_force(RID p_joint) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0.0f); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, 0.0f); + JoltGeneric6DOFJoint3D *g6dof_joint = static_cast(joint); + + return g6dof_joint->get_applied_force(); +} + +float JoltPhysicsServer3D::generic_6dof_joint_get_applied_torque(RID p_joint) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0.0f); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, 0.0f); + JoltGeneric6DOFJoint3D *g6dof_joint = static_cast(joint); + + return g6dof_joint->get_applied_torque(); +} diff --git a/modules/jolt_physics/jolt_physics_server_3d.h b/modules/jolt_physics/jolt_physics_server_3d.h new file mode 100644 index 000000000000..00f527e7f649 --- /dev/null +++ b/modules/jolt_physics/jolt_physics_server_3d.h @@ -0,0 +1,503 @@ +/**************************************************************************/ +/* jolt_physics_server_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_PHYSICS_SERVER_3D_H +#define JOLT_PHYSICS_SERVER_3D_H + +#include "core/templates/hash_set.h" +#include "core/templates/rid_owner.h" +#include "servers/physics_server_3d.h" + +class JoltArea3D; +class JoltBody3D; +class JoltJobSystem; +class JoltJoint3D; +class JoltShape3D; +class JoltSoftBody3D; +class JoltSpace3D; + +class JoltPhysicsServer3D final : public PhysicsServer3D { + GDCLASS(JoltPhysicsServer3D, PhysicsServer3D) + + inline static JoltPhysicsServer3D *singleton = nullptr; + + mutable RID_PtrOwner space_owner; + mutable RID_PtrOwner area_owner; + mutable RID_PtrOwner body_owner; + mutable RID_PtrOwner soft_body_owner; + mutable RID_PtrOwner shape_owner; + mutable RID_PtrOwner joint_owner; + + HashSet active_spaces; + + JoltJobSystem *job_system = nullptr; + + bool on_separate_thread = false; + bool active = true; + bool flushing_queries = false; + bool doing_sync = false; + +public: + enum HingeJointParamJolt { + HINGE_JOINT_LIMIT_SPRING_FREQUENCY = 100, + HINGE_JOINT_LIMIT_SPRING_DAMPING, + HINGE_JOINT_MOTOR_MAX_TORQUE, + }; + + enum HingeJointFlagJolt { + HINGE_JOINT_FLAG_USE_LIMIT_SPRING = 100, + }; + + enum SliderJointParamJolt { + SLIDER_JOINT_LIMIT_SPRING_FREQUENCY = 100, + SLIDER_JOINT_LIMIT_SPRING_DAMPING, + SLIDER_JOINT_MOTOR_TARGET_VELOCITY, + SLIDER_JOINT_MOTOR_MAX_FORCE, + }; + + enum SliderJointFlagJolt { + SLIDER_JOINT_FLAG_USE_LIMIT = 100, + SLIDER_JOINT_FLAG_USE_LIMIT_SPRING, + SLIDER_JOINT_FLAG_ENABLE_MOTOR, + }; + + enum ConeTwistJointParamJolt { + CONE_TWIST_JOINT_SWING_MOTOR_TARGET_VELOCITY_Y = 100, + CONE_TWIST_JOINT_SWING_MOTOR_TARGET_VELOCITY_Z, + CONE_TWIST_JOINT_TWIST_MOTOR_TARGET_VELOCITY, + CONE_TWIST_JOINT_SWING_MOTOR_MAX_TORQUE, + CONE_TWIST_JOINT_TWIST_MOTOR_MAX_TORQUE, + }; + + enum ConeTwistJointFlagJolt { + CONE_TWIST_JOINT_FLAG_USE_SWING_LIMIT = 100, + CONE_TWIST_JOINT_FLAG_USE_TWIST_LIMIT, + CONE_TWIST_JOINT_FLAG_ENABLE_SWING_MOTOR, + CONE_TWIST_JOINT_FLAG_ENABLE_TWIST_MOTOR, + }; + + enum G6DOFJointAxisParamJolt { + G6DOF_JOINT_LINEAR_SPRING_FREQUENCY = 100, + G6DOF_JOINT_LINEAR_LIMIT_SPRING_FREQUENCY, + G6DOF_JOINT_LINEAR_LIMIT_SPRING_DAMPING, + G6DOF_JOINT_ANGULAR_SPRING_FREQUENCY, + G6DOF_JOINT_LINEAR_SPRING_MAX_FORCE, + G6DOF_JOINT_ANGULAR_SPRING_MAX_TORQUE, + }; + + enum G6DOFJointAxisFlagJolt { + G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT_SPRING = 100, + G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING_FREQUENCY, + G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING_FREQUENCY, + }; + +private: + static void _bind_methods() {} + +public: + explicit JoltPhysicsServer3D(bool p_on_separate_thread); + ~JoltPhysicsServer3D(); + + static JoltPhysicsServer3D *get_singleton() { return singleton; } + + virtual RID world_boundary_shape_create() override; + virtual RID separation_ray_shape_create() override; + virtual RID sphere_shape_create() override; + virtual RID box_shape_create() override; + virtual RID capsule_shape_create() override; + virtual RID cylinder_shape_create() override; + virtual RID convex_polygon_shape_create() override; + virtual RID concave_polygon_shape_create() override; + virtual RID heightmap_shape_create() override; + virtual RID custom_shape_create() override; + + virtual void shape_set_data(RID p_shape, const Variant &p_data) override; + virtual Variant shape_get_data(RID p_shape) const override; + + virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias) override; + + virtual void shape_set_margin(RID p_shape, real_t p_margin) override; + virtual real_t shape_get_margin(RID p_shape) const override; + + virtual PhysicsServer3D::ShapeType shape_get_type(RID p_shape) const override; + + virtual real_t shape_get_custom_solver_bias(RID p_shape) const override; + + virtual RID space_create() override; + + virtual void space_set_active(RID p_space, bool p_active) override; + virtual bool space_is_active(RID p_space) const override; + + virtual void space_set_param(RID p_space, PhysicsServer3D::SpaceParameter p_param, real_t p_value) override; + virtual real_t space_get_param(RID p_space, PhysicsServer3D::SpaceParameter p_param) const override; + + virtual PhysicsDirectSpaceState3D *space_get_direct_state(RID p_space) override; + + virtual void space_set_debug_contacts(RID p_space, int p_max_contacts) override; + virtual PackedVector3Array space_get_contacts(RID p_space) const override; + virtual int space_get_contact_count(RID p_space) const override; + + virtual RID area_create() override; + + virtual void area_set_space(RID p_area, RID p_space) override; + virtual RID area_get_space(RID p_area) const override; + + virtual void area_add_shape(RID p_area, RID p_shape, const Transform3D &p_transform, bool p_disabled) override; + + virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape) override; + virtual RID area_get_shape(RID p_area, int p_shape_idx) const override; + + virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform3D &p_transform) override; + virtual Transform3D area_get_shape_transform(RID p_area, int p_shape_idx) const override; + + virtual void area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) override; + + virtual int area_get_shape_count(RID p_area) const override; + + virtual void area_remove_shape(RID p_area, int p_shape_idx) override; + virtual void area_clear_shapes(RID p_area) override; + + virtual void area_attach_object_instance_id(RID p_area, ObjectID p_id) override; + virtual ObjectID area_get_object_instance_id(RID p_area) const override; + + virtual void area_set_param(RID p_area, PhysicsServer3D::AreaParameter p_param, const Variant &p_value) override; + virtual Variant area_get_param(RID p_area, PhysicsServer3D::AreaParameter p_param) const override; + + virtual void area_set_transform(RID p_area, const Transform3D &p_transform) override; + virtual Transform3D area_get_transform(RID p_area) const override; + + virtual void area_set_collision_layer(RID p_area, uint32_t p_layer) override; + virtual uint32_t area_get_collision_layer(RID p_area) const override; + + virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) override; + virtual uint32_t area_get_collision_mask(RID p_area) const override; + + virtual void area_set_monitorable(RID p_area, bool p_monitorable) override; + + virtual void area_set_ray_pickable(RID p_area, bool p_enable) override; + + virtual void area_set_monitor_callback(RID p_area, const Callable &p_callback) override; + virtual void area_set_area_monitor_callback(RID p_area, const Callable &p_callback) override; + + virtual RID body_create() override; + + virtual void body_set_space(RID p_body, RID p_space) override; + virtual RID body_get_space(RID p_body) const override; + + virtual void body_set_mode(RID p_body, PhysicsServer3D::BodyMode p_mode) override; + virtual PhysicsServer3D::BodyMode body_get_mode(RID p_body) const override; + + virtual void body_add_shape(RID p_body, RID p_shape, const Transform3D &p_transform, bool p_disabled) override; + + virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape) override; + virtual RID body_get_shape(RID p_body, int p_shape_idx) const override; + + virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform3D &p_transform) override; + virtual Transform3D body_get_shape_transform(RID p_body, int p_shape_idx) const override; + + virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) override; + + virtual int body_get_shape_count(RID p_body) const override; + + virtual void body_remove_shape(RID p_body, int p_shape_idx) override; + virtual void body_clear_shapes(RID p_body) override; + + virtual void body_attach_object_instance_id(RID p_body, ObjectID p_id) override; + virtual ObjectID body_get_object_instance_id(RID p_body) const override; + + virtual void body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) override; + virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const override; + + virtual void body_set_collision_layer(RID p_body, uint32_t p_layer) override; + virtual uint32_t body_get_collision_layer(RID p_body) const override; + + virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) override; + virtual uint32_t body_get_collision_mask(RID p_body) const override; + + virtual void body_set_collision_priority(RID p_body, real_t p_priority) override; + virtual real_t body_get_collision_priority(RID p_body) const override; + + virtual void body_set_user_flags(RID p_body, uint32_t p_flags) override; + virtual uint32_t body_get_user_flags(RID p_body) const override; + + virtual void body_set_param(RID p_body, PhysicsServer3D::BodyParameter p_param, const Variant &p_value) override; + virtual Variant body_get_param(RID p_body, PhysicsServer3D::BodyParameter p_param) const override; + + virtual void body_reset_mass_properties(RID p_body) override; + + virtual void body_set_state(RID p_body, PhysicsServer3D::BodyState p_state, const Variant &p_value) override; + virtual Variant body_get_state(RID p_body, PhysicsServer3D::BodyState p_state) const override; + + virtual void body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) override; + virtual void body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position) override; + virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) override; + + virtual void body_apply_central_force(RID p_body, const Vector3 &p_force) override; + virtual void body_apply_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) override; + virtual void body_apply_torque(RID p_body, const Vector3 &p_torque) override; + + virtual void body_add_constant_central_force(RID p_body, const Vector3 &p_force) override; + virtual void body_add_constant_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) override; + virtual void body_add_constant_torque(RID p_body, const Vector3 &p_torque) override; + + virtual void body_set_constant_force(RID p_body, const Vector3 &p_force) override; + virtual Vector3 body_get_constant_force(RID p_body) const override; + + virtual void body_set_constant_torque(RID p_body, const Vector3 &p_torque) override; + virtual Vector3 body_get_constant_torque(RID p_body) const override; + + virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) override; + + virtual void body_set_axis_lock(RID p_body, PhysicsServer3D::BodyAxis p_axis, bool p_lock) override; + virtual bool body_is_axis_locked(RID p_body, PhysicsServer3D::BodyAxis p_axis) const override; + + virtual void body_add_collision_exception(RID p_body, RID p_excepted_body) override; + virtual void body_remove_collision_exception(RID p_body, RID p_excepted_body) override; + virtual void body_get_collision_exceptions(RID p_body, List *p_exceptions) override; + + virtual void body_set_max_contacts_reported(RID p_body, int p_amount) override; + virtual int body_get_max_contacts_reported(RID p_body) const override; + + virtual void body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) override; + virtual real_t body_get_contacts_reported_depth_threshold(RID p_body) const override; + + virtual void body_set_omit_force_integration(RID p_body, bool p_enable) override; + virtual bool body_is_omitting_force_integration(RID p_body) const override; + + virtual void body_set_state_sync_callback(RID p_body, const Callable &p_callable) override; + virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_userdata) override; + + virtual void body_set_ray_pickable(RID p_body, bool p_enable) override; + + virtual bool body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result) override; + + virtual PhysicsDirectBodyState3D *body_get_direct_state(RID p_body) override; + + virtual RID soft_body_create() override; + + virtual void soft_body_update_rendering_server(RID p_body, PhysicsServer3DRenderingServerHandler *p_rendering_server_handler) override; + + virtual void soft_body_set_space(RID p_body, RID p_space) override; + virtual RID soft_body_get_space(RID p_body) const override; + + virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) override; + + virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer) override; + virtual uint32_t soft_body_get_collision_layer(RID p_body) const override; + + virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask) override; + virtual uint32_t soft_body_get_collision_mask(RID p_body) const override; + + virtual void soft_body_add_collision_exception(RID p_body, RID p_excepted_body) override; + virtual void soft_body_remove_collision_exception(RID p_body, RID p_excepted_body) override; + virtual void soft_body_get_collision_exceptions(RID p_body, List *p_exceptions) override; + + virtual void soft_body_set_state(RID p_body, PhysicsServer3D::BodyState p_state, const Variant &p_value) override; + virtual Variant soft_body_get_state(RID p_body, PhysicsServer3D::BodyState p_state) const override; + + virtual void soft_body_set_transform(RID p_body, const Transform3D &p_transform) override; + + virtual void soft_body_set_simulation_precision(RID p_body, int p_precision) override; + virtual int soft_body_get_simulation_precision(RID p_body) const override; + + virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass) override; + virtual real_t soft_body_get_total_mass(RID p_body) const override; + + virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_coefficient) override; + virtual real_t soft_body_get_linear_stiffness(RID p_body) const override; + + virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_coefficient) override; + virtual real_t soft_body_get_pressure_coefficient(RID p_body) const override; + + virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_coefficient) override; + virtual real_t soft_body_get_damping_coefficient(RID p_body) const override; + + virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_coefficient) override; + virtual real_t soft_body_get_drag_coefficient(RID p_body) const override; + + virtual void soft_body_set_mesh(RID p_body, RID p_mesh) override; + + virtual AABB soft_body_get_bounds(RID p_body) const override; + + virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) override; + + virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) const override; + + virtual void soft_body_remove_all_pinned_points(RID p_body) override; + + virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) override; + virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) const override; + + virtual RID joint_create() override; + virtual void joint_clear(RID p_joint) override; + + virtual void joint_make_pin(RID p_joint, RID p_body_a, const Vector3 &p_local_a, RID p_body_b, const Vector3 &p_local_b) override; + + virtual void pin_joint_set_param(RID p_joint, PhysicsServer3D::PinJointParam p_param, real_t p_value) override; + virtual real_t pin_joint_get_param(RID p_joint, PhysicsServer3D::PinJointParam p_param) const override; + + virtual void pin_joint_set_local_a(RID p_joint, const Vector3 &p_local_a) override; + virtual Vector3 pin_joint_get_local_a(RID p_joint) const override; + + virtual void pin_joint_set_local_b(RID p_joint, const Vector3 &p_local_b) override; + virtual Vector3 pin_joint_get_local_b(RID p_joint) const override; + + virtual void joint_make_hinge(RID p_joint, RID p_body_a, const Transform3D &p_hinge_a, RID p_body_b, const Transform3D &p_hinge_b) override; + + virtual void joint_make_hinge_simple(RID p_joint, RID p_body_a, const Vector3 &p_pivot_a, const Vector3 &p_axis_a, RID p_body_b, const Vector3 &p_pivot_b, const Vector3 &p_axis_b) override; + + virtual void hinge_joint_set_param(RID p_joint, PhysicsServer3D::HingeJointParam p_param, real_t p_value) override; + virtual real_t hinge_joint_get_param(RID p_joint, PhysicsServer3D::HingeJointParam p_param) const override; + + virtual void hinge_joint_set_flag(RID p_joint, PhysicsServer3D::HingeJointFlag p_flag, bool p_enabled) override; + virtual bool hinge_joint_get_flag(RID p_joint, PhysicsServer3D::HingeJointFlag p_flag) const override; + + virtual void joint_make_slider(RID p_joint, RID p_body_a, const Transform3D &p_local_ref_a, RID p_body_b, const Transform3D &p_local_ref_b) override; + + virtual void slider_joint_set_param(RID p_joint, PhysicsServer3D::SliderJointParam p_param, real_t p_value) override; + virtual real_t slider_joint_get_param(RID p_joint, PhysicsServer3D::SliderJointParam p_param) const override; + + virtual void joint_make_cone_twist(RID p_joint, RID p_body_a, const Transform3D &p_local_ref_a, RID p_body_b, const Transform3D &p_local_ref_b) override; + + virtual void cone_twist_joint_set_param(RID p_joint, PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value) override; + virtual real_t cone_twist_joint_get_param(RID p_joint, PhysicsServer3D::ConeTwistJointParam p_param) const override; + + virtual void joint_make_generic_6dof(RID p_joint, RID p_body_a, const Transform3D &p_local_ref_a, RID p_body_b, const Transform3D &p_local_ref_b) override; + + virtual void generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value) override; + virtual real_t generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const override; + + virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_enable) override; + virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const override; + + virtual PhysicsServer3D::JointType joint_get_type(RID p_joint) const override; + + virtual void joint_set_solver_priority(RID p_joint, int p_priority) override; + virtual int joint_get_solver_priority(RID p_joint) const override; + + virtual void joint_disable_collisions_between_bodies(RID p_joint, bool p_disable) override; + virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const override; + + virtual void free(RID p_rid) override; + + virtual void set_active(bool p_active) override; + + virtual void init() override; + virtual void finish() override; + + virtual void step(real_t p_step) override; + + virtual void sync() override; + virtual void end_sync() override; + + virtual void flush_queries() override; + virtual bool is_flushing_queries() const override; + + virtual int get_process_info(PhysicsServer3D::ProcessInfo p_process_info) override; + + bool is_active() const { return active; } + + void free_space(JoltSpace3D *p_space); + void free_area(JoltArea3D *p_area); + void free_body(JoltBody3D *p_body); + void free_soft_body(JoltSoftBody3D *p_body); + void free_shape(JoltShape3D *p_shape); + void free_joint(JoltJoint3D *p_joint); + + JoltSpace3D *get_space(RID p_rid) const { return space_owner.get_or_null(p_rid); } + JoltArea3D *get_area(RID p_rid) const { return area_owner.get_or_null(p_rid); } + JoltBody3D *get_body(RID p_rid) const { return body_owner.get_or_null(p_rid); } + JoltShape3D *get_shape(RID p_rid) const { return shape_owner.get_or_null(p_rid); } + JoltJoint3D *get_joint(RID p_rid) const { return joint_owner.get_or_null(p_rid); } + +#ifdef DEBUG_ENABLED + void dump_debug_snapshots(const String &p_dir); + + void space_dump_debug_snapshot(RID p_space, const String &p_dir); +#endif + + bool joint_get_enabled(RID p_joint) const; + void joint_set_enabled(RID p_joint, bool p_enabled); + + int joint_get_solver_velocity_iterations(RID p_joint); + void joint_set_solver_velocity_iterations(RID p_joint, int p_value); + + int joint_get_solver_position_iterations(RID p_joint); + void joint_set_solver_position_iterations(RID p_joint, int p_value); + + float pin_joint_get_applied_force(RID p_joint); + + double hinge_joint_get_jolt_param(RID p_joint, HingeJointParamJolt p_param) const; + void hinge_joint_set_jolt_param(RID p_joint, HingeJointParamJolt p_param, double p_value); + + bool hinge_joint_get_jolt_flag(RID p_joint, HingeJointFlagJolt p_flag) const; + void hinge_joint_set_jolt_flag(RID p_joint, HingeJointFlagJolt p_flag, bool p_enabled); + + float hinge_joint_get_applied_force(RID p_joint); + float hinge_joint_get_applied_torque(RID p_joint); + + double slider_joint_get_jolt_param(RID p_joint, SliderJointParamJolt p_param) const; + void slider_joint_set_jolt_param(RID p_joint, SliderJointParamJolt p_param, double p_value); + + bool slider_joint_get_jolt_flag(RID p_joint, SliderJointFlagJolt p_flag) const; + void slider_joint_set_jolt_flag(RID p_joint, SliderJointFlagJolt p_flag, bool p_enabled); + + float slider_joint_get_applied_force(RID p_joint); + float slider_joint_get_applied_torque(RID p_joint); + + double cone_twist_joint_get_jolt_param(RID p_joint, ConeTwistJointParamJolt p_param) const; + void cone_twist_joint_set_jolt_param(RID p_joint, ConeTwistJointParamJolt p_param, double p_value); + + bool cone_twist_joint_get_jolt_flag(RID p_joint, ConeTwistJointFlagJolt p_flag) const; + void cone_twist_joint_set_jolt_flag(RID p_joint, ConeTwistJointFlagJolt p_flag, bool p_enabled); + + float cone_twist_joint_get_applied_force(RID p_joint); + float cone_twist_joint_get_applied_torque(RID p_joint); + + double generic_6dof_joint_get_jolt_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParamJolt p_param) const; + void generic_6dof_joint_set_jolt_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParamJolt p_param, double p_value); + + bool generic_6dof_joint_get_jolt_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlagJolt p_flag) const; + void generic_6dof_joint_set_jolt_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlagJolt p_flag, bool p_enabled); + + float generic_6dof_joint_get_applied_force(RID p_joint); + float generic_6dof_joint_get_applied_torque(RID p_joint); +}; + +VARIANT_ENUM_CAST(JoltPhysicsServer3D::HingeJointParamJolt) +VARIANT_ENUM_CAST(JoltPhysicsServer3D::HingeJointFlagJolt) +VARIANT_ENUM_CAST(JoltPhysicsServer3D::SliderJointParamJolt) +VARIANT_ENUM_CAST(JoltPhysicsServer3D::SliderJointFlagJolt) +VARIANT_ENUM_CAST(JoltPhysicsServer3D::ConeTwistJointParamJolt) +VARIANT_ENUM_CAST(JoltPhysicsServer3D::ConeTwistJointFlagJolt) +VARIANT_ENUM_CAST(JoltPhysicsServer3D::G6DOFJointAxisParamJolt) +VARIANT_ENUM_CAST(JoltPhysicsServer3D::G6DOFJointAxisFlagJolt) + +#endif // JOLT_PHYSICS_SERVER_3D_H diff --git a/modules/jolt_physics/jolt_project_settings.cpp b/modules/jolt_physics/jolt_project_settings.cpp new file mode 100644 index 000000000000..5b530f23da99 --- /dev/null +++ b/modules/jolt_physics/jolt_project_settings.cpp @@ -0,0 +1,256 @@ +/**************************************************************************/ +/* jolt_project_settings.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_project_settings.h" + +#include "core/config/project_settings.h" +#include "core/error/error_macros.h" +#include "core/math/math_funcs.h" + +namespace { + +enum JoltJointWorldNode : int { + JOLT_JOINT_WORLD_NODE_A, + JOLT_JOINT_WORLD_NODE_B, +}; + +} // namespace + +void JoltProjectSettings::register_settings() { + GLOBAL_DEF(PropertyInfo(Variant::INT, "physics/jolt_physics_3d/simulation/velocity_steps", PROPERTY_HINT_RANGE, U"2,16,or_greater"), 10); + GLOBAL_DEF(PropertyInfo(Variant::INT, "physics/jolt_physics_3d/simulation/position_steps", PROPERTY_HINT_RANGE, U"1,16,or_greater"), 2); + GLOBAL_DEF(PropertyInfo(Variant::BOOL, "physics/jolt_physics_3d/simulation/use_enhanced_internal_edge_removal"), true); + GLOBAL_DEF(PropertyInfo(Variant::BOOL, "physics/jolt_physics_3d/simulation/areas_detect_static_bodies"), false); + GLOBAL_DEF(PropertyInfo(Variant::BOOL, "physics/jolt_physics_3d/simulation/generate_all_kinematic_contacts"), false); + GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/simulation/penetration_slop", PROPERTY_HINT_RANGE, U"0,1,0.00001,or_greater,suffix:m"), 0.02f); + GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/simulation/speculative_contact_distance", PROPERTY_HINT_RANGE, U"0,1,0.00001,or_greater,suffix:m"), 0.02f); + GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/simulation/baumgarte_stabilization_factor", PROPERTY_HINT_RANGE, U"0,1,0.01"), 0.2f); + GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/simulation/soft_body_point_radius", PROPERTY_HINT_RANGE, U"0,1,0.001,or_greater,suffix:m"), 0.01f); + GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/simulation/bounce_velocity_threshold", PROPERTY_HINT_NONE, U"suffix:m/s"), 1.0f); + GLOBAL_DEF(PropertyInfo(Variant::BOOL, "physics/jolt_physics_3d/simulation/allow_sleep"), true); + GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/simulation/sleep_velocity_threshold", PROPERTY_HINT_NONE, U"suffix:m/s"), 0.03f); + GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/simulation/sleep_time_threshold", PROPERTY_HINT_RANGE, U"0,5,0.01,or_greater,suffix:s"), 0.5f); + GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/simulation/continuous_cd_movement_threshold", PROPERTY_HINT_RANGE, U"0,1,0.01"), 0.75f); + GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/simulation/continuous_cd_max_penetration", PROPERTY_HINT_RANGE, U"0,1,0.01"), 0.25f); + GLOBAL_DEF(PropertyInfo(Variant::BOOL, "physics/jolt_physics_3d/simulation/body_pair_contact_cache_enabled"), true); + GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/simulation/body_pair_contact_cache_distance_threshold", PROPERTY_HINT_RANGE, U"0,0.01,0.00001,or_greater,suffix:m"), 0.001f); + GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/simulation/body_pair_contact_cache_angle_threshold", PROPERTY_HINT_RANGE, U"0,180,0.01,radians_as_degrees"), Math::deg_to_rad(2.0f)); + + GLOBAL_DEF(PropertyInfo(Variant::BOOL, "physics/jolt_physics_3d/queries/use_enhanced_internal_edge_removal"), false); + GLOBAL_DEF_RST(PropertyInfo(Variant::BOOL, "physics/jolt_physics_3d/queries/enable_ray_cast_face_index"), false); + + GLOBAL_DEF(PropertyInfo(Variant::BOOL, "physics/jolt_physics_3d/motion_queries/use_enhanced_internal_edge_removal"), true); + GLOBAL_DEF(PropertyInfo(Variant::INT, "physics/jolt_physics_3d/motion_queries/recovery_iterations", PROPERTY_HINT_RANGE, U"1,8,or_greater"), 4); + GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/motion_queries/recovery_amount", PROPERTY_HINT_RANGE, U"0,1,0.01"), 0.4f); + + GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/collisions/collision_margin_fraction", PROPERTY_HINT_RANGE, U"0,1,0.00001"), 0.08f); + GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/collisions/active_edge_threshold", PROPERTY_HINT_RANGE, U"0,90,0.01,radians_as_degrees"), Math::deg_to_rad(50.0f)); + + GLOBAL_DEF(PropertyInfo(Variant::INT, "physics/jolt_physics_3d/joints/world_node", PROPERTY_HINT_ENUM, U"Node A,Node B"), JOLT_JOINT_WORLD_NODE_A); + + GLOBAL_DEF(PropertyInfo(Variant::INT, "physics/jolt_physics_3d/limits/temporary_memory_buffer_size", PROPERTY_HINT_RANGE, U"1,32,or_greater,suffix:MiB"), 32); + GLOBAL_DEF_RST(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/limits/world_boundary_shape_size", PROPERTY_HINT_RANGE, U"2,2000,0.1,or_greater,suffix:m"), 2000.0f); + GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/limits/max_linear_velocity", PROPERTY_HINT_RANGE, U"0,500,0.01,or_greater,suffix:m/s"), 500.0f); + GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/limits/max_angular_velocity", PROPERTY_HINT_RANGE, U"0,2700,0.01,or_greater,radians_as_degrees,suffix:°/s"), Math::deg_to_rad(2700.0f)); + GLOBAL_DEF_RST(PropertyInfo(Variant::INT, "physics/jolt_physics_3d/limits/max_bodies", PROPERTY_HINT_RANGE, U"1,10240,or_greater"), 10240); + GLOBAL_DEF(PropertyInfo(Variant::INT, "physics/jolt_physics_3d/limits/max_body_pairs", PROPERTY_HINT_RANGE, U"8,65536,or_greater"), 65536); + GLOBAL_DEF(PropertyInfo(Variant::INT, "physics/jolt_physics_3d/limits/max_contact_constraints", PROPERTY_HINT_RANGE, U"8,20480,or_greater"), 20480); +} + +int JoltProjectSettings::get_simulation_velocity_steps() { + static const int value = GLOBAL_GET("physics/jolt_physics_3d/simulation/velocity_steps"); + return value; +} + +int JoltProjectSettings::get_simulation_position_steps() { + static const int value = GLOBAL_GET("physics/jolt_physics_3d/simulation/position_steps"); + return value; +} + +bool JoltProjectSettings::use_enhanced_internal_edge_removal_for_bodies() { + static const bool value = GLOBAL_GET("physics/jolt_physics_3d/simulation/use_enhanced_internal_edge_removal"); + return value; +} + +bool JoltProjectSettings::areas_detect_static_bodies() { + static const bool value = GLOBAL_GET("physics/jolt_physics_3d/simulation/areas_detect_static_bodies"); + return value; +} + +bool JoltProjectSettings::should_generate_all_kinematic_contacts() { + static const bool value = GLOBAL_GET("physics/jolt_physics_3d/simulation/generate_all_kinematic_contacts"); + return value; +} + +float JoltProjectSettings::get_penetration_slop() { + static const float value = GLOBAL_GET("physics/jolt_physics_3d/simulation/penetration_slop"); + return value; +} + +float JoltProjectSettings::get_speculative_contact_distance() { + static const float value = GLOBAL_GET("physics/jolt_physics_3d/simulation/speculative_contact_distance"); + return value; +} + +float JoltProjectSettings::get_baumgarte_stabilization_factor() { + static const float value = (float)GLOBAL_GET("physics/jolt_physics_3d/simulation/baumgarte_stabilization_factor"); + return value; +} + +float JoltProjectSettings::get_soft_body_point_radius() { + static const float value = GLOBAL_GET("physics/jolt_physics_3d/simulation/soft_body_point_radius"); + return value; +} + +float JoltProjectSettings::get_bounce_velocity_threshold() { + static const float value = GLOBAL_GET("physics/jolt_physics_3d/simulation/bounce_velocity_threshold"); + return value; +} + +bool JoltProjectSettings::is_sleep_allowed() { + static const bool value = GLOBAL_GET("physics/jolt_physics_3d/simulation/allow_sleep"); + return value; +} + +float JoltProjectSettings::get_sleep_velocity_threshold() { + static const float value = GLOBAL_GET("physics/jolt_physics_3d/simulation/sleep_velocity_threshold"); + return value; +} + +float JoltProjectSettings::get_sleep_time_threshold() { + static const float value = GLOBAL_GET("physics/jolt_physics_3d/simulation/sleep_time_threshold"); + return value; +} + +float JoltProjectSettings::get_ccd_movement_threshold() { + static const float value = (float)GLOBAL_GET("physics/jolt_physics_3d/simulation/continuous_cd_movement_threshold"); + return value; +} + +float JoltProjectSettings::get_ccd_max_penetration() { + static const float value = (float)GLOBAL_GET("physics/jolt_physics_3d/simulation/continuous_cd_max_penetration"); + return value; +} + +bool JoltProjectSettings::is_body_pair_contact_cache_enabled() { + static const bool value = GLOBAL_GET("physics/jolt_physics_3d/simulation/body_pair_contact_cache_enabled"); + return value; +} + +float JoltProjectSettings::get_body_pair_cache_distance_sq() { + static const float value = GLOBAL_GET("physics/jolt_physics_3d/simulation/body_pair_contact_cache_distance_threshold"); + static const float squared_value = value * value; + return squared_value; +} + +float JoltProjectSettings::get_body_pair_cache_angle_cos_div2() { + static const float value = Math::cos((float)GLOBAL_GET("physics/jolt_physics_3d/simulation/body_pair_contact_cache_angle_threshold") / 2.0f); + return value; +} + +bool JoltProjectSettings::use_enhanced_internal_edge_removal_for_queries() { + static const bool value = GLOBAL_GET("physics/jolt_physics_3d/queries/use_enhanced_internal_edge_removal"); + return value; +} + +bool JoltProjectSettings::enable_ray_cast_face_index() { + static const bool value = GLOBAL_GET("physics/jolt_physics_3d/queries/enable_ray_cast_face_index"); + return value; +} + +bool JoltProjectSettings::use_enhanced_internal_edge_removal_for_motion_queries() { + static const bool value = GLOBAL_GET("physics/jolt_physics_3d/motion_queries/use_enhanced_internal_edge_removal"); + return value; +} + +int JoltProjectSettings::get_motion_query_recovery_iterations() { + static const int value = GLOBAL_GET("physics/jolt_physics_3d/motion_queries/recovery_iterations"); + return value; +} + +float JoltProjectSettings::get_motion_query_recovery_amount() { + static const float value = (float)GLOBAL_GET("physics/jolt_physics_3d/motion_queries/recovery_amount"); + return value; +} + +float JoltProjectSettings::get_collision_margin_fraction() { + static const float value = GLOBAL_GET("physics/jolt_physics_3d/collisions/collision_margin_fraction"); + return value; +} + +float JoltProjectSettings::get_active_edge_threshold() { + static const float value = Math::cos((float)GLOBAL_GET("physics/jolt_physics_3d/collisions/active_edge_threshold")); + return value; +} + +bool JoltProjectSettings::use_joint_world_node_a() { + static const bool value = (int)GLOBAL_GET("physics/jolt_physics_3d/joints/world_node") == JOLT_JOINT_WORLD_NODE_A; + return value; +} + +int JoltProjectSettings::get_temp_memory_mib() { + static const int value = GLOBAL_GET("physics/jolt_physics_3d/limits/temporary_memory_buffer_size"); + return value; +} + +int64_t JoltProjectSettings::get_temp_memory_b() { + static const int64_t value = get_temp_memory_mib() * 1024 * 1024; + return value; +} + +float JoltProjectSettings::get_world_boundary_shape_size() { + static const float value = GLOBAL_GET("physics/jolt_physics_3d/limits/world_boundary_shape_size"); + return value; +} + +float JoltProjectSettings::get_max_linear_velocity() { + static const float value = GLOBAL_GET("physics/jolt_physics_3d/limits/max_linear_velocity"); + return value; +} + +float JoltProjectSettings::get_max_angular_velocity() { + static const float value = GLOBAL_GET("physics/jolt_physics_3d/limits/max_angular_velocity"); + return value; +} + +int JoltProjectSettings::get_max_bodies() { + static const int value = GLOBAL_GET("physics/jolt_physics_3d/limits/max_bodies"); + return value; +} + +int JoltProjectSettings::get_max_pairs() { + static const int value = GLOBAL_GET("physics/jolt_physics_3d/limits/max_body_pairs"); + return value; +} + +int JoltProjectSettings::get_max_contact_constraints() { + static const int value = GLOBAL_GET("physics/jolt_physics_3d/limits/max_contact_constraints"); + return value; +} diff --git a/modules/jolt_physics/jolt_project_settings.h b/modules/jolt_physics/jolt_project_settings.h new file mode 100644 index 000000000000..48710d41033c --- /dev/null +++ b/modules/jolt_physics/jolt_project_settings.h @@ -0,0 +1,81 @@ +/**************************************************************************/ +/* jolt_project_settings.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_PROJECT_SETTINGS_H +#define JOLT_PROJECT_SETTINGS_H + +#include + +class JoltProjectSettings { +public: + static void register_settings(); + + static int get_simulation_velocity_steps(); + static int get_simulation_position_steps(); + static bool use_enhanced_internal_edge_removal_for_bodies(); + static bool areas_detect_static_bodies(); + static bool should_generate_all_kinematic_contacts(); + static float get_penetration_slop(); + static float get_speculative_contact_distance(); + static float get_baumgarte_stabilization_factor(); + static float get_soft_body_point_radius(); + static float get_bounce_velocity_threshold(); + static bool is_sleep_allowed(); + static float get_sleep_velocity_threshold(); + static float get_sleep_time_threshold(); + static float get_ccd_movement_threshold(); + static float get_ccd_max_penetration(); + static bool is_body_pair_contact_cache_enabled(); + static float get_body_pair_cache_distance_sq(); + static float get_body_pair_cache_angle_cos_div2(); + + static bool use_enhanced_internal_edge_removal_for_queries(); + static bool enable_ray_cast_face_index(); + + static bool use_enhanced_internal_edge_removal_for_motion_queries(); + static int get_motion_query_recovery_iterations(); + static float get_motion_query_recovery_amount(); + + static float get_collision_margin_fraction(); + static float get_active_edge_threshold(); + + static bool use_joint_world_node_a(); + + static int get_temp_memory_mib(); + static int64_t get_temp_memory_b(); + static float get_world_boundary_shape_size(); + static float get_max_linear_velocity(); + static float get_max_angular_velocity(); + static int get_max_bodies(); + static int get_max_pairs(); + static int get_max_contact_constraints(); +}; + +#endif // JOLT_PROJECT_SETTINGS_H diff --git a/modules/jolt_physics/misc/jolt_stream_wrappers.h b/modules/jolt_physics/misc/jolt_stream_wrappers.h new file mode 100644 index 000000000000..0200a4515730 --- /dev/null +++ b/modules/jolt_physics/misc/jolt_stream_wrappers.h @@ -0,0 +1,81 @@ +/**************************************************************************/ +/* jolt_stream_wrappers.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_STREAM_WRAPPERS_H +#define JOLT_STREAM_WRAPPERS_H + +#ifdef DEBUG_ENABLED + +#include "core/io/file_access.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Core/StreamIn.h" +#include "Jolt/Core/StreamOut.h" + +class JoltStreamOutputWrapper final : public JPH::StreamOut { + Ref file_access; + +public: + explicit JoltStreamOutputWrapper(const Ref &p_file_access) : + file_access(p_file_access) {} + + virtual void WriteBytes(const void *p_data, size_t p_bytes) override { + file_access->store_buffer(static_cast(p_data), static_cast(p_bytes)); + } + + virtual bool IsFailed() const override { + return file_access->get_error() != OK; + } +}; + +class JoltStreamInputWrapper final : public JPH::StreamIn { + Ref file_access; + +public: + explicit JoltStreamInputWrapper(const Ref &p_file_access) : + file_access(p_file_access) {} + + virtual void ReadBytes(void *p_data, size_t p_bytes) override { + file_access->get_buffer(static_cast(p_data), static_cast(p_bytes)); + } + + virtual bool IsEOF() const override { + return file_access->eof_reached(); + } + + virtual bool IsFailed() const override { + return file_access->get_error() != OK; + } +}; + +#endif + +#endif // JOLT_STREAM_WRAPPERS_H diff --git a/modules/jolt_physics/misc/jolt_type_conversions.h b/modules/jolt_physics/misc/jolt_type_conversions.h new file mode 100644 index 000000000000..f735d6e06bc2 --- /dev/null +++ b/modules/jolt_physics/misc/jolt_type_conversions.h @@ -0,0 +1,147 @@ +/**************************************************************************/ +/* jolt_type_conversions.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_TYPE_CONVERSIONS_H +#define JOLT_TYPE_CONVERSIONS_H + +#include "core/math/aabb.h" +#include "core/math/color.h" +#include "core/math/plane.h" +#include "core/math/quaternion.h" +#include "core/math/transform_3d.h" +#include "core/string/ustring.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Core/Color.h" +#include "Jolt/Geometry/AABox.h" +#include "Jolt/Geometry/Plane.h" +#include "Jolt/Math/Mat44.h" +#include "Jolt/Math/Quat.h" +#include "Jolt/Math/Vec3.h" + +_FORCE_INLINE_ Vector3 to_godot(const JPH::Vec3 &p_vec) { + return Vector3((real_t)p_vec.GetX(), (real_t)p_vec.GetY(), (real_t)p_vec.GetZ()); +} + +_FORCE_INLINE_ Vector3 to_godot(const JPH::DVec3 &p_vec) { + return Vector3((real_t)p_vec.GetX(), (real_t)p_vec.GetY(), (real_t)p_vec.GetZ()); +} + +_FORCE_INLINE_ Basis to_godot(const JPH::Quat &p_quat) { + return Basis(Quaternion(p_quat.GetX(), p_quat.GetY(), p_quat.GetZ(), p_quat.GetW())); +} + +_FORCE_INLINE_ Transform3D to_godot(const JPH::Mat44 &p_mat) { + return Transform3D( + Vector3(p_mat(0, 0), p_mat(1, 0), p_mat(2, 0)), + Vector3(p_mat(0, 1), p_mat(1, 1), p_mat(2, 1)), + Vector3(p_mat(0, 2), p_mat(1, 2), p_mat(2, 2)), + Vector3(p_mat(0, 3), p_mat(1, 3), p_mat(2, 3))); +} + +_FORCE_INLINE_ Color to_godot(const JPH::Color &p_color) { + const float r = (float)p_color.r; + const float g = (float)p_color.g; + const float b = (float)p_color.b; + const float a = (float)p_color.a; + + return Color( + r == 0.0f ? 0.0f : 255.0f / r, + g == 0.0f ? 0.0f : 255.0f / g, + b == 0.0f ? 0.0f : 255.0f / b, + a == 0.0f ? 0.0f : 255.0f / a); +} + +_FORCE_INLINE_ String to_godot(const JPH::String &p_str) { + return String::utf8(p_str.c_str(), (int)p_str.length()); +} + +_FORCE_INLINE_ AABB to_godot(const JPH::AABox &p_aabb) { + return AABB(to_godot(p_aabb.mMin), to_godot(p_aabb.mMax - p_aabb.mMin)); +} + +_FORCE_INLINE_ Plane to_godot(const JPH::Plane &p_plane) { + return Plane(to_godot(p_plane.GetNormal()), (real_t)p_plane.GetConstant()); +} + +_FORCE_INLINE_ JPH::Vec3 to_jolt(const Vector3 &p_vec) { + return JPH::Vec3((float)p_vec.x, (float)p_vec.y, (float)p_vec.z); +} + +_FORCE_INLINE_ JPH::Quat to_jolt(const Basis &p_basis) { + const Quaternion quat = p_basis.get_quaternion().normalized(); + return JPH::Quat((float)quat.x, (float)quat.y, (float)quat.z, (float)quat.w); +} + +_FORCE_INLINE_ JPH::Mat44 to_jolt(const Transform3D &p_transform) { + const Basis &b = p_transform.basis; + const Vector3 &o = p_transform.origin; + + return JPH::Mat44( + JPH::Vec4(b[0][0], b[1][0], b[2][0], 0.0f), + JPH::Vec4(b[0][1], b[1][1], b[2][1], 0.0f), + JPH::Vec4(b[0][2], b[1][2], b[2][2], 0.0f), + JPH::Vec3(o.x, o.y, o.z)); +} + +_FORCE_INLINE_ JPH::Color to_jolt(const Color &p_color) { + return JPH::Color((JPH::uint32)p_color.to_abgr32()); +} + +_FORCE_INLINE_ JPH::String to_jolt(const String &p_str) { + const CharString str_utf8 = p_str.utf8(); + return JPH::String(str_utf8.get_data(), (size_t)str_utf8.length()); +} + +_FORCE_INLINE_ JPH::AABox to_jolt(const AABB &p_aabb) { + return JPH::AABox(to_jolt(p_aabb.position), to_jolt(p_aabb.position + p_aabb.size)); +} + +_FORCE_INLINE_ JPH::Plane to_jolt(const Plane &p_plane) { + return JPH::Plane(to_jolt(p_plane.normal), (float)p_plane.d); +} + +_FORCE_INLINE_ JPH::RVec3 to_jolt_r(const Vector3 &p_vec) { + return JPH::RVec3(p_vec.x, p_vec.y, p_vec.z); +} + +_FORCE_INLINE_ JPH::RMat44 to_jolt_r(const Transform3D &p_transform) { + const Basis &b = p_transform.basis; + const Vector3 &o = p_transform.origin; + + return JPH::RMat44( + JPH::Vec4(b[0][0], b[1][0], b[2][0], 0.0f), + JPH::Vec4(b[0][1], b[1][1], b[2][1], 0.0f), + JPH::Vec4(b[0][2], b[1][2], b[2][2], 0.0f), + JPH::RVec3(o.x, o.y, o.z)); +} + +#endif // JOLT_TYPE_CONVERSIONS_H diff --git a/modules/jolt_physics/objects/jolt_area_3d.cpp b/modules/jolt_physics/objects/jolt_area_3d.cpp new file mode 100644 index 000000000000..84313477b11c --- /dev/null +++ b/modules/jolt_physics/objects/jolt_area_3d.cpp @@ -0,0 +1,655 @@ +/**************************************************************************/ +/* jolt_area_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_area_3d.h" + +#include "../jolt_project_settings.h" +#include "../misc/jolt_type_conversions.h" +#include "../shapes/jolt_shape_3d.h" +#include "../spaces/jolt_broad_phase_layer.h" +#include "../spaces/jolt_space_3d.h" +#include "jolt_body_3d.h" +#include "jolt_group_filter.h" +#include "jolt_soft_body_3d.h" + +#include "core/error/error_macros.h" + +namespace { + +constexpr double DEFAULT_WIND_FORCE_MAGNITUDE = 0.0; +constexpr double DEFAULT_WIND_ATTENUATION_FACTOR = 0.0; + +const Vector3 DEFAULT_WIND_SOURCE = Vector3(); +const Vector3 DEFAULT_WIND_DIRECTION = Vector3(); + +} // namespace + +JPH::BroadPhaseLayer JoltArea3D::_get_broad_phase_layer() const { + return monitorable ? JoltBroadPhaseLayer::AREA_DETECTABLE : JoltBroadPhaseLayer::AREA_UNDETECTABLE; +} + +JPH::ObjectLayer JoltArea3D::_get_object_layer() const { + ERR_FAIL_NULL_V(space, 0); + + return space->map_to_object_layer(_get_broad_phase_layer(), collision_layer, collision_mask); +} + +void JoltArea3D::_add_to_space() { + jolt_shape = build_shape(); + + JPH::CollisionGroup::GroupID group_id = 0; + JPH::CollisionGroup::SubGroupID sub_group_id = 0; + JoltGroupFilter::encode_object(this, group_id, sub_group_id); + + jolt_settings->mUserData = reinterpret_cast(this); + jolt_settings->mObjectLayer = _get_object_layer(); + jolt_settings->mCollisionGroup = JPH::CollisionGroup(nullptr, group_id, sub_group_id); + jolt_settings->mMotionType = _get_motion_type(); + jolt_settings->mIsSensor = true; + jolt_settings->mUseManifoldReduction = false; + + if (JoltProjectSettings::areas_detect_static_bodies()) { + jolt_settings->mCollideKinematicVsNonDynamic = true; + } + + jolt_settings->SetShape(build_shape()); + + const JPH::BodyID new_jolt_id = space->add_rigid_body(*this, *jolt_settings); + if (!new_jolt_id.IsInvalid()) { + jolt_id = new_jolt_id; + } + + delete jolt_settings; + jolt_settings = nullptr; +} + +void JoltArea3D::_add_shape_pair(Overlap &p_overlap, const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) { + const JoltReadableBody3D other_jolt_body = space->read_body(p_body_id); + const JoltShapedObject3D *other_object = other_jolt_body.as_shaped(); + ERR_FAIL_NULL(other_object); + + p_overlap.rid = other_object->get_rid(); + p_overlap.instance_id = other_object->get_instance_id(); + + ShapeIndexPair &shape_indices = p_overlap.shape_pairs[{ p_other_shape_id, p_self_shape_id }]; + + shape_indices.other = other_object->find_shape_index(p_other_shape_id); + shape_indices.self = find_shape_index(p_self_shape_id); + + p_overlap.pending_added.push_back(shape_indices); +} + +bool JoltArea3D::_remove_shape_pair(Overlap &p_overlap, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) { + HashMap::Iterator shape_pair = p_overlap.shape_pairs.find(ShapeIDPair(p_other_shape_id, p_self_shape_id)); + + if (shape_pair == p_overlap.shape_pairs.end()) { + return false; + } + + p_overlap.pending_removed.push_back(shape_pair->value); + p_overlap.shape_pairs.remove(shape_pair); + + return true; +} + +void JoltArea3D::_flush_events(OverlapsById &p_objects, const Callable &p_callback) { + for (OverlapsById::Iterator E = p_objects.begin(); E;) { + Overlap &overlap = E->value; + + if (p_callback.is_valid()) { + for (ShapeIndexPair &shape_indices : overlap.pending_removed) { + _report_event(p_callback, PhysicsServer3D::AREA_BODY_REMOVED, overlap.rid, overlap.instance_id, shape_indices.other, shape_indices.self); + } + + for (ShapeIndexPair &shape_indices : overlap.pending_added) { + _report_event(p_callback, PhysicsServer3D::AREA_BODY_ADDED, overlap.rid, overlap.instance_id, shape_indices.other, shape_indices.self); + } + } + + overlap.pending_removed.clear(); + overlap.pending_added.clear(); + + OverlapsById::Iterator next = E; + ++next; + + if (overlap.shape_pairs.is_empty()) { + p_objects.remove(E); + } + + E = next; + } +} + +void JoltArea3D::_report_event(const Callable &p_callback, PhysicsServer3D::AreaBodyStatus p_status, const RID &p_other_rid, ObjectID p_other_instance_id, int p_other_shape_index, int p_self_shape_index) const { + ERR_FAIL_COND(!p_callback.is_valid()); + + static thread_local Array arguments = []() { + Array array; + array.resize(5); + return array; + }(); + + arguments[0] = p_status; + arguments[1] = p_other_rid; + arguments[2] = p_other_instance_id; + arguments[3] = p_other_shape_index; + arguments[4] = p_self_shape_index; + + p_callback.callv(arguments); +} + +void JoltArea3D::_notify_body_entered(const JPH::BodyID &p_body_id) { + const JoltReadableBody3D jolt_body = space->read_body(p_body_id); + + JoltBody3D *body = jolt_body.as_body(); + if (unlikely(body == nullptr)) { + return; + } + + body->add_area(this); +} + +void JoltArea3D::_notify_body_exited(const JPH::BodyID &p_body_id) { + const JoltReadableBody3D jolt_body = space->read_body(p_body_id); + + JoltBody3D *body = jolt_body.as_body(); + if (unlikely(body == nullptr)) { + return; + } + + body->remove_area(this); +} + +void JoltArea3D::_force_bodies_entered() { + for (auto &[id, body] : bodies_by_id) { + for (const auto &[id_pair, index_pair] : body.shape_pairs) { + body.pending_removed.erase(index_pair); + body.pending_added.push_back(index_pair); + } + } +} + +void JoltArea3D::_force_bodies_exited(bool p_remove) { + for (auto &[id, body] : bodies_by_id) { + for (const auto &[id_pair, index_pair] : body.shape_pairs) { + body.pending_added.erase(index_pair); + body.pending_removed.push_back(index_pair); + } + + if (p_remove) { + body.shape_pairs.clear(); + _notify_body_exited(id); + } + } +} + +void JoltArea3D::_force_areas_entered() { + for (auto &[id, area] : areas_by_id) { + for (const auto &[id_pair, index_pair] : area.shape_pairs) { + area.pending_removed.erase(index_pair); + area.pending_added.push_back(index_pair); + } + } +} + +void JoltArea3D::_force_areas_exited(bool p_remove) { + for (auto &[id, area] : areas_by_id) { + for (const auto &[id_pair, index_pair] : area.shape_pairs) { + area.pending_added.erase(index_pair); + area.pending_removed.push_back(index_pair); + } + + if (p_remove) { + area.shape_pairs.clear(); + } + } +} + +void JoltArea3D::_update_group_filter() { + if (!in_space()) { + return; + } + + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + body->GetCollisionGroup().SetGroupFilter(JoltGroupFilter::instance); +} + +void JoltArea3D::_update_default_gravity() { + if (is_default_area()) { + space->get_physics_system().SetGravity(to_jolt(gravity_vector) * gravity); + } +} + +void JoltArea3D::_space_changing() { + JoltShapedObject3D::_space_changing(); + + if (space != nullptr) { + // Ideally we would rely on our contact listener to report all the exits when we move + // between (or out of) spaces, but because our Jolt body is going to be destroyed when we + // leave this space the contact listener won't be able to retrieve the corresponding area + // and as such cannot report any exits, so we're forced to do it manually instead. + _force_bodies_exited(true); + _force_areas_exited(true); + } +} + +void JoltArea3D::_space_changed() { + JoltShapedObject3D::_space_changed(); + + _update_group_filter(); + _update_default_gravity(); +} + +void JoltArea3D::_body_monitoring_changed() { + if (has_body_monitor_callback()) { + _force_bodies_entered(); + } else { + _force_bodies_exited(false); + } +} + +void JoltArea3D::_area_monitoring_changed() { + if (has_area_monitor_callback()) { + _force_areas_entered(); + } else { + _force_areas_exited(false); + } +} + +void JoltArea3D::_monitorable_changed() { + _update_object_layer(); +} + +void JoltArea3D::_gravity_changed() { + _update_default_gravity(); +} + +JoltArea3D::JoltArea3D() : + JoltShapedObject3D(OBJECT_TYPE_AREA) { +} + +bool JoltArea3D::is_default_area() const { + return space != nullptr && space->get_default_area() == this; +} + +void JoltArea3D::set_default_area(bool p_value) { + if (p_value) { + _update_default_gravity(); + } +} + +void JoltArea3D::set_transform(Transform3D p_transform) { + JOLT_ENSURE_SCALE_NOT_ZERO(p_transform, vformat("An invalid transform was passed to area '%s'.", to_string())); + + const Vector3 new_scale = p_transform.basis.get_scale(); + + // Ideally we would do an exact comparison here, but due to floating-point precision this would be invalidated very often. + if (!scale.is_equal_approx(new_scale)) { + scale = new_scale; + _shapes_changed(); + } + + p_transform.basis.orthonormalize(); + + if (!in_space()) { + jolt_settings->mPosition = to_jolt_r(p_transform.origin); + jolt_settings->mRotation = to_jolt(p_transform.basis); + } else { + space->get_body_iface().SetPositionAndRotation(jolt_id, to_jolt_r(p_transform.origin), to_jolt(p_transform.basis), JPH::EActivation::DontActivate); + } +} + +Variant JoltArea3D::get_param(PhysicsServer3D::AreaParameter p_param) const { + switch (p_param) { + case PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE: { + return get_gravity_mode(); + } + case PhysicsServer3D::AREA_PARAM_GRAVITY: { + return get_gravity(); + } + case PhysicsServer3D::AREA_PARAM_GRAVITY_VECTOR: { + return get_gravity_vector(); + } + case PhysicsServer3D::AREA_PARAM_GRAVITY_IS_POINT: { + return is_point_gravity(); + } + case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_UNIT_DISTANCE: { + return get_point_gravity_distance(); + } + case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE: { + return get_linear_damp_mode(); + } + case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP: { + return get_linear_damp(); + } + case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE: { + return get_angular_damp_mode(); + } + case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP: { + return get_angular_damp(); + } + case PhysicsServer3D::AREA_PARAM_PRIORITY: { + return get_priority(); + } + case PhysicsServer3D::AREA_PARAM_WIND_FORCE_MAGNITUDE: { + return DEFAULT_WIND_FORCE_MAGNITUDE; + } + case PhysicsServer3D::AREA_PARAM_WIND_SOURCE: { + return DEFAULT_WIND_SOURCE; + } + case PhysicsServer3D::AREA_PARAM_WIND_DIRECTION: { + return DEFAULT_WIND_DIRECTION; + } + case PhysicsServer3D::AREA_PARAM_WIND_ATTENUATION_FACTOR: { + return DEFAULT_WIND_ATTENUATION_FACTOR; + } + default: { + ERR_FAIL_V_MSG(Variant(), vformat("Unhandled area parameter: '%d'. This should not happen. Please report this.", p_param)); + } + } +} + +void JoltArea3D::set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value) { + switch (p_param) { + case PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE: { + set_gravity_mode((OverrideMode)(int)p_value); + } break; + case PhysicsServer3D::AREA_PARAM_GRAVITY: { + set_gravity(p_value); + } break; + case PhysicsServer3D::AREA_PARAM_GRAVITY_VECTOR: { + set_gravity_vector(p_value); + } break; + case PhysicsServer3D::AREA_PARAM_GRAVITY_IS_POINT: { + set_point_gravity(p_value); + } break; + case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_UNIT_DISTANCE: { + set_point_gravity_distance(p_value); + } break; + case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE: { + set_linear_damp_mode((OverrideMode)(int)p_value); + } break; + case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP: { + set_area_linear_damp(p_value); + } break; + case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE: { + set_angular_damp_mode((OverrideMode)(int)p_value); + } break; + case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP: { + set_area_angular_damp(p_value); + } break; + case PhysicsServer3D::AREA_PARAM_PRIORITY: { + set_priority(p_value); + } break; + case PhysicsServer3D::AREA_PARAM_WIND_FORCE_MAGNITUDE: { + if (!Math::is_equal_approx((double)p_value, DEFAULT_WIND_FORCE_MAGNITUDE)) { + WARN_PRINT(vformat("Invalid wind force magnitude for '%s'. Area wind force magnitude is not supported when using Jolt Physics. Any such value will be ignored.", to_string())); + } + } break; + case PhysicsServer3D::AREA_PARAM_WIND_SOURCE: { + if (!((Vector3)p_value).is_equal_approx(DEFAULT_WIND_SOURCE)) { + WARN_PRINT(vformat("Invalid wind source for '%s'. Area wind source is not supported when using Jolt Physics. Any such value will be ignored.", to_string())); + } + } break; + case PhysicsServer3D::AREA_PARAM_WIND_DIRECTION: { + if (!((Vector3)p_value).is_equal_approx(DEFAULT_WIND_DIRECTION)) { + WARN_PRINT(vformat("Invalid wind direction for '%s'. Area wind direction is not supported when using Jolt Physics. Any such value will be ignored.", to_string())); + } + } break; + case PhysicsServer3D::AREA_PARAM_WIND_ATTENUATION_FACTOR: { + if (!Math::is_equal_approx((double)p_value, DEFAULT_WIND_ATTENUATION_FACTOR)) { + WARN_PRINT(vformat("Invalid wind attenuation for '%s'. Area wind attenuation is not supported when using Jolt Physics. Any such value will be ignored.", to_string())); + } + } break; + default: { + ERR_FAIL_MSG(vformat("Unhandled area parameter: '%d'. This should not happen. Please report this.", p_param)); + } break; + } +} + +void JoltArea3D::set_body_monitor_callback(const Callable &p_callback) { + if (p_callback == body_monitor_callback) { + return; + } + + body_monitor_callback = p_callback; + + _body_monitoring_changed(); +} + +void JoltArea3D::set_area_monitor_callback(const Callable &p_callback) { + if (p_callback == area_monitor_callback) { + return; + } + + area_monitor_callback = p_callback; + + _area_monitoring_changed(); +} + +void JoltArea3D::set_monitorable(bool p_monitorable) { + if (p_monitorable == monitorable) { + return; + } + + monitorable = p_monitorable; + + _monitorable_changed(); +} + +bool JoltArea3D::can_monitor(const JoltBody3D &p_other) const { + return (collision_mask & p_other.get_collision_layer()) != 0; +} + +bool JoltArea3D::can_monitor(const JoltSoftBody3D &p_other) const { + return false; +} + +bool JoltArea3D::can_monitor(const JoltArea3D &p_other) const { + return p_other.is_monitorable() && (collision_mask & p_other.get_collision_layer()) != 0; +} + +bool JoltArea3D::can_interact_with(const JoltBody3D &p_other) const { + return can_monitor(p_other); +} + +bool JoltArea3D::can_interact_with(const JoltSoftBody3D &p_other) const { + return false; +} + +bool JoltArea3D::can_interact_with(const JoltArea3D &p_other) const { + return can_monitor(p_other) || p_other.can_monitor(*this); +} + +Vector3 JoltArea3D::get_velocity_at_position(const Vector3 &p_position) const { + return { 0.0f, 0.0f, 0.0f }; +} + +void JoltArea3D::set_point_gravity(bool p_enabled) { + if (point_gravity == p_enabled) { + return; + } + + point_gravity = p_enabled; + + _gravity_changed(); +} + +void JoltArea3D::set_gravity(float p_gravity) { + if (gravity == p_gravity) { + return; + } + + gravity = p_gravity; + + _gravity_changed(); +} + +void JoltArea3D::set_point_gravity_distance(float p_distance) { + if (point_gravity_distance == p_distance) { + return; + } + + point_gravity_distance = p_distance; + + _gravity_changed(); +} + +void JoltArea3D::set_gravity_mode(OverrideMode p_mode) { + if (gravity_mode == p_mode) { + return; + } + + gravity_mode = p_mode; + + _gravity_changed(); +} + +void JoltArea3D::set_gravity_vector(const Vector3 &p_vector) { + if (gravity_vector == p_vector) { + return; + } + + gravity_vector = p_vector; + + _gravity_changed(); +} + +Vector3 JoltArea3D::compute_gravity(const Vector3 &p_position) const { + if (!point_gravity) { + return gravity_vector * gravity; + } + + const Vector3 point = get_transform_scaled().xform(gravity_vector); + const Vector3 to_point = point - p_position; + const real_t to_point_dist_sq = MAX(to_point.length_squared(), (real_t)CMP_EPSILON); + const Vector3 to_point_dir = to_point / Math::sqrt(to_point_dist_sq); + + if (point_gravity_distance == 0.0f) { + return to_point_dir * gravity; + } + + const float gravity_dist_sq = point_gravity_distance * point_gravity_distance; + + return to_point_dir * (gravity * gravity_dist_sq / to_point_dist_sq); +} + +void JoltArea3D::body_shape_entered(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) { + Overlap &overlap = bodies_by_id[p_body_id]; + + if (overlap.shape_pairs.is_empty()) { + _notify_body_entered(p_body_id); + } + + _add_shape_pair(overlap, p_body_id, p_other_shape_id, p_self_shape_id); +} + +bool JoltArea3D::body_shape_exited(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) { + Overlap *overlap = bodies_by_id.getptr(p_body_id); + + if (overlap == nullptr) { + return false; + } + + if (!_remove_shape_pair(*overlap, p_other_shape_id, p_self_shape_id)) { + return false; + } + + if (overlap->shape_pairs.is_empty()) { + _notify_body_exited(p_body_id); + } + + return true; +} + +void JoltArea3D::area_shape_entered(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) { + _add_shape_pair(areas_by_id[p_body_id], p_body_id, p_other_shape_id, p_self_shape_id); +} + +bool JoltArea3D::area_shape_exited(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) { + Overlap *overlap = areas_by_id.getptr(p_body_id); + + if (overlap == nullptr) { + return false; + } + + return _remove_shape_pair(*overlap, p_other_shape_id, p_self_shape_id); +} + +bool JoltArea3D::shape_exited(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) { + return body_shape_exited(p_body_id, p_other_shape_id, p_self_shape_id) || area_shape_exited(p_body_id, p_other_shape_id, p_self_shape_id); +} + +void JoltArea3D::body_exited(const JPH::BodyID &p_body_id, bool p_notify) { + Overlap *overlap = bodies_by_id.getptr(p_body_id); + if (unlikely(overlap == nullptr)) { + return; + } + + if (unlikely(overlap->shape_pairs.is_empty())) { + return; + } + + for (auto &[id_pair, index_pair] : overlap->shape_pairs) { + overlap->pending_added.erase(index_pair); + overlap->pending_removed.push_back(index_pair); + } + + overlap->shape_pairs.clear(); + + if (p_notify) { + _notify_body_exited(p_body_id); + } +} + +void JoltArea3D::area_exited(const JPH::BodyID &p_body_id) { + Overlap *overlap = areas_by_id.getptr(p_body_id); + if (unlikely(overlap == nullptr)) { + return; + } + + if (unlikely(overlap->shape_pairs.is_empty())) { + return; + } + + for (const auto &[id_pair, index_pair] : overlap->shape_pairs) { + overlap->pending_added.erase(index_pair); + overlap->pending_removed.push_back(index_pair); + } + + overlap->shape_pairs.clear(); +} + +void JoltArea3D::call_queries(JPH::Body &p_jolt_body) { + _flush_events(bodies_by_id, body_monitor_callback); + _flush_events(areas_by_id, area_monitor_callback); +} diff --git a/modules/jolt_physics/objects/jolt_area_3d.h b/modules/jolt_physics/objects/jolt_area_3d.h new file mode 100644 index 000000000000..2c8a67b4c2aa --- /dev/null +++ b/modules/jolt_physics/objects/jolt_area_3d.h @@ -0,0 +1,228 @@ +/**************************************************************************/ +/* jolt_area_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_AREA_3D_H +#define JOLT_AREA_3D_H + +#include "jolt_shaped_object_3d.h" + +#include "core/templates/hashfuncs.h" +#include "core/variant/callable.h" +#include "servers/physics_server_3d.h" + +class JoltBody3D; +class JoltSoftBody3D; + +class JoltArea3D final : public JoltShapedObject3D { +public: + typedef PhysicsServer3D::AreaSpaceOverrideMode OverrideMode; + +private: + struct BodyIDHasher { + static uint32_t hash(const JPH::BodyID &p_id) { return hash_fmix32(p_id.GetIndexAndSequenceNumber()); } + }; + + struct ShapeIDPair { + JPH::SubShapeID other; + JPH::SubShapeID self; + + ShapeIDPair(JPH::SubShapeID p_other, JPH::SubShapeID p_self) : + other(p_other), self(p_self) {} + + static uint32_t hash(const ShapeIDPair &p_pair) { + uint32_t hash = hash_murmur3_one_32(p_pair.other.GetValue()); + hash = hash_murmur3_one_32(p_pair.self.GetValue(), hash); + return hash_fmix32(hash); + } + + friend bool operator==(const ShapeIDPair &p_lhs, const ShapeIDPair &p_rhs) { + return (p_lhs.other == p_rhs.other) && (p_lhs.self == p_rhs.self); + } + }; + + struct ShapeIndexPair { + int other = -1; + int self = -1; + + ShapeIndexPair() = default; + + ShapeIndexPair(int p_other, int p_self) : + other(p_other), self(p_self) {} + + friend bool operator==(const ShapeIndexPair &p_lhs, const ShapeIndexPair &p_rhs) { + return (p_lhs.other == p_rhs.other) && (p_lhs.self == p_rhs.self); + } + }; + + struct Overlap { + HashMap shape_pairs; + LocalVector pending_added; + LocalVector pending_removed; + RID rid; + ObjectID instance_id; + }; + + typedef HashMap OverlapsById; + + OverlapsById bodies_by_id; + OverlapsById areas_by_id; + + Vector3 gravity_vector = Vector3(0, -1, 0); + + Callable body_monitor_callback; + Callable area_monitor_callback; + + float priority = 0.0f; + float gravity = 9.8f; + float point_gravity_distance = 0.0f; + float linear_damp = 0.1f; + float angular_damp = 0.1f; + + OverrideMode gravity_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; + OverrideMode linear_damp_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; + OverrideMode angular_damp_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; + + bool monitorable = false; + bool point_gravity = false; + + virtual JPH::BroadPhaseLayer _get_broad_phase_layer() const override; + virtual JPH::ObjectLayer _get_object_layer() const override; + + virtual JPH::EMotionType _get_motion_type() const override { return JPH::EMotionType::Kinematic; } + + virtual void _add_to_space() override; + + void _add_shape_pair(Overlap &p_overlap, const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id); + bool _remove_shape_pair(Overlap &p_overlap, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id); + + void _flush_events(OverlapsById &p_objects, const Callable &p_callback); + + void _report_event(const Callable &p_callback, PhysicsServer3D::AreaBodyStatus p_status, const RID &p_other_rid, ObjectID p_other_instance_id, int p_other_shape_index, int p_self_shape_index) const; + + void _notify_body_entered(const JPH::BodyID &p_body_id); + void _notify_body_exited(const JPH::BodyID &p_body_id); + + void _force_bodies_entered(); + void _force_bodies_exited(bool p_remove); + + void _force_areas_entered(); + void _force_areas_exited(bool p_remove); + + void _update_group_filter(); + void _update_default_gravity(); + + virtual void _space_changing() override; + virtual void _space_changed() override; + void _body_monitoring_changed(); + void _area_monitoring_changed(); + void _monitorable_changed(); + void _gravity_changed(); + +public: + JoltArea3D(); + + bool is_default_area() const; + void set_default_area(bool p_value); + + void set_transform(Transform3D p_transform); + + Variant get_param(PhysicsServer3D::AreaParameter p_param) const; + void set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value); + + bool has_body_monitor_callback() const { return body_monitor_callback.is_valid(); } + void set_body_monitor_callback(const Callable &p_callback); + + bool has_area_monitor_callback() const { return area_monitor_callback.is_valid(); } + void set_area_monitor_callback(const Callable &p_callback); + + bool is_monitorable() const { return monitorable; } + void set_monitorable(bool p_monitorable); + + bool can_monitor(const JoltBody3D &p_other) const; + bool can_monitor(const JoltSoftBody3D &p_other) const; + bool can_monitor(const JoltArea3D &p_other) const; + + virtual bool can_interact_with(const JoltBody3D &p_other) const override; + virtual bool can_interact_with(const JoltSoftBody3D &p_other) const override; + virtual bool can_interact_with(const JoltArea3D &p_other) const override; + + virtual Vector3 get_velocity_at_position(const Vector3 &p_position) const override; + + virtual bool reports_contacts() const override { return false; } + + bool is_point_gravity() const { return point_gravity; } + void set_point_gravity(bool p_enabled); + + float get_priority() const { return priority; } + void set_priority(float p_priority) { priority = p_priority; } + + float get_gravity() const { return gravity; } + void set_gravity(float p_gravity); + + float get_point_gravity_distance() const { return point_gravity_distance; } + void set_point_gravity_distance(float p_distance); + + float get_linear_damp() const { return linear_damp; } + void set_area_linear_damp(float p_damp) { linear_damp = p_damp; } + + float get_angular_damp() const { return angular_damp; } + void set_area_angular_damp(float p_damp) { angular_damp = p_damp; } + + OverrideMode get_gravity_mode() const { return gravity_mode; } + void set_gravity_mode(OverrideMode p_mode); + + OverrideMode get_linear_damp_mode() const { return linear_damp_mode; } + void set_linear_damp_mode(OverrideMode p_mode) { linear_damp_mode = p_mode; } + + OverrideMode get_angular_damp_mode() const { return angular_damp_mode; } + void set_angular_damp_mode(OverrideMode p_mode) { angular_damp_mode = p_mode; } + + Vector3 get_gravity_vector() const { return gravity_vector; } + void set_gravity_vector(const Vector3 &p_vector); + + Vector3 compute_gravity(const Vector3 &p_position) const; + + void body_shape_entered(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id); + bool body_shape_exited(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id); + + void area_shape_entered(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id); + bool area_shape_exited(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id); + + bool shape_exited(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id); + void body_exited(const JPH::BodyID &p_body_id, bool p_notify = true); + void area_exited(const JPH::BodyID &p_body_id); + + void call_queries(JPH::Body &p_jolt_body); + + virtual bool has_custom_center_of_mass() const override { return false; } + virtual Vector3 get_center_of_mass_custom() const override { return { 0, 0, 0 }; } +}; + +#endif // JOLT_AREA_3D_H diff --git a/modules/jolt_physics/objects/jolt_body_3d.cpp b/modules/jolt_physics/objects/jolt_body_3d.cpp new file mode 100644 index 000000000000..af0037538ee6 --- /dev/null +++ b/modules/jolt_physics/objects/jolt_body_3d.cpp @@ -0,0 +1,1439 @@ +/**************************************************************************/ +/* jolt_body_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_body_3d.h" + +#include "../joints/jolt_joint_3d.h" +#include "../jolt_project_settings.h" +#include "../misc/jolt_type_conversions.h" +#include "../shapes/jolt_shape_3d.h" +#include "../spaces/jolt_broad_phase_layer.h" +#include "../spaces/jolt_space_3d.h" +#include "jolt_area_3d.h" +#include "jolt_group_filter.h" +#include "jolt_physics_direct_body_state_3d.h" +#include "jolt_soft_body_3d.h" + +#include "core/error/error_macros.h" + +namespace { + +template +bool integrate(TValue &p_value, PhysicsServer3D::AreaSpaceOverrideMode p_mode, TGetter &&p_getter) { + switch (p_mode) { + case PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED: { + return false; + } + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: { + p_value += p_getter(); + return false; + } + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + p_value += p_getter(); + return true; + } + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: { + p_value = p_getter(); + return true; + } + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { + p_value = p_getter(); + return false; + } + default: { + ERR_FAIL_V_MSG(false, vformat("Unhandled override mode: '%d'. This should not happen. Please report this.", p_mode)); + } + } +} + +} // namespace + +JPH::BroadPhaseLayer JoltBody3D::_get_broad_phase_layer() const { + switch (mode) { + case PhysicsServer3D::BODY_MODE_STATIC: { + return _is_big() ? JoltBroadPhaseLayer::BODY_STATIC_BIG : JoltBroadPhaseLayer::BODY_STATIC; + } + case PhysicsServer3D::BODY_MODE_KINEMATIC: + case PhysicsServer3D::BODY_MODE_RIGID: + case PhysicsServer3D::BODY_MODE_RIGID_LINEAR: { + return JoltBroadPhaseLayer::BODY_DYNAMIC; + } + default: { + ERR_FAIL_V_MSG(JoltBroadPhaseLayer::BODY_STATIC, vformat("Unhandled body mode: '%d'. This should not happen. Please report this.", mode)); + } + } +} + +JPH::ObjectLayer JoltBody3D::_get_object_layer() const { + ERR_FAIL_NULL_V(space, 0); + + return space->map_to_object_layer(_get_broad_phase_layer(), collision_layer, collision_mask); +} + +JPH::EMotionType JoltBody3D::_get_motion_type() const { + switch (mode) { + case PhysicsServer3D::BODY_MODE_STATIC: { + return JPH::EMotionType::Static; + } + case PhysicsServer3D::BODY_MODE_KINEMATIC: { + return JPH::EMotionType::Kinematic; + } + case PhysicsServer3D::BODY_MODE_RIGID: + case PhysicsServer3D::BODY_MODE_RIGID_LINEAR: { + return JPH::EMotionType::Dynamic; + } + default: { + ERR_FAIL_V_MSG(JPH::EMotionType::Static, vformat("Unhandled body mode: '%d'. This should not happen. Please report this.", mode)); + } + } +} + +void JoltBody3D::_add_to_space() { + jolt_shape = build_shape(); + + JPH::CollisionGroup::GroupID group_id = 0; + JPH::CollisionGroup::SubGroupID sub_group_id = 0; + JoltGroupFilter::encode_object(this, group_id, sub_group_id); + + jolt_settings->mUserData = reinterpret_cast(this); + jolt_settings->mObjectLayer = _get_object_layer(); + jolt_settings->mCollisionGroup = JPH::CollisionGroup(nullptr, group_id, sub_group_id); + jolt_settings->mMotionType = _get_motion_type(); + jolt_settings->mAllowedDOFs = _calculate_allowed_dofs(); + jolt_settings->mAllowDynamicOrKinematic = true; + jolt_settings->mCollideKinematicVsNonDynamic = reports_all_kinematic_contacts(); + jolt_settings->mUseManifoldReduction = !reports_contacts(); + jolt_settings->mLinearDamping = 0.0f; + jolt_settings->mAngularDamping = 0.0f; + jolt_settings->mMaxLinearVelocity = JoltProjectSettings::get_max_linear_velocity(); + jolt_settings->mMaxAngularVelocity = JoltProjectSettings::get_max_angular_velocity(); + + if (JoltProjectSettings::use_enhanced_internal_edge_removal_for_bodies()) { + jolt_settings->mEnhancedInternalEdgeRemoval = true; + } + + jolt_settings->mOverrideMassProperties = JPH::EOverrideMassProperties::MassAndInertiaProvided; + jolt_settings->mMassPropertiesOverride = _calculate_mass_properties(); + + jolt_settings->SetShape(jolt_shape); + + const JPH::BodyID new_jolt_id = space->add_rigid_body(*this, *jolt_settings, sleep_initially); + if (!new_jolt_id.IsInvalid()) { + jolt_id = new_jolt_id; + } + + delete jolt_settings; + jolt_settings = nullptr; +} + +void JoltBody3D::_integrate_forces(float p_step, JPH::Body &p_jolt_body) { + if (!p_jolt_body.IsActive()) { + return; + } + + _update_gravity(p_jolt_body); + + if (!custom_integrator) { + JPH::MotionProperties &motion_properties = *p_jolt_body.GetMotionPropertiesUnchecked(); + + JPH::Vec3 linear_velocity = motion_properties.GetLinearVelocity(); + JPH::Vec3 angular_velocity = motion_properties.GetAngularVelocity(); + + // Jolt applies damping differently from Godot Physics, where Godot Physics applies damping before integrating + // forces whereas Jolt does it after integrating forces. The way Godot Physics does it seems to yield more + // consistent results across different update frequencies when using high (>1) damping values, so we apply the + // damping ourselves instead, before any force integration happens. + + linear_velocity *= MAX(1.0f - total_linear_damp * p_step, 0.0f); + angular_velocity *= MAX(1.0f - total_angular_damp * p_step, 0.0f); + + linear_velocity += to_jolt(gravity) * p_step; + + motion_properties.SetLinearVelocityClamped(linear_velocity); + motion_properties.SetAngularVelocityClamped(angular_velocity); + + p_jolt_body.AddForce(to_jolt(constant_force)); + p_jolt_body.AddTorque(to_jolt(constant_torque)); + } + + sync_state = true; +} + +void JoltBody3D::_move_kinematic(float p_step, JPH::Body &p_jolt_body) { + p_jolt_body.SetLinearVelocity(JPH::Vec3::sZero()); + p_jolt_body.SetAngularVelocity(JPH::Vec3::sZero()); + + const JPH::RVec3 current_position = p_jolt_body.GetPosition(); + const JPH::Quat current_rotation = p_jolt_body.GetRotation(); + + const JPH::RVec3 new_position = to_jolt_r(kinematic_transform.origin); + const JPH::Quat new_rotation = to_jolt(kinematic_transform.basis); + + if (new_position == current_position && new_rotation == current_rotation) { + return; + } + + p_jolt_body.MoveKinematic(new_position, new_rotation, p_step); + + sync_state = true; +} + +void JoltBody3D::_pre_step_static(float p_step, JPH::Body &p_jolt_body) { + // Nothing to do. +} + +void JoltBody3D::_pre_step_rigid(float p_step, JPH::Body &p_jolt_body) { + _integrate_forces(p_step, p_jolt_body); +} + +void JoltBody3D::_pre_step_kinematic(float p_step, JPH::Body &p_jolt_body) { + _update_gravity(p_jolt_body); + + _move_kinematic(p_step, p_jolt_body); + + if (reports_contacts()) { + // This seems to emulate the behavior of Godot Physics, where kinematic bodies are set as active (and thereby + // have their state synchronized on every step) only if its max reported contacts is non-zero. + sync_state = true; + } +} + +JPH::EAllowedDOFs JoltBody3D::_calculate_allowed_dofs() const { + if (is_static()) { + return JPH::EAllowedDOFs::All; + } + + JPH::EAllowedDOFs allowed_dofs = JPH::EAllowedDOFs::All; + + if (is_axis_locked(PhysicsServer3D::BODY_AXIS_LINEAR_X)) { + allowed_dofs &= ~JPH::EAllowedDOFs::TranslationX; + } + + if (is_axis_locked(PhysicsServer3D::BODY_AXIS_LINEAR_Y)) { + allowed_dofs &= ~JPH::EAllowedDOFs::TranslationY; + } + + if (is_axis_locked(PhysicsServer3D::BODY_AXIS_LINEAR_Z)) { + allowed_dofs &= ~JPH::EAllowedDOFs::TranslationZ; + } + + if (is_axis_locked(PhysicsServer3D::BODY_AXIS_ANGULAR_X) || is_rigid_linear()) { + allowed_dofs &= ~JPH::EAllowedDOFs::RotationX; + } + + if (is_axis_locked(PhysicsServer3D::BODY_AXIS_ANGULAR_Y) || is_rigid_linear()) { + allowed_dofs &= ~JPH::EAllowedDOFs::RotationY; + } + + if (is_axis_locked(PhysicsServer3D::BODY_AXIS_ANGULAR_Z) || is_rigid_linear()) { + allowed_dofs &= ~JPH::EAllowedDOFs::RotationZ; + } + + ERR_FAIL_COND_V_MSG(allowed_dofs == JPH::EAllowedDOFs::None, JPH::EAllowedDOFs::All, vformat("Invalid axis locks for '%s'. Locking all axes is not supported when using Jolt Physics. All axes will be unlocked. Considering freezing the body instead.", to_string())); + + return allowed_dofs; +} + +JPH::MassProperties JoltBody3D::_calculate_mass_properties(const JPH::Shape &p_shape) const { + const bool calculate_mass = mass <= 0; + const bool calculate_inertia = inertia.x <= 0 || inertia.y <= 0 || inertia.z <= 0; + + JPH::MassProperties mass_properties = p_shape.GetMassProperties(); + + if (calculate_mass && calculate_inertia) { + // Use the mass properties calculated by the shape + } else if (calculate_inertia) { + mass_properties.ScaleToMass(mass); + } else { + mass_properties.mMass = mass; + } + + if (inertia.x > 0) { + mass_properties.mInertia(0, 0) = (float)inertia.x; + mass_properties.mInertia(0, 1) = 0; + mass_properties.mInertia(0, 2) = 0; + mass_properties.mInertia(1, 0) = 0; + mass_properties.mInertia(2, 0) = 0; + } + + if (inertia.y > 0) { + mass_properties.mInertia(1, 1) = (float)inertia.y; + mass_properties.mInertia(1, 0) = 0; + mass_properties.mInertia(1, 2) = 0; + mass_properties.mInertia(0, 1) = 0; + mass_properties.mInertia(2, 1) = 0; + } + + if (inertia.z > 0) { + mass_properties.mInertia(2, 2) = (float)inertia.z; + mass_properties.mInertia(2, 0) = 0; + mass_properties.mInertia(2, 1) = 0; + mass_properties.mInertia(0, 2) = 0; + mass_properties.mInertia(1, 2) = 0; + } + + mass_properties.mInertia(3, 3) = 1.0f; + + return mass_properties; +} + +JPH::MassProperties JoltBody3D::_calculate_mass_properties() const { + return _calculate_mass_properties(*jolt_shape); +} + +void JoltBody3D::_update_mass_properties() { + if (!in_space()) { + return; + } + + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + body->GetMotionPropertiesUnchecked()->SetMassProperties(_calculate_allowed_dofs(), _calculate_mass_properties()); +} + +void JoltBody3D::_update_gravity(JPH::Body &p_jolt_body) { + gravity = Vector3(); + + const Vector3 position = to_godot(p_jolt_body.GetPosition()); + + bool gravity_done = false; + + for (const JoltArea3D *area : areas) { + gravity_done = integrate(gravity, area->get_gravity_mode(), [&]() { return area->compute_gravity(position); }); + + if (gravity_done) { + break; + } + } + + if (!gravity_done) { + gravity += space->get_default_area()->compute_gravity(position); + } + + gravity *= gravity_scale; +} + +void JoltBody3D::_update_damp() { + if (!in_space()) { + return; + } + + total_linear_damp = 0.0; + total_angular_damp = 0.0; + + bool linear_damp_done = linear_damp_mode == PhysicsServer3D::BODY_DAMP_MODE_REPLACE; + bool angular_damp_done = angular_damp_mode == PhysicsServer3D::BODY_DAMP_MODE_REPLACE; + + for (const JoltArea3D *area : areas) { + if (!linear_damp_done) { + linear_damp_done = integrate(total_linear_damp, area->get_linear_damp_mode(), [&]() { return area->get_linear_damp(); }); + } + + if (!angular_damp_done) { + angular_damp_done = integrate(total_angular_damp, area->get_angular_damp_mode(), [&]() { return area->get_angular_damp(); }); + } + + if (linear_damp_done && angular_damp_done) { + break; + } + } + + const JoltArea3D *default_area = space->get_default_area(); + + if (!linear_damp_done) { + total_linear_damp += default_area->get_linear_damp(); + } + + if (!angular_damp_done) { + total_angular_damp += default_area->get_angular_damp(); + } + + switch (linear_damp_mode) { + case PhysicsServer3D::BODY_DAMP_MODE_COMBINE: { + total_linear_damp += linear_damp; + } break; + case PhysicsServer3D::BODY_DAMP_MODE_REPLACE: { + total_linear_damp = linear_damp; + } break; + } + + switch (angular_damp_mode) { + case PhysicsServer3D::BODY_DAMP_MODE_COMBINE: { + total_angular_damp += angular_damp; + } break; + case PhysicsServer3D::BODY_DAMP_MODE_REPLACE: { + total_angular_damp = angular_damp; + } break; + } + + _motion_changed(); +} + +void JoltBody3D::_update_kinematic_transform() { + if (is_kinematic()) { + kinematic_transform = get_transform_unscaled(); + } +} + +void JoltBody3D::_update_joint_constraints() { + for (JoltJoint3D *joint : joints) { + joint->rebuild(); + } +} + +void JoltBody3D::_update_possible_kinematic_contacts() { + const bool value = reports_all_kinematic_contacts(); + + if (!in_space()) { + jolt_settings->mCollideKinematicVsNonDynamic = value; + } else { + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + body->SetCollideKinematicVsNonDynamic(value); + } +} + +void JoltBody3D::_destroy_joint_constraints() { + for (JoltJoint3D *joint : joints) { + joint->destroy(); + } +} + +void JoltBody3D::_exit_all_areas() { + for (JoltArea3D *area : areas) { + area->body_exited(jolt_id, false); + } + + areas.clear(); +} + +void JoltBody3D::_update_group_filter() { + JPH::GroupFilter *group_filter = !exceptions.is_empty() ? JoltGroupFilter::instance : nullptr; + + if (!in_space()) { + jolt_settings->mCollisionGroup.SetGroupFilter(group_filter); + return; + } + + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + body->GetCollisionGroup().SetGroupFilter(group_filter); +} + +void JoltBody3D::_mode_changed() { + _update_object_layer(); + _update_kinematic_transform(); + _update_mass_properties(); + wake_up(); +} + +void JoltBody3D::_shapes_built() { + JoltShapedObject3D::_shapes_built(); + + _update_mass_properties(); + _update_joint_constraints(); + wake_up(); +} + +void JoltBody3D::_space_changing() { + JoltShapedObject3D::_space_changing(); + + sleep_initially = is_sleeping(); + + _destroy_joint_constraints(); + _exit_all_areas(); +} + +void JoltBody3D::_space_changed() { + JoltShapedObject3D::_space_changed(); + + _update_kinematic_transform(); + _update_group_filter(); + _update_joint_constraints(); + _areas_changed(); + + sync_state = false; +} + +void JoltBody3D::_areas_changed() { + _update_damp(); + wake_up(); +} + +void JoltBody3D::_joints_changed() { + wake_up(); +} + +void JoltBody3D::_transform_changed() { + wake_up(); +} + +void JoltBody3D::_motion_changed() { + wake_up(); +} + +void JoltBody3D::_exceptions_changed() { + _update_group_filter(); +} + +void JoltBody3D::_axis_lock_changed() { + _update_mass_properties(); + wake_up(); +} + +void JoltBody3D::_contact_reporting_changed() { + _update_possible_kinematic_contacts(); + wake_up(); +} + +JoltBody3D::JoltBody3D() : + JoltShapedObject3D(OBJECT_TYPE_BODY) { +} + +JoltBody3D::~JoltBody3D() { + if (direct_state != nullptr) { + memdelete(direct_state); + direct_state = nullptr; + } +} + +void JoltBody3D::set_transform(Transform3D p_transform) { + JOLT_ENSURE_SCALE_NOT_ZERO(p_transform, vformat("An invalid transform was passed to physics body '%s'.", to_string())); + + const Vector3 new_scale = p_transform.basis.get_scale(); + + // Ideally we would do an exact comparison here, but due to floating-point precision this would be invalidated very often. + if (!scale.is_equal_approx(new_scale)) { + scale = new_scale; + _shapes_changed(); + } + + p_transform.basis.orthonormalize(); + + if (!in_space()) { + jolt_settings->mPosition = to_jolt_r(p_transform.origin); + jolt_settings->mRotation = to_jolt(p_transform.basis); + } else if (is_kinematic()) { + kinematic_transform = p_transform; + } else { + space->get_body_iface().SetPositionAndRotation(jolt_id, to_jolt_r(p_transform.origin), to_jolt(p_transform.basis), JPH::EActivation::DontActivate); + } + + _transform_changed(); +} + +Variant JoltBody3D::get_state(PhysicsServer3D::BodyState p_state) const { + switch (p_state) { + case PhysicsServer3D::BODY_STATE_TRANSFORM: { + return get_transform_scaled(); + } + case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { + return get_linear_velocity(); + } + case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { + return get_angular_velocity(); + } + case PhysicsServer3D::BODY_STATE_SLEEPING: { + return is_sleeping(); + } + case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { + return can_sleep(); + } + default: { + ERR_FAIL_V_MSG(Variant(), vformat("Unhandled body state: '%d'. This should not happen. Please report this.", p_state)); + } + } +} + +void JoltBody3D::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_value) { + switch (p_state) { + case PhysicsServer3D::BODY_STATE_TRANSFORM: { + set_transform(p_value); + } break; + case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { + set_linear_velocity(p_value); + } break; + case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { + set_angular_velocity(p_value); + } break; + case PhysicsServer3D::BODY_STATE_SLEEPING: { + set_is_sleeping(p_value); + } break; + case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { + set_can_sleep(p_value); + } break; + default: { + ERR_FAIL_MSG(vformat("Unhandled body state: '%d'. This should not happen. Please report this.", p_state)); + } break; + } +} + +Variant JoltBody3D::get_param(PhysicsServer3D::BodyParameter p_param) const { + switch (p_param) { + case PhysicsServer3D::BODY_PARAM_BOUNCE: { + return get_bounce(); + } + case PhysicsServer3D::BODY_PARAM_FRICTION: { + return get_friction(); + } + case PhysicsServer3D::BODY_PARAM_MASS: { + return get_mass(); + } + case PhysicsServer3D::BODY_PARAM_INERTIA: { + return get_inertia(); + } + case PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS: { + return get_center_of_mass_custom(); + } + case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE: { + return get_gravity_scale(); + } + case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP_MODE: { + return get_linear_damp_mode(); + } + case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP_MODE: { + return get_angular_damp_mode(); + } + case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP: { + return get_linear_damp(); + } + case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP: { + return get_angular_damp(); + } + default: { + ERR_FAIL_V_MSG(Variant(), vformat("Unhandled body parameter: '%d'. This should not happen. Please report this.", p_param)); + } + } +} + +void JoltBody3D::set_param(PhysicsServer3D::BodyParameter p_param, const Variant &p_value) { + switch (p_param) { + case PhysicsServer3D::BODY_PARAM_BOUNCE: { + set_bounce(p_value); + } break; + case PhysicsServer3D::BODY_PARAM_FRICTION: { + set_friction(p_value); + } break; + case PhysicsServer3D::BODY_PARAM_MASS: { + set_mass(p_value); + } break; + case PhysicsServer3D::BODY_PARAM_INERTIA: { + set_inertia(p_value); + } break; + case PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS: { + set_center_of_mass_custom(p_value); + } break; + case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE: { + set_gravity_scale(p_value); + } break; + case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP_MODE: { + set_linear_damp_mode((DampMode)(int)p_value); + } break; + case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP_MODE: { + set_angular_damp_mode((DampMode)(int)p_value); + } break; + case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP: { + set_linear_damp(p_value); + } break; + case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP: { + set_angular_damp(p_value); + } break; + default: { + ERR_FAIL_MSG(vformat("Unhandled body parameter: '%d'. This should not happen. Please report this.", p_param)); + } break; + } +} + +void JoltBody3D::set_custom_integrator(bool p_enabled) { + if (custom_integrator == p_enabled) { + return; + } + + custom_integrator = p_enabled; + + if (!in_space()) { + _motion_changed(); + return; + } + + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + body->ResetForce(); + body->ResetTorque(); + + _motion_changed(); +} + +bool JoltBody3D::is_sleeping() const { + if (!in_space()) { + return sleep_initially; + } + + const JoltReadableBody3D body = space->read_body(jolt_id); + ERR_FAIL_COND_V(body.is_invalid(), false); + + return !body->IsActive(); +} + +void JoltBody3D::set_is_sleeping(bool p_enabled) { + if (!in_space()) { + sleep_initially = p_enabled; + return; + } + + JPH::BodyInterface &body_iface = space->get_body_iface(); + + if (p_enabled) { + body_iface.DeactivateBody(jolt_id); + } else { + body_iface.ActivateBody(jolt_id); + } +} + +bool JoltBody3D::can_sleep() const { + if (!in_space()) { + return jolt_settings->mAllowSleeping; + } + + const JoltReadableBody3D body = space->read_body(jolt_id); + ERR_FAIL_COND_V(body.is_invalid(), false); + + return body->GetAllowSleeping(); +} + +void JoltBody3D::set_can_sleep(bool p_enabled) { + if (!in_space()) { + jolt_settings->mAllowSleeping = p_enabled; + return; + } + + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + body->SetAllowSleeping(p_enabled); +} + +Basis JoltBody3D::get_principal_inertia_axes() const { + ERR_FAIL_NULL_V_MSG(space, Basis(), vformat("Failed to retrieve principal inertia axes of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string())); + + if (unlikely(is_static() || is_kinematic())) { + return Basis(); + } + + const JoltReadableBody3D body = space->read_body(jolt_id); + ERR_FAIL_COND_V(body.is_invalid(), Basis()); + + return to_godot(body->GetRotation() * body->GetMotionProperties()->GetInertiaRotation()); +} + +Vector3 JoltBody3D::get_inverse_inertia() const { + ERR_FAIL_NULL_V_MSG(space, Vector3(), vformat("Failed to retrieve inverse inertia of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string())); + + if (unlikely(is_static() || is_kinematic())) { + return Vector3(); + } + + const JoltReadableBody3D body = space->read_body(jolt_id); + ERR_FAIL_COND_V(body.is_invalid(), Vector3()); + + const JPH::MotionProperties &motion_properties = *body->GetMotionPropertiesUnchecked(); + + return to_godot(motion_properties.GetLocalSpaceInverseInertia().GetDiagonal3()); +} + +Basis JoltBody3D::get_inverse_inertia_tensor() const { + ERR_FAIL_NULL_V_MSG(space, Basis(), vformat("Failed to retrieve inverse inertia tensor of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string())); + + if (unlikely(is_static() || is_kinematic())) { + return Basis(); + } + + const JoltReadableBody3D body = space->read_body(jolt_id); + ERR_FAIL_COND_V(body.is_invalid(), Basis()); + + return to_godot(body->GetInverseInertia()).basis; +} + +void JoltBody3D::set_linear_velocity(const Vector3 &p_velocity) { + if (is_static() || is_kinematic()) { + linear_surface_velocity = p_velocity; + _motion_changed(); + return; + } + + if (!in_space()) { + jolt_settings->mLinearVelocity = to_jolt(p_velocity); + _motion_changed(); + return; + } + + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + body->GetMotionPropertiesUnchecked()->SetLinearVelocityClamped(to_jolt(p_velocity)); + + _motion_changed(); +} + +void JoltBody3D::set_angular_velocity(const Vector3 &p_velocity) { + if (is_static() || is_kinematic()) { + angular_surface_velocity = p_velocity; + _motion_changed(); + return; + } + + if (!in_space()) { + jolt_settings->mAngularVelocity = to_jolt(p_velocity); + _motion_changed(); + return; + } + + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + body->GetMotionPropertiesUnchecked()->SetAngularVelocityClamped(to_jolt(p_velocity)); + + _motion_changed(); +} + +void JoltBody3D::set_axis_velocity(const Vector3 &p_axis_velocity) { + const Vector3 axis = p_axis_velocity.normalized(); + + if (!in_space()) { + Vector3 linear_velocity = to_godot(jolt_settings->mLinearVelocity); + linear_velocity -= axis * axis.dot(linear_velocity); + linear_velocity += p_axis_velocity; + jolt_settings->mLinearVelocity = to_jolt(linear_velocity); + } else { + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + Vector3 linear_velocity = get_linear_velocity(); + linear_velocity -= axis * axis.dot(linear_velocity); + linear_velocity += p_axis_velocity; + set_linear_velocity(linear_velocity); + } + + _motion_changed(); +} + +Vector3 JoltBody3D::get_velocity_at_position(const Vector3 &p_position) const { + if (unlikely(!in_space())) { + return Vector3(); + } + + const JoltReadableBody3D body = space->read_body(jolt_id); + ERR_FAIL_COND_V(body.is_invalid(), Vector3()); + + const JPH::MotionProperties &motion_properties = *body->GetMotionPropertiesUnchecked(); + + const Vector3 total_linear_velocity = to_godot(motion_properties.GetLinearVelocity()) + linear_surface_velocity; + const Vector3 total_angular_velocity = to_godot(motion_properties.GetAngularVelocity()) + angular_surface_velocity; + const Vector3 com_to_pos = p_position - to_godot(body->GetCenterOfMassPosition()); + + return total_linear_velocity + total_angular_velocity.cross(com_to_pos); +} + +void JoltBody3D::set_center_of_mass_custom(const Vector3 &p_center_of_mass) { + if (custom_center_of_mass && p_center_of_mass == center_of_mass_custom) { + return; + } + + custom_center_of_mass = true; + center_of_mass_custom = p_center_of_mass; + + _shapes_changed(); +} + +void JoltBody3D::set_max_contacts_reported(int p_count) { + ERR_FAIL_COND(p_count < 0); + + if (unlikely((int)contacts.size() == p_count)) { + return; + } + + contacts.resize(p_count); + contact_count = MIN(contact_count, p_count); + + const bool use_manifold_reduction = !reports_contacts(); + + if (!in_space()) { + jolt_settings->mUseManifoldReduction = use_manifold_reduction; + _contact_reporting_changed(); + return; + } + + JPH::BodyInterface &body_iface = space->get_body_iface(); + + body_iface.SetUseManifoldReduction(jolt_id, use_manifold_reduction); + + _contact_reporting_changed(); +} + +bool JoltBody3D::reports_all_kinematic_contacts() const { + return reports_contacts() && JoltProjectSettings::should_generate_all_kinematic_contacts(); +} + +void JoltBody3D::add_contact(const JoltBody3D *p_collider, float p_depth, int p_shape_index, int p_collider_shape_index, const Vector3 &p_normal, const Vector3 &p_position, const Vector3 &p_collider_position, const Vector3 &p_velocity, const Vector3 &p_collider_velocity, const Vector3 &p_impulse) { + const int max_contacts = get_max_contacts_reported(); + + if (max_contacts == 0) { + return; + } + + Contact *contact = nullptr; + + if (contact_count < max_contacts) { + contact = &contacts[contact_count++]; + } else { + Contact *shallowest_contact = &contacts[0]; + + for (int i = 1; i < (int)contacts.size(); i++) { + Contact &other_contact = contacts[i]; + if (other_contact.depth < shallowest_contact->depth) { + shallowest_contact = &other_contact; + } + } + + if (shallowest_contact->depth < p_depth) { + contact = shallowest_contact; + } + } + + if (contact != nullptr) { + contact->normal = p_normal; + contact->position = p_position; + contact->collider_position = p_collider_position; + contact->velocity = p_velocity; + contact->collider_velocity = p_collider_velocity; + contact->impulse = p_impulse; + contact->collider_id = p_collider->get_instance_id(); + contact->collider_rid = p_collider->get_rid(); + contact->shape_index = p_shape_index; + contact->collider_shape_index = p_collider_shape_index; + } +} + +void JoltBody3D::reset_mass_properties() { + if (custom_center_of_mass) { + custom_center_of_mass = false; + center_of_mass_custom.zero(); + + _shapes_changed(); + } + + inertia.zero(); + + _update_mass_properties(); +} + +void JoltBody3D::apply_force(const Vector3 &p_force, const Vector3 &p_position) { + ERR_FAIL_NULL_MSG(space, vformat("Failed to apply force to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string())); + + if (unlikely(!is_rigid())) { + return; + } + + if (custom_integrator || p_force == Vector3()) { + return; + } + + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + body->AddForce(to_jolt(p_force), body->GetPosition() + to_jolt(p_position)); + + _motion_changed(); +} + +void JoltBody3D::apply_central_force(const Vector3 &p_force) { + ERR_FAIL_NULL_MSG(space, vformat("Failed to apply central force to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string())); + + if (unlikely(!is_rigid())) { + return; + } + + if (custom_integrator || p_force == Vector3()) { + return; + } + + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + body->AddForce(to_jolt(p_force)); + + _motion_changed(); +} + +void JoltBody3D::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) { + ERR_FAIL_NULL_MSG(space, vformat("Failed to apply impulse to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string())); + + if (unlikely(!is_rigid())) { + return; + } + + if (p_impulse == Vector3()) { + return; + } + + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + body->AddImpulse(to_jolt(p_impulse), body->GetPosition() + to_jolt(p_position)); + + _motion_changed(); +} + +void JoltBody3D::apply_central_impulse(const Vector3 &p_impulse) { + ERR_FAIL_NULL_MSG(space, vformat("Failed to apply central impulse to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string())); + + if (unlikely(!is_rigid())) { + return; + } + + if (p_impulse == Vector3()) { + return; + } + + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + body->AddImpulse(to_jolt(p_impulse)); + + _motion_changed(); +} + +void JoltBody3D::apply_torque(const Vector3 &p_torque) { + ERR_FAIL_NULL_MSG(space, vformat("Failed to apply torque to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string())); + + if (unlikely(!is_rigid())) { + return; + } + + if (custom_integrator || p_torque == Vector3()) { + return; + } + + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + body->AddTorque(to_jolt(p_torque)); + + _motion_changed(); +} + +void JoltBody3D::apply_torque_impulse(const Vector3 &p_impulse) { + ERR_FAIL_NULL_MSG(space, vformat("Failed to apply torque impulse to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string())); + + if (unlikely(!is_rigid())) { + return; + } + + if (p_impulse == Vector3()) { + return; + } + + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + body->AddAngularImpulse(to_jolt(p_impulse)); + + _motion_changed(); +} + +void JoltBody3D::add_constant_central_force(const Vector3 &p_force) { + if (p_force == Vector3()) { + return; + } + + constant_force += p_force; + + _motion_changed(); +} + +void JoltBody3D::add_constant_force(const Vector3 &p_force, const Vector3 &p_position) { + if (p_force == Vector3()) { + return; + } + + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + constant_force += p_force; + constant_torque += (p_position - get_center_of_mass_relative()).cross(p_force); + + _motion_changed(); +} + +void JoltBody3D::add_constant_torque(const Vector3 &p_torque) { + if (p_torque == Vector3()) { + return; + } + + constant_torque += p_torque; + + _motion_changed(); +} + +Vector3 JoltBody3D::get_constant_force() const { + return constant_force; +} + +void JoltBody3D::set_constant_force(const Vector3 &p_force) { + if (constant_force == p_force) { + return; + } + + constant_force = p_force; + + _motion_changed(); +} + +Vector3 JoltBody3D::get_constant_torque() const { + return constant_torque; +} + +void JoltBody3D::set_constant_torque(const Vector3 &p_torque) { + if (constant_torque == p_torque) { + return; + } + + constant_torque = p_torque; + + _motion_changed(); +} + +void JoltBody3D::add_collision_exception(const RID &p_excepted_body) { + exceptions.push_back(p_excepted_body); + + _exceptions_changed(); +} + +void JoltBody3D::remove_collision_exception(const RID &p_excepted_body) { + exceptions.erase(p_excepted_body); + + _exceptions_changed(); +} + +bool JoltBody3D::has_collision_exception(const RID &p_excepted_body) const { + return exceptions.find(p_excepted_body) >= 0; +} + +void JoltBody3D::add_area(JoltArea3D *p_area) { + int i = 0; + for (; i < (int)areas.size(); i++) { + if (p_area->get_priority() > areas[i]->get_priority()) { + break; + } + } + + areas.insert(i, p_area); + + _areas_changed(); +} + +void JoltBody3D::remove_area(JoltArea3D *p_area) { + areas.erase(p_area); + + _areas_changed(); +} + +void JoltBody3D::add_joint(JoltJoint3D *p_joint) { + joints.push_back(p_joint); + + _joints_changed(); +} + +void JoltBody3D::remove_joint(JoltJoint3D *p_joint) { + joints.erase(p_joint); + + _joints_changed(); +} + +void JoltBody3D::call_queries(JPH::Body &p_jolt_body) { + if (!sync_state) { + return; + } + + if (custom_integration_callback.is_valid()) { + if (custom_integration_userdata.get_type() != Variant::NIL) { + static thread_local Array arguments = []() { + Array array; + array.resize(2); + return array; + }(); + + arguments[0] = get_direct_state(); + arguments[1] = custom_integration_userdata; + + custom_integration_callback.callv(arguments); + } else { + static thread_local Array arguments = []() { + Array array; + array.resize(1); + return array; + }(); + + arguments[0] = get_direct_state(); + + custom_integration_callback.callv(arguments); + } + } + + if (state_sync_callback.is_valid()) { + static thread_local Array arguments = []() { + Array array; + array.resize(1); + return array; + }(); + + arguments[0] = get_direct_state(); + + state_sync_callback.callv(arguments); + } + + sync_state = false; +} + +void JoltBody3D::pre_step(float p_step, JPH::Body &p_jolt_body) { + JoltObject3D::pre_step(p_step, p_jolt_body); + + switch (mode) { + case PhysicsServer3D::BODY_MODE_STATIC: { + _pre_step_static(p_step, p_jolt_body); + } break; + case PhysicsServer3D::BODY_MODE_RIGID: + case PhysicsServer3D::BODY_MODE_RIGID_LINEAR: { + _pre_step_rigid(p_step, p_jolt_body); + } break; + case PhysicsServer3D::BODY_MODE_KINEMATIC: { + _pre_step_kinematic(p_step, p_jolt_body); + } break; + } + + contact_count = 0; +} + +JoltPhysicsDirectBodyState3D *JoltBody3D::get_direct_state() { + if (direct_state == nullptr) { + direct_state = memnew(JoltPhysicsDirectBodyState3D(this)); + } + + return direct_state; +} + +void JoltBody3D::set_mode(PhysicsServer3D::BodyMode p_mode) { + if (p_mode == mode) { + return; + } + + mode = p_mode; + + if (!in_space()) { + _mode_changed(); + return; + } + + const JPH::EMotionType motion_type = _get_motion_type(); + + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + if (motion_type == JPH::EMotionType::Static) { + put_to_sleep(); + } + + body->SetMotionType(motion_type); + + if (motion_type != JPH::EMotionType::Static) { + wake_up(); + } + + if (motion_type == JPH::EMotionType::Kinematic) { + body->SetLinearVelocity(JPH::Vec3::sZero()); + body->SetAngularVelocity(JPH::Vec3::sZero()); + } + + linear_surface_velocity = Vector3(); + angular_surface_velocity = Vector3(); + + _mode_changed(); +} + +bool JoltBody3D::is_ccd_enabled() const { + if (!in_space()) { + return jolt_settings->mMotionQuality == JPH::EMotionQuality::LinearCast; + } + + const JPH::BodyInterface &body_iface = space->get_body_iface(); + + return body_iface.GetMotionQuality(jolt_id) == JPH::EMotionQuality::LinearCast; +} + +void JoltBody3D::set_ccd_enabled(bool p_enabled) { + const JPH::EMotionQuality motion_quality = p_enabled ? JPH::EMotionQuality::LinearCast : JPH::EMotionQuality::Discrete; + + if (!in_space()) { + jolt_settings->mMotionQuality = motion_quality; + return; + } + + JPH::BodyInterface &body_iface = space->get_body_iface(); + + body_iface.SetMotionQuality(jolt_id, motion_quality); +} + +void JoltBody3D::set_mass(float p_mass) { + if (p_mass != mass) { + mass = p_mass; + _update_mass_properties(); + } +} + +void JoltBody3D::set_inertia(const Vector3 &p_inertia) { + if (p_inertia != inertia) { + inertia = p_inertia; + _update_mass_properties(); + } +} + +float JoltBody3D::get_bounce() const { + if (!in_space()) { + return jolt_settings->mRestitution; + } + + const JoltReadableBody3D body = space->read_body(jolt_id); + ERR_FAIL_COND_V(body.is_invalid(), 0.0f); + + return body->GetRestitution(); +} + +void JoltBody3D::set_bounce(float p_bounce) { + if (!in_space()) { + jolt_settings->mRestitution = p_bounce; + return; + } + + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + body->SetRestitution(p_bounce); +} + +float JoltBody3D::get_friction() const { + if (!in_space()) { + return jolt_settings->mFriction; + } + + const JoltReadableBody3D body = space->read_body(jolt_id); + ERR_FAIL_COND_V(body.is_invalid(), 0.0f); + + return body->GetFriction(); +} + +void JoltBody3D::set_friction(float p_friction) { + if (!in_space()) { + jolt_settings->mFriction = p_friction; + return; + } + + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + body->SetFriction(p_friction); +} + +void JoltBody3D::set_gravity_scale(float p_scale) { + if (gravity_scale == p_scale) { + return; + } + + gravity_scale = p_scale; + + _motion_changed(); +} + +void JoltBody3D::set_linear_damp(float p_damp) { + p_damp = MAX(0.0f, p_damp); + + if (p_damp == linear_damp) { + return; + } + + linear_damp = p_damp; + + _update_damp(); +} + +void JoltBody3D::set_angular_damp(float p_damp) { + p_damp = MAX(0.0f, p_damp); + + if (p_damp == angular_damp) { + return; + } + + angular_damp = p_damp; + + _update_damp(); +} + +bool JoltBody3D::is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const { + return (locked_axes & (uint32_t)p_axis) != 0; +} + +void JoltBody3D::set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool p_enabled) { + const uint32_t previous_locked_axes = locked_axes; + + if (p_enabled) { + locked_axes |= (uint32_t)p_axis; + } else { + locked_axes &= ~(uint32_t)p_axis; + } + + if (previous_locked_axes != locked_axes) { + _axis_lock_changed(); + } +} + +bool JoltBody3D::can_interact_with(const JoltBody3D &p_other) const { + return (can_collide_with(p_other) || p_other.can_collide_with(*this)) && !has_collision_exception(p_other.get_rid()) && !p_other.has_collision_exception(rid); +} + +bool JoltBody3D::can_interact_with(const JoltSoftBody3D &p_other) const { + return p_other.can_interact_with(*this); +} + +bool JoltBody3D::can_interact_with(const JoltArea3D &p_other) const { + return p_other.can_interact_with(*this); +} diff --git a/modules/jolt_physics/objects/jolt_body_3d.h b/modules/jolt_physics/objects/jolt_body_3d.h new file mode 100644 index 000000000000..50a5c2a293fd --- /dev/null +++ b/modules/jolt_physics/objects/jolt_body_3d.h @@ -0,0 +1,304 @@ +/**************************************************************************/ +/* jolt_body_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_BODY_3D_H +#define JOLT_BODY_3D_H + +#include "jolt_physics_direct_body_state_3d.h" +#include "jolt_shaped_object_3d.h" + +class JoltArea3D; +class JoltJoint3D; +class JoltSoftBody3D; + +class JoltBody3D final : public JoltShapedObject3D { +public: + typedef PhysicsServer3D::BodyDampMode DampMode; + + struct Contact { + Vector3 normal; + Vector3 position; + Vector3 collider_position; + Vector3 velocity; + Vector3 collider_velocity; + Vector3 impulse; + ObjectID collider_id; + RID collider_rid; + float depth = 0.0f; + int shape_index = 0; + int collider_shape_index = 0; + }; + +private: + LocalVector exceptions; + LocalVector contacts; + LocalVector areas; + LocalVector joints; + + Variant custom_integration_userdata; + + Transform3D kinematic_transform; + + Vector3 inertia; + Vector3 center_of_mass_custom; + Vector3 constant_force; + Vector3 constant_torque; + Vector3 linear_surface_velocity; + Vector3 angular_surface_velocity; + Vector3 gravity; + + Callable state_sync_callback; + Callable custom_integration_callback; + + JoltPhysicsDirectBodyState3D *direct_state = nullptr; + + PhysicsServer3D::BodyMode mode = PhysicsServer3D::BODY_MODE_RIGID; + + DampMode linear_damp_mode = PhysicsServer3D::BODY_DAMP_MODE_COMBINE; + DampMode angular_damp_mode = PhysicsServer3D::BODY_DAMP_MODE_COMBINE; + + float mass = 1.0f; + float linear_damp = 0.0f; + float angular_damp = 0.0f; + float total_linear_damp = 0.0f; + float total_angular_damp = 0.0f; + float gravity_scale = 1.0f; + float collision_priority = 1.0f; + + int contact_count = 0; + + uint32_t locked_axes = 0; + + bool sync_state = false; + bool sleep_initially = false; + bool custom_center_of_mass = false; + bool custom_integrator = false; + + virtual JPH::BroadPhaseLayer _get_broad_phase_layer() const override; + virtual JPH::ObjectLayer _get_object_layer() const override; + + virtual JPH::EMotionType _get_motion_type() const override; + + virtual void _add_to_space() override; + + void _integrate_forces(float p_step, JPH::Body &p_jolt_body); + + void _move_kinematic(float p_step, JPH::Body &p_jolt_body); + + void _pre_step_static(float p_step, JPH::Body &p_jolt_body); + void _pre_step_rigid(float p_step, JPH::Body &p_jolt_body); + void _pre_step_kinematic(float p_step, JPH::Body &p_jolt_body); + + JPH::EAllowedDOFs _calculate_allowed_dofs() const; + + JPH::MassProperties _calculate_mass_properties(const JPH::Shape &p_shape) const; + JPH::MassProperties _calculate_mass_properties() const; + + void _update_mass_properties(); + void _update_gravity(JPH::Body &p_jolt_body); + void _update_damp(); + void _update_kinematic_transform(); + void _update_group_filter(); + void _update_joint_constraints(); + void _update_possible_kinematic_contacts(); + + void _destroy_joint_constraints(); + + void _exit_all_areas(); + + void _mode_changed(); + virtual void _shapes_built() override; + virtual void _space_changing() override; + virtual void _space_changed() override; + void _areas_changed(); + void _joints_changed(); + void _transform_changed(); + void _motion_changed(); + void _exceptions_changed(); + void _axis_lock_changed(); + void _contact_reporting_changed(); + +public: + JoltBody3D(); + virtual ~JoltBody3D() override; + + void set_transform(Transform3D p_transform); + + Variant get_state(PhysicsServer3D::BodyState p_state) const; + void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_value); + + Variant get_param(PhysicsServer3D::BodyParameter p_param) const; + void set_param(PhysicsServer3D::BodyParameter p_param, const Variant &p_value); + + bool has_state_sync_callback() const { return state_sync_callback.is_valid(); } + void set_state_sync_callback(const Callable &p_callback) { state_sync_callback = p_callback; } + + bool has_custom_integration_callback() const { return custom_integration_callback.is_valid(); } + void set_custom_integration_callback(const Callable &p_callback, const Variant &p_userdata) { + custom_integration_callback = p_callback; + custom_integration_userdata = p_userdata; + } + + bool has_custom_integrator() const { return custom_integrator; } + void set_custom_integrator(bool p_enabled); + + bool is_sleeping() const; + void set_is_sleeping(bool p_enabled); + + void put_to_sleep() { set_is_sleeping(true); } + void wake_up() { set_is_sleeping(false); } + + bool can_sleep() const; + void set_can_sleep(bool p_enabled); + + Basis get_principal_inertia_axes() const; + Vector3 get_inverse_inertia() const; + Basis get_inverse_inertia_tensor() const; + + void set_linear_velocity(const Vector3 &p_velocity); + void set_angular_velocity(const Vector3 &p_velocity); + void set_axis_velocity(const Vector3 &p_axis_velocity); + + virtual Vector3 get_velocity_at_position(const Vector3 &p_position) const override; + + virtual bool has_custom_center_of_mass() const override { return custom_center_of_mass; } + virtual Vector3 get_center_of_mass_custom() const override { return center_of_mass_custom; } + void set_center_of_mass_custom(const Vector3 &p_center_of_mass); + + int get_max_contacts_reported() const { return contacts.size(); } + void set_max_contacts_reported(int p_count); + + int get_contact_count() const { return contact_count; } + const Contact &get_contact(int p_index) { return contacts[p_index]; } + virtual bool reports_contacts() const override { return !contacts.is_empty(); } + + bool reports_all_kinematic_contacts() const; + + void add_contact(const JoltBody3D *p_collider, float p_depth, int p_shape_index, int p_collider_shape_index, const Vector3 &p_normal, const Vector3 &p_position, const Vector3 &p_collider_position, const Vector3 &p_velocity, const Vector3 &p_collider_velocity, const Vector3 &p_impulse); + + void reset_mass_properties(); + + void apply_force(const Vector3 &p_force, const Vector3 &p_position); + void apply_central_force(const Vector3 &p_force); + void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position); + + void apply_central_impulse(const Vector3 &p_impulse); + void apply_torque(const Vector3 &p_torque); + void apply_torque_impulse(const Vector3 &p_impulse); + + void add_constant_central_force(const Vector3 &p_force); + void add_constant_force(const Vector3 &p_force, const Vector3 &p_position); + void add_constant_torque(const Vector3 &p_torque); + + Vector3 get_constant_force() const; + void set_constant_force(const Vector3 &p_force); + + Vector3 get_constant_torque() const; + void set_constant_torque(const Vector3 &p_torque); + + Vector3 get_linear_surface_velocity() const { return linear_surface_velocity; } + Vector3 get_angular_surface_velocity() const { return angular_surface_velocity; } + + void add_collision_exception(const RID &p_excepted_body); + void remove_collision_exception(const RID &p_excepted_body); + bool has_collision_exception(const RID &p_excepted_body) const; + + const LocalVector &get_collision_exceptions() const { return exceptions; } + + void add_area(JoltArea3D *p_area); + void remove_area(JoltArea3D *p_area); + + void add_joint(JoltJoint3D *p_joint); + void remove_joint(JoltJoint3D *p_joint); + + void call_queries(JPH::Body &p_jolt_body); + + virtual void pre_step(float p_step, JPH::Body &p_jolt_body) override; + + JoltPhysicsDirectBodyState3D *get_direct_state(); + + PhysicsServer3D::BodyMode get_mode() const { return mode; } + + void set_mode(PhysicsServer3D::BodyMode p_mode); + + bool is_static() const { return mode == PhysicsServer3D::BODY_MODE_STATIC; } + bool is_kinematic() const { return mode == PhysicsServer3D::BODY_MODE_KINEMATIC; } + bool is_rigid_free() const { return mode == PhysicsServer3D::BODY_MODE_RIGID; } + bool is_rigid_linear() const { return mode == PhysicsServer3D::BODY_MODE_RIGID_LINEAR; } + bool is_rigid() const { return is_rigid_free() || is_rigid_linear(); } + + bool is_ccd_enabled() const; + void set_ccd_enabled(bool p_enabled); + + float get_mass() const { return mass; } + void set_mass(float p_mass); + + Vector3 get_inertia() const { return inertia; } + void set_inertia(const Vector3 &p_inertia); + + float get_bounce() const; + void set_bounce(float p_bounce); + + float get_friction() const; + void set_friction(float p_friction); + + float get_gravity_scale() const { return gravity_scale; } + void set_gravity_scale(float p_scale); + + Vector3 get_gravity() const { return gravity; } + + float get_linear_damp() const { return linear_damp; } + void set_linear_damp(float p_damp); + + float get_angular_damp() const { return angular_damp; } + void set_angular_damp(float p_damp); + + float get_total_linear_damp() const { return total_linear_damp; } + float get_total_angular_damp() const { return total_angular_damp; } + + float get_collision_priority() const { return collision_priority; } + void set_collision_priority(float p_priority) { collision_priority = p_priority; } + + DampMode get_linear_damp_mode() const { return linear_damp_mode; } + void set_linear_damp_mode(DampMode p_mode) { linear_damp_mode = p_mode; } + + DampMode get_angular_damp_mode() const { return angular_damp_mode; } + void set_angular_damp_mode(DampMode p_mode) { angular_damp_mode = p_mode; } + + bool is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const; + void set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool p_enabled); + bool are_axes_locked() const { return locked_axes != 0; } + + virtual bool can_interact_with(const JoltBody3D &p_other) const override; + virtual bool can_interact_with(const JoltSoftBody3D &p_other) const override; + virtual bool can_interact_with(const JoltArea3D &p_other) const override; +}; + +#endif // JOLT_BODY_3D_H diff --git a/modules/jolt_physics/objects/jolt_group_filter.cpp b/modules/jolt_physics/objects/jolt_group_filter.cpp new file mode 100644 index 000000000000..b06ce8c66b5e --- /dev/null +++ b/modules/jolt_physics/objects/jolt_group_filter.cpp @@ -0,0 +1,60 @@ +/**************************************************************************/ +/* jolt_group_filter.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_group_filter.h" + +#include "jolt_area_3d.h" +#include "jolt_body_3d.h" +#include "jolt_object_3d.h" + +bool JoltGroupFilter::CanCollide(const JPH::CollisionGroup &p_group1, const JPH::CollisionGroup &p_group2) const { + const JoltObject3D *object1 = decode_object(p_group1.GetGroupID(), p_group1.GetSubGroupID()); + const JoltObject3D *object2 = decode_object(p_group2.GetGroupID(), p_group2.GetSubGroupID()); + return object1->can_interact_with(*object2); +} + +void JoltGroupFilter::encode_object(const JoltObject3D *p_object, JPH::CollisionGroup::GroupID &r_group_id, JPH::CollisionGroup::SubGroupID &r_sub_group_id) { + // Since group filters don't grant us access to the bodies or their user data we are instead forced use the + // collision group to carry the upper and lower bits of our pointer, which we can access and decode in `CanCollide`. + const uint64_t address = reinterpret_cast(p_object); + r_group_id = JPH::CollisionGroup::GroupID(address >> 32U); + r_sub_group_id = JPH::CollisionGroup::SubGroupID(address & 0xFFFFFFFFULL); +} + +const JoltObject3D *JoltGroupFilter::decode_object(JPH::CollisionGroup::GroupID p_group_id, JPH::CollisionGroup::SubGroupID p_sub_group_id) { + const uint64_t upper_bits = (uint64_t)p_group_id << 32U; + const uint64_t lower_bits = (uint64_t)p_sub_group_id; + const uint64_t address = uint64_t(upper_bits | lower_bits); + return reinterpret_cast(address); +} + +static_assert(sizeof(JoltObject3D *) <= 8, "Pointer size greater than expected."); +static_assert(sizeof(JPH::CollisionGroup::GroupID) == 4, "Size of Jolt's collision group ID has changed."); +static_assert(sizeof(JPH::CollisionGroup::SubGroupID) == 4, "Size of Jolt's collision sub-group ID has changed."); diff --git a/modules/jolt_physics/objects/jolt_group_filter.h b/modules/jolt_physics/objects/jolt_group_filter.h new file mode 100644 index 000000000000..cadffb83ecaa --- /dev/null +++ b/modules/jolt_physics/objects/jolt_group_filter.h @@ -0,0 +1,51 @@ +/**************************************************************************/ +/* jolt_group_filter.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_GROUP_FILTER_H +#define JOLT_GROUP_FILTER_H + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/Collision/CollisionGroup.h" +#include "Jolt/Physics/Collision/GroupFilter.h" + +class JoltObject3D; + +class JoltGroupFilter final : public JPH::GroupFilter { + virtual bool CanCollide(const JPH::CollisionGroup &p_group1, const JPH::CollisionGroup &p_group2) const override; + +public: + inline static JoltGroupFilter *instance = nullptr; + + static void encode_object(const JoltObject3D *p_object, JPH::CollisionGroup::GroupID &r_group_id, JPH::CollisionGroup::SubGroupID &r_sub_group_id); + static const JoltObject3D *decode_object(JPH::CollisionGroup::GroupID p_group_id, JPH::CollisionGroup::SubGroupID p_sub_group_id); +}; + +#endif // JOLT_GROUP_FILTER_H diff --git a/modules/jolt_physics/objects/jolt_object_3d.cpp b/modules/jolt_physics/objects/jolt_object_3d.cpp new file mode 100644 index 000000000000..4d4dd46992c2 --- /dev/null +++ b/modules/jolt_physics/objects/jolt_object_3d.cpp @@ -0,0 +1,151 @@ +/**************************************************************************/ +/* jolt_object_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_object_3d.h" + +#include "../jolt_project_settings.h" +#include "../spaces/jolt_layers.h" +#include "../spaces/jolt_space_3d.h" +#include "jolt_group_filter.h" + +#include "core/error/error_macros.h" +#include "core/object/object.h" + +void JoltObject3D::_remove_from_space() { + if (unlikely(jolt_id.IsInvalid())) { + return; + } + + space->remove_body(jolt_id); + + jolt_id = JPH::BodyID(); +} + +void JoltObject3D::_reset_space() { + ERR_FAIL_NULL(space); + + _space_changing(); + _remove_from_space(); + _add_to_space(); + _space_changed(); +} + +void JoltObject3D::_update_object_layer() { + if (!in_space()) { + return; + } + + space->get_body_iface().SetObjectLayer(jolt_id, _get_object_layer()); +} + +void JoltObject3D::_collision_layer_changed() { + _update_object_layer(); +} + +void JoltObject3D::_collision_mask_changed() { + _update_object_layer(); +} + +JoltObject3D::JoltObject3D(ObjectType p_object_type) : + object_type(p_object_type) { +} + +JoltObject3D::~JoltObject3D() = default; + +Object *JoltObject3D::get_instance() const { + return ObjectDB::get_instance(instance_id); +} + +void JoltObject3D::set_space(JoltSpace3D *p_space) { + if (space == p_space) { + return; + } + + _space_changing(); + + if (space != nullptr) { + _remove_from_space(); + } + + space = p_space; + + if (space != nullptr) { + _add_to_space(); + } + + _space_changed(); +} + +void JoltObject3D::set_collision_layer(uint32_t p_layer) { + if (p_layer == collision_layer) { + return; + } + + collision_layer = p_layer; + + _collision_layer_changed(); +} + +void JoltObject3D::set_collision_mask(uint32_t p_mask) { + if (p_mask == collision_mask) { + return; + } + + collision_mask = p_mask; + + _collision_mask_changed(); +} + +bool JoltObject3D::can_collide_with(const JoltObject3D &p_other) const { + return (collision_mask & p_other.get_collision_layer()) != 0; +} + +bool JoltObject3D::can_interact_with(const JoltObject3D &p_other) const { + if (const JoltBody3D *other_body = p_other.as_body()) { + return can_interact_with(*other_body); + } else if (const JoltArea3D *other_area = p_other.as_area()) { + return can_interact_with(*other_area); + } else if (const JoltSoftBody3D *other_soft_body = p_other.as_soft_body()) { + return can_interact_with(*other_soft_body); + } else { + ERR_FAIL_V_MSG(false, vformat("Unhandled object type: '%d'. This should not happen. Please report this.", p_other.get_type())); + } +} + +void JoltObject3D::pre_step(float p_step, JPH::Body &p_jolt_body) { +} + +void JoltObject3D::post_step(float p_step, JPH::Body &p_jolt_body) { +} + +String JoltObject3D::to_string() const { + Object *instance = get_instance(); + return instance != nullptr ? instance->to_string() : ""; +} diff --git a/modules/jolt_physics/objects/jolt_object_3d.h b/modules/jolt_physics/objects/jolt_object_3d.h new file mode 100644 index 000000000000..6fa9cc79050d --- /dev/null +++ b/modules/jolt_physics/objects/jolt_object_3d.h @@ -0,0 +1,157 @@ +/**************************************************************************/ +/* jolt_object_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_OBJECT_3D_H +#define JOLT_OBJECT_3D_H + +#include "../shapes/jolt_shape_instance_3d.h" + +#include "core/math/vector3.h" +#include "core/object/object.h" +#include "core/string/ustring.h" +#include "core/templates/local_vector.h" +#include "core/templates/rid.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/Body/Body.h" +#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h" +#include "Jolt/Physics/Collision/ObjectLayer.h" + +class JoltArea3D; +class JoltBody3D; +class JoltShapedObject3D; +class JoltShape3D; +class JoltSoftBody3D; +class JoltSpace3D; + +class JoltObject3D { +public: + enum ObjectType : char { + OBJECT_TYPE_INVALID, + OBJECT_TYPE_BODY, + OBJECT_TYPE_SOFT_BODY, + OBJECT_TYPE_AREA, + }; + +protected: + LocalVector shapes; + + RID rid; + ObjectID instance_id; + JoltSpace3D *space = nullptr; + JPH::BodyID jolt_id; + + uint32_t collision_layer = 1; + uint32_t collision_mask = 1; + + ObjectType object_type = OBJECT_TYPE_INVALID; + + bool pickable = false; + + virtual JPH::BroadPhaseLayer _get_broad_phase_layer() const = 0; + virtual JPH::ObjectLayer _get_object_layer() const = 0; + + virtual void _add_to_space() = 0; + virtual void _remove_from_space(); + + void _reset_space(); + + void _update_object_layer(); + + virtual void _collision_layer_changed(); + virtual void _collision_mask_changed(); + + virtual void _space_changing() {} + virtual void _space_changed() {} + +public: + explicit JoltObject3D(ObjectType p_object_type); + virtual ~JoltObject3D() = 0; + + ObjectType get_type() const { return object_type; } + + bool is_body() const { return object_type == OBJECT_TYPE_BODY; } + bool is_soft_body() const { return object_type == OBJECT_TYPE_SOFT_BODY; } + bool is_area() const { return object_type == OBJECT_TYPE_AREA; } + bool is_shaped() const { return object_type != OBJECT_TYPE_SOFT_BODY; } + + JoltShapedObject3D *as_shaped() { return is_shaped() ? reinterpret_cast(this) : nullptr; } + const JoltShapedObject3D *as_shaped() const { return is_shaped() ? reinterpret_cast(this) : nullptr; } + + JoltBody3D *as_body() { return is_body() ? reinterpret_cast(this) : nullptr; } + const JoltBody3D *as_body() const { return is_body() ? reinterpret_cast(this) : nullptr; } + + JoltSoftBody3D *as_soft_body() { return is_soft_body() ? reinterpret_cast(this) : nullptr; } + const JoltSoftBody3D *as_soft_body() const { return is_soft_body() ? reinterpret_cast(this) : nullptr; } + + JoltArea3D *as_area() { return is_area() ? reinterpret_cast(this) : nullptr; } + const JoltArea3D *as_area() const { return is_area() ? reinterpret_cast(this) : nullptr; } + + RID get_rid() const { return rid; } + void set_rid(const RID &p_rid) { rid = p_rid; } + + ObjectID get_instance_id() const { return instance_id; } + void set_instance_id(ObjectID p_id) { instance_id = p_id; } + Object *get_instance() const; + + JPH::BodyID get_jolt_id() const { return jolt_id; } + + JoltSpace3D *get_space() const { return space; } + void set_space(JoltSpace3D *p_space); + bool in_space() const { return space != nullptr && !jolt_id.IsInvalid(); } + + uint32_t get_collision_layer() const { return collision_layer; } + void set_collision_layer(uint32_t p_layer); + + uint32_t get_collision_mask() const { return collision_mask; } + void set_collision_mask(uint32_t p_mask); + + virtual Vector3 get_velocity_at_position(const Vector3 &p_position) const = 0; + + bool is_pickable() const { return pickable; } + void set_pickable(bool p_enabled) { pickable = p_enabled; } + + bool can_collide_with(const JoltObject3D &p_other) const; + bool can_interact_with(const JoltObject3D &p_other) const; + + virtual bool can_interact_with(const JoltBody3D &p_other) const = 0; + virtual bool can_interact_with(const JoltSoftBody3D &p_other) const = 0; + virtual bool can_interact_with(const JoltArea3D &p_other) const = 0; + + virtual bool reports_contacts() const = 0; + + virtual void pre_step(float p_step, JPH::Body &p_jolt_body); + virtual void post_step(float p_step, JPH::Body &p_jolt_body); + + String to_string() const; +}; + +#endif // JOLT_OBJECT_3D_H diff --git a/modules/jolt_physics/objects/jolt_physics_direct_body_state_3d.cpp b/modules/jolt_physics/objects/jolt_physics_direct_body_state_3d.cpp new file mode 100644 index 000000000000..1d5090a9c34f --- /dev/null +++ b/modules/jolt_physics/objects/jolt_physics_direct_body_state_3d.cpp @@ -0,0 +1,247 @@ +/**************************************************************************/ +/* jolt_physics_direct_body_state_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_physics_direct_body_state_3d.h" + +#include "../spaces/jolt_physics_direct_space_state_3d.h" +#include "../spaces/jolt_space_3d.h" +#include "jolt_body_3d.h" + +#include "core/error/error_macros.h" + +JoltPhysicsDirectBodyState3D::JoltPhysicsDirectBodyState3D(JoltBody3D *p_body) : + body(p_body) { +} + +Vector3 JoltPhysicsDirectBodyState3D::get_total_gravity() const { + return body->get_gravity(); +} + +real_t JoltPhysicsDirectBodyState3D::get_total_angular_damp() const { + return (real_t)body->get_total_angular_damp(); +} + +real_t JoltPhysicsDirectBodyState3D::get_total_linear_damp() const { + return (real_t)body->get_total_linear_damp(); +} + +Vector3 JoltPhysicsDirectBodyState3D::get_center_of_mass() const { + return body->get_center_of_mass_relative(); +} + +Vector3 JoltPhysicsDirectBodyState3D::get_center_of_mass_local() const { + return body->get_center_of_mass_local(); +} + +Basis JoltPhysicsDirectBodyState3D::get_principal_inertia_axes() const { + return body->get_principal_inertia_axes(); +} + +real_t JoltPhysicsDirectBodyState3D::get_inverse_mass() const { + return 1.0 / body->get_mass(); +} + +Vector3 JoltPhysicsDirectBodyState3D::get_inverse_inertia() const { + return body->get_inverse_inertia(); +} + +Basis JoltPhysicsDirectBodyState3D::get_inverse_inertia_tensor() const { + return body->get_inverse_inertia_tensor(); +} + +Vector3 JoltPhysicsDirectBodyState3D::get_linear_velocity() const { + return body->get_linear_velocity(); +} + +void JoltPhysicsDirectBodyState3D::set_linear_velocity(const Vector3 &p_velocity) { + return body->set_linear_velocity(p_velocity); +} + +Vector3 JoltPhysicsDirectBodyState3D::get_angular_velocity() const { + return body->get_angular_velocity(); +} + +void JoltPhysicsDirectBodyState3D::set_angular_velocity(const Vector3 &p_velocity) { + return body->set_angular_velocity(p_velocity); +} + +void JoltPhysicsDirectBodyState3D::set_transform(const Transform3D &p_transform) { + return body->set_transform(p_transform); +} + +Transform3D JoltPhysicsDirectBodyState3D::get_transform() const { + return body->get_transform_scaled(); +} + +Vector3 JoltPhysicsDirectBodyState3D::get_velocity_at_local_position(const Vector3 &p_local_position) const { + return body->get_velocity_at_position(body->get_position() + p_local_position); +} + +void JoltPhysicsDirectBodyState3D::apply_central_impulse(const Vector3 &p_impulse) { + return body->apply_central_impulse(p_impulse); +} + +void JoltPhysicsDirectBodyState3D::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) { + return body->apply_impulse(p_impulse, p_position); +} + +void JoltPhysicsDirectBodyState3D::apply_torque_impulse(const Vector3 &p_impulse) { + return body->apply_torque_impulse(p_impulse); +} + +void JoltPhysicsDirectBodyState3D::apply_central_force(const Vector3 &p_force) { + return body->apply_central_force(p_force); +} + +void JoltPhysicsDirectBodyState3D::apply_force(const Vector3 &p_force, const Vector3 &p_position) { + return body->apply_force(p_force, p_position); +} + +void JoltPhysicsDirectBodyState3D::apply_torque(const Vector3 &p_torque) { + return body->apply_torque(p_torque); +} + +void JoltPhysicsDirectBodyState3D::add_constant_central_force(const Vector3 &p_force) { + return body->add_constant_central_force(p_force); +} + +void JoltPhysicsDirectBodyState3D::add_constant_force(const Vector3 &p_force, const Vector3 &p_position) { + return body->add_constant_force(p_force, p_position); +} + +void JoltPhysicsDirectBodyState3D::add_constant_torque(const Vector3 &p_torque) { + return body->add_constant_torque(p_torque); +} + +Vector3 JoltPhysicsDirectBodyState3D::get_constant_force() const { + return body->get_constant_force(); +} + +void JoltPhysicsDirectBodyState3D::set_constant_force(const Vector3 &p_force) { + return body->set_constant_force(p_force); +} + +Vector3 JoltPhysicsDirectBodyState3D::get_constant_torque() const { + return body->get_constant_torque(); +} + +void JoltPhysicsDirectBodyState3D::set_constant_torque(const Vector3 &p_torque) { + return body->set_constant_torque(p_torque); +} + +bool JoltPhysicsDirectBodyState3D::is_sleeping() const { + return body->is_sleeping(); +} + +void JoltPhysicsDirectBodyState3D::set_sleep_state(bool p_enabled) { + body->set_is_sleeping(p_enabled); +} + +int JoltPhysicsDirectBodyState3D::get_contact_count() const { + return body->get_contact_count(); +} + +Vector3 JoltPhysicsDirectBodyState3D::get_contact_local_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), Vector3()); + return body->get_contact(p_contact_idx).position; +} + +Vector3 JoltPhysicsDirectBodyState3D::get_contact_local_normal(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), Vector3()); + return body->get_contact(p_contact_idx).normal; +} + +Vector3 JoltPhysicsDirectBodyState3D::get_contact_impulse(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), Vector3()); + return body->get_contact(p_contact_idx).impulse; +} + +int JoltPhysicsDirectBodyState3D::get_contact_local_shape(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), 0); + return body->get_contact(p_contact_idx).shape_index; +} + +Vector3 JoltPhysicsDirectBodyState3D::get_contact_local_velocity_at_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), Vector3()); + return body->get_contact(p_contact_idx).velocity; +} + +RID JoltPhysicsDirectBodyState3D::get_contact_collider(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), RID()); + return body->get_contact(p_contact_idx).collider_rid; +} + +Vector3 JoltPhysicsDirectBodyState3D::get_contact_collider_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), Vector3()); + return body->get_contact(p_contact_idx).collider_position; +} + +ObjectID JoltPhysicsDirectBodyState3D::get_contact_collider_id(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), ObjectID()); + return body->get_contact(p_contact_idx).collider_id; +} + +Object *JoltPhysicsDirectBodyState3D::get_contact_collider_object(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), nullptr); + return ObjectDB::get_instance(body->get_contact(p_contact_idx).collider_id); +} + +int JoltPhysicsDirectBodyState3D::get_contact_collider_shape(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), 0); + return body->get_contact(p_contact_idx).collider_shape_index; +} + +Vector3 JoltPhysicsDirectBodyState3D::get_contact_collider_velocity_at_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), Vector3()); + return body->get_contact(p_contact_idx).collider_velocity; +} + +real_t JoltPhysicsDirectBodyState3D::get_step() const { + return (real_t)body->get_space()->get_last_step(); +} + +void JoltPhysicsDirectBodyState3D::integrate_forces() { + const float step = (float)get_step(); + + Vector3 linear_velocity = get_linear_velocity(); + Vector3 angular_velocity = get_angular_velocity(); + + linear_velocity *= MAX(1.0f - (float)get_total_linear_damp() * step, 0.0f); + angular_velocity *= MAX(1.0f - (float)get_total_angular_damp() * step, 0.0f); + + linear_velocity += get_total_gravity() * step; + + set_linear_velocity(linear_velocity); + set_angular_velocity(angular_velocity); +} + +PhysicsDirectSpaceState3D *JoltPhysicsDirectBodyState3D::get_space_state() { + return body->get_space()->get_direct_state(); +} diff --git a/modules/jolt_physics/objects/jolt_physics_direct_body_state_3d.h b/modules/jolt_physics/objects/jolt_physics_direct_body_state_3d.h new file mode 100644 index 000000000000..e2a7ad78cb6a --- /dev/null +++ b/modules/jolt_physics/objects/jolt_physics_direct_body_state_3d.h @@ -0,0 +1,116 @@ +/**************************************************************************/ +/* jolt_physics_direct_body_state_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_PHYSICS_DIRECT_BODY_STATE_3D_H +#define JOLT_PHYSICS_DIRECT_BODY_STATE_3D_H + +#include "servers/physics_server_3d.h" + +class JoltBody3D; + +class JoltPhysicsDirectBodyState3D final : public PhysicsDirectBodyState3D { + GDCLASS(JoltPhysicsDirectBodyState3D, PhysicsDirectBodyState3D) + + JoltBody3D *body = nullptr; + + static void _bind_methods() {} + +public: + JoltPhysicsDirectBodyState3D() = default; + + explicit JoltPhysicsDirectBodyState3D(JoltBody3D *p_body); + + virtual Vector3 get_total_gravity() const override; + virtual real_t get_total_linear_damp() const override; + virtual real_t get_total_angular_damp() const override; + + virtual Vector3 get_center_of_mass() const override; + virtual Vector3 get_center_of_mass_local() const override; + virtual Basis get_principal_inertia_axes() const override; + + virtual real_t get_inverse_mass() const override; + virtual Vector3 get_inverse_inertia() const override; + virtual Basis get_inverse_inertia_tensor() const override; + + virtual void set_linear_velocity(const Vector3 &p_velocity) override; + virtual Vector3 get_linear_velocity() const override; + + virtual void set_angular_velocity(const Vector3 &p_velocity) override; + virtual Vector3 get_angular_velocity() const override; + + virtual void set_transform(const Transform3D &p_transform) override; + virtual Transform3D get_transform() const override; + + virtual Vector3 get_velocity_at_local_position(const Vector3 &p_local_position) const override; + + virtual void apply_central_impulse(const Vector3 &p_impulse) override; + virtual void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) override; + virtual void apply_torque_impulse(const Vector3 &p_impulse) override; + + virtual void apply_central_force(const Vector3 &p_force) override; + virtual void apply_force(const Vector3 &p_force, const Vector3 &p_position) override; + virtual void apply_torque(const Vector3 &p_torque) override; + + virtual void add_constant_central_force(const Vector3 &p_force) override; + virtual void add_constant_force(const Vector3 &p_force, const Vector3 &p_position) override; + virtual void add_constant_torque(const Vector3 &p_torque) override; + + virtual void set_constant_force(const Vector3 &p_force) override; + virtual Vector3 get_constant_force() const override; + + virtual void set_constant_torque(const Vector3 &p_torque) override; + virtual Vector3 get_constant_torque() const override; + + virtual void set_sleep_state(bool p_enabled) override; + virtual bool is_sleeping() const override; + + virtual int get_contact_count() const override; + + virtual Vector3 get_contact_local_position(int p_contact_idx) const override; + virtual Vector3 get_contact_local_normal(int p_contact_idx) const override; + virtual Vector3 get_contact_impulse(int p_contact_idx) const override; + virtual int get_contact_local_shape(int p_contact_idx) const override; + virtual Vector3 get_contact_local_velocity_at_position(int p_contact_idx) const override; + + virtual RID get_contact_collider(int p_contact_idx) const override; + virtual Vector3 get_contact_collider_position(int p_contact_idx) const override; + virtual ObjectID get_contact_collider_id(int p_contact_idx) const override; + virtual Object *get_contact_collider_object(int p_contact_idx) const override; + virtual int get_contact_collider_shape(int p_contact_idx) const override; + virtual Vector3 get_contact_collider_velocity_at_position(int p_contact_idx) const override; + + virtual real_t get_step() const override; + + virtual void integrate_forces() override; + + virtual PhysicsDirectSpaceState3D *get_space_state() override; +}; + +#endif // JOLT_PHYSICS_DIRECT_BODY_STATE_3D_H diff --git a/modules/jolt_physics/objects/jolt_shaped_object_3d.cpp b/modules/jolt_physics/objects/jolt_shaped_object_3d.cpp new file mode 100644 index 000000000000..e68f30348b8d --- /dev/null +++ b/modules/jolt_physics/objects/jolt_shaped_object_3d.cpp @@ -0,0 +1,433 @@ +/**************************************************************************/ +/* jolt_shaped_object_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_shaped_object_3d.h" + +#include "../misc/jolt_type_conversions.h" +#include "../shapes/jolt_custom_double_sided_shape.h" +#include "../shapes/jolt_shape_3d.h" +#include "../spaces/jolt_space_3d.h" + +#include "core/error/error_macros.h" + +#include "Jolt/Physics/Collision/Shape/EmptyShape.h" +#include "Jolt/Physics/Collision/Shape/StaticCompoundShape.h" + +bool JoltShapedObject3D::_is_big() const { + // This number is completely arbitrary, and mostly just needs to capture `WorldBoundaryShape3D`, which needs to be kept out of the normal broadphase layers. + return get_aabb().get_longest_axis_size() >= 1000.0f; +} + +JPH::ShapeRefC JoltShapedObject3D::_try_build_shape() { + int built_shapes = 0; + + for (JoltShapeInstance3D &shape : shapes) { + if (shape.is_enabled() && shape.try_build()) { + built_shapes += 1; + } + } + + if (unlikely(built_shapes == 0)) { + return nullptr; + } + + JPH::ShapeRefC result = built_shapes == 1 ? _try_build_single_shape() : _try_build_compound_shape(); + if (unlikely(result == nullptr)) { + return nullptr; + } + + if (has_custom_center_of_mass()) { + result = JoltShape3D::with_center_of_mass(result, get_center_of_mass_custom()); + } + + if (scale != Vector3(1, 1, 1)) { + Vector3 actual_scale = scale; + JOLT_ENSURE_SCALE_VALID(result, actual_scale, vformat("Failed to correctly scale body '%s'.", to_string())); + result = JoltShape3D::with_scale(result, actual_scale); + } + + if (is_area()) { + result = JoltShape3D::with_double_sided(result, true); + } + + return result; +} + +JPH::ShapeRefC JoltShapedObject3D::_try_build_single_shape() { + for (int shape_index = 0; shape_index < (int)shapes.size(); ++shape_index) { + const JoltShapeInstance3D &sub_shape = shapes[shape_index]; + + if (!sub_shape.is_enabled() || !sub_shape.is_built()) { + continue; + } + + JPH::ShapeRefC jolt_sub_shape = sub_shape.get_jolt_ref(); + + Vector3 sub_shape_scale = sub_shape.get_scale(); + const Transform3D sub_shape_transform = sub_shape.get_transform_unscaled(); + + if (sub_shape_scale != Vector3(1, 1, 1)) { + JOLT_ENSURE_SCALE_VALID(jolt_sub_shape, sub_shape_scale, vformat("Failed to correctly scale shape at index %d in body '%s'.", shape_index, to_string())); + jolt_sub_shape = JoltShape3D::with_scale(jolt_sub_shape, sub_shape_scale); + } + + if (sub_shape_transform != Transform3D()) { + jolt_sub_shape = JoltShape3D::with_basis_origin(jolt_sub_shape, sub_shape_transform.basis, sub_shape_transform.origin); + } + + return jolt_sub_shape; + } + + return nullptr; +} + +JPH::ShapeRefC JoltShapedObject3D::_try_build_compound_shape() { + JPH::StaticCompoundShapeSettings compound_shape_settings; + + for (int shape_index = 0; shape_index < (int)shapes.size(); ++shape_index) { + const JoltShapeInstance3D &sub_shape = shapes[shape_index]; + + if (!sub_shape.is_enabled() || !sub_shape.is_built()) { + continue; + } + + JPH::ShapeRefC jolt_sub_shape = sub_shape.get_jolt_ref(); + + Vector3 sub_shape_scale = sub_shape.get_scale(); + const Transform3D sub_shape_transform = sub_shape.get_transform_unscaled(); + + if (sub_shape_scale != Vector3(1, 1, 1)) { + JOLT_ENSURE_SCALE_VALID(jolt_sub_shape, sub_shape_scale, vformat("Failed to correctly scale shape at index %d in body '%s'.", shape_index, to_string())); + jolt_sub_shape = JoltShape3D::with_scale(jolt_sub_shape, sub_shape_scale); + } + + compound_shape_settings.AddShape(to_jolt(sub_shape_transform.origin), to_jolt(sub_shape_transform.basis), jolt_sub_shape); + } + + const JPH::ShapeSettings::ShapeResult shape_result = compound_shape_settings.Create(); + ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to create compound shape with sub-shape count '%d'. It returned the following error: '%s'.", (int)compound_shape_settings.mSubShapes.size(), to_godot(shape_result.GetError()))); + + return shape_result.Get(); +} + +void JoltShapedObject3D::_shapes_changed() { + _update_shape(); + _update_object_layer(); +} + +void JoltShapedObject3D::_space_changing() { + JoltObject3D::_space_changing(); + + if (space != nullptr) { + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + jolt_settings = new JPH::BodyCreationSettings(body->GetBodyCreationSettings()); + } +} + +void JoltShapedObject3D::_update_shape() { + if (!in_space()) { + _shapes_built(); + return; + } + + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + previous_jolt_shape = jolt_shape; + jolt_shape = build_shape(); + + if (jolt_shape == previous_jolt_shape) { + return; + } + + space->get_body_iface().SetShape(jolt_id, jolt_shape, false, JPH::EActivation::DontActivate); + + _shapes_built(); +} + +JoltShapedObject3D::JoltShapedObject3D(ObjectType p_object_type) : + JoltObject3D(p_object_type) { + jolt_settings->mAllowSleeping = true; + jolt_settings->mFriction = 1.0f; + jolt_settings->mRestitution = 0.0f; + jolt_settings->mLinearDamping = 0.0f; + jolt_settings->mAngularDamping = 0.0f; + jolt_settings->mGravityFactor = 0.0f; +} + +JoltShapedObject3D::~JoltShapedObject3D() { + if (jolt_settings != nullptr) { + delete jolt_settings; + jolt_settings = nullptr; + } +} + +Transform3D JoltShapedObject3D::get_transform_unscaled() const { + if (!in_space()) { + return { to_godot(jolt_settings->mRotation), to_godot(jolt_settings->mPosition) }; + } + + const JoltReadableBody3D body = space->read_body(jolt_id); + ERR_FAIL_COND_V(body.is_invalid(), Transform3D()); + + return { to_godot(body->GetRotation()), to_godot(body->GetPosition()) }; +} + +Transform3D JoltShapedObject3D::get_transform_scaled() const { + return get_transform_unscaled().scaled_local(scale); +} + +Basis JoltShapedObject3D::get_basis() const { + if (!in_space()) { + return to_godot(jolt_settings->mRotation); + } + + const JoltReadableBody3D body = space->read_body(jolt_id); + ERR_FAIL_COND_V(body.is_invalid(), Basis()); + + return to_godot(body->GetRotation()); +} + +Vector3 JoltShapedObject3D::get_position() const { + if (!in_space()) { + return to_godot(jolt_settings->mPosition); + } + + const JoltReadableBody3D body = space->read_body(jolt_id); + ERR_FAIL_COND_V(body.is_invalid(), Vector3()); + + return to_godot(body->GetPosition()); +} + +Vector3 JoltShapedObject3D::get_center_of_mass() const { + ERR_FAIL_NULL_V_MSG(space, Vector3(), vformat("Failed to retrieve center-of-mass of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string())); + + const JoltReadableBody3D body = space->read_body(jolt_id); + ERR_FAIL_COND_V(body.is_invalid(), Vector3()); + + return to_godot(body->GetCenterOfMassPosition()); +} + +Vector3 JoltShapedObject3D::get_center_of_mass_relative() const { + return get_center_of_mass() - get_position(); +} + +Vector3 JoltShapedObject3D::get_center_of_mass_local() const { + ERR_FAIL_NULL_V_MSG(space, Vector3(), vformat("Failed to retrieve local center-of-mass of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string())); + + return get_transform_scaled().xform_inv(get_center_of_mass()); +} + +Vector3 JoltShapedObject3D::get_linear_velocity() const { + if (!in_space()) { + return to_godot(jolt_settings->mLinearVelocity); + } + + const JoltReadableBody3D body = space->read_body(jolt_id); + ERR_FAIL_COND_V(body.is_invalid(), Vector3()); + + return to_godot(body->GetLinearVelocity()); +} + +Vector3 JoltShapedObject3D::get_angular_velocity() const { + if (!in_space()) { + return to_godot(jolt_settings->mAngularVelocity); + } + + const JoltReadableBody3D body = space->read_body(jolt_id); + ERR_FAIL_COND_V(body.is_invalid(), Vector3()); + + return to_godot(body->GetAngularVelocity()); +} + +AABB JoltShapedObject3D::get_aabb() const { + AABB result; + + for (const JoltShapeInstance3D &shape : shapes) { + if (shape.is_disabled()) { + continue; + } + + if (result == AABB()) { + result = shape.get_aabb(); + } else { + result.merge_with(shape.get_aabb()); + } + } + + return get_transform_scaled().xform(result); +} + +JPH::ShapeRefC JoltShapedObject3D::build_shape() { + JPH::ShapeRefC new_shape = _try_build_shape(); + + if (new_shape == nullptr) { + if (has_custom_center_of_mass()) { + new_shape = JPH::EmptyShapeSettings(to_jolt(get_center_of_mass_custom())).Create().Get(); + } else { + new_shape = new JPH::EmptyShape(); + } + } + + return new_shape; +} + +void JoltShapedObject3D::add_shape(JoltShape3D *p_shape, Transform3D p_transform, bool p_disabled) { + JOLT_ENSURE_SCALE_NOT_ZERO(p_transform, vformat("An invalid transform was passed when adding shape at index %d to physics body '%s'.", shapes.size(), to_string())); + + shapes.push_back(JoltShapeInstance3D(this, p_shape, p_transform.orthonormalized(), p_transform.basis.get_scale(), p_disabled)); + + _shapes_changed(); +} + +void JoltShapedObject3D::remove_shape(const JoltShape3D *p_shape) { + for (int i = shapes.size() - 1; i >= 0; i--) { + if (shapes[i].get_shape() == p_shape) { + shapes.remove_at(i); + } + } + + _shapes_changed(); +} + +void JoltShapedObject3D::remove_shape(int p_index) { + ERR_FAIL_INDEX(p_index, (int)shapes.size()); + shapes.remove_at(p_index); + + _shapes_changed(); +} + +JoltShape3D *JoltShapedObject3D::get_shape(int p_index) const { + ERR_FAIL_INDEX_V(p_index, (int)shapes.size(), nullptr); + return shapes[p_index].get_shape(); +} + +void JoltShapedObject3D::set_shape(int p_index, JoltShape3D *p_shape) { + ERR_FAIL_INDEX(p_index, (int)shapes.size()); + shapes[p_index] = JoltShapeInstance3D(this, p_shape); + + _shapes_changed(); +} + +void JoltShapedObject3D::clear_shapes() { + shapes.clear(); + + _shapes_changed(); +} + +int JoltShapedObject3D::find_shape_index(uint32_t p_shape_instance_id) const { + for (int i = 0; i < (int)shapes.size(); ++i) { + if (shapes[i].get_id() == p_shape_instance_id) { + return i; + } + } + + return -1; +} + +int JoltShapedObject3D::find_shape_index(const JPH::SubShapeID &p_sub_shape_id) const { + ERR_FAIL_NULL_V(jolt_shape, -1); + return find_shape_index((uint32_t)jolt_shape->GetSubShapeUserData(p_sub_shape_id)); +} + +JoltShape3D *JoltShapedObject3D::find_shape(uint32_t p_shape_instance_id) const { + const int shape_index = find_shape_index(p_shape_instance_id); + return shape_index != -1 ? shapes[shape_index].get_shape() : nullptr; +} + +JoltShape3D *JoltShapedObject3D::find_shape(const JPH::SubShapeID &p_sub_shape_id) const { + const int shape_index = find_shape_index(p_sub_shape_id); + return shape_index != -1 ? shapes[shape_index].get_shape() : nullptr; +} + +Transform3D JoltShapedObject3D::get_shape_transform_unscaled(int p_index) const { + ERR_FAIL_INDEX_V(p_index, (int)shapes.size(), Transform3D()); + return shapes[p_index].get_transform_unscaled(); +} + +Transform3D JoltShapedObject3D::get_shape_transform_scaled(int p_index) const { + ERR_FAIL_INDEX_V(p_index, (int)shapes.size(), Transform3D()); + return shapes[p_index].get_transform_scaled(); +} + +void JoltShapedObject3D::set_shape_transform(int p_index, Transform3D p_transform) { + ERR_FAIL_INDEX(p_index, (int)shapes.size()); + JOLT_ENSURE_SCALE_NOT_ZERO(p_transform, "Failed to correctly set transform for shape at index %d in body '%s'."); + + Vector3 new_scale = p_transform.basis.get_scale(); + p_transform.basis.orthonormalize(); + + JoltShapeInstance3D &shape = shapes[p_index]; + + if (shape.get_transform_unscaled() == p_transform && shape.get_scale() == new_scale) { + return; + } + + shape.set_transform(p_transform); + shape.set_scale(new_scale); + + _shapes_changed(); +} + +Vector3 JoltShapedObject3D::get_shape_scale(int p_index) const { + ERR_FAIL_INDEX_V(p_index, (int)shapes.size(), Vector3()); + return shapes[p_index].get_scale(); +} + +bool JoltShapedObject3D::is_shape_disabled(int p_index) const { + ERR_FAIL_INDEX_V(p_index, (int)shapes.size(), false); + return shapes[p_index].is_disabled(); +} + +void JoltShapedObject3D::set_shape_disabled(int p_index, bool p_disabled) { + ERR_FAIL_INDEX(p_index, (int)shapes.size()); + + JoltShapeInstance3D &shape = shapes[p_index]; + + if (shape.is_disabled() == p_disabled) { + return; + } + + if (p_disabled) { + shape.disable(); + } else { + shape.enable(); + } + + _shapes_changed(); +} + +void JoltShapedObject3D::post_step(float p_step, JPH::Body &p_jolt_body) { + JoltObject3D::post_step(p_step, p_jolt_body); + + previous_jolt_shape = nullptr; +} diff --git a/modules/jolt_physics/objects/jolt_shaped_object_3d.h b/modules/jolt_physics/objects/jolt_shaped_object_3d.h new file mode 100644 index 000000000000..9632746287b9 --- /dev/null +++ b/modules/jolt_physics/objects/jolt_shaped_object_3d.h @@ -0,0 +1,123 @@ +/**************************************************************************/ +/* jolt_shaped_object_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_SHAPED_OBJECT_3D_H +#define JOLT_SHAPED_OBJECT_3D_H + +#include "jolt_object_3d.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/Body/Body.h" +#include "Jolt/Physics/Body/BodyCreationSettings.h" + +class JoltShapedObject3D : public JoltObject3D { + friend class JoltShape3D; + +protected: + Vector3 scale = Vector3(1, 1, 1); + + JPH::ShapeRefC jolt_shape; + JPH::ShapeRefC previous_jolt_shape; + + JPH::BodyCreationSettings *jolt_settings = new JPH::BodyCreationSettings(); + + virtual JPH::EMotionType _get_motion_type() const = 0; + + bool _is_big() const; + + JPH::ShapeRefC _try_build_shape(); + JPH::ShapeRefC _try_build_single_shape(); + JPH::ShapeRefC _try_build_compound_shape(); + + virtual void _shapes_changed(); + virtual void _shapes_built() {} + virtual void _space_changing() override; + + void _update_shape(); + +public: + explicit JoltShapedObject3D(ObjectType p_object_type); + virtual ~JoltShapedObject3D() override; + + Transform3D get_transform_unscaled() const; + Transform3D get_transform_scaled() const; + + Vector3 get_scale() const { return scale; } + Basis get_basis() const; + Vector3 get_position() const; + + Vector3 get_center_of_mass() const; + Vector3 get_center_of_mass_relative() const; + Vector3 get_center_of_mass_local() const; + + Vector3 get_linear_velocity() const; + Vector3 get_angular_velocity() const; + + AABB get_aabb() const; + + virtual bool has_custom_center_of_mass() const = 0; + virtual Vector3 get_center_of_mass_custom() const = 0; + + JPH::ShapeRefC build_shape(); + + const JPH::Shape *get_jolt_shape() const { return jolt_shape; } + const JPH::Shape *get_previous_jolt_shape() const { return previous_jolt_shape; } + + void add_shape(JoltShape3D *p_shape, Transform3D p_transform, bool p_disabled); + void remove_shape(const JoltShape3D *p_shape); + void remove_shape(int p_index); + + JoltShape3D *get_shape(int p_index) const; + void set_shape(int p_index, JoltShape3D *p_shape); + + void clear_shapes(); + + int get_shape_count() const { return shapes.size(); } + + int find_shape_index(uint32_t p_shape_instance_id) const; + int find_shape_index(const JPH::SubShapeID &p_sub_shape_id) const; + + JoltShape3D *find_shape(uint32_t p_shape_instance_id) const; + JoltShape3D *find_shape(const JPH::SubShapeID &p_sub_shape_id) const; + + Transform3D get_shape_transform_unscaled(int p_index) const; + Transform3D get_shape_transform_scaled(int p_index) const; + void set_shape_transform(int p_index, Transform3D p_transform); + + Vector3 get_shape_scale(int p_index) const; + + bool is_shape_disabled(int p_index) const; + void set_shape_disabled(int p_index, bool p_disabled); + + virtual void post_step(float p_step, JPH::Body &p_jolt_body) override; +}; + +#endif // JOLT_SHAPED_OBJECT_3D_H diff --git a/modules/jolt_physics/objects/jolt_soft_body_3d.cpp b/modules/jolt_physics/objects/jolt_soft_body_3d.cpp new file mode 100644 index 000000000000..142eb10526c0 --- /dev/null +++ b/modules/jolt_physics/objects/jolt_soft_body_3d.cpp @@ -0,0 +1,733 @@ +/**************************************************************************/ +/* jolt_soft_body_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_soft_body_3d.h" + +#include "../jolt_project_settings.h" +#include "../misc/jolt_type_conversions.h" +#include "../spaces/jolt_broad_phase_layer.h" +#include "../spaces/jolt_space_3d.h" +#include "jolt_area_3d.h" +#include "jolt_body_3d.h" +#include "jolt_group_filter.h" + +#include "core/error/error_macros.h" +#include "servers/rendering_server.h" + +#include "Jolt/Physics/SoftBody/SoftBodyMotionProperties.h" + +namespace { + +bool is_face_degenerate(const int p_face[3]) { + return p_face[0] == p_face[1] || p_face[0] == p_face[2] || p_face[1] == p_face[2]; +} + +template +void pin_vertices(const JoltSoftBody3D &p_body, const HashSet &p_pinned_vertices, const LocalVector &p_mesh_to_physics, JPH::Array &r_physics_vertices) { + const int mesh_vertex_count = p_mesh_to_physics.size(); + const int physics_vertex_count = (int)r_physics_vertices.size(); + + for (int mesh_index : p_pinned_vertices) { + ERR_CONTINUE_MSG(mesh_index < 0 || mesh_index >= mesh_vertex_count, vformat("Index %d of pinned vertex in soft body '%s' is out of bounds. There are only %d vertices in the current mesh.", mesh_index, p_body.to_string(), mesh_vertex_count)); + + const int physics_index = p_mesh_to_physics[mesh_index]; + ERR_CONTINUE_MSG(physics_index < 0 || physics_index >= physics_vertex_count, vformat("Index %d of pinned vertex in soft body '%s' is out of bounds. There are only %d vertices in the current mesh. This should not happen. Please report this.", physics_index, p_body.to_string(), physics_vertex_count)); + + r_physics_vertices[physics_index].mInvMass = 0.0f; + } +} + +} // namespace + +JPH::BroadPhaseLayer JoltSoftBody3D::_get_broad_phase_layer() const { + return JoltBroadPhaseLayer::BODY_DYNAMIC; +} + +JPH::ObjectLayer JoltSoftBody3D::_get_object_layer() const { + ERR_FAIL_NULL_V(space, 0); + + return space->map_to_object_layer(_get_broad_phase_layer(), collision_layer, collision_mask); +} + +void JoltSoftBody3D::_space_changing() { + JoltObject3D::_space_changing(); + + _deref_shared_data(); + + if (space != nullptr && !jolt_id.IsInvalid()) { + const JoltReadableBody3D body = space->read_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + jolt_settings = new JPH::SoftBodyCreationSettings(body->GetSoftBodyCreationSettings()); + jolt_settings->mSettings = nullptr; + } +} + +void JoltSoftBody3D::_space_changed() { + JoltObject3D::_space_changed(); + + _update_mass(); + _update_pressure(); + _update_damping(); + _update_simulation_precision(); + _update_group_filter(); +} + +void JoltSoftBody3D::_add_to_space() { + if (unlikely(space == nullptr || !mesh.is_valid())) { + return; + } + + const bool has_valid_shared = _ref_shared_data(); + ERR_FAIL_COND(!has_valid_shared); + + JPH::CollisionGroup::GroupID group_id = 0; + JPH::CollisionGroup::SubGroupID sub_group_id = 0; + JoltGroupFilter::encode_object(this, group_id, sub_group_id); + + jolt_settings->mSettings = shared->settings; + jolt_settings->mUserData = reinterpret_cast(this); + jolt_settings->mObjectLayer = _get_object_layer(); + jolt_settings->mCollisionGroup = JPH::CollisionGroup(nullptr, group_id, sub_group_id); + jolt_settings->mMaxLinearVelocity = JoltProjectSettings::get_max_linear_velocity(); + + const JPH::BodyID new_jolt_id = space->add_soft_body(*this, *jolt_settings); + if (!new_jolt_id.IsInvalid()) { + jolt_id = new_jolt_id; + } + + delete jolt_settings; + jolt_settings = nullptr; +} + +bool JoltSoftBody3D::_ref_shared_data() { + HashMap::Iterator iter_shared_data = mesh_to_shared.find(mesh); + + if (iter_shared_data == mesh_to_shared.end()) { + RenderingServer *rendering = RenderingServer::get_singleton(); + + const Array mesh_data = rendering->mesh_surface_get_arrays(mesh, 0); + ERR_FAIL_COND_V(mesh_data.is_empty(), false); + + const PackedInt32Array mesh_indices = mesh_data[RenderingServer::ARRAY_INDEX]; + ERR_FAIL_COND_V(mesh_indices.is_empty(), false); + + const PackedVector3Array mesh_vertices = mesh_data[RenderingServer::ARRAY_VERTEX]; + ERR_FAIL_COND_V(mesh_vertices.is_empty(), false); + + iter_shared_data = mesh_to_shared.insert(mesh, Shared()); + + LocalVector &mesh_to_physics = iter_shared_data->value.mesh_to_physics; + + JPH::SoftBodySharedSettings &settings = *iter_shared_data->value.settings; + settings.mVertexRadius = JoltProjectSettings::get_soft_body_point_radius(); + + JPH::Array &physics_vertices = settings.mVertices; + JPH::Array &physics_faces = settings.mFaces; + + HashMap vertex_to_physics; + + const int mesh_vertex_count = mesh_vertices.size(); + const int mesh_index_count = mesh_indices.size(); + + mesh_to_physics.resize(mesh_vertex_count); + physics_vertices.reserve(mesh_vertex_count); + vertex_to_physics.reserve(mesh_vertex_count); + + int physics_index_count = 0; + + for (int i = 0; i < mesh_index_count; i += 3) { + int physics_face[3]; + int mesh_face[3]; + + for (int j = 0; j < 3; ++j) { + const int mesh_index = mesh_indices[i + j]; + const Vector3 vertex = mesh_vertices[mesh_index]; + + HashMap::Iterator iter_physics_index = vertex_to_physics.find(vertex); + + if (iter_physics_index == vertex_to_physics.end()) { + physics_vertices.emplace_back(JPH::Float3((float)vertex.x, (float)vertex.y, (float)vertex.z), JPH::Float3(0.0f, 0.0f, 0.0f), 1.0f); + + iter_physics_index = vertex_to_physics.insert(vertex, physics_index_count++); + } + + mesh_face[j] = mesh_index; + physics_face[j] = iter_physics_index->value; + mesh_to_physics[mesh_index] = iter_physics_index->value; + } + + ERR_CONTINUE_MSG(is_face_degenerate(physics_face), vformat("Failed to append face to soft body '%s'. Face was found to be degenerate. Face consist of indices %d, %d and %d.", to_string(), mesh_face[0], mesh_face[1], mesh_face[2])); + + // Jolt uses a different winding order, so we swap the indices to account for that. + physics_faces.emplace_back((JPH::uint32)physics_face[2], (JPH::uint32)physics_face[1], (JPH::uint32)physics_face[0]); + } + + // Pin whatever pinned vertices we have currently. This is used during the `Optimize` call below to order the + // constraints. Note that it's fine if the pinned vertices change later, but that will reduce the effectiveness + // of the constraints a bit. + pin_vertices(*this, pinned_vertices, mesh_to_physics, physics_vertices); + + // Since Godot's stiffness is input as a coefficient between 0 and 1, and Jolt uses actual stiffness for its + // edge constraints, we crudely map one to the other with an arbitrary constant. + const float stiffness = MAX(Math::pow(stiffness_coefficient, 3.0f) * 100000.0f, 0.000001f); + const float inverse_stiffness = 1.0f / stiffness; + + JPH::SoftBodySharedSettings::VertexAttributes vertex_attrib; + vertex_attrib.mCompliance = vertex_attrib.mShearCompliance = inverse_stiffness; + + settings.CreateConstraints(&vertex_attrib, 1, JPH::SoftBodySharedSettings::EBendType::None); + settings.Optimize(); + } else { + iter_shared_data->value.ref_count++; + } + + shared = &iter_shared_data->value; + + return true; +} + +void JoltSoftBody3D::_deref_shared_data() { + if (unlikely(shared == nullptr)) { + return; + } + + HashMap::Iterator iter = mesh_to_shared.find(mesh); + if (unlikely(iter == mesh_to_shared.end())) { + return; + } + + if (--iter->value.ref_count == 0) { + mesh_to_shared.remove(iter); + } + + shared = nullptr; +} + +void JoltSoftBody3D::_update_mass() { + if (!in_space()) { + return; + } + + JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + JPH::SoftBodyMotionProperties &motion_properties = static_cast(*body->GetMotionPropertiesUnchecked()); + + JPH::Array &physics_vertices = motion_properties.GetVertices(); + + const float inverse_vertex_mass = mass == 0.0f ? 1.0f : (float)physics_vertices.size() / mass; + + for (JPH::SoftBodyVertex &vertex : physics_vertices) { + vertex.mInvMass = inverse_vertex_mass; + } + + pin_vertices(*this, pinned_vertices, shared->mesh_to_physics, physics_vertices); +} + +void JoltSoftBody3D::_update_pressure() { + if (!in_space()) { + jolt_settings->mPressure = pressure; + return; + } + + JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + JPH::SoftBodyMotionProperties &motion_properties = static_cast(*body->GetMotionPropertiesUnchecked()); + + motion_properties.SetPressure(pressure); +} + +void JoltSoftBody3D::_update_damping() { + if (!in_space()) { + jolt_settings->mLinearDamping = linear_damping; + return; + } + + JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + JPH::SoftBodyMotionProperties &motion_properties = static_cast(*body->GetMotionPropertiesUnchecked()); + + motion_properties.SetLinearDamping(linear_damping); +} + +void JoltSoftBody3D::_update_simulation_precision() { + if (!in_space()) { + jolt_settings->mNumIterations = (JPH::uint32)simulation_precision; + return; + } + + JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + JPH::SoftBodyMotionProperties &motion_properties = static_cast(*body->GetMotionPropertiesUnchecked()); + + motion_properties.SetNumIterations((JPH::uint32)simulation_precision); +} + +void JoltSoftBody3D::_update_group_filter() { + JPH::GroupFilter *group_filter = !exceptions.is_empty() ? JoltGroupFilter::instance : nullptr; + + if (!in_space()) { + jolt_settings->mCollisionGroup.SetGroupFilter(group_filter); + return; + } + + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + body->GetCollisionGroup().SetGroupFilter(group_filter); +} + +void JoltSoftBody3D::_try_rebuild() { + if (space != nullptr) { + _reset_space(); + } +} + +void JoltSoftBody3D::_mesh_changed() { + _try_rebuild(); +} + +void JoltSoftBody3D::_simulation_precision_changed() { + wake_up(); +} + +void JoltSoftBody3D::_mass_changed() { + wake_up(); +} + +void JoltSoftBody3D::_pressure_changed() { + _update_pressure(); + wake_up(); +} + +void JoltSoftBody3D::_damping_changed() { + _update_damping(); + wake_up(); +} + +void JoltSoftBody3D::_pins_changed() { + _update_mass(); + wake_up(); +} + +void JoltSoftBody3D::_vertices_changed() { + wake_up(); +} + +void JoltSoftBody3D::_exceptions_changed() { + _update_group_filter(); +} + +JoltSoftBody3D::JoltSoftBody3D() : + JoltObject3D(OBJECT_TYPE_SOFT_BODY) { + jolt_settings->mRestitution = 0.0f; + jolt_settings->mFriction = 1.0f; + jolt_settings->mUpdatePosition = false; + jolt_settings->mMakeRotationIdentity = false; +} + +JoltSoftBody3D::~JoltSoftBody3D() { + if (jolt_settings != nullptr) { + delete jolt_settings; + jolt_settings = nullptr; + } +} + +bool JoltSoftBody3D::in_space() const { + return JoltObject3D::in_space() && shared != nullptr; +} + +void JoltSoftBody3D::add_collision_exception(const RID &p_excepted_body) { + exceptions.push_back(p_excepted_body); + + _exceptions_changed(); +} + +void JoltSoftBody3D::remove_collision_exception(const RID &p_excepted_body) { + exceptions.erase(p_excepted_body); + + _exceptions_changed(); +} + +bool JoltSoftBody3D::has_collision_exception(const RID &p_excepted_body) const { + return exceptions.find(p_excepted_body) >= 0; +} + +bool JoltSoftBody3D::can_interact_with(const JoltBody3D &p_other) const { + return (can_collide_with(p_other) || p_other.can_collide_with(*this)) && !has_collision_exception(p_other.get_rid()) && !p_other.has_collision_exception(rid); +} + +bool JoltSoftBody3D::can_interact_with(const JoltSoftBody3D &p_other) const { + return (can_collide_with(p_other) || p_other.can_collide_with(*this)) && !has_collision_exception(p_other.get_rid()) && !p_other.has_collision_exception(rid); +} + +bool JoltSoftBody3D::can_interact_with(const JoltArea3D &p_other) const { + return p_other.can_interact_with(*this); +} + +Vector3 JoltSoftBody3D::get_velocity_at_position(const Vector3 &p_position) const { + return { 0.0f, 0.0f, 0.0f }; +} + +void JoltSoftBody3D::set_mesh(const RID &p_mesh) { + if (unlikely(mesh == p_mesh)) { + return; + } + + _deref_shared_data(); + + mesh = p_mesh; + + _mesh_changed(); +} + +bool JoltSoftBody3D::is_sleeping() const { + if (!in_space()) { + return false; + } + + const JoltReadableBody3D body = space->read_body(jolt_id); + ERR_FAIL_COND_V(body.is_invalid(), false); + + return !body->IsActive(); +} + +void JoltSoftBody3D::set_is_sleeping(bool p_enabled) { + if (!in_space()) { + return; + } + + JPH::BodyInterface &body_iface = space->get_body_iface(); + + if (p_enabled) { + body_iface.DeactivateBody(jolt_id); + } else { + body_iface.ActivateBody(jolt_id); + } +} + +bool JoltSoftBody3D::can_sleep() const { + if (!in_space()) { + return true; + } + + const JoltReadableBody3D body = space->read_body(jolt_id); + ERR_FAIL_COND_V(body.is_invalid(), false); + + return body->GetAllowSleeping(); +} + +void JoltSoftBody3D::set_can_sleep(bool p_enabled) { + if (!in_space()) { + return; + } + + const JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + body->SetAllowSleeping(p_enabled); +} + +void JoltSoftBody3D::set_simulation_precision(int p_precision) { + if (unlikely(simulation_precision == p_precision)) { + return; + } + + simulation_precision = MAX(p_precision, 0); + + _simulation_precision_changed(); +} + +void JoltSoftBody3D::set_mass(float p_mass) { + if (unlikely(mass == p_mass)) { + return; + } + + mass = MAX(p_mass, 0.0f); + + _mass_changed(); +} + +float JoltSoftBody3D::get_stiffness_coefficient() const { + return stiffness_coefficient; +} + +void JoltSoftBody3D::set_stiffness_coefficient(float p_coefficient) { + stiffness_coefficient = CLAMP(p_coefficient, 0.0f, 1.0f); +} + +void JoltSoftBody3D::set_pressure(float p_pressure) { + if (unlikely(pressure == p_pressure)) { + return; + } + + pressure = MAX(p_pressure, 0.0f); + + _pressure_changed(); +} + +void JoltSoftBody3D::set_linear_damping(float p_damping) { + if (unlikely(linear_damping == p_damping)) { + return; + } + + linear_damping = MAX(p_damping, 0.0f); + + _damping_changed(); +} + +float JoltSoftBody3D::get_drag() const { + // Drag is not a thing in Jolt, and not supported by Godot Physics either. + return 0.0f; +} + +void JoltSoftBody3D::set_drag(float p_drag) { + // Drag is not a thing in Jolt, and not supported by Godot Physics either. +} + +Variant JoltSoftBody3D::get_state(PhysicsServer3D::BodyState p_state) const { + switch (p_state) { + case PhysicsServer3D::BODY_STATE_TRANSFORM: { + return get_transform(); + } + case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { + ERR_FAIL_V_MSG(Variant(), "Linear velocity is not supported for soft bodies."); + } + case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { + ERR_FAIL_V_MSG(Variant(), "Angular velocity is not supported for soft bodies."); + } + case PhysicsServer3D::BODY_STATE_SLEEPING: { + return is_sleeping(); + } + case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { + return can_sleep(); + } + default: { + ERR_FAIL_V_MSG(Variant(), vformat("Unhandled body state: '%d'. This should not happen. Please report this.", p_state)); + } + } +} + +void JoltSoftBody3D::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_value) { + switch (p_state) { + case PhysicsServer3D::BODY_STATE_TRANSFORM: { + set_transform(p_value); + } break; + case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { + ERR_FAIL_MSG("Linear velocity is not supported for soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { + ERR_FAIL_MSG("Angular velocity is not supported for soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_SLEEPING: { + set_is_sleeping(p_value); + } break; + case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { + set_can_sleep(p_value); + } break; + default: { + ERR_FAIL_MSG(vformat("Unhandled body state: '%d'. This should not happen. Please report this.", p_state)); + } break; + } +} + +Transform3D JoltSoftBody3D::get_transform() const { + // Since any transform gets baked into the vertices anyway we can just return identity here. + return Transform3D(); +} + +void JoltSoftBody3D::set_transform(const Transform3D &p_transform) { + ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to set transform for '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string())); + + JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + // For whatever reason this has to be interpreted as a relative global-space transform rather than an absolute one, + // because `SoftBody3D` will immediately upon entering the scene tree set itself to be top-level and also set its + // transform to be identity, while still expecting to stay in its original position. + // + // We also discard any scaling, since we have no way of scaling the actual edge lengths. + const JPH::Mat44 relative_transform = to_jolt(p_transform.orthonormalized()); + + JPH::SoftBodyMotionProperties &motion_properties = static_cast(*body->GetMotionPropertiesUnchecked()); + JPH::Array &physics_vertices = motion_properties.GetVertices(); + + for (JPH::SoftBodyVertex &vertex : physics_vertices) { + vertex.mPosition = vertex.mPreviousPosition = relative_transform * vertex.mPosition; + vertex.mVelocity = JPH::Vec3::sZero(); + } +} + +AABB JoltSoftBody3D::get_bounds() const { + ERR_FAIL_COND_V_MSG(!in_space(), AABB(), vformat("Failed to retrieve world bounds of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string())); + + const JoltReadableBody3D body = space->read_body(jolt_id); + ERR_FAIL_COND_V(body.is_invalid(), AABB()); + + return to_godot(body->GetWorldSpaceBounds()); +} + +void JoltSoftBody3D::update_rendering_server(PhysicsServer3DRenderingServerHandler *p_rendering_server_handler) { + // Ideally we would emit an actual error here, but that would spam the logs to the point where the actual cause will be drowned out. + if (unlikely(!in_space())) { + return; + } + + const JoltReadableBody3D body = space->read_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + const JPH::SoftBodyMotionProperties &motion_properties = static_cast(*body->GetMotionPropertiesUnchecked()); + + typedef JPH::SoftBodyMotionProperties::Vertex SoftBodyVertex; + typedef JPH::SoftBodyMotionProperties::Face SoftBodyFace; + + const JPH::Array &physics_vertices = motion_properties.GetVertices(); + const JPH::Array &physics_faces = motion_properties.GetFaces(); + + const int physics_vertex_count = (int)physics_vertices.size(); + + normals.resize(physics_vertex_count); + + for (const SoftBodyFace &physics_face : physics_faces) { + // Jolt uses a different winding order, so we swap the indices to account for that. + + const uint32_t i0 = physics_face.mVertex[2]; + const uint32_t i1 = physics_face.mVertex[1]; + const uint32_t i2 = physics_face.mVertex[0]; + + const Vector3 v0 = to_godot(physics_vertices[i0].mPosition); + const Vector3 v1 = to_godot(physics_vertices[i1].mPosition); + const Vector3 v2 = to_godot(physics_vertices[i2].mPosition); + + const Vector3 normal = (v2 - v0).cross(v1 - v0).normalized(); + + normals[i0] = normal; + normals[i1] = normal; + normals[i2] = normal; + } + + const int mesh_vertex_count = shared->mesh_to_physics.size(); + + for (int i = 0; i < mesh_vertex_count; ++i) { + const int physics_index = shared->mesh_to_physics[i]; + + const Vector3 vertex = to_godot(physics_vertices[(size_t)physics_index].mPosition); + const Vector3 normal = normals[(uint32_t)physics_index]; + + p_rendering_server_handler->set_vertex(i, vertex); + p_rendering_server_handler->set_normal(i, normal); + } + + p_rendering_server_handler->set_aabb(get_bounds()); +} + +Vector3 JoltSoftBody3D::get_vertex_position(int p_index) { + ERR_FAIL_COND_V_MSG(!in_space(), Vector3(), vformat("Failed to retrieve point position for '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string())); + + ERR_FAIL_NULL_V(shared, Vector3()); + ERR_FAIL_INDEX_V(p_index, (int)shared->mesh_to_physics.size(), Vector3()); + const size_t physics_index = (size_t)shared->mesh_to_physics[p_index]; + + const JoltReadableBody3D body = space->read_body(jolt_id); + ERR_FAIL_COND_V(body.is_invalid(), Vector3()); + + const JPH::SoftBodyMotionProperties &motion_properties = static_cast(*body->GetMotionPropertiesUnchecked()); + const JPH::Array &physics_vertices = motion_properties.GetVertices(); + const JPH::SoftBodyVertex &physics_vertex = physics_vertices[physics_index]; + + return to_godot(body->GetCenterOfMassPosition() + physics_vertex.mPosition); +} + +void JoltSoftBody3D::set_vertex_position(int p_index, const Vector3 &p_position) { + ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to set point position for '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string())); + + ERR_FAIL_NULL(shared); + ERR_FAIL_INDEX(p_index, (int)shared->mesh_to_physics.size()); + const size_t physics_index = (size_t)shared->mesh_to_physics[p_index]; + + const float last_step = space->get_last_step(); + if (unlikely(last_step == 0.0f)) { + return; + } + + JoltWritableBody3D body = space->write_body(jolt_id); + ERR_FAIL_COND(body.is_invalid()); + + JPH::SoftBodyMotionProperties &motion_properties = static_cast(*body->GetMotionPropertiesUnchecked()); + + JPH::Array &physics_vertices = motion_properties.GetVertices(); + JPH::SoftBodyVertex &physics_vertex = physics_vertices[physics_index]; + + const JPH::RVec3 center_of_mass = body->GetCenterOfMassPosition(); + const JPH::Vec3 local_position = JPH::Vec3(to_jolt_r(p_position) - center_of_mass); + const JPH::Vec3 displacement = local_position - physics_vertex.mPosition; + const JPH::Vec3 velocity = displacement / last_step; + + physics_vertex.mVelocity = velocity; + + _vertices_changed(); +} + +void JoltSoftBody3D::pin_vertex(int p_index) { + pinned_vertices.insert(p_index); + + _pins_changed(); +} + +void JoltSoftBody3D::unpin_vertex(int p_index) { + pinned_vertices.erase(p_index); + + _pins_changed(); +} + +void JoltSoftBody3D::unpin_all_vertices() { + pinned_vertices.clear(); + + _pins_changed(); +} + +bool JoltSoftBody3D::is_vertex_pinned(int p_index) const { + ERR_FAIL_COND_V_MSG(!in_space(), false, vformat("Failed retrieve pin status of point for '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string())); + + ERR_FAIL_NULL_V(shared, false); + ERR_FAIL_INDEX_V(p_index, (int)shared->mesh_to_physics.size(), false); + const int physics_index = shared->mesh_to_physics[p_index]; + + return pinned_vertices.has(physics_index); +} + +String JoltSoftBody3D::to_string() const { + Object *instance = get_instance(); + return instance != nullptr ? instance->to_string() : ""; +} diff --git a/modules/jolt_physics/objects/jolt_soft_body_3d.h b/modules/jolt_physics/objects/jolt_soft_body_3d.h new file mode 100644 index 000000000000..5a8a7ddf4c38 --- /dev/null +++ b/modules/jolt_physics/objects/jolt_soft_body_3d.h @@ -0,0 +1,175 @@ +/**************************************************************************/ +/* jolt_soft_body_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_SOFT_BODY_3D_H +#define JOLT_SOFT_BODY_3D_H + +#include "jolt_object_3d.h" + +#include "core/variant/typed_array.h" +#include "servers/physics_server_3d.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/SoftBody/SoftBodyCreationSettings.h" +#include "Jolt/Physics/SoftBody/SoftBodySharedSettings.h" + +class JoltSpace3D; + +class JoltSoftBody3D final : public JoltObject3D { + struct Shared { + LocalVector mesh_to_physics; + JPH::Ref settings = new JPH::SoftBodySharedSettings(); + int ref_count = 1; + }; + + inline static HashMap mesh_to_shared; + + HashSet pinned_vertices; + LocalVector exceptions; + LocalVector normals; + + const Shared *shared = nullptr; + + RID mesh; + + JPH::SoftBodyCreationSettings *jolt_settings = new JPH::SoftBodyCreationSettings(); + + float mass = 0.0f; + float pressure = 0.0f; + float linear_damping = 0.01f; + float stiffness_coefficient = 0.5f; + + int simulation_precision = 5; + + virtual JPH::BroadPhaseLayer _get_broad_phase_layer() const override; + virtual JPH::ObjectLayer _get_object_layer() const override; + + virtual void _space_changing() override; + virtual void _space_changed() override; + + virtual void _add_to_space() override; + + bool _ref_shared_data(); + void _deref_shared_data(); + + void _update_mass(); + void _update_pressure(); + void _update_damping(); + void _update_simulation_precision(); + void _update_group_filter(); + + void _try_rebuild(); + + void _mesh_changed(); + void _simulation_precision_changed(); + void _mass_changed(); + void _pressure_changed(); + void _damping_changed(); + void _pins_changed(); + void _vertices_changed(); + void _exceptions_changed(); + +public: + JoltSoftBody3D(); + virtual ~JoltSoftBody3D() override; + + bool in_space() const; + + void add_collision_exception(const RID &p_excepted_body); + void remove_collision_exception(const RID &p_excepted_body); + bool has_collision_exception(const RID &p_excepted_body) const; + + const LocalVector &get_collision_exceptions() const { return exceptions; } + + virtual bool can_interact_with(const JoltBody3D &p_other) const override; + virtual bool can_interact_with(const JoltSoftBody3D &p_other) const override; + virtual bool can_interact_with(const JoltArea3D &p_other) const override; + + virtual bool reports_contacts() const override { return false; } + + virtual Vector3 get_velocity_at_position(const Vector3 &p_position) const override; + + void set_mesh(const RID &p_mesh); + + bool is_pickable() const { return pickable; } + void set_pickable(bool p_enabled) { pickable = p_enabled; } + + bool is_sleeping() const; + void set_is_sleeping(bool p_enabled); + + bool can_sleep() const; + void set_can_sleep(bool p_enabled); + + void put_to_sleep() { set_is_sleeping(true); } + void wake_up() { set_is_sleeping(false); } + + int get_simulation_precision() const { return simulation_precision; } + void set_simulation_precision(int p_precision); + + float get_mass() const { return mass; } + void set_mass(float p_mass); + + float get_stiffness_coefficient() const; + void set_stiffness_coefficient(float p_coefficient); + + float get_pressure() const { return pressure; } + void set_pressure(float p_pressure); + + float get_linear_damping() const { return linear_damping; } + void set_linear_damping(float p_damping); + + float get_drag() const; + void set_drag(float p_drag); + + Variant get_state(PhysicsServer3D::BodyState p_state) const; + void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_value); + + Transform3D get_transform() const; + void set_transform(const Transform3D &p_transform); + + AABB get_bounds() const; + + void update_rendering_server(PhysicsServer3DRenderingServerHandler *p_rendering_server_handler); + + Vector3 get_vertex_position(int p_index); + void set_vertex_position(int p_index, const Vector3 &p_position); + + void pin_vertex(int p_index); + void unpin_vertex(int p_index); + + void unpin_all_vertices(); + + bool is_vertex_pinned(int p_index) const; + + String to_string() const; +}; + +#endif // JOLT_SOFT_BODY_3D_H diff --git a/modules/jolt_physics/register_types.cpp b/modules/jolt_physics/register_types.cpp new file mode 100644 index 000000000000..3f4588841c61 --- /dev/null +++ b/modules/jolt_physics/register_types.cpp @@ -0,0 +1,81 @@ +/**************************************************************************/ +/* register_types.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "register_types.h" + +#include "jolt_globals.h" +#include "jolt_physics_server_3d.h" +#include "jolt_project_settings.h" +#include "objects/jolt_physics_direct_body_state_3d.h" +#include "spaces/jolt_physics_direct_space_state_3d.h" + +#include "servers/physics_server_3d_wrap_mt.h" + +PhysicsServer3D *create_jolt_physics_server() { +#ifdef THREADS_ENABLED + bool run_on_separate_thread = GLOBAL_GET("physics/3d/run_on_separate_thread"); +#else + bool run_on_separate_thread = false; +#endif + + JoltPhysicsServer3D *physics_server = memnew(JoltPhysicsServer3D(run_on_separate_thread)); + + return memnew(PhysicsServer3DWrapMT(physics_server, run_on_separate_thread)); +} + +void initialize_jolt_physics_module(ModuleInitializationLevel p_level) { + switch (p_level) { + case MODULE_INITIALIZATION_LEVEL_CORE: { + } break; + case MODULE_INITIALIZATION_LEVEL_SERVERS: { + jolt_initialize(); + PhysicsServer3DManager::get_singleton()->register_server("JoltPhysics3D", callable_mp_static(&create_jolt_physics_server)); + JoltProjectSettings::register_settings(); + } break; + case MODULE_INITIALIZATION_LEVEL_SCENE: { + } break; + case MODULE_INITIALIZATION_LEVEL_EDITOR: { + } break; + } +} + +void uninitialize_jolt_physics_module(ModuleInitializationLevel p_level) { + switch (p_level) { + case MODULE_INITIALIZATION_LEVEL_CORE: { + } break; + case MODULE_INITIALIZATION_LEVEL_SERVERS: { + jolt_deinitialize(); + } break; + case MODULE_INITIALIZATION_LEVEL_SCENE: { + } break; + case MODULE_INITIALIZATION_LEVEL_EDITOR: { + } break; + } +} diff --git a/modules/jolt_physics/register_types.h b/modules/jolt_physics/register_types.h new file mode 100644 index 000000000000..1be106d02ac1 --- /dev/null +++ b/modules/jolt_physics/register_types.h @@ -0,0 +1,39 @@ +/**************************************************************************/ +/* register_types.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_PHYSICS_REGISTER_TYPES_H +#define JOLT_PHYSICS_REGISTER_TYPES_H + +#include "modules/register_module_types.h" + +void initialize_jolt_physics_module(ModuleInitializationLevel p_level); +void uninitialize_jolt_physics_module(ModuleInitializationLevel p_level); + +#endif // JOLT_PHYSICS_REGISTER_TYPES_H diff --git a/modules/jolt_physics/shapes/jolt_box_shape_3d.cpp b/modules/jolt_physics/shapes/jolt_box_shape_3d.cpp new file mode 100644 index 000000000000..122b7d2ff8e8 --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_box_shape_3d.cpp @@ -0,0 +1,85 @@ +/**************************************************************************/ +/* jolt_box_shape_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_box_shape_3d.h" + +#include "../jolt_project_settings.h" +#include "../misc/jolt_type_conversions.h" + +#include "core/error/error_macros.h" + +#include "Jolt/Physics/Collision/Shape/BoxShape.h" + +JPH::ShapeRefC JoltBoxShape3D::_build() const { + const float min_half_extent = (float)half_extents[half_extents.min_axis_index()]; + const float actual_margin = MIN(margin, min_half_extent * JoltProjectSettings::get_collision_margin_fraction()); + + const JPH::BoxShapeSettings shape_settings(to_jolt(half_extents), actual_margin); + const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create(); + + ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to build Jolt Physics box shape with %s. It returned the following error: '%s'. This shape belongs to %s.", to_string(), to_godot(shape_result.GetError()), _owners_to_string())); + + return shape_result.Get(); +} + +Variant JoltBoxShape3D::get_data() const { + return half_extents; +} + +void JoltBoxShape3D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR3); + + const Vector3 new_half_extents = p_data; + if (unlikely(new_half_extents == half_extents)) { + return; + } + + half_extents = new_half_extents; + + destroy(); +} + +void JoltBoxShape3D::set_margin(float p_margin) { + if (unlikely(margin == p_margin)) { + return; + } + + margin = p_margin; + + destroy(); +} + +String JoltBoxShape3D::to_string() const { + return vformat("{half_extents=%v margin=%f}", half_extents, margin); +} + +AABB JoltBoxShape3D::get_aabb() const { + return AABB(-half_extents, half_extents * 2.0f); +} diff --git a/modules/jolt_physics/shapes/jolt_box_shape_3d.h b/modules/jolt_physics/shapes/jolt_box_shape_3d.h new file mode 100644 index 000000000000..896caffefd9a --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_box_shape_3d.h @@ -0,0 +1,57 @@ +/**************************************************************************/ +/* jolt_box_shape_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_BOX_SHAPE_3D_H +#define JOLT_BOX_SHAPE_3D_H + +#include "jolt_shape_3d.h" + +class JoltBoxShape3D final : public JoltShape3D { + Vector3 half_extents; + float margin = 0.04f; + + virtual JPH::ShapeRefC _build() const override; + +public: + virtual ShapeType get_type() const override { return ShapeType::SHAPE_BOX; } + virtual bool is_convex() const override { return true; } + + virtual Variant get_data() const override; + virtual void set_data(const Variant &p_data) override; + + virtual float get_margin() const override { return margin; } + virtual void set_margin(float p_margin) override; + + virtual AABB get_aabb() const override; + + String to_string() const; +}; + +#endif // JOLT_BOX_SHAPE_3D_H diff --git a/modules/jolt_physics/shapes/jolt_capsule_shape_3d.cpp b/modules/jolt_physics/shapes/jolt_capsule_shape_3d.cpp new file mode 100644 index 000000000000..6659b60f590d --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_capsule_shape_3d.cpp @@ -0,0 +1,92 @@ +/**************************************************************************/ +/* jolt_capsule_shape_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_capsule_shape_3d.h" + +#include "../misc/jolt_type_conversions.h" + +#include "core/error/error_macros.h" + +#include "Jolt/Physics/Collision/Shape/CapsuleShape.h" + +JPH::ShapeRefC JoltCapsuleShape3D::_build() const { + ERR_FAIL_COND_V_MSG(radius <= 0.0f, nullptr, vformat("Failed to build Jolt Physics capsule shape with %s. Its radius must be greater than 0. This shape belongs to %s.", to_string(), _owners_to_string())); + ERR_FAIL_COND_V_MSG(height <= 0.0f, nullptr, vformat("Failed to build Jolt Physics capsule shape with %s. Its height must be greater than 0. This shape belongs to %s.", to_string(), _owners_to_string())); + ERR_FAIL_COND_V_MSG(height < radius * 2.0f, nullptr, vformat("Failed to build Jolt Physics capsule shape with %s. Its height must be at least double that of its radius. This shape belongs to %s.", to_string(), _owners_to_string())); + + const float half_height = height / 2.0f; + const float cylinder_height = half_height - radius; + + const JPH::CapsuleShapeSettings shape_settings(cylinder_height, radius); + const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create(); + ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to build Jolt Physics capsule shape with %s. It returned the following error: '%s'. This shape belongs to %s.", to_string(), to_godot(shape_result.GetError()), _owners_to_string())); + + return shape_result.Get(); +} + +Variant JoltCapsuleShape3D::get_data() const { + Dictionary data; + data["height"] = height; + data["radius"] = radius; + return data; +} + +void JoltCapsuleShape3D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY); + + const Dictionary data = p_data; + + const Variant maybe_height = data.get("height", Variant()); + ERR_FAIL_COND(maybe_height.get_type() != Variant::FLOAT); + + const Variant maybe_radius = data.get("radius", Variant()); + ERR_FAIL_COND(maybe_radius.get_type() != Variant::FLOAT); + + const float new_height = maybe_height; + const float new_radius = maybe_radius; + + if (unlikely(new_height == height && new_radius == radius)) { + return; + } + + height = new_height; + radius = new_radius; + + destroy(); +} + +AABB JoltCapsuleShape3D::get_aabb() const { + const Vector3 half_extents(radius, height / 2.0f, radius); + return AABB(-half_extents, half_extents * 2.0f); +} + +String JoltCapsuleShape3D::to_string() const { + return vformat("{height=%f radius=%f}", height, radius); +} diff --git a/modules/jolt_physics/shapes/jolt_capsule_shape_3d.h b/modules/jolt_physics/shapes/jolt_capsule_shape_3d.h new file mode 100644 index 000000000000..8e3f932e91ac --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_capsule_shape_3d.h @@ -0,0 +1,57 @@ +/**************************************************************************/ +/* jolt_capsule_shape_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_CAPSULE_SHAPE_3D_H +#define JOLT_CAPSULE_SHAPE_3D_H + +#include "jolt_shape_3d.h" + +class JoltCapsuleShape3D final : public JoltShape3D { + float height = 0.0f; + float radius = 0.0f; + + virtual JPH::ShapeRefC _build() const override; + +public: + virtual ShapeType get_type() const override { return ShapeType::SHAPE_CAPSULE; } + virtual bool is_convex() const override { return true; } + + virtual Variant get_data() const override; + virtual void set_data(const Variant &p_data) override; + + virtual float get_margin() const override { return 0.0f; } + virtual void set_margin(float p_margin) override {} + + virtual AABB get_aabb() const override; + + String to_string() const; +}; + +#endif // JOLT_CAPSULE_SHAPE_3D_H diff --git a/modules/jolt_physics/shapes/jolt_concave_polygon_shape_3d.cpp b/modules/jolt_physics/shapes/jolt_concave_polygon_shape_3d.cpp new file mode 100644 index 000000000000..c11978d39e5c --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_concave_polygon_shape_3d.cpp @@ -0,0 +1,127 @@ +/**************************************************************************/ +/* jolt_concave_polygon_shape_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_concave_polygon_shape_3d.h" + +#include "../jolt_project_settings.h" +#include "../misc/jolt_type_conversions.h" + +#include "core/error/error_macros.h" + +#include "Jolt/Physics/Collision/Shape/MeshShape.h" + +JPH::ShapeRefC JoltConcavePolygonShape3D::_build() const { + const int vertex_count = (int)faces.size(); + const int face_count = vertex_count / 3; + const int excess_vertex_count = vertex_count % 3; + + if (unlikely(vertex_count == 0)) { + return nullptr; + } + + ERR_FAIL_COND_V_MSG(vertex_count < 3, nullptr, vformat("Failed to build Jolt Physics concave polygon shape with %s. It must have a vertex count of at least 3. This shape belongs to %s.", to_string(), _owners_to_string())); + ERR_FAIL_COND_V_MSG(excess_vertex_count != 0, nullptr, vformat("Failed to build Jolt Physics concave polygon shape with %s. It must have a vertex count that is divisible by 3. This shape belongs to %s.", to_string(), _owners_to_string())); + + JPH::TriangleList jolt_faces; + jolt_faces.reserve((size_t)face_count); + + const Vector3 *faces_begin = &faces[0]; + const Vector3 *faces_end = faces_begin + vertex_count; + JPH::uint32 triangle_index = 0; + + for (const Vector3 *vertex = faces_begin; vertex != faces_end; vertex += 3) { + const Vector3 *v0 = vertex + 0; + const Vector3 *v1 = vertex + 1; + const Vector3 *v2 = vertex + 2; + + // Jolt uses a different winding order, so we swizzle the vertices to account for that. + jolt_faces.emplace_back( + JPH::Float3((float)v2->x, (float)v2->y, (float)v2->z), + JPH::Float3((float)v1->x, (float)v1->y, (float)v1->z), + JPH::Float3((float)v0->x, (float)v0->y, (float)v0->z), + 0, + triangle_index++); + } + + JPH::MeshShapeSettings shape_settings(jolt_faces); + shape_settings.mActiveEdgeCosThresholdAngle = JoltProjectSettings::get_active_edge_threshold(); + shape_settings.mPerTriangleUserData = JoltProjectSettings::enable_ray_cast_face_index(); + + const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create(); + ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to build Jolt Physics concave polygon shape with %s. It returned the following error: '%s'. This shape belongs to %s.", to_string(), to_godot(shape_result.GetError()), _owners_to_string())); + + return JoltShape3D::with_double_sided(shape_result.Get(), back_face_collision); +} + +AABB JoltConcavePolygonShape3D::_calculate_aabb() const { + AABB result; + + for (int i = 0; i < faces.size(); ++i) { + const Vector3 &vertex = faces[i]; + + if (i == 0) { + result.position = vertex; + } else { + result.expand_to(vertex); + } + } + + return result; +} + +Variant JoltConcavePolygonShape3D::get_data() const { + Dictionary data; + data["faces"] = faces; + data["backface_collision"] = back_face_collision; + return data; +} + +void JoltConcavePolygonShape3D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY); + + const Dictionary data = p_data; + + const Variant maybe_faces = data.get("faces", Variant()); + ERR_FAIL_COND(maybe_faces.get_type() != Variant::PACKED_VECTOR3_ARRAY); + + const Variant maybe_back_face_collision = data.get("backface_collision", Variant()); + ERR_FAIL_COND(maybe_back_face_collision.get_type() != Variant::BOOL); + + faces = maybe_faces; + back_face_collision = maybe_back_face_collision; + + aabb = _calculate_aabb(); + + destroy(); +} + +String JoltConcavePolygonShape3D::to_string() const { + return vformat("{vertex_count=%d}", faces.size()); +} diff --git a/modules/jolt_physics/shapes/jolt_concave_polygon_shape_3d.h b/modules/jolt_physics/shapes/jolt_concave_polygon_shape_3d.h new file mode 100644 index 000000000000..d865c86aa72f --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_concave_polygon_shape_3d.h @@ -0,0 +1,60 @@ +/**************************************************************************/ +/* jolt_concave_polygon_shape_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_CONCAVE_POLYGON_SHAPE_3D_H +#define JOLT_CONCAVE_POLYGON_SHAPE_3D_H + +#include "jolt_shape_3d.h" + +class JoltConcavePolygonShape3D final : public JoltShape3D { + AABB aabb; + PackedVector3Array faces; + bool back_face_collision = false; + + virtual JPH::ShapeRefC _build() const override; + + AABB _calculate_aabb() const; + +public: + virtual ShapeType get_type() const override { return ShapeType::SHAPE_CONCAVE_POLYGON; } + virtual bool is_convex() const override { return false; } + + virtual Variant get_data() const override; + virtual void set_data(const Variant &p_data) override; + + virtual float get_margin() const override { return 0.0f; } + virtual void set_margin(float p_margin) override {} + + virtual AABB get_aabb() const override { return aabb; } + + String to_string() const; +}; + +#endif // JOLT_CONCAVE_POLYGON_SHAPE_3D_H diff --git a/modules/jolt_physics/shapes/jolt_convex_polygon_shape_3d.cpp b/modules/jolt_physics/shapes/jolt_convex_polygon_shape_3d.cpp new file mode 100644 index 000000000000..65d9ffe66df3 --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_convex_polygon_shape_3d.cpp @@ -0,0 +1,109 @@ +/**************************************************************************/ +/* jolt_convex_polygon_shape_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_convex_polygon_shape_3d.h" + +#include "../jolt_project_settings.h" +#include "../misc/jolt_type_conversions.h" + +#include "core/error/error_macros.h" + +#include "Jolt/Physics/Collision/Shape/ConvexHullShape.h" + +JPH::ShapeRefC JoltConvexPolygonShape3D::_build() const { + const int vertex_count = (int)vertices.size(); + + if (unlikely(vertex_count == 0)) { + return nullptr; + } + + ERR_FAIL_COND_V_MSG(vertex_count < 3, nullptr, vformat("Failed to build Jolt Physics convex polygon shape with %s. It must have a vertex count of at least 3. This shape belongs to %s.", to_string(), _owners_to_string())); + + JPH::Array jolt_vertices; + jolt_vertices.reserve((size_t)vertex_count); + + const Vector3 *vertices_begin = &vertices[0]; + const Vector3 *vertices_end = vertices_begin + vertex_count; + + for (const Vector3 *vertex = vertices_begin; vertex != vertices_end; ++vertex) { + jolt_vertices.emplace_back((float)vertex->x, (float)vertex->y, (float)vertex->z); + } + + const float min_half_extent = _calculate_aabb().get_shortest_axis_size() * 0.5f; + const float actual_margin = MIN(margin, min_half_extent * JoltProjectSettings::get_collision_margin_fraction()); + + const JPH::ConvexHullShapeSettings shape_settings(jolt_vertices, actual_margin); + const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create(); + ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to build Jolt Physics convex polygon shape with %s. It returned the following error: '%s'. This shape belongs to %s.", to_string(), to_godot(shape_result.GetError()), _owners_to_string())); + + return shape_result.Get(); +} + +AABB JoltConvexPolygonShape3D::_calculate_aabb() const { + AABB result; + + for (int i = 0; i < vertices.size(); ++i) { + if (i == 0) { + result.position = vertices[i]; + } else { + result.expand_to(vertices[i]); + } + } + + return result; +} + +Variant JoltConvexPolygonShape3D::get_data() const { + return vertices; +} + +void JoltConvexPolygonShape3D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR3_ARRAY); + + vertices = p_data; + + aabb = _calculate_aabb(); + + destroy(); +} + +void JoltConvexPolygonShape3D::set_margin(float p_margin) { + if (unlikely(margin == p_margin)) { + return; + } + + margin = p_margin; + + destroy(); +} + +String JoltConvexPolygonShape3D::to_string() const { + return vformat("{vertex_count=%d margin=%f}", vertices.size(), margin); +} diff --git a/modules/jolt_physics/shapes/jolt_convex_polygon_shape_3d.h b/modules/jolt_physics/shapes/jolt_convex_polygon_shape_3d.h new file mode 100644 index 000000000000..f623ce7599d3 --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_convex_polygon_shape_3d.h @@ -0,0 +1,60 @@ +/**************************************************************************/ +/* jolt_convex_polygon_shape_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_CONVEX_POLYGON_SHAPE_3D_H +#define JOLT_CONVEX_POLYGON_SHAPE_3D_H + +#include "jolt_shape_3d.h" + +class JoltConvexPolygonShape3D final : public JoltShape3D { + AABB aabb; + PackedVector3Array vertices; + float margin = 0.04f; + + virtual JPH::ShapeRefC _build() const override; + + AABB _calculate_aabb() const; + +public: + virtual ShapeType get_type() const override { return ShapeType::SHAPE_CONVEX_POLYGON; } + virtual bool is_convex() const override { return true; } + + virtual Variant get_data() const override; + virtual void set_data(const Variant &p_data) override; + + virtual float get_margin() const override { return margin; } + virtual void set_margin(float p_margin) override; + + virtual AABB get_aabb() const override { return aabb; } + + String to_string() const; +}; + +#endif // JOLT_CONVEX_POLYGON_SHAPE_3D_H diff --git a/modules/jolt_physics/shapes/jolt_custom_decorated_shape.h b/modules/jolt_physics/shapes/jolt_custom_decorated_shape.h new file mode 100644 index 000000000000..ac956f7b61f1 --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_custom_decorated_shape.h @@ -0,0 +1,95 @@ +/**************************************************************************/ +/* jolt_custom_decorated_shape.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_CUSTOM_DECORATED_SHAPE_H +#define JOLT_CUSTOM_DECORATED_SHAPE_H + +#include "jolt_custom_shape_type.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/Collision/Shape/DecoratedShape.h" +#include "Jolt/Physics/Collision/TransformedShape.h" + +class JoltCustomDecoratedShapeSettings : public JPH::DecoratedShapeSettings { +public: + using JPH::DecoratedShapeSettings::DecoratedShapeSettings; +}; + +class JoltCustomDecoratedShape : public JPH::DecoratedShape { +public: + using JPH::DecoratedShape::DecoratedShape; + + virtual JPH::AABox GetLocalBounds() const override { return mInnerShape->GetLocalBounds(); } + + virtual JPH::AABox GetWorldSpaceBounds(JPH::Mat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale) const override { return mInnerShape->GetWorldSpaceBounds(p_center_of_mass_transform, p_scale); } + + virtual float GetInnerRadius() const override { return mInnerShape->GetInnerRadius(); } + + virtual JPH::MassProperties GetMassProperties() const override { return mInnerShape->GetMassProperties(); } + + virtual JPH::Vec3 GetSurfaceNormal(const JPH::SubShapeID &p_sub_shape_id, JPH::Vec3Arg p_local_surface_position) const override { return mInnerShape->GetSurfaceNormal(p_sub_shape_id, p_local_surface_position); } + + virtual JPH::uint64 GetSubShapeUserData(const JPH::SubShapeID &p_sub_shape_id) const override { return mInnerShape->GetSubShapeUserData(p_sub_shape_id); } + + virtual JPH::TransformedShape GetSubShapeTransformedShape(const JPH::SubShapeID &p_sub_shape_id, JPH::Vec3Arg p_position_com, JPH::QuatArg p_rotation, JPH::Vec3Arg p_scale, JPH::SubShapeID &p_remainder) const override { return mInnerShape->GetSubShapeTransformedShape(p_sub_shape_id, p_position_com, p_rotation, p_scale, p_remainder); } + + virtual void GetSubmergedVolume(JPH::Mat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, const JPH::Plane &p_surface, float &p_total_volume, float &p_submerged_volume, JPH::Vec3 &p_center_of_buoyancy JPH_IF_DEBUG_RENDERER(, JPH::RVec3Arg p_base_offset)) const override { mInnerShape->GetSubmergedVolume(p_center_of_mass_transform, p_scale, p_surface, p_total_volume, p_submerged_volume, p_center_of_buoyancy JPH_IF_DEBUG_RENDERER(, p_base_offset)); } + +#ifdef JPH_DEBUG_RENDERER + virtual void Draw(JPH::DebugRenderer *p_renderer, JPH::RMat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, JPH::ColorArg p_color, bool p_use_material_colors, bool p_draw_wireframe) const override { mInnerShape->Draw(p_renderer, p_center_of_mass_transform, p_scale, p_color, p_use_material_colors, p_draw_wireframe); } + + virtual void DrawGetSupportFunction(JPH::DebugRenderer *p_renderer, JPH::RMat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, JPH::ColorArg p_color, bool p_draw_support_direction) const override { mInnerShape->DrawGetSupportFunction(p_renderer, p_center_of_mass_transform, p_scale, p_color, p_draw_support_direction); } + + virtual void DrawGetSupportingFace(JPH::DebugRenderer *p_renderer, JPH::RMat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale) const override { mInnerShape->DrawGetSupportingFace(p_renderer, p_center_of_mass_transform, p_scale); } +#endif + + virtual bool CastRay(const JPH::RayCast &p_ray, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::RayCastResult &p_hit) const override { return mInnerShape->CastRay(p_ray, p_sub_shape_id_creator, p_hit); } + + virtual void CastRay(const JPH::RayCast &p_ray, const JPH::RayCastSettings &p_ray_cast_settings, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::CastRayCollector &p_collector, const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const override { return mInnerShape->CastRay(p_ray, p_ray_cast_settings, p_sub_shape_id_creator, p_collector, p_shape_filter); } + + virtual void CollidePoint(JPH::Vec3Arg p_point, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::CollidePointCollector &p_collector, const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const override { mInnerShape->CollidePoint(p_point, p_sub_shape_id_creator, p_collector, p_shape_filter); } + + virtual void CollideSoftBodyVertices(JPH::Mat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, const JPH::CollideSoftBodyVertexIterator &p_vertices, JPH::uint p_num_vertices, int p_colliding_shape_index) const override { mInnerShape->CollideSoftBodyVertices(p_center_of_mass_transform, p_scale, p_vertices, p_num_vertices, p_colliding_shape_index); } + + virtual void CollectTransformedShapes(const JPH::AABox &p_box, JPH::Vec3Arg p_position_com, JPH::QuatArg p_rotation, JPH::Vec3Arg p_scale, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::TransformedShapeCollector &p_collector, const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const override { mInnerShape->CollectTransformedShapes(p_box, p_position_com, p_rotation, p_scale, p_sub_shape_id_creator, p_collector, p_shape_filter); } + + virtual void TransformShape(JPH::Mat44Arg p_center_of_mass_transform, JPH::TransformedShapeCollector &p_collector) const override { mInnerShape->TransformShape(p_center_of_mass_transform, p_collector); } + + virtual void GetTrianglesStart(GetTrianglesContext &p_context, const JPH::AABox &p_box, JPH::Vec3Arg p_position_com, JPH::QuatArg p_rotation, JPH::Vec3Arg p_scale) const override { mInnerShape->GetTrianglesStart(p_context, p_box, p_position_com, p_rotation, p_scale); } + + virtual int GetTrianglesNext(GetTrianglesContext &p_context, int p_max_triangles_requested, JPH::Float3 *p_triangle_vertices, const JPH::PhysicsMaterial **p_materials = nullptr) const override { return mInnerShape->GetTrianglesNext(p_context, p_max_triangles_requested, p_triangle_vertices, p_materials); } + + virtual Stats GetStats() const override { return { sizeof(*this), 0 }; } + + virtual float GetVolume() const override { return mInnerShape->GetVolume(); } +}; + +#endif // JOLT_CUSTOM_DECORATED_SHAPE_H diff --git a/modules/jolt_physics/shapes/jolt_custom_double_sided_shape.cpp b/modules/jolt_physics/shapes/jolt_custom_double_sided_shape.cpp new file mode 100644 index 000000000000..7511d8f2ac72 --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_custom_double_sided_shape.cpp @@ -0,0 +1,113 @@ +/**************************************************************************/ +/* jolt_custom_double_sided_shape.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_custom_double_sided_shape.h" + +#include "../jolt_project_settings.h" + +#include "core/error/error_macros.h" + +#include "Jolt/Physics/Collision/CollisionDispatch.h" +#include "Jolt/Physics/Collision/RayCast.h" + +namespace { + +JPH::Shape *construct_double_sided() { + return new JoltCustomDoubleSidedShape(); +} + +void collide_double_sided_vs_shape(const JPH::Shape *p_shape1, const JPH::Shape *p_shape2, JPH::Vec3Arg p_scale1, JPH::Vec3Arg p_scale2, JPH::Mat44Arg p_center_of_mass_transform1, JPH::Mat44Arg p_center_of_mass_transform2, const JPH::SubShapeIDCreator &p_sub_shape_id_creator1, const JPH::SubShapeIDCreator &p_sub_shape_id_creator2, const JPH::CollideShapeSettings &p_collide_shape_settings, JPH::CollideShapeCollector &p_collector, const JPH::ShapeFilter &p_shape_filter) { + ERR_FAIL_COND(p_shape1->GetSubType() != JoltCustomShapeSubType::DOUBLE_SIDED); + + const JoltCustomDoubleSidedShape *shape1 = static_cast(p_shape1); + + JPH::CollideShapeSettings new_collide_shape_settings = p_collide_shape_settings; + new_collide_shape_settings.mBackFaceMode = JPH::EBackFaceMode::CollideWithBackFaces; + + JPH::CollisionDispatch::sCollideShapeVsShape(shape1->GetInnerShape(), p_shape2, p_scale1, p_scale2, p_center_of_mass_transform1, p_center_of_mass_transform2, p_sub_shape_id_creator1, p_sub_shape_id_creator2, new_collide_shape_settings, p_collector, p_shape_filter); +} + +void collide_shape_vs_double_sided(const JPH::Shape *p_shape1, const JPH::Shape *p_shape2, JPH::Vec3Arg p_scale1, JPH::Vec3Arg p_scale2, JPH::Mat44Arg p_center_of_mass_transform1, JPH::Mat44Arg p_center_of_mass_transform2, const JPH::SubShapeIDCreator &p_sub_shape_id_creator1, const JPH::SubShapeIDCreator &p_sub_shape_id_creator2, const JPH::CollideShapeSettings &p_collide_shape_settings, JPH::CollideShapeCollector &p_collector, const JPH::ShapeFilter &p_shape_filter) { + ERR_FAIL_COND(p_shape2->GetSubType() != JoltCustomShapeSubType::DOUBLE_SIDED); + + const JoltCustomDoubleSidedShape *shape2 = static_cast(p_shape2); + + JPH::CollideShapeSettings new_collide_shape_settings = p_collide_shape_settings; + new_collide_shape_settings.mBackFaceMode = JPH::EBackFaceMode::CollideWithBackFaces; + + JPH::CollisionDispatch::sCollideShapeVsShape(p_shape1, shape2->GetInnerShape(), p_scale1, p_scale2, p_center_of_mass_transform1, p_center_of_mass_transform2, p_sub_shape_id_creator1, p_sub_shape_id_creator2, new_collide_shape_settings, p_collector, p_shape_filter); +} + +void cast_shape_vs_double_sided(const JPH::ShapeCast &p_shape_cast, const JPH::ShapeCastSettings &p_shape_cast_settings, const JPH::Shape *p_shape, JPH::Vec3Arg p_scale, const JPH::ShapeFilter &p_shape_filter, JPH::Mat44Arg p_center_of_mass_transform2, const JPH::SubShapeIDCreator &p_sub_shape_id_creator1, const JPH::SubShapeIDCreator &p_sub_shape_id_creator2, JPH::CastShapeCollector &p_collector) { + ERR_FAIL_COND(p_shape->GetSubType() != JoltCustomShapeSubType::DOUBLE_SIDED); + + const auto *shape = static_cast(p_shape); + + JPH::ShapeCastSettings new_shape_cast_settings = p_shape_cast_settings; + new_shape_cast_settings.mBackFaceModeTriangles = JPH::EBackFaceMode::CollideWithBackFaces; + + JPH::CollisionDispatch::sCastShapeVsShapeLocalSpace(p_shape_cast, new_shape_cast_settings, shape->GetInnerShape(), p_scale, p_shape_filter, p_center_of_mass_transform2, p_sub_shape_id_creator1, p_sub_shape_id_creator2, p_collector); +} + +} // namespace + +JPH::ShapeSettings::ShapeResult JoltCustomDoubleSidedShapeSettings::Create() const { + if (mCachedResult.IsEmpty()) { + new JoltCustomDoubleSidedShape(*this, mCachedResult); + } + + return mCachedResult; +} + +void JoltCustomDoubleSidedShape::register_type() { + JPH::ShapeFunctions &shape_functions = JPH::ShapeFunctions::sGet(JoltCustomShapeSubType::DOUBLE_SIDED); + + shape_functions.mConstruct = construct_double_sided; + shape_functions.mColor = JPH::Color::sPurple; + + for (const JPH::EShapeSubType sub_type : JPH::sAllSubShapeTypes) { + JPH::CollisionDispatch::sRegisterCollideShape(JoltCustomShapeSubType::DOUBLE_SIDED, sub_type, collide_double_sided_vs_shape); + JPH::CollisionDispatch::sRegisterCollideShape(sub_type, JoltCustomShapeSubType::DOUBLE_SIDED, collide_shape_vs_double_sided); + } + + for (const JPH::EShapeSubType sub_type : JPH::sConvexSubShapeTypes) { + JPH::CollisionDispatch::sRegisterCastShape(sub_type, JoltCustomShapeSubType::DOUBLE_SIDED, cast_shape_vs_double_sided); + } +} + +void JoltCustomDoubleSidedShape::CastRay(const JPH::RayCast &p_ray, const JPH::RayCastSettings &p_ray_cast_settings, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::CastRayCollector &p_collector, const JPH::ShapeFilter &p_shape_filter) const { + JPH::RayCastSettings new_ray_cast_settings = p_ray_cast_settings; + + if (!back_face_collision) { + new_ray_cast_settings.SetBackFaceMode(JPH::EBackFaceMode::IgnoreBackFaces); + } + + return mInnerShape->CastRay(p_ray, new_ray_cast_settings, p_sub_shape_id_creator, p_collector, p_shape_filter); +} diff --git a/modules/jolt_physics/shapes/jolt_custom_double_sided_shape.h b/modules/jolt_physics/shapes/jolt_custom_double_sided_shape.h new file mode 100644 index 000000000000..664b633a2129 --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_custom_double_sided_shape.h @@ -0,0 +1,74 @@ +/**************************************************************************/ +/* jolt_custom_double_sided_shape.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_CUSTOM_DOUBLE_SIDED_SHAPE_H +#define JOLT_CUSTOM_DOUBLE_SIDED_SHAPE_H + +#include "jolt_custom_decorated_shape.h" +#include "jolt_custom_shape_type.h" + +class JoltCustomDoubleSidedShapeSettings final : public JoltCustomDecoratedShapeSettings { +public: + bool back_face_collision = false; + + JoltCustomDoubleSidedShapeSettings() = default; + + JoltCustomDoubleSidedShapeSettings(const ShapeSettings *p_inner_settings, bool p_back_face_collision) : + JoltCustomDecoratedShapeSettings(p_inner_settings), back_face_collision(p_back_face_collision) {} + + JoltCustomDoubleSidedShapeSettings(const JPH::Shape *p_inner_shape, bool p_back_face_collision) : + JoltCustomDecoratedShapeSettings(p_inner_shape), back_face_collision(p_back_face_collision) {} + + virtual JPH::Shape::ShapeResult Create() const override; +}; + +class JoltCustomDoubleSidedShape final : public JoltCustomDecoratedShape { + bool back_face_collision = false; + +public: + static void register_type(); + + JoltCustomDoubleSidedShape() : + JoltCustomDecoratedShape(JoltCustomShapeSubType::DOUBLE_SIDED) {} + + JoltCustomDoubleSidedShape(const JoltCustomDoubleSidedShapeSettings &p_settings, JPH::Shape::ShapeResult &p_result) : + JoltCustomDecoratedShape(JoltCustomShapeSubType::DOUBLE_SIDED, p_settings, p_result), back_face_collision(p_settings.back_face_collision) { + if (!p_result.HasError()) { + p_result.Set(this); + } + } + + JoltCustomDoubleSidedShape(const JPH::Shape *p_inner_shape, bool p_back_face_collision) : + JoltCustomDecoratedShape(JoltCustomShapeSubType::DOUBLE_SIDED, p_inner_shape), back_face_collision(p_back_face_collision) {} + + virtual void CastRay(const JPH::RayCast &p_ray, const JPH::RayCastSettings &p_ray_cast_settings, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::CastRayCollector &p_collector, const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const override; +}; + +#endif // JOLT_CUSTOM_DOUBLE_SIDED_SHAPE_H diff --git a/modules/jolt_physics/shapes/jolt_custom_motion_shape.cpp b/modules/jolt_physics/shapes/jolt_custom_motion_shape.cpp new file mode 100644 index 000000000000..c0cd63833701 --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_custom_motion_shape.cpp @@ -0,0 +1,78 @@ +/**************************************************************************/ +/* jolt_custom_motion_shape.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_custom_motion_shape.h" + +namespace { + +class JoltMotionConvexSupport final : public JPH::ConvexShape::Support { +public: + JoltMotionConvexSupport(JPH::Vec3Arg p_motion, const JPH::ConvexShape::Support *p_inner_support) : + motion(p_motion), + inner_support(p_inner_support) {} + + virtual JPH::Vec3 GetSupport(JPH::Vec3Arg p_direction) const override { + JPH::Vec3 support = inner_support->GetSupport(p_direction); + + if (p_direction.Dot(motion) > 0) { + support += motion; + } + + return support; + } + + virtual float GetConvexRadius() const override { return inner_support->GetConvexRadius(); } + +private: + JPH::Vec3 motion = JPH::Vec3::sZero(); + + const JPH::ConvexShape::Support *inner_support = nullptr; +}; + +} // namespace + +JPH::AABox JoltCustomMotionShape::GetLocalBounds() const { + JPH::AABox aabb = inner_shape.GetLocalBounds(); + JPH::AABox aabb_translated = aabb; + aabb_translated.Translate(motion); + aabb.Encapsulate(aabb_translated); + + return aabb; +} + +void JoltCustomMotionShape::GetSupportingFace(const JPH::SubShapeID &p_sub_shape_id, JPH::Vec3Arg p_direction, JPH::Vec3Arg p_scale, JPH::Mat44Arg p_center_of_mass_transform, JPH::Shape::SupportingFace &p_vertices) const { + // This is technically called when using the enhanced internal edge removal, but `JPH::InternalEdgeRemovingCollector` will + // only ever use the faces of the second shape in the collision pair, and this shape will always be the first in the pair, so + // we can safely skip this. +} + +const JPH::ConvexShape::Support *JoltCustomMotionShape::GetSupportFunction(JPH::ConvexShape::ESupportMode p_mode, JPH::ConvexShape::SupportBuffer &p_buffer, JPH::Vec3Arg p_scale) const { + return new (&p_buffer) JoltMotionConvexSupport(motion, inner_shape.GetSupportFunction(p_mode, inner_support_buffer, p_scale)); +} diff --git a/modules/jolt_physics/shapes/jolt_custom_motion_shape.h b/modules/jolt_physics/shapes/jolt_custom_motion_shape.h new file mode 100644 index 000000000000..52a907c010e5 --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_custom_motion_shape.h @@ -0,0 +1,117 @@ +/**************************************************************************/ +/* jolt_custom_motion_shape.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_CUSTOM_MOTION_SHAPE_H +#define JOLT_CUSTOM_MOTION_SHAPE_H + +#include "jolt_custom_shape_type.h" + +#include "core/error/error_macros.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/Collision/Shape/ConvexShape.h" +#include "Jolt/Physics/Collision/TransformedShape.h" + +class JoltCustomMotionShape final : public JPH::ConvexShape { + mutable JPH::ConvexShape::SupportBuffer inner_support_buffer; + + JPH::Vec3 motion = JPH::Vec3::sZero(); + + const JPH::ConvexShape &inner_shape; + +public: + explicit JoltCustomMotionShape(const JPH::ConvexShape &p_shape) : + JPH::ConvexShape(JoltCustomShapeSubType::MOTION), inner_shape(p_shape) {} + + virtual bool MustBeStatic() const override { return false; } + + virtual JPH::Vec3 GetCenterOfMass() const override { ERR_FAIL_V_MSG(JPH::Vec3::sZero(), "Not implemented."); } + + virtual JPH::AABox GetLocalBounds() const override; + + virtual JPH::uint GetSubShapeIDBitsRecursive() const override { ERR_FAIL_V_MSG(0, "Not implemented."); } + + virtual JPH::AABox GetWorldSpaceBounds(JPH::Mat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale) const override { ERR_FAIL_V_MSG(JPH::AABox(), "Not implemented."); } + + virtual float GetInnerRadius() const override { ERR_FAIL_V_MSG(0.0f, "Not implemented."); } + + virtual JPH::MassProperties GetMassProperties() const override { ERR_FAIL_V_MSG(JPH::MassProperties(), "Not implemented."); } + + virtual const JPH::PhysicsMaterial *GetMaterial(const JPH::SubShapeID &p_sub_shape_id) const override { ERR_FAIL_V_MSG(nullptr, "Not implemented."); } + + virtual JPH::Vec3 GetSurfaceNormal(const JPH::SubShapeID &p_sub_shape_id, JPH::Vec3Arg p_local_surface_position) const override { ERR_FAIL_V_MSG(JPH::Vec3::sZero(), "Not implemented."); } + + virtual void GetSupportingFace(const JPH::SubShapeID &p_sub_shape_id, JPH::Vec3Arg p_direction, JPH::Vec3Arg p_scale, JPH::Mat44Arg p_center_of_mass_transform, JPH::Shape::SupportingFace &p_vertices) const override; + + virtual JPH::uint64 GetSubShapeUserData(const JPH::SubShapeID &p_sub_shape_id) const override { ERR_FAIL_V_MSG(0, "Not implemented."); } + + virtual JPH::TransformedShape GetSubShapeTransformedShape(const JPH::SubShapeID &p_sub_shape_id, JPH::Vec3Arg p_position_com, JPH::QuatArg p_rotation, JPH::Vec3Arg p_scale, JPH::SubShapeID &p_remainder) const override { ERR_FAIL_V_MSG(JPH::TransformedShape(), "Not implemented."); } + + virtual void GetSubmergedVolume(JPH::Mat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, const JPH::Plane &p_surface, float &p_total_volume, float &p_submerged_volume, JPH::Vec3 &p_center_of_buoyancy JPH_IF_DEBUG_RENDERER(, JPH::RVec3Arg p_base_offset)) const override { ERR_FAIL_MSG("Not implemented."); } + + virtual const JPH::ConvexShape::Support *GetSupportFunction(JPH::ConvexShape::ESupportMode p_mode, JPH::ConvexShape::SupportBuffer &p_buffer, JPH::Vec3Arg p_scale) const override; + +#ifdef JPH_DEBUG_RENDERER + virtual void Draw(JPH::DebugRenderer *p_renderer, JPH::RMat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, JPH::ColorArg p_color, bool p_use_material_colors, bool p_draw_wireframe) const override { ERR_FAIL_MSG("Not implemented."); } + + virtual void DrawGetSupportFunction(JPH::DebugRenderer *p_renderer, JPH::RMat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, JPH::ColorArg p_color, bool p_draw_support_direction) const override { ERR_FAIL_MSG("Not implemented."); } + + virtual void DrawGetSupportingFace(JPH::DebugRenderer *p_renderer, JPH::RMat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale) const override { ERR_FAIL_MSG("Not implemented."); } +#endif + + virtual bool CastRay(const JPH::RayCast &p_ray, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::RayCastResult &p_hit) const override { ERR_FAIL_V_MSG(false, "Not implemented."); } + + virtual void CastRay(const JPH::RayCast &p_ray, const JPH::RayCastSettings &p_ray_cast_settings, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::CastRayCollector &p_collector, const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const override { ERR_FAIL_MSG("Not implemented."); } + + virtual void CollidePoint(JPH::Vec3Arg p_point, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::CollidePointCollector &p_collector, const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const override { ERR_FAIL_MSG("Not implemented."); } + + virtual void CollideSoftBodyVertices(JPH::Mat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, const JPH::CollideSoftBodyVertexIterator &p_vertices, JPH::uint p_num_vertices, int p_colliding_shape_index) const override { ERR_FAIL_MSG("Not implemented."); } + + virtual void CollectTransformedShapes(const JPH::AABox &p_box, JPH::Vec3Arg p_position_com, JPH::QuatArg p_rotation, JPH::Vec3Arg p_scale, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::TransformedShapeCollector &p_collector, const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const override { ERR_FAIL_MSG("Not implemented."); } + + virtual void TransformShape(JPH::Mat44Arg p_center_of_mass_transform, JPH::TransformedShapeCollector &p_collector) const override { ERR_FAIL_MSG("Not implemented."); } + + virtual void GetTrianglesStart(GetTrianglesContext &p_context, const JPH::AABox &p_box, JPH::Vec3Arg p_position_com, JPH::QuatArg p_rotation, JPH::Vec3Arg p_scale) const override { ERR_FAIL_MSG("Not implemented."); } + + virtual int GetTrianglesNext(GetTrianglesContext &p_context, int p_max_triangles_requested, JPH::Float3 *p_triangle_vertices, const JPH::PhysicsMaterial **p_materials = nullptr) const override { ERR_FAIL_V_MSG(0, "Not implemented."); } + + virtual JPH::Shape::Stats GetStats() const override { return { sizeof(*this), 0 }; } + + virtual float GetVolume() const override { ERR_FAIL_V_MSG(0.0f, "Not implemented."); } + + virtual bool IsValidScale(JPH::Vec3Arg p_scale) const override { ERR_FAIL_V_MSG(false, "Not implemented."); } + + const JPH::ConvexShape &get_inner_shape() const { return inner_shape; } + + void set_motion(JPH::Vec3Arg p_motion) { motion = p_motion; } +}; + +#endif // JOLT_CUSTOM_MOTION_SHAPE_H diff --git a/modules/jolt_physics/shapes/jolt_custom_ray_shape.cpp b/modules/jolt_physics/shapes/jolt_custom_ray_shape.cpp new file mode 100644 index 000000000000..92ab10df73dc --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_custom_ray_shape.cpp @@ -0,0 +1,234 @@ +/**************************************************************************/ +/* jolt_custom_ray_shape.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_custom_ray_shape.h" + +#include "../spaces/jolt_query_collectors.h" + +#include "core/error/error_macros.h" + +#include "Jolt/Physics/Collision/CastResult.h" +#include "Jolt/Physics/Collision/RayCast.h" +#include "Jolt/Physics/Collision/TransformedShape.h" + +#ifdef JPH_DEBUG_RENDERER +#include "Jolt/Renderer/DebugRenderer.h" +#endif + +namespace { + +class JoltCustomRayShapeSupport final : public JPH::ConvexShape::Support { +public: + explicit JoltCustomRayShapeSupport(float p_length) : + length(p_length) {} + + virtual JPH::Vec3 GetSupport(JPH::Vec3Arg p_direction) const override { + if (p_direction.GetZ() > 0.0f) { + return { 0.0f, 0.0f, length }; + } else { + return JPH::Vec3::sZero(); + } + } + + virtual float GetConvexRadius() const override { return 0.0f; } + +private: + float length = 0.0f; +}; + +static_assert(sizeof(JoltCustomRayShapeSupport) <= sizeof(JPH::ConvexShape::SupportBuffer), "Size of SeparationRayShape3D support is larger than size of support buffer."); + +JPH::Shape *construct_ray() { + return new JoltCustomRayShape(); +} + +void collide_ray_vs_shape(const JPH::Shape *p_shape1, const JPH::Shape *p_shape2, JPH::Vec3Arg p_scale1, JPH::Vec3Arg p_scale2, JPH::Mat44Arg p_center_of_mass_transform1, JPH::Mat44Arg p_center_of_mass_transform2, const JPH::SubShapeIDCreator &p_sub_shape_id_creator1, const JPH::SubShapeIDCreator &p_sub_shape_id_creator2, const JPH::CollideShapeSettings &p_collide_shape_settings, JPH::CollideShapeCollector &p_collector, const JPH::ShapeFilter &p_shape_filter) { + ERR_FAIL_COND(p_shape1->GetSubType() != JoltCustomShapeSubType::RAY); + + const JoltCustomRayShape *shape1 = static_cast(p_shape1); + + const float margin = p_collide_shape_settings.mMaxSeparationDistance; + const float ray_length = shape1->length; + const float ray_length_padded = ray_length + margin; + + const JPH::Mat44 transform1 = p_center_of_mass_transform1 * JPH::Mat44::sScale(p_scale1); + const JPH::Mat44 transform2 = p_center_of_mass_transform2 * JPH::Mat44::sScale(p_scale2); + const JPH::Mat44 transform_inv2 = transform2.Inversed(); + + const JPH::Vec3 ray_start = transform1.GetTranslation(); + const JPH::Vec3 ray_direction = transform1.GetAxisZ(); + const JPH::Vec3 ray_vector = ray_direction * ray_length; + const JPH::Vec3 ray_vector_padded = ray_direction * ray_length_padded; + + const JPH::Vec3 ray_start2 = transform_inv2 * ray_start; + const JPH::Vec3 ray_direction2 = transform_inv2.Multiply3x3(ray_direction); + const JPH::Vec3 ray_vector_padded2 = transform_inv2.Multiply3x3(ray_vector_padded); + + const JPH::RayCast ray_cast(ray_start2, ray_vector_padded2); + + JPH::RayCastSettings ray_cast_settings; + ray_cast_settings.mTreatConvexAsSolid = false; + ray_cast_settings.mBackFaceModeTriangles = p_collide_shape_settings.mBackFaceMode; + + JoltQueryCollectorClosest ray_collector; + + p_shape2->CastRay(ray_cast, ray_cast_settings, p_sub_shape_id_creator2, ray_collector); + + if (!ray_collector.had_hit()) { + return; + } + + const JPH::RayCastResult &hit = ray_collector.get_hit(); + + const float hit_distance = ray_length_padded * hit.mFraction; + const float hit_depth = ray_length - hit_distance; + + if (-hit_depth >= p_collector.GetEarlyOutFraction()) { + return; + } + + // Since `hit.mSubShapeID2` could represent a path not only from `p_shape2` but also any + // compound shape that it's contained within, we need to split this path into something that + // `p_shape2` can actually understand. + JPH::SubShapeID local_sub_shape_id2; + hit.mSubShapeID2.PopID(p_sub_shape_id_creator2.GetNumBitsWritten(), local_sub_shape_id2); + + const JPH::Vec3 hit_point2 = ray_cast.GetPointOnRay(hit.mFraction); + + const JPH::Vec3 hit_point_on_1 = ray_start + ray_vector; + const JPH::Vec3 hit_point_on_2 = transform2 * hit_point2; + + JPH::Vec3 hit_normal2 = JPH::Vec3::sZero(); + + if (shape1->slide_on_slope) { + hit_normal2 = p_shape2->GetSurfaceNormal(local_sub_shape_id2, hit_point2); + + // If we got a back-face normal we need to flip it. + if (hit_normal2.Dot(ray_direction2) > 0) { + hit_normal2 = -hit_normal2; + } + } else { + hit_normal2 = -ray_direction2; + } + + const JPH::Vec3 hit_normal = transform2.Multiply3x3(hit_normal2); + + JPH::CollideShapeResult result(hit_point_on_1, hit_point_on_2, -hit_normal, hit_depth, p_sub_shape_id_creator1.GetID(), hit.mSubShapeID2, JPH::TransformedShape::sGetBodyID(p_collector.GetContext())); + + if (p_collide_shape_settings.mCollectFacesMode == JPH::ECollectFacesMode::CollectFaces) { + p_shape2->GetSupportingFace(local_sub_shape_id2, ray_direction2, p_scale2, p_center_of_mass_transform2, result.mShape2Face); + } + + p_collector.AddHit(result); +} + +void collide_noop(const JPH::Shape *p_shape1, const JPH::Shape *p_shape2, JPH::Vec3Arg p_scale1, JPH::Vec3Arg p_scale2, JPH::Mat44Arg p_center_of_mass_transform1, JPH::Mat44Arg p_center_of_mass_transform2, const JPH::SubShapeIDCreator &p_sub_shape_id_creator1, const JPH::SubShapeIDCreator &p_sub_shape_id_creator2, const JPH::CollideShapeSettings &p_collide_shape_settings, JPH::CollideShapeCollector &p_collector, const JPH::ShapeFilter &p_shape_filter) { +} + +void cast_noop(const JPH::ShapeCast &p_shape_cast, const JPH::ShapeCastSettings &p_shape_cast_settings, const JPH::Shape *p_shape, JPH::Vec3Arg p_scale, const JPH::ShapeFilter &p_shape_filter, JPH::Mat44Arg p_center_of_mass_transform2, const JPH::SubShapeIDCreator &p_sub_shape_id_creator1, const JPH::SubShapeIDCreator &p_sub_shape_id_creator2, JPH::CastShapeCollector &p_collector) { +} + +} // namespace + +JPH::ShapeSettings::ShapeResult JoltCustomRayShapeSettings::Create() const { + if (mCachedResult.IsEmpty()) { + new JoltCustomRayShape(*this, mCachedResult); + } + + return mCachedResult; +} + +void JoltCustomRayShape::register_type() { + JPH::ShapeFunctions &shape_functions = JPH::ShapeFunctions::sGet(JoltCustomShapeSubType::RAY); + + shape_functions.mConstruct = construct_ray; + shape_functions.mColor = JPH::Color::sDarkRed; + + static constexpr JPH::EShapeSubType concrete_sub_types[] = { + JPH::EShapeSubType::Sphere, + JPH::EShapeSubType::Box, + JPH::EShapeSubType::Triangle, + JPH::EShapeSubType::Capsule, + JPH::EShapeSubType::TaperedCapsule, + JPH::EShapeSubType::Cylinder, + JPH::EShapeSubType::ConvexHull, + JPH::EShapeSubType::Mesh, + JPH::EShapeSubType::HeightField, + JPH::EShapeSubType::Plane, + JPH::EShapeSubType::TaperedCylinder + }; + + for (const JPH::EShapeSubType concrete_sub_type : concrete_sub_types) { + JPH::CollisionDispatch::sRegisterCollideShape(JoltCustomShapeSubType::RAY, concrete_sub_type, collide_ray_vs_shape); + JPH::CollisionDispatch::sRegisterCollideShape(concrete_sub_type, JoltCustomShapeSubType::RAY, JPH::CollisionDispatch::sReversedCollideShape); + } + + JPH::CollisionDispatch::sRegisterCollideShape(JoltCustomShapeSubType::RAY, JoltCustomShapeSubType::RAY, collide_noop); + + for (const JPH::EShapeSubType sub_type : JPH::sAllSubShapeTypes) { + JPH::CollisionDispatch::sRegisterCastShape(JoltCustomShapeSubType::RAY, sub_type, cast_noop); + JPH::CollisionDispatch::sRegisterCastShape(sub_type, JoltCustomShapeSubType::RAY, cast_noop); + } +} + +JPH::AABox JoltCustomRayShape::GetLocalBounds() const { + const float radius = GetInnerRadius(); + return { JPH::Vec3(-radius, -radius, 0.0f), JPH::Vec3(radius, radius, length) }; +} + +float JoltCustomRayShape::GetInnerRadius() const { + // There is no sensible value here, since this shape is infinitely thin, so we pick something + // that's hopefully small enough to effectively be zero, but big enough to not cause any + // numerical issues. + return 0.0001f; +} + +JPH::MassProperties JoltCustomRayShape::GetMassProperties() const { + JPH::MassProperties mass_properties; + + // Since this shape has no volume we can't really give it a correct set of mass properties, so + // instead we just give it some arbitrary ones. + mass_properties.mMass = 1.0f; + mass_properties.mInertia = JPH::Mat44::sIdentity(); + + return mass_properties; +} + +#ifdef JPH_DEBUG_RENDERER + +void JoltCustomRayShape::Draw(JPH::DebugRenderer *p_renderer, JPH::RMat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, JPH::ColorArg p_color, bool p_use_material_colors, bool p_draw_wireframe) const { + p_renderer->DrawArrow(p_center_of_mass_transform.GetTranslation(), p_center_of_mass_transform * JPH::Vec3(0, 0, length * p_scale.GetZ()), p_use_material_colors ? GetMaterial()->GetDebugColor() : p_color, 0.1f); +} + +#endif + +const JPH::ConvexShape::Support *JoltCustomRayShape::GetSupportFunction(JPH::ConvexShape::ESupportMode p_mode, JPH::ConvexShape::SupportBuffer &p_buffer, JPH::Vec3Arg p_scale) const { + return new (&p_buffer) JoltCustomRayShapeSupport(p_scale.GetZ() * length); +} diff --git a/modules/jolt_physics/shapes/jolt_custom_ray_shape.h b/modules/jolt_physics/shapes/jolt_custom_ray_shape.h new file mode 100644 index 000000000000..30da52f17008 --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_custom_ray_shape.h @@ -0,0 +1,107 @@ +/**************************************************************************/ +/* jolt_custom_ray_shape.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_CUSTOM_RAY_SHAPE_H +#define JOLT_CUSTOM_RAY_SHAPE_H + +#include "jolt_custom_shape_type.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/Collision/Shape/ConvexShape.h" + +class JoltCustomRayShapeSettings final : public JPH::ConvexShapeSettings { +public: + JPH::RefConst material; + float length = 1.0f; + bool slide_on_slope = false; + + JoltCustomRayShapeSettings() = default; + JoltCustomRayShapeSettings(float p_length, bool p_slide_on_slope, const JPH::PhysicsMaterial *p_material = nullptr) : + material(p_material), length(p_length), slide_on_slope(p_slide_on_slope) {} + + virtual JPH::ShapeSettings::ShapeResult Create() const override; +}; + +class JoltCustomRayShape final : public JPH::ConvexShape { +public: + JPH::RefConst material; + float length = 0.0f; + bool slide_on_slope = false; + + static void register_type(); + + JoltCustomRayShape() : + JPH::ConvexShape(JoltCustomShapeSubType::RAY) {} + + JoltCustomRayShape(const JoltCustomRayShapeSettings &p_settings, JPH::Shape::ShapeResult &p_result) : + JPH::ConvexShape(JoltCustomShapeSubType::RAY, p_settings, p_result), material(p_settings.material), length(p_settings.length), slide_on_slope(p_settings.slide_on_slope) { + if (!p_result.HasError()) { + p_result.Set(this); + } + } + + JoltCustomRayShape(float p_length, bool p_slide_on_slope, const JPH::PhysicsMaterial *p_material = nullptr) : + JPH::ConvexShape(JoltCustomShapeSubType::RAY), material(p_material), length(p_length), slide_on_slope(p_slide_on_slope) {} + + virtual JPH::AABox GetLocalBounds() const override; + + virtual float GetInnerRadius() const override; + + virtual JPH::MassProperties GetMassProperties() const override; + + virtual JPH::Vec3 GetSurfaceNormal(const JPH::SubShapeID &p_sub_shape_id, JPH::Vec3Arg p_local_surface_position) const override { return JPH::Vec3::sAxisZ(); } + + virtual void GetSubmergedVolume(JPH::Mat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, const JPH::Plane &p_surface, float &p_total_volume, float &p_submerged_volume, JPH::Vec3 &p_center_of_buoyancy JPH_IF_DEBUG_RENDERER(, JPH::RVec3Arg p_base_offset)) const override { + p_total_volume = 0.0f; + p_submerged_volume = 0.0f; + p_center_of_buoyancy = JPH::Vec3::sZero(); + } + +#ifdef JPH_DEBUG_RENDERER + virtual void Draw(JPH::DebugRenderer *p_renderer, JPH::RMat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, JPH::ColorArg p_color, bool p_use_material_colors, bool p_draw_wireframe) const override; +#endif + + virtual bool CastRay(const JPH::RayCast &p_ray, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::RayCastResult &p_hit) const override { return false; } + + virtual void CastRay(const JPH::RayCast &p_ray, const JPH::RayCastSettings &p_ray_cast_settings, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::CastRayCollector &p_collector, const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const override {} + + virtual void CollidePoint(JPH::Vec3Arg p_point, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::CollidePointCollector &p_collector, const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const override {} + + virtual void CollideSoftBodyVertices(JPH::Mat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, const JPH::CollideSoftBodyVertexIterator &p_vertices, JPH::uint p_num_vertices, int p_colliding_shape_index) const override {} + + virtual JPH::Shape::Stats GetStats() const override { return { sizeof(*this), 0 }; } + + virtual float GetVolume() const override { return 0.0f; } + + virtual const JPH::ConvexShape::Support *GetSupportFunction(JPH::ConvexShape::ESupportMode p_mode, JPH::ConvexShape::SupportBuffer &p_buffer, JPH::Vec3Arg p_scale) const override; +}; + +#endif // JOLT_CUSTOM_RAY_SHAPE_H diff --git a/modules/jolt_physics/shapes/jolt_custom_shape_type.h b/modules/jolt_physics/shapes/jolt_custom_shape_type.h new file mode 100644 index 000000000000..77371640c2a3 --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_custom_shape_type.h @@ -0,0 +1,47 @@ +/**************************************************************************/ +/* jolt_custom_shape_type.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_CUSTOM_SHAPE_TYPE_H +#define JOLT_CUSTOM_SHAPE_TYPE_H + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/Collision/Shape/Shape.h" + +namespace JoltCustomShapeSubType { + +constexpr JPH::EShapeSubType OVERRIDE_USER_DATA = JPH::EShapeSubType::User1; +constexpr JPH::EShapeSubType DOUBLE_SIDED = JPH::EShapeSubType::User2; +constexpr JPH::EShapeSubType RAY = JPH::EShapeSubType::UserConvex1; +constexpr JPH::EShapeSubType MOTION = JPH::EShapeSubType::UserConvex2; + +} // namespace JoltCustomShapeSubType + +#endif // JOLT_CUSTOM_SHAPE_TYPE_H diff --git a/modules/jolt_physics/shapes/jolt_custom_user_data_shape.cpp b/modules/jolt_physics/shapes/jolt_custom_user_data_shape.cpp new file mode 100644 index 000000000000..53637ebe6322 --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_custom_user_data_shape.cpp @@ -0,0 +1,99 @@ +/**************************************************************************/ +/* jolt_custom_user_data_shape.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_custom_user_data_shape.h" + +#include "core/error/error_macros.h" + +#include "Jolt/Physics/Collision/CollisionDispatch.h" +#include "Jolt/Physics/Collision/ShapeCast.h" + +namespace { + +JPH::Shape *construct_override_user_data() { + return new JoltCustomUserDataShape(); +} + +void collide_override_user_data_vs_shape(const JPH::Shape *p_shape1, const JPH::Shape *p_shape2, JPH::Vec3Arg p_scale1, JPH::Vec3Arg p_scale2, JPH::Mat44Arg p_center_of_mass_transform1, JPH::Mat44Arg p_center_of_mass_transform2, const JPH::SubShapeIDCreator &p_sub_shape_id_creator1, const JPH::SubShapeIDCreator &p_sub_shape_id_creator2, const JPH::CollideShapeSettings &p_collide_shape_settings, JPH::CollideShapeCollector &p_collector, const JPH::ShapeFilter &p_shape_filter) { + ERR_FAIL_COND(p_shape1->GetSubType() != JoltCustomShapeSubType::OVERRIDE_USER_DATA); + + const JoltCustomUserDataShape *shape1 = static_cast(p_shape1); + + JPH::CollisionDispatch::sCollideShapeVsShape(shape1->GetInnerShape(), p_shape2, p_scale1, p_scale2, p_center_of_mass_transform1, p_center_of_mass_transform2, p_sub_shape_id_creator1, p_sub_shape_id_creator2, p_collide_shape_settings, p_collector, p_shape_filter); +} + +void collide_shape_vs_override_user_data(const JPH::Shape *p_shape1, const JPH::Shape *p_shape2, JPH::Vec3Arg p_scale1, JPH::Vec3Arg p_scale2, JPH::Mat44Arg p_center_of_mass_transform1, JPH::Mat44Arg p_center_of_mass_transform2, const JPH::SubShapeIDCreator &p_sub_shape_id_creator1, const JPH::SubShapeIDCreator &p_sub_shape_id_creator2, const JPH::CollideShapeSettings &p_collide_shape_settings, JPH::CollideShapeCollector &p_collector, const JPH::ShapeFilter &p_shape_filter) { + ERR_FAIL_COND(p_shape2->GetSubType() != JoltCustomShapeSubType::OVERRIDE_USER_DATA); + + const JoltCustomUserDataShape *shape2 = static_cast(p_shape2); + + JPH::CollisionDispatch::sCollideShapeVsShape(p_shape1, shape2->GetInnerShape(), p_scale1, p_scale2, p_center_of_mass_transform1, p_center_of_mass_transform2, p_sub_shape_id_creator1, p_sub_shape_id_creator2, p_collide_shape_settings, p_collector, p_shape_filter); +} + +void cast_override_user_data_vs_shape(const JPH::ShapeCast &p_shape_cast, const JPH::ShapeCastSettings &p_shape_cast_settings, const JPH::Shape *p_shape, JPH::Vec3Arg p_scale, const JPH::ShapeFilter &p_shape_filter, JPH::Mat44Arg p_center_of_mass_transform2, const JPH::SubShapeIDCreator &p_sub_shape_id_creator1, const JPH::SubShapeIDCreator &p_sub_shape_id_creator2, JPH::CastShapeCollector &p_collector) { + ERR_FAIL_COND(p_shape_cast.mShape->GetSubType() != JoltCustomShapeSubType::OVERRIDE_USER_DATA); + + const JoltCustomUserDataShape *shape = static_cast(p_shape_cast.mShape); + const JPH::ShapeCast shape_cast(shape->GetInnerShape(), p_shape_cast.mScale, p_shape_cast.mCenterOfMassStart, p_shape_cast.mDirection); + + JPH::CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, p_shape_cast_settings, p_shape, p_scale, p_shape_filter, p_center_of_mass_transform2, p_sub_shape_id_creator1, p_sub_shape_id_creator2, p_collector); +} + +void cast_shape_vs_override_user_data(const JPH::ShapeCast &p_shape_cast, const JPH::ShapeCastSettings &p_shape_cast_settings, const JPH::Shape *p_shape, JPH::Vec3Arg p_scale, const JPH::ShapeFilter &p_shape_filter, JPH::Mat44Arg p_center_of_mass_transform2, const JPH::SubShapeIDCreator &p_sub_shape_id_creator1, const JPH::SubShapeIDCreator &p_sub_shape_id_creator2, JPH::CastShapeCollector &p_collector) { + ERR_FAIL_COND(p_shape->GetSubType() != JoltCustomShapeSubType::OVERRIDE_USER_DATA); + + const JoltCustomUserDataShape *shape = static_cast(p_shape); + + JPH::CollisionDispatch::sCastShapeVsShapeLocalSpace(p_shape_cast, p_shape_cast_settings, shape->GetInnerShape(), p_scale, p_shape_filter, p_center_of_mass_transform2, p_sub_shape_id_creator1, p_sub_shape_id_creator2, p_collector); +} + +} // namespace + +JPH::ShapeSettings::ShapeResult JoltCustomUserDataShapeSettings::Create() const { + if (mCachedResult.IsEmpty()) { + new JoltCustomUserDataShape(*this, mCachedResult); + } + + return mCachedResult; +} + +void JoltCustomUserDataShape::register_type() { + JPH::ShapeFunctions &shape_functions = JPH::ShapeFunctions::sGet(JoltCustomShapeSubType::OVERRIDE_USER_DATA); + + shape_functions.mConstruct = construct_override_user_data; + shape_functions.mColor = JPH::Color::sCyan; + + for (const JPH::EShapeSubType sub_type : JPH::sAllSubShapeTypes) { + JPH::CollisionDispatch::sRegisterCollideShape(JoltCustomShapeSubType::OVERRIDE_USER_DATA, sub_type, collide_override_user_data_vs_shape); + JPH::CollisionDispatch::sRegisterCollideShape(sub_type, JoltCustomShapeSubType::OVERRIDE_USER_DATA, collide_shape_vs_override_user_data); + JPH::CollisionDispatch::sRegisterCastShape(JoltCustomShapeSubType::OVERRIDE_USER_DATA, sub_type, cast_override_user_data_vs_shape); + JPH::CollisionDispatch::sRegisterCastShape(sub_type, JoltCustomShapeSubType::OVERRIDE_USER_DATA, cast_shape_vs_override_user_data); + } +} diff --git a/modules/jolt_physics/shapes/jolt_custom_user_data_shape.h b/modules/jolt_physics/shapes/jolt_custom_user_data_shape.h new file mode 100644 index 000000000000..a6610c74625c --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_custom_user_data_shape.h @@ -0,0 +1,61 @@ +/**************************************************************************/ +/* jolt_custom_user_data_shape.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_CUSTOM_USER_DATA_SHAPE_H +#define JOLT_CUSTOM_USER_DATA_SHAPE_H + +#include "jolt_custom_decorated_shape.h" +#include "jolt_custom_shape_type.h" + +class JoltCustomUserDataShapeSettings final : public JoltCustomDecoratedShapeSettings { +public: + using JoltCustomDecoratedShapeSettings::JoltCustomDecoratedShapeSettings; + + virtual ShapeResult Create() const override; +}; + +class JoltCustomUserDataShape final : public JoltCustomDecoratedShape { +public: + static void register_type(); + + JoltCustomUserDataShape() : + JoltCustomDecoratedShape(JoltCustomShapeSubType::OVERRIDE_USER_DATA) {} + + JoltCustomUserDataShape(const JoltCustomUserDataShapeSettings &p_settings, ShapeResult &p_result) : + JoltCustomDecoratedShape(JoltCustomShapeSubType::OVERRIDE_USER_DATA, p_settings, p_result) { + if (!p_result.HasError()) { + p_result.Set(this); + } + } + + virtual JPH::uint64 GetSubShapeUserData(const JPH::SubShapeID &p_sub_shape_id) const override { return GetUserData(); } +}; + +#endif // JOLT_CUSTOM_USER_DATA_SHAPE_H diff --git a/modules/jolt_physics/shapes/jolt_cylinder_shape_3d.cpp b/modules/jolt_physics/shapes/jolt_cylinder_shape_3d.cpp new file mode 100644 index 000000000000..cafc360c78e3 --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_cylinder_shape_3d.cpp @@ -0,0 +1,100 @@ +/**************************************************************************/ +/* jolt_cylinder_shape_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_cylinder_shape_3d.h" + +#include "../jolt_project_settings.h" +#include "../misc/jolt_type_conversions.h" + +#include "core/error/error_macros.h" + +#include "Jolt/Physics/Collision/Shape/CylinderShape.h" + +JPH::ShapeRefC JoltCylinderShape3D::_build() const { + const float half_height = height / 2.0f; + const float min_half_extent = MIN(half_height, radius); + const float actual_margin = MIN(margin, min_half_extent * JoltProjectSettings::get_collision_margin_fraction()); + + const JPH::CylinderShapeSettings shape_settings(half_height, radius, actual_margin); + const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create(); + ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to build Jolt Physics cylinder shape with %s. It returned the following error: '%s'. This shape belongs to %s.", to_string(), to_godot(shape_result.GetError()), _owners_to_string())); + + return shape_result.Get(); +} + +Variant JoltCylinderShape3D::get_data() const { + Dictionary data; + data["height"] = height; + data["radius"] = radius; + return data; +} + +void JoltCylinderShape3D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY); + + const Dictionary data = p_data; + + const Variant maybe_height = data.get("height", Variant()); + ERR_FAIL_COND(maybe_height.get_type() != Variant::FLOAT); + + const Variant maybe_radius = data.get("radius", Variant()); + ERR_FAIL_COND(maybe_radius.get_type() != Variant::FLOAT); + + const float new_height = maybe_height; + const float new_radius = maybe_radius; + + if (unlikely(new_height == height && new_radius == radius)) { + return; + } + + height = new_height; + radius = new_radius; + + destroy(); +} + +void JoltCylinderShape3D::set_margin(float p_margin) { + if (unlikely(margin == p_margin)) { + return; + } + + margin = p_margin; + + destroy(); +} + +AABB JoltCylinderShape3D::get_aabb() const { + const Vector3 half_extents(radius, height / 2.0f, radius); + return AABB(-half_extents, half_extents * 2.0f); +} + +String JoltCylinderShape3D::to_string() const { + return vformat("{height=%f radius=%f margin=%f}", height, radius, margin); +} diff --git a/modules/jolt_physics/shapes/jolt_cylinder_shape_3d.h b/modules/jolt_physics/shapes/jolt_cylinder_shape_3d.h new file mode 100644 index 000000000000..889a038c74a8 --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_cylinder_shape_3d.h @@ -0,0 +1,58 @@ +/**************************************************************************/ +/* jolt_cylinder_shape_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_CYLINDER_SHAPE_3D_H +#define JOLT_CYLINDER_SHAPE_3D_H + +#include "jolt_shape_3d.h" + +class JoltCylinderShape3D final : public JoltShape3D { + float height = 0.0f; + float radius = 0.0f; + float margin = 0.04f; + + virtual JPH::ShapeRefC _build() const override; + +public: + virtual ShapeType get_type() const override { return ShapeType::SHAPE_CYLINDER; } + virtual bool is_convex() const override { return true; } + + virtual Variant get_data() const override; + virtual void set_data(const Variant &p_data) override; + + virtual float get_margin() const override { return margin; } + virtual void set_margin(float p_margin) override; + + virtual AABB get_aabb() const override; + + String to_string() const; +}; + +#endif // JOLT_CYLINDER_SHAPE_3D_H diff --git a/modules/jolt_physics/shapes/jolt_height_map_shape_3d.cpp b/modules/jolt_physics/shapes/jolt_height_map_shape_3d.cpp new file mode 100644 index 000000000000..f4dd4e67d0d6 --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_height_map_shape_3d.cpp @@ -0,0 +1,235 @@ +/**************************************************************************/ +/* jolt_height_map_shape_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_height_map_shape_3d.h" + +#include "../jolt_project_settings.h" +#include "../misc/jolt_type_conversions.h" + +#include "core/error/error_macros.h" + +#include "Jolt/Physics/Collision/Shape/HeightFieldShape.h" +#include "Jolt/Physics/Collision/Shape/MeshShape.h" + +namespace { + +bool _is_vertex_hole(const JPH::VertexList &p_vertices, int p_index) { + const float height = p_vertices[(size_t)p_index].y; + return height == FLT_MAX || Math::is_nan(height); +} + +bool _is_triangle_hole(const JPH::VertexList &p_vertices, int p_index0, int p_index1, int p_index2) { + return _is_vertex_hole(p_vertices, p_index0) || _is_vertex_hole(p_vertices, p_index1) || _is_vertex_hole(p_vertices, p_index2); +} + +} // namespace + +JPH::ShapeRefC JoltHeightMapShape3D::_build() const { + const int height_count = (int)heights.size(); + if (unlikely(height_count == 0)) { + return nullptr; + } + + ERR_FAIL_COND_V_MSG(height_count != width * depth, nullptr, vformat("Failed to build Jolt Physics height map shape with %s. Height count must be the product of width and depth. This shape belongs to %s.", to_string(), _owners_to_string())); + ERR_FAIL_COND_V_MSG(width < 2 || depth < 2, nullptr, vformat("Failed to build Jolt Physics height map shape with %s. The height map must be at least 2x2. This shape belongs to %s.", to_string(), _owners_to_string())); + + if (width != depth) { + return JoltShape3D::with_double_sided(_build_mesh(), true); + } + + const int block_size = 2; // Default of JPH::HeightFieldShapeSettings::mBlockSize + const int block_count = width / block_size; + + if (block_count < 2) { + return JoltShape3D::with_double_sided(_build_mesh(), true); + } + + return JoltShape3D::with_double_sided(_build_height_field(), true); +} + +JPH::ShapeRefC JoltHeightMapShape3D::_build_height_field() const { + const int quad_count_x = width - 1; + const int quad_count_y = depth - 1; + + const float offset_x = (float)-quad_count_x / 2.0f; + const float offset_y = (float)-quad_count_y / 2.0f; + + // Jolt triangulates the height map differently from how Godot Physics does it, so we mirror the shape along the + // Z-axis to get the desired triangulation and reverse the rows to undo the mirroring. + + LocalVector heights_rev; + heights_rev.resize(heights.size()); + + const real_t *heights_ptr = heights.ptr(); + float *heights_rev_ptr = heights_rev.ptr(); + + for (int z = 0; z < depth; ++z) { + const int z_rev = (depth - 1) - z; + + const real_t *row = heights_ptr + ptrdiff_t(z * width); + float *row_rev = heights_rev_ptr + ptrdiff_t(z_rev * width); + + for (int x = 0; x < width; ++x) { + const real_t height = row[x]; + + // Godot has undocumented (accidental?) support for holes by passing NaN as the height value, whereas Jolt + // uses `FLT_MAX` instead, so we translate any NaN to `FLT_MAX` in order to be drop-in compatible. + row_rev[x] = Math::is_nan(height) ? FLT_MAX : (float)height; + } + } + + JPH::HeightFieldShapeSettings shape_settings(heights_rev.ptr(), JPH::Vec3(offset_x, 0, offset_y), JPH::Vec3::sReplicate(1.0f), (JPH::uint32)width); + + shape_settings.mBitsPerSample = shape_settings.CalculateBitsPerSampleForError(0.0f); + shape_settings.mActiveEdgeCosThresholdAngle = JoltProjectSettings::get_active_edge_threshold(); + + const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create(); + ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to build Jolt Physics height map shape with %s. It returned the following error: '%s'. This shape belongs to %s.", to_string(), to_godot(shape_result.GetError()), _owners_to_string())); + + return with_scale(shape_result.Get(), Vector3(1, 1, -1)); +} + +JPH::ShapeRefC JoltHeightMapShape3D::_build_mesh() const { + const int height_count = (int)heights.size(); + + const int quad_count_x = width - 1; + const int quad_count_z = depth - 1; + + const int quad_count = quad_count_x * quad_count_z; + const int triangle_count = quad_count * 2; + + JPH::VertexList vertices; + vertices.reserve((size_t)height_count); + + JPH::IndexedTriangleList indices; + indices.reserve((size_t)triangle_count); + + const float offset_x = (float)-quad_count_x / 2.0f; + const float offset_z = (float)-quad_count_z / 2.0f; + + for (int z = 0; z < depth; ++z) { + for (int x = 0; x < width; ++x) { + const float vertex_x = offset_x + (float)x; + const float vertex_y = (float)heights[z * width + x]; + const float vertex_z = offset_z + (float)z; + + vertices.emplace_back(vertex_x, vertex_y, vertex_z); + } + } + + for (int z = 0; z < quad_count_z; ++z) { + for (int x = 0; x < quad_count_x; ++x) { + const int index_lower_right = z * width + x; + const int index_lower_left = z * width + (x + 1); + const int index_upper_right = (z + 1) * width + x; + const int index_upper_left = (z + 1) * width + (x + 1); + + if (!_is_triangle_hole(vertices, index_lower_right, index_upper_right, index_lower_left)) { + indices.emplace_back(index_lower_right, index_upper_right, index_lower_left); + } + + if (!_is_triangle_hole(vertices, index_lower_left, index_upper_right, index_upper_left)) { + indices.emplace_back(index_lower_left, index_upper_right, index_upper_left); + } + } + } + + JPH::MeshShapeSettings shape_settings(std::move(vertices), std::move(indices)); + shape_settings.mActiveEdgeCosThresholdAngle = JoltProjectSettings::get_active_edge_threshold(); + + const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create(); + ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to build Jolt Physics height map shape (as polygon) with %s. It returned the following error: '%s'. This shape belongs to %s.", to_string(), to_godot(shape_result.GetError()), _owners_to_string())); + + return shape_result.Get(); +} + +AABB JoltHeightMapShape3D::_calculate_aabb() const { + AABB result; + + const int quad_count_x = width - 1; + const int quad_count_z = depth - 1; + + const float offset_x = (float)-quad_count_x / 2.0f; + const float offset_z = (float)-quad_count_z / 2.0f; + + for (int z = 0; z < depth; ++z) { + for (int x = 0; x < width; ++x) { + const Vector3 vertex(offset_x + (float)x, (float)heights[z * width + x], offset_z + (float)z); + + if (x == 0 && z == 0) { + result.position = vertex; + } else { + result.expand_to(vertex); + } + } + } + + return result; +} + +Variant JoltHeightMapShape3D::get_data() const { + Dictionary data; + data["width"] = width; + data["depth"] = depth; + data["heights"] = heights; + return data; +} + +void JoltHeightMapShape3D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY); + + const Dictionary data = p_data; + + const Variant maybe_heights = data.get("heights", Variant()); + +#ifdef REAL_T_IS_DOUBLE + ERR_FAIL_COND(maybe_heights.get_type() != Variant::PACKED_FLOAT64_ARRAY); +#else + ERR_FAIL_COND(maybe_heights.get_type() != Variant::PACKED_FLOAT32_ARRAY); +#endif + + const Variant maybe_width = data.get("width", Variant()); + ERR_FAIL_COND(maybe_width.get_type() != Variant::INT); + + const Variant maybe_depth = data.get("depth", Variant()); + ERR_FAIL_COND(maybe_depth.get_type() != Variant::INT); + + heights = maybe_heights; + width = maybe_width; + depth = maybe_depth; + + aabb = _calculate_aabb(); + + destroy(); +} + +String JoltHeightMapShape3D::to_string() const { + return vformat("{height_count=%d width=%d depth=%d}", heights.size(), width, depth); +} diff --git a/modules/jolt_physics/shapes/jolt_height_map_shape_3d.h b/modules/jolt_physics/shapes/jolt_height_map_shape_3d.h new file mode 100644 index 000000000000..a929970f78f9 --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_height_map_shape_3d.h @@ -0,0 +1,69 @@ +/**************************************************************************/ +/* jolt_height_map_shape_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_HEIGHT_MAP_SHAPE_3D_H +#define JOLT_HEIGHT_MAP_SHAPE_3D_H + +#include "jolt_shape_3d.h" + +class JoltHeightMapShape3D final : public JoltShape3D { + AABB aabb; + +#ifdef REAL_T_IS_DOUBLE + PackedFloat64Array heights; +#else + PackedFloat32Array heights; +#endif + + int width = 0; + int depth = 0; + + virtual JPH::ShapeRefC _build() const override; + JPH::ShapeRefC _build_height_field() const; + JPH::ShapeRefC _build_mesh() const; + + AABB _calculate_aabb() const; + +public: + virtual ShapeType get_type() const override { return ShapeType::SHAPE_HEIGHTMAP; } + virtual bool is_convex() const override { return false; } + + virtual Variant get_data() const override; + virtual void set_data(const Variant &p_data) override; + + virtual float get_margin() const override { return 0.0f; } + virtual void set_margin(float p_margin) override {} + + virtual AABB get_aabb() const override { return aabb; } + + String to_string() const; +}; + +#endif // JOLT_HEIGHT_MAP_SHAPE_3D_H diff --git a/modules/jolt_physics/shapes/jolt_separation_ray_shape_3d.cpp b/modules/jolt_physics/shapes/jolt_separation_ray_shape_3d.cpp new file mode 100644 index 000000000000..ab7df972337d --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_separation_ray_shape_3d.cpp @@ -0,0 +1,87 @@ +/**************************************************************************/ +/* jolt_separation_ray_shape_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_separation_ray_shape_3d.h" + +#include "../misc/jolt_type_conversions.h" +#include "jolt_custom_ray_shape.h" + +#include "core/error/error_macros.h" + +JPH::ShapeRefC JoltSeparationRayShape3D::_build() const { + ERR_FAIL_COND_V_MSG(length <= 0.0f, nullptr, vformat("Failed to build Jolt Physics separation ray shape with %s. Its length must be greater than 0. This shape belongs to %s.", to_string(), _owners_to_string())); + + const JoltCustomRayShapeSettings shape_settings(length, slide_on_slope); + const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create(); + ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to build Jolt Physics separation ray shape with %s. It returned the following error: '%s'. This shape belongs to %s.", to_string(), to_godot(shape_result.GetError()), _owners_to_string())); + + return shape_result.Get(); +} + +Variant JoltSeparationRayShape3D::get_data() const { + Dictionary data; + data["length"] = length; + data["slide_on_slope"] = slide_on_slope; + return data; +} + +void JoltSeparationRayShape3D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY); + + const Dictionary data = p_data; + + const Variant maybe_length = data.get("length", Variant()); + ERR_FAIL_COND(maybe_length.get_type() != Variant::FLOAT); + + const Variant maybe_slide_on_slope = data.get("slide_on_slope", Variant()); + ERR_FAIL_COND(maybe_slide_on_slope.get_type() != Variant::BOOL); + + const float new_length = maybe_length; + const bool new_slide_on_slope = maybe_slide_on_slope; + + if (unlikely(new_length == length && new_slide_on_slope == slide_on_slope)) { + return; + } + + length = new_length; + slide_on_slope = new_slide_on_slope; + + destroy(); +} + +AABB JoltSeparationRayShape3D::get_aabb() const { + constexpr float size_xy = 0.1f; + constexpr float half_size_xy = size_xy / 2.0f; + return AABB(Vector3(-half_size_xy, -half_size_xy, 0.0f), Vector3(size_xy, size_xy, length)); +} + +String JoltSeparationRayShape3D::to_string() const { + return vformat("{length=%f slide_on_slope=%s}", length, slide_on_slope); +} diff --git a/modules/jolt_physics/shapes/jolt_separation_ray_shape_3d.h b/modules/jolt_physics/shapes/jolt_separation_ray_shape_3d.h new file mode 100644 index 000000000000..b858d84febaa --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_separation_ray_shape_3d.h @@ -0,0 +1,57 @@ +/**************************************************************************/ +/* jolt_separation_ray_shape_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_SEPARATION_RAY_SHAPE_3D_H +#define JOLT_SEPARATION_RAY_SHAPE_3D_H + +#include "jolt_shape_3d.h" + +class JoltSeparationRayShape3D final : public JoltShape3D { + float length = 0.0f; + bool slide_on_slope = false; + + virtual JPH::ShapeRefC _build() const override; + +public: + virtual ShapeType get_type() const override { return ShapeType::SHAPE_SEPARATION_RAY; } + virtual bool is_convex() const override { return true; } + + virtual Variant get_data() const override; + virtual void set_data(const Variant &p_data) override; + + virtual float get_margin() const override { return 0.0f; } + virtual void set_margin(float p_margin) override {} + + virtual AABB get_aabb() const override; + + String to_string() const; +}; + +#endif // JOLT_SEPARATION_RAY_SHAPE_3D_H diff --git a/modules/jolt_physics/shapes/jolt_shape_3d.cpp b/modules/jolt_physics/shapes/jolt_shape_3d.cpp new file mode 100644 index 000000000000..dca898f6e415 --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_shape_3d.cpp @@ -0,0 +1,280 @@ +/**************************************************************************/ +/* jolt_shape_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_shape_3d.h" + +#include "../misc/jolt_type_conversions.h" +#include "../objects/jolt_shaped_object_3d.h" +#include "jolt_custom_double_sided_shape.h" +#include "jolt_custom_user_data_shape.h" + +#include "core/error/error_macros.h" + +#include "Jolt/Physics/Collision/Shape/MutableCompoundShape.h" +#include "Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h" +#include "Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h" +#include "Jolt/Physics/Collision/Shape/ScaledShape.h" +#include "Jolt/Physics/Collision/Shape/SphereShape.h" +#include "Jolt/Physics/Collision/Shape/StaticCompoundShape.h" + +namespace { + +constexpr float DEFAULT_SOLVER_BIAS = 0.0; + +} // namespace + +String JoltShape3D::_owners_to_string() const { + const int owner_count = ref_counts_by_owner.size(); + + if (owner_count == 0) { + return "'' and 0 other object(s)"; + } + + const JoltShapedObject3D &random_owner = *ref_counts_by_owner.begin()->key; + + return vformat("'%s' and %d other object(s)", random_owner.to_string(), owner_count - 1); +} + +JoltShape3D::~JoltShape3D() = default; + +void JoltShape3D::add_owner(JoltShapedObject3D *p_owner) { + ref_counts_by_owner[p_owner]++; +} + +void JoltShape3D::remove_owner(JoltShapedObject3D *p_owner) { + if (--ref_counts_by_owner[p_owner] <= 0) { + ref_counts_by_owner.erase(p_owner); + } +} + +void JoltShape3D::remove_self() { + // `remove_owner` will be called when we `remove_shape`, so we need to copy the map since the + // iterator would be invalidated from underneath us. + const HashMap ref_counts_by_owner_copy = ref_counts_by_owner; + + for (const auto &[owner, ref_count] : ref_counts_by_owner_copy) { + owner->remove_shape(this); + } +} + +float JoltShape3D::get_solver_bias() const { + return DEFAULT_SOLVER_BIAS; +} + +void JoltShape3D::set_solver_bias(float p_bias) { + if (!Math::is_equal_approx(p_bias, DEFAULT_SOLVER_BIAS)) { + WARN_PRINT(vformat("Custom solver bias for shapes is not supported when using Jolt Physics. Any such value will be ignored. This shape belongs to %s.", _owners_to_string())); + } +} + +JPH::ShapeRefC JoltShape3D::try_build() { + jolt_ref_mutex.lock(); + + if (jolt_ref == nullptr) { + jolt_ref = _build(); + } + + jolt_ref_mutex.unlock(); + + return jolt_ref; +} + +void JoltShape3D::destroy() { + jolt_ref_mutex.lock(); + jolt_ref = nullptr; + jolt_ref_mutex.unlock(); + + for (const auto &[owner, ref_count] : ref_counts_by_owner) { + owner->_shapes_changed(); + } +} + +JPH::ShapeRefC JoltShape3D::with_scale(const JPH::Shape *p_shape, const Vector3 &p_scale) { + ERR_FAIL_NULL_V(p_shape, nullptr); + + const JPH::ScaledShapeSettings shape_settings(p_shape, to_jolt(p_scale)); + const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create(); + ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to scale shape with {scale=%v}. It returned the following error: '%s'.", p_scale, to_godot(shape_result.GetError()))); + + return shape_result.Get(); +} + +JPH::ShapeRefC JoltShape3D::with_basis_origin(const JPH::Shape *p_shape, const Basis &p_basis, const Vector3 &p_origin) { + ERR_FAIL_NULL_V(p_shape, nullptr); + + const JPH::RotatedTranslatedShapeSettings shape_settings(to_jolt(p_origin), to_jolt(p_basis), p_shape); + + const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create(); + ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to offset shape with {basis=%s origin=%v}. It returned the following error: '%s'.", p_basis, p_origin, to_godot(shape_result.GetError()))); + + return shape_result.Get(); +} + +JPH::ShapeRefC JoltShape3D::with_center_of_mass_offset(const JPH::Shape *p_shape, const Vector3 &p_offset) { + ERR_FAIL_NULL_V(p_shape, nullptr); + + const JPH::OffsetCenterOfMassShapeSettings shape_settings(to_jolt(p_offset), p_shape); + const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create(); + ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to offset center of mass with {offset=%v}. It returned the following error: '%s'.", p_offset, to_godot(shape_result.GetError()))); + + return shape_result.Get(); +} + +JPH::ShapeRefC JoltShape3D::with_center_of_mass(const JPH::Shape *p_shape, const Vector3 &p_center_of_mass) { + ERR_FAIL_NULL_V(p_shape, nullptr); + + const Vector3 center_of_mass_inner = to_godot(p_shape->GetCenterOfMass()); + const Vector3 center_of_mass_offset = p_center_of_mass - center_of_mass_inner; + + if (center_of_mass_offset == Vector3()) { + return p_shape; + } + + return with_center_of_mass_offset(p_shape, center_of_mass_offset); +} + +JPH::ShapeRefC JoltShape3D::with_user_data(const JPH::Shape *p_shape, uint64_t p_user_data) { + JoltCustomUserDataShapeSettings shape_settings(p_shape); + shape_settings.mUserData = (JPH::uint64)p_user_data; + + const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create(); + ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to override user data. It returned the following error: '%s'.", to_godot(shape_result.GetError()))); + + return shape_result.Get(); +} + +JPH::ShapeRefC JoltShape3D::with_double_sided(const JPH::Shape *p_shape, bool p_back_face_collision) { + ERR_FAIL_NULL_V(p_shape, nullptr); + + const JoltCustomDoubleSidedShapeSettings shape_settings(p_shape, p_back_face_collision); + const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create(); + ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to make shape double-sided. It returned the following error: '%s'.", to_godot(shape_result.GetError()))); + + return shape_result.Get(); +} + +JPH::ShapeRefC JoltShape3D::without_custom_shapes(const JPH::Shape *p_shape) { + switch (p_shape->GetSubType()) { + case JoltCustomShapeSubType::RAY: + case JoltCustomShapeSubType::MOTION: { + // Replace unsupported shapes with a small sphere. + return new JPH::SphereShape(0.1f); + } + + case JoltCustomShapeSubType::OVERRIDE_USER_DATA: + case JoltCustomShapeSubType::DOUBLE_SIDED: { + const JPH::DecoratedShape *shape = static_cast(p_shape); + + // Replace unsupported decorator shapes with the inner shape. + return without_custom_shapes(shape->GetInnerShape()); + } + + case JPH::EShapeSubType::StaticCompound: { + const JPH::StaticCompoundShape *shape = static_cast(p_shape); + + JPH::StaticCompoundShapeSettings settings; + + for (const JPH::CompoundShape::SubShape &sub_shape : shape->GetSubShapes()) { + settings.AddShape(shape->GetCenterOfMass() + sub_shape.GetPositionCOM() - sub_shape.GetRotation() * sub_shape.mShape->GetCenterOfMass(), sub_shape.GetRotation(), without_custom_shapes(sub_shape.mShape)); + } + + const JPH::ShapeSettings::ShapeResult shape_result = settings.Create(); + ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to recreate static compound shape during filtering of custom shapes. It returned the following error: '%s'.", to_godot(shape_result.GetError()))); + + return shape_result.Get(); + } + + case JPH::EShapeSubType::MutableCompound: { + const JPH::MutableCompoundShape *shape = static_cast(p_shape); + + JPH::MutableCompoundShapeSettings settings; + + for (const JPH::MutableCompoundShape::SubShape &sub_shape : shape->GetSubShapes()) { + settings.AddShape(shape->GetCenterOfMass() + sub_shape.GetPositionCOM() - sub_shape.GetRotation() * sub_shape.mShape->GetCenterOfMass(), sub_shape.GetRotation(), without_custom_shapes(sub_shape.mShape)); + } + + const JPH::ShapeSettings::ShapeResult shape_result = settings.Create(); + ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to recreate mutable compound shape during filtering of custom shapes. It returned the following error: '%s'.", to_godot(shape_result.GetError()))); + + return shape_result.Get(); + } + + case JPH::EShapeSubType::RotatedTranslated: { + const JPH::RotatedTranslatedShape *shape = static_cast(p_shape); + + const JPH::Shape *inner_shape = shape->GetInnerShape(); + const JPH::ShapeRefC new_inner_shape = without_custom_shapes(inner_shape); + + if (inner_shape == new_inner_shape) { + return p_shape; + } + + return new JPH::RotatedTranslatedShape(shape->GetPosition(), shape->GetRotation(), new_inner_shape); + } + + case JPH::EShapeSubType::Scaled: { + const JPH::ScaledShape *shape = static_cast(p_shape); + + const JPH::Shape *inner_shape = shape->GetInnerShape(); + const JPH::ShapeRefC new_inner_shape = without_custom_shapes(inner_shape); + + if (inner_shape == new_inner_shape) { + return p_shape; + } + + return new JPH::ScaledShape(new_inner_shape, shape->GetScale()); + } + + case JPH::EShapeSubType::OffsetCenterOfMass: { + const JPH::OffsetCenterOfMassShape *shape = static_cast(p_shape); + + const JPH::Shape *inner_shape = shape->GetInnerShape(); + const JPH::ShapeRefC new_inner_shape = without_custom_shapes(inner_shape); + + if (inner_shape == new_inner_shape) { + return p_shape; + } + + return new JPH::OffsetCenterOfMassShape(new_inner_shape, shape->GetOffset()); + } + + default: { + return p_shape; + } + } +} + +Vector3 JoltShape3D::make_scale_valid(const JPH::Shape *p_shape, const Vector3 &p_scale) { + return to_godot(p_shape->MakeScaleValid(to_jolt(p_scale))); +} + +bool JoltShape3D::is_scale_valid(const Vector3 &p_scale, const Vector3 &p_valid_scale, real_t p_tolerance) { + return Math::is_equal_approx(p_scale.x, p_valid_scale.x, p_tolerance) && Math::is_equal_approx(p_scale.y, p_valid_scale.y, p_tolerance) && Math::is_equal_approx(p_scale.z, p_valid_scale.z, p_tolerance); +} diff --git a/modules/jolt_physics/shapes/jolt_shape_3d.h b/modules/jolt_physics/shapes/jolt_shape_3d.h new file mode 100644 index 000000000000..3ababc3ad13b --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_shape_3d.h @@ -0,0 +1,141 @@ +/**************************************************************************/ +/* jolt_shape_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_SHAPE_3D_H +#define JOLT_SHAPE_3D_H + +#include "core/error/error_macros.h" +#include "core/math/aabb.h" +#include "core/os/mutex.h" +#include "core/templates/hash_map.h" +#include "core/variant/variant.h" +#include "servers/physics_server_3d.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/Collision/Shape/Shape.h" + +class JoltShapedObject3D; + +class JoltShape3D { +protected: + HashMap ref_counts_by_owner; + Mutex jolt_ref_mutex; + RID rid; + JPH::ShapeRefC jolt_ref; + + virtual JPH::ShapeRefC _build() const = 0; + + String _owners_to_string() const; + +public: + typedef PhysicsServer3D::ShapeType ShapeType; + + virtual ~JoltShape3D() = 0; + + RID get_rid() const { return rid; } + void set_rid(const RID &p_rid) { rid = p_rid; } + + void add_owner(JoltShapedObject3D *p_owner); + void remove_owner(JoltShapedObject3D *p_owner); + void remove_self(); + + virtual ShapeType get_type() const = 0; + virtual bool is_convex() const = 0; + + virtual Variant get_data() const = 0; + virtual void set_data(const Variant &p_data) = 0; + + virtual float get_margin() const = 0; + virtual void set_margin(float p_margin) = 0; + + virtual AABB get_aabb() const = 0; + + float get_solver_bias() const; + void set_solver_bias(float p_bias); + + JPH::ShapeRefC try_build(); + + void destroy(); + + const JPH::Shape *get_jolt_ref() const { return jolt_ref; } + + static JPH::ShapeRefC with_scale(const JPH::Shape *p_shape, const Vector3 &p_scale); + static JPH::ShapeRefC with_basis_origin(const JPH::Shape *p_shape, const Basis &p_basis, const Vector3 &p_origin); + static JPH::ShapeRefC with_center_of_mass_offset(const JPH::Shape *p_shape, const Vector3 &p_offset); + static JPH::ShapeRefC with_center_of_mass(const JPH::Shape *p_shape, const Vector3 &p_center_of_mass); + static JPH::ShapeRefC with_user_data(const JPH::Shape *p_shape, uint64_t p_user_data); + static JPH::ShapeRefC with_double_sided(const JPH::Shape *p_shape, bool p_back_face_collision); + static JPH::ShapeRefC without_custom_shapes(const JPH::Shape *p_shape); + + static Vector3 make_scale_valid(const JPH::Shape *p_shape, const Vector3 &p_scale); + static bool is_scale_valid(const Vector3 &p_scale, const Vector3 &p_valid_scale, real_t p_tolerance = 0.01f); +}; + +#ifdef DEBUG_ENABLED + +#define JOLT_ENSURE_SCALE_NOT_ZERO(m_transform, m_msg) \ + if (unlikely((m_transform).basis.determinant() == 0.0f)) { \ + WARN_PRINT(vformat("%s " \ + "The basis of the transform was singular, which is not supported by Jolt Physics. " \ + "This is likely caused by one or more axes having a scale of zero. " \ + "The basis (and thus its scale) will be treated as identity.", \ + m_msg)); \ + \ + (m_transform).basis = Basis(); \ + } else \ + ((void)0) + +#define ERR_PRINT_INVALID_SCALE_MSG(m_scale, m_valid_scale, m_msg) \ + if (unlikely(!JoltShape3D::is_scale_valid(m_scale, valid_scale))) { \ + ERR_PRINT(vformat("%s " \ + "A scale of %v is not supported by Jolt Physics for this shape/body. " \ + "The scale will instead be treated as %v.", \ + m_msg, m_scale, valid_scale)); \ + } else \ + ((void)0) + +#else + +#define JOLT_ENSURE_SCALE_NOT_ZERO(m_transform, m_msg) + +#define ERR_PRINT_INVALID_SCALE_MSG(m_scale, m_valid_scale, m_msg) + +#endif + +#define JOLT_ENSURE_SCALE_VALID(m_shape, m_scale, m_msg) \ + if (true) { \ + const Vector3 valid_scale = JoltShape3D::make_scale_valid(m_shape, m_scale); \ + ERR_PRINT_INVALID_SCALE_MSG(m_scale, valid_scale, m_msg); \ + (m_scale) = valid_scale; \ + } else \ + ((void)0) + +#endif // JOLT_SHAPE_3D_H diff --git a/modules/jolt_physics/shapes/jolt_shape_instance_3d.cpp b/modules/jolt_physics/shapes/jolt_shape_instance_3d.cpp new file mode 100644 index 000000000000..e21b5e595bd0 --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_shape_instance_3d.cpp @@ -0,0 +1,108 @@ +/**************************************************************************/ +/* jolt_shape_instance_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_shape_instance_3d.h" + +#include "jolt_shape_3d.h" + +#include "core/error/error_macros.h" + +#include "Jolt/Physics/Collision/Shape/DecoratedShape.h" + +JoltShapeInstance3D::ShapeReference::ShapeReference(JoltShapedObject3D *p_parent, JoltShape3D *p_shape) : + parent(p_parent), + shape(p_shape) { + if (shape != nullptr) { + shape->add_owner(parent); + } +} + +JoltShapeInstance3D::ShapeReference::ShapeReference(const ShapeReference &p_other) : + parent(p_other.parent), + shape(p_other.shape) { + if (shape != nullptr) { + shape->add_owner(parent); + } +} + +JoltShapeInstance3D::ShapeReference::~ShapeReference() { + if (shape != nullptr) { + shape->remove_owner(parent); + } +} + +JoltShapeInstance3D::ShapeReference &JoltShapeInstance3D::ShapeReference::operator=(const ShapeReference &p_other) { + if (shape != nullptr) { + shape->remove_owner(parent); + } + + parent = p_other.parent; + shape = p_other.shape; + + if (shape != nullptr) { + shape->add_owner(parent); + } + + return *this; +} + +JoltShapeInstance3D::JoltShapeInstance3D(JoltShapedObject3D *p_parent, JoltShape3D *p_shape, const Transform3D &p_transform, const Vector3 &p_scale, bool p_disabled) : + transform(p_transform), + scale(p_scale), + shape(p_parent, p_shape), + disabled(p_disabled) { +} + +AABB JoltShapeInstance3D::get_aabb() const { + return get_transform_scaled().xform(shape->get_aabb()); +} + +bool JoltShapeInstance3D::try_build() { + ERR_FAIL_COND_V(is_disabled(), false); + + const JPH::ShapeRefC maybe_new_shape = shape->try_build(); + + if (maybe_new_shape == nullptr) { + jolt_ref = nullptr; + return false; + } + + if (jolt_ref != nullptr) { + const JPH::DecoratedShape *outer_shape = static_cast(jolt_ref.GetPtr()); + + if (outer_shape->GetInnerShape() == maybe_new_shape) { + return true; + } + } + + jolt_ref = JoltShape3D::with_user_data(maybe_new_shape, (uint64_t)id); + + return true; +} diff --git a/modules/jolt_physics/shapes/jolt_shape_instance_3d.h b/modules/jolt_physics/shapes/jolt_shape_instance_3d.h new file mode 100644 index 000000000000..0a24d06a904d --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_shape_instance_3d.h @@ -0,0 +1,103 @@ +/**************************************************************************/ +/* jolt_shape_instance_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_SHAPE_INSTANCE_3D_H +#define JOLT_SHAPE_INSTANCE_3D_H + +#include "core/math/transform_3d.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/Collision/Shape/Shape.h" + +class JoltShapedObject3D; +class JoltShape3D; + +class JoltShapeInstance3D { + // This RAII helper exists solely to avoid needing to maintain copy construction/assignment in the shape instance. + // Ideally this would be move-only instead, but Godot's containers don't support that at the moment. + struct ShapeReference { + JoltShapedObject3D *parent = nullptr; + JoltShape3D *shape = nullptr; + + ShapeReference() = default; + ShapeReference(JoltShapedObject3D *p_parent, JoltShape3D *p_shape); + ShapeReference(const ShapeReference &p_other); + ShapeReference(ShapeReference &&p_other) = delete; + ~ShapeReference(); + + ShapeReference &operator=(const ShapeReference &p_other); + ShapeReference &operator=(ShapeReference &&p_other) = delete; + + JoltShape3D *operator*() const { return shape; } + JoltShape3D *operator->() const { return shape; } + operator JoltShape3D *() const { return shape; } + }; + + inline static uint32_t next_id = 1; + + Transform3D transform; + Vector3 scale; + ShapeReference shape; + JPH::ShapeRefC jolt_ref; + uint32_t id = next_id++; + bool disabled = false; + +public: + JoltShapeInstance3D() = default; + JoltShapeInstance3D(JoltShapedObject3D *p_parent, JoltShape3D *p_shape, const Transform3D &p_transform = Transform3D(), const Vector3 &p_scale = Vector3(1.0f, 1.0f, 1.0f), bool p_disabled = false); + + uint32_t get_id() const { return id; } + + JoltShape3D *get_shape() const { return shape; } + + const JPH::Shape *get_jolt_ref() const { return jolt_ref; } + + const Transform3D &get_transform_unscaled() const { return transform; } + Transform3D get_transform_scaled() const { return transform.scaled_local(scale); } + void set_transform(const Transform3D &p_transform) { transform = p_transform; } + + const Vector3 &get_scale() const { return scale; } + void set_scale(const Vector3 &p_scale) { scale = p_scale; } + + AABB get_aabb() const; + + bool is_built() const { return jolt_ref != nullptr; } + + bool is_enabled() const { return !disabled; } + bool is_disabled() const { return disabled; } + + void enable() { disabled = false; } + void disable() { disabled = true; } + + bool try_build(); +}; + +#endif // JOLT_SHAPE_INSTANCE_3D_H diff --git a/modules/jolt_physics/shapes/jolt_sphere_shape_3d.cpp b/modules/jolt_physics/shapes/jolt_sphere_shape_3d.cpp new file mode 100644 index 000000000000..7157deb72d88 --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_sphere_shape_3d.cpp @@ -0,0 +1,73 @@ +/**************************************************************************/ +/* jolt_sphere_shape_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_sphere_shape_3d.h" + +#include "../misc/jolt_type_conversions.h" + +#include "core/error/error_macros.h" + +#include "Jolt/Physics/Collision/Shape/SphereShape.h" + +JPH::ShapeRefC JoltSphereShape3D::_build() const { + ERR_FAIL_COND_V_MSG(radius <= 0.0f, nullptr, vformat("Failed to build Jolt Physics sphere shape with %s. Its radius must be greater than 0. This shape belongs to %s.", to_string(), _owners_to_string())); + + const JPH::SphereShapeSettings shape_settings(radius); + const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create(); + ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to build Jolt Physics sphere shape with %s. It returned the following error: '%s'. This shape belongs to %s.", to_string(), to_godot(shape_result.GetError()), _owners_to_string())); + + return shape_result.Get(); +} + +Variant JoltSphereShape3D::get_data() const { + return radius; +} + +void JoltSphereShape3D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::FLOAT); + + const float new_radius = p_data; + if (unlikely(new_radius == radius)) { + return; + } + + radius = new_radius; + + destroy(); +} + +AABB JoltSphereShape3D::get_aabb() const { + const Vector3 half_extents(radius, radius, radius); + return AABB(-half_extents, half_extents * 2.0f); +} + +String JoltSphereShape3D::to_string() const { + return vformat("{radius=%f}", radius); +} diff --git a/modules/jolt_physics/shapes/jolt_sphere_shape_3d.h b/modules/jolt_physics/shapes/jolt_sphere_shape_3d.h new file mode 100644 index 000000000000..4127f6a5f989 --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_sphere_shape_3d.h @@ -0,0 +1,56 @@ +/**************************************************************************/ +/* jolt_sphere_shape_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_SPHERE_SHAPE_3D_H +#define JOLT_SPHERE_SHAPE_3D_H + +#include "jolt_shape_3d.h" + +class JoltSphereShape3D final : public JoltShape3D { + float radius = 0.0f; + + virtual JPH::ShapeRefC _build() const override; + +public: + virtual ShapeType get_type() const override { return ShapeType::SHAPE_SPHERE; } + virtual bool is_convex() const override { return true; } + + virtual Variant get_data() const override; + virtual void set_data(const Variant &p_data) override; + + virtual float get_margin() const override { return 0.0f; } + virtual void set_margin(float p_margin) override {} + + virtual AABB get_aabb() const override; + + String to_string() const; +}; + +#endif // JOLT_SPHERE_SHAPE_3D_H diff --git a/modules/jolt_physics/shapes/jolt_world_boundary_shape_3d.cpp b/modules/jolt_physics/shapes/jolt_world_boundary_shape_3d.cpp new file mode 100644 index 000000000000..f930056f6ac3 --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_world_boundary_shape_3d.cpp @@ -0,0 +1,77 @@ +/**************************************************************************/ +/* jolt_world_boundary_shape_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_world_boundary_shape_3d.h" + +#include "../jolt_project_settings.h" +#include "../misc/jolt_type_conversions.h" + +#include "core/error/error_macros.h" + +#include "Jolt/Physics/Collision/Shape/PlaneShape.h" + +JPH::ShapeRefC JoltWorldBoundaryShape3D::_build() const { + const Plane normalized_plane = plane.normalized(); + ERR_FAIL_COND_V_MSG(normalized_plane == Plane(), nullptr, vformat("Failed to build Jolt Physics world boundary shape with %s. The plane's normal must not be zero. This shape belongs to %s.", to_string(), _owners_to_string())); + + const float half_size = JoltProjectSettings::get_world_boundary_shape_size() / 2.0f; + const JPH::PlaneShapeSettings shape_settings(to_jolt(normalized_plane), nullptr, half_size); + const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create(); + ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to build Jolt Physics world boundary shape with %s. It returned the following error: '%s'. This shape belongs to %s.", to_string(), to_godot(shape_result.GetError()), _owners_to_string())); + + return shape_result.Get(); +} + +Variant JoltWorldBoundaryShape3D::get_data() const { + return plane; +} + +void JoltWorldBoundaryShape3D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::PLANE); + + const Plane new_plane = p_data; + if (unlikely(new_plane == plane)) { + return; + } + + plane = p_data; + + destroy(); +} + +AABB JoltWorldBoundaryShape3D::get_aabb() const { + const float size = JoltProjectSettings::get_world_boundary_shape_size(); + const float half_size = size / 2.0f; + return AABB(Vector3(-half_size, -half_size, -half_size), Vector3(size, half_size, size)); +} + +String JoltWorldBoundaryShape3D::to_string() const { + return vformat("{plane=%s}", plane); +} diff --git a/modules/jolt_physics/shapes/jolt_world_boundary_shape_3d.h b/modules/jolt_physics/shapes/jolt_world_boundary_shape_3d.h new file mode 100644 index 000000000000..2d22c0f5f594 --- /dev/null +++ b/modules/jolt_physics/shapes/jolt_world_boundary_shape_3d.h @@ -0,0 +1,56 @@ +/**************************************************************************/ +/* jolt_world_boundary_shape_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_WORLD_BOUNDARY_SHAPE_3D_H +#define JOLT_WORLD_BOUNDARY_SHAPE_3D_H + +#include "jolt_shape_3d.h" + +class JoltWorldBoundaryShape3D final : public JoltShape3D { + Plane plane; + + virtual JPH::ShapeRefC _build() const override; + +public: + virtual ShapeType get_type() const override { return ShapeType::SHAPE_WORLD_BOUNDARY; } + virtual bool is_convex() const override { return false; } + + virtual Variant get_data() const override; + virtual void set_data(const Variant &p_data) override; + + virtual float get_margin() const override { return 0.0f; } + virtual void set_margin(float p_margin) override {} + + virtual AABB get_aabb() const override; + + String to_string() const; +}; + +#endif // JOLT_WORLD_BOUNDARY_SHAPE_3D_H diff --git a/modules/jolt_physics/spaces/jolt_body_accessor_3d.cpp b/modules/jolt_physics/spaces/jolt_body_accessor_3d.cpp new file mode 100644 index 000000000000..e2128a1f205e --- /dev/null +++ b/modules/jolt_physics/spaces/jolt_body_accessor_3d.cpp @@ -0,0 +1,198 @@ +/**************************************************************************/ +/* jolt_body_accessor_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_body_accessor_3d.h" + +#include "jolt_space_3d.h" + +#include "core/error/error_macros.h" + +namespace { + +template +struct VariantVisitors : TTypes... { + using TTypes::operator()...; +}; + +template +VariantVisitors(TTypes...) -> VariantVisitors; + +} // namespace + +JoltBodyAccessor3D::JoltBodyAccessor3D(const JoltSpace3D *p_space) : + space(p_space) { +} + +JoltBodyAccessor3D::~JoltBodyAccessor3D() = default; + +void JoltBodyAccessor3D::acquire(const JPH::BodyID *p_ids, int p_id_count) { + ERR_FAIL_NULL(space); + + lock_iface = &space->get_lock_iface(); + ids = BodyIDSpan(p_ids, p_id_count); + _acquire_internal(p_ids, p_id_count); +} + +void JoltBodyAccessor3D::acquire(const JPH::BodyID &p_id) { + ERR_FAIL_NULL(space); + + lock_iface = &space->get_lock_iface(); + ids = p_id; + _acquire_internal(&p_id, 1); +} + +void JoltBodyAccessor3D::acquire_active() { + const JPH::PhysicsSystem &physics_system = space->get_physics_system(); + + acquire(physics_system.GetActiveBodiesUnsafe(JPH::EBodyType::RigidBody), (int)physics_system.GetNumActiveBodies(JPH::EBodyType::RigidBody)); +} + +void JoltBodyAccessor3D::acquire_all() { + ERR_FAIL_NULL(space); + + lock_iface = &space->get_lock_iface(); + + JPH::BodyIDVector *vector = std::get_if(&ids); + + if (vector == nullptr) { + ids = JPH::BodyIDVector(); + vector = std::get_if(&ids); + } + + space->get_physics_system().GetBodies(*vector); + + _acquire_internal(vector->data(), (int)vector->size()); +} + +void JoltBodyAccessor3D::release() { + _release_internal(); + lock_iface = nullptr; +} + +const JPH::BodyID *JoltBodyAccessor3D::get_ids() const { + ERR_FAIL_COND_V(not_acquired(), nullptr); + + return std::visit( + VariantVisitors{ + [](const JPH::BodyID &p_id) { return &p_id; }, + [](const JPH::BodyIDVector &p_vector) { return p_vector.data(); }, + [](const BodyIDSpan &p_span) { return p_span.ptr; } }, + ids); +} + +int JoltBodyAccessor3D::get_count() const { + ERR_FAIL_COND_V(not_acquired(), 0); + + return std::visit( + VariantVisitors{ + [](const JPH::BodyID &p_id) { return 1; }, + [](const JPH::BodyIDVector &p_vector) { return (int)p_vector.size(); }, + [](const BodyIDSpan &p_span) { return p_span.count; } }, + ids); +} + +const JPH::BodyID &JoltBodyAccessor3D::get_at(int p_index) const { + CRASH_BAD_INDEX(p_index, get_count()); + return get_ids()[p_index]; +} + +void JoltBodyReader3D::_acquire_internal(const JPH::BodyID *p_ids, int p_id_count) { + mutex_mask = lock_iface->GetMutexMask(p_ids, p_id_count); + lock_iface->LockRead(mutex_mask); +} + +void JoltBodyReader3D::_release_internal() { + ERR_FAIL_COND(not_acquired()); + lock_iface->UnlockRead(mutex_mask); +} + +JoltBodyReader3D::JoltBodyReader3D(const JoltSpace3D *p_space) : + JoltBodyAccessor3D(p_space) { +} + +const JPH::Body *JoltBodyReader3D::try_get(const JPH::BodyID &p_id) const { + if (unlikely(p_id.IsInvalid())) { + return nullptr; + } + + ERR_FAIL_COND_V(not_acquired(), nullptr); + + return lock_iface->TryGetBody(p_id); +} + +const JPH::Body *JoltBodyReader3D::try_get(int p_index) const { + const int count = get_count(); + if (unlikely(p_index < 0 || p_index >= count)) { + return nullptr; + } + + return try_get(get_at(p_index)); +} + +const JPH::Body *JoltBodyReader3D::try_get() const { + return try_get(0); +} + +void JoltBodyWriter3D::_acquire_internal(const JPH::BodyID *p_ids, int p_id_count) { + mutex_mask = lock_iface->GetMutexMask(p_ids, p_id_count); + lock_iface->LockWrite(mutex_mask); +} + +void JoltBodyWriter3D::_release_internal() { + ERR_FAIL_COND(not_acquired()); + lock_iface->UnlockWrite(mutex_mask); +} + +JoltBodyWriter3D::JoltBodyWriter3D(const JoltSpace3D *p_space) : + JoltBodyAccessor3D(p_space) { +} + +JPH::Body *JoltBodyWriter3D::try_get(const JPH::BodyID &p_id) const { + if (unlikely(p_id.IsInvalid())) { + return nullptr; + } + + ERR_FAIL_COND_V(not_acquired(), nullptr); + + return lock_iface->TryGetBody(p_id); +} + +JPH::Body *JoltBodyWriter3D::try_get(int p_index) const { + const int count = get_count(); + if (unlikely(p_index < 0 || p_index >= count)) { + return nullptr; + } + + return try_get(get_at(p_index)); +} + +JPH::Body *JoltBodyWriter3D::try_get() const { + return try_get(0); +} diff --git a/modules/jolt_physics/spaces/jolt_body_accessor_3d.h b/modules/jolt_physics/spaces/jolt_body_accessor_3d.h new file mode 100644 index 000000000000..0d2681468368 --- /dev/null +++ b/modules/jolt_physics/spaces/jolt_body_accessor_3d.h @@ -0,0 +1,215 @@ +/**************************************************************************/ +/* jolt_body_accessor_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_BODY_ACCESSOR_3D_H +#define JOLT_BODY_ACCESSOR_3D_H + +#include "../objects/jolt_object_3d.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/Body/BodyLockInterface.h" + +#include + +class JoltArea3D; +class JoltBody3D; +class JoltShapedObject3D; +class JoltSpace3D; + +class JoltBodyAccessor3D { +protected: + struct BodyIDSpan { + BodyIDSpan(const JPH::BodyID *p_ptr, int p_count) : + ptr(p_ptr), count(p_count) {} + + const JPH::BodyID *ptr; + int count; + }; + + virtual void _acquire_internal(const JPH::BodyID *p_ids, int p_id_count) = 0; + virtual void _release_internal() = 0; + + const JoltSpace3D *space = nullptr; + + const JPH::BodyLockInterface *lock_iface = nullptr; + + std::variant ids; + +public: + explicit JoltBodyAccessor3D(const JoltSpace3D *p_space); + virtual ~JoltBodyAccessor3D() = 0; + + void acquire(const JPH::BodyID *p_ids, int p_id_count); + void acquire(const JPH::BodyID &p_id); + void acquire_active(); + void acquire_all(); + void release(); + + bool is_acquired() const { return lock_iface != nullptr; } + bool not_acquired() const { return lock_iface == nullptr; } + + const JoltSpace3D &get_space() const { return *space; } + const JPH::BodyID *get_ids() const; + int get_count() const; + + const JPH::BodyID &get_at(int p_index) const; +}; + +class JoltBodyReader3D final : public JoltBodyAccessor3D { + virtual void _acquire_internal(const JPH::BodyID *p_ids, int p_id_count) override; + virtual void _release_internal() override; + + JPH::BodyLockInterface::MutexMask mutex_mask = 0; + +public: + explicit JoltBodyReader3D(const JoltSpace3D *p_space); + + const JPH::Body *try_get(const JPH::BodyID &p_id) const; + const JPH::Body *try_get(int p_index) const; + const JPH::Body *try_get() const; +}; + +class JoltBodyWriter3D final : public JoltBodyAccessor3D { + virtual void _acquire_internal(const JPH::BodyID *p_ids, int p_id_count) override; + virtual void _release_internal() override; + + JPH::BodyLockInterface::MutexMask mutex_mask = 0; + +public: + explicit JoltBodyWriter3D(const JoltSpace3D *p_space); + + JPH::Body *try_get(const JPH::BodyID &p_id) const; + JPH::Body *try_get(int p_index) const; + JPH::Body *try_get() const; +}; + +template +class JoltScopedBodyAccessor3D { + TBodyAccessor inner; + +public: + JoltScopedBodyAccessor3D(const JoltSpace3D &p_space, const JPH::BodyID *p_ids, int p_id_count) : + inner(&p_space) { inner.acquire(p_ids, p_id_count); } + + JoltScopedBodyAccessor3D(const JoltSpace3D &p_space, const JPH::BodyID &p_id) : + inner(&p_space) { inner.acquire(p_id); } + + JoltScopedBodyAccessor3D(const JoltScopedBodyAccessor3D &p_other) = delete; + JoltScopedBodyAccessor3D(JoltScopedBodyAccessor3D &&p_other) = default; + ~JoltScopedBodyAccessor3D() { inner.release(); } + + const JoltSpace3D &get_space() const { return inner.get_space(); } + int get_count() const { return inner.get_count(); } + const JPH::BodyID &get_at(int p_index) const { return inner.get_at(p_index); } + + JoltScopedBodyAccessor3D &operator=(const JoltScopedBodyAccessor3D &p_other) = delete; + JoltScopedBodyAccessor3D &operator=(JoltScopedBodyAccessor3D &&p_other) = default; + + decltype(auto) try_get(const JPH::BodyID &p_id) const { return inner.try_get(p_id); } + decltype(auto) try_get(int p_index) const { return inner.try_get(p_index); } + decltype(auto) try_get() const { return inner.try_get(); } +}; + +template +class JoltAccessibleBody3D { + TAccessor accessor; + TBody *body = nullptr; + +public: + JoltAccessibleBody3D(const JoltSpace3D &p_space, const JPH::BodyID &p_id) : + accessor(p_space, p_id), body(accessor.try_get()) {} + + bool is_valid() const { return body != nullptr; } + bool is_invalid() const { return body == nullptr; } + + JoltObject3D *as_object() const { + if (body != nullptr) { + return reinterpret_cast(body->GetUserData()); + } else { + return nullptr; + } + } + + JoltShapedObject3D *as_shaped() const { + if (JoltObject3D *object = as_object(); object != nullptr && object->is_shaped()) { + return reinterpret_cast(body->GetUserData()); + } else { + return nullptr; + } + } + + JoltBody3D *as_body() const { + if (JoltObject3D *object = as_object(); object != nullptr && object->is_body()) { + return reinterpret_cast(body->GetUserData()); + } else { + return nullptr; + } + } + + JoltArea3D *as_area() const { + if (JoltObject3D *object = as_object(); object != nullptr && object->is_area()) { + return reinterpret_cast(body->GetUserData()); + } else { + return nullptr; + } + } + + TBody *operator->() const { return body; } + TBody &operator*() const { return *body; } + + explicit operator TBody *() const { return body; } +}; + +template +class JoltAccessibleBodies3D { + TAccessor accessor; + +public: + JoltAccessibleBodies3D(const JoltSpace3D &p_space, const JPH::BodyID *p_ids, int p_id_count) : + accessor(p_space, p_ids, p_id_count) {} + + JoltAccessibleBody3D operator[](int p_index) const { + const JPH::BodyID &body_id = p_index < accessor.get_count() ? accessor.get_at(p_index) : JPH::BodyID(); + + return { accessor.get_space(), body_id }; + } +}; + +typedef JoltScopedBodyAccessor3D JoltScopedBodyReader3D; +typedef JoltScopedBodyAccessor3D JoltScopedBodyWriter3D; + +typedef JoltAccessibleBody3D JoltReadableBody3D; +typedef JoltAccessibleBody3D JoltWritableBody3D; + +typedef JoltAccessibleBodies3D JoltReadableBodies3D; +typedef JoltAccessibleBodies3D JoltWritableBodies3D; + +#endif // JOLT_BODY_ACCESSOR_3D_H diff --git a/modules/jolt_physics/spaces/jolt_broad_phase_layer.h b/modules/jolt_physics/spaces/jolt_broad_phase_layer.h new file mode 100644 index 000000000000..8cc115e1d42a --- /dev/null +++ b/modules/jolt_physics/spaces/jolt_broad_phase_layer.h @@ -0,0 +1,52 @@ +/**************************************************************************/ +/* jolt_broad_phase_layer.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_BROAD_PHASE_LAYER_H +#define JOLT_BROAD_PHASE_LAYER_H + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h" + +#include + +namespace JoltBroadPhaseLayer { + +constexpr JPH::BroadPhaseLayer BODY_STATIC(0); +constexpr JPH::BroadPhaseLayer BODY_STATIC_BIG(1); +constexpr JPH::BroadPhaseLayer BODY_DYNAMIC(2); +constexpr JPH::BroadPhaseLayer AREA_DETECTABLE(3); +constexpr JPH::BroadPhaseLayer AREA_UNDETECTABLE(4); + +constexpr uint32_t COUNT = 5; + +} // namespace JoltBroadPhaseLayer + +#endif // JOLT_BROAD_PHASE_LAYER_H diff --git a/modules/jolt_physics/spaces/jolt_contact_listener_3d.cpp b/modules/jolt_physics/spaces/jolt_contact_listener_3d.cpp new file mode 100644 index 000000000000..2442a8f7af39 --- /dev/null +++ b/modules/jolt_physics/spaces/jolt_contact_listener_3d.cpp @@ -0,0 +1,547 @@ +/**************************************************************************/ +/* jolt_contact_listener_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_contact_listener_3d.h" + +#include "../jolt_project_settings.h" +#include "../misc/jolt_type_conversions.h" +#include "../objects/jolt_area_3d.h" +#include "../objects/jolt_body_3d.h" +#include "../objects/jolt_soft_body_3d.h" +#include "jolt_space_3d.h" + +#include "core/error/error_macros.h" + +#include "Jolt/Physics/Collision/EstimateCollisionResponse.h" +#include "Jolt/Physics/SoftBody/SoftBodyManifold.h" + +void JoltContactListener3D::OnContactAdded(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings) { + _try_override_collision_response(p_body1, p_body2, p_settings); + _try_apply_surface_velocities(p_body1, p_body2, p_settings); + _try_add_contacts(p_body1, p_body2, p_manifold, p_settings); + _try_evaluate_area_overlap(p_body1, p_body2, p_manifold); + +#ifdef DEBUG_ENABLED + _try_add_debug_contacts(p_body1, p_body2, p_manifold); +#endif +} + +void JoltContactListener3D::OnContactPersisted(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings) { + _try_override_collision_response(p_body1, p_body2, p_settings); + _try_apply_surface_velocities(p_body1, p_body2, p_settings); + _try_add_contacts(p_body1, p_body2, p_manifold, p_settings); + _try_evaluate_area_overlap(p_body1, p_body2, p_manifold); + +#ifdef DEBUG_ENABLED + _try_add_debug_contacts(p_body1, p_body2, p_manifold); +#endif +} + +void JoltContactListener3D::OnContactRemoved(const JPH::SubShapeIDPair &p_shape_pair) { + if (_try_remove_contacts(p_shape_pair)) { + return; + } + + if (_try_remove_area_overlap(p_shape_pair)) { + return; + } +} + +JPH::SoftBodyValidateResult JoltContactListener3D::OnSoftBodyContactValidate(const JPH::Body &p_soft_body, const JPH::Body &p_other_body, JPH::SoftBodyContactSettings &p_settings) { + _try_override_collision_response(p_soft_body, p_other_body, p_settings); + + return JPH::SoftBodyValidateResult::AcceptContact; +} + +#ifdef DEBUG_ENABLED + +void JoltContactListener3D::OnSoftBodyContactAdded(const JPH::Body &p_soft_body, const JPH::SoftBodyManifold &p_manifold) { + _try_add_debug_contacts(p_soft_body, p_manifold); +} + +#endif + +bool JoltContactListener3D::_is_listening_for(const JPH::Body &p_body) const { + return listening_for.has(p_body.GetID()); +} + +bool JoltContactListener3D::_try_override_collision_response(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, JPH::ContactSettings &p_settings) { + if (p_jolt_body1.IsSensor() || p_jolt_body2.IsSensor()) { + return false; + } + + if (!p_jolt_body1.IsDynamic() && !p_jolt_body2.IsDynamic()) { + return false; + } + + const JoltBody3D *body1 = reinterpret_cast(p_jolt_body1.GetUserData()); + const JoltBody3D *body2 = reinterpret_cast(p_jolt_body2.GetUserData()); + + const bool can_collide1 = body1->can_collide_with(*body2); + const bool can_collide2 = body2->can_collide_with(*body1); + + if (can_collide1 && !can_collide2) { + p_settings.mInvMassScale2 = 0.0f; + p_settings.mInvInertiaScale2 = 0.0f; + } else if (can_collide2 && !can_collide1) { + p_settings.mInvMassScale1 = 0.0f; + p_settings.mInvInertiaScale1 = 0.0f; + } + + return true; +} + +bool JoltContactListener3D::_try_override_collision_response(const JPH::Body &p_jolt_soft_body, const JPH::Body &p_jolt_other_body, JPH::SoftBodyContactSettings &p_settings) { + if (p_jolt_other_body.IsSensor()) { + return false; + } + + const JoltSoftBody3D *soft_body = reinterpret_cast(p_jolt_soft_body.GetUserData()); + const JoltBody3D *other_body = reinterpret_cast(p_jolt_other_body.GetUserData()); + + const bool can_collide1 = soft_body->can_collide_with(*other_body); + const bool can_collide2 = other_body->can_collide_with(*soft_body); + + if (can_collide1 && !can_collide2) { + p_settings.mInvMassScale2 = 0.0f; + p_settings.mInvInertiaScale2 = 0.0f; + } else if (can_collide2 && !can_collide1) { + p_settings.mInvMassScale1 = 0.0f; + } + + return true; +} + +bool JoltContactListener3D::_try_apply_surface_velocities(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, JPH::ContactSettings &p_settings) { + if (p_jolt_body1.IsSensor() || p_jolt_body2.IsSensor()) { + return false; + } + + const bool supports_surface_velocity1 = !p_jolt_body1.IsDynamic(); + const bool supports_surface_velocity2 = !p_jolt_body2.IsDynamic(); + + if (supports_surface_velocity1 == supports_surface_velocity2) { + return false; + } + + const JoltBody3D *body1 = reinterpret_cast(p_jolt_body1.GetUserData()); + const JoltBody3D *body2 = reinterpret_cast(p_jolt_body2.GetUserData()); + + const bool has_surface_velocity1 = supports_surface_velocity1 && (body1->get_linear_surface_velocity() != Vector3() || body1->get_angular_surface_velocity() != Vector3()); + const bool has_surface_velocity2 = supports_surface_velocity2 && (body2->get_linear_surface_velocity() != Vector3() || body2->get_angular_surface_velocity() != Vector3()); + + if (has_surface_velocity1 == has_surface_velocity2) { + return false; + } + + const JPH::Vec3 linear_velocity1 = to_jolt(body1->get_linear_surface_velocity()); + const JPH::Vec3 angular_velocity1 = to_jolt(body1->get_angular_surface_velocity()); + + const JPH::Vec3 linear_velocity2 = to_jolt(body2->get_linear_surface_velocity()); + const JPH::Vec3 angular_velocity2 = to_jolt(body2->get_angular_surface_velocity()); + + const JPH::RVec3 com1 = p_jolt_body1.GetCenterOfMassPosition(); + const JPH::RVec3 com2 = p_jolt_body2.GetCenterOfMassPosition(); + const JPH::Vec3 rel_com2 = JPH::Vec3(com2 - com1); + + const JPH::Vec3 angular_linear_velocity2 = rel_com2.Cross(angular_velocity2); + const JPH::Vec3 total_linear_velocity2 = linear_velocity2 + angular_linear_velocity2; + + p_settings.mRelativeLinearSurfaceVelocity = total_linear_velocity2 - linear_velocity1; + p_settings.mRelativeAngularSurfaceVelocity = angular_velocity2 - angular_velocity1; + + return true; +} + +bool JoltContactListener3D::_try_add_contacts(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings) { + if (p_body1.IsSensor() || p_body2.IsSensor()) { + return false; + } + + if (!_is_listening_for(p_body1) && !_is_listening_for(p_body2)) { + return false; + } + + const JPH::SubShapeIDPair shape_pair(p_body1.GetID(), p_manifold.mSubShapeID1, p_body2.GetID(), p_manifold.mSubShapeID2); + + Manifold &manifold = [&]() -> Manifold & { + const MutexLock write_lock(write_mutex); + return manifolds_by_shape_pair[shape_pair]; + }(); + + const JPH::uint contact_count = p_manifold.mRelativeContactPointsOn1.size(); + + manifold.contacts1.reserve((uint32_t)contact_count); + manifold.contacts2.reserve((uint32_t)contact_count); + manifold.depth = p_manifold.mPenetrationDepth; + + JPH::CollisionEstimationResult collision; + + JPH::EstimateCollisionResponse(p_body1, p_body2, p_manifold, collision, p_settings.mCombinedFriction, p_settings.mCombinedRestitution, JoltProjectSettings::get_bounce_velocity_threshold(), 5); + + for (JPH::uint i = 0; i < contact_count; ++i) { + const JPH::RVec3 relative_point1 = JPH::RVec3(p_manifold.mRelativeContactPointsOn1[i]); + const JPH::RVec3 relative_point2 = JPH::RVec3(p_manifold.mRelativeContactPointsOn2[i]); + + const JPH::RVec3 world_point1 = p_manifold.mBaseOffset + relative_point1; + const JPH::RVec3 world_point2 = p_manifold.mBaseOffset + relative_point2; + + const JPH::Vec3 velocity1 = p_body1.GetPointVelocity(world_point1); + const JPH::Vec3 velocity2 = p_body2.GetPointVelocity(world_point2); + + const JPH::CollisionEstimationResult::Impulse &impulse = collision.mImpulses[i]; + + const JPH::Vec3 contact_impulse = p_manifold.mWorldSpaceNormal * impulse.mContactImpulse; + const JPH::Vec3 friction_impulse1 = collision.mTangent1 * impulse.mFrictionImpulse1; + const JPH::Vec3 friction_impulse2 = collision.mTangent2 * impulse.mFrictionImpulse2; + const JPH::Vec3 combined_impulse = contact_impulse + friction_impulse1 + friction_impulse2; + + Contact contact1; + contact1.point_self = world_point1; + contact1.point_other = world_point2; + contact1.normal = -p_manifold.mWorldSpaceNormal; + contact1.velocity_self = velocity1; + contact1.velocity_other = velocity2; + contact1.impulse = -combined_impulse; + manifold.contacts1.push_back(contact1); + + Contact contact2; + contact2.point_self = world_point2; + contact2.point_other = world_point1; + contact2.normal = p_manifold.mWorldSpaceNormal; + contact2.velocity_self = velocity2; + contact2.velocity_other = velocity1; + contact2.impulse = combined_impulse; + manifold.contacts2.push_back(contact2); + } + + return true; +} + +bool JoltContactListener3D::_try_evaluate_area_overlap(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold) { + if (!p_body1.IsSensor() && !p_body2.IsSensor()) { + return false; + } + + auto evaluate = [&](const auto &p_area, const auto &p_object, const JPH::SubShapeIDPair &p_shape_pair) { + const MutexLock write_lock(write_mutex); + + if (p_area.can_monitor(p_object)) { + if (!area_overlaps.has(p_shape_pair)) { + area_overlaps.insert(p_shape_pair); + area_enters.insert(p_shape_pair); + } + } else { + if (area_overlaps.erase(p_shape_pair)) { + area_exits.insert(p_shape_pair); + } + } + }; + + const JPH::SubShapeIDPair shape_pair1(p_body1.GetID(), p_manifold.mSubShapeID1, p_body2.GetID(), p_manifold.mSubShapeID2); + + const JPH::SubShapeIDPair shape_pair2(p_body2.GetID(), p_manifold.mSubShapeID2, p_body1.GetID(), p_manifold.mSubShapeID1); + + const JoltObject3D *object1 = reinterpret_cast(p_body1.GetUserData()); + const JoltObject3D *object2 = reinterpret_cast(p_body2.GetUserData()); + + const JoltArea3D *area1 = object1->as_area(); + const JoltArea3D *area2 = object2->as_area(); + + const JoltBody3D *body1 = object1->as_body(); + const JoltBody3D *body2 = object2->as_body(); + + if (area1 != nullptr && area2 != nullptr) { + evaluate(*area1, *area2, shape_pair1); + evaluate(*area2, *area1, shape_pair2); + } else if (area1 != nullptr && body2 != nullptr) { + evaluate(*area1, *body2, shape_pair1); + } else if (area2 != nullptr && body1 != nullptr) { + evaluate(*area2, *body1, shape_pair2); + } + + return true; +} + +bool JoltContactListener3D::_try_remove_contacts(const JPH::SubShapeIDPair &p_shape_pair) { + const MutexLock write_lock(write_mutex); + + return manifolds_by_shape_pair.erase(p_shape_pair); +} + +bool JoltContactListener3D::_try_remove_area_overlap(const JPH::SubShapeIDPair &p_shape_pair) { + const JPH::SubShapeIDPair swapped_shape_pair(p_shape_pair.GetBody2ID(), p_shape_pair.GetSubShapeID2(), p_shape_pair.GetBody1ID(), p_shape_pair.GetSubShapeID1()); + + const MutexLock write_lock(write_mutex); + + bool removed = false; + + if (area_overlaps.erase(p_shape_pair)) { + area_exits.insert(p_shape_pair); + removed = true; + } + + if (area_overlaps.erase(swapped_shape_pair)) { + area_exits.insert(swapped_shape_pair); + removed = true; + } + + return removed; +} + +#ifdef DEBUG_ENABLED + +bool JoltContactListener3D::_try_add_debug_contacts(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold) { + if (p_body1.IsSensor() || p_body2.IsSensor()) { + return false; + } + + const int64_t max_count = debug_contacts.size(); + + if (max_count == 0) { + return false; + } + + const int additional_pairs = (int)p_manifold.mRelativeContactPointsOn1.size(); + const int additional_contacts = additional_pairs * 2; + + int current_count = debug_contact_count.load(std::memory_order_relaxed); + bool exchanged = false; + + do { + const int new_count = current_count + additional_contacts; + + if (new_count > max_count) { + return false; + } + + exchanged = debug_contact_count.compare_exchange_weak(current_count, new_count, std::memory_order_release, std::memory_order_relaxed); + } while (!exchanged); + + for (int i = 0; i < additional_pairs; ++i) { + const int pair_index = current_count + i * 2; + + const JPH::RVec3 point_on_1 = p_manifold.GetWorldSpaceContactPointOn1((JPH::uint)i); + const JPH::RVec3 point_on_2 = p_manifold.GetWorldSpaceContactPointOn2((JPH::uint)i); + + debug_contacts.write[pair_index + 0] = to_godot(point_on_1); + debug_contacts.write[pair_index + 1] = to_godot(point_on_2); + } + + return true; +} + +bool JoltContactListener3D::_try_add_debug_contacts(const JPH::Body &p_soft_body, const JPH::SoftBodyManifold &p_manifold) { + const int64_t max_count = debug_contacts.size(); + if (max_count == 0) { + return false; + } + + int additional_contacts = 0; + + for (const JPH::SoftBodyVertex &vertex : p_manifold.GetVertices()) { + if (p_manifold.HasContact(vertex)) { + additional_contacts += 1; + } + } + + int current_count = debug_contact_count.load(std::memory_order_relaxed); + bool exchanged = false; + + do { + const int new_count = current_count + additional_contacts; + + if (new_count > max_count) { + return false; + } + + exchanged = debug_contact_count.compare_exchange_weak(current_count, new_count, std::memory_order_release, std::memory_order_relaxed); + } while (!exchanged); + + int contact_index = current_count; + + for (const JPH::SoftBodyVertex &vertex : p_manifold.GetVertices()) { + if (!p_manifold.HasContact(vertex)) { + continue; + } + + const JPH::RMat44 body_com_transform = p_soft_body.GetCenterOfMassTransform(); + const JPH::Vec3 local_contact_point = p_manifold.GetLocalContactPoint(vertex); + const JPH::RVec3 contact_point = body_com_transform * local_contact_point; + + debug_contacts.write[contact_index++] = to_godot(contact_point); + } + + return true; +} + +#endif + +void JoltContactListener3D::_flush_contacts() { + for (auto &&[shape_pair, manifold] : manifolds_by_shape_pair) { + const JPH::BodyID body_ids[2] = { shape_pair.GetBody1ID(), shape_pair.GetBody2ID() }; + const JoltReadableBodies3D jolt_bodies = space->read_bodies(body_ids, 2); + + JoltBody3D *body1 = jolt_bodies[0].as_body(); + ERR_FAIL_NULL(body1); + + JoltBody3D *body2 = jolt_bodies[1].as_body(); + ERR_FAIL_NULL(body2); + + const int shape_index1 = body1->find_shape_index(shape_pair.GetSubShapeID1()); + const int shape_index2 = body2->find_shape_index(shape_pair.GetSubShapeID2()); + + for (const Contact &contact : manifold.contacts1) { + body1->add_contact(body2, manifold.depth, shape_index1, shape_index2, to_godot(contact.normal), to_godot(contact.point_self), to_godot(contact.point_other), to_godot(contact.velocity_self), to_godot(contact.velocity_other), to_godot(contact.impulse)); + } + + for (const Contact &contact : manifold.contacts2) { + body2->add_contact(body1, manifold.depth, shape_index2, shape_index1, to_godot(contact.normal), to_godot(contact.point_self), to_godot(contact.point_other), to_godot(contact.velocity_self), to_godot(contact.velocity_other), to_godot(contact.impulse)); + } + + manifold.contacts1.clear(); + manifold.contacts2.clear(); + } +} + +void JoltContactListener3D::_flush_area_enters() { + for (const JPH::SubShapeIDPair &shape_pair : area_enters) { + const JPH::BodyID &body_id1 = shape_pair.GetBody1ID(); + const JPH::BodyID &body_id2 = shape_pair.GetBody2ID(); + + const JPH::SubShapeID &sub_shape_id1 = shape_pair.GetSubShapeID1(); + const JPH::SubShapeID &sub_shape_id2 = shape_pair.GetSubShapeID2(); + + const JPH::BodyID body_ids[2] = { body_id1, body_id2 }; + const JoltReadableBodies3D jolt_bodies = space->read_bodies(body_ids, 2); + + const JoltReadableBody3D jolt_body1 = jolt_bodies[0]; + const JoltReadableBody3D jolt_body2 = jolt_bodies[1]; + + if (jolt_body1.is_invalid() || jolt_body2.is_invalid()) { + continue; + } + + JoltArea3D *area1 = jolt_body1.as_area(); + JoltArea3D *area2 = jolt_body2.as_area(); + + if (area1 != nullptr && area2 != nullptr) { + area1->area_shape_entered(body_id2, sub_shape_id2, sub_shape_id1); + } else if (area1 != nullptr && area2 == nullptr) { + area1->body_shape_entered(body_id2, sub_shape_id2, sub_shape_id1); + } else if (area1 == nullptr && area2 != nullptr) { + area2->body_shape_entered(body_id1, sub_shape_id1, sub_shape_id2); + } + } + + area_enters.clear(); +} + +void JoltContactListener3D::_flush_area_shifts() { + for (const JPH::SubShapeIDPair &shape_pair : area_overlaps) { + auto is_shifted = [&](const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_sub_shape_id) { + const JoltReadableBody3D jolt_body = space->read_body(p_body_id); + const JoltShapedObject3D *object = jolt_body.as_shaped(); + ERR_FAIL_NULL_V(object, false); + + if (object->get_previous_jolt_shape() == nullptr) { + return false; + } + + const JPH::Shape ¤t_shape = *object->get_jolt_shape(); + const JPH::Shape &previous_shape = *object->get_previous_jolt_shape(); + + const uint32_t current_id = (uint32_t)current_shape.GetSubShapeUserData(p_sub_shape_id); + const uint32_t previous_id = (uint32_t)previous_shape.GetSubShapeUserData(p_sub_shape_id); + + return current_id != previous_id; + }; + + if (is_shifted(shape_pair.GetBody1ID(), shape_pair.GetSubShapeID1()) || is_shifted(shape_pair.GetBody2ID(), shape_pair.GetSubShapeID2())) { + area_enters.insert(shape_pair); + area_exits.insert(shape_pair); + } + } +} + +void JoltContactListener3D::_flush_area_exits() { + for (const JPH::SubShapeIDPair &shape_pair : area_exits) { + const JPH::BodyID &body_id1 = shape_pair.GetBody1ID(); + const JPH::BodyID &body_id2 = shape_pair.GetBody2ID(); + + const JPH::SubShapeID &sub_shape_id1 = shape_pair.GetSubShapeID1(); + const JPH::SubShapeID &sub_shape_id2 = shape_pair.GetSubShapeID2(); + + const JPH::BodyID body_ids[2] = { body_id1, body_id2 }; + const JoltReadableBodies3D jolt_bodies = space->read_bodies(body_ids, 2); + + const JoltReadableBody3D jolt_body1 = jolt_bodies[0]; + const JoltReadableBody3D jolt_body2 = jolt_bodies[1]; + + JoltArea3D *area1 = jolt_body1.as_area(); + JoltArea3D *area2 = jolt_body2.as_area(); + + const JoltBody3D *body1 = jolt_body1.as_body(); + const JoltBody3D *body2 = jolt_body2.as_body(); + + if (area1 != nullptr && area2 != nullptr) { + area1->area_shape_exited(body_id2, sub_shape_id2, sub_shape_id1); + } else if (area1 != nullptr && body2 != nullptr) { + area1->body_shape_exited(body_id2, sub_shape_id2, sub_shape_id1); + } else if (body1 != nullptr && area2 != nullptr) { + area2->body_shape_exited(body_id1, sub_shape_id1, sub_shape_id2); + } else if (area1 != nullptr) { + area1->shape_exited(body_id2, sub_shape_id2, sub_shape_id1); + } else if (area2 != nullptr) { + area2->shape_exited(body_id1, sub_shape_id1, sub_shape_id2); + } + } + + area_exits.clear(); +} + +void JoltContactListener3D::listen_for(JoltShapedObject3D *p_object) { + listening_for.insert(p_object->get_jolt_id()); +} + +void JoltContactListener3D::pre_step() { + listening_for.clear(); + +#ifdef DEBUG_ENABLED + debug_contact_count = 0; +#endif +} + +void JoltContactListener3D::post_step() { + _flush_contacts(); + _flush_area_shifts(); + _flush_area_exits(); + _flush_area_enters(); +} diff --git a/modules/jolt_physics/spaces/jolt_contact_listener_3d.h b/modules/jolt_physics/spaces/jolt_contact_listener_3d.h new file mode 100644 index 000000000000..34f348140a8f --- /dev/null +++ b/modules/jolt_physics/spaces/jolt_contact_listener_3d.h @@ -0,0 +1,147 @@ +/**************************************************************************/ +/* jolt_contact_listener_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_CONTACT_LISTENER_3D_H +#define JOLT_CONTACT_LISTENER_3D_H + +#include "core/templates/hash_map.h" +#include "core/templates/hash_set.h" +#include "core/templates/hashfuncs.h" +#include "core/templates/local_vector.h" +#include "core/templates/safe_refcount.h" +#include "core/variant/variant.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/Body/Body.h" +#include "Jolt/Physics/Collision/ContactListener.h" +#include "Jolt/Physics/SoftBody/SoftBodyContactListener.h" + +#include +#include + +class JoltShapedObject3D; +class JoltSpace3D; + +class JoltContactListener3D final + : public JPH::ContactListener, + public JPH::SoftBodyContactListener { + struct BodyIDHasher { + static uint32_t hash(const JPH::BodyID &p_id) { return hash_fmix32(p_id.GetIndexAndSequenceNumber()); } + }; + + struct ShapePairHasher { + static uint32_t hash(const JPH::SubShapeIDPair &p_pair) { + uint32_t hash = hash_murmur3_one_32(p_pair.GetBody1ID().GetIndexAndSequenceNumber()); + hash = hash_murmur3_one_32(p_pair.GetSubShapeID1().GetValue(), hash); + hash = hash_murmur3_one_32(p_pair.GetBody2ID().GetIndexAndSequenceNumber(), hash); + hash = hash_murmur3_one_32(p_pair.GetSubShapeID2().GetValue(), hash); + return hash_fmix32(hash); + } + }; + + struct Contact { + JPH::RVec3 point_self = JPH::RVec3::sZero(); + JPH::RVec3 point_other = JPH::RVec3::sZero(); + JPH::Vec3 normal = JPH::Vec3::sZero(); + JPH::Vec3 velocity_self = JPH::Vec3::sZero(); + JPH::Vec3 velocity_other = JPH::Vec3::sZero(); + JPH::Vec3 impulse = JPH::Vec3::sZero(); + }; + + typedef LocalVector Contacts; + + struct Manifold { + Contacts contacts1; + Contacts contacts2; + float depth = 0.0f; + }; + + HashMap manifolds_by_shape_pair; + HashSet listening_for; + HashSet area_overlaps; + HashSet area_enters; + HashSet area_exits; + Mutex write_mutex; + JoltSpace3D *space = nullptr; + +#ifdef DEBUG_ENABLED + PackedVector3Array debug_contacts; + std::atomic_int debug_contact_count; +#endif + + virtual void OnContactAdded(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings) override; + virtual void OnContactPersisted(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings) override; + virtual void OnContactRemoved(const JPH::SubShapeIDPair &p_shape_pair) override; + + virtual JPH::SoftBodyValidateResult OnSoftBodyContactValidate(const JPH::Body &p_soft_body, const JPH::Body &p_other_body, JPH::SoftBodyContactSettings &p_settings) override; + +#ifdef DEBUG_ENABLED + virtual void OnSoftBodyContactAdded(const JPH::Body &p_soft_body, const JPH::SoftBodyManifold &p_manifold) override; +#endif + + bool _is_listening_for(const JPH::Body &p_body) const; + + bool _try_override_collision_response(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, JPH::ContactSettings &p_settings); + bool _try_override_collision_response(const JPH::Body &p_jolt_soft_body, const JPH::Body &p_jolt_other_body, JPH::SoftBodyContactSettings &p_settings); + bool _try_apply_surface_velocities(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, JPH::ContactSettings &p_settings); + bool _try_add_contacts(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings); + bool _try_evaluate_area_overlap(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold); + bool _try_remove_contacts(const JPH::SubShapeIDPair &p_shape_pair); + bool _try_remove_area_overlap(const JPH::SubShapeIDPair &p_shape_pair); + +#ifdef DEBUG_ENABLED + bool _try_add_debug_contacts(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold); + bool _try_add_debug_contacts(const JPH::Body &p_soft_body, const JPH::SoftBodyManifold &p_manifold); +#endif + + void _flush_contacts(); + void _flush_area_enters(); + void _flush_area_shifts(); + void _flush_area_exits(); + +public: + explicit JoltContactListener3D(JoltSpace3D *p_space) : + space(p_space) {} + + void listen_for(JoltShapedObject3D *p_object); + + void pre_step(); + void post_step(); + +#ifdef DEBUG_ENABLED + const PackedVector3Array &get_debug_contacts() const { return debug_contacts; } + int get_debug_contact_count() const { return debug_contact_count.load(std::memory_order_acquire); } + int get_max_debug_contacts() const { return (int)debug_contacts.size(); } + void set_max_debug_contacts(int p_count) { debug_contacts.resize(p_count); } +#endif +}; + +#endif // JOLT_CONTACT_LISTENER_3D_H diff --git a/modules/jolt_physics/spaces/jolt_job_system.cpp b/modules/jolt_physics/spaces/jolt_job_system.cpp new file mode 100644 index 000000000000..b4cae4fb133d --- /dev/null +++ b/modules/jolt_physics/spaces/jolt_job_system.cpp @@ -0,0 +1,203 @@ +/**************************************************************************/ +/* jolt_job_system.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_job_system.h" + +#include "../jolt_project_settings.h" + +#include "core/debugger/engine_debugger.h" +#include "core/error/error_macros.h" +#include "core/math/math_defs.h" +#include "core/object/worker_thread_pool.h" +#include "core/os/os.h" +#include "core/os/time.h" + +#include "Jolt/Physics/PhysicsSettings.h" + +void JoltJobSystem::Job::_execute(void *p_user_data) { + Job *job = static_cast(p_user_data); + +#ifdef DEBUG_ENABLED + const uint64_t time_start = Time::get_singleton()->get_ticks_usec(); +#endif + + job->Execute(); + +#ifdef DEBUG_ENABLED + const uint64_t time_end = Time::get_singleton()->get_ticks_usec(); + const uint64_t time_elapsed = time_end - time_start; + + timings_lock.lock(); + timings_by_job[job->name] += time_elapsed; + timings_lock.unlock(); +#endif + + job->Release(); +} + +JoltJobSystem::Job::Job(const char *p_name, JPH::ColorArg p_color, JPH::JobSystem *p_job_system, const JPH::JobSystem::JobFunction &p_job_function, JPH::uint32 p_dependency_count) : + JPH::JobSystem::Job(p_name, p_color, p_job_system, p_job_function, p_dependency_count) +#ifdef DEBUG_ENABLED + , + name(p_name) +#endif +{ +} + +JoltJobSystem::Job::~Job() { + if (task_id != -1) { + WorkerThreadPool::get_singleton()->wait_for_task_completion(task_id); + } +} + +void JoltJobSystem::Job::push_completed(Job *p_job) { + Job *prev_head = nullptr; + + do { + prev_head = completed_head.load(std::memory_order_relaxed); + p_job->completed_next = prev_head; + } while (!completed_head.compare_exchange_weak(prev_head, p_job, std::memory_order_release, std::memory_order_relaxed)); +} + +JoltJobSystem::Job *JoltJobSystem::Job::pop_completed() { + Job *prev_head = nullptr; + + do { + prev_head = completed_head.load(std::memory_order_relaxed); + if (prev_head == nullptr) { + return nullptr; + } + } while (!completed_head.compare_exchange_weak(prev_head, prev_head->completed_next, std::memory_order_acquire, std::memory_order_relaxed)); + + return prev_head; +} + +void JoltJobSystem::Job::queue() { + AddRef(); + + // Ideally we would use Jolt's actual job name here, but I'd rather not incur the overhead of a memory allocation or + // thread-safe lookup every time we create/queue a task. So instead we use the same cached description for all of them. + static const String task_name("JoltPhysics"); + + task_id = WorkerThreadPool::get_singleton()->add_native_task(&_execute, this, true, task_name); +} + +int JoltJobSystem::GetMaxConcurrency() const { + return thread_count; +} + +JPH::JobHandle JoltJobSystem::CreateJob(const char *p_name, JPH::ColorArg p_color, const JPH::JobSystem::JobFunction &p_job_function, JPH::uint32 p_dependency_count) { + Job *job = nullptr; + + while (true) { + JPH::uint32 job_index = jobs.ConstructObject(p_name, p_color, this, p_job_function, p_dependency_count); + + if (job_index != JPH::FixedSizeFreeList::cInvalidObjectIndex) { + job = &jobs.Get(job_index); + break; + } + + WARN_PRINT_ONCE("Jolt Physics job system exceeded the maximum number of jobs. This should not happen. Please report this. Waiting for jobs to become available..."); + + OS::get_singleton()->delay_usec(100); + + _reclaim_jobs(); + } + + // This will increment the job's reference count, so must happen before we queue the job + JPH::JobHandle job_handle(job); + + if (p_dependency_count == 0) { + QueueJob(job); + } + + return job_handle; +} + +void JoltJobSystem::QueueJob(JPH::JobSystem::Job *p_job) { + static_cast(p_job)->queue(); +} + +void JoltJobSystem::QueueJobs(JPH::JobSystem::Job **p_jobs, JPH::uint p_job_count) { + for (JPH::uint i = 0; i < p_job_count; ++i) { + QueueJob(p_jobs[i]); + } +} + +void JoltJobSystem::FreeJob(JPH::JobSystem::Job *p_job) { + Job::push_completed(static_cast(p_job)); +} + +void JoltJobSystem::_reclaim_jobs() { + while (Job *job = Job::pop_completed()) { + jobs.DestructObject(job); + } +} + +JoltJobSystem::JoltJobSystem() : + JPH::JobSystemWithBarrier(JPH::cMaxPhysicsBarriers), + thread_count(MAX(1, WorkerThreadPool::get_singleton()->get_thread_count())) { + jobs.Init(JPH::cMaxPhysicsJobs, JPH::cMaxPhysicsJobs); +} + +void JoltJobSystem::pre_step() { + // Nothing to do. +} + +void JoltJobSystem::post_step() { + _reclaim_jobs(); +} + +#ifdef DEBUG_ENABLED + +void JoltJobSystem::flush_timings() { + static const StringName profiler_name("servers"); + + EngineDebugger *engine_debugger = EngineDebugger::get_singleton(); + + if (engine_debugger->is_profiling(profiler_name)) { + Array timings; + + for (auto &&[name, usec] : timings_by_job) { + timings.push_back(static_cast(name)); + timings.push_back(USEC_TO_SEC(usec)); + } + + timings.push_front("physics_3d"); + + engine_debugger->profiler_add_frame_data(profiler_name, timings); + } + + for (auto &&[name, usec] : timings_by_job) { + usec = 0; + } +} + +#endif diff --git a/modules/jolt_physics/spaces/jolt_job_system.h b/modules/jolt_physics/spaces/jolt_job_system.h new file mode 100644 index 000000000000..573a8b174192 --- /dev/null +++ b/modules/jolt_physics/spaces/jolt_job_system.h @@ -0,0 +1,107 @@ +/**************************************************************************/ +/* jolt_job_system.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_JOB_SYSTEM_H +#define JOLT_JOB_SYSTEM_H + +#include "core/os/spin_lock.h" +#include "core/templates/hash_map.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Core/FixedSizeFreeList.h" +#include "Jolt/Core/JobSystemWithBarrier.h" + +#include +#include + +class JoltJobSystem final : public JPH::JobSystemWithBarrier { + class Job : public JPH::JobSystem::Job { + inline static std::atomic completed_head = nullptr; + +#ifdef DEBUG_ENABLED + const char *name = nullptr; +#endif + + int64_t task_id = -1; + + std::atomic completed_next = nullptr; + + static void _execute(void *p_user_data); + + public: + Job(const char *p_name, JPH::ColorArg p_color, JPH::JobSystem *p_job_system, const JPH::JobSystem::JobFunction &p_job_function, JPH::uint32 p_dependency_count); + Job(const Job &p_other) = delete; + Job(Job &&p_other) = delete; + ~Job(); + + static void push_completed(Job *p_job); + static Job *pop_completed(); + + void queue(); + + Job &operator=(const Job &p_other) = delete; + Job &operator=(Job &&p_other) = delete; + }; + +#ifdef DEBUG_ENABLED + // We use `const void*` here to avoid the cost of hashing the actual string, since the job names + // are always literals and as such will point to the same address every time. + inline static HashMap timings_by_job; + + // TODO: Check whether the usage of SpinLock is justified or if this should be a mutex instead. + inline static SpinLock timings_lock; +#endif + + JPH::FixedSizeFreeList jobs; + + int thread_count = 0; + + virtual int GetMaxConcurrency() const override; + + virtual JPH::JobHandle CreateJob(const char *p_name, JPH::ColorArg p_color, const JPH::JobSystem::JobFunction &p_job_function, JPH::uint32 p_dependency_count = 0) override; + virtual void QueueJob(JPH::JobSystem::Job *p_job) override; + virtual void QueueJobs(JPH::JobSystem::Job **p_jobs, JPH::uint p_job_count) override; + virtual void FreeJob(JPH::JobSystem::Job *p_job) override; + + void _reclaim_jobs(); + +public: + JoltJobSystem(); + + void pre_step(); + void post_step(); + +#ifdef DEBUG_ENABLED + void flush_timings(); +#endif +}; + +#endif // JOLT_JOB_SYSTEM_H diff --git a/modules/jolt_physics/spaces/jolt_layers.cpp b/modules/jolt_physics/spaces/jolt_layers.cpp new file mode 100644 index 000000000000..c39c3a339702 --- /dev/null +++ b/modules/jolt_physics/spaces/jolt_layers.cpp @@ -0,0 +1,223 @@ +/**************************************************************************/ +/* jolt_layers.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_layers.h" + +#include "../jolt_project_settings.h" +#include "jolt_broad_phase_layer.h" + +#include "core/error/error_macros.h" +#include "core/variant/variant.h" + +static_assert(sizeof(JPH::ObjectLayer) == 2, "Size of Jolt's object layer has changed."); +static_assert(sizeof(JPH::BroadPhaseLayer::Type) == 1, "Size of Jolt's broadphase layer has changed."); +static_assert(JoltBroadPhaseLayer::COUNT <= 8, "Maximum number of broadphase layers exceeded."); + +namespace { + +template +class JoltBroadPhaseMatrix { + typedef JPH::BroadPhaseLayer LayerType; + typedef LayerType::Type UnderlyingType; + +public: + JoltBroadPhaseMatrix() { + using namespace JoltBroadPhaseLayer; + + allow_collision(BODY_STATIC, BODY_DYNAMIC); + allow_collision(BODY_STATIC_BIG, BODY_DYNAMIC); + allow_collision(BODY_DYNAMIC, BODY_STATIC); + allow_collision(BODY_DYNAMIC, BODY_STATIC_BIG); + allow_collision(BODY_DYNAMIC, BODY_DYNAMIC); + allow_collision(BODY_DYNAMIC, AREA_DETECTABLE); + allow_collision(BODY_DYNAMIC, AREA_UNDETECTABLE); + allow_collision(AREA_DETECTABLE, BODY_DYNAMIC); + allow_collision(AREA_DETECTABLE, AREA_DETECTABLE); + allow_collision(AREA_DETECTABLE, AREA_UNDETECTABLE); + allow_collision(AREA_UNDETECTABLE, BODY_DYNAMIC); + allow_collision(AREA_UNDETECTABLE, AREA_DETECTABLE); + + if (JoltProjectSettings::areas_detect_static_bodies()) { + allow_collision(BODY_STATIC, AREA_DETECTABLE); + allow_collision(BODY_STATIC, AREA_UNDETECTABLE); + allow_collision(BODY_STATIC_BIG, AREA_DETECTABLE); + allow_collision(BODY_STATIC_BIG, AREA_UNDETECTABLE); + allow_collision(AREA_DETECTABLE, BODY_STATIC); + allow_collision(AREA_DETECTABLE, BODY_STATIC_BIG); + allow_collision(AREA_UNDETECTABLE, BODY_STATIC); + allow_collision(AREA_UNDETECTABLE, BODY_STATIC_BIG); + } + } + + void allow_collision(UnderlyingType p_layer1, UnderlyingType p_layer2) { masks[p_layer1] |= uint8_t(1U << p_layer2); } + void allow_collision(LayerType p_layer1, LayerType p_layer2) { allow_collision((UnderlyingType)p_layer1, (UnderlyingType)p_layer2); } + + bool should_collide(UnderlyingType p_layer1, UnderlyingType p_layer2) const { return (masks[p_layer1] & uint8_t(1U << p_layer2)) != 0; } + bool should_collide(LayerType p_layer1, LayerType p_layer2) const { return should_collide((UnderlyingType)p_layer1, (UnderlyingType)p_layer2); } + +private: + uint8_t masks[TSize] = {}; +}; + +constexpr JPH::ObjectLayer encode_layers(JPH::BroadPhaseLayer p_broad_phase_layer, JPH::ObjectLayer p_object_layer) { + const uint16_t upper_bits = uint16_t((uint8_t)p_broad_phase_layer << 13U); + const uint16_t lower_bits = uint16_t(p_object_layer); + return JPH::ObjectLayer(upper_bits | lower_bits); +} + +constexpr void decode_layers(JPH::ObjectLayer p_encoded_layers, JPH::BroadPhaseLayer &r_broad_phase_layer, JPH::ObjectLayer &r_object_layer) { + r_broad_phase_layer = JPH::BroadPhaseLayer(uint8_t(p_encoded_layers >> 13U)); + r_object_layer = JPH::ObjectLayer(p_encoded_layers & 0b0001'1111'1111'1111U); +} + +constexpr uint64_t encode_collision(uint32_t p_collision_layer, uint32_t p_collision_mask) { + const uint64_t upper_bits = (uint64_t)p_collision_layer << 32U; + const uint64_t lower_bits = (uint64_t)p_collision_mask; + return upper_bits | lower_bits; +} + +constexpr void decode_collision(uint64_t p_collision, uint32_t &r_collision_layer, uint32_t &r_collision_mask) { + r_collision_layer = uint32_t(p_collision >> 32U); + r_collision_mask = uint32_t(p_collision & 0xFFFFFFFFU); +} + +} // namespace + +uint32_t JoltLayers::GetNumBroadPhaseLayers() const { + return JoltBroadPhaseLayer::COUNT; +} + +JPH::BroadPhaseLayer JoltLayers::GetBroadPhaseLayer(JPH::ObjectLayer p_layer) const { + JPH::BroadPhaseLayer broad_phase_layer = JoltBroadPhaseLayer::BODY_STATIC; + JPH::ObjectLayer object_layer = 0; + decode_layers(p_layer, broad_phase_layer, object_layer); + + return broad_phase_layer; +} + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + +const char *JoltLayers::GetBroadPhaseLayerName(JPH::BroadPhaseLayer p_layer) const { + switch ((JPH::BroadPhaseLayer::Type)p_layer) { + case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_STATIC: { + return "BODY_STATIC"; + } + case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_STATIC_BIG: { + return "BODY_STATIC_BIG"; + } + case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_DYNAMIC: { + return "BODY_DYNAMIC"; + } + case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::AREA_DETECTABLE: { + return "AREA_DETECTABLE"; + } + case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::AREA_UNDETECTABLE: { + return "AREA_UNDETECTABLE"; + } + default: { + return "UNKNOWN"; + } + } +} + +#endif + +bool JoltLayers::ShouldCollide(JPH::ObjectLayer p_encoded_layer1, JPH::ObjectLayer p_encoded_layer2) const { + JPH::BroadPhaseLayer broad_phase_layer1 = JoltBroadPhaseLayer::BODY_STATIC; + uint32_t collision_layer1 = 0; + uint32_t collision_mask1 = 0; + from_object_layer(p_encoded_layer1, broad_phase_layer1, collision_layer1, collision_mask1); + + JPH::BroadPhaseLayer broad_phase_layer2 = JoltBroadPhaseLayer::BODY_STATIC; + uint32_t collision_layer2 = 0; + uint32_t collision_mask2 = 0; + from_object_layer(p_encoded_layer2, broad_phase_layer2, collision_layer2, collision_mask2); + + const bool first_scans_second = (collision_mask1 & collision_layer2) != 0; + const bool second_scans_first = (collision_mask2 & collision_layer1) != 0; + + return first_scans_second || second_scans_first; +} + +bool JoltLayers::ShouldCollide(JPH::ObjectLayer p_encoded_layer1, JPH::BroadPhaseLayer p_broad_phase_layer2) const { + static const JoltBroadPhaseMatrix matrix; + + JPH::BroadPhaseLayer broad_phase_layer1 = JoltBroadPhaseLayer::BODY_STATIC; + JPH::ObjectLayer object_layer1 = 0; + decode_layers(p_encoded_layer1, broad_phase_layer1, object_layer1); + + return matrix.should_collide(broad_phase_layer1, p_broad_phase_layer2); +} + +JPH::ObjectLayer JoltLayers::_allocate_object_layer(uint64_t p_collision) { + const JPH::ObjectLayer new_object_layer = next_object_layer++; + + collisions_by_layer.resize(new_object_layer + 1); + collisions_by_layer[new_object_layer] = p_collision; + + layers_by_collision[p_collision] = new_object_layer; + + return new_object_layer; +} + +JoltLayers::JoltLayers() { + _allocate_object_layer(0); +} + +JPH::ObjectLayer JoltLayers::to_object_layer(JPH::BroadPhaseLayer p_broad_phase_layer, uint32_t p_collision_layer, uint32_t p_collision_mask) { + const uint64_t collision = encode_collision(p_collision_layer, p_collision_mask); + + JPH::ObjectLayer object_layer = 0; + + HashMap::Iterator iter = layers_by_collision.find(collision); + if (iter != layers_by_collision.end()) { + object_layer = iter->value; + } else { + constexpr uint16_t object_layer_count = 1U << 13U; + + ERR_FAIL_COND_V_MSG(next_object_layer == object_layer_count, 0, + vformat("Maximum number of object layers (%d) reached. " + "This means there are %d combinations of collision layers and masks." + "This should not happen under normal circumstances. Consider reporting this.", + object_layer_count, object_layer_count)); + + object_layer = _allocate_object_layer(collision); + } + + return encode_layers(p_broad_phase_layer, object_layer); +} + +void JoltLayers::from_object_layer(JPH::ObjectLayer p_encoded_layer, JPH::BroadPhaseLayer &r_broad_phase_layer, uint32_t &r_collision_layer, uint32_t &r_collision_mask) const { + JPH::ObjectLayer object_layer = 0; + decode_layers(p_encoded_layer, r_broad_phase_layer, object_layer); + + const uint64_t collision = collisions_by_layer[object_layer]; + decode_collision(collision, r_collision_layer, r_collision_mask); +} diff --git a/modules/jolt_physics/spaces/jolt_layers.h b/modules/jolt_physics/spaces/jolt_layers.h new file mode 100644 index 000000000000..f610477a6650 --- /dev/null +++ b/modules/jolt_physics/spaces/jolt_layers.h @@ -0,0 +1,71 @@ +/**************************************************************************/ +/* jolt_layers.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_LAYERS_H +#define JOLT_LAYERS_H + +#include "core/templates/hash_map.h" +#include "core/templates/local_vector.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h" +#include "Jolt/Physics/Collision/ObjectLayer.h" + +#include + +class JoltLayers final + : public JPH::BroadPhaseLayerInterface, + public JPH::ObjectLayerPairFilter, + public JPH::ObjectVsBroadPhaseLayerFilter { + LocalVector collisions_by_layer; + HashMap layers_by_collision; + JPH::ObjectLayer next_object_layer = 0; + + virtual uint32_t GetNumBroadPhaseLayers() const override; + virtual JPH::BroadPhaseLayer GetBroadPhaseLayer(JPH::ObjectLayer p_layer) const override; + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + virtual const char *GetBroadPhaseLayerName(JPH::BroadPhaseLayer p_layer) const override; +#endif + + virtual bool ShouldCollide(JPH::ObjectLayer p_encoded_layer1, JPH::ObjectLayer p_encoded_layer2) const override; + virtual bool ShouldCollide(JPH::ObjectLayer p_encoded_layer1, JPH::BroadPhaseLayer p_broad_phase_layer2) const override; + + JPH::ObjectLayer _allocate_object_layer(uint64_t p_collision); + +public: + JoltLayers(); + + JPH::ObjectLayer to_object_layer(JPH::BroadPhaseLayer p_broad_phase_layer, uint32_t p_collision_layer, uint32_t p_collision_mask); + void from_object_layer(JPH::ObjectLayer p_encoded_layer, JPH::BroadPhaseLayer &r_broad_phase_layer, uint32_t &r_collision_layer, uint32_t &r_collision_mask) const; +}; + +#endif // JOLT_LAYERS_H diff --git a/modules/jolt_physics/spaces/jolt_motion_filter_3d.cpp b/modules/jolt_physics/spaces/jolt_motion_filter_3d.cpp new file mode 100644 index 000000000000..7175a89c4d99 --- /dev/null +++ b/modules/jolt_physics/spaces/jolt_motion_filter_3d.cpp @@ -0,0 +1,116 @@ +/**************************************************************************/ +/* jolt_motion_filter_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_motion_filter_3d.h" + +#include "../objects/jolt_body_3d.h" +#include "../objects/jolt_object_3d.h" +#include "../shapes/jolt_custom_motion_shape.h" +#include "../shapes/jolt_custom_ray_shape.h" +#include "../shapes/jolt_custom_shape_type.h" +#include "../shapes/jolt_shape_3d.h" +#include "jolt_broad_phase_layer.h" +#include "jolt_space_3d.h" + +#include "core/error/error_macros.h" + +JoltMotionFilter3D::JoltMotionFilter3D(const JoltBody3D &p_body, const HashSet &p_excluded_bodies, const HashSet &p_excluded_objects, bool p_collide_separation_ray) : + body_self(p_body), + space(*body_self.get_space()), + excluded_bodies(p_excluded_bodies), + excluded_objects(p_excluded_objects), + collide_separation_ray(p_collide_separation_ray) { +} + +bool JoltMotionFilter3D::ShouldCollide(JPH::BroadPhaseLayer p_broad_phase_layer) const { + const JPH::BroadPhaseLayer::Type broad_phase_layer = (JPH::BroadPhaseLayer::Type)p_broad_phase_layer; + + switch (broad_phase_layer) { + case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_STATIC: + case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_STATIC_BIG: + case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_DYNAMIC: { + return true; + } break; + case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::AREA_DETECTABLE: + case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::AREA_UNDETECTABLE: { + return false; + } break; + default: { + ERR_FAIL_V_MSG(false, vformat("Unhandled broad phase layer: '%d'. This should not happen. Please report this.", broad_phase_layer)); + } + } +} + +bool JoltMotionFilter3D::ShouldCollide(JPH::ObjectLayer p_object_layer) const { + JPH::BroadPhaseLayer object_broad_phase_layer = JoltBroadPhaseLayer::BODY_STATIC; + uint32_t object_collision_layer = 0; + uint32_t object_collision_mask = 0; + + space.map_from_object_layer(p_object_layer, object_broad_phase_layer, object_collision_layer, object_collision_mask); + + return (body_self.get_collision_mask() & object_collision_layer) != 0; +} + +bool JoltMotionFilter3D::ShouldCollide(const JPH::BodyID &p_jolt_id) const { + return p_jolt_id != body_self.get_jolt_id(); +} + +bool JoltMotionFilter3D::ShouldCollideLocked(const JPH::Body &p_jolt_body) const { + if (p_jolt_body.IsSoftBody()) { + return false; + } + + const JoltObject3D *object = reinterpret_cast(p_jolt_body.GetUserData()); + if (excluded_objects.has(object->get_instance_id()) || excluded_bodies.has(object->get_rid())) { + return false; + } + + const JoltReadableBody3D jolt_body_self = space.read_body(body_self); + return jolt_body_self->GetCollisionGroup().CanCollide(p_jolt_body.GetCollisionGroup()); +} + +bool JoltMotionFilter3D::ShouldCollide(const JPH::Shape *p_jolt_shape, const JPH::SubShapeID &p_jolt_shape_id) const { + return true; +} + +bool JoltMotionFilter3D::ShouldCollide(const JPH::Shape *p_jolt_shape_self, const JPH::SubShapeID &p_jolt_shape_id_self, const JPH::Shape *p_jolt_shape_other, const JPH::SubShapeID &p_jolt_shape_id_other) const { + if (collide_separation_ray) { + return true; + } + + const JoltCustomMotionShape *motion_shape = static_cast(p_jolt_shape_self); + const JPH::ConvexShape &actual_shape_self = motion_shape->get_inner_shape(); + if (actual_shape_self.GetSubType() == JoltCustomShapeSubType::RAY) { + // When `slide_on_slope` is enabled the ray shape acts as a regular shape. + return static_cast(actual_shape_self).slide_on_slope; + } + + return true; +} diff --git a/modules/jolt_physics/spaces/jolt_motion_filter_3d.h b/modules/jolt_physics/spaces/jolt_motion_filter_3d.h new file mode 100644 index 000000000000..1a3bd6379f2f --- /dev/null +++ b/modules/jolt_physics/spaces/jolt_motion_filter_3d.h @@ -0,0 +1,72 @@ +/**************************************************************************/ +/* jolt_motion_filter_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_MOTION_FILTER_3D_H +#define JOLT_MOTION_FILTER_3D_H + +#include "core/object/object_id.h" +#include "core/templates/hash_set.h" +#include "core/templates/rid.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/Body/Body.h" +#include "Jolt/Physics/Body/BodyFilter.h" +#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h" +#include "Jolt/Physics/Collision/ObjectLayer.h" +#include "Jolt/Physics/Collision/ShapeFilter.h" + +class JoltBody3D; +class JoltPhysicsServer3D; +class JoltSpace3D; + +class JoltMotionFilter3D final + : public JPH::BroadPhaseLayerFilter, + public JPH::ObjectLayerFilter, + public JPH::BodyFilter, + public JPH::ShapeFilter { + const JoltBody3D &body_self; + const JoltSpace3D &space; + const HashSet &excluded_bodies; + const HashSet &excluded_objects; + bool collide_separation_ray = false; + +public: + explicit JoltMotionFilter3D(const JoltBody3D &p_body, const HashSet &p_excluded_bodies, const HashSet &p_excluded_objects, bool p_collide_separation_ray = true); + + virtual bool ShouldCollide(JPH::BroadPhaseLayer p_broad_phase_layer) const override; + virtual bool ShouldCollide(JPH::ObjectLayer p_object_layer) const override; + virtual bool ShouldCollide(const JPH::BodyID &p_jolt_id) const override; + virtual bool ShouldCollideLocked(const JPH::Body &p_jolt_body) const override; + virtual bool ShouldCollide(const JPH::Shape *p_jolt_shape, const JPH::SubShapeID &p_jolt_shape_id) const override; + virtual bool ShouldCollide(const JPH::Shape *p_jolt_shape_self, const JPH::SubShapeID &p_jolt_shape_id_self, const JPH::Shape *p_jolt_shape_other, const JPH::SubShapeID &p_jolt_shape_id_other) const override; +}; + +#endif // JOLT_MOTION_FILTER_3D_H diff --git a/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp b/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp new file mode 100644 index 000000000000..285ae1dd8670 --- /dev/null +++ b/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp @@ -0,0 +1,940 @@ +/**************************************************************************/ +/* jolt_physics_direct_space_state_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_physics_direct_space_state_3d.h" + +#include "../jolt_physics_server_3d.h" +#include "../jolt_project_settings.h" +#include "../misc/jolt_type_conversions.h" +#include "../objects/jolt_area_3d.h" +#include "../objects/jolt_body_3d.h" +#include "../objects/jolt_object_3d.h" +#include "../shapes/jolt_custom_motion_shape.h" +#include "../shapes/jolt_shape_3d.h" +#include "jolt_motion_filter_3d.h" +#include "jolt_query_collectors.h" +#include "jolt_query_filter_3d.h" +#include "jolt_space_3d.h" + +#include "core/error/error_macros.h" + +#include "Jolt/Geometry/GJKClosestPoint.h" +#include "Jolt/Physics/Body/Body.h" +#include "Jolt/Physics/Body/BodyFilter.h" +#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h" +#include "Jolt/Physics/Collision/CastResult.h" +#include "Jolt/Physics/Collision/CollidePointResult.h" +#include "Jolt/Physics/Collision/NarrowPhaseQuery.h" +#include "Jolt/Physics/Collision/RayCast.h" +#include "Jolt/Physics/Collision/Shape/MeshShape.h" +#include "Jolt/Physics/PhysicsSystem.h" + +bool JoltPhysicsDirectSpaceState3D::_cast_motion_impl(const JPH::Shape &p_jolt_shape, const Transform3D &p_transform_com, const Vector3 &p_scale, const Vector3 &p_motion, bool p_use_edge_removal, bool p_ignore_overlaps, const JPH::CollideShapeSettings &p_settings, const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter, const JPH::ObjectLayerFilter &p_object_layer_filter, const JPH::BodyFilter &p_body_filter, const JPH::ShapeFilter &p_shape_filter, real_t &r_closest_safe, real_t &r_closest_unsafe) const { + r_closest_safe = 1.0f; + r_closest_unsafe = 1.0f; + + ERR_FAIL_COND_V_MSG(p_jolt_shape.GetType() != JPH::EShapeType::Convex, false, "Shape-casting with non-convex shapes is not supported."); + + const float motion_length = (float)p_motion.length(); + + if (p_ignore_overlaps && motion_length == 0.0f) { + return false; + } + + const JPH::RMat44 transform_com = to_jolt_r(p_transform_com); + const JPH::Vec3 scale = to_jolt(p_scale); + const JPH::Vec3 motion = to_jolt(p_motion); + const JPH::Vec3 motion_local = transform_com.Multiply3x3Transposed(motion); + + JPH::AABox aabb = p_jolt_shape.GetWorldSpaceBounds(transform_com, scale); + JPH::AABox aabb_translated = aabb; + aabb_translated.Translate(motion); + aabb.Encapsulate(aabb_translated); + + JoltQueryCollectorAnyMulti aabb_collector; + space->get_broad_phase_query().CollideAABox(aabb, aabb_collector, p_broad_phase_layer_filter, p_object_layer_filter); + + if (!aabb_collector.had_hit()) { + return false; + } + + const JPH::RVec3 base_offset = transform_com.GetTranslation(); + + JoltCustomMotionShape motion_shape(static_cast(p_jolt_shape)); + + auto collides = [&](const JPH::Body &p_other_body, float p_fraction) { + motion_shape.set_motion(motion_local * p_fraction); + + const JPH::TransformedShape other_shape = p_other_body.GetTransformedShape(); + + JoltQueryCollectorAny collector; + + if (p_use_edge_removal) { + JPH::CollideShapeSettings eier_settings = p_settings; + eier_settings.mActiveEdgeMode = JPH::EActiveEdgeMode::CollideWithAll; + eier_settings.mCollectFacesMode = JPH::ECollectFacesMode::CollectFaces; + + JPH::InternalEdgeRemovingCollector eier_collector(collector); + other_shape.CollideShape(&motion_shape, scale, transform_com, eier_settings, base_offset, eier_collector, p_shape_filter); + eier_collector.Flush(); + } else { + other_shape.CollideShape(&motion_shape, scale, transform_com, p_settings, base_offset, collector, p_shape_filter); + } + + return collector.had_hit(); + }; + + // Figure out the number of steps we need in our binary search in order to achieve millimeter precision, within reason. + const int step_count = CLAMP(int(logf(1000.0f * motion_length) / (float)Math_LN2), 4, 16); + + bool collided = false; + + for (int i = 0; i < aabb_collector.get_hit_count(); ++i) { + const JPH::BodyID other_jolt_id = aabb_collector.get_hit(i); + if (!p_body_filter.ShouldCollide(other_jolt_id)) { + continue; + } + + const JoltReadableBody3D other_jolt_body = space->read_body(other_jolt_id); + if (!p_body_filter.ShouldCollideLocked(*other_jolt_body)) { + continue; + } + + if (!collides(*other_jolt_body, 1.0f)) { + continue; + } + + if (p_ignore_overlaps && collides(*other_jolt_body, 0.0f)) { + continue; + } + + float lo = 0.0f; + float hi = 1.0f; + float coeff = 0.5f; + + for (int j = 0; j < step_count; ++j) { + const float fraction = lo + (hi - lo) * coeff; + + if (collides(*other_jolt_body, fraction)) { + collided = true; + + hi = fraction; + + if (j == 0 || lo > 0.0f) { + coeff = 0.5f; + } else { + coeff = 0.25f; + } + } else { + lo = fraction; + + if (j == 0 || hi < 1.0f) { + coeff = 0.5f; + } else { + coeff = 0.75f; + } + } + } + + if (lo < r_closest_safe) { + r_closest_safe = lo; + r_closest_unsafe = hi; + } + } + + return collided; +} + +bool JoltPhysicsDirectSpaceState3D::_body_motion_recover(const JoltBody3D &p_body, const Transform3D &p_transform, float p_margin, const HashSet &p_excluded_bodies, const HashSet &p_excluded_objects, Vector3 &r_recovery) const { + const int recovery_iterations = JoltProjectSettings::get_motion_query_recovery_iterations(); + const float recovery_amount = JoltProjectSettings::get_motion_query_recovery_amount(); + + const JPH::Shape *jolt_shape = p_body.get_jolt_shape(); + + const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass()); + Transform3D transform_com = p_transform.translated_local(com_scaled); + + JPH::CollideShapeSettings settings; + settings.mMaxSeparationDistance = p_margin; + + const Vector3 &base_offset = transform_com.origin; + + const JoltMotionFilter3D motion_filter(p_body, p_excluded_bodies, p_excluded_objects); + JoltQueryCollectorAnyMulti collector; + + bool recovered = false; + + for (int i = 0; i < recovery_iterations; ++i) { + collector.reset(); + + _collide_shape_kinematics(jolt_shape, JPH::Vec3::sReplicate(1.0f), to_jolt_r(transform_com), settings, to_jolt_r(base_offset), collector, motion_filter, motion_filter, motion_filter, motion_filter); + + if (!collector.had_hit()) { + break; + } + + const int hit_count = collector.get_hit_count(); + + float combined_priority = 0.0; + + for (int j = 0; j < hit_count; j++) { + const JPH::CollideShapeResult &hit = collector.get_hit(j); + + const JoltReadableBody3D other_jolt_body = space->read_body(hit.mBodyID2); + const JoltBody3D *other_body = other_jolt_body.as_body(); + ERR_CONTINUE(other_body == nullptr); + + combined_priority += other_body->get_collision_priority(); + } + + const float average_priority = MAX(combined_priority / (float)hit_count, (float)CMP_EPSILON); + + recovered = true; + + Vector3 recovery; + + for (int j = 0; j < hit_count; ++j) { + const JPH::CollideShapeResult &hit = collector.get_hit(j); + + const Vector3 penetration_axis = to_godot(hit.mPenetrationAxis.Normalized()); + const Vector3 margin_offset = penetration_axis * p_margin; + + const Vector3 point_on_1 = base_offset + to_godot(hit.mContactPointOn1) + margin_offset; + const Vector3 point_on_2 = base_offset + to_godot(hit.mContactPointOn2); + + const real_t distance_to_1 = penetration_axis.dot(point_on_1 + recovery); + const real_t distance_to_2 = penetration_axis.dot(point_on_2); + + const float penetration_depth = float(distance_to_1 - distance_to_2); + + if (penetration_depth <= 0.0f) { + continue; + } + + const JoltReadableBody3D other_jolt_body = space->read_body(hit.mBodyID2); + const JoltBody3D *other_body = other_jolt_body.as_body(); + ERR_CONTINUE(other_body == nullptr); + + const float recovery_distance = penetration_depth * recovery_amount; + const float other_priority = other_body->get_collision_priority(); + const float other_priority_normalized = other_priority / average_priority; + const float scaled_recovery_distance = recovery_distance * other_priority_normalized; + + recovery -= penetration_axis * scaled_recovery_distance; + } + + if (recovery == Vector3()) { + break; + } + + r_recovery += recovery; + transform_com.origin += recovery; + } + + return recovered; +} + +bool JoltPhysicsDirectSpaceState3D::_body_motion_cast(const JoltBody3D &p_body, const Transform3D &p_transform, const Vector3 &p_scale, const Vector3 &p_motion, bool p_collide_separation_ray, const HashSet &p_excluded_bodies, const HashSet &p_excluded_objects, real_t &r_safe_fraction, real_t &r_unsafe_fraction) const { + const Transform3D body_transform = p_transform.scaled_local(p_scale); + + const JPH::CollideShapeSettings settings; + const JoltMotionFilter3D motion_filter(p_body, p_excluded_bodies, p_excluded_objects, p_collide_separation_ray); + + bool collided = false; + + for (int i = 0; i < p_body.get_shape_count(); ++i) { + if (p_body.is_shape_disabled(i)) { + continue; + } + + JoltShape3D *shape = p_body.get_shape(i); + + if (!shape->is_convex()) { + continue; + } + + const JPH::ShapeRefC jolt_shape = shape->try_build(); + if (unlikely(jolt_shape == nullptr)) { + return false; + } + + const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass()); + const Transform3D transform_local = p_body.get_shape_transform_scaled(i); + const Transform3D transform_com_local = transform_local.translated_local(com_scaled); + Transform3D transform_com = body_transform * transform_com_local; + + Vector3 scale = transform_com.basis.get_scale(); + JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "body_test_motion was passed an invalid transform along with body '%s'. This results in invalid scaling for shape at index %d."); + + transform_com.basis.orthonormalize(); + + real_t shape_safe_fraction = 1.0; + real_t shape_unsafe_fraction = 1.0; + + collided |= _cast_motion_impl(*jolt_shape, transform_com, scale, p_motion, JoltProjectSettings::use_enhanced_internal_edge_removal_for_motion_queries(), false, settings, motion_filter, motion_filter, motion_filter, motion_filter, shape_safe_fraction, shape_unsafe_fraction); + + r_safe_fraction = MIN(r_safe_fraction, shape_safe_fraction); + r_unsafe_fraction = MIN(r_unsafe_fraction, shape_unsafe_fraction); + } + + return collided; +} + +bool JoltPhysicsDirectSpaceState3D::_body_motion_collide(const JoltBody3D &p_body, const Transform3D &p_transform, const Vector3 &p_motion, float p_margin, int p_max_collisions, const HashSet &p_excluded_bodies, const HashSet &p_excluded_objects, PhysicsServer3D::MotionResult *p_result) const { + if (p_max_collisions == 0) { + return false; + } + + const JPH::Shape *jolt_shape = p_body.get_jolt_shape(); + + const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass()); + const Transform3D transform_com = p_transform.translated_local(com_scaled); + + JPH::CollideShapeSettings settings; + settings.mCollectFacesMode = JPH::ECollectFacesMode::CollectFaces; + settings.mMaxSeparationDistance = p_margin; + + const Vector3 &base_offset = transform_com.origin; + + const JoltMotionFilter3D motion_filter(p_body, p_excluded_bodies, p_excluded_objects); + JoltQueryCollectorClosestMulti collector(p_max_collisions); + _collide_shape_kinematics(jolt_shape, JPH::Vec3::sReplicate(1.0f), to_jolt_r(transform_com), settings, to_jolt_r(base_offset), collector, motion_filter, motion_filter, motion_filter, motion_filter); + + if (!collector.had_hit() || p_result == nullptr) { + return collector.had_hit(); + } + + int count = 0; + + for (int i = 0; i < collector.get_hit_count(); ++i) { + const JPH::CollideShapeResult &hit = collector.get_hit(i); + + const float penetration_depth = hit.mPenetrationDepth + p_margin; + + if (penetration_depth <= 0.0f) { + continue; + } + + const Vector3 normal = to_godot(-hit.mPenetrationAxis.Normalized()); + + if (p_motion.length_squared() > 0) { + const Vector3 direction = p_motion.normalized(); + + if (direction.dot(normal) >= -CMP_EPSILON) { + continue; + } + } + + JPH::ContactPoints contact_points1; + JPH::ContactPoints contact_points2; + + if (p_max_collisions > 1) { + _generate_manifold(hit, contact_points1, contact_points2 JPH_IF_DEBUG_RENDERER(, to_jolt_r(base_offset))); + } else { + contact_points2.push_back(hit.mContactPointOn2); + } + + const JoltReadableBody3D collider_jolt_body = space->read_body(hit.mBodyID2); + const JoltShapedObject3D *collider = collider_jolt_body.as_shaped(); + ERR_FAIL_NULL_V(collider, false); + + const int local_shape = p_body.find_shape_index(hit.mSubShapeID1); + ERR_FAIL_COND_V(local_shape == -1, false); + + const int collider_shape = collider->find_shape_index(hit.mSubShapeID2); + ERR_FAIL_COND_V(collider_shape == -1, false); + + for (JPH::Vec3 contact_point : contact_points2) { + const Vector3 position = base_offset + to_godot(contact_point); + + PhysicsServer3D::MotionCollision &collision = p_result->collisions[count++]; + + collision.position = position; + collision.normal = normal; + collision.collider_velocity = collider->get_velocity_at_position(position); + collision.collider_angular_velocity = collider->get_angular_velocity(); + collision.depth = penetration_depth; + collision.local_shape = local_shape; + collision.collider_id = collider->get_instance_id(); + collision.collider = collider->get_rid(); + collision.collider_shape = collider_shape; + + if (count == p_max_collisions) { + break; + } + } + + if (count == p_max_collisions) { + break; + } + } + + p_result->collision_count = count; + + return count > 0; +} + +int JoltPhysicsDirectSpaceState3D::_try_get_face_index(const JPH::Body &p_body, const JPH::SubShapeID &p_sub_shape_id) { + if (!JoltProjectSettings::enable_ray_cast_face_index()) { + return -1; + } + + const JPH::Shape *root_shape = p_body.GetShape(); + JPH::SubShapeID sub_shape_id_remainder; + const JPH::Shape *leaf_shape = root_shape->GetLeafShape(p_sub_shape_id, sub_shape_id_remainder); + + if (leaf_shape->GetType() != JPH::EShapeType::Mesh) { + return -1; + } + + const JPH::MeshShape *mesh_shape = static_cast(leaf_shape); + return (int)mesh_shape->GetTriangleUserData(sub_shape_id_remainder); +} + +void JoltPhysicsDirectSpaceState3D::_generate_manifold(const JPH::CollideShapeResult &p_hit, JPH::ContactPoints &r_contact_points1, JPH::ContactPoints &r_contact_points2 JPH_IF_DEBUG_RENDERER(, JPH::RVec3Arg p_center_of_mass)) const { + const JPH::PhysicsSystem &physics_system = space->get_physics_system(); + const JPH::PhysicsSettings &physics_settings = physics_system.GetPhysicsSettings(); + const JPH::Vec3 penetration_axis = p_hit.mPenetrationAxis.Normalized(); + + JPH::ManifoldBetweenTwoFaces(p_hit.mContactPointOn1, p_hit.mContactPointOn2, penetration_axis, physics_settings.mManifoldToleranceSq, p_hit.mShape1Face, p_hit.mShape2Face, r_contact_points1, r_contact_points2 JPH_IF_DEBUG_RENDERER(, p_center_of_mass)); + + if (r_contact_points1.size() > 4) { + JPH::PruneContactPoints(penetration_axis, r_contact_points1, r_contact_points2 JPH_IF_DEBUG_RENDERER(, p_center_of_mass)); + } +} + +void JoltPhysicsDirectSpaceState3D::_collide_shape_queries( + const JPH::Shape *p_shape, + JPH::Vec3Arg p_scale, + JPH::RMat44Arg p_transform_com, + const JPH::CollideShapeSettings &p_settings, + JPH::RVec3Arg p_base_offset, + JPH::CollideShapeCollector &p_collector, + const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter, + const JPH::ObjectLayerFilter &p_object_layer_filter, + const JPH::BodyFilter &p_body_filter, + const JPH::ShapeFilter &p_shape_filter) const { + if (JoltProjectSettings::use_enhanced_internal_edge_removal_for_queries()) { + space->get_narrow_phase_query().CollideShapeWithInternalEdgeRemoval(p_shape, p_scale, p_transform_com, p_settings, p_base_offset, p_collector, p_broad_phase_layer_filter, p_object_layer_filter, p_body_filter, p_shape_filter); + } else { + space->get_narrow_phase_query().CollideShape(p_shape, p_scale, p_transform_com, p_settings, p_base_offset, p_collector, p_broad_phase_layer_filter, p_object_layer_filter, p_body_filter, p_shape_filter); + } +} + +void JoltPhysicsDirectSpaceState3D::_collide_shape_kinematics( + const JPH::Shape *p_shape, + JPH::Vec3Arg p_scale, + JPH::RMat44Arg p_transform_com, + const JPH::CollideShapeSettings &p_settings, + JPH::RVec3Arg p_base_offset, + JPH::CollideShapeCollector &p_collector, + const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter, + const JPH::ObjectLayerFilter &p_object_layer_filter, + const JPH::BodyFilter &p_body_filter, + const JPH::ShapeFilter &p_shape_filter) const { + if (JoltProjectSettings::use_enhanced_internal_edge_removal_for_motion_queries()) { + space->get_narrow_phase_query().CollideShapeWithInternalEdgeRemoval(p_shape, p_scale, p_transform_com, p_settings, p_base_offset, p_collector, p_broad_phase_layer_filter, p_object_layer_filter, p_body_filter, p_shape_filter); + } else { + space->get_narrow_phase_query().CollideShape(p_shape, p_scale, p_transform_com, p_settings, p_base_offset, p_collector, p_broad_phase_layer_filter, p_object_layer_filter, p_body_filter, p_shape_filter); + } +} + +JoltPhysicsDirectSpaceState3D::JoltPhysicsDirectSpaceState3D(JoltSpace3D *p_space) : + space(p_space) { +} + +bool JoltPhysicsDirectSpaceState3D::intersect_ray(const RayParameters &p_parameters, RayResult &r_result) { + ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "intersect_ray must not be called while the physics space is being stepped."); + + space->try_optimize(); + + const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.pick_ray); + + const JPH::RVec3 from = to_jolt_r(p_parameters.from); + const JPH::RVec3 to = to_jolt_r(p_parameters.to); + const JPH::Vec3 vector = JPH::Vec3(to - from); + const JPH::RRayCast ray(from, vector); + + const JPH::EBackFaceMode back_face_mode = p_parameters.hit_back_faces ? JPH::EBackFaceMode::CollideWithBackFaces : JPH::EBackFaceMode::IgnoreBackFaces; + + JPH::RayCastSettings settings; + settings.mTreatConvexAsSolid = p_parameters.hit_from_inside; + settings.mBackFaceModeTriangles = back_face_mode; + + JoltQueryCollectorClosest collector; + space->get_narrow_phase_query().CastRay(ray, settings, collector, query_filter, query_filter, query_filter); + + if (!collector.had_hit()) { + return false; + } + + const JPH::RayCastResult &hit = collector.get_hit(); + + const JPH::BodyID &body_id = hit.mBodyID; + const JPH::SubShapeID &sub_shape_id = hit.mSubShapeID2; + + const JoltReadableBody3D body = space->read_body(body_id); + const JoltObject3D *object = body.as_object(); + ERR_FAIL_NULL_V(object, false); + + const JPH::RVec3 position = ray.GetPointOnRay(hit.mFraction); + + JPH::Vec3 normal = JPH::Vec3::sZero(); + + if (!p_parameters.hit_from_inside || hit.mFraction > 0.0f) { + normal = body->GetWorldSpaceSurfaceNormal(sub_shape_id, position); + + // If we got a back-face normal we need to flip it. + if (normal.Dot(vector) > 0) { + normal = -normal; + } + } + + r_result.position = to_godot(position); + r_result.normal = to_godot(normal); + r_result.rid = object->get_rid(); + r_result.collider_id = object->get_instance_id(); + r_result.collider = object->get_instance(); + r_result.shape = 0; + + if (const JoltShapedObject3D *shaped_object = object->as_shaped()) { + const int shape_index = shaped_object->find_shape_index(sub_shape_id); + ERR_FAIL_COND_V(shape_index == -1, false); + r_result.shape = shape_index; + r_result.face_index = _try_get_face_index(*body, sub_shape_id); + } + + return true; +} + +int JoltPhysicsDirectSpaceState3D::intersect_point(const PointParameters &p_parameters, ShapeResult *r_results, int p_result_max) { + ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "intersect_point must not be called while the physics space is being stepped."); + + if (p_result_max == 0) { + return 0; + } + + space->try_optimize(); + + const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude); + + JoltQueryCollectorAnyMulti collector(p_result_max); + space->get_narrow_phase_query().CollidePoint(to_jolt_r(p_parameters.position), collector, query_filter, query_filter, query_filter); + + const int hit_count = collector.get_hit_count(); + + for (int i = 0; i < hit_count; ++i) { + const JPH::CollidePointResult &hit = collector.get_hit(i); + + const JoltReadableBody3D body = space->read_body(hit.mBodyID); + const JoltObject3D *object = body.as_object(); + ERR_FAIL_NULL_V(object, 0); + + ShapeResult &result = *r_results++; + + result.shape = 0; + + if (const JoltShapedObject3D *shaped_object = object->as_shaped()) { + const int shape_index = shaped_object->find_shape_index(hit.mSubShapeID2); + ERR_FAIL_COND_V(shape_index == -1, 0); + result.shape = shape_index; + } + + result.rid = object->get_rid(); + result.collider_id = object->get_instance_id(); + result.collider = object->get_instance(); + } + + return hit_count; +} + +int JoltPhysicsDirectSpaceState3D::intersect_shape(const ShapeParameters &p_parameters, ShapeResult *r_results, int p_result_max) { + ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "intersect_shape must not be called while the physics space is being stepped."); + + if (p_result_max == 0) { + return 0; + } + + space->try_optimize(); + + JoltShape3D *shape = JoltPhysicsServer3D::get_singleton()->get_shape(p_parameters.shape_rid); + ERR_FAIL_NULL_V(shape, 0); + + const JPH::ShapeRefC jolt_shape = shape->try_build(); + ERR_FAIL_NULL_V(jolt_shape, 0); + + Transform3D transform = p_parameters.transform; + JOLT_ENSURE_SCALE_NOT_ZERO(transform, "intersect_shape was passed an invalid transform."); + + Vector3 scale = transform.basis.get_scale(); + JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "intersect_shape was passed an invalid transform."); + + transform.basis.orthonormalize(); + + const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass()); + const Transform3D transform_com = transform.translated_local(com_scaled); + + JPH::CollideShapeSettings settings; + settings.mMaxSeparationDistance = (float)p_parameters.margin; + + const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude); + JoltQueryCollectorAnyMulti collector(p_result_max); + _collide_shape_queries(jolt_shape, to_jolt(scale), to_jolt_r(transform_com), settings, to_jolt_r(transform_com.origin), collector, query_filter, query_filter, query_filter); + + const int hit_count = collector.get_hit_count(); + + for (int i = 0; i < hit_count; ++i) { + const JPH::CollideShapeResult &hit = collector.get_hit(i); + + const JoltReadableBody3D body = space->read_body(hit.mBodyID2); + const JoltObject3D *object = body.as_object(); + ERR_FAIL_NULL_V(object, 0); + + ShapeResult &result = *r_results++; + + result.shape = 0; + + if (const JoltShapedObject3D *shaped_object = object->as_shaped()) { + const int shape_index = shaped_object->find_shape_index(hit.mSubShapeID2); + ERR_FAIL_COND_V(shape_index == -1, 0); + result.shape = shape_index; + } + + result.rid = object->get_rid(); + result.collider_id = object->get_instance_id(); + result.collider = object->get_instance(); + } + + return hit_count; +} + +bool JoltPhysicsDirectSpaceState3D::cast_motion(const ShapeParameters &p_parameters, real_t &r_closest_safe, real_t &r_closest_unsafe, ShapeRestInfo *r_info) { + ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "cast_motion must not be called while the physics space is being stepped."); + ERR_FAIL_COND_V_MSG(r_info != nullptr, false, "Providing rest info as part of cast_motion is not supported when using Jolt Physics."); + + space->try_optimize(); + + JoltShape3D *shape = JoltPhysicsServer3D::get_singleton()->get_shape(p_parameters.shape_rid); + ERR_FAIL_NULL_V(shape, false); + + const JPH::ShapeRefC jolt_shape = shape->try_build(); + ERR_FAIL_NULL_V(jolt_shape, false); + + Transform3D transform = p_parameters.transform; + JOLT_ENSURE_SCALE_NOT_ZERO(transform, "cast_motion (maybe from ShapeCast3D?) was passed an invalid transform."); + + Vector3 scale = transform.basis.get_scale(); + JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "cast_motion (maybe from ShapeCast3D?) was passed an invalid transform."); + + transform.basis.orthonormalize(); + + const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass()); + Transform3D transform_com = transform.translated_local(com_scaled); + + JPH::CollideShapeSettings settings; + settings.mMaxSeparationDistance = (float)p_parameters.margin; + + const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude); + _cast_motion_impl(*jolt_shape, transform_com, scale, p_parameters.motion, JoltProjectSettings::use_enhanced_internal_edge_removal_for_queries(), true, settings, query_filter, query_filter, query_filter, JPH::ShapeFilter(), r_closest_safe, r_closest_unsafe); + + return true; +} + +bool JoltPhysicsDirectSpaceState3D::collide_shape(const ShapeParameters &p_parameters, Vector3 *r_results, int p_result_max, int &r_result_count) { + r_result_count = 0; + + ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "collide_shape must not be called while the physics space is being stepped."); + + if (p_result_max == 0) { + return false; + } + + space->try_optimize(); + + JoltShape3D *shape = JoltPhysicsServer3D::get_singleton()->get_shape(p_parameters.shape_rid); + ERR_FAIL_NULL_V(shape, false); + + const JPH::ShapeRefC jolt_shape = shape->try_build(); + ERR_FAIL_NULL_V(jolt_shape, false); + + Transform3D transform = p_parameters.transform; + JOLT_ENSURE_SCALE_NOT_ZERO(transform, "collide_shape was passed an invalid transform."); + + Vector3 scale = transform.basis.get_scale(); + JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "collide_shape was passed an invalid transform."); + + transform.basis.orthonormalize(); + + const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass()); + const Transform3D transform_com = transform.translated_local(com_scaled); + + JPH::CollideShapeSettings settings; + settings.mCollectFacesMode = JPH::ECollectFacesMode::CollectFaces; + settings.mMaxSeparationDistance = (float)p_parameters.margin; + + const Vector3 &base_offset = transform_com.origin; + + const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude); + JoltQueryCollectorAnyMulti collector(p_result_max); + _collide_shape_queries(jolt_shape, to_jolt(scale), to_jolt_r(transform_com), settings, to_jolt_r(base_offset), collector, query_filter, query_filter, query_filter); + + if (!collector.had_hit()) { + return false; + } + + const int max_points = p_result_max * 2; + + int point_count = 0; + + for (int i = 0; i < collector.get_hit_count(); ++i) { + const JPH::CollideShapeResult &hit = collector.get_hit(i); + + const Vector3 penetration_axis = to_godot(hit.mPenetrationAxis.Normalized()); + const Vector3 margin_offset = penetration_axis * (float)p_parameters.margin; + + JPH::ContactPoints contact_points1; + JPH::ContactPoints contact_points2; + + _generate_manifold(hit, contact_points1, contact_points2 JPH_IF_DEBUG_RENDERER(, to_jolt_r(base_offset))); + + for (JPH::uint j = 0; j < contact_points1.size(); ++j) { + r_results[point_count++] = base_offset + to_godot(contact_points1[j]) + margin_offset; + r_results[point_count++] = base_offset + to_godot(contact_points2[j]); + + if (point_count >= max_points) { + break; + } + } + + if (point_count >= max_points) { + break; + } + } + + r_result_count = point_count / 2; + + return true; +} + +bool JoltPhysicsDirectSpaceState3D::rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) { + ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "get_rest_info must not be called while the physics space is being stepped."); + + space->try_optimize(); + + JoltShape3D *shape = JoltPhysicsServer3D::get_singleton()->get_shape(p_parameters.shape_rid); + ERR_FAIL_NULL_V(shape, false); + + const JPH::ShapeRefC jolt_shape = shape->try_build(); + ERR_FAIL_NULL_V(jolt_shape, false); + + Transform3D transform = p_parameters.transform; + JOLT_ENSURE_SCALE_NOT_ZERO(transform, "get_rest_info (maybe from ShapeCast3D?) was passed an invalid transform."); + + Vector3 scale = transform.basis.get_scale(); + JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "get_rest_info (maybe from ShapeCast3D?) was passed an invalid transform."); + + transform.basis.orthonormalize(); + + const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass()); + const Transform3D transform_com = transform.translated_local(com_scaled); + + JPH::CollideShapeSettings settings; + settings.mMaxSeparationDistance = (float)p_parameters.margin; + + const Vector3 &base_offset = transform_com.origin; + + const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude); + JoltQueryCollectorClosest collector; + _collide_shape_queries(jolt_shape, to_jolt(scale), to_jolt_r(transform_com), settings, to_jolt_r(base_offset), collector, query_filter, query_filter, query_filter); + + if (!collector.had_hit()) { + return false; + } + + const JPH::CollideShapeResult &hit = collector.get_hit(); + const JoltReadableBody3D body = space->read_body(hit.mBodyID2); + const JoltObject3D *object = body.as_object(); + ERR_FAIL_NULL_V(object, false); + + r_info->shape = 0; + + if (const JoltShapedObject3D *shaped_object = object->as_shaped()) { + const int shape_index = shaped_object->find_shape_index(hit.mSubShapeID2); + ERR_FAIL_COND_V(shape_index == -1, false); + r_info->shape = shape_index; + } + + const Vector3 hit_point = base_offset + to_godot(hit.mContactPointOn2); + + r_info->point = hit_point; + r_info->normal = to_godot(-hit.mPenetrationAxis.Normalized()); + r_info->rid = object->get_rid(); + r_info->collider_id = object->get_instance_id(); + r_info->shape = 0; + r_info->linear_velocity = object->get_velocity_at_position(hit_point); + + return true; +} + +Vector3 JoltPhysicsDirectSpaceState3D::get_closest_point_to_object_volume(RID p_object, Vector3 p_point) const { + ERR_FAIL_COND_V_MSG(space->is_stepping(), Vector3(), "get_closest_point_to_object_volume must not be called while the physics space is being stepped."); + + space->try_optimize(); + + JoltPhysicsServer3D *physics_server = JoltPhysicsServer3D::get_singleton(); + JoltObject3D *object = physics_server->get_area(p_object); + + if (object == nullptr) { + object = physics_server->get_body(p_object); + } + + ERR_FAIL_NULL_V(object, Vector3()); + ERR_FAIL_COND_V(object->get_space() != space, Vector3()); + + const JoltReadableBody3D body = space->read_body(*object); + const JPH::TransformedShape root_shape = body->GetTransformedShape(); + + JoltQueryCollectorAll collector; + root_shape.CollectTransformedShapes(body->GetWorldSpaceBounds(), collector); + + const JPH::RVec3 point = to_jolt_r(p_point); + + float closest_distance_sq = FLT_MAX; + JPH::RVec3 closest_point = JPH::RVec3::sZero(); + + bool found_point = false; + + for (int i = 0; i < collector.get_hit_count(); ++i) { + const JPH::TransformedShape &shape_transformed = collector.get_hit(i); + const JPH::Shape &shape = *shape_transformed.mShape; + + if (shape.GetType() != JPH::EShapeType::Convex) { + continue; + } + + const JPH::ConvexShape &shape_convex = static_cast(shape); + + JPH::GJKClosestPoint gjk; + + JPH::ConvexShape::SupportBuffer shape_support_buffer; + + const JPH::ConvexShape::Support *shape_support = shape_convex.GetSupportFunction(JPH::ConvexShape::ESupportMode::IncludeConvexRadius, shape_support_buffer, shape_transformed.GetShapeScale()); + + const JPH::Quat &shape_rotation = shape_transformed.mShapeRotation; + const JPH::RVec3 &shape_pos_com = shape_transformed.mShapePositionCOM; + const JPH::RMat44 shape_3x3 = JPH::RMat44::sRotation(shape_rotation); + const JPH::Vec3 shape_com_local = shape.GetCenterOfMass(); + const JPH::Vec3 shape_com = shape_3x3.Multiply3x3(shape_com_local); + const JPH::RVec3 shape_pos = shape_pos_com - JPH::RVec3(shape_com); + const JPH::RMat44 shape_4x4 = shape_3x3.PostTranslated(shape_pos); + const JPH::RMat44 shape_4x4_inv = shape_4x4.InversedRotationTranslation(); + + JPH::PointConvexSupport point_support; + point_support.mPoint = JPH::Vec3(shape_4x4_inv * point); + + JPH::Vec3 separating_axis = JPH::Vec3::sAxisX(); + JPH::Vec3 point_on_a = JPH::Vec3::sZero(); + JPH::Vec3 point_on_b = JPH::Vec3::sZero(); + + const float distance_sq = gjk.GetClosestPoints(*shape_support, point_support, JPH::cDefaultCollisionTolerance, FLT_MAX, separating_axis, point_on_a, point_on_b); + + if (distance_sq == 0.0f) { + closest_point = point; + found_point = true; + break; + } + + if (distance_sq < closest_distance_sq) { + closest_distance_sq = distance_sq; + closest_point = shape_4x4 * point_on_a; + found_point = true; + } + } + + if (found_point) { + return to_godot(closest_point); + } else { + return to_godot(body->GetPosition()); + } +} + +bool JoltPhysicsDirectSpaceState3D::body_test_motion(const JoltBody3D &p_body, const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult *r_result) const { + ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "body_test_motion (maybe from move_and_slide?) must not be called while the physics space is being stepped."); + + const float margin = MAX((float)p_parameters.margin, 0.0001f); + const int max_collisions = MIN(p_parameters.max_collisions, 32); + + Transform3D transform = p_parameters.from; + JOLT_ENSURE_SCALE_NOT_ZERO(transform, vformat("body_test_motion (maybe from move_and_slide?) was passed an invalid transform along with body '%s'.", p_body.to_string())); + + Vector3 scale = transform.basis.get_scale(); + transform.basis.orthonormalize(); + + space->try_optimize(); + + Vector3 recovery; + const bool recovered = _body_motion_recover(p_body, transform, margin, p_parameters.exclude_bodies, p_parameters.exclude_objects, recovery); + + transform.origin += recovery; + + real_t safe_fraction = 1.0; + real_t unsafe_fraction = 1.0; + + const bool hit = _body_motion_cast(p_body, transform, scale, p_parameters.motion, p_parameters.collide_separation_ray, p_parameters.exclude_bodies, p_parameters.exclude_objects, safe_fraction, unsafe_fraction); + + bool collided = false; + + if (hit || (recovered && p_parameters.recovery_as_collision)) { + collided = _body_motion_collide(p_body, transform.translated(p_parameters.motion * unsafe_fraction), p_parameters.motion, margin, max_collisions, p_parameters.exclude_bodies, p_parameters.exclude_objects, r_result); + } + + if (r_result == nullptr) { + return collided; + } + + if (collided) { + const PhysicsServer3D::MotionCollision &deepest = r_result->collisions[0]; + + r_result->travel = recovery + p_parameters.motion * safe_fraction; + r_result->remainder = p_parameters.motion - p_parameters.motion * safe_fraction; + r_result->collision_depth = deepest.depth; + r_result->collision_safe_fraction = safe_fraction; + r_result->collision_unsafe_fraction = unsafe_fraction; + } else { + r_result->travel = recovery + p_parameters.motion; + r_result->remainder = Vector3(); + r_result->collision_depth = 0.0f; + r_result->collision_safe_fraction = 1.0f; + r_result->collision_unsafe_fraction = 1.0f; + r_result->collision_count = 0; + } + + return collided; +} diff --git a/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.h b/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.h new file mode 100644 index 000000000000..ced8d8dd5a17 --- /dev/null +++ b/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.h @@ -0,0 +1,87 @@ +/**************************************************************************/ +/* jolt_physics_direct_space_state_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_PHYSICS_DIRECT_SPACE_STATE_3D_H +#define JOLT_PHYSICS_DIRECT_SPACE_STATE_3D_H + +#include "servers/physics_server_3d.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/Body/Body.h" +#include "Jolt/Physics/Body/BodyFilter.h" +#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h" +#include "Jolt/Physics/Collision/ContactListener.h" +#include "Jolt/Physics/Collision/ObjectLayer.h" +#include "Jolt/Physics/Collision/Shape/Shape.h" +#include "Jolt/Physics/Collision/ShapeFilter.h" + +class JoltBody3D; +class JoltShape3D; +class JoltSpace3D; + +class JoltPhysicsDirectSpaceState3D final : public PhysicsDirectSpaceState3D { + GDCLASS(JoltPhysicsDirectSpaceState3D, PhysicsDirectSpaceState3D) + + JoltSpace3D *space = nullptr; + + static void _bind_methods() {} + + bool _cast_motion_impl(const JPH::Shape &p_jolt_shape, const Transform3D &p_transform_com, const Vector3 &p_scale, const Vector3 &p_motion, bool p_use_edge_removal, bool p_ignore_overlaps, const JPH::CollideShapeSettings &p_settings, const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter, const JPH::ObjectLayerFilter &p_object_layer_filter, const JPH::BodyFilter &p_body_filter, const JPH::ShapeFilter &p_shape_filter, real_t &r_closest_safe, real_t &r_closest_unsafe) const; + + bool _body_motion_recover(const JoltBody3D &p_body, const Transform3D &p_transform, float p_margin, const HashSet &p_excluded_bodies, const HashSet &p_excluded_objects, Vector3 &r_recovery) const; + bool _body_motion_cast(const JoltBody3D &p_body, const Transform3D &p_transform, const Vector3 &p_scale, const Vector3 &p_motion, bool p_collide_separation_ray, const HashSet &p_excluded_bodies, const HashSet &p_excluded_objects, real_t &r_safe_fraction, real_t &r_unsafe_fraction) const; + bool _body_motion_collide(const JoltBody3D &p_body, const Transform3D &p_transform, const Vector3 &p_motion, float p_margin, int p_max_collisions, const HashSet &p_excluded_bodies, const HashSet &p_excluded_objects, PhysicsServer3D::MotionResult *r_result) const; + + int _try_get_face_index(const JPH::Body &p_body, const JPH::SubShapeID &p_sub_shape_id); + + void _generate_manifold(const JPH::CollideShapeResult &p_hit, JPH::ContactPoints &r_contact_points1, JPH::ContactPoints &r_contact_points2 JPH_IF_DEBUG_RENDERER(, JPH::RVec3Arg p_center_of_mass)) const; + + void _collide_shape_queries(const JPH::Shape *p_shape, JPH::Vec3Arg p_scale, JPH::RMat44Arg p_transform_com, const JPH::CollideShapeSettings &p_settings, JPH::RVec3Arg p_base_offset, JPH::CollideShapeCollector &p_collector, const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter = JPH::BroadPhaseLayerFilter(), const JPH::ObjectLayerFilter &p_object_layer_filter = JPH::ObjectLayerFilter(), const JPH::BodyFilter &p_body_filter = JPH::BodyFilter(), const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const; + void _collide_shape_kinematics(const JPH::Shape *p_shape, JPH::Vec3Arg p_scale, JPH::RMat44Arg p_transform_com, const JPH::CollideShapeSettings &p_settings, JPH::RVec3Arg p_base_offset, JPH::CollideShapeCollector &p_collector, const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter = JPH::BroadPhaseLayerFilter(), const JPH::ObjectLayerFilter &p_object_layer_filter = JPH::ObjectLayerFilter(), const JPH::BodyFilter &p_body_filter = JPH::BodyFilter(), const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const; + +public: + JoltPhysicsDirectSpaceState3D() = default; + explicit JoltPhysicsDirectSpaceState3D(JoltSpace3D *p_space); + + virtual bool intersect_ray(const RayParameters &p_parameters, RayResult &r_result) override; + virtual int intersect_point(const PointParameters &p_parameters, ShapeResult *r_results, int p_result_max) override; + virtual int intersect_shape(const ShapeParameters &p_parameters, ShapeResult *r_results, int p_result_max) override; + virtual bool cast_motion(const ShapeParameters &p_parameters, real_t &r_closest_safe, real_t &r_closest_unsafe, ShapeRestInfo *r_info = nullptr) override; + virtual bool collide_shape(const ShapeParameters &p_parameters, Vector3 *r_results, int p_result_max, int &r_result_count) override; + virtual bool rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) override; + virtual Vector3 get_closest_point_to_object_volume(RID p_object, Vector3 p_point) const override; + + bool body_test_motion(const JoltBody3D &p_body, const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult *r_result) const; + + JoltSpace3D &get_space() const { return *space; } +}; + +#endif // JOLT_PHYSICS_DIRECT_SPACE_STATE_3D_H diff --git a/modules/jolt_physics/spaces/jolt_query_collectors.h b/modules/jolt_physics/spaces/jolt_query_collectors.h new file mode 100644 index 000000000000..a7065ad66705 --- /dev/null +++ b/modules/jolt_physics/spaces/jolt_query_collectors.h @@ -0,0 +1,238 @@ +/**************************************************************************/ +/* jolt_query_collectors.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_QUERY_COLLECTORS_H +#define JOLT_QUERY_COLLECTORS_H + +#include "../jolt_project_settings.h" +#include "jolt_space_3d.h" + +#include "core/templates/local_vector.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/Collision/InternalEdgeRemovingCollector.h" +#include "Jolt/Physics/Collision/Shape/Shape.h" + +template +class JoltQueryCollectorAll final : public TBase { +public: + typedef typename TBase::ResultType Hit; + +private: + LocalVector hits; + +public: + bool had_hit() const { + return !hits.is_empty(); + } + + int get_hit_count() const { + return hits.size(); + } + + const Hit &get_hit(int p_index) const { + return hits[p_index]; + } + + void reset() { Reset(); } + + virtual void Reset() override { + TBase::Reset(); + hits.clear(); + } + + virtual void AddHit(const Hit &p_hit) override { + hits.push_back(p_hit); + } +}; + +template +class JoltQueryCollectorAny final : public TBase { +public: + typedef typename TBase::ResultType Hit; + +private: + Hit hit; + bool valid = false; + +public: + bool had_hit() const { return valid; } + + const Hit &get_hit() const { return hit; } + + void reset() { + Reset(); + } + + virtual void Reset() override { + TBase::Reset(); + valid = false; + } + + virtual void AddHit(const Hit &p_hit) override { + hit = p_hit; + valid = true; + + TBase::ForceEarlyOut(); + } +}; + +template +class JoltQueryCollectorAnyMulti final : public TBase { +public: + typedef typename TBase::ResultType Hit; + +private: + LocalVector hits; + int max_hits = 0; + +public: + explicit JoltQueryCollectorAnyMulti(int p_max_hits = TDefaultCapacity) : + max_hits(p_max_hits) {} + + bool had_hit() const { + return hits.size() > 0; + } + + int get_hit_count() const { + return hits.size(); + } + + const Hit &get_hit(int p_index) const { + return hits[p_index]; + } + + void reset() { + Reset(); + } + + virtual void Reset() override { + TBase::Reset(); + hits.clear(); + } + + virtual void AddHit(const Hit &p_hit) override { + if ((int)hits.size() < max_hits) { + hits.push_back(p_hit); + } + + if ((int)hits.size() == max_hits) { + TBase::ForceEarlyOut(); + } + } +}; + +template +class JoltQueryCollectorClosest final : public TBase { +public: + typedef typename TBase::ResultType Hit; + +private: + Hit hit; + bool valid = false; + +public: + bool had_hit() const { return valid; } + + const Hit &get_hit() const { return hit; } + + void reset() { + Reset(); + } + + virtual void Reset() override { + TBase::Reset(); + valid = false; + } + + virtual void AddHit(const Hit &p_hit) override { + const float early_out = p_hit.GetEarlyOutFraction(); + + if (!valid || early_out < hit.GetEarlyOutFraction()) { + TBase::UpdateEarlyOutFraction(early_out); + + hit = p_hit; + valid = true; + } + } +}; + +template +class JoltQueryCollectorClosestMulti final : public TBase { +public: + typedef typename TBase::ResultType Hit; + +private: + LocalVector hits; + int max_hits = 0; + +public: + explicit JoltQueryCollectorClosestMulti(int p_max_hits = TDefaultCapacity) : + max_hits(p_max_hits) {} + + bool had_hit() const { + return hits.size() > 0; + } + + int get_hit_count() const { + return hits.size(); + } + + const Hit &get_hit(int p_index) const { + return hits[p_index]; + } + + void reset() { + Reset(); + } + + virtual void Reset() override { + TBase::Reset(); + hits.clear(); + } + + virtual void AddHit(const Hit &p_hit) override { + int i = 0; + for (; i < (int)hits.size(); i++) { + if (p_hit.GetEarlyOutFraction() < hits[i].GetEarlyOutFraction()) { + break; + } + } + + hits.insert(i, p_hit); + + if ((int)hits.size() > max_hits) { + hits.resize(max_hits); + } + } +}; + +#endif // JOLT_QUERY_COLLECTORS_H diff --git a/modules/jolt_physics/spaces/jolt_query_filter_3d.cpp b/modules/jolt_physics/spaces/jolt_query_filter_3d.cpp new file mode 100644 index 000000000000..b677c67d7f23 --- /dev/null +++ b/modules/jolt_physics/spaces/jolt_query_filter_3d.cpp @@ -0,0 +1,85 @@ +/**************************************************************************/ +/* jolt_query_filter_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_query_filter_3d.h" + +#include "../objects/jolt_object_3d.h" +#include "jolt_broad_phase_layer.h" +#include "jolt_physics_direct_space_state_3d.h" +#include "jolt_space_3d.h" + +#include "core/error/error_macros.h" + +JoltQueryFilter3D::JoltQueryFilter3D(const JoltPhysicsDirectSpaceState3D &p_space_state, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, const HashSet &p_excluded, bool p_picking) : + space(p_space_state.get_space()), + excluded(p_excluded), + collision_mask(p_collision_mask), + collide_with_bodies(p_collide_with_bodies), + collide_with_areas(p_collide_with_areas), + picking(p_picking) { +} + +bool JoltQueryFilter3D::ShouldCollide(JPH::BroadPhaseLayer p_broad_phase_layer) const { + const JPH::BroadPhaseLayer::Type broad_phase_layer = (JPH::BroadPhaseLayer::Type)p_broad_phase_layer; + + switch (broad_phase_layer) { + case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_STATIC: + case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_STATIC_BIG: + case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_DYNAMIC: { + return collide_with_bodies; + } break; + case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::AREA_DETECTABLE: + case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::AREA_UNDETECTABLE: { + return collide_with_areas; + } break; + default: { + ERR_FAIL_V_MSG(false, vformat("Unhandled broad phase layer: '%d'. This should not happen. Please report this.", broad_phase_layer)); + } + } +} + +bool JoltQueryFilter3D::ShouldCollide(JPH::ObjectLayer p_object_layer) const { + JPH::BroadPhaseLayer object_broad_phase_layer = JoltBroadPhaseLayer::BODY_STATIC; + uint32_t object_collision_layer = 0; + uint32_t object_collision_mask = 0; + + space.map_from_object_layer(p_object_layer, object_broad_phase_layer, object_collision_layer, object_collision_mask); + + return (collision_mask & object_collision_layer) != 0; +} + +bool JoltQueryFilter3D::ShouldCollide(const JPH::BodyID &p_body_id) const { + return true; +} + +bool JoltQueryFilter3D::ShouldCollideLocked(const JPH::Body &p_body) const { + JoltObject3D *object = reinterpret_cast(p_body.GetUserData()); + return (!picking || object->is_pickable()) && !excluded.has(object->get_rid()); +} diff --git a/modules/jolt_physics/spaces/jolt_query_filter_3d.h b/modules/jolt_physics/spaces/jolt_query_filter_3d.h new file mode 100644 index 000000000000..cba5986b0275 --- /dev/null +++ b/modules/jolt_physics/spaces/jolt_query_filter_3d.h @@ -0,0 +1,67 @@ +/**************************************************************************/ +/* jolt_query_filter_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_QUERY_FILTER_3D_H +#define JOLT_QUERY_FILTER_3D_H + +#include "core/templates/hash_set.h" +#include "core/templates/rid.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Physics/Body/Body.h" +#include "Jolt/Physics/Body/BodyFilter.h" +#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h" +#include "Jolt/Physics/Collision/ObjectLayer.h" + +class JoltPhysicsDirectSpaceState3D; +class JoltSpace3D; + +class JoltQueryFilter3D final + : public JPH::BroadPhaseLayerFilter, + public JPH::ObjectLayerFilter, + public JPH::BodyFilter { + const JoltSpace3D &space; + const HashSet &excluded; + uint32_t collision_mask = 0; + bool collide_with_bodies = false; + bool collide_with_areas = false; + bool picking = false; + +public: + JoltQueryFilter3D(const JoltPhysicsDirectSpaceState3D &p_space_state, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, const HashSet &p_excluded, bool p_picking = false); + + virtual bool ShouldCollide(JPH::BroadPhaseLayer p_broad_phase_layer) const override; + virtual bool ShouldCollide(JPH::ObjectLayer p_object_layer) const override; + virtual bool ShouldCollide(const JPH::BodyID &p_body_id) const override; + virtual bool ShouldCollideLocked(const JPH::Body &p_body) const override; +}; + +#endif // JOLT_QUERY_FILTER_3D_H diff --git a/modules/jolt_physics/spaces/jolt_space_3d.cpp b/modules/jolt_physics/spaces/jolt_space_3d.cpp new file mode 100644 index 000000000000..0417735a56ac --- /dev/null +++ b/modules/jolt_physics/spaces/jolt_space_3d.cpp @@ -0,0 +1,499 @@ +/**************************************************************************/ +/* jolt_space_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "jolt_space_3d.h" + +#include "../joints/jolt_joint_3d.h" +#include "../jolt_physics_server_3d.h" +#include "../jolt_project_settings.h" +#include "../misc/jolt_stream_wrappers.h" +#include "../objects/jolt_area_3d.h" +#include "../objects/jolt_body_3d.h" +#include "../shapes/jolt_custom_shape_type.h" +#include "../shapes/jolt_shape_3d.h" +#include "jolt_contact_listener_3d.h" +#include "jolt_layers.h" +#include "jolt_physics_direct_space_state_3d.h" +#include "jolt_temp_allocator.h" + +#include "core/error/error_macros.h" +#include "core/io/file_access.h" +#include "core/os/time.h" +#include "core/string/print_string.h" +#include "core/variant/variant_utility.h" + +#include "Jolt/Physics/PhysicsScene.h" + +namespace { + +constexpr double DEFAULT_CONTACT_RECYCLE_RADIUS = 0.01; +constexpr double DEFAULT_CONTACT_MAX_SEPARATION = 0.05; +constexpr double DEFAULT_CONTACT_MAX_ALLOWED_PENETRATION = 0.01; +constexpr double DEFAULT_CONTACT_DEFAULT_BIAS = 0.8; +constexpr double DEFAULT_SLEEP_THRESHOLD_LINEAR = 0.1; +constexpr double DEFAULT_SLEEP_THRESHOLD_ANGULAR = 8.0 * Math_PI / 180; +constexpr double DEFAULT_SOLVER_ITERATIONS = 8; + +} // namespace + +void JoltSpace3D::_pre_step(float p_step) { + body_accessor.acquire_all(); + + contact_listener->pre_step(); + + const int body_count = body_accessor.get_count(); + + for (int i = 0; i < body_count; ++i) { + if (JPH::Body *jolt_body = body_accessor.try_get(i)) { + if (jolt_body->IsSoftBody()) { + continue; + } + + JoltShapedObject3D *object = reinterpret_cast(jolt_body->GetUserData()); + + object->pre_step(p_step, *jolt_body); + + if (object->reports_contacts()) { + contact_listener->listen_for(object); + } + } + } + + body_accessor.release(); +} + +void JoltSpace3D::_post_step(float p_step) { + body_accessor.acquire_all(); + + contact_listener->post_step(); + + const int body_count = body_accessor.get_count(); + + for (int i = 0; i < body_count; ++i) { + if (JPH::Body *jolt_body = body_accessor.try_get(i)) { + if (jolt_body->IsSoftBody()) { + continue; + } + + JoltObject3D *object = reinterpret_cast(jolt_body->GetUserData()); + + object->post_step(p_step, *jolt_body); + } + } + + body_accessor.release(); +} + +JoltSpace3D::JoltSpace3D(JPH::JobSystem *p_job_system) : + body_accessor(this), + job_system(p_job_system), + temp_allocator(new JoltTempAllocator()), + layers(new JoltLayers()), + contact_listener(new JoltContactListener3D(this)), + physics_system(new JPH::PhysicsSystem()) { + physics_system->Init((JPH::uint)JoltProjectSettings::get_max_bodies(), 0, (JPH::uint)JoltProjectSettings::get_max_pairs(), (JPH::uint)JoltProjectSettings::get_max_contact_constraints(), *layers, *layers, *layers); + + JPH::PhysicsSettings settings; + settings.mBaumgarte = JoltProjectSettings::get_baumgarte_stabilization_factor(); + settings.mSpeculativeContactDistance = JoltProjectSettings::get_speculative_contact_distance(); + settings.mPenetrationSlop = JoltProjectSettings::get_penetration_slop(); + settings.mLinearCastThreshold = JoltProjectSettings::get_ccd_movement_threshold(); + settings.mLinearCastMaxPenetration = JoltProjectSettings::get_ccd_max_penetration(); + settings.mBodyPairCacheMaxDeltaPositionSq = JoltProjectSettings::get_body_pair_cache_distance_sq(); + settings.mBodyPairCacheCosMaxDeltaRotationDiv2 = JoltProjectSettings::get_body_pair_cache_angle_cos_div2(); + settings.mNumVelocitySteps = (JPH::uint)JoltProjectSettings::get_simulation_velocity_steps(); + settings.mNumPositionSteps = (JPH::uint)JoltProjectSettings::get_simulation_position_steps(); + settings.mMinVelocityForRestitution = JoltProjectSettings::get_bounce_velocity_threshold(); + settings.mTimeBeforeSleep = JoltProjectSettings::get_sleep_time_threshold(); + settings.mPointVelocitySleepThreshold = JoltProjectSettings::get_sleep_velocity_threshold(); + settings.mUseBodyPairContactCache = JoltProjectSettings::is_body_pair_contact_cache_enabled(); + settings.mAllowSleeping = JoltProjectSettings::is_sleep_allowed(); + + physics_system->SetPhysicsSettings(settings); + physics_system->SetGravity(JPH::Vec3::sZero()); + physics_system->SetContactListener(contact_listener); + physics_system->SetSoftBodyContactListener(contact_listener); + + physics_system->SetCombineFriction([](const JPH::Body &p_body1, const JPH::SubShapeID &p_sub_shape_id1, const JPH::Body &p_body2, const JPH::SubShapeID &p_sub_shape_id2) { + return ABS(MIN(p_body1.GetFriction(), p_body2.GetFriction())); + }); + + physics_system->SetCombineRestitution([](const JPH::Body &p_body1, const JPH::SubShapeID &p_sub_shape_id1, const JPH::Body &p_body2, const JPH::SubShapeID &p_sub_shape_id2) { + return CLAMP(p_body1.GetRestitution() + p_body2.GetRestitution(), 0.0f, 1.0f); + }); +} + +JoltSpace3D::~JoltSpace3D() { + if (direct_state != nullptr) { + memdelete(direct_state); + direct_state = nullptr; + } + + if (physics_system != nullptr) { + delete physics_system; + physics_system = nullptr; + } + + if (contact_listener != nullptr) { + delete contact_listener; + contact_listener = nullptr; + } + + if (layers != nullptr) { + delete layers; + layers = nullptr; + } + + if (temp_allocator != nullptr) { + delete temp_allocator; + temp_allocator = nullptr; + } +} + +void JoltSpace3D::step(float p_step) { + stepping = true; + last_step = p_step; + + _pre_step(p_step); + + const JPH::EPhysicsUpdateError update_error = physics_system->Update(p_step, 1, temp_allocator, job_system); + + if ((update_error & JPH::EPhysicsUpdateError::ManifoldCacheFull) != JPH::EPhysicsUpdateError::None) { + WARN_PRINT_ONCE(vformat("Jolt Physics manifold cache exceeded capacity and contacts were ignored. " + "Consider increasing maximum number of contact constraints in project settings. " + "Maximum number of contact constraints is currently set to %d.", + JoltProjectSettings::get_max_contact_constraints())); + } + + if ((update_error & JPH::EPhysicsUpdateError::BodyPairCacheFull) != JPH::EPhysicsUpdateError::None) { + WARN_PRINT_ONCE(vformat("Jolt Physics body pair cache exceeded capacity and contacts were ignored. " + "Consider increasing maximum number of body pairs in project settings. " + "Maximum number of body pairs is currently set to %d.", + JoltProjectSettings::get_max_pairs())); + } + + if ((update_error & JPH::EPhysicsUpdateError::ContactConstraintsFull) != JPH::EPhysicsUpdateError::None) { + WARN_PRINT_ONCE(vformat("Jolt Physics contact constraint buffer exceeded capacity and contacts were ignored. " + "Consider increasing maximum number of contact constraints in project settings. " + "Maximum number of contact constraints is currently set to %d.", + JoltProjectSettings::get_max_contact_constraints())); + } + + _post_step(p_step); + + bodies_added_since_optimizing = 0; + has_stepped = true; + stepping = false; +} + +void JoltSpace3D::call_queries() { + if (!has_stepped) { + // We need to skip the first invocation of this method, because there will be pending notifications that need to + // be flushed first, which can cause weird conflicts with things like `_integrate_forces`. This happens to also + // emulate the behavior of Godot Physics, where (active) collision objects must register to have `call_queries` + // invoked, which they don't do until the physics step, which happens after this. + // + // TODO: This would be better solved by just doing what Godot Physics does with `GodotSpace*D::active_list`. + return; + } + + body_accessor.acquire_all(); + + const int body_count = body_accessor.get_count(); + + for (int i = 0; i < body_count; ++i) { + if (JPH::Body *jolt_body = body_accessor.try_get(i)) { + if (!jolt_body->IsSensor() && !jolt_body->IsSoftBody()) { + JoltBody3D *body = reinterpret_cast(jolt_body->GetUserData()); + body->call_queries(*jolt_body); + } + } + } + + for (int i = 0; i < body_count; ++i) { + if (JPH::Body *jolt_body = body_accessor.try_get(i)) { + if (jolt_body->IsSensor()) { + JoltArea3D *area = reinterpret_cast(jolt_body->GetUserData()); + area->call_queries(*jolt_body); + } + } + } + + body_accessor.release(); +} + +double JoltSpace3D::get_param(PhysicsServer3D::SpaceParameter p_param) const { + switch (p_param) { + case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: { + return DEFAULT_CONTACT_RECYCLE_RADIUS; + } + case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION: { + return DEFAULT_CONTACT_MAX_SEPARATION; + } + case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION: { + return DEFAULT_CONTACT_MAX_ALLOWED_PENETRATION; + } + case PhysicsServer3D::SPACE_PARAM_CONTACT_DEFAULT_BIAS: { + return DEFAULT_CONTACT_DEFAULT_BIAS; + } + case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: { + return DEFAULT_SLEEP_THRESHOLD_LINEAR; + } + case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: { + return DEFAULT_SLEEP_THRESHOLD_ANGULAR; + } + case PhysicsServer3D::SPACE_PARAM_BODY_TIME_TO_SLEEP: { + return JoltProjectSettings::get_sleep_time_threshold(); + } + case PhysicsServer3D::SPACE_PARAM_SOLVER_ITERATIONS: { + return DEFAULT_SOLVER_ITERATIONS; + } + default: { + ERR_FAIL_V_MSG(0.0, vformat("Unhandled space parameter: '%d'. This should not happen. Please report this.", p_param)); + } + } +} + +void JoltSpace3D::set_param(PhysicsServer3D::SpaceParameter p_param, double p_value) { + switch (p_param) { + case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: { + WARN_PRINT("Space-specific contact recycle radius is not supported when using Jolt Physics. Any such value will be ignored."); + } break; + case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION: { + WARN_PRINT("Space-specific contact max separation is not supported when using Jolt Physics. Any such value will be ignored."); + } break; + case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION: { + WARN_PRINT("Space-specific contact max allowed penetration is not supported when using Jolt Physics. Any such value will be ignored."); + } break; + case PhysicsServer3D::SPACE_PARAM_CONTACT_DEFAULT_BIAS: { + WARN_PRINT("Space-specific contact default bias is not supported when using Jolt Physics. Any such value will be ignored."); + } break; + case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: { + WARN_PRINT("Space-specific linear velocity sleep threshold is not supported when using Jolt Physics. Any such value will be ignored."); + } break; + case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: { + WARN_PRINT("Space-specific angular velocity sleep threshold is not supported when using Jolt Physics. Any such value will be ignored."); + } break; + case PhysicsServer3D::SPACE_PARAM_BODY_TIME_TO_SLEEP: { + WARN_PRINT("Space-specific body sleep time is not supported when using Jolt Physics. Any such value will be ignored."); + } break; + case PhysicsServer3D::SPACE_PARAM_SOLVER_ITERATIONS: { + WARN_PRINT("Space-specific solver iterations is not supported when using Jolt Physics. Any such value will be ignored."); + } break; + default: { + ERR_FAIL_MSG(vformat("Unhandled space parameter: '%d'. This should not happen. Please report this.", p_param)); + } break; + } +} + +JPH::BodyInterface &JoltSpace3D::get_body_iface() { + return physics_system->GetBodyInterfaceNoLock(); +} + +const JPH::BodyInterface &JoltSpace3D::get_body_iface() const { + return physics_system->GetBodyInterfaceNoLock(); +} + +const JPH::BodyLockInterface &JoltSpace3D::get_lock_iface() const { + return physics_system->GetBodyLockInterfaceNoLock(); +} + +const JPH::BroadPhaseQuery &JoltSpace3D::get_broad_phase_query() const { + return physics_system->GetBroadPhaseQuery(); +} + +const JPH::NarrowPhaseQuery &JoltSpace3D::get_narrow_phase_query() const { + return physics_system->GetNarrowPhaseQueryNoLock(); +} + +JPH::ObjectLayer JoltSpace3D::map_to_object_layer(JPH::BroadPhaseLayer p_broad_phase_layer, uint32_t p_collision_layer, uint32_t p_collision_mask) { + return layers->to_object_layer(p_broad_phase_layer, p_collision_layer, p_collision_mask); +} + +void JoltSpace3D::map_from_object_layer(JPH::ObjectLayer p_object_layer, JPH::BroadPhaseLayer &r_broad_phase_layer, uint32_t &r_collision_layer, uint32_t &r_collision_mask) const { + layers->from_object_layer(p_object_layer, r_broad_phase_layer, r_collision_layer, r_collision_mask); +} + +JoltReadableBody3D JoltSpace3D::read_body(const JPH::BodyID &p_body_id) const { + return { *this, p_body_id }; +} + +JoltReadableBody3D JoltSpace3D::read_body(const JoltObject3D &p_object) const { + return read_body(p_object.get_jolt_id()); +} + +JoltWritableBody3D JoltSpace3D::write_body(const JPH::BodyID &p_body_id) const { + return { *this, p_body_id }; +} + +JoltWritableBody3D JoltSpace3D::write_body(const JoltObject3D &p_object) const { + return write_body(p_object.get_jolt_id()); +} + +JoltReadableBodies3D JoltSpace3D::read_bodies(const JPH::BodyID *p_body_ids, int p_body_count) const { + return { *this, p_body_ids, p_body_count }; +} + +JoltWritableBodies3D JoltSpace3D::write_bodies(const JPH::BodyID *p_body_ids, int p_body_count) const { + return { *this, p_body_ids, p_body_count }; +} + +JoltPhysicsDirectSpaceState3D *JoltSpace3D::get_direct_state() { + if (direct_state == nullptr) { + direct_state = memnew(JoltPhysicsDirectSpaceState3D(this)); + } + + return direct_state; +} + +void JoltSpace3D::set_default_area(JoltArea3D *p_area) { + if (default_area == p_area) { + return; + } + + if (default_area != nullptr) { + default_area->set_default_area(false); + } + + default_area = p_area; + + if (default_area != nullptr) { + default_area->set_default_area(true); + } +} + +JPH::BodyID JoltSpace3D::add_rigid_body(const JoltObject3D &p_object, const JPH::BodyCreationSettings &p_settings, bool p_sleeping) { + const JPH::BodyID body_id = get_body_iface().CreateAndAddBody(p_settings, p_sleeping ? JPH::EActivation::DontActivate : JPH::EActivation::Activate); + ERR_FAIL_COND_V_MSG(body_id.IsInvalid(), JPH::BodyID(), vformat("Failed to create underlying Jolt Physics body for '%s'. Consider increasing maximum number of bodies in project settings. Maximum number of bodies is currently set to %d.", p_object.to_string(), JoltProjectSettings::get_max_bodies())); + + bodies_added_since_optimizing += 1; + + return body_id; +} + +JPH::BodyID JoltSpace3D::add_soft_body(const JoltObject3D &p_object, const JPH::SoftBodyCreationSettings &p_settings, bool p_sleeping) { + const JPH::BodyID body_id = get_body_iface().CreateAndAddSoftBody(p_settings, p_sleeping ? JPH::EActivation::DontActivate : JPH::EActivation::Activate); + ERR_FAIL_COND_V_MSG(body_id.IsInvalid(), JPH::BodyID(), vformat("Failed to create underlying Jolt Physics body for '%s'. Consider increasing maximum number of bodies in project settings. Maximum number of bodies is currently set to %d.", p_object.to_string(), JoltProjectSettings::get_max_bodies())); + + bodies_added_since_optimizing += 1; + + return body_id; +} + +void JoltSpace3D::remove_body(const JPH::BodyID &p_body_id) { + JPH::BodyInterface &body_iface = get_body_iface(); + + body_iface.RemoveBody(p_body_id); + body_iface.DestroyBody(p_body_id); +} + +void JoltSpace3D::try_optimize() { + // This makes assumptions about the underlying acceleration structure of Jolt's broad-phase, which currently uses a + // quadtree, and which gets walked with a fixed-size node stack of 128. This means that when the quadtree is + // completely unbalanced, as is the case if we add bodies one-by-one without ever stepping the simulation, like in + // the editor viewport, we would exceed this stack size (resulting in an incomplete search) as soon as we perform a + // physics query after having added somewhere in the order of 128 * 3 bodies. We leave a hefty margin just in case. + + if (likely(bodies_added_since_optimizing < 128)) { + return; + } + + physics_system->OptimizeBroadPhase(); + + bodies_added_since_optimizing = 0; +} + +void JoltSpace3D::add_joint(JPH::Constraint *p_jolt_ref) { + physics_system->AddConstraint(p_jolt_ref); +} + +void JoltSpace3D::add_joint(JoltJoint3D *p_joint) { + add_joint(p_joint->get_jolt_ref()); +} + +void JoltSpace3D::remove_joint(JPH::Constraint *p_jolt_ref) { + physics_system->RemoveConstraint(p_jolt_ref); +} + +void JoltSpace3D::remove_joint(JoltJoint3D *p_joint) { + remove_joint(p_joint->get_jolt_ref()); +} + +#ifdef DEBUG_ENABLED + +void JoltSpace3D::dump_debug_snapshot(const String &p_dir) { + const Dictionary datetime = Time::get_singleton()->get_datetime_dict_from_system(); + const String datetime_str = vformat("%04d-%02d-%02d_%02d-%02d-%02d", datetime["year"], datetime["month"], datetime["day"], datetime["hour"], datetime["minute"], datetime["second"]); + const String path = p_dir + vformat("/jolt_snapshot_%s_%d.bin", datetime_str, rid.get_id()); + + Ref file_access = FileAccess::open(path, FileAccess::ModeFlags::WRITE); + ERR_FAIL_COND_MSG(file_access.is_null(), vformat("Failed to open '%s' for writing when saving snapshot of physics space with RID '%d'.", path, rid.get_id())); + + JPH::PhysicsScene physics_scene; + physics_scene.FromPhysicsSystem(physics_system); + + for (JPH::BodyCreationSettings &settings : physics_scene.GetBodies()) { + const JoltObject3D *object = reinterpret_cast(settings.mUserData); + + if (const JoltBody3D *body = object->as_body()) { + // Since we do our own integration of gravity and damping, while leaving Jolt's own values at zero, we need to transfer over the correct values. + settings.mGravityFactor = body->get_gravity_scale(); + settings.mLinearDamping = body->get_total_linear_damp(); + settings.mAngularDamping = body->get_total_angular_damp(); + } + + settings.SetShape(JoltShape3D::without_custom_shapes(settings.GetShape())); + } + + JoltStreamOutputWrapper output_stream(file_access); + physics_scene.SaveBinaryState(output_stream, true, false); + + ERR_FAIL_COND_MSG(file_access->get_error() != OK, vformat("Writing snapshot of physics space with RID '%d' to '%s' failed with error '%s'.", rid.get_id(), path, VariantUtilityFunctions::error_string(file_access->get_error()))); + + print_line(vformat("Snapshot of physics space with RID '%d' saved to '%s'.", rid.get_id(), path)); +} + +const PackedVector3Array &JoltSpace3D::get_debug_contacts() const { + return contact_listener->get_debug_contacts(); +} + +int JoltSpace3D::get_debug_contact_count() const { + return contact_listener->get_debug_contact_count(); +} + +int JoltSpace3D::get_max_debug_contacts() const { + return contact_listener->get_max_debug_contacts(); +} + +void JoltSpace3D::set_max_debug_contacts(int p_count) { + contact_listener->set_max_debug_contacts(p_count); +} + +#endif diff --git a/modules/jolt_physics/spaces/jolt_space_3d.h b/modules/jolt_physics/spaces/jolt_space_3d.h new file mode 100644 index 000000000000..eb666644eae6 --- /dev/null +++ b/modules/jolt_physics/spaces/jolt_space_3d.h @@ -0,0 +1,149 @@ +/**************************************************************************/ +/* jolt_space_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_SPACE_3D_H +#define JOLT_SPACE_3D_H + +#include "jolt_body_accessor_3d.h" + +#include "servers/physics_server_3d.h" + +#include "Jolt/Jolt.h" + +#include "Jolt/Core/JobSystem.h" +#include "Jolt/Core/TempAllocator.h" +#include "Jolt/Physics/Body/BodyInterface.h" +#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h" +#include "Jolt/Physics/Collision/NarrowPhaseQuery.h" +#include "Jolt/Physics/Constraints/Constraint.h" +#include "Jolt/Physics/PhysicsSystem.h" + +#include + +class JoltArea3D; +class JoltContactListener3D; +class JoltJoint3D; +class JoltLayers; +class JoltObject3D; +class JoltPhysicsDirectSpaceState3D; + +class JoltSpace3D { + JoltBodyWriter3D body_accessor; + + RID rid; + + JPH::JobSystem *job_system = nullptr; + JPH::TempAllocator *temp_allocator = nullptr; + JoltLayers *layers = nullptr; + JoltContactListener3D *contact_listener = nullptr; + JPH::PhysicsSystem *physics_system = nullptr; + JoltPhysicsDirectSpaceState3D *direct_state = nullptr; + JoltArea3D *default_area = nullptr; + + float last_step = 0.0f; + + int bodies_added_since_optimizing = 0; + + bool active = false; + bool stepping = false; + bool has_stepped = false; + + void _pre_step(float p_step); + void _post_step(float p_step); + +public: + explicit JoltSpace3D(JPH::JobSystem *p_job_system); + ~JoltSpace3D(); + + void step(float p_step); + + void call_queries(); + + RID get_rid() const { return rid; } + void set_rid(const RID &p_rid) { rid = p_rid; } + + bool is_active() const { return active; } + void set_active(bool p_active) { active = p_active; } + + bool is_stepping() const { return stepping; } + + double get_param(PhysicsServer3D::SpaceParameter p_param) const; + void set_param(PhysicsServer3D::SpaceParameter p_param, double p_value); + + JPH::PhysicsSystem &get_physics_system() const { return *physics_system; } + + JPH::BodyInterface &get_body_iface(); + const JPH::BodyInterface &get_body_iface() const; + const JPH::BodyLockInterface &get_lock_iface() const; + + const JPH::BroadPhaseQuery &get_broad_phase_query() const; + const JPH::NarrowPhaseQuery &get_narrow_phase_query() const; + + JPH::ObjectLayer map_to_object_layer(JPH::BroadPhaseLayer p_broad_phase_layer, uint32_t p_collision_layer, uint32_t p_collision_mask); + void map_from_object_layer(JPH::ObjectLayer p_object_layer, JPH::BroadPhaseLayer &r_broad_phase_layer, uint32_t &r_collision_layer, uint32_t &r_collision_mask) const; + + JoltReadableBody3D read_body(const JPH::BodyID &p_body_id) const; + JoltReadableBody3D read_body(const JoltObject3D &p_object) const; + + JoltWritableBody3D write_body(const JPH::BodyID &p_body_id) const; + JoltWritableBody3D write_body(const JoltObject3D &p_object) const; + + JoltReadableBodies3D read_bodies(const JPH::BodyID *p_body_ids, int p_body_count) const; + JoltWritableBodies3D write_bodies(const JPH::BodyID *p_body_ids, int p_body_count) const; + + JoltPhysicsDirectSpaceState3D *get_direct_state(); + + JoltArea3D *get_default_area() const { return default_area; } + void set_default_area(JoltArea3D *p_area); + + float get_last_step() const { return last_step; } + + JPH::BodyID add_rigid_body(const JoltObject3D &p_object, const JPH::BodyCreationSettings &p_settings, bool p_sleeping = false); + JPH::BodyID add_soft_body(const JoltObject3D &p_object, const JPH::SoftBodyCreationSettings &p_settings, bool p_sleeping = false); + + void remove_body(const JPH::BodyID &p_body_id); + + void try_optimize(); + + void add_joint(JPH::Constraint *p_jolt_ref); + void add_joint(JoltJoint3D *p_joint); + void remove_joint(JPH::Constraint *p_jolt_ref); + void remove_joint(JoltJoint3D *p_joint); + +#ifdef DEBUG_ENABLED + void dump_debug_snapshot(const String &p_dir); + const PackedVector3Array &get_debug_contacts() const; + int get_debug_contact_count() const; + int get_max_debug_contacts() const; + void set_max_debug_contacts(int p_count); +#endif +}; + +#endif // JOLT_SPACE_3D_H diff --git a/modules/jolt_physics/spaces/jolt_temp_allocator.cpp b/modules/jolt_physics/spaces/jolt_temp_allocator.cpp new file mode 100644 index 000000000000..b2584001a311 --- /dev/null +++ b/modules/jolt_physics/spaces/jolt_temp_allocator.cpp @@ -0,0 +1,117 @@ +/**************************************************************************/ +/* jolt_temp_allocator.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +/* +Adapted to Godot from the Jolt Physics library. +*/ + +/* +Copyright 2021 Jorrit Rouwe + +Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in +the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of +the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR +A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN +ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +*/ + +#include "jolt_temp_allocator.h" + +#include "../jolt_project_settings.h" + +#include "core/error/error_macros.h" +#include "core/variant/variant.h" + +#include "Jolt/Core/Memory.h" + +namespace { + +template +constexpr TValue align_up(TValue p_value, TAlignment p_alignment) { + return (p_value + p_alignment - 1) & ~(p_alignment - 1); +} + +} //namespace + +JoltTempAllocator::JoltTempAllocator() : + capacity((uint64_t)JoltProjectSettings::get_temp_memory_b()), + base(static_cast(JPH::Allocate((size_t)capacity))) { +} + +JoltTempAllocator::~JoltTempAllocator() { + JPH::Free(base); +} + +void *JoltTempAllocator::Allocate(uint32_t p_size) { + if (p_size == 0) { + return nullptr; + } + + p_size = align_up(p_size, 16U); + + const uint64_t new_top = top + p_size; + + void *ptr = nullptr; + + if (new_top <= capacity) { + ptr = base + top; + } else { + WARN_PRINT_ONCE(vformat("Jolt Physics temporary memory allocator exceeded capacity of %d MiB. Falling back to slower general-purpose allocator. Consider increasing maximum temporary memory in project settings.", JoltProjectSettings::get_temp_memory_mib())); + ptr = JPH::Allocate(p_size); + } + + top = new_top; + + return ptr; +} + +void JoltTempAllocator::Free(void *p_ptr, uint32_t p_size) { + if (p_ptr == nullptr) { + return; + } + + p_size = align_up(p_size, 16U); + + const uint64_t new_top = top - p_size; + + if (top <= capacity) { + if (base + new_top != p_ptr) { + CRASH_NOW_MSG("Jolt Physics temporary memory was freed in the wrong order."); + } + } else { + JPH::Free(p_ptr); + } + + top = new_top; +} diff --git a/modules/jolt_physics/spaces/jolt_temp_allocator.h b/modules/jolt_physics/spaces/jolt_temp_allocator.h new file mode 100644 index 000000000000..8d519ae00ec7 --- /dev/null +++ b/modules/jolt_physics/spaces/jolt_temp_allocator.h @@ -0,0 +1,53 @@ +/**************************************************************************/ +/* jolt_temp_allocator.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef JOLT_TEMP_ALLOCATOR_H +#define JOLT_TEMP_ALLOCATOR_H + +#include "Jolt/Jolt.h" + +#include "Jolt/Core/TempAllocator.h" + +#include + +class JoltTempAllocator final : public JPH::TempAllocator { + uint64_t capacity = 0; + uint64_t top = 0; + uint8_t *base = nullptr; + +public: + explicit JoltTempAllocator(); + virtual ~JoltTempAllocator() override; + + virtual void *Allocate(JPH::uint p_size) override; + virtual void Free(void *p_ptr, JPH::uint p_size) override; +}; + +#endif // JOLT_TEMP_ALLOCATOR_H diff --git a/pyproject.toml b/pyproject.toml index 403d9fd67535..253ec2d7a6aa 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -88,4 +88,5 @@ ignore-words-list = [ "textin", "thirdparty", "vai", + "streamin", ] diff --git a/thirdparty/README.md b/thirdparty/README.md index 79d7e5b7b6ad..729949b970a5 100644 --- a/thirdparty/README.md +++ b/thirdparty/README.md @@ -433,6 +433,18 @@ Files generated from upstream source: 5. Copy `source/data/out/icudt76l.dat` to the `{GODOT_SOURCE}/thirdparty/icu4c/icudt76l.dat` +## jolt_physics + +- Upstream: https://github.com/jrouwe/JoltPhysics +- Version: 5.2.1 (e3d3cdf644389b621914bb6e73d52ee3137591a7, 2024) +- License: MIT + +Files extracted from upstream source: + +- All files in `Jolt/` except `Jolt.cmake` +- `LICENSE` + + ## jpeg-compressor - Upstream: https://github.com/richgel999/jpeg-compressor diff --git a/thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeBuilder.cpp b/thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeBuilder.cpp new file mode 100644 index 000000000000..4024132a6345 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeBuilder.cpp @@ -0,0 +1,242 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +uint AABBTreeBuilder::Node::GetMinDepth(const Array &inNodes) const +{ + if (HasChildren()) + { + uint left = inNodes[mChild[0]].GetMinDepth(inNodes); + uint right = inNodes[mChild[1]].GetMinDepth(inNodes); + return min(left, right) + 1; + } + else + return 1; +} + +uint AABBTreeBuilder::Node::GetMaxDepth(const Array &inNodes) const +{ + if (HasChildren()) + { + uint left = inNodes[mChild[0]].GetMaxDepth(inNodes); + uint right = inNodes[mChild[1]].GetMaxDepth(inNodes); + return max(left, right) + 1; + } + else + return 1; +} + +uint AABBTreeBuilder::Node::GetNodeCount(const Array &inNodes) const +{ + if (HasChildren()) + return inNodes[mChild[0]].GetNodeCount(inNodes) + inNodes[mChild[1]].GetNodeCount(inNodes) + 1; + else + return 1; +} + +uint AABBTreeBuilder::Node::GetLeafNodeCount(const Array &inNodes) const +{ + if (HasChildren()) + return inNodes[mChild[0]].GetLeafNodeCount(inNodes) + inNodes[mChild[1]].GetLeafNodeCount(inNodes); + else + return 1; +} + +uint AABBTreeBuilder::Node::GetTriangleCountInTree(const Array &inNodes) const +{ + if (HasChildren()) + return inNodes[mChild[0]].GetTriangleCountInTree(inNodes) + inNodes[mChild[1]].GetTriangleCountInTree(inNodes); + else + return GetTriangleCount(); +} + +void AABBTreeBuilder::Node::GetTriangleCountPerNode(const Array &inNodes, float &outAverage, uint &outMin, uint &outMax) const +{ + outMin = INT_MAX; + outMax = 0; + outAverage = 0; + uint avg_divisor = 0; + GetTriangleCountPerNodeInternal(inNodes, outAverage, avg_divisor, outMin, outMax); + if (avg_divisor > 0) + outAverage /= avg_divisor; +} + +float AABBTreeBuilder::Node::CalculateSAHCost(const Array &inNodes, float inCostTraversal, float inCostLeaf) const +{ + float surface_area = mBounds.GetSurfaceArea(); + return surface_area > 0.0f? CalculateSAHCostInternal(inNodes, inCostTraversal / surface_area, inCostLeaf / surface_area) : 0.0f; +} + +void AABBTreeBuilder::Node::GetNChildren(const Array &inNodes, uint inN, Array &outChildren) const +{ + JPH_ASSERT(outChildren.empty()); + + // Check if there is anything to expand + if (!HasChildren()) + return; + + // Start with the children of this node + outChildren.push_back(&inNodes[mChild[0]]); + outChildren.push_back(&inNodes[mChild[1]]); + + size_t next = 0; + bool all_triangles = true; + while (outChildren.size() < inN) + { + // If we have looped over all nodes, start over with the first node again + if (next >= outChildren.size()) + { + // If there only triangle nodes left, we have to terminate + if (all_triangles) + return; + next = 0; + all_triangles = true; + } + + // Try to expand this node into its two children + const Node *to_expand = outChildren[next]; + if (to_expand->HasChildren()) + { + outChildren.erase(outChildren.begin() + next); + outChildren.push_back(&inNodes[to_expand->mChild[0]]); + outChildren.push_back(&inNodes[to_expand->mChild[1]]); + all_triangles = false; + } + else + { + ++next; + } + } +} + +float AABBTreeBuilder::Node::CalculateSAHCostInternal(const Array &inNodes, float inCostTraversalDivSurfaceArea, float inCostLeafDivSurfaceArea) const +{ + if (HasChildren()) + return inCostTraversalDivSurfaceArea * mBounds.GetSurfaceArea() + + inNodes[mChild[0]].CalculateSAHCostInternal(inNodes, inCostTraversalDivSurfaceArea, inCostLeafDivSurfaceArea) + + inNodes[mChild[1]].CalculateSAHCostInternal(inNodes, inCostTraversalDivSurfaceArea, inCostLeafDivSurfaceArea); + else + return inCostLeafDivSurfaceArea * mBounds.GetSurfaceArea() * GetTriangleCount(); +} + +void AABBTreeBuilder::Node::GetTriangleCountPerNodeInternal(const Array &inNodes, float &outAverage, uint &outAverageDivisor, uint &outMin, uint &outMax) const +{ + if (HasChildren()) + { + inNodes[mChild[0]].GetTriangleCountPerNodeInternal(inNodes, outAverage, outAverageDivisor, outMin, outMax); + inNodes[mChild[1]].GetTriangleCountPerNodeInternal(inNodes, outAverage, outAverageDivisor, outMin, outMax); + } + else + { + outAverage += GetTriangleCount(); + outAverageDivisor++; + outMin = min(outMin, GetTriangleCount()); + outMax = max(outMax, GetTriangleCount()); + } +} + +AABBTreeBuilder::AABBTreeBuilder(TriangleSplitter &inSplitter, uint inMaxTrianglesPerLeaf) : + mTriangleSplitter(inSplitter), + mMaxTrianglesPerLeaf(inMaxTrianglesPerLeaf) +{ +} + +AABBTreeBuilder::Node *AABBTreeBuilder::Build(AABBTreeBuilderStats &outStats) +{ + TriangleSplitter::Range initial = mTriangleSplitter.GetInitialRange(); + + // Worst case for number of nodes: 1 leaf node per triangle. At each level above, the number of nodes is half that of the level below. + // This means that at most we'll be allocating 2x the number of triangles in nodes. + mNodes.reserve(2 * initial.Count()); + mTriangles.reserve(initial.Count()); + + // Build the tree + Node &root = mNodes[BuildInternal(initial)]; + + // Collect stats + float avg_triangles_per_leaf; + uint min_triangles_per_leaf, max_triangles_per_leaf; + root.GetTriangleCountPerNode(mNodes, avg_triangles_per_leaf, min_triangles_per_leaf, max_triangles_per_leaf); + + mTriangleSplitter.GetStats(outStats.mSplitterStats); + + outStats.mSAHCost = root.CalculateSAHCost(mNodes, 1.0f, 1.0f); + outStats.mMinDepth = root.GetMinDepth(mNodes); + outStats.mMaxDepth = root.GetMaxDepth(mNodes); + outStats.mNodeCount = root.GetNodeCount(mNodes); + outStats.mLeafNodeCount = root.GetLeafNodeCount(mNodes); + outStats.mMaxTrianglesPerLeaf = mMaxTrianglesPerLeaf; + outStats.mTreeMinTrianglesPerLeaf = min_triangles_per_leaf; + outStats.mTreeMaxTrianglesPerLeaf = max_triangles_per_leaf; + outStats.mTreeAvgTrianglesPerLeaf = avg_triangles_per_leaf; + + return &root; +} + +uint AABBTreeBuilder::BuildInternal(const TriangleSplitter::Range &inTriangles) +{ + // Check if there are too many triangles left + if (inTriangles.Count() > mMaxTrianglesPerLeaf) + { + // Split triangles in two batches + TriangleSplitter::Range left, right; + if (!mTriangleSplitter.Split(inTriangles, left, right)) + { + // When the trace below triggers: + // + // This code builds a tree structure to accelerate collision detection. + // At top level it will start with all triangles in a mesh and then divides the triangles into two batches. + // This process repeats until until the batch size is smaller than mMaxTrianglePerLeaf. + // + // It uses a TriangleSplitter to find a good split. When this warning triggers, the splitter was not able + // to create a reasonable split for the triangles. This usually happens when the triangles in a batch are + // intersecting. They could also be overlapping when projected on the 3 coordinate axis. + // + // To solve this issue, you could try to pass your mesh through a mesh cleaning / optimization algorithm. + // You could also inspect the triangles that cause this issue and see if that part of the mesh can be fixed manually. + // + // When you do not fix this warning, the tree will be less efficient for collision detection, but it will still work. + JPH_IF_DEBUG(Trace("AABBTreeBuilder: Doing random split for %d triangles (max per node: %u)!", (int)inTriangles.Count(), mMaxTrianglesPerLeaf);) + int half = inTriangles.Count() / 2; + JPH_ASSERT(half > 0); + left = TriangleSplitter::Range(inTriangles.mBegin, inTriangles.mBegin + half); + right = TriangleSplitter::Range(inTriangles.mBegin + half, inTriangles.mEnd); + } + + // Recursively build + const uint node_index = (uint)mNodes.size(); + mNodes.push_back(Node()); + uint left_index = BuildInternal(left); + uint right_index = BuildInternal(right); + Node &node = mNodes[node_index]; + node.mChild[0] = left_index; + node.mChild[1] = right_index; + node.mBounds = mNodes[node.mChild[0]].mBounds; + node.mBounds.Encapsulate(mNodes[node.mChild[1]].mBounds); + return node_index; + } + + // Create leaf node + const uint node_index = (uint)mNodes.size(); + mNodes.push_back(Node()); + Node &node = mNodes.back(); + node.mTrianglesBegin = (uint)mTriangles.size(); + node.mNumTriangles = inTriangles.mEnd - inTriangles.mBegin; + const VertexList &v = mTriangleSplitter.GetVertices(); + for (uint i = inTriangles.mBegin; i < inTriangles.mEnd; ++i) + { + const IndexedTriangle &t = mTriangleSplitter.GetTriangle(i); + mTriangles.push_back(t); + node.mBounds.Encapsulate(v, t); + } + + return node_index; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeBuilder.h b/thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeBuilder.h new file mode 100644 index 000000000000..3b0635c11ad0 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeBuilder.h @@ -0,0 +1,118 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +struct AABBTreeBuilderStats +{ + ///@name Splitter stats + TriangleSplitter::Stats mSplitterStats; ///< Stats returned by the triangle splitter algorithm + + ///@name Tree structure + float mSAHCost = 0.0f; ///< Surface Area Heuristic cost of this tree + int mMinDepth = 0; ///< Minimal depth of tree (number of nodes) + int mMaxDepth = 0; ///< Maximum depth of tree (number of nodes) + int mNodeCount = 0; ///< Number of nodes in the tree + int mLeafNodeCount = 0; ///< Number of leaf nodes (that contain triangles) + + ///@name Configured stats + int mMaxTrianglesPerLeaf = 0; ///< Configured max triangles per leaf + + ///@name Actual stats + int mTreeMinTrianglesPerLeaf = 0; ///< Minimal amount of triangles in a leaf + int mTreeMaxTrianglesPerLeaf = 0; ///< Maximal amount of triangles in a leaf + float mTreeAvgTrianglesPerLeaf = 0.0f; ///< Average amount of triangles in leaf nodes +}; + +/// Helper class to build an AABB tree +class JPH_EXPORT AABBTreeBuilder +{ +public: + /// A node in the tree, contains the AABox for the tree and any child nodes or triangles + class Node + { + public: + JPH_OVERRIDE_NEW_DELETE + + /// Indicates that there is no child + static constexpr uint cInvalidNodeIndex = ~uint(0); + + /// Get number of triangles in this node + inline uint GetTriangleCount() const { return mNumTriangles; } + + /// Check if this node has any children + inline bool HasChildren() const { return mChild[0] != cInvalidNodeIndex || mChild[1] != cInvalidNodeIndex; } + + /// Min depth of tree + uint GetMinDepth(const Array &inNodes) const; + + /// Max depth of tree + uint GetMaxDepth(const Array &inNodes) const; + + /// Number of nodes in tree + uint GetNodeCount(const Array &inNodes) const; + + /// Number of leaf nodes in tree + uint GetLeafNodeCount(const Array &inNodes) const; + + /// Get triangle count in tree + uint GetTriangleCountInTree(const Array &inNodes) const; + + /// Calculate min and max triangles per node + void GetTriangleCountPerNode(const Array &inNodes, float &outAverage, uint &outMin, uint &outMax) const; + + /// Calculate the total cost of the tree using the surface area heuristic + float CalculateSAHCost(const Array &inNodes, float inCostTraversal, float inCostLeaf) const; + + /// Recursively get children (breadth first) to get in total inN children (or less if there are no more) + void GetNChildren(const Array &inNodes, uint inN, Array &outChildren) const; + + /// Bounding box + AABox mBounds; + + /// Triangles (if no child nodes) + uint mTrianglesBegin; // Index into mTriangles + uint mNumTriangles = 0; + + /// Child node indices (if no triangles) + uint mChild[2] = { cInvalidNodeIndex, cInvalidNodeIndex }; + + private: + friend class AABBTreeBuilder; + + /// Recursive helper function to calculate cost of the tree + float CalculateSAHCostInternal(const Array &inNodes, float inCostTraversalDivSurfaceArea, float inCostLeafDivSurfaceArea) const; + + /// Recursive helper function to calculate min and max triangles per node + void GetTriangleCountPerNodeInternal(const Array &inNodes, float &outAverage, uint &outAverageDivisor, uint &outMin, uint &outMax) const; + }; + + /// Constructor + AABBTreeBuilder(TriangleSplitter &inSplitter, uint inMaxTrianglesPerLeaf = 16); + + /// Recursively build tree, returns the root node of the tree + Node * Build(AABBTreeBuilderStats &outStats); + + /// Get all nodes + const Array & GetNodes() const { return mNodes; } + + /// Get all triangles + const Array &GetTriangles() const { return mTriangles; } + +private: + uint BuildInternal(const TriangleSplitter::Range &inTriangles); + + TriangleSplitter & mTriangleSplitter; + const uint mMaxTrianglesPerLeaf; + Array mNodes; + Array mTriangles; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeToBuffer.h b/thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeToBuffer.h new file mode 100644 index 000000000000..d50c75061ae8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeToBuffer.h @@ -0,0 +1,296 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Conversion algorithm that converts an AABB tree to an optimized binary buffer +template +class AABBTreeToBuffer +{ +public: + /// Header for the tree + using NodeHeader = typename NodeCodec::Header; + + /// Size in bytes of the header of the tree + static const int HeaderSize = NodeCodec::HeaderSize; + + /// Maximum number of children per node in the tree + static const int NumChildrenPerNode = NodeCodec::NumChildrenPerNode; + + /// Header for the triangles + using TriangleHeader = typename TriangleCodec::TriangleHeader; + + /// Size in bytes of the header for the triangles + static const int TriangleHeaderSize = TriangleCodec::TriangleHeaderSize; + + /// Convert AABB tree. Returns false if failed. + bool Convert(const Array &inTriangles, const Array &inNodes, const VertexList &inVertices, const AABBTreeBuilder::Node *inRoot, bool inStoreUserData, const char *&outError) + { + typename NodeCodec::EncodingContext node_ctx; + typename TriangleCodec::EncodingContext tri_ctx(inVertices); + + // Child nodes out of loop so we don't constantly realloc it + Array child_nodes; + child_nodes.reserve(NumChildrenPerNode); + + // First calculate how big the tree is going to be. + // Since the tree can be huge for very large meshes, we don't want + // to reallocate the buffer as it may cause out of memory situations. + // This loop mimics the construction loop below. + uint64 total_size = HeaderSize + TriangleHeaderSize; + size_t node_count = 1; // Start with root node + size_t to_process_max_size = 1; // Track size of queues so we can do a single reserve below + size_t to_process_triangles_max_size = 0; + { // A scope to free the memory associated with to_estimate and to_estimate_triangles + Array to_estimate; + Array to_estimate_triangles; + to_estimate.push_back(inRoot); + for (;;) + { + while (!to_estimate.empty()) + { + // Get the next node to process + const AABBTreeBuilder::Node *node = to_estimate.back(); + to_estimate.pop_back(); + + // Update total size + node_ctx.PrepareNodeAllocate(node, total_size); + + if (node->HasChildren()) + { + // Collect the first NumChildrenPerNode sub-nodes in the tree + child_nodes.clear(); // Won't free the memory + node->GetNChildren(inNodes, NumChildrenPerNode, child_nodes); + + // Increment the number of nodes we're going to store + node_count += child_nodes.size(); + + // Insert in reverse order so we estimate left child first when taking nodes from the back + for (int idx = int(child_nodes.size()) - 1; idx >= 0; --idx) + { + // Store triangles in separate list so we process them last + const AABBTreeBuilder::Node *child = child_nodes[idx]; + if (child->HasChildren()) + { + to_estimate.push_back(child); + to_process_max_size = max(to_estimate.size(), to_process_max_size); + } + else + { + to_estimate_triangles.push_back(child); + to_process_triangles_max_size = max(to_estimate_triangles.size(), to_process_triangles_max_size); + } + } + } + else + { + // Update total size + tri_ctx.PreparePack(&inTriangles[node->mTrianglesBegin], node->mNumTriangles, inStoreUserData, total_size); + } + } + + // If we've got triangles to estimate, loop again with just the triangles + if (to_estimate_triangles.empty()) + break; + else + to_estimate.swap(to_estimate_triangles); + } + } + + // Finalize the prepare stage for the triangle context + tri_ctx.FinalizePreparePack(total_size); + + // Reserve the buffer + if (size_t(total_size) != total_size) + { + outError = "AABBTreeToBuffer: Out of memory!"; + return false; + } + mTree.reserve(size_t(total_size)); + + // Add headers + NodeHeader *header = HeaderSize > 0? mTree.Allocate() : nullptr; + TriangleHeader *triangle_header = TriangleHeaderSize > 0? mTree.Allocate() : nullptr; + + struct NodeData + { + const AABBTreeBuilder::Node * mNode = nullptr; // Node that this entry belongs to + Vec3 mNodeBoundsMin; // Quantized node bounds + Vec3 mNodeBoundsMax; + size_t mNodeStart = size_t(-1); // Start of node in mTree + size_t mTriangleStart = size_t(-1); // Start of the triangle data in mTree + size_t mChildNodeStart[NumChildrenPerNode]; // Start of the children of the node in mTree + size_t mChildTrianglesStart[NumChildrenPerNode]; // Start of the triangle data in mTree + size_t * mParentChildNodeStart = nullptr; // Where to store mNodeStart (to patch mChildNodeStart of my parent) + size_t * mParentTrianglesStart = nullptr; // Where to store mTriangleStart (to patch mChildTrianglesStart of my parent) + uint mNumChildren = 0; // Number of children + }; + + Array to_process; + to_process.reserve(to_process_max_size); + Array to_process_triangles; + to_process_triangles.reserve(to_process_triangles_max_size); + Array node_list; + node_list.reserve(node_count); // Needed to ensure that array is not reallocated, so we can keep pointers in the array + + NodeData root; + root.mNode = inRoot; + root.mNodeBoundsMin = inRoot->mBounds.mMin; + root.mNodeBoundsMax = inRoot->mBounds.mMax; + node_list.push_back(root); + to_process.push_back(&node_list.back()); + + for (;;) + { + while (!to_process.empty()) + { + // Get the next node to process + NodeData *node_data = to_process.back(); + to_process.pop_back(); + + // Due to quantization box could have become bigger, not smaller + JPH_ASSERT(AABox(node_data->mNodeBoundsMin, node_data->mNodeBoundsMax).Contains(node_data->mNode->mBounds), "AABBTreeToBuffer: Bounding box became smaller!"); + + // Collect the first NumChildrenPerNode sub-nodes in the tree + child_nodes.clear(); // Won't free the memory + node_data->mNode->GetNChildren(inNodes, NumChildrenPerNode, child_nodes); + node_data->mNumChildren = (uint)child_nodes.size(); + + // Fill in default child bounds + Vec3 child_bounds_min[NumChildrenPerNode], child_bounds_max[NumChildrenPerNode]; + for (size_t i = 0; i < NumChildrenPerNode; ++i) + if (i < child_nodes.size()) + { + child_bounds_min[i] = child_nodes[i]->mBounds.mMin; + child_bounds_max[i] = child_nodes[i]->mBounds.mMax; + } + else + { + child_bounds_min[i] = Vec3::sZero(); + child_bounds_max[i] = Vec3::sZero(); + } + + // Start a new node + node_data->mNodeStart = node_ctx.NodeAllocate(node_data->mNode, node_data->mNodeBoundsMin, node_data->mNodeBoundsMax, child_nodes, child_bounds_min, child_bounds_max, mTree, outError); + if (node_data->mNodeStart == size_t(-1)) + return false; + + if (node_data->mNode->HasChildren()) + { + // Insert in reverse order so we process left child first when taking nodes from the back + for (int idx = int(child_nodes.size()) - 1; idx >= 0; --idx) + { + const AABBTreeBuilder::Node *child_node = child_nodes[idx]; + + // Due to quantization box could have become bigger, not smaller + JPH_ASSERT(AABox(child_bounds_min[idx], child_bounds_max[idx]).Contains(child_node->mBounds), "AABBTreeToBuffer: Bounding box became smaller!"); + + // Add child to list of nodes to be processed + NodeData child; + child.mNode = child_node; + child.mNodeBoundsMin = child_bounds_min[idx]; + child.mNodeBoundsMax = child_bounds_max[idx]; + child.mParentChildNodeStart = &node_data->mChildNodeStart[idx]; + child.mParentTrianglesStart = &node_data->mChildTrianglesStart[idx]; + node_list.push_back(child); + + // Store triangles in separate list so we process them last + if (child_node->HasChildren()) + to_process.push_back(&node_list.back()); + else + to_process_triangles.push_back(&node_list.back()); + } + } + else + { + // Add triangles + node_data->mTriangleStart = tri_ctx.Pack(&inTriangles[node_data->mNode->mTrianglesBegin], node_data->mNode->mNumTriangles, inStoreUserData, mTree, outError); + if (node_data->mTriangleStart == size_t(-1)) + return false; + } + + // Patch offset into parent + if (node_data->mParentChildNodeStart != nullptr) + { + *node_data->mParentChildNodeStart = node_data->mNodeStart; + *node_data->mParentTrianglesStart = node_data->mTriangleStart; + } + } + + // If we've got triangles to process, loop again with just the triangles + if (to_process_triangles.empty()) + break; + else + to_process.swap(to_process_triangles); + } + + // Assert that our reservation was correct (we don't know if we swapped the arrays or not) + JPH_ASSERT(to_process_max_size == to_process.capacity() || to_process_triangles_max_size == to_process.capacity()); + JPH_ASSERT(to_process_max_size == to_process_triangles.capacity() || to_process_triangles_max_size == to_process_triangles.capacity()); + + // Finalize all nodes + for (NodeData &n : node_list) + if (!node_ctx.NodeFinalize(n.mNode, n.mNodeStart, n.mNumChildren, n.mChildNodeStart, n.mChildTrianglesStart, mTree, outError)) + return false; + + // Finalize the triangles + tri_ctx.Finalize(inVertices, triangle_header, mTree); + + // Validate that our reservations were correct + if (node_count != node_list.size()) + { + outError = "Internal Error: Node memory estimate was incorrect, memory corruption!"; + return false; + } + if (total_size != mTree.size()) + { + outError = "Internal Error: Tree memory estimate was incorrect, memory corruption!"; + return false; + } + + // Finalize the nodes + return node_ctx.Finalize(header, inRoot, node_list[0].mNodeStart, node_list[0].mTriangleStart, outError); + } + + /// Get resulting data + inline const ByteBuffer & GetBuffer() const + { + return mTree; + } + + /// Get resulting data + inline ByteBuffer & GetBuffer() + { + return mTree; + } + + /// Get header for tree + inline const NodeHeader * GetNodeHeader() const + { + return mTree.Get(0); + } + + /// Get header for triangles + inline const TriangleHeader * GetTriangleHeader() const + { + return mTree.Get(HeaderSize); + } + + /// Get root of resulting tree + inline const void * GetRoot() const + { + return mTree.Get(HeaderSize + TriangleHeaderSize); + } + +private: + ByteBuffer mTree; ///< Resulting tree structure +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/AABBTree/NodeCodec/NodeCodecQuadTreeHalfFloat.h b/thirdparty/jolt_physics/Jolt/AABBTree/NodeCodec/NodeCodecQuadTreeHalfFloat.h new file mode 100644 index 000000000000..15717a2da74b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/AABBTree/NodeCodec/NodeCodecQuadTreeHalfFloat.h @@ -0,0 +1,313 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class NodeCodecQuadTreeHalfFloat +{ +public: + /// Number of child nodes of this node + static constexpr int NumChildrenPerNode = 4; + + /// Header for the tree + struct Header + { + Float3 mRootBoundsMin; + Float3 mRootBoundsMax; + uint32 mRootProperties; + uint8 mBlockIDBits; ///< Number of bits to address a triangle block + uint8 mPadding[3] = { 0 }; + }; + + /// Size of the header (an empty struct is always > 0 bytes so this needs a separate variable) + static constexpr int HeaderSize = sizeof(Header); + + /// Stack size to use during DecodingContext::sWalkTree + static constexpr int StackSize = 128; + + /// Node properties + enum : uint32 + { + TRIANGLE_COUNT_BITS = 4, + TRIANGLE_COUNT_SHIFT = 28, + TRIANGLE_COUNT_MASK = (1 << TRIANGLE_COUNT_BITS) - 1, + OFFSET_BITS = 28, + OFFSET_MASK = (1 << OFFSET_BITS) - 1, + OFFSET_NON_SIGNIFICANT_BITS = 2, + OFFSET_NON_SIGNIFICANT_MASK = (1 << OFFSET_NON_SIGNIFICANT_BITS) - 1, + }; + + /// Node structure + struct Node + { + HalfFloat mBoundsMinX[4]; ///< 4 child bounding boxes + HalfFloat mBoundsMinY[4]; + HalfFloat mBoundsMinZ[4]; + HalfFloat mBoundsMaxX[4]; + HalfFloat mBoundsMaxY[4]; + HalfFloat mBoundsMaxZ[4]; + uint32 mNodeProperties[4]; ///< 4 child node properties + }; + + static_assert(sizeof(Node) == 64, "Node should be 64 bytes"); + + /// This class encodes and compresses quad tree nodes + class EncodingContext + { + public: + /// Mimics the size a call to NodeAllocate() would add to the buffer + void PrepareNodeAllocate(const AABBTreeBuilder::Node *inNode, uint64 &ioBufferSize) const + { + // We don't emit nodes for leafs + if (!inNode->HasChildren()) + return; + + // Add size of node + ioBufferSize += sizeof(Node); + } + + /// Allocate a new node for inNode. + /// Algorithm can modify the order of ioChildren to indicate in which order children should be compressed + /// Algorithm can enlarge the bounding boxes of the children during compression and returns these in outChildBoundsMin, outChildBoundsMax + /// inNodeBoundsMin, inNodeBoundsMax is the bounding box if inNode possibly widened by compressing the parent node + /// Returns size_t(-1) on error and reports the error in outError + size_t NodeAllocate(const AABBTreeBuilder::Node *inNode, Vec3Arg inNodeBoundsMin, Vec3Arg inNodeBoundsMax, Array &ioChildren, Vec3 outChildBoundsMin[NumChildrenPerNode], Vec3 outChildBoundsMax[NumChildrenPerNode], ByteBuffer &ioBuffer, const char *&outError) const + { + // We don't emit nodes for leafs + if (!inNode->HasChildren()) + return ioBuffer.size(); + + // Remember the start of the node + size_t node_start = ioBuffer.size(); + + // Fill in bounds + Node *node = ioBuffer.Allocate(); + + for (size_t i = 0; i < 4; ++i) + { + if (i < ioChildren.size()) + { + const AABBTreeBuilder::Node *this_node = ioChildren[i]; + + // Copy bounding box + node->mBoundsMinX[i] = HalfFloatConversion::FromFloat(this_node->mBounds.mMin.GetX()); + node->mBoundsMinY[i] = HalfFloatConversion::FromFloat(this_node->mBounds.mMin.GetY()); + node->mBoundsMinZ[i] = HalfFloatConversion::FromFloat(this_node->mBounds.mMin.GetZ()); + node->mBoundsMaxX[i] = HalfFloatConversion::FromFloat(this_node->mBounds.mMax.GetX()); + node->mBoundsMaxY[i] = HalfFloatConversion::FromFloat(this_node->mBounds.mMax.GetY()); + node->mBoundsMaxZ[i] = HalfFloatConversion::FromFloat(this_node->mBounds.mMax.GetZ()); + + // Store triangle count + node->mNodeProperties[i] = this_node->GetTriangleCount() << TRIANGLE_COUNT_SHIFT; + if (this_node->GetTriangleCount() >= TRIANGLE_COUNT_MASK) + { + outError = "NodeCodecQuadTreeHalfFloat: Too many triangles"; + return size_t(-1); + } + } + else + { + // Make this an invalid triangle node + node->mNodeProperties[i] = uint32(TRIANGLE_COUNT_MASK) << TRIANGLE_COUNT_SHIFT; + + // Make bounding box invalid + node->mBoundsMinX[i] = HALF_FLT_MAX; + node->mBoundsMinY[i] = HALF_FLT_MAX; + node->mBoundsMinZ[i] = HALF_FLT_MAX; + node->mBoundsMaxX[i] = HALF_FLT_MAX; + node->mBoundsMaxY[i] = HALF_FLT_MAX; + node->mBoundsMaxZ[i] = HALF_FLT_MAX; + } + } + + // Since we don't keep track of the bounding box while descending the tree, we keep the root bounds at all levels for triangle compression + for (int i = 0; i < NumChildrenPerNode; ++i) + { + outChildBoundsMin[i] = inNodeBoundsMin; + outChildBoundsMax[i] = inNodeBoundsMax; + } + + return node_start; + } + + /// Once all nodes have been added, this call finalizes all nodes by patching in the offsets of the child nodes (that were added after the node itself was added) + bool NodeFinalize(const AABBTreeBuilder::Node *inNode, size_t inNodeStart, uint inNumChildren, const size_t *inChildrenNodeStart, const size_t *inChildrenTrianglesStart, ByteBuffer &ioBuffer, const char *&outError) + { + if (!inNode->HasChildren()) + return true; + + Node *node = ioBuffer.Get(inNodeStart); + for (uint i = 0; i < inNumChildren; ++i) + { + size_t offset; + if (node->mNodeProperties[i] != 0) + { + // This is a triangle block + offset = inChildrenTrianglesStart[i]; + + // Store highest block with triangles so we can count the number of bits we need + mHighestTriangleBlock = max(mHighestTriangleBlock, offset); + } + else + { + // This is a node block + offset = inChildrenNodeStart[i]; + } + + // Store offset of next node / triangles + if (offset & OFFSET_NON_SIGNIFICANT_MASK) + { + outError = "NodeCodecQuadTreeHalfFloat: Internal Error: Offset has non-significant bits set"; + return false; + } + offset >>= OFFSET_NON_SIGNIFICANT_BITS; + if (offset > OFFSET_MASK) + { + outError = "NodeCodecQuadTreeHalfFloat: Offset too large. Too much data."; + return false; + } + node->mNodeProperties[i] |= uint32(offset); + } + + return true; + } + + /// Once all nodes have been finalized, this will finalize the header of the nodes + bool Finalize(Header *outHeader, const AABBTreeBuilder::Node *inRoot, size_t inRootNodeStart, size_t inRootTrianglesStart, const char *&outError) const + { + // Check if we can address the root node + size_t offset = inRoot->HasChildren()? inRootNodeStart : inRootTrianglesStart; + if (offset & OFFSET_NON_SIGNIFICANT_MASK) + { + outError = "NodeCodecQuadTreeHalfFloat: Internal Error: Offset has non-significant bits set"; + return false; + } + offset >>= OFFSET_NON_SIGNIFICANT_BITS; + if (offset > OFFSET_MASK) + { + outError = "NodeCodecQuadTreeHalfFloat: Offset too large. Too much data."; + return false; + } + + // If the root has triangles, we need to take that offset instead since the mHighestTriangleBlock will be zero + size_t highest_triangle_block = inRootTrianglesStart != size_t(-1)? inRootTrianglesStart : mHighestTriangleBlock; + highest_triangle_block >>= OFFSET_NON_SIGNIFICANT_BITS; + + inRoot->mBounds.mMin.StoreFloat3(&outHeader->mRootBoundsMin); + inRoot->mBounds.mMax.StoreFloat3(&outHeader->mRootBoundsMax); + outHeader->mRootProperties = uint32(offset) + (inRoot->GetTriangleCount() << TRIANGLE_COUNT_SHIFT); + outHeader->mBlockIDBits = uint8(32 - CountLeadingZeros(uint32(highest_triangle_block))); + if (inRoot->GetTriangleCount() >= TRIANGLE_COUNT_MASK) + { + outError = "NodeCodecQuadTreeHalfFloat: Too many triangles"; + return false; + } + + return true; + } + + private: + size_t mHighestTriangleBlock = 0; + }; + + /// This class decodes and decompresses quad tree nodes + class DecodingContext + { + public: + /// Get the amount of bits needed to store an ID to a triangle block + inline static uint sTriangleBlockIDBits(const Header *inHeader) + { + return inHeader->mBlockIDBits; + } + + /// Convert a triangle block ID to the start of the triangle buffer + inline static const void * sGetTriangleBlockStart(const uint8 *inBufferStart, uint inTriangleBlockID) + { + return inBufferStart + (inTriangleBlockID << OFFSET_NON_SIGNIFICANT_BITS); + } + + /// Constructor + JPH_INLINE explicit DecodingContext(const Header *inHeader) + { + // Start with the root node on the stack + mNodeStack[0] = inHeader->mRootProperties; + } + + /// Walk the node tree calling the Visitor::VisitNodes for each node encountered and Visitor::VisitTriangles for each triangle encountered + template + JPH_INLINE void WalkTree(const uint8 *inBufferStart, const TriangleContext &inTriangleContext, Visitor &ioVisitor) + { + do + { + // Test if node contains triangles + uint32 node_properties = mNodeStack[mTop]; + uint32 tri_count = node_properties >> TRIANGLE_COUNT_SHIFT; + if (tri_count == 0) + { + const Node *node = reinterpret_cast(inBufferStart + (node_properties << OFFSET_NON_SIGNIFICANT_BITS)); + + // Unpack bounds + UVec4 bounds_minxy = UVec4::sLoadInt4(reinterpret_cast(&node->mBoundsMinX[0])); + Vec4 bounds_minx = HalfFloatConversion::ToFloat(bounds_minxy); + Vec4 bounds_miny = HalfFloatConversion::ToFloat(bounds_minxy.Swizzle()); + + UVec4 bounds_minzmaxx = UVec4::sLoadInt4(reinterpret_cast(&node->mBoundsMinZ[0])); + Vec4 bounds_minz = HalfFloatConversion::ToFloat(bounds_minzmaxx); + Vec4 bounds_maxx = HalfFloatConversion::ToFloat(bounds_minzmaxx.Swizzle()); + + UVec4 bounds_maxyz = UVec4::sLoadInt4(reinterpret_cast(&node->mBoundsMaxY[0])); + Vec4 bounds_maxy = HalfFloatConversion::ToFloat(bounds_maxyz); + Vec4 bounds_maxz = HalfFloatConversion::ToFloat(bounds_maxyz.Swizzle()); + + // Load properties for 4 children + UVec4 properties = UVec4::sLoadInt4(&node->mNodeProperties[0]); + + // Check which sub nodes to visit + int num_results = ioVisitor.VisitNodes(bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz, properties, mTop); + + // Push them onto the stack + JPH_ASSERT(mTop + 4 < StackSize); + properties.StoreInt4(&mNodeStack[mTop]); + mTop += num_results; + } + else if (tri_count != TRIANGLE_COUNT_MASK) // TRIANGLE_COUNT_MASK indicates a padding node, normally we shouldn't visit these nodes but when querying with a big enough box you could touch HALF_FLT_MAX (about 65K) + { + // Node contains triangles, do individual tests + uint32 triangle_block_id = node_properties & OFFSET_MASK; + const void *triangles = sGetTriangleBlockStart(inBufferStart, triangle_block_id); + + ioVisitor.VisitTriangles(inTriangleContext, triangles, tri_count, triangle_block_id); + } + + // Check if we're done + if (ioVisitor.ShouldAbort()) + break; + + // Fetch next node until we find one that the visitor wants to see + do + --mTop; + while (mTop >= 0 && !ioVisitor.ShouldVisitNode(mTop)); + } + while (mTop >= 0); + } + + /// This can be used to have the visitor early out (ioVisitor.ShouldAbort() returns true) and later continue again (call WalkTree() again) + bool IsDoneWalking() const + { + return mTop < 0; + } + + private: + uint32 mNodeStack[StackSize]; + int mTop = 0; + }; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/AABBTree/TriangleCodec/TriangleCodecIndexed8BitPackSOA4Flags.h b/thirdparty/jolt_physics/Jolt/AABBTree/TriangleCodec/TriangleCodecIndexed8BitPackSOA4Flags.h new file mode 100644 index 000000000000..2168a5e43a70 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/AABBTree/TriangleCodec/TriangleCodecIndexed8BitPackSOA4Flags.h @@ -0,0 +1,549 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Store vertices in 64 bits and indices in 8 bits + 8 bit of flags per triangle like this: +/// +/// TriangleBlockHeader, +/// TriangleBlock (4 triangles and their flags in 16 bytes), +/// TriangleBlock... +/// [Optional] UserData (4 bytes per triangle) +/// +/// Vertices are stored: +/// +/// VertexData (1 vertex in 64 bits), +/// VertexData... +/// +/// They're compressed relative to the bounding box as provided by the node codec. +class TriangleCodecIndexed8BitPackSOA4Flags +{ +public: + class TriangleHeader + { + public: + Float3 mOffset; ///< Offset of all vertices + Float3 mScale; ///< Scale of all vertices, vertex_position = mOffset + mScale * compressed_vertex_position + }; + + /// Size of the header (an empty struct is always > 0 bytes so this needs a separate variable) + static constexpr int TriangleHeaderSize = sizeof(TriangleHeader); + + /// If this codec could return a different offset than the current buffer size when calling Pack() + static constexpr bool ChangesOffsetOnPack = false; + + /// Amount of bits per component + enum EComponentData : uint32 + { + COMPONENT_BITS = 21, + COMPONENT_MASK = (1 << COMPONENT_BITS) - 1, + }; + + /// Packed X and Y coordinate + enum EVertexXY : uint32 + { + COMPONENT_X = 0, + COMPONENT_Y1 = COMPONENT_BITS, + COMPONENT_Y1_BITS = 32 - COMPONENT_BITS, + }; + + /// Packed Z and Y coordinate + enum EVertexZY : uint32 + { + COMPONENT_Z = 0, + COMPONENT_Y2 = COMPONENT_BITS, + COMPONENT_Y2_BITS = 31 - COMPONENT_BITS, + }; + + /// A single packed vertex + struct VertexData + { + uint32 mVertexXY; + uint32 mVertexZY; + }; + + static_assert(sizeof(VertexData) == 8, "Compiler added padding"); + + /// A block of 4 triangles + struct TriangleBlock + { + uint8 mIndices[3][4]; ///< 8 bit indices to triangle vertices for 4 triangles in the form mIndices[vertex][triangle] where vertex in [0, 2] and triangle in [0, 3] + uint8 mFlags[4]; ///< Triangle flags (could contain material and active edges) + }; + + static_assert(sizeof(TriangleBlock) == 16, "Compiler added padding"); + + enum ETriangleBlockHeaderFlags : uint32 + { + OFFSET_TO_VERTICES_BITS = 29, ///< Offset from current block to start of vertices in bytes + OFFSET_TO_VERTICES_MASK = (1 << OFFSET_TO_VERTICES_BITS) - 1, + OFFSET_NON_SIGNIFICANT_BITS = 2, ///< The offset from the current block to the start of the vertices must be a multiple of 4 bytes + OFFSET_NON_SIGNIFICANT_MASK = (1 << OFFSET_NON_SIGNIFICANT_BITS) - 1, + OFFSET_TO_USERDATA_BITS = 3, ///< When user data is stored, this is the number of blocks to skip to get to the user data (0 = no user data) + OFFSET_TO_USERDATA_MASK = (1 << OFFSET_TO_USERDATA_BITS) - 1, + }; + + /// A triangle header, will be followed by one or more TriangleBlocks + struct TriangleBlockHeader + { + const VertexData * GetVertexData() const { return reinterpret_cast(reinterpret_cast(this) + ((mFlags & OFFSET_TO_VERTICES_MASK) << OFFSET_NON_SIGNIFICANT_BITS)); } + const TriangleBlock * GetTriangleBlock() const { return reinterpret_cast(reinterpret_cast(this) + sizeof(TriangleBlockHeader)); } + const uint32 * GetUserData() const { uint32 offset = mFlags >> OFFSET_TO_VERTICES_BITS; return offset == 0? nullptr : reinterpret_cast(GetTriangleBlock() + offset); } + + uint32 mFlags; + }; + + static_assert(sizeof(TriangleBlockHeader) == 4, "Compiler added padding"); + + /// This class is used to validate that the triangle data will not be degenerate after compression + class ValidationContext + { + public: + /// Constructor + ValidationContext(const IndexedTriangleList &inTriangles, const VertexList &inVertices) : + mVertices(inVertices) + { + // Only used the referenced triangles, just like EncodingContext::Finalize does + for (const IndexedTriangle &i : inTriangles) + for (uint32 idx : i.mIdx) + mBounds.Encapsulate(Vec3(inVertices[idx])); + } + + /// Test if a triangle will be degenerate after quantization + bool IsDegenerate(const IndexedTriangle &inTriangle) const + { + // Quantize the triangle in the same way as EncodingContext::Finalize does + UVec4 quantized_vertex[3]; + Vec3 compress_scale = Vec3::sReplicate(COMPONENT_MASK) / Vec3::sMax(mBounds.GetSize(), Vec3::sReplicate(1.0e-20f)); + for (int i = 0; i < 3; ++i) + quantized_vertex[i] = ((Vec3(mVertices[inTriangle.mIdx[i]]) - mBounds.mMin) * compress_scale + Vec3::sReplicate(0.5f)).ToInt(); + return quantized_vertex[0] == quantized_vertex[1] || quantized_vertex[1] == quantized_vertex[2] || quantized_vertex[0] == quantized_vertex[2]; + } + + private: + const VertexList & mVertices; + AABox mBounds; + }; + + /// This class is used to encode and compress triangle data into a byte buffer + class EncodingContext + { + public: + /// Indicates a vertex hasn't been seen yet in the triangle list + static constexpr uint32 cNotFound = 0xffffffff; + + /// Construct the encoding context + explicit EncodingContext(const VertexList &inVertices) : + mVertexMap(inVertices.size(), cNotFound) + { + } + + /// Mimics the size a call to Pack() would add to the buffer + void PreparePack(const IndexedTriangle *inTriangles, uint inNumTriangles, bool inStoreUserData, uint64 &ioBufferSize) + { + // Add triangle block header + ioBufferSize += sizeof(TriangleBlockHeader); + + // Compute first vertex that this batch will use (ensuring there's enough room if none of the vertices are shared) + uint start_vertex = Clamp((int)mVertexCount - 256 + (int)inNumTriangles * 3, 0, (int)mVertexCount); + + // Pack vertices + uint padded_triangle_count = AlignUp(inNumTriangles, 4); + for (uint t = 0; t < padded_triangle_count; t += 4) + { + // Add triangle block header + ioBufferSize += sizeof(TriangleBlock); + + for (uint vertex_nr = 0; vertex_nr < 3; ++vertex_nr) + for (uint block_tri_idx = 0; block_tri_idx < 4; ++block_tri_idx) + { + // Fetch vertex index. Create degenerate triangles for padding triangles. + bool triangle_available = t + block_tri_idx < inNumTriangles; + uint32 src_vertex_index = triangle_available? inTriangles[t + block_tri_idx].mIdx[vertex_nr] : inTriangles[inNumTriangles - 1].mIdx[0]; + + // Check if we've seen this vertex before and if it is in the range that we can encode + uint32 &vertex_index = mVertexMap[src_vertex_index]; + if (vertex_index == cNotFound || vertex_index < start_vertex) + { + // Add vertex + vertex_index = mVertexCount; + mVertexCount++; + } + } + } + + // Add user data + if (inStoreUserData) + ioBufferSize += inNumTriangles * sizeof(uint32); + } + + /// Mimics the size the Finalize() call would add to ioBufferSize + void FinalizePreparePack(uint64 &ioBufferSize) + { + // Remember where the vertices are going to start in the output buffer + JPH_ASSERT(IsAligned(ioBufferSize, 4)); + mVerticesStartIdx = size_t(ioBufferSize); + + // Add vertices to buffer + ioBufferSize += uint64(mVertexCount) * sizeof(VertexData); + + // Reserve the amount of memory we need for the vertices + mVertices.reserve(mVertexCount); + + // Set vertex map back to 'not found' + for (uint32 &v : mVertexMap) + v = cNotFound; + } + + /// Pack the triangles in inContainer to ioBuffer. This stores the mMaterialIndex of a triangle in the 8 bit flags. + /// Returns size_t(-1) on error. + size_t Pack(const IndexedTriangle *inTriangles, uint inNumTriangles, bool inStoreUserData, ByteBuffer &ioBuffer, const char *&outError) + { + JPH_ASSERT(inNumTriangles > 0); + + // Determine position of triangles start + size_t triangle_block_start = ioBuffer.size(); + + // Allocate triangle block header + TriangleBlockHeader *header = ioBuffer.Allocate(); + + // Compute first vertex that this batch will use (ensuring there's enough room if none of the vertices are shared) + uint start_vertex = Clamp((int)mVertices.size() - 256 + (int)inNumTriangles * 3, 0, (int)mVertices.size()); + + // Store the start vertex offset relative to TriangleBlockHeader + size_t offset_to_vertices = mVerticesStartIdx - triangle_block_start + size_t(start_vertex) * sizeof(VertexData); + if (offset_to_vertices & OFFSET_NON_SIGNIFICANT_MASK) + { + outError = "TriangleCodecIndexed8BitPackSOA4Flags: Internal Error: Offset has non-significant bits set"; + return size_t(-1); + } + offset_to_vertices >>= OFFSET_NON_SIGNIFICANT_BITS; + if (offset_to_vertices > OFFSET_TO_VERTICES_MASK) + { + outError = "TriangleCodecIndexed8BitPackSOA4Flags: Offset to vertices doesn't fit. Too much data."; + return size_t(-1); + } + header->mFlags = uint32(offset_to_vertices); + + // When we store user data we need to store the offset to the user data in TriangleBlocks + uint padded_triangle_count = AlignUp(inNumTriangles, 4); + if (inStoreUserData) + { + uint32 num_blocks = padded_triangle_count >> 2; + JPH_ASSERT(num_blocks <= OFFSET_TO_USERDATA_MASK); + header->mFlags |= num_blocks << OFFSET_TO_VERTICES_BITS; + } + + // Pack vertices + for (uint t = 0; t < padded_triangle_count; t += 4) + { + TriangleBlock *block = ioBuffer.Allocate(); + for (uint vertex_nr = 0; vertex_nr < 3; ++vertex_nr) + for (uint block_tri_idx = 0; block_tri_idx < 4; ++block_tri_idx) + { + // Fetch vertex index. Create degenerate triangles for padding triangles. + bool triangle_available = t + block_tri_idx < inNumTriangles; + uint32 src_vertex_index = triangle_available? inTriangles[t + block_tri_idx].mIdx[vertex_nr] : inTriangles[inNumTriangles - 1].mIdx[0]; + + // Check if we've seen this vertex before and if it is in the range that we can encode + uint32 &vertex_index = mVertexMap[src_vertex_index]; + if (vertex_index == cNotFound || vertex_index < start_vertex) + { + // Add vertex + vertex_index = (uint32)mVertices.size(); + mVertices.push_back(src_vertex_index); + } + + // Store vertex index + uint32 vertex_offset = vertex_index - start_vertex; + if (vertex_offset > 0xff) + { + outError = "TriangleCodecIndexed8BitPackSOA4Flags: Offset doesn't fit in 8 bit"; + return size_t(-1); + } + block->mIndices[vertex_nr][block_tri_idx] = (uint8)vertex_offset; + + // Store flags + uint32 flags = triangle_available? inTriangles[t + block_tri_idx].mMaterialIndex : 0; + if (flags > 0xff) + { + outError = "TriangleCodecIndexed8BitPackSOA4Flags: Material index doesn't fit in 8 bit"; + return size_t(-1); + } + block->mFlags[block_tri_idx] = (uint8)flags; + } + } + + // Store user data + if (inStoreUserData) + { + uint32 *user_data = ioBuffer.Allocate(inNumTriangles); + for (uint t = 0; t < inNumTriangles; ++t) + user_data[t] = inTriangles[t].mUserData; + } + + return triangle_block_start; + } + + /// After all triangles have been packed, this finalizes the header and triangle buffer + void Finalize(const VertexList &inVertices, TriangleHeader *ioHeader, ByteBuffer &ioBuffer) const + { + // Assert that our reservations were correct + JPH_ASSERT(mVertices.size() == mVertexCount); + JPH_ASSERT(ioBuffer.size() == mVerticesStartIdx); + + // Check if anything to do + if (mVertices.empty()) + return; + + // Calculate bounding box + AABox bounds; + for (uint32 v : mVertices) + bounds.Encapsulate(Vec3(inVertices[v])); + + // Compress vertices + VertexData *vertices = ioBuffer.Allocate(mVertices.size()); + Vec3 compress_scale = Vec3::sReplicate(COMPONENT_MASK) / Vec3::sMax(bounds.GetSize(), Vec3::sReplicate(1.0e-20f)); + for (uint32 v : mVertices) + { + UVec4 c = ((Vec3(inVertices[v]) - bounds.mMin) * compress_scale + Vec3::sReplicate(0.5f)).ToInt(); + JPH_ASSERT(c.GetX() <= COMPONENT_MASK); + JPH_ASSERT(c.GetY() <= COMPONENT_MASK); + JPH_ASSERT(c.GetZ() <= COMPONENT_MASK); + vertices->mVertexXY = c.GetX() + (c.GetY() << COMPONENT_Y1); + vertices->mVertexZY = c.GetZ() + ((c.GetY() >> COMPONENT_Y1_BITS) << COMPONENT_Y2); + ++vertices; + } + + // Store decompression information + bounds.mMin.StoreFloat3(&ioHeader->mOffset); + (bounds.GetSize() / Vec3::sReplicate(COMPONENT_MASK)).StoreFloat3(&ioHeader->mScale); + } + + private: + using VertexMap = Array; + + uint32 mVertexCount = 0; ///< Number of vertices calculated during PreparePack + size_t mVerticesStartIdx = 0; ///< Start of the vertices in the output buffer, calculated during PreparePack + Array mVertices; ///< Output vertices as an index into the original vertex list (inVertices), sorted according to occurrence + VertexMap mVertexMap; ///< Maps from the original mesh vertex index (inVertices) to the index in our output vertices (mVertices) + }; + + /// This class is used to decode and decompress triangle data packed by the EncodingContext + class DecodingContext + { + private: + /// Private helper functions to unpack the 1 vertex of 4 triangles (outX contains the x coordinate of triangle 0 .. 3 etc.) + JPH_INLINE void Unpack(const VertexData *inVertices, UVec4Arg inIndex, Vec4 &outX, Vec4 &outY, Vec4 &outZ) const + { + // Get compressed data + UVec4 c1 = UVec4::sGatherInt4<8>(&inVertices->mVertexXY, inIndex); + UVec4 c2 = UVec4::sGatherInt4<8>(&inVertices->mVertexZY, inIndex); + + // Unpack the x y and z component + UVec4 xc = UVec4::sAnd(c1, UVec4::sReplicate(COMPONENT_MASK)); + UVec4 yc = UVec4::sOr(c1.LogicalShiftRight(), c2.LogicalShiftRight().LogicalShiftLeft()); + UVec4 zc = UVec4::sAnd(c2, UVec4::sReplicate(COMPONENT_MASK)); + + // Convert to float + outX = Vec4::sFusedMultiplyAdd(xc.ToFloat(), mScaleX, mOffsetX); + outY = Vec4::sFusedMultiplyAdd(yc.ToFloat(), mScaleY, mOffsetY); + outZ = Vec4::sFusedMultiplyAdd(zc.ToFloat(), mScaleZ, mOffsetZ); + } + + public: + JPH_INLINE explicit DecodingContext(const TriangleHeader *inHeader) : + mOffsetX(Vec4::sReplicate(inHeader->mOffset.x)), + mOffsetY(Vec4::sReplicate(inHeader->mOffset.y)), + mOffsetZ(Vec4::sReplicate(inHeader->mOffset.z)), + mScaleX(Vec4::sReplicate(inHeader->mScale.x)), + mScaleY(Vec4::sReplicate(inHeader->mScale.y)), + mScaleZ(Vec4::sReplicate(inHeader->mScale.z)) + { + } + + /// Unpacks triangles in the format t1v1,t1v2,t1v3, t2v1,t2v2,t2v3, ... + JPH_INLINE void Unpack(const void *inTriangleStart, uint32 inNumTriangles, Vec3 *outTriangles) const + { + JPH_ASSERT(inNumTriangles > 0); + const TriangleBlockHeader *header = reinterpret_cast(inTriangleStart); + const VertexData *vertices = header->GetVertexData(); + const TriangleBlock *t = header->GetTriangleBlock(); + const TriangleBlock *end = t + ((inNumTriangles + 3) >> 2); + + int triangles_left = inNumTriangles; + + do + { + // Get the indices for the three vertices (reads 4 bytes extra, but these are the flags so that's ok) + UVec4 indices = UVec4::sLoadInt4(reinterpret_cast(&t->mIndices[0])); + UVec4 iv1 = indices.Expand4Byte0(); + UVec4 iv2 = indices.Expand4Byte4(); + UVec4 iv3 = indices.Expand4Byte8(); + + // Decompress the triangle data + Vec4 v1x, v1y, v1z, v2x, v2y, v2z, v3x, v3y, v3z; + Unpack(vertices, iv1, v1x, v1y, v1z); + Unpack(vertices, iv2, v2x, v2y, v2z); + Unpack(vertices, iv3, v3x, v3y, v3z); + + // Transpose it so we get normal vectors + Mat44 v1 = Mat44(v1x, v1y, v1z, Vec4::sZero()).Transposed(); + Mat44 v2 = Mat44(v2x, v2y, v2z, Vec4::sZero()).Transposed(); + Mat44 v3 = Mat44(v3x, v3y, v3z, Vec4::sZero()).Transposed(); + + // Store triangle data + for (int i = 0; i < 4 && triangles_left > 0; ++i, --triangles_left) + { + *outTriangles++ = v1.GetColumn3(i); + *outTriangles++ = v2.GetColumn3(i); + *outTriangles++ = v3.GetColumn3(i); + } + + ++t; + } + while (t < end); + } + + /// Tests a ray against the packed triangles + JPH_INLINE float TestRay(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, const void *inTriangleStart, uint32 inNumTriangles, float inClosest, uint32 &outClosestTriangleIndex) const + { + JPH_ASSERT(inNumTriangles > 0); + const TriangleBlockHeader *header = reinterpret_cast(inTriangleStart); + const VertexData *vertices = header->GetVertexData(); + const TriangleBlock *t = header->GetTriangleBlock(); + const TriangleBlock *end = t + ((inNumTriangles + 3) >> 2); + + Vec4 closest = Vec4::sReplicate(inClosest); + UVec4 closest_triangle_idx = UVec4::sZero(); + + UVec4 start_triangle_idx = UVec4::sZero(); + do + { + // Get the indices for the three vertices (reads 4 bytes extra, but these are the flags so that's ok) + UVec4 indices = UVec4::sLoadInt4(reinterpret_cast(&t->mIndices[0])); + UVec4 iv1 = indices.Expand4Byte0(); + UVec4 iv2 = indices.Expand4Byte4(); + UVec4 iv3 = indices.Expand4Byte8(); + + // Decompress the triangle data + Vec4 v1x, v1y, v1z, v2x, v2y, v2z, v3x, v3y, v3z; + Unpack(vertices, iv1, v1x, v1y, v1z); + Unpack(vertices, iv2, v2x, v2y, v2z); + Unpack(vertices, iv3, v3x, v3y, v3z); + + // Perform ray vs triangle test + Vec4 distance = RayTriangle4(inRayOrigin, inRayDirection, v1x, v1y, v1z, v2x, v2y, v2z, v3x, v3y, v3z); + + // Update closest with the smaller values + UVec4 smaller = Vec4::sLess(distance, closest); + closest = Vec4::sSelect(closest, distance, smaller); + + // Update triangle index with the smallest values + UVec4 triangle_idx = start_triangle_idx + UVec4(0, 1, 2, 3); + closest_triangle_idx = UVec4::sSelect(closest_triangle_idx, triangle_idx, smaller); + + // Next block + ++t; + start_triangle_idx += UVec4::sReplicate(4); + } + while (t < end); + + // Get the smallest component + Vec4::sSort4(closest, closest_triangle_idx); + outClosestTriangleIndex = closest_triangle_idx.GetX(); + return closest.GetX(); + } + + /// Decode a single triangle + inline void GetTriangle(const void *inTriangleStart, uint32 inTriangleIdx, Vec3 &outV1, Vec3 &outV2, Vec3 &outV3) const + { + const TriangleBlockHeader *header = reinterpret_cast(inTriangleStart); + const VertexData *vertices = header->GetVertexData(); + const TriangleBlock *block = header->GetTriangleBlock() + (inTriangleIdx >> 2); + uint32 block_triangle_idx = inTriangleIdx & 0b11; + + // Get the 3 vertices + const VertexData &v1 = vertices[block->mIndices[0][block_triangle_idx]]; + const VertexData &v2 = vertices[block->mIndices[1][block_triangle_idx]]; + const VertexData &v3 = vertices[block->mIndices[2][block_triangle_idx]]; + + // Pack the vertices + UVec4 c1(v1.mVertexXY, v2.mVertexXY, v3.mVertexXY, 0); + UVec4 c2(v1.mVertexZY, v2.mVertexZY, v3.mVertexZY, 0); + + // Unpack the x y and z component + UVec4 xc = UVec4::sAnd(c1, UVec4::sReplicate(COMPONENT_MASK)); + UVec4 yc = UVec4::sOr(c1.LogicalShiftRight(), c2.LogicalShiftRight().LogicalShiftLeft()); + UVec4 zc = UVec4::sAnd(c2, UVec4::sReplicate(COMPONENT_MASK)); + + // Convert to float + Vec4 vx = Vec4::sFusedMultiplyAdd(xc.ToFloat(), mScaleX, mOffsetX); + Vec4 vy = Vec4::sFusedMultiplyAdd(yc.ToFloat(), mScaleY, mOffsetY); + Vec4 vz = Vec4::sFusedMultiplyAdd(zc.ToFloat(), mScaleZ, mOffsetZ); + + // Transpose it so we get normal vectors + Mat44 trans = Mat44(vx, vy, vz, Vec4::sZero()).Transposed(); + outV1 = trans.GetAxisX(); + outV2 = trans.GetAxisY(); + outV3 = trans.GetAxisZ(); + } + + /// Get user data for a triangle + JPH_INLINE uint32 GetUserData(const void *inTriangleStart, uint32 inTriangleIdx) const + { + const TriangleBlockHeader *header = reinterpret_cast(inTriangleStart); + const uint32 *user_data = header->GetUserData(); + return user_data != nullptr? user_data[inTriangleIdx] : 0; + } + + /// Get flags for entire triangle block + JPH_INLINE static void sGetFlags(const void *inTriangleStart, uint32 inNumTriangles, uint8 *outTriangleFlags) + { + JPH_ASSERT(inNumTriangles > 0); + const TriangleBlockHeader *header = reinterpret_cast(inTriangleStart); + const TriangleBlock *t = header->GetTriangleBlock(); + const TriangleBlock *end = t + ((inNumTriangles + 3) >> 2); + + int triangles_left = inNumTriangles; + do + { + for (int i = 0; i < 4 && triangles_left > 0; ++i, --triangles_left) + *outTriangleFlags++ = t->mFlags[i]; + + ++t; + } + while (t < end); + } + + /// Get flags for a particular triangle + JPH_INLINE static uint8 sGetFlags(const void *inTriangleStart, int inTriangleIndex) + { + const TriangleBlockHeader *header = reinterpret_cast(inTriangleStart); + const TriangleBlock *first_block = header->GetTriangleBlock(); + return first_block[inTriangleIndex >> 2].mFlags[inTriangleIndex & 0b11]; + } + + /// Unpacks triangles and flags, convenience function + JPH_INLINE void Unpack(const void *inTriangleStart, uint32 inNumTriangles, Vec3 *outTriangles, uint8 *outTriangleFlags) const + { + Unpack(inTriangleStart, inNumTriangles, outTriangles); + sGetFlags(inTriangleStart, inNumTriangles, outTriangleFlags); + } + + private: + Vec4 mOffsetX; + Vec4 mOffsetY; + Vec4 mOffsetZ; + Vec4 mScaleX; + Vec4 mScaleY; + Vec4 mScaleZ; + }; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/ConfigurationString.h b/thirdparty/jolt_physics/Jolt/ConfigurationString.h new file mode 100644 index 000000000000..55e5d8c35ff8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ConfigurationString.h @@ -0,0 +1,94 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Construct a string that lists the most important configuration settings +inline const char *GetConfigurationString() +{ + return JPH_IF_SINGLE_PRECISION_ELSE("Single", "Double") " precision " +#if defined(JPH_CPU_X86) + "x86 " +#elif defined(JPH_CPU_ARM) + "ARM " +#elif defined(JPH_PLATFORM_WASM) + "WASM " +#endif +#if JPH_CPU_ADDRESS_BITS == 64 + "64-bit " +#elif JPH_CPU_ADDRESS_BITS == 32 + "32-bit " +#endif + "with instructions: " +#ifdef JPH_USE_NEON + "NEON " +#endif +#ifdef JPH_USE_SSE + "SSE2 " +#endif +#ifdef JPH_USE_SSE4_1 + "SSE4.1 " +#endif +#ifdef JPH_USE_SSE4_2 + "SSE4.2 " +#endif +#ifdef JPH_USE_AVX + "AVX " +#endif +#ifdef JPH_USE_AVX2 + "AVX2 " +#endif +#ifdef JPH_USE_AVX512 + "AVX512 " +#endif +#ifdef JPH_USE_F16C + "F16C " +#endif +#ifdef JPH_USE_LZCNT + "LZCNT " +#endif +#ifdef JPH_USE_TZCNT + "TZCNT " +#endif +#ifdef JPH_USE_FMADD + "FMADD " +#endif +#ifdef JPH_CROSS_PLATFORM_DETERMINISTIC + "(Cross Platform Deterministic) " +#endif +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + "(FP Exceptions) " +#endif +#ifdef JPH_DEBUG_RENDERER + "(Debug Renderer) " +#endif +#ifdef JPH_PROFILE_ENABLED + "(Profile) " +#endif +#if defined(JPH_OBJECT_LAYER_BITS) && JPH_OBJECT_LAYER_BITS == 32 + "(32-bit ObjectLayer) " +#else + "(16-bit ObjectLayer) " +#endif +#ifdef JPH_ENABLE_ASSERTS + "(Assertions) " +#endif +#ifdef JPH_OBJECT_STREAM + "(ObjectStream) " +#endif +#ifdef JPH_DEBUG + "(Debug) " +#endif +#if defined(__cpp_rtti) && __cpp_rtti + "(C++ RTTI) " +#endif +#if defined(__cpp_exceptions) && __cpp_exceptions + "(C++ Exceptions) " +#endif + ; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/ARMNeon.h b/thirdparty/jolt_physics/Jolt/Core/ARMNeon.h new file mode 100644 index 000000000000..6273945dc067 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/ARMNeon.h @@ -0,0 +1,94 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifdef JPH_USE_NEON + +// Constructing NEON values +#ifdef JPH_COMPILER_MSVC + #define JPH_NEON_INT32x4(v1, v2, v3, v4) { int64_t(v1) + (int64_t(v2) << 32), int64_t(v3) + (int64_t(v4) << 32) } + #define JPH_NEON_UINT32x4(v1, v2, v3, v4) { uint64_t(v1) + (uint64_t(v2) << 32), uint64_t(v3) + (uint64_t(v4) << 32) } + #define JPH_NEON_INT8x16(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16) { int64_t(v1) + (int64_t(v2) << 8) + (int64_t(v3) << 16) + (int64_t(v4) << 24) + (int64_t(v5) << 32) + (int64_t(v6) << 40) + (int64_t(v7) << 48) + (int64_t(v8) << 56), int64_t(v9) + (int64_t(v10) << 8) + (int64_t(v11) << 16) + (int64_t(v12) << 24) + (int64_t(v13) << 32) + (int64_t(v14) << 40) + (int64_t(v15) << 48) + (int64_t(v16) << 56) } + #define JPH_NEON_UINT8x16(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16) { uint64_t(v1) + (uint64_t(v2) << 8) + (uint64_t(v3) << 16) + (uint64_t(v4) << 24) + (uint64_t(v5) << 32) + (uint64_t(v6) << 40) + (uint64_t(v7) << 48) + (uint64_t(v8) << 56), uint64_t(v9) + (uint64_t(v10) << 8) + (uint64_t(v11) << 16) + (uint64_t(v12) << 24) + (uint64_t(v13) << 32) + (uint64_t(v14) << 40) + (uint64_t(v15) << 48) + (uint64_t(v16) << 56) } +#else + #define JPH_NEON_INT32x4(v1, v2, v3, v4) { v1, v2, v3, v4 } + #define JPH_NEON_UINT32x4(v1, v2, v3, v4) { v1, v2, v3, v4 } + #define JPH_NEON_INT8x16(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16) { v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16 } + #define JPH_NEON_UINT8x16(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16) { v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16 } +#endif + +// MSVC and GCC prior to version 12 don't define __builtin_shufflevector +#if defined(JPH_COMPILER_MSVC) || (defined(JPH_COMPILER_GCC) && __GNUC__ < 12) + JPH_NAMESPACE_BEGIN + + // Generic shuffle vector template + template + JPH_INLINE float32x4_t NeonShuffleFloat32x4(float32x4_t inV1, float32x4_t inV2) + { + float32x4_t ret; + ret = vmovq_n_f32(vgetq_lane_f32(I1 >= 4? inV2 : inV1, I1 & 0b11)); + ret = vsetq_lane_f32(vgetq_lane_f32(I2 >= 4? inV2 : inV1, I2 & 0b11), ret, 1); + ret = vsetq_lane_f32(vgetq_lane_f32(I3 >= 4? inV2 : inV1, I3 & 0b11), ret, 2); + ret = vsetq_lane_f32(vgetq_lane_f32(I4 >= 4? inV2 : inV1, I4 & 0b11), ret, 3); + return ret; + } + + // Specializations + template <> + JPH_INLINE float32x4_t NeonShuffleFloat32x4<0, 1, 2, 2>(float32x4_t inV1, float32x4_t inV2) + { + return vcombine_f32(vget_low_f32(inV1), vdup_lane_f32(vget_high_f32(inV1), 0)); + } + + template <> + JPH_INLINE float32x4_t NeonShuffleFloat32x4<0, 1, 3, 3>(float32x4_t inV1, float32x4_t inV2) + { + return vcombine_f32(vget_low_f32(inV1), vdup_lane_f32(vget_high_f32(inV1), 1)); + } + + template <> + JPH_INLINE float32x4_t NeonShuffleFloat32x4<0, 1, 2, 3>(float32x4_t inV1, float32x4_t inV2) + { + return inV1; + } + + template <> + JPH_INLINE float32x4_t NeonShuffleFloat32x4<1, 0, 3, 2>(float32x4_t inV1, float32x4_t inV2) + { + return vcombine_f32(vrev64_f32(vget_low_f32(inV1)), vrev64_f32(vget_high_f32(inV1))); + } + + template <> + JPH_INLINE float32x4_t NeonShuffleFloat32x4<2, 2, 1, 0>(float32x4_t inV1, float32x4_t inV2) + { + return vcombine_f32(vdup_lane_f32(vget_high_f32(inV1), 0), vrev64_f32(vget_low_f32(inV1))); + } + + template <> + JPH_INLINE float32x4_t NeonShuffleFloat32x4<2, 3, 0, 1>(float32x4_t inV1, float32x4_t inV2) + { + return vcombine_f32(vget_high_f32(inV1), vget_low_f32(inV1)); + } + + // Used extensively by cross product + template <> + JPH_INLINE float32x4_t NeonShuffleFloat32x4<1, 2, 0, 0>(float32x4_t inV1, float32x4_t inV2) + { + static uint8x16_t table = JPH_NEON_UINT8x16(0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x00, 0x01, 0x02, 0x03, 0x00, 0x01, 0x02, 0x03); + return vreinterpretq_f32_u8(vqtbl1q_u8(vreinterpretq_u8_f32(inV1), table)); + } + + // Shuffle a vector + #define JPH_NEON_SHUFFLE_F32x4(vec1, vec2, index1, index2, index3, index4) NeonShuffleFloat32x4(vec1, vec2) + #define JPH_NEON_SHUFFLE_U32x4(vec1, vec2, index1, index2, index3, index4) vreinterpretq_u32_f32((NeonShuffleFloat32x4(vreinterpretq_f32_u32(vec1), vreinterpretq_f32_u32(vec2)))) + + JPH_NAMESPACE_END +#else + // Shuffle a vector + #define JPH_NEON_SHUFFLE_F32x4(vec1, vec2, index1, index2, index3, index4) __builtin_shufflevector(vec1, vec2, index1, index2, index3, index4) + #define JPH_NEON_SHUFFLE_U32x4(vec1, vec2, index1, index2, index3, index4) __builtin_shufflevector(vec1, vec2, index1, index2, index3, index4) +#endif + +#endif // JPH_USE_NEON diff --git a/thirdparty/jolt_physics/Jolt/Core/Array.h b/thirdparty/jolt_physics/Jolt/Core/Array.h new file mode 100644 index 000000000000..dd0a9e1ea040 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/Array.h @@ -0,0 +1,604 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +#ifdef JPH_USE_STD_VECTOR + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +template > using Array = std::vector; + +JPH_NAMESPACE_END + +#else + +JPH_NAMESPACE_BEGIN + +/// Simple replacement for std::vector +/// +/// Major differences: +/// - Memory is not initialized to zero (this was causing a lot of page faults when deserializing large MeshShapes / HeightFieldShapes) +/// - Iterators are simple pointers (for now) +/// - No exception safety +/// - No specialization like std::vector has +/// - Not all functions have been implemented +template > +class [[nodiscard]] Array : private Allocator +{ +public: + using value_type = T; + using allocator_type = Allocator; + using size_type = size_t; + using difference_type = typename Allocator::difference_type; + using pointer = T *; + using const_pointer = const T *; + using reference = T &; + using const_reference = const T &; + + using const_iterator = const T *; + using iterator = T *; + +private: + /// Move elements from one location to another + inline void move(pointer inDestination, pointer inSource, size_type inCount) + { + if constexpr (std::is_trivially_copyable()) + memmove(inDestination, inSource, inCount * sizeof(T)); + else + { + if (inDestination < inSource) + { + for (T *destination_end = inDestination + inCount; inDestination < destination_end; ++inDestination, ++inSource) + { + ::new (inDestination) T(std::move(*inSource)); + inSource->~T(); + } + } + else + { + for (T *destination = inDestination + inCount - 1, *source = inSource + inCount - 1; destination >= inDestination; --destination, --source) + { + ::new (destination) T(std::move(*source)); + source->~T(); + } + } + } + } + + /// Reallocate the data block to inNewCapacity + inline void reallocate(size_type inNewCapacity) + { + JPH_ASSERT(inNewCapacity > 0 && inNewCapacity >= mSize); + + pointer ptr; + if constexpr (AllocatorHasReallocate::sValue) + { + // Reallocate data block + ptr = get_allocator().reallocate(mElements, mCapacity, inNewCapacity); + } + else + { + // Copy data to a new location + ptr = get_allocator().allocate(inNewCapacity); + if (mElements != nullptr) + { + move(ptr, mElements, mSize); + get_allocator().deallocate(mElements, mCapacity); + } + } + mElements = ptr; + mCapacity = inNewCapacity; + } + + /// Destruct elements [inStart, inEnd - 1] + inline void destruct(size_type inStart, size_type inEnd) + { + if constexpr (!std::is_trivially_destructible()) + if (inStart < inEnd) + for (T *element = mElements + inStart, *element_end = mElements + inEnd; element < element_end; ++element) + element->~T(); + } + +public: + /// Reserve array space + inline void reserve(size_type inNewSize) + { + if (mCapacity < inNewSize) + reallocate(inNewSize); + } + + /// Resize array to new length + inline void resize(size_type inNewSize) + { + destruct(inNewSize, mSize); + reserve(inNewSize); + + if constexpr (!std::is_trivially_constructible()) + for (T *element = mElements + mSize, *element_end = mElements + inNewSize; element < element_end; ++element) + ::new (element) T; + mSize = inNewSize; + } + + /// Resize array to new length and initialize all elements with inValue + inline void resize(size_type inNewSize, const T &inValue) + { + JPH_ASSERT(&inValue < mElements || &inValue >= mElements + mSize, "Can't pass an element from the array to resize"); + + destruct(inNewSize, mSize); + reserve(inNewSize); + + for (T *element = mElements + mSize, *element_end = mElements + inNewSize; element < element_end; ++element) + ::new (element) T(inValue); + mSize = inNewSize; + } + + /// Destruct all elements and set length to zero + inline void clear() + { + destruct(0, mSize); + mSize = 0; + } + +private: + /// Grow the array by at least inAmount elements + inline void grow(size_type inAmount = 1) + { + size_type min_size = mSize + inAmount; + if (min_size > mCapacity) + { + size_type new_capacity = max(min_size, mCapacity * 2); + reserve(new_capacity); + } + } + + /// Free memory + inline void free() + { + get_allocator().deallocate(mElements, mCapacity); + mElements = nullptr; + mCapacity = 0; + } + + /// Destroy all elements and free memory + inline void destroy() + { + if (mElements != nullptr) + { + clear(); + free(); + } + } + +public: + /// Replace the contents of this array with inBegin .. inEnd + template + inline void assign(Iterator inBegin, Iterator inEnd) + { + clear(); + reserve(size_type(std::distance(inBegin, inEnd))); + + for (Iterator element = inBegin; element != inEnd; ++element) + ::new (&mElements[mSize++]) T(*element); + } + + /// Replace the contents of this array with inList + inline void assign(std::initializer_list inList) + { + clear(); + reserve(size_type(inList.size())); + + for (const T &v : inList) + ::new (&mElements[mSize++]) T(v); + } + + /// Default constructor + Array() = default; + + /// Constructor with allocator + explicit inline Array(const Allocator &inAllocator) : + Allocator(inAllocator) + { + } + + /// Constructor with length + explicit inline Array(size_type inLength, const Allocator &inAllocator = { }) : + Allocator(inAllocator) + { + resize(inLength); + } + + /// Constructor with length and value + inline Array(size_type inLength, const T &inValue, const Allocator &inAllocator = { }) : + Allocator(inAllocator) + { + resize(inLength, inValue); + } + + /// Constructor from initializer list + inline Array(std::initializer_list inList, const Allocator &inAllocator = { }) : + Allocator(inAllocator) + { + assign(inList); + } + + /// Constructor from iterator + inline Array(const_iterator inBegin, const_iterator inEnd, const Allocator &inAllocator = { }) : + Allocator(inAllocator) + { + assign(inBegin, inEnd); + } + + /// Copy constructor + inline Array(const Array &inRHS) : + Allocator(inRHS.get_allocator()) + { + assign(inRHS.begin(), inRHS.end()); + } + + /// Move constructor + inline Array(Array &&inRHS) noexcept : + Allocator(std::move(inRHS.get_allocator())), + mSize(inRHS.mSize), + mCapacity(inRHS.mCapacity), + mElements(inRHS.mElements) + { + inRHS.mSize = 0; + inRHS.mCapacity = 0; + inRHS.mElements = nullptr; + } + + /// Destruct all elements + inline ~Array() + { + destroy(); + } + + /// Get the allocator + inline Allocator & get_allocator() + { + return *this; + } + + inline const Allocator &get_allocator() const + { + return *this; + } + + /// Add element to the back of the array + inline void push_back(const T &inValue) + { + JPH_ASSERT(&inValue < mElements || &inValue >= mElements + mSize, "Can't pass an element from the array to push_back"); + + grow(); + + T *element = mElements + mSize++; + ::new (element) T(inValue); + } + + inline void push_back(T &&inValue) + { + grow(); + + T *element = mElements + mSize++; + ::new (element) T(std::move(inValue)); + } + + /// Construct element at the back of the array + template + inline T & emplace_back(A &&... inValue) + { + grow(); + + T *element = mElements + mSize++; + ::new (element) T(std::forward(inValue)...); + return *element; + } + + /// Remove element from the back of the array + inline void pop_back() + { + JPH_ASSERT(mSize > 0); + mElements[--mSize].~T(); + } + + /// Returns true if there are no elements in the array + inline bool empty() const + { + return mSize == 0; + } + + /// Returns amount of elements in the array + inline size_type size() const + { + return mSize; + } + + /// Returns maximum amount of elements the array can hold + inline size_type capacity() const + { + return mCapacity; + } + + /// Reduce the capacity of the array to match its size + void shrink_to_fit() + { + if (mElements != nullptr) + { + if (mSize == 0) + free(); + else if (mCapacity > mSize) + reallocate(mSize); + } + } + + /// Swap the contents of two arrays + void swap(Array &inRHS) noexcept + { + std::swap(get_allocator(), inRHS.get_allocator()); + std::swap(mSize, inRHS.mSize); + std::swap(mCapacity, inRHS.mCapacity); + std::swap(mElements, inRHS.mElements); + } + + template + void insert(const_iterator inPos, Iterator inBegin, Iterator inEnd) + { + size_type num_elements = size_type(std::distance(inBegin, inEnd)); + if (num_elements > 0) + { + // After grow() inPos may be invalid + size_type first_element = inPos - mElements; + + grow(num_elements); + + T *element_begin = mElements + first_element; + T *element_end = element_begin + num_elements; + move(element_end, element_begin, mSize - first_element); + + for (T *element = element_begin; element < element_end; ++element, ++inBegin) + ::new (element) T(*inBegin); + + mSize += num_elements; + } + } + + void insert(const_iterator inPos, const T &inValue) + { + JPH_ASSERT(&inValue < mElements || &inValue >= mElements + mSize, "Can't pass an element from the array to insert"); + + // After grow() inPos may be invalid + size_type first_element = inPos - mElements; + + grow(); + + T *element = mElements + first_element; + move(element + 1, element, mSize - first_element); + + ::new (element) T(inValue); + mSize++; + } + + /// Remove one element from the array + void erase(const_iterator inIter) + { + size_type p = size_type(inIter - begin()); + JPH_ASSERT(p < mSize); + mElements[p].~T(); + if (p + 1 < mSize) + move(mElements + p, mElements + p + 1, mSize - p - 1); + --mSize; + } + + /// Remove multiple element from the array + void erase(const_iterator inBegin, const_iterator inEnd) + { + size_type p = size_type(inBegin - begin()); + size_type n = size_type(inEnd - inBegin); + JPH_ASSERT(inEnd <= end()); + destruct(p, p + n); + if (p + n < mSize) + move(mElements + p, mElements + p + n, mSize - p - n); + mSize -= n; + } + + /// Iterators + inline const_iterator begin() const + { + return mElements; + } + + inline const_iterator end() const + { + return mElements + mSize; + } + + inline const_iterator cbegin() const + { + return mElements; + } + + inline const_iterator cend() const + { + return mElements + mSize; + } + + inline iterator begin() + { + return mElements; + } + + inline iterator end() + { + return mElements + mSize; + } + + inline const T * data() const + { + return mElements; + } + + inline T * data() + { + return mElements; + } + + /// Access element + inline T & operator [] (size_type inIdx) + { + JPH_ASSERT(inIdx < mSize); + return mElements[inIdx]; + } + + inline const T & operator [] (size_type inIdx) const + { + JPH_ASSERT(inIdx < mSize); + return mElements[inIdx]; + } + + /// Access element + inline T & at(size_type inIdx) + { + JPH_ASSERT(inIdx < mSize); + return mElements[inIdx]; + } + + inline const T & at(size_type inIdx) const + { + JPH_ASSERT(inIdx < mSize); + return mElements[inIdx]; + } + + /// First element in the array + inline const T & front() const + { + JPH_ASSERT(mSize > 0); + return mElements[0]; + } + + inline T & front() + { + JPH_ASSERT(mSize > 0); + return mElements[0]; + } + + /// Last element in the array + inline const T & back() const + { + JPH_ASSERT(mSize > 0); + return mElements[mSize - 1]; + } + + inline T & back() + { + JPH_ASSERT(mSize > 0); + return mElements[mSize - 1]; + } + + /// Assignment operator + Array & operator = (const Array &inRHS) + { + if (static_cast(this) != static_cast(&inRHS)) + assign(inRHS.begin(), inRHS.end()); + + return *this; + } + + /// Assignment move operator + Array & operator = (Array &&inRHS) noexcept + { + if (static_cast(this) != static_cast(&inRHS)) + { + destroy(); + + get_allocator() = std::move(inRHS.get_allocator()); + + mSize = inRHS.mSize; + mCapacity = inRHS.mCapacity; + mElements = inRHS.mElements; + + inRHS.mSize = 0; + inRHS.mCapacity = 0; + inRHS.mElements = nullptr; + } + + return *this; + } + + /// Assignment operator + Array & operator = (std::initializer_list inRHS) + { + assign(inRHS); + + return *this; + } + + /// Comparing arrays + bool operator == (const Array &inRHS) const + { + if (mSize != inRHS.mSize) + return false; + for (size_type i = 0; i < mSize; ++i) + if (!(mElements[i] == inRHS.mElements[i])) + return false; + return true; + } + + bool operator != (const Array &inRHS) const + { + if (mSize != inRHS.mSize) + return true; + for (size_type i = 0; i < mSize; ++i) + if (mElements[i] != inRHS.mElements[i]) + return true; + return false; + } + + /// Get hash for this array + uint64 GetHash() const + { + // Hash length first + uint64 ret = Hash { } (uint32(size())); + + // Then hash elements + for (const T *element = mElements, *element_end = mElements + mSize; element < element_end; ++element) + HashCombine(ret, *element); + + return ret; + } + +private: + size_type mSize = 0; + size_type mCapacity = 0; + T * mElements = nullptr; +}; + +JPH_NAMESPACE_END + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat") + +namespace std +{ + /// Declare std::hash for Array + template + struct hash> + { + size_t operator () (const JPH::Array &inRHS) const + { + return std::size_t(inRHS.GetHash()); + } + }; +} + +JPH_SUPPRESS_WARNING_POP + +#endif // JPH_USE_STD_VECTOR diff --git a/thirdparty/jolt_physics/Jolt/Core/Atomics.h b/thirdparty/jolt_physics/Jolt/Core/Atomics.h new file mode 100644 index 000000000000..a53faa5c8772 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/Atomics.h @@ -0,0 +1,44 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +// Things we're using from STL +using std::atomic; +using std::memory_order; +using std::memory_order_relaxed; +using std::memory_order_acquire; +using std::memory_order_release; +using std::memory_order_acq_rel; +using std::memory_order_seq_cst; + +/// Atomically compute the min(ioAtomic, inValue) and store it in ioAtomic, returns true if value was updated +template +bool AtomicMin(atomic &ioAtomic, const T inValue, const memory_order inMemoryOrder = memory_order_seq_cst) +{ + T cur_value = ioAtomic.load(memory_order_relaxed); + while (cur_value > inValue) + if (ioAtomic.compare_exchange_weak(cur_value, inValue, inMemoryOrder)) + return true; + return false; +} + +/// Atomically compute the max(ioAtomic, inValue) and store it in ioAtomic, returns true if value was updated +template +bool AtomicMax(atomic &ioAtomic, const T inValue, const memory_order inMemoryOrder = memory_order_seq_cst) +{ + T cur_value = ioAtomic.load(memory_order_relaxed); + while (cur_value < inValue) + if (ioAtomic.compare_exchange_weak(cur_value, inValue, inMemoryOrder)) + return true; + return false; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/BinaryHeap.h b/thirdparty/jolt_physics/Jolt/Core/BinaryHeap.h new file mode 100644 index 000000000000..3c542e7eaa6b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/BinaryHeap.h @@ -0,0 +1,96 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Push a new element into a binary max-heap. +/// [inBegin, inEnd - 1) must be a a valid heap. Element inEnd - 1 will be inserted into the heap. The heap will be [inBegin, inEnd) after this call. +/// inPred is a function that returns true if the first element is less or equal than the second element. +/// See: https://en.wikipedia.org/wiki/Binary_heap +template +void BinaryHeapPush(Iterator inBegin, Iterator inEnd, Pred inPred) +{ + using diff_t = typename std::iterator_traits::difference_type; + using elem_t = typename std::iterator_traits::value_type; + + // New heap size + diff_t count = std::distance(inBegin, inEnd); + + // Start from the last element + diff_t current = count - 1; + while (current > 0) + { + // Get current element + elem_t ¤t_elem = *(inBegin + current); + + // Get parent element + diff_t parent = (current - 1) >> 1; + elem_t &parent_elem = *(inBegin + parent); + + // Sort them so that the parent is larger than the child + if (inPred(parent_elem, current_elem)) + { + std::swap(parent_elem, current_elem); + current = parent; + } + else + { + // When there's no change, we're done + break; + } + } +} + +/// Pop an element from a binary max-heap. +/// [inBegin, inEnd) must be a valid heap. The largest element will be removed from the heap. The heap will be [inBegin, inEnd - 1) after this call. +/// inPred is a function that returns true if the first element is less or equal than the second element. +/// See: https://en.wikipedia.org/wiki/Binary_heap +template +void BinaryHeapPop(Iterator inBegin, Iterator inEnd, Pred inPred) +{ + using diff_t = typename std::iterator_traits::difference_type; + + // Begin by moving the highest element to the end, this is the popped element + std::swap(*(inEnd - 1), *inBegin); + + // New heap size + diff_t count = std::distance(inBegin, inEnd) - 1; + + // Start from the root + diff_t largest = 0; + for (;;) + { + // Get first child + diff_t child = (largest << 1) + 1; + + // Check if we're beyond the end of the heap, if so the 2nd child is also beyond the end + if (child >= count) + break; + + // Remember the largest element from the previous iteration + diff_t prev_largest = largest; + + // Check if first child is bigger, if so select it + if (inPred(*(inBegin + largest), *(inBegin + child))) + largest = child; + + // Switch to the second child + ++child; + + // Check if second child is bigger, if so select it + if (child < count && inPred(*(inBegin + largest), *(inBegin + child))) + largest = child; + + // If there was no change, we're done + if (prev_largest == largest) + break; + + // Swap element + std::swap(*(inBegin + prev_largest), *(inBegin + largest)); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/ByteBuffer.h b/thirdparty/jolt_physics/Jolt/Core/ByteBuffer.h new file mode 100644 index 000000000000..48d19703f3e2 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/ByteBuffer.h @@ -0,0 +1,74 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Underlying data type for ByteBuffer +using ByteBufferVector = Array>; + +/// Simple byte buffer, aligned to a cache line +class ByteBuffer : public ByteBufferVector +{ +public: + /// Align the size to a multiple of inSize, returns the length after alignment + size_t Align(size_t inSize) + { + // Assert power of 2 + JPH_ASSERT(IsPowerOf2(inSize)); + + // Calculate new size and resize buffer + size_t s = AlignUp(size(), inSize); + resize(s, 0); + + return s; + } + + /// Allocate block of data of inSize elements and return the pointer + template + Type * Allocate(size_t inSize = 1) + { + // Reserve space + size_t s = size(); + resize(s + inSize * sizeof(Type)); + + // Get data pointer + Type *data = reinterpret_cast(&at(s)); + + // Construct elements + for (Type *d = data, *d_end = data + inSize; d < d_end; ++d) + ::new (d) Type; + + // Return pointer + return data; + } + + /// Append inData to the buffer + template + void AppendVector(const Array &inData) + { + size_t size = inData.size() * sizeof(Type); + uint8 *data = Allocate(size); + memcpy(data, &inData[0], size); + } + + /// Get object at inPosition (an offset in bytes) + template + const Type * Get(size_t inPosition) const + { + return reinterpret_cast(&at(inPosition)); + } + + /// Get object at inPosition (an offset in bytes) + template + Type * Get(size_t inPosition) + { + return reinterpret_cast(&at(inPosition)); + } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/Color.cpp b/thirdparty/jolt_physics/Jolt/Core/Color.cpp new file mode 100644 index 000000000000..93d3cabc75b5 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/Color.cpp @@ -0,0 +1,38 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +// Predefined colors +const Color Color::sBlack(0, 0, 0); +const Color Color::sDarkRed(128, 0, 0); +const Color Color::sRed(255, 0, 0); +const Color Color::sDarkGreen(0, 128, 0); +const Color Color::sGreen(0, 255, 0); +const Color Color::sDarkBlue(0, 0, 128); +const Color Color::sBlue(0, 0, 255); +const Color Color::sYellow(255, 255, 0); +const Color Color::sPurple(255, 0, 255); +const Color Color::sCyan(0, 255, 255); +const Color Color::sOrange(255, 128, 0); +const Color Color::sDarkOrange(128, 64, 0); +const Color Color::sGrey(128, 128, 128); +const Color Color::sLightGrey(192, 192, 192); +const Color Color::sWhite(255, 255, 255); + +// Generated by: http://phrogz.net/css/distinct-colors.html (this algo: https://en.wikipedia.org/wiki/Color_difference#CMC_l:c_.281984.29) +static constexpr Color sColors[] = { Color(255, 0, 0), Color(204, 143, 102), Color(226, 242, 0), Color(41, 166, 124), Color(0, 170, 255), Color(69, 38, 153), Color(153, 38, 130), Color(229, 57, 80), Color(204, 0, 0), Color(255, 170, 0), Color(85, 128, 0), Color(64, 255, 217), Color(0, 75, 140), Color(161, 115, 230), Color(242, 61, 157), Color(178, 101, 89), Color(140, 94, 0), Color(181, 217, 108), Color(64, 242, 255), Color(77, 117, 153), Color(157, 61, 242), Color(140, 0, 56), Color(127, 57, 32), Color(204, 173, 51), Color(64, 255, 64), Color(38, 145, 153), Color(0, 102, 255), Color(242, 0, 226), Color(153, 77, 107), Color(229, 92, 0), Color(140, 126, 70), Color(0, 179, 71), Color(0, 194, 242), Color(27, 0, 204), Color(230, 115, 222), Color(127, 0, 17) }; + +Color Color::sGetDistinctColor(int inIndex) +{ + JPH_ASSERT(inIndex >= 0); + + return sColors[inIndex % (sizeof(sColors) / sizeof(uint32))]; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/Color.h b/thirdparty/jolt_physics/Jolt/Core/Color.h new file mode 100644 index 000000000000..d07390de1cb3 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/Color.h @@ -0,0 +1,84 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +class Color; + +/// Type to use for passing arguments to a function +using ColorArg = Color; + +/// Class that holds an RGBA color with 8-bits per component +class [[nodiscard]] JPH_EXPORT_GCC_BUG_WORKAROUND Color +{ +public: + /// Constructors + Color() = default; ///< Intentionally not initialized for performance reasons + Color(const Color &inRHS) = default; + Color & operator = (const Color &inRHS) = default; + explicit constexpr Color(uint32 inColor) : mU32(inColor) { } + constexpr Color(uint8 inRed, uint8 inGreen, uint8 inBlue, uint8 inAlpha = 255) : r(inRed), g(inGreen), b(inBlue), a(inAlpha) { } + constexpr Color(ColorArg inRHS, uint8 inAlpha) : r(inRHS.r), g(inRHS.g), b(inRHS.b), a(inAlpha) { } + + /// Comparison + inline bool operator == (ColorArg inRHS) const { return mU32 == inRHS.mU32; } + inline bool operator != (ColorArg inRHS) const { return mU32 != inRHS.mU32; } + + /// Convert to uint32 + uint32 GetUInt32() const { return mU32; } + + /// Element access, 0 = red, 1 = green, 2 = blue, 3 = alpha + inline uint8 operator () (uint inIdx) const { JPH_ASSERT(inIdx < 4); return (&r)[inIdx]; } + inline uint8 & operator () (uint inIdx) { JPH_ASSERT(inIdx < 4); return (&r)[inIdx]; } + + /// Multiply two colors + inline Color operator * (const Color &inRHS) const { return Color(uint8((uint32(r) * inRHS.r) >> 8), uint8((uint32(g) * inRHS.g) >> 8), uint8((uint32(b) * inRHS.b) >> 8), uint8((uint32(a) * inRHS.a) >> 8)); } + + /// Multiply color with intensity in the range [0, 1] + inline Color operator * (float inIntensity) const { return Color(uint8(r * inIntensity), uint8(g * inIntensity), uint8(b * inIntensity), a); } + + /// Convert to Vec4 with range [0, 1] + inline Vec4 ToVec4() const { return Vec4(r, g, b, a) / 255.0f; } + + /// Get grayscale intensity of color + inline uint8 GetIntensity() const { return uint8((uint32(r) * 54 + g * 183 + b * 19) >> 8); } + + /// Get a visually distinct color + static Color sGetDistinctColor(int inIndex); + + /// Predefined colors + static const Color sBlack; + static const Color sDarkRed; + static const Color sRed; + static const Color sDarkGreen; + static const Color sGreen; + static const Color sDarkBlue; + static const Color sBlue; + static const Color sYellow; + static const Color sPurple; + static const Color sCyan; + static const Color sOrange; + static const Color sDarkOrange; + static const Color sGrey; + static const Color sLightGrey; + static const Color sWhite; + + union + { + uint32 mU32; ///< Combined value for red, green, blue and alpha + struct + { + uint8 r; ///< Red channel + uint8 g; ///< Green channel + uint8 b; ///< Blue channel + uint8 a; ///< Alpha channel + }; + }; +}; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/Core.h b/thirdparty/jolt_physics/Jolt/Core/Core.h new file mode 100644 index 000000000000..a0ad97eeb63c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/Core.h @@ -0,0 +1,593 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +// Jolt library version +#define JPH_VERSION_MAJOR 5 +#define JPH_VERSION_MINOR 2 +#define JPH_VERSION_PATCH 1 + +// Determine which features the library was compiled with +#ifdef JPH_DOUBLE_PRECISION + #define JPH_VERSION_FEATURE_BIT_1 1 +#else + #define JPH_VERSION_FEATURE_BIT_1 0 +#endif +#ifdef JPH_CROSS_PLATFORM_DETERMINISTIC + #define JPH_VERSION_FEATURE_BIT_2 1 +#else + #define JPH_VERSION_FEATURE_BIT_2 0 +#endif +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + #define JPH_VERSION_FEATURE_BIT_3 1 +#else + #define JPH_VERSION_FEATURE_BIT_3 0 +#endif +#ifdef JPH_PROFILE_ENABLED + #define JPH_VERSION_FEATURE_BIT_4 1 +#else + #define JPH_VERSION_FEATURE_BIT_4 0 +#endif +#ifdef JPH_EXTERNAL_PROFILE + #define JPH_VERSION_FEATURE_BIT_5 1 +#else + #define JPH_VERSION_FEATURE_BIT_5 0 +#endif +#ifdef JPH_DEBUG_RENDERER + #define JPH_VERSION_FEATURE_BIT_6 1 +#else + #define JPH_VERSION_FEATURE_BIT_6 0 +#endif +#ifdef JPH_DISABLE_TEMP_ALLOCATOR + #define JPH_VERSION_FEATURE_BIT_7 1 +#else + #define JPH_VERSION_FEATURE_BIT_7 0 +#endif +#ifdef JPH_DISABLE_CUSTOM_ALLOCATOR + #define JPH_VERSION_FEATURE_BIT_8 1 +#else + #define JPH_VERSION_FEATURE_BIT_8 0 +#endif +#if defined(JPH_OBJECT_LAYER_BITS) && JPH_OBJECT_LAYER_BITS == 32 + #define JPH_VERSION_FEATURE_BIT_9 1 +#else + #define JPH_VERSION_FEATURE_BIT_9 0 +#endif +#ifdef JPH_ENABLE_ASSERTS + #define JPH_VERSION_FEATURE_BIT_10 1 +#else + #define JPH_VERSION_FEATURE_BIT_10 0 +#endif +#ifdef JPH_OBJECT_STREAM + #define JPH_VERSION_FEATURE_BIT_11 1 +#else + #define JPH_VERSION_FEATURE_BIT_11 0 +#endif +#define JPH_VERSION_FEATURES (uint64(JPH_VERSION_FEATURE_BIT_1) | (JPH_VERSION_FEATURE_BIT_2 << 1) | (JPH_VERSION_FEATURE_BIT_3 << 2) | (JPH_VERSION_FEATURE_BIT_4 << 3) | (JPH_VERSION_FEATURE_BIT_5 << 4) | (JPH_VERSION_FEATURE_BIT_6 << 5) | (JPH_VERSION_FEATURE_BIT_7 << 6) | (JPH_VERSION_FEATURE_BIT_8 << 7) | (JPH_VERSION_FEATURE_BIT_9 << 8) | (JPH_VERSION_FEATURE_BIT_10 << 9) | (JPH_VERSION_FEATURE_BIT_11 << 10)) + +// Combine the version and features in a single ID +#define JPH_VERSION_ID ((JPH_VERSION_FEATURES << 24) | (JPH_VERSION_MAJOR << 16) | (JPH_VERSION_MINOR << 8) | JPH_VERSION_PATCH) + +// Determine platform +#if defined(JPH_PLATFORM_BLUE) + // Correct define already defined, this overrides everything else +#elif defined(_WIN32) || defined(_WIN64) + #include + #if WINAPI_FAMILY == WINAPI_FAMILY_APP + #define JPH_PLATFORM_WINDOWS_UWP // Building for Universal Windows Platform + #endif + #define JPH_PLATFORM_WINDOWS +#elif defined(__ANDROID__) // Android is linux too, so that's why we check it first + #define JPH_PLATFORM_ANDROID +#elif defined(__linux__) + #define JPH_PLATFORM_LINUX +#elif defined(__FreeBSD__) + #define JPH_PLATFORM_FREEBSD +#elif defined(__APPLE__) + #include + #if defined(TARGET_OS_IPHONE) && !TARGET_OS_IPHONE + #define JPH_PLATFORM_MACOS + #else + #define JPH_PLATFORM_IOS + #endif +#elif defined(__EMSCRIPTEN__) + #define JPH_PLATFORM_WASM +#endif + +// Platform helper macros +#ifdef JPH_PLATFORM_ANDROID + #define JPH_IF_NOT_ANDROID(x) +#else + #define JPH_IF_NOT_ANDROID(x) x +#endif + +// Determine compiler +#if defined(__clang__) + #define JPH_COMPILER_CLANG +#elif defined(__GNUC__) + #define JPH_COMPILER_GCC +#elif defined(_MSC_VER) + #define JPH_COMPILER_MSVC +#endif + +#if defined(__MINGW64__) || defined (__MINGW32__) + #define JPH_COMPILER_MINGW +#endif + +// Detect CPU architecture +#if defined(__x86_64__) || defined(_M_X64) || defined(__i386__) || defined(_M_IX86) + // X86 CPU architecture + #define JPH_CPU_X86 + #if defined(__x86_64__) || defined(_M_X64) + #define JPH_CPU_ADDRESS_BITS 64 + #else + #define JPH_CPU_ADDRESS_BITS 32 + #endif + #define JPH_USE_SSE + #define JPH_VECTOR_ALIGNMENT 16 + #define JPH_DVECTOR_ALIGNMENT 32 + + // Detect enabled instruction sets + #if defined(__AVX512F__) && defined(__AVX512VL__) && defined(__AVX512DQ__) && !defined(JPH_USE_AVX512) + #define JPH_USE_AVX512 + #endif + #if (defined(__AVX2__) || defined(JPH_USE_AVX512)) && !defined(JPH_USE_AVX2) + #define JPH_USE_AVX2 + #endif + #if (defined(__AVX__) || defined(JPH_USE_AVX2)) && !defined(JPH_USE_AVX) + #define JPH_USE_AVX + #endif + #if (defined(__SSE4_2__) || defined(JPH_USE_AVX)) && !defined(JPH_USE_SSE4_2) + #define JPH_USE_SSE4_2 + #endif + #if (defined(__SSE4_1__) || defined(JPH_USE_SSE4_2)) && !defined(JPH_USE_SSE4_1) + #define JPH_USE_SSE4_1 + #endif + #if (defined(__F16C__) || defined(JPH_USE_AVX2)) && !defined(JPH_USE_F16C) + #define JPH_USE_F16C + #endif + #if (defined(__LZCNT__) || defined(JPH_USE_AVX2)) && !defined(JPH_USE_LZCNT) + #define JPH_USE_LZCNT + #endif + #if (defined(__BMI__) || defined(JPH_USE_AVX2)) && !defined(JPH_USE_TZCNT) + #define JPH_USE_TZCNT + #endif + #ifndef JPH_CROSS_PLATFORM_DETERMINISTIC // FMA is not compatible with cross platform determinism + #if defined(JPH_COMPILER_CLANG) || defined(JPH_COMPILER_GCC) + #if defined(__FMA__) && !defined(JPH_USE_FMADD) + #define JPH_USE_FMADD + #endif + #elif defined(JPH_COMPILER_MSVC) + #if defined(__AVX2__) && !defined(JPH_USE_FMADD) // AVX2 also enables fused multiply add + #define JPH_USE_FMADD + #endif + #else + #error Undefined compiler + #endif + #endif +#elif defined(__aarch64__) || defined(_M_ARM64) || defined(__arm__) || defined(_M_ARM) + // ARM CPU architecture + #define JPH_CPU_ARM + #if defined(__aarch64__) || defined(_M_ARM64) + #define JPH_CPU_ADDRESS_BITS 64 + #define JPH_USE_NEON + #define JPH_VECTOR_ALIGNMENT 16 + #define JPH_DVECTOR_ALIGNMENT 32 + #else + #define JPH_CPU_ADDRESS_BITS 32 + #define JPH_VECTOR_ALIGNMENT 8 // 32-bit ARM does not support aligning on the stack on 16 byte boundaries + #define JPH_DVECTOR_ALIGNMENT 8 + #endif +#elif defined(JPH_PLATFORM_WASM) + // WebAssembly CPU architecture + #define JPH_CPU_WASM + #define JPH_CPU_ADDRESS_BITS 32 + #define JPH_VECTOR_ALIGNMENT 16 + #define JPH_DVECTOR_ALIGNMENT 32 + #ifdef __wasm_simd128__ + #define JPH_USE_SSE + #define JPH_USE_SSE4_1 + #define JPH_USE_SSE4_2 + #endif +#elif defined(__e2k__) + // E2K CPU architecture (MCST Elbrus 2000) + #define JPH_CPU_E2K + #define JPH_CPU_ADDRESS_BITS 64 + #define JPH_VECTOR_ALIGNMENT 16 + #define JPH_DVECTOR_ALIGNMENT 32 + + // Compiler flags on e2k arch determine CPU features + #if defined(__SSE__) && !defined(JPH_USE_SSE) + #define JPH_USE_SSE + #endif +#else + #error Unsupported CPU architecture +#endif + +// If this define is set, Jolt is compiled as a shared library +#ifdef JPH_SHARED_LIBRARY + #ifdef JPH_BUILD_SHARED_LIBRARY + // While building the shared library, we must export these symbols + #if defined(JPH_PLATFORM_WINDOWS) && !defined(JPH_COMPILER_MINGW) + #define JPH_EXPORT __declspec(dllexport) + #else + #define JPH_EXPORT __attribute__ ((visibility ("default"))) + #if defined(JPH_COMPILER_GCC) + // Prevents an issue with GCC attribute parsing (see https://gcc.gnu.org/bugzilla/show_bug.cgi?id=69585) + #define JPH_EXPORT_GCC_BUG_WORKAROUND [[gnu::visibility("default")]] + #endif + #endif + #else + // When linking against Jolt, we must import these symbols + #if defined(JPH_PLATFORM_WINDOWS) && !defined(JPH_COMPILER_MINGW) + #define JPH_EXPORT __declspec(dllimport) + #else + #define JPH_EXPORT __attribute__ ((visibility ("default"))) + #if defined(JPH_COMPILER_GCC) + // Prevents an issue with GCC attribute parsing (see https://gcc.gnu.org/bugzilla/show_bug.cgi?id=69585) + #define JPH_EXPORT_GCC_BUG_WORKAROUND [[gnu::visibility("default")]] + #endif + #endif + #endif +#else + // If the define is not set, we use static linking and symbols don't need to be imported or exported + #define JPH_EXPORT +#endif + +#ifndef JPH_EXPORT_GCC_BUG_WORKAROUND + #define JPH_EXPORT_GCC_BUG_WORKAROUND JPH_EXPORT +#endif + +// Macro used by the RTTI macros to not export a function +#define JPH_NO_EXPORT + +// Pragmas to store / restore the warning state and to disable individual warnings +#ifdef JPH_COMPILER_CLANG +#define JPH_PRAGMA(x) _Pragma(#x) +#define JPH_SUPPRESS_WARNING_PUSH JPH_PRAGMA(clang diagnostic push) +#define JPH_SUPPRESS_WARNING_POP JPH_PRAGMA(clang diagnostic pop) +#define JPH_CLANG_SUPPRESS_WARNING(w) JPH_PRAGMA(clang diagnostic ignored w) +#if __clang_major__ >= 13 + #define JPH_CLANG_13_PLUS_SUPPRESS_WARNING(w) JPH_CLANG_SUPPRESS_WARNING(w) +#else + #define JPH_CLANG_13_PLUS_SUPPRESS_WARNING(w) +#endif +#if __clang_major__ >= 16 + #define JPH_CLANG_16_PLUS_SUPPRESS_WARNING(w) JPH_CLANG_SUPPRESS_WARNING(w) +#else + #define JPH_CLANG_16_PLUS_SUPPRESS_WARNING(w) +#endif +#else +#define JPH_CLANG_SUPPRESS_WARNING(w) +#define JPH_CLANG_13_PLUS_SUPPRESS_WARNING(w) +#define JPH_CLANG_16_PLUS_SUPPRESS_WARNING(w) +#endif +#ifdef JPH_COMPILER_GCC +#define JPH_PRAGMA(x) _Pragma(#x) +#define JPH_SUPPRESS_WARNING_PUSH JPH_PRAGMA(GCC diagnostic push) +#define JPH_SUPPRESS_WARNING_POP JPH_PRAGMA(GCC diagnostic pop) +#define JPH_GCC_SUPPRESS_WARNING(w) JPH_PRAGMA(GCC diagnostic ignored w) +#else +#define JPH_GCC_SUPPRESS_WARNING(w) +#endif +#ifdef JPH_COMPILER_MSVC +#define JPH_PRAGMA(x) __pragma(x) +#define JPH_SUPPRESS_WARNING_PUSH JPH_PRAGMA(warning (push)) +#define JPH_SUPPRESS_WARNING_POP JPH_PRAGMA(warning (pop)) +#define JPH_MSVC_SUPPRESS_WARNING(w) JPH_PRAGMA(warning (disable : w)) +#if _MSC_VER >= 1920 && _MSC_VER < 1930 + #define JPH_MSVC2019_SUPPRESS_WARNING(w) JPH_MSVC_SUPPRESS_WARNING(w) +#else + #define JPH_MSVC2019_SUPPRESS_WARNING(w) +#endif +#else +#define JPH_MSVC_SUPPRESS_WARNING(w) +#define JPH_MSVC2019_SUPPRESS_WARNING(w) +#endif + +// Disable common warnings triggered by Jolt when compiling with -Wall +#define JPH_SUPPRESS_WARNINGS \ + JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat") \ + JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic") \ + JPH_CLANG_SUPPRESS_WARNING("-Wfloat-equal") \ + JPH_CLANG_SUPPRESS_WARNING("-Wsign-conversion") \ + JPH_CLANG_SUPPRESS_WARNING("-Wold-style-cast") \ + JPH_CLANG_SUPPRESS_WARNING("-Wgnu-anonymous-struct") \ + JPH_CLANG_SUPPRESS_WARNING("-Wnested-anon-types") \ + JPH_CLANG_SUPPRESS_WARNING("-Wglobal-constructors") \ + JPH_CLANG_SUPPRESS_WARNING("-Wexit-time-destructors") \ + JPH_CLANG_SUPPRESS_WARNING("-Wnonportable-system-include-path") \ + JPH_CLANG_SUPPRESS_WARNING("-Wlanguage-extension-token") \ + JPH_CLANG_SUPPRESS_WARNING("-Wunused-parameter") \ + JPH_CLANG_SUPPRESS_WARNING("-Wformat-nonliteral") \ + JPH_CLANG_SUPPRESS_WARNING("-Wcovered-switch-default") \ + JPH_CLANG_SUPPRESS_WARNING("-Wcast-align") \ + JPH_CLANG_SUPPRESS_WARNING("-Winvalid-offsetof") \ + JPH_CLANG_SUPPRESS_WARNING("-Wgnu-zero-variadic-macro-arguments") \ + JPH_CLANG_SUPPRESS_WARNING("-Wdocumentation-unknown-command") \ + JPH_CLANG_SUPPRESS_WARNING("-Wctad-maybe-unsupported") \ + JPH_CLANG_SUPPRESS_WARNING("-Wswitch-default") \ + JPH_CLANG_13_PLUS_SUPPRESS_WARNING("-Wdeprecated-copy") \ + JPH_CLANG_13_PLUS_SUPPRESS_WARNING("-Wdeprecated-copy-with-dtor") \ + JPH_CLANG_16_PLUS_SUPPRESS_WARNING("-Wunsafe-buffer-usage") \ + JPH_IF_NOT_ANDROID(JPH_CLANG_SUPPRESS_WARNING("-Wimplicit-int-float-conversion")) \ + \ + JPH_GCC_SUPPRESS_WARNING("-Wcomment") \ + JPH_GCC_SUPPRESS_WARNING("-Winvalid-offsetof") \ + JPH_GCC_SUPPRESS_WARNING("-Wclass-memaccess") \ + JPH_GCC_SUPPRESS_WARNING("-Wpedantic") \ + JPH_GCC_SUPPRESS_WARNING("-Wunused-parameter") \ + JPH_GCC_SUPPRESS_WARNING("-Wmaybe-uninitialized") \ + \ + JPH_MSVC_SUPPRESS_WARNING(4619) /* #pragma warning: there is no warning number 'XXXX' */ \ + JPH_MSVC_SUPPRESS_WARNING(4514) /* 'X' : unreferenced inline function has been removed */ \ + JPH_MSVC_SUPPRESS_WARNING(4710) /* 'X' : function not inlined */ \ + JPH_MSVC_SUPPRESS_WARNING(4711) /* function 'X' selected for automatic inline expansion */ \ + JPH_MSVC_SUPPRESS_WARNING(4820) /* 'X': 'Y' bytes padding added after data member 'Z' */ \ + JPH_MSVC_SUPPRESS_WARNING(4100) /* 'X' : unreferenced formal parameter */ \ + JPH_MSVC_SUPPRESS_WARNING(4626) /* 'X' : assignment operator was implicitly defined as deleted because a base class assignment operator is inaccessible or deleted */ \ + JPH_MSVC_SUPPRESS_WARNING(5027) /* 'X' : move assignment operator was implicitly defined as deleted because a base class move assignment operator is inaccessible or deleted */ \ + JPH_MSVC_SUPPRESS_WARNING(4365) /* 'argument' : conversion from 'X' to 'Y', signed / unsigned mismatch */ \ + JPH_MSVC_SUPPRESS_WARNING(4324) /* 'X' : structure was padded due to alignment specifier */ \ + JPH_MSVC_SUPPRESS_WARNING(4625) /* 'X' : copy constructor was implicitly defined as deleted because a base class copy constructor is inaccessible or deleted */ \ + JPH_MSVC_SUPPRESS_WARNING(5026) /* 'X': move constructor was implicitly defined as deleted because a base class move constructor is inaccessible or deleted */ \ + JPH_MSVC_SUPPRESS_WARNING(4623) /* 'X' : default constructor was implicitly defined as deleted */ \ + JPH_MSVC_SUPPRESS_WARNING(4201) /* nonstandard extension used: nameless struct/union */ \ + JPH_MSVC_SUPPRESS_WARNING(4371) /* 'X': layout of class may have changed from a previous version of the compiler due to better packing of member 'Y' */ \ + JPH_MSVC_SUPPRESS_WARNING(5045) /* Compiler will insert Spectre mitigation for memory load if /Qspectre switch specified */ \ + JPH_MSVC_SUPPRESS_WARNING(4583) /* 'X': destructor is not implicitly called */ \ + JPH_MSVC_SUPPRESS_WARNING(4582) /* 'X': constructor is not implicitly called */ \ + JPH_MSVC_SUPPRESS_WARNING(5219) /* implicit conversion from 'X' to 'Y', possible loss of data */ \ + JPH_MSVC_SUPPRESS_WARNING(4826) /* Conversion from 'X *' to 'JPH::uint64' is sign-extended. This may cause unexpected runtime behavior. (32-bit) */ \ + JPH_MSVC_SUPPRESS_WARNING(5264) /* 'X': 'const' variable is not used */ \ + JPH_MSVC_SUPPRESS_WARNING(4251) /* class 'X' needs to have DLL-interface to be used by clients of class 'Y' */ \ + JPH_MSVC_SUPPRESS_WARNING(4738) /* storing 32-bit float result in memory, possible loss of performance */ \ + JPH_MSVC2019_SUPPRESS_WARNING(5246) /* the initialization of a subobject should be wrapped in braces */ + +// OS-specific includes +#if defined(JPH_PLATFORM_WINDOWS) + #define JPH_BREAKPOINT __debugbreak() +#elif defined(JPH_PLATFORM_BLUE) + // Configuration for a popular game console. + // This file is not distributed because it would violate an NDA. + // Creating one should only be a couple of minutes of work if you have the documentation for the platform + // (you only need to define JPH_BREAKPOINT, JPH_PLATFORM_BLUE_GET_TICKS, JPH_PLATFORM_BLUE_MUTEX*, JPH_PLATFORM_BLUE_RWLOCK* and include the right header). + #include +#elif defined(JPH_PLATFORM_LINUX) || defined(JPH_PLATFORM_ANDROID) || defined(JPH_PLATFORM_MACOS) || defined(JPH_PLATFORM_IOS) || defined(JPH_PLATFORM_FREEBSD) + #if defined(JPH_CPU_X86) + #define JPH_BREAKPOINT __asm volatile ("int $0x3") + #elif defined(JPH_CPU_ARM) + #define JPH_BREAKPOINT __builtin_trap() + #elif defined(JPH_CPU_E2K) + #define JPH_BREAKPOINT __builtin_trap() + #endif +#elif defined(JPH_PLATFORM_WASM) + #define JPH_BREAKPOINT do { } while (false) // Not supported +#else + #error Unknown platform +#endif + +// Begin the JPH namespace +#define JPH_NAMESPACE_BEGIN \ + JPH_SUPPRESS_WARNING_PUSH \ + JPH_SUPPRESS_WARNINGS \ + namespace JPH { + +// End the JPH namespace +#define JPH_NAMESPACE_END \ + } \ + JPH_SUPPRESS_WARNING_POP + +// Suppress warnings generated by the standard template library +#define JPH_SUPPRESS_WARNINGS_STD_BEGIN \ + JPH_SUPPRESS_WARNING_PUSH \ + JPH_MSVC_SUPPRESS_WARNING(4365) \ + JPH_MSVC_SUPPRESS_WARNING(4619) \ + JPH_MSVC_SUPPRESS_WARNING(4710) \ + JPH_MSVC_SUPPRESS_WARNING(4711) \ + JPH_MSVC_SUPPRESS_WARNING(4820) \ + JPH_MSVC_SUPPRESS_WARNING(4514) \ + JPH_MSVC_SUPPRESS_WARNING(5262) \ + JPH_MSVC_SUPPRESS_WARNING(5264) \ + JPH_MSVC_SUPPRESS_WARNING(4738) + +#define JPH_SUPPRESS_WARNINGS_STD_END \ + JPH_SUPPRESS_WARNING_POP + +// Standard C++ includes +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_COMPILER_MSVC + #include // for alloca +#endif +#if defined(JPH_USE_SSE) + #include +#elif defined(JPH_USE_NEON) + #ifdef JPH_COMPILER_MSVC + #include + #include + #else + #include + #endif +#endif +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +// Commonly used STL types +using std::min; +using std::max; +using std::abs; +using std::sqrt; +using std::ceil; +using std::floor; +using std::trunc; +using std::round; +using std::fmod; +using std::string_view; +using std::function; +using std::numeric_limits; +using std::isfinite; +using std::isnan; +using std::ostream; +using std::istream; + +// Standard types +using uint = unsigned int; +using uint8 = std::uint8_t; +using uint16 = std::uint16_t; +using uint32 = std::uint32_t; +using uint64 = std::uint64_t; + +// Assert sizes of types +static_assert(sizeof(uint) >= 4, "Invalid size of uint"); +static_assert(sizeof(uint8) == 1, "Invalid size of uint8"); +static_assert(sizeof(uint16) == 2, "Invalid size of uint16"); +static_assert(sizeof(uint32) == 4, "Invalid size of uint32"); +static_assert(sizeof(uint64) == 8, "Invalid size of uint64"); +static_assert(sizeof(void *) == (JPH_CPU_ADDRESS_BITS == 64? 8 : 4), "Invalid size of pointer" ); + +// Determine if we want extra debugging code to be active +#if !defined(NDEBUG) && !defined(JPH_NO_DEBUG) + #define JPH_DEBUG +#endif + +// Define inline macro +#if defined(JPH_NO_FORCE_INLINE) + #define JPH_INLINE inline +#elif defined(JPH_COMPILER_CLANG) + #define JPH_INLINE __inline__ __attribute__((always_inline)) +#elif defined(JPH_COMPILER_GCC) + // On gcc 14 using always_inline in debug mode causes error: "inlining failed in call to 'always_inline' 'XXX': function not considered for inlining" + // See: https://github.com/jrouwe/JoltPhysics/issues/1096 + #if __GNUC__ >= 14 && defined(JPH_DEBUG) + #define JPH_INLINE inline + #else + #define JPH_INLINE __inline__ __attribute__((always_inline)) + #endif +#elif defined(JPH_COMPILER_MSVC) + #define JPH_INLINE __forceinline +#else + #error Undefined +#endif + +// Cache line size (used for aligning to cache line) +#ifndef JPH_CACHE_LINE_SIZE + #define JPH_CACHE_LINE_SIZE 64 +#endif + +// Define macro to get current function name +#if defined(JPH_COMPILER_CLANG) || defined(JPH_COMPILER_GCC) + #define JPH_FUNCTION_NAME __PRETTY_FUNCTION__ +#elif defined(JPH_COMPILER_MSVC) + #define JPH_FUNCTION_NAME __FUNCTION__ +#else + #error Undefined +#endif + +// Stack allocation +#define JPH_STACK_ALLOC(n) alloca(n) + +// Shorthand for #ifdef JPH_DEBUG / #endif +#ifdef JPH_DEBUG + #define JPH_IF_DEBUG(...) __VA_ARGS__ + #define JPH_IF_NOT_DEBUG(...) +#else + #define JPH_IF_DEBUG(...) + #define JPH_IF_NOT_DEBUG(...) __VA_ARGS__ +#endif + +// Shorthand for #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED / #endif +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + #define JPH_IF_FLOATING_POINT_EXCEPTIONS_ENABLED(...) __VA_ARGS__ +#else + #define JPH_IF_FLOATING_POINT_EXCEPTIONS_ENABLED(...) +#endif + +// Helper macros to detect if we're running in single or double precision mode +#ifdef JPH_DOUBLE_PRECISION + #define JPH_IF_SINGLE_PRECISION(...) + #define JPH_IF_SINGLE_PRECISION_ELSE(s, d) d + #define JPH_IF_DOUBLE_PRECISION(...) __VA_ARGS__ +#else + #define JPH_IF_SINGLE_PRECISION(...) __VA_ARGS__ + #define JPH_IF_SINGLE_PRECISION_ELSE(s, d) s + #define JPH_IF_DOUBLE_PRECISION(...) +#endif + +// Helper macro to detect if the debug renderer is active +#ifdef JPH_DEBUG_RENDERER + #define JPH_IF_DEBUG_RENDERER(...) __VA_ARGS__ + #define JPH_IF_NOT_DEBUG_RENDERER(...) +#else + #define JPH_IF_DEBUG_RENDERER(...) + #define JPH_IF_NOT_DEBUG_RENDERER(...) __VA_ARGS__ +#endif + +// Macro to indicate that a parameter / variable is unused +#define JPH_UNUSED(x) (void)x + +// Macro to enable floating point precise mode and to disable fused multiply add instructions +#if defined(JPH_COMPILER_GCC) || defined(JPH_CROSS_PLATFORM_DETERMINISTIC) + // We compile without -ffast-math and -ffp-contract=fast, so we don't need to disable anything + #define JPH_PRECISE_MATH_ON + #define JPH_PRECISE_MATH_OFF +#elif defined(JPH_COMPILER_CLANG) + // We compile without -ffast-math because pragma float_control(precise, on) doesn't seem to actually negate all of the -ffast-math effects and causes the unit tests to fail (even if the pragma is added to all files) + // On clang 14 and later we can turn off float contraction through a pragma (before it was buggy), so if FMA is on we can disable it through this macro + #if (defined(JPH_CPU_ARM) && !defined(JPH_PLATFORM_ANDROID) && __clang_major__ >= 16) || (defined(JPH_CPU_X86) && __clang_major__ >= 14) + #define JPH_PRECISE_MATH_ON \ + _Pragma("float_control(precise, on, push)") \ + _Pragma("clang fp contract(off)") + #define JPH_PRECISE_MATH_OFF \ + _Pragma("float_control(pop)") + #elif __clang_major__ >= 14 && (defined(JPH_USE_FMADD) || defined(FP_FAST_FMA)) + #define JPH_PRECISE_MATH_ON \ + _Pragma("clang fp contract(off)") + #define JPH_PRECISE_MATH_OFF \ + _Pragma("clang fp contract(on)") + #else + #define JPH_PRECISE_MATH_ON + #define JPH_PRECISE_MATH_OFF + #endif +#elif defined(JPH_COMPILER_MSVC) + // Unfortunately there is no way to push the state of fp_contract, so we have to assume it was turned on before JPH_PRECISE_MATH_ON + #define JPH_PRECISE_MATH_ON \ + __pragma(float_control(precise, on, push)) \ + __pragma(fp_contract(off)) + #define JPH_PRECISE_MATH_OFF \ + __pragma(fp_contract(on)) \ + __pragma(float_control(pop)) +#else + #error Undefined +#endif + +// Check if Thread Sanitizer is enabled +#ifdef __has_feature + #if __has_feature(thread_sanitizer) + #define JPH_TSAN_ENABLED + #endif +#else + #ifdef __SANITIZE_THREAD__ + #define JPH_TSAN_ENABLED + #endif +#endif + +// Attribute to disable Thread Sanitizer for a particular function +#ifdef JPH_TSAN_ENABLED + #define JPH_TSAN_NO_SANITIZE __attribute__((no_sanitize("thread"))) +#else + #define JPH_TSAN_NO_SANITIZE +#endif + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/FPControlWord.h b/thirdparty/jolt_physics/Jolt/Core/FPControlWord.h new file mode 100644 index 000000000000..0c8b3f1ff097 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/FPControlWord.h @@ -0,0 +1,135 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +#if defined(JPH_CPU_WASM) + +// Not supported + +#elif defined(JPH_USE_SSE) + +/// Helper class that needs to be put on the stack to update the state of the floating point control word. +/// This state is kept per thread. +template +class FPControlWord : public NonCopyable +{ +public: + FPControlWord() + { + mPrevState = _mm_getcsr(); + _mm_setcsr((mPrevState & ~Mask) | Value); + } + + ~FPControlWord() + { + _mm_setcsr((_mm_getcsr() & ~Mask) | (mPrevState & Mask)); + } + +private: + uint mPrevState; +}; + +#elif defined(JPH_CPU_ARM) && defined(JPH_COMPILER_MSVC) + +/// Helper class that needs to be put on the stack to update the state of the floating point control word. +/// This state is kept per thread. +template +class FPControlWord : public NonCopyable +{ +public: + FPControlWord() + { + // Read state before change + _controlfp_s(&mPrevState, 0, 0); + + // Update the state + unsigned int dummy; + _controlfp_s(&dummy, Value, Mask); + } + + ~FPControlWord() + { + // Restore state + unsigned int dummy; + _controlfp_s(&dummy, mPrevState, Mask); + } + +private: + unsigned int mPrevState; +}; + +#elif defined(JPH_CPU_ARM) && defined(JPH_USE_NEON) + +/// Helper class that needs to be put on the stack to update the state of the floating point control word. +/// This state is kept per thread. +template +class FPControlWord : public NonCopyable +{ +public: + FPControlWord() + { + uint64 val; + asm volatile("mrs %0, fpcr" : "=r" (val)); + mPrevState = val; + val &= ~Mask; + val |= Value; + asm volatile("msr fpcr, %0" : /* no output */ : "r" (val)); + } + + ~FPControlWord() + { + uint64 val; + asm volatile("mrs %0, fpcr" : "=r" (val)); + val &= ~Mask; + val |= mPrevState & Mask; + asm volatile("msr fpcr, %0" : /* no output */ : "r" (val)); + } + +private: + uint64 mPrevState; +}; + +#elif defined(JPH_CPU_ARM) + +/// Helper class that needs to be put on the stack to update the state of the floating point control word. +/// This state is kept per thread. +template +class FPControlWord : public NonCopyable +{ +public: + FPControlWord() + { + uint32 val; + asm volatile("vmrs %0, fpscr" : "=r" (val)); + mPrevState = val; + val &= ~Mask; + val |= Value; + asm volatile("vmsr fpscr, %0" : /* no output */ : "r" (val)); + } + + ~FPControlWord() + { + uint32 val; + asm volatile("vmrs %0, fpscr" : "=r" (val)); + val &= ~Mask; + val |= mPrevState & Mask; + asm volatile("vmsr fpscr, %0" : /* no output */ : "r" (val)); + } + +private: + uint32 mPrevState; +}; + +#else + +#error Unsupported CPU architecture + +#endif + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/FPException.h b/thirdparty/jolt_physics/Jolt/Core/FPException.h new file mode 100644 index 000000000000..3083f05c0508 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/FPException.h @@ -0,0 +1,74 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + +#if defined(JPH_CPU_WASM) + +// Not supported +class FPExceptionsEnable { }; +class FPExceptionDisableInvalid { }; +class FPExceptionDisableDivByZero { }; + +#elif defined(JPH_USE_SSE) + +/// Enable floating point divide by zero exception and exceptions on invalid numbers +class FPExceptionsEnable : public FPControlWord<0, _MM_MASK_DIV_ZERO | _MM_MASK_INVALID> { }; + +/// Disable invalid floating point value exceptions +class FPExceptionDisableInvalid : public FPControlWord<_MM_MASK_INVALID, _MM_MASK_INVALID> { }; + +/// Disable division by zero floating point exceptions +class FPExceptionDisableDivByZero : public FPControlWord<_MM_MASK_DIV_ZERO, _MM_MASK_DIV_ZERO> { }; + +#elif defined(JPH_CPU_ARM) && defined(JPH_COMPILER_MSVC) + +/// Enable floating point divide by zero exception and exceptions on invalid numbers +class FPExceptionsEnable : public FPControlWord<0, _EM_INVALID | _EM_ZERODIVIDE> { }; + +/// Disable invalid floating point value exceptions +class FPExceptionDisableInvalid : public FPControlWord<_EM_INVALID, _EM_INVALID> { }; + +/// Disable division by zero floating point exceptions +class FPExceptionDisableDivByZero : public FPControlWord<_EM_ZERODIVIDE, _EM_ZERODIVIDE> { }; + +#elif defined(JPH_CPU_ARM) + +/// Invalid operation exception bit +static constexpr uint64 FP_IOE = 1 << 8; + +/// Enable divide by zero exception bit +static constexpr uint64 FP_DZE = 1 << 9; + +/// Enable floating point divide by zero exception and exceptions on invalid numbers +class FPExceptionsEnable : public FPControlWord { }; + +/// Disable invalid floating point value exceptions +class FPExceptionDisableInvalid : public FPControlWord<0, FP_IOE> { }; + +/// Disable division by zero floating point exceptions +class FPExceptionDisableDivByZero : public FPControlWord<0, FP_DZE> { }; + +#else + +#error Unsupported CPU architecture + +#endif + +#else + +/// Dummy implementations +class FPExceptionsEnable { }; +class FPExceptionDisableInvalid { }; +class FPExceptionDisableDivByZero { }; + +#endif + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/FPFlushDenormals.h b/thirdparty/jolt_physics/Jolt/Core/FPFlushDenormals.h new file mode 100644 index 000000000000..672a19dbecf5 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/FPFlushDenormals.h @@ -0,0 +1,41 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +#if defined(JPH_CPU_WASM) + +// Not supported +class FPFlushDenormals { }; + +#elif defined(JPH_USE_SSE) + +/// Helper class that needs to be put on the stack to enable flushing denormals to zero +/// This can make floating point operations much faster when working with very small numbers +class FPFlushDenormals : public FPControlWord<_MM_FLUSH_ZERO_ON, _MM_FLUSH_ZERO_MASK> { }; + +#elif defined(JPH_CPU_ARM) && defined(JPH_COMPILER_MSVC) + +class FPFlushDenormals : public FPControlWord<_DN_FLUSH, _MCW_DN> { }; + +#elif defined(JPH_CPU_ARM) + +/// Flush denormals to zero bit +static constexpr uint64 FP_FZ = 1 << 24; + +/// Helper class that needs to be put on the stack to enable flushing denormals to zero +/// This can make floating point operations much faster when working with very small numbers +class FPFlushDenormals : public FPControlWord { }; + +#else + +#error Unsupported CPU architecture + +#endif + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/Factory.cpp b/thirdparty/jolt_physics/Jolt/Core/Factory.cpp new file mode 100644 index 000000000000..1890c037f3fc --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/Factory.cpp @@ -0,0 +1,92 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +Factory *Factory::sInstance = nullptr; + +void *Factory::CreateObject(const char *inName) +{ + const RTTI *ci = Find(inName); + return ci != nullptr? ci->CreateObject() : nullptr; +} + +const RTTI *Factory::Find(const char *inName) +{ + ClassNameMap::iterator c = mClassNameMap.find(inName); + return c != mClassNameMap.end()? c->second : nullptr; +} + +const RTTI *Factory::Find(uint32 inHash) +{ + ClassHashMap::iterator c = mClassHashMap.find(inHash); + return c != mClassHashMap.end()? c->second : nullptr; +} + +bool Factory::Register(const RTTI *inRTTI) +{ + // Check if we already know the type + if (Find(inRTTI->GetName()) != nullptr) + return true; + + // Insert this class by name + mClassNameMap.try_emplace(inRTTI->GetName(), inRTTI); + + // Insert this class by hash + if (!mClassHashMap.try_emplace(inRTTI->GetHash(), inRTTI).second) + { + JPH_ASSERT(false, "Hash collision registering type!"); + return false; + } + + // Register base classes + for (int i = 0; i < inRTTI->GetBaseClassCount(); ++i) + if (!Register(inRTTI->GetBaseClass(i))) + return false; + +#ifdef JPH_OBJECT_STREAM + // Register attribute classes + for (int i = 0; i < inRTTI->GetAttributeCount(); ++i) + { + const RTTI *rtti = inRTTI->GetAttribute(i).GetMemberPrimitiveType(); + if (rtti != nullptr && !Register(rtti)) + return false; + } +#endif // JPH_OBJECT_STREAM + + return true; +} + +bool Factory::Register(const RTTI **inRTTIs, uint inNumber) +{ + mClassHashMap.reserve(mClassHashMap.size() + inNumber); + mClassNameMap.reserve(mClassNameMap.size() + inNumber); + + for (const RTTI **rtti = inRTTIs; rtti < inRTTIs + inNumber; ++rtti) + if (!Register(*rtti)) + return false; + + return true; +} + +void Factory::Clear() +{ + mClassNameMap.clear(); + mClassHashMap.clear(); +} + +Array Factory::GetAllClasses() const +{ + Array all_classes; + all_classes.reserve(mClassNameMap.size()); + for (const ClassNameMap::value_type &c : mClassNameMap) + all_classes.push_back(c.second); + return all_classes; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/Factory.h b/thirdparty/jolt_physics/Jolt/Core/Factory.h new file mode 100644 index 000000000000..557f38173f6e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/Factory.h @@ -0,0 +1,54 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// This class is responsible for creating instances of classes based on their name or hash and is mainly used for deserialization of saved data. +class JPH_EXPORT Factory +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Create an object + void * CreateObject(const char *inName); + + /// Find type info for a specific class by name + const RTTI * Find(const char *inName); + + /// Find type info for a specific class by hash + const RTTI * Find(uint32 inHash); + + /// Register an object with the factory. Returns false on failure. + bool Register(const RTTI *inRTTI); + + /// Register a list of objects with the factory. Returns false on failure. + bool Register(const RTTI **inRTTIs, uint inNumber); + + /// Unregisters all types + void Clear(); + + /// Get all registered classes + Array GetAllClasses() const; + + /// Singleton factory instance + static Factory * sInstance; + +private: + using ClassNameMap = UnorderedMap; + + using ClassHashMap = UnorderedMap; + + /// Map of class names to type info + ClassNameMap mClassNameMap; + + // Map of class hash to type info + ClassHashMap mClassHashMap; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/FixedSizeFreeList.h b/thirdparty/jolt_physics/Jolt/Core/FixedSizeFreeList.h new file mode 100644 index 000000000000..51f2d1368be9 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/FixedSizeFreeList.h @@ -0,0 +1,122 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that allows lock free creation / destruction of objects (unless a new page of objects needs to be allocated) +/// It contains a fixed pool of objects and also allows batching up a lot of objects to be destroyed +/// and doing the actual free in a single atomic operation +template +class FixedSizeFreeList : public NonCopyable +{ +private: + /// Storage type for an Object + struct ObjectStorage + { + /// The object we're storing + Object mObject; + + /// When the object is freed (or in the process of being freed as a batch) this will contain the next free object + /// When an object is in use it will contain the object's index in the free list + atomic mNextFreeObject; + }; + + static_assert(alignof(ObjectStorage) == alignof(Object), "Object not properly aligned"); + + /// Access the object storage given the object index + const ObjectStorage & GetStorage(uint32 inObjectIndex) const { return mPages[inObjectIndex >> mPageShift][inObjectIndex & mObjectMask]; } + ObjectStorage & GetStorage(uint32 inObjectIndex) { return mPages[inObjectIndex >> mPageShift][inObjectIndex & mObjectMask]; } + + /// Size (in objects) of a single page + uint32 mPageSize; + + /// Number of bits to shift an object index to the right to get the page number + uint32 mPageShift; + + /// Mask to and an object index with to get the page number + uint32 mObjectMask; + + /// Total number of pages that are usable + uint32 mNumPages; + + /// Total number of objects that have been allocated + uint32 mNumObjectsAllocated; + + /// Array of pages of objects + ObjectStorage ** mPages = nullptr; + + /// Mutex that is used to allocate a new page if the storage runs out + /// This variable is aligned to the cache line to prevent false sharing with + /// the constants used to index into the list via `Get()`. + alignas(JPH_CACHE_LINE_SIZE) Mutex mPageMutex; + + /// Number of objects that we currently have in the free list / new pages +#ifdef JPH_ENABLE_ASSERTS + atomic mNumFreeObjects; +#endif // JPH_ENABLE_ASSERTS + + /// Simple counter that makes the first free object pointer update with every CAS so that we don't suffer from the ABA problem + atomic mAllocationTag; + + /// Index of first free object, the first 32 bits of an object are used to point to the next free object + atomic mFirstFreeObjectAndTag; + + /// The first free object to use when the free list is empty (may need to allocate a new page) + atomic mFirstFreeObjectInNewPage; + +public: + /// Invalid index + static const uint32 cInvalidObjectIndex = 0xffffffff; + + /// Size of an object + bookkeeping for the freelist + static const int ObjectStorageSize = sizeof(ObjectStorage); + + /// Destructor + inline ~FixedSizeFreeList(); + + /// Initialize the free list, up to inMaxObjects can be allocated + inline void Init(uint inMaxObjects, uint inPageSize); + + /// Lockless construct a new object, inParameters are passed on to the constructor + template + inline uint32 ConstructObject(Parameters &&... inParameters); + + /// Lockless destruct an object and return it to the free pool + inline void DestructObject(uint32 inObjectIndex); + + /// Lockless destruct an object and return it to the free pool + inline void DestructObject(Object *inObject); + + /// A batch of objects that can be destructed + struct Batch + { + uint32 mFirstObjectIndex = cInvalidObjectIndex; + uint32 mLastObjectIndex = cInvalidObjectIndex; + uint32 mNumObjects = 0; + }; + + /// Add a object to an existing batch to be destructed. + /// Adding objects to a batch does not destroy or modify the objects, this will merely link them + /// so that the entire batch can be returned to the free list in a single atomic operation + inline void AddObjectToBatch(Batch &ioBatch, uint32 inObjectIndex); + + /// Lockless destruct batch of objects + inline void DestructObjectBatch(Batch &ioBatch); + + /// Access an object by index. + inline Object & Get(uint32 inObjectIndex) { return GetStorage(inObjectIndex).mObject; } + + /// Access an object by index. + inline const Object & Get(uint32 inObjectIndex) const { return GetStorage(inObjectIndex).mObject; } +}; + +JPH_NAMESPACE_END + +#include "FixedSizeFreeList.inl" diff --git a/thirdparty/jolt_physics/Jolt/Core/FixedSizeFreeList.inl b/thirdparty/jolt_physics/Jolt/Core/FixedSizeFreeList.inl new file mode 100644 index 000000000000..29d4734c009f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/FixedSizeFreeList.inl @@ -0,0 +1,215 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +JPH_NAMESPACE_BEGIN + +template +FixedSizeFreeList::~FixedSizeFreeList() +{ + // Check if we got our Init call + if (mPages != nullptr) + { + // Ensure everything is freed before the freelist is destructed + JPH_ASSERT(mNumFreeObjects.load(memory_order_relaxed) == mNumPages * mPageSize); + + // Free memory for pages + uint32 num_pages = mNumObjectsAllocated / mPageSize; + for (uint32 page = 0; page < num_pages; ++page) + AlignedFree(mPages[page]); + Free(mPages); + } +} + +template +void FixedSizeFreeList::Init(uint inMaxObjects, uint inPageSize) +{ + // Check sanity + JPH_ASSERT(inPageSize > 0 && IsPowerOf2(inPageSize)); + JPH_ASSERT(mPages == nullptr); + + // Store configuration parameters + mNumPages = (inMaxObjects + inPageSize - 1) / inPageSize; + mPageSize = inPageSize; + mPageShift = CountTrailingZeros(inPageSize); + mObjectMask = inPageSize - 1; + JPH_IF_ENABLE_ASSERTS(mNumFreeObjects = mNumPages * inPageSize;) + + // Allocate page table + mPages = reinterpret_cast(Allocate(mNumPages * sizeof(ObjectStorage *))); + + // We didn't yet use any objects of any page + mNumObjectsAllocated = 0; + mFirstFreeObjectInNewPage = 0; + + // Start with 1 as the first tag + mAllocationTag = 1; + + // Set first free object (with tag 0) + mFirstFreeObjectAndTag = cInvalidObjectIndex; +} + +template +template +uint32 FixedSizeFreeList::ConstructObject(Parameters &&... inParameters) +{ + for (;;) + { + // Get first object from the linked list + uint64 first_free_object_and_tag = mFirstFreeObjectAndTag.load(memory_order_acquire); + uint32 first_free = uint32(first_free_object_and_tag); + if (first_free == cInvalidObjectIndex) + { + // The free list is empty, we take an object from the page that has never been used before + first_free = mFirstFreeObjectInNewPage.fetch_add(1, memory_order_relaxed); + if (first_free >= mNumObjectsAllocated) + { + // Allocate new page + lock_guard lock(mPageMutex); + while (first_free >= mNumObjectsAllocated) + { + uint32 next_page = mNumObjectsAllocated / mPageSize; + if (next_page == mNumPages) + return cInvalidObjectIndex; // Out of space! + mPages[next_page] = reinterpret_cast(AlignedAllocate(mPageSize * sizeof(ObjectStorage), max(alignof(ObjectStorage), JPH_CACHE_LINE_SIZE))); + mNumObjectsAllocated += mPageSize; + } + } + + // Allocation successful + JPH_IF_ENABLE_ASSERTS(mNumFreeObjects.fetch_sub(1, memory_order_relaxed);) + ObjectStorage &storage = GetStorage(first_free); + ::new (&storage.mObject) Object(std::forward(inParameters)...); + storage.mNextFreeObject.store(first_free, memory_order_release); + return first_free; + } + else + { + // Load next pointer + uint32 new_first_free = GetStorage(first_free).mNextFreeObject.load(memory_order_acquire); + + // Construct a new first free object tag + uint64 new_first_free_object_and_tag = uint64(new_first_free) + (uint64(mAllocationTag.fetch_add(1, memory_order_relaxed)) << 32); + + // Compare and swap + if (mFirstFreeObjectAndTag.compare_exchange_weak(first_free_object_and_tag, new_first_free_object_and_tag, memory_order_release)) + { + // Allocation successful + JPH_IF_ENABLE_ASSERTS(mNumFreeObjects.fetch_sub(1, memory_order_relaxed);) + ObjectStorage &storage = GetStorage(first_free); + ::new (&storage.mObject) Object(std::forward(inParameters)...); + storage.mNextFreeObject.store(first_free, memory_order_release); + return first_free; + } + } + } +} + +template +void FixedSizeFreeList::AddObjectToBatch(Batch &ioBatch, uint32 inObjectIndex) +{ + JPH_ASSERT(ioBatch.mNumObjects != uint32(-1), "Trying to reuse a batch that has already been freed"); + + // Reset next index + atomic &next_free_object = GetStorage(inObjectIndex).mNextFreeObject; + JPH_ASSERT(next_free_object.load(memory_order_relaxed) == inObjectIndex, "Trying to add a object to the batch that is already in a free list"); + next_free_object.store(cInvalidObjectIndex, memory_order_release); + + // Link object in batch to free + if (ioBatch.mFirstObjectIndex == cInvalidObjectIndex) + ioBatch.mFirstObjectIndex = inObjectIndex; + else + GetStorage(ioBatch.mLastObjectIndex).mNextFreeObject.store(inObjectIndex, memory_order_release); + ioBatch.mLastObjectIndex = inObjectIndex; + ioBatch.mNumObjects++; +} + +template +void FixedSizeFreeList::DestructObjectBatch(Batch &ioBatch) +{ + if (ioBatch.mFirstObjectIndex != cInvalidObjectIndex) + { + // Call destructors + if constexpr (!std::is_trivially_destructible()) + { + uint32 object_idx = ioBatch.mFirstObjectIndex; + do + { + ObjectStorage &storage = GetStorage(object_idx); + storage.mObject.~Object(); + object_idx = storage.mNextFreeObject.load(memory_order_relaxed); + } + while (object_idx != cInvalidObjectIndex); + } + + // Add to objects free list + ObjectStorage &storage = GetStorage(ioBatch.mLastObjectIndex); + for (;;) + { + // Get first object from the list + uint64 first_free_object_and_tag = mFirstFreeObjectAndTag.load(memory_order_acquire); + uint32 first_free = uint32(first_free_object_and_tag); + + // Make it the next pointer of the last object in the batch that is to be freed + storage.mNextFreeObject.store(first_free, memory_order_release); + + // Construct a new first free object tag + uint64 new_first_free_object_and_tag = uint64(ioBatch.mFirstObjectIndex) + (uint64(mAllocationTag.fetch_add(1, memory_order_relaxed)) << 32); + + // Compare and swap + if (mFirstFreeObjectAndTag.compare_exchange_weak(first_free_object_and_tag, new_first_free_object_and_tag, memory_order_release)) + { + // Free successful + JPH_IF_ENABLE_ASSERTS(mNumFreeObjects.fetch_add(ioBatch.mNumObjects, memory_order_relaxed);) + + // Mark the batch as freed +#ifdef JPH_ENABLE_ASSERTS + ioBatch.mNumObjects = uint32(-1); +#endif + return; + } + } + } +} + +template +void FixedSizeFreeList::DestructObject(uint32 inObjectIndex) +{ + JPH_ASSERT(inObjectIndex != cInvalidObjectIndex); + + // Call destructor + ObjectStorage &storage = GetStorage(inObjectIndex); + storage.mObject.~Object(); + + // Add to object free list + for (;;) + { + // Get first object from the list + uint64 first_free_object_and_tag = mFirstFreeObjectAndTag.load(memory_order_acquire); + uint32 first_free = uint32(first_free_object_and_tag); + + // Make it the next pointer of the last object in the batch that is to be freed + storage.mNextFreeObject.store(first_free, memory_order_release); + + // Construct a new first free object tag + uint64 new_first_free_object_and_tag = uint64(inObjectIndex) + (uint64(mAllocationTag.fetch_add(1, memory_order_relaxed)) << 32); + + // Compare and swap + if (mFirstFreeObjectAndTag.compare_exchange_weak(first_free_object_and_tag, new_first_free_object_and_tag, memory_order_release)) + { + // Free successful + JPH_IF_ENABLE_ASSERTS(mNumFreeObjects.fetch_add(1, memory_order_relaxed);) + return; + } + } +} + +template +inline void FixedSizeFreeList::DestructObject(Object *inObject) +{ + uint32 index = reinterpret_cast(inObject)->mNextFreeObject.load(memory_order_relaxed); + JPH_ASSERT(index < mNumObjectsAllocated); + DestructObject(index); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/HashCombine.h b/thirdparty/jolt_physics/Jolt/Core/HashCombine.h new file mode 100644 index 000000000000..e04b54b7443f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/HashCombine.h @@ -0,0 +1,212 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Implements the FNV-1a hash algorithm +/// @see https://en.wikipedia.org/wiki/Fowler%E2%80%93Noll%E2%80%93Vo_hash_function +/// @param inData Data block of bytes +/// @param inSize Number of bytes +/// @param inSeed Seed of the hash (can be used to pass in the hash of a previous operation, otherwise leave default) +/// @return Hash +inline uint64 HashBytes(const void *inData, uint inSize, uint64 inSeed = 0xcbf29ce484222325UL) +{ + uint64 hash = inSeed; + for (const uint8 *data = reinterpret_cast(inData); data < reinterpret_cast(inData) + inSize; ++data) + { + hash = hash ^ uint64(*data); + hash = hash * 0x100000001b3UL; + } + return hash; +} + +/// Calculate the FNV-1a hash of inString. +/// @see https://en.wikipedia.org/wiki/Fowler%E2%80%93Noll%E2%80%93Vo_hash_function +constexpr uint64 HashString(const char *inString, uint64 inSeed = 0xcbf29ce484222325UL) +{ + uint64 hash = inSeed; + for (const char *c = inString; *c != 0; ++c) + { + hash ^= uint64(*c); + hash = hash * 0x100000001b3UL; + } + return hash; +} + +/// A 64 bit hash function by Thomas Wang, Jan 1997 +/// See: http://web.archive.org/web/20071223173210/http://www.concentric.net/~Ttwang/tech/inthash.htm +/// @param inValue Value to hash +/// @return Hash +inline uint64 Hash64(uint64 inValue) +{ + uint64 hash = inValue; + hash = (~hash) + (hash << 21); // hash = (hash << 21) - hash - 1; + hash = hash ^ (hash >> 24); + hash = (hash + (hash << 3)) + (hash << 8); // hash * 265 + hash = hash ^ (hash >> 14); + hash = (hash + (hash << 2)) + (hash << 4); // hash * 21 + hash = hash ^ (hash >> 28); + hash = hash + (hash << 31); + return hash; +} + +/// Fallback hash function that calls T::GetHash() +template +struct Hash +{ + uint64 operator () (const T &inValue) const + { + return inValue.GetHash(); + } +}; + +/// A hash function for floats +template <> +struct Hash +{ + uint64 operator () (float inValue) const + { + float value = inValue == 0.0f? 0.0f : inValue; // Convert -0.0f to 0.0f + return HashBytes(&value, sizeof(value)); + } +}; + +/// A hash function for doubles +template <> +struct Hash +{ + uint64 operator () (double inValue) const + { + double value = inValue == 0.0? 0.0 : inValue; // Convert -0.0 to 0.0 + return HashBytes(&value, sizeof(value)); + } +}; + +/// A hash function for character pointers +template <> +struct Hash +{ + uint64 operator () (const char *inValue) const + { + return HashString(inValue); + } +}; + +/// A hash function for std::string_view +template <> +struct Hash +{ + uint64 operator () (const std::string_view &inValue) const + { + return HashBytes(inValue.data(), uint(inValue.size())); + } +}; + +/// A hash function for String +template <> +struct Hash +{ + uint64 operator () (const String &inValue) const + { + return HashBytes(inValue.data(), uint(inValue.size())); + } +}; + +/// A fallback function for generic pointers +template +struct Hash +{ + uint64 operator () (T *inValue) const + { + return HashBytes(&inValue, sizeof(inValue)); + } +}; + +/// Helper macro to define a hash function for trivial types +#define JPH_DEFINE_TRIVIAL_HASH(type) \ +template <> \ +struct Hash \ +{ \ + uint64 operator () (const type &inValue) const \ + { \ + return HashBytes(&inValue, sizeof(inValue)); \ + } \ +}; + +/// Commonly used types +JPH_DEFINE_TRIVIAL_HASH(char) +JPH_DEFINE_TRIVIAL_HASH(int) +JPH_DEFINE_TRIVIAL_HASH(uint32) +JPH_DEFINE_TRIVIAL_HASH(uint64) + +/// @brief Helper function that hashes a single value into ioSeed +/// Taken from: https://stackoverflow.com/questions/2590677/how-do-i-combine-hash-values-in-c0x +template +inline void HashCombine(uint64 &ioSeed, const T &inValue) +{ + ioSeed ^= Hash { } (inValue) + 0x9e3779b9 + (ioSeed << 6) + (ioSeed >> 2); +} + +/// Hash combiner to use a custom struct in an unordered map or set +/// +/// Usage: +/// +/// struct SomeHashKey +/// { +/// std::string key1; +/// std::string key2; +/// bool key3; +/// }; +/// +/// JPH_MAKE_HASHABLE(SomeHashKey, t.key1, t.key2, t.key3) +template +inline uint64 HashCombineArgs(const FirstValue &inFirstValue, Values... inValues) +{ + // Prime the seed by hashing the first value + uint64 seed = Hash { } (inFirstValue); + + // Hash all remaining values together using a fold expression + (HashCombine(seed, inValues), ...); + + return seed; +} + +JPH_NAMESPACE_END + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic") + +#define JPH_MAKE_HASH_STRUCT(type, name, ...) \ + struct [[nodiscard]] name \ + { \ + ::JPH::uint64 operator()(const type &t) const \ + { \ + return ::JPH::HashCombineArgs(__VA_ARGS__); \ + } \ + }; + +#define JPH_MAKE_HASHABLE(type, ...) \ + JPH_SUPPRESS_WARNING_PUSH \ + JPH_SUPPRESS_WARNINGS \ + namespace JPH \ + { \ + template<> \ + JPH_MAKE_HASH_STRUCT(type, Hash, __VA_ARGS__) \ + } \ + namespace std \ + { \ + template<> \ + struct [[nodiscard]] hash \ + { \ + std::size_t operator()(const type &t) const \ + { \ + return std::size_t(::JPH::Hash{ }(t));\ + } \ + }; \ + } \ + JPH_SUPPRESS_WARNING_POP + +JPH_SUPPRESS_WARNING_POP diff --git a/thirdparty/jolt_physics/Jolt/Core/HashTable.h b/thirdparty/jolt_physics/Jolt/Core/HashTable.h new file mode 100644 index 000000000000..baca08255baa --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/HashTable.h @@ -0,0 +1,854 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Helper class for implementing an UnorderedSet or UnorderedMap +/// Based on CppCon 2017: Matt Kulukundis "Designing a Fast, Efficient, Cache-friendly Hash Table, Step by Step" +/// See: https://www.youtube.com/watch?v=ncHmEUmJZf4 +template +class HashTable +{ +public: + /// Properties + using value_type = KeyValue; + using size_type = uint32; + using difference_type = ptrdiff_t; + +private: + /// Base class for iterators + template + class IteratorBase + { + public: + /// Properties + using difference_type = typename Table::difference_type; + using value_type = typename Table::value_type; + using iterator_category = std::forward_iterator_tag; + + /// Copy constructor + IteratorBase(const IteratorBase &inRHS) = default; + + /// Assignment operator + IteratorBase & operator = (const IteratorBase &inRHS) = default; + + /// Iterator at start of table + explicit IteratorBase(Table *inTable) : + mTable(inTable), + mIndex(0) + { + while (mIndex < mTable->mMaxSize && (mTable->mControl[mIndex] & cBucketUsed) == 0) + ++mIndex; + } + + /// Iterator at specific index + IteratorBase(Table *inTable, size_type inIndex) : + mTable(inTable), + mIndex(inIndex) + { + } + + /// Prefix increment + Iterator & operator ++ () + { + JPH_ASSERT(IsValid()); + + do + { + ++mIndex; + } + while (mIndex < mTable->mMaxSize && (mTable->mControl[mIndex] & cBucketUsed) == 0); + + return static_cast(*this); + } + + /// Postfix increment + Iterator operator ++ (int) + { + Iterator result(mTable, mIndex); + ++(*this); + return result; + } + + /// Access to key value pair + const KeyValue & operator * () const + { + JPH_ASSERT(IsValid()); + return mTable->mData[mIndex]; + } + + /// Access to key value pair + const KeyValue * operator -> () const + { + JPH_ASSERT(IsValid()); + return mTable->mData + mIndex; + } + + /// Equality operator + bool operator == (const Iterator &inRHS) const + { + return mIndex == inRHS.mIndex && mTable == inRHS.mTable; + } + + /// Inequality operator + bool operator != (const Iterator &inRHS) const + { + return !(*this == inRHS); + } + + /// Check that the iterator is valid + bool IsValid() const + { + return mIndex < mTable->mMaxSize + && (mTable->mControl[mIndex] & cBucketUsed) != 0; + } + + Table * mTable; + size_type mIndex; + }; + + /// Get the maximum number of elements that we can support given a number of buckets + static constexpr size_type sGetMaxLoad(size_type inBucketCount) + { + return uint32((cMaxLoadFactorNumerator * inBucketCount) / cMaxLoadFactorDenominator); + } + + /// Update the control value for a bucket + JPH_INLINE void SetControlValue(size_type inIndex, uint8 inValue) + { + JPH_ASSERT(inIndex < mMaxSize); + mControl[inIndex] = inValue; + + // Mirror the first 15 bytes to the 15 bytes beyond mMaxSize + // Note that this is equivalent to: + // if (inIndex < 15) + // mControl[inIndex + mMaxSize] = inValue + // else + // mControl[inIndex] = inValue + // Which performs a needless write if inIndex >= 15 but at least it is branch-less + mControl[((inIndex - 15) & (mMaxSize - 1)) + 15] = inValue; + } + + /// Get the index and control value for a particular key + JPH_INLINE void GetIndexAndControlValue(const Key &inKey, size_type &outIndex, uint8 &outControl) const + { + // Calculate hash + uint64 hash_value = Hash { } (inKey); + + // Split hash into index and control value + outIndex = size_type(hash_value >> 7) & (mMaxSize - 1); + outControl = cBucketUsed | uint8(hash_value); + } + + /// Allocate space for the hash table + void AllocateTable(size_type inMaxSize) + { + JPH_ASSERT(mData == nullptr); + + mMaxSize = inMaxSize; + mLoadLeft = sGetMaxLoad(inMaxSize); + size_t required_size = size_t(mMaxSize) * (sizeof(KeyValue) + 1) + 15; // Add 15 bytes to mirror the first 15 bytes of the control values + if constexpr (cNeedsAlignedAllocate) + mData = reinterpret_cast(AlignedAllocate(required_size, alignof(KeyValue))); + else + mData = reinterpret_cast(Allocate(required_size)); + mControl = reinterpret_cast(mData + mMaxSize); + } + + /// Copy the contents of another hash table + void CopyTable(const HashTable &inRHS) + { + if (inRHS.empty()) + return; + + AllocateTable(inRHS.mMaxSize); + + // Copy control bytes + memcpy(mControl, inRHS.mControl, mMaxSize + 15); + + // Copy elements + uint index = 0; + for (const uint8 *control = mControl, *control_end = mControl + mMaxSize; control != control_end; ++control, ++index) + if (*control & cBucketUsed) + ::new (mData + index) KeyValue(inRHS.mData[index]); + mSize = inRHS.mSize; + } + + /// Grow the table to the next power of 2 + void GrowTable() + { + // Calculate new size + size_type new_max_size = max(mMaxSize << 1, 16); + if (new_max_size < mMaxSize) + { + JPH_ASSERT(false, "Overflow in hash table size, can't grow!"); + return; + } + + // Move the old table to a temporary structure + size_type old_max_size = mMaxSize; + KeyValue *old_data = mData; + const uint8 *old_control = mControl; + mData = nullptr; + mControl = nullptr; + mSize = 0; + mMaxSize = 0; + mLoadLeft = 0; + + // Allocate new table + AllocateTable(new_max_size); + + // Reset all control bytes + memset(mControl, cBucketEmpty, mMaxSize + 15); + + if (old_data != nullptr) + { + // Copy all elements from the old table + for (size_type i = 0; i < old_max_size; ++i) + if (old_control[i] & cBucketUsed) + { + size_type index; + KeyValue *element = old_data + i; + JPH_IF_ENABLE_ASSERTS(bool inserted =) InsertKey(HashTableDetail::sGetKey(*element), index); + JPH_ASSERT(inserted); + ::new (mData + index) KeyValue(std::move(*element)); + element->~KeyValue(); + } + + // Free memory + if constexpr (cNeedsAlignedAllocate) + AlignedFree(old_data); + else + Free(old_data); + } + } + +protected: + /// Get an element by index + KeyValue & GetElement(size_type inIndex) const + { + return mData[inIndex]; + } + + /// Insert a key into the map, returns true if the element was inserted, false if it already existed. + /// outIndex is the index at which the element should be constructed / where it is located. + template + bool InsertKey(const Key &inKey, size_type &outIndex) + { + // Ensure we have enough space + if (mLoadLeft == 0) + { + // Should not be growing if we're already growing! + if constexpr (InsertAfterGrow) + JPH_ASSERT(false); + + // Decide if we need to clean up all tombstones or if we need to grow the map + size_type num_deleted = sGetMaxLoad(mMaxSize) - mSize; + if (num_deleted * cMaxDeletedElementsDenominator > mMaxSize * cMaxDeletedElementsNumerator) + rehash(0); + else + GrowTable(); + } + + // Split hash into index and control value + size_type index; + uint8 control; + GetIndexAndControlValue(inKey, index, control); + + // Keeps track of the index of the first deleted bucket we found + constexpr size_type cNoDeleted = ~size_type(0); + size_type first_deleted_index = cNoDeleted; + + // Linear probing + KeyEqual equal; + size_type bucket_mask = mMaxSize - 1; + BVec16 control16 = BVec16::sReplicate(control); + BVec16 bucket_empty = BVec16::sZero(); + BVec16 bucket_deleted = BVec16::sReplicate(cBucketDeleted); + for (;;) + { + // Read 16 control values (note that we added 15 bytes at the end of the control values that mirror the first 15 bytes) + BVec16 control_bytes = BVec16::sLoadByte16(mControl + index); + + // Check if we must find the element before we can insert + if constexpr (!InsertAfterGrow) + { + // Check for the control value we're looking for + // Note that when deleting we can create empty buckets instead of deleted buckets. + // This means we must unconditionally check all buckets in this batch for equality + // (also beyond the first empty bucket). + uint32 control_equal = uint32(BVec16::sEquals(control_bytes, control16).GetTrues()); + + // Index within the 16 buckets + size_type local_index = index; + + // Loop while there's still buckets to process + while (control_equal != 0) + { + // Get the first equal bucket + uint first_equal = CountTrailingZeros(control_equal); + + // Skip to the bucket + local_index += first_equal; + + // Make sure that our index is not beyond the end of the table + local_index &= bucket_mask; + + // We found a bucket with same control value + if (equal(HashTableDetail::sGetKey(mData[local_index]), inKey)) + { + // Element already exists + outIndex = local_index; + return false; + } + + // Skip past this bucket + control_equal >>= first_equal + 1; + local_index++; + } + + // Check if we're still scanning for deleted buckets + if (first_deleted_index == cNoDeleted) + { + // Check if any buckets have been deleted, if so store the first one + uint32 control_deleted = uint32(BVec16::sEquals(control_bytes, bucket_deleted).GetTrues()); + if (control_deleted != 0) + first_deleted_index = index + CountTrailingZeros(control_deleted); + } + } + + // Check for empty buckets + uint32 control_empty = uint32(BVec16::sEquals(control_bytes, bucket_empty).GetTrues()); + if (control_empty != 0) + { + // If we found a deleted bucket, use it. + // It doesn't matter if it is before or after the first empty bucket we found + // since we will always be scanning in batches of 16 buckets. + if (first_deleted_index == cNoDeleted || InsertAfterGrow) + { + index += CountTrailingZeros(control_empty); + --mLoadLeft; // Using an empty bucket decreases the load left + } + else + { + index = first_deleted_index; + } + + // Make sure that our index is not beyond the end of the table + index &= bucket_mask; + + // Update control byte + SetControlValue(index, control); + ++mSize; + + // Return index to newly allocated bucket + outIndex = index; + return true; + } + + // Move to next batch of 16 buckets + index = (index + 16) & bucket_mask; + } + } + +public: + /// Non-const iterator + class iterator : public IteratorBase + { + using Base = IteratorBase; + + public: + /// Properties + using reference = typename Base::value_type &; + using pointer = typename Base::value_type *; + + /// Constructors + explicit iterator(HashTable *inTable) : Base(inTable) { } + iterator(HashTable *inTable, size_type inIndex) : Base(inTable, inIndex) { } + iterator(const iterator &inIterator) : Base(inIterator) { } + + /// Assignment + iterator & operator = (const iterator &inRHS) { Base::operator = (inRHS); return *this; } + + using Base::operator *; + + /// Non-const access to key value pair + KeyValue & operator * () + { + JPH_ASSERT(this->IsValid()); + return this->mTable->mData[this->mIndex]; + } + + using Base::operator ->; + + /// Non-const access to key value pair + KeyValue * operator -> () + { + JPH_ASSERT(this->IsValid()); + return this->mTable->mData + this->mIndex; + } + }; + + /// Const iterator + class const_iterator : public IteratorBase + { + using Base = IteratorBase; + + public: + /// Properties + using reference = const typename Base::value_type &; + using pointer = const typename Base::value_type *; + + /// Constructors + explicit const_iterator(const HashTable *inTable) : Base(inTable) { } + const_iterator(const HashTable *inTable, size_type inIndex) : Base(inTable, inIndex) { } + const_iterator(const const_iterator &inRHS) : Base(inRHS) { } + const_iterator(const iterator &inIterator) : Base(inIterator.mTable, inIterator.mIndex) { } + + /// Assignment + const_iterator & operator = (const iterator &inRHS) { this->mTable = inRHS.mTable; this->mIndex = inRHS.mIndex; return *this; } + const_iterator & operator = (const const_iterator &inRHS) { Base::operator = (inRHS); return *this; } + }; + + /// Default constructor + HashTable() = default; + + /// Copy constructor + HashTable(const HashTable &inRHS) + { + CopyTable(inRHS); + } + + /// Move constructor + HashTable(HashTable &&ioRHS) noexcept : + mData(ioRHS.mData), + mControl(ioRHS.mControl), + mSize(ioRHS.mSize), + mMaxSize(ioRHS.mMaxSize), + mLoadLeft(ioRHS.mLoadLeft) + { + ioRHS.mData = nullptr; + ioRHS.mControl = nullptr; + ioRHS.mSize = 0; + ioRHS.mMaxSize = 0; + ioRHS.mLoadLeft = 0; + } + + /// Assignment operator + HashTable & operator = (const HashTable &inRHS) + { + if (this != &inRHS) + { + clear(); + + CopyTable(inRHS); + } + + return *this; + } + + /// Move assignment operator + HashTable & operator = (HashTable &&ioRHS) noexcept + { + if (this != &ioRHS) + { + clear(); + + mData = ioRHS.mData; + mControl = ioRHS.mControl; + mSize = ioRHS.mSize; + mMaxSize = ioRHS.mMaxSize; + mLoadLeft = ioRHS.mLoadLeft; + + ioRHS.mData = nullptr; + ioRHS.mControl = nullptr; + ioRHS.mSize = 0; + ioRHS.mMaxSize = 0; + ioRHS.mLoadLeft = 0; + } + + return *this; + } + + /// Destructor + ~HashTable() + { + clear(); + } + + /// Reserve memory for a certain number of elements + void reserve(size_type inMaxSize) + { + // Calculate max size based on load factor + size_type max_size = GetNextPowerOf2(max((cMaxLoadFactorDenominator * inMaxSize) / cMaxLoadFactorNumerator, 16)); + if (max_size <= mMaxSize) + return; + + // Allocate buffers + AllocateTable(max_size); + + // Reset all control bytes + memset(mControl, cBucketEmpty, mMaxSize + 15); + } + + /// Destroy the entire hash table + void clear() + { + // Delete all elements + if constexpr (!std::is_trivially_destructible()) + if (!empty()) + for (size_type i = 0; i < mMaxSize; ++i) + if (mControl[i] & cBucketUsed) + mData[i].~KeyValue(); + + if (mData != nullptr) + { + // Free memory + if constexpr (cNeedsAlignedAllocate) + AlignedFree(mData); + else + Free(mData); + + // Reset members + mData = nullptr; + mControl = nullptr; + mSize = 0; + mMaxSize = 0; + mLoadLeft = 0; + } + } + + /// Iterator to first element + iterator begin() + { + return iterator(this); + } + + /// Iterator to one beyond last element + iterator end() + { + return iterator(this, mMaxSize); + } + + /// Iterator to first element + const_iterator begin() const + { + return const_iterator(this); + } + + /// Iterator to one beyond last element + const_iterator end() const + { + return const_iterator(this, mMaxSize); + } + + /// Iterator to first element + const_iterator cbegin() const + { + return const_iterator(this); + } + + /// Iterator to one beyond last element + const_iterator cend() const + { + return const_iterator(this, mMaxSize); + } + + /// Number of buckets in the table + size_type bucket_count() const + { + return mMaxSize; + } + + /// Max number of buckets that the table can have + constexpr size_type max_bucket_count() const + { + return size_type(1) << (sizeof(size_type) * 8 - 1); + } + + /// Check if there are no elements in the table + bool empty() const + { + return mSize == 0; + } + + /// Number of elements in the table + size_type size() const + { + return mSize; + } + + /// Max number of elements that the table can hold + constexpr size_type max_size() const + { + return size_type((uint64(max_bucket_count()) * cMaxLoadFactorNumerator) / cMaxLoadFactorDenominator); + } + + /// Get the max load factor for this table (max number of elements / number of buckets) + constexpr float max_load_factor() const + { + return float(cMaxLoadFactorNumerator) / float(cMaxLoadFactorDenominator); + } + + /// Insert a new element, returns iterator and if the element was inserted + std::pair insert(const value_type &inValue) + { + size_type index; + bool inserted = InsertKey(HashTableDetail::sGetKey(inValue), index); + if (inserted) + ::new (mData + index) KeyValue(inValue); + return std::make_pair(iterator(this, index), inserted); + } + + /// Find an element, returns iterator to element or end() if not found + const_iterator find(const Key &inKey) const + { + // Check if we have any data + if (empty()) + return cend(); + + // Split hash into index and control value + size_type index; + uint8 control; + GetIndexAndControlValue(inKey, index, control); + + // Linear probing + KeyEqual equal; + size_type bucket_mask = mMaxSize - 1; + BVec16 control16 = BVec16::sReplicate(control); + BVec16 bucket_empty = BVec16::sZero(); + for (;;) + { + // Read 16 control values + // (note that we added 15 bytes at the end of the control values that mirror the first 15 bytes) + BVec16 control_bytes = BVec16::sLoadByte16(mControl + index); + + // Check for the control value we're looking for + // Note that when deleting we can create empty buckets instead of deleted buckets. + // This means we must unconditionally check all buckets in this batch for equality + // (also beyond the first empty bucket). + uint32 control_equal = uint32(BVec16::sEquals(control_bytes, control16).GetTrues()); + + // Index within the 16 buckets + size_type local_index = index; + + // Loop while there's still buckets to process + while (control_equal != 0) + { + // Get the first equal bucket + uint first_equal = CountTrailingZeros(control_equal); + + // Skip to the bucket + local_index += first_equal; + + // Make sure that our index is not beyond the end of the table + local_index &= bucket_mask; + + // We found a bucket with same control value + if (equal(HashTableDetail::sGetKey(mData[local_index]), inKey)) + { + // Element found + return const_iterator(this, local_index); + } + + // Skip past this bucket + control_equal >>= first_equal + 1; + local_index++; + } + + // Check for empty buckets + uint32 control_empty = uint32(BVec16::sEquals(control_bytes, bucket_empty).GetTrues()); + if (control_empty != 0) + { + // An empty bucket was found, we didn't find the element + return cend(); + } + + // Move to next batch of 16 buckets + index = (index + 16) & bucket_mask; + } + } + + /// @brief Erase an element by iterator + void erase(const const_iterator &inIterator) + { + JPH_ASSERT(inIterator.IsValid()); + + // Read 16 control values before and after the current index + // (note that we added 15 bytes at the end of the control values that mirror the first 15 bytes) + BVec16 control_bytes_before = BVec16::sLoadByte16(mControl + ((inIterator.mIndex - 16) & (mMaxSize - 1))); + BVec16 control_bytes_after = BVec16::sLoadByte16(mControl + inIterator.mIndex); + BVec16 bucket_empty = BVec16::sZero(); + uint32 control_empty_before = uint32(BVec16::sEquals(control_bytes_before, bucket_empty).GetTrues()); + uint32 control_empty_after = uint32(BVec16::sEquals(control_bytes_after, bucket_empty).GetTrues()); + + // If (this index including) there exist 16 consecutive non-empty slots (represented by a bit being 0) then + // a probe looking for some element needs to continue probing so we cannot mark the bucket as empty + // but must mark it as deleted instead. + // Note that we use: CountLeadingZeros(uint16) = CountLeadingZeros(uint32) - 16. + uint8 control_value = CountLeadingZeros(control_empty_before) - 16 + CountTrailingZeros(control_empty_after) < 16? cBucketEmpty : cBucketDeleted; + + // Mark the bucket as empty/deleted + SetControlValue(inIterator.mIndex, control_value); + + // Destruct the element + mData[inIterator.mIndex].~KeyValue(); + + // If we marked the bucket as empty we can increase the load left + if (control_value == cBucketEmpty) + ++mLoadLeft; + + // Decrease size + --mSize; + } + + /// @brief Erase an element by key + size_type erase(const Key &inKey) + { + const_iterator it = find(inKey); + if (it == cend()) + return 0; + + erase(it); + return 1; + } + + /// Swap the contents of two hash tables + void swap(HashTable &ioRHS) noexcept + { + std::swap(mData, ioRHS.mData); + std::swap(mControl, ioRHS.mControl); + std::swap(mSize, ioRHS.mSize); + std::swap(mMaxSize, ioRHS.mMaxSize); + std::swap(mLoadLeft, ioRHS.mLoadLeft); + } + + /// In place re-hashing of all elements in the table. Removes all cBucketDeleted elements + /// The std version takes a bucket count, but we just re-hash to the same size. + void rehash(size_type) + { + // Update the control value for all buckets + for (size_type i = 0; i < mMaxSize; ++i) + { + uint8 &control = mControl[i]; + switch (control) + { + case cBucketDeleted: + // Deleted buckets become empty + control = cBucketEmpty; + break; + case cBucketEmpty: + // Remains empty + break; + default: + // Mark all occupied as deleted, to indicate it needs to move to the correct place + control = cBucketDeleted; + break; + } + } + + // Replicate control values to the last 15 entries + for (size_type i = 0; i < 15; ++i) + mControl[mMaxSize + i] = mControl[i]; + + // Loop over all elements that have been 'deleted' and move them to their new spot + BVec16 bucket_used = BVec16::sReplicate(cBucketUsed); + size_type bucket_mask = mMaxSize - 1; + uint32 probe_mask = bucket_mask & ~uint32(0b1111); // Mask out lower 4 bits because we test 16 buckets at a time + for (size_type src = 0; src < mMaxSize; ++src) + if (mControl[src] == cBucketDeleted) + for (;;) + { + // Split hash into index and control value + size_type src_index; + uint8 src_control; + GetIndexAndControlValue(HashTableDetail::sGetKey(mData[src]), src_index, src_control); + + // Linear probing + size_type dst = src_index; + for (;;) + { + // Check if any buckets are free + BVec16 control_bytes = BVec16::sLoadByte16(mControl + dst); + uint32 control_free = uint32(BVec16::sAnd(control_bytes, bucket_used).GetTrues()) ^ 0xffff; + if (control_free != 0) + { + // Select this bucket as destination + dst += CountTrailingZeros(control_free); + dst &= bucket_mask; + break; + } + + // Move to next batch of 16 buckets + dst = (dst + 16) & bucket_mask; + } + + // Check if we stay in the same probe group + if (((dst - src_index) & probe_mask) == ((src - src_index) & probe_mask)) + { + // We stay in the same group, we can stay where we are + SetControlValue(src, src_control); + break; + } + else if (mControl[dst] == cBucketEmpty) + { + // There's an empty bucket, move us there + SetControlValue(dst, src_control); + SetControlValue(src, cBucketEmpty); + ::new (mData + dst) KeyValue(std::move(mData[src])); + mData[src].~KeyValue(); + break; + } + else + { + // There's an element in the bucket we want to move to, swap them + JPH_ASSERT(mControl[dst] == cBucketDeleted); + SetControlValue(dst, src_control); + std::swap(mData[src], mData[dst]); + // Iterate again with the same source bucket + } + } + + // Reinitialize load left + mLoadLeft = sGetMaxLoad(mMaxSize) - mSize; + } + +private: + /// If this allocator needs to fall back to aligned allocations because the type requires it + static constexpr bool cNeedsAlignedAllocate = alignof(KeyValue) > (JPH_CPU_ADDRESS_BITS == 32? 8 : 16); + + /// Max load factor is cMaxLoadFactorNumerator / cMaxLoadFactorDenominator + static constexpr uint64 cMaxLoadFactorNumerator = 7; + static constexpr uint64 cMaxLoadFactorDenominator = 8; + + /// If we can recover this fraction of deleted elements, we'll reshuffle the buckets in place rather than growing the table + static constexpr uint64 cMaxDeletedElementsNumerator = 1; + static constexpr uint64 cMaxDeletedElementsDenominator = 8; + + /// Values that the control bytes can have + static constexpr uint8 cBucketEmpty = 0; + static constexpr uint8 cBucketDeleted = 0x7f; + static constexpr uint8 cBucketUsed = 0x80; // Lowest 7 bits are lowest 7 bits of the hash value + + /// The buckets, an array of size mMaxSize + KeyValue * mData = nullptr; + + /// Control bytes, an array of size mMaxSize + 15 + uint8 * mControl = nullptr; + + /// Number of elements in the table + size_type mSize = 0; + + /// Max number of elements that can be stored in the table + size_type mMaxSize = 0; + + /// Number of elements we can add to the table before we need to grow + size_type mLoadLeft = 0; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/InsertionSort.h b/thirdparty/jolt_physics/Jolt/Core/InsertionSort.h new file mode 100644 index 000000000000..8cd8798be20c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/InsertionSort.h @@ -0,0 +1,58 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Implementation of the insertion sort algorithm. +template +inline void InsertionSort(Iterator inBegin, Iterator inEnd, Compare inCompare) +{ + // Empty arrays don't need to be sorted + if (inBegin != inEnd) + { + // Start at the second element + for (Iterator i = inBegin + 1; i != inEnd; ++i) + { + // Move this element to a temporary value + auto x = std::move(*i); + + // Check if the element goes before inBegin (we can't decrement the iterator before inBegin so this needs to be a separate branch) + if (inCompare(x, *inBegin)) + { + // Move all elements to the right to make space for x + Iterator prev; + for (Iterator j = i; j != inBegin; j = prev) + { + prev = j - 1; + *j = *prev; + } + + // Move x to the first place + *inBegin = std::move(x); + } + else + { + // Move elements to the right as long as they are bigger than x + Iterator j = i; + for (Iterator prev = j - 1; inCompare(x, *prev); j = prev, --prev) + *j = std::move(*prev); + + // Move x into place + *j = std::move(x); + } + } + } +} + +/// Implementation of insertion sort algorithm without comparator. +template +inline void InsertionSort(Iterator inBegin, Iterator inEnd) +{ + std::less<> compare; + InsertionSort(inBegin, inEnd, compare); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/IssueReporting.cpp b/thirdparty/jolt_physics/Jolt/Core/IssueReporting.cpp new file mode 100644 index 000000000000..ff32448000f8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/IssueReporting.cpp @@ -0,0 +1,27 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +JPH_NAMESPACE_BEGIN + +static void DummyTrace([[maybe_unused]] const char *inFMT, ...) +{ + JPH_ASSERT(false); +}; + +TraceFunction Trace = DummyTrace; + +#ifdef JPH_ENABLE_ASSERTS + +static bool DummyAssertFailed(const char *inExpression, const char *inMessage, const char *inFile, uint inLine) +{ + return true; // Trigger breakpoint +}; + +AssertFailedFunction AssertFailed = DummyAssertFailed; + +#endif // JPH_ENABLE_ASSERTS + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/IssueReporting.h b/thirdparty/jolt_physics/Jolt/Core/IssueReporting.h new file mode 100644 index 000000000000..efbf9a0d88c2 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/IssueReporting.h @@ -0,0 +1,38 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Trace function, needs to be overridden by application. This should output a line of text to the log / TTY. +using TraceFunction = void (*)(const char *inFMT, ...); +JPH_EXPORT extern TraceFunction Trace; + +// Always turn on asserts in Debug mode +#if defined(JPH_DEBUG) && !defined(JPH_ENABLE_ASSERTS) + #define JPH_ENABLE_ASSERTS +#endif + +#ifdef JPH_ENABLE_ASSERTS + /// Function called when an assertion fails. This function should return true if a breakpoint needs to be triggered + using AssertFailedFunction = bool(*)(const char *inExpression, const char *inMessage, const char *inFile, uint inLine); + JPH_EXPORT extern AssertFailedFunction AssertFailed; + + // Helper functions to pass message on to failed function + struct AssertLastParam { }; + inline bool AssertFailedParamHelper(const char *inExpression, const char *inFile, uint inLine, AssertLastParam) { return AssertFailed(inExpression, nullptr, inFile, inLine); } + inline bool AssertFailedParamHelper(const char *inExpression, const char *inFile, uint inLine, const char *inMessage, AssertLastParam) { return AssertFailed(inExpression, inMessage, inFile, inLine); } + + /// Main assert macro, usage: JPH_ASSERT(condition, message) or JPH_ASSERT(condition) + #define JPH_ASSERT(inExpression, ...) do { if (!(inExpression) && AssertFailedParamHelper(#inExpression, __FILE__, JPH::uint(__LINE__), ##__VA_ARGS__, JPH::AssertLastParam())) JPH_BREAKPOINT; } while (false) + + #define JPH_IF_ENABLE_ASSERTS(...) __VA_ARGS__ +#else + #define JPH_ASSERT(...) ((void)0) + + #define JPH_IF_ENABLE_ASSERTS(...) +#endif // JPH_ENABLE_ASSERTS + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/JobSystem.h b/thirdparty/jolt_physics/Jolt/Core/JobSystem.h new file mode 100644 index 000000000000..1bd621a466fe --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/JobSystem.h @@ -0,0 +1,311 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// A class that allows units of work (Jobs) to be scheduled across multiple threads. +/// It allows dependencies between the jobs so that the jobs form a graph. +/// +/// The pattern for using this class is: +/// +/// // Create job system +/// JobSystem *job_system = new JobSystemThreadPool(...); +/// +/// // Create some jobs +/// JobHandle second_job = job_system->CreateJob("SecondJob", Color::sRed, []() { ... }, 1); // Create a job with 1 dependency +/// JobHandle first_job = job_system->CreateJob("FirstJob", Color::sGreen, [second_job]() { ....; second_job.RemoveDependency(); }, 0); // Job can start immediately, will start second job when it's done +/// JobHandle third_job = job_system->CreateJob("ThirdJob", Color::sBlue, []() { ... }, 0); // This job can run immediately as well and can run in parallel to job 1 and 2 +/// +/// // Add the jobs to the barrier so that we can execute them while we're waiting +/// Barrier *barrier = job_system->CreateBarrier(); +/// barrier->AddJob(first_job); +/// barrier->AddJob(second_job); +/// barrier->AddJob(third_job); +/// job_system->WaitForJobs(barrier); +/// +/// // Clean up +/// job_system->DestroyBarrier(barrier); +/// delete job_system; +/// +/// Jobs are guaranteed to be started in the order that their dependency counter becomes zero (in case they're scheduled on a background thread) +/// or in the order they're added to the barrier (when dependency count is zero and when executing on the thread that calls WaitForJobs). +/// +/// If you want to implement your own job system, inherit from JobSystem and implement: +/// +/// * JobSystem::GetMaxConcurrency - This should return the maximum number of jobs that can run in parallel. +/// * JobSystem::CreateJob - This should create a Job object and return it to the caller. +/// * JobSystem::FreeJob - This should free the memory associated with the job object. It is called by the Job destructor when it is Release()-ed for the last time. +/// * JobSystem::QueueJob/QueueJobs - These should store the job pointer in an internal queue to run immediately (dependencies are tracked internally, this function is called when the job can run). +/// The Job objects are reference counted and are guaranteed to stay alive during the QueueJob(s) call. If you store the job in your own data structure you need to call AddRef() to take a reference. +/// After the job has been executed you need to call Release() to release the reference. Make sure you no longer dereference the job pointer after calling Release(). +/// +/// JobSystem::Barrier is used to track the completion of a set of jobs. Jobs will be created by other jobs and added to the barrier while it is being waited on. This means that you cannot +/// create a dependency graph beforehand as the graph changes while jobs are running. Implement the following functions: +/// +/// * Barrier::AddJob/AddJobs - Add a job to the barrier, any call to WaitForJobs will now also wait for this job to complete. +/// If you store the job in a data structure in the Barrier you need to call AddRef() on the job to keep it alive and Release() after you're done with it. +/// * Barrier::OnJobFinished - This function is called when a job has finished executing, you can use this to track completion and remove the job from the list of jobs to wait on. +/// +/// The functions on JobSystem that need to be implemented to support barriers are: +/// +/// * JobSystem::CreateBarrier - Create a new barrier. +/// * JobSystem::DestroyBarrier - Destroy a barrier. +/// * JobSystem::WaitForJobs - This is the main function that is used to wait for all jobs that have been added to a Barrier. WaitForJobs can execute jobs that have +/// been added to the barrier while waiting. It is not wise to execute other jobs that touch physics structures as this can cause race conditions and deadlocks. Please keep in mind that the barrier is +/// only intended to wait on the completion of the Jolt jobs added to it, if you scheduled any jobs in your engine's job system to execute the Jolt jobs as part of QueueJob/QueueJobs, you might still need +/// to wait for these in this function after the barrier is finished waiting. +/// +/// An example implementation is JobSystemThreadPool. If you don't want to write the Barrier class you can also inherit from JobSystemWithBarrier. +class JPH_EXPORT JobSystem : public NonCopyable +{ +protected: + class Job; + +public: + JPH_OVERRIDE_NEW_DELETE + + /// A job handle contains a reference to a job. The job will be deleted as soon as there are no JobHandles. + /// referring to the job and when it is not in the job queue / being processed. + class JobHandle : private Ref + { + public: + /// Constructor + inline JobHandle() = default; + inline JobHandle(const JobHandle &inHandle) = default; + inline JobHandle(JobHandle &&inHandle) noexcept : Ref(std::move(inHandle)) { } + + /// Constructor, only to be used by JobSystem + inline explicit JobHandle(Job *inJob) : Ref(inJob) { } + + /// Assignment + inline JobHandle & operator = (const JobHandle &inHandle) = default; + inline JobHandle & operator = (JobHandle &&inHandle) noexcept = default; + + /// Check if this handle contains a job + inline bool IsValid() const { return GetPtr() != nullptr; } + + /// Check if this job has finished executing + inline bool IsDone() const { return GetPtr() != nullptr && GetPtr()->IsDone(); } + + /// Add to the dependency counter. + inline void AddDependency(int inCount = 1) const { GetPtr()->AddDependency(inCount); } + + /// Remove from the dependency counter. Job will start whenever the dependency counter reaches zero + /// and if it does it is no longer valid to call the AddDependency/RemoveDependency functions. + inline void RemoveDependency(int inCount = 1) const { GetPtr()->RemoveDependencyAndQueue(inCount); } + + /// Remove a dependency from a batch of jobs at once, this can be more efficient than removing them one by one as it requires less locking + static inline void sRemoveDependencies(const JobHandle *inHandles, uint inNumHandles, int inCount = 1); + + /// Helper function to remove dependencies on a static array of job handles + template + static inline void sRemoveDependencies(StaticArray &inHandles, int inCount = 1) + { + sRemoveDependencies(inHandles.data(), inHandles.size(), inCount); + } + + /// Inherit the GetPtr function, only to be used by the JobSystem + using Ref::GetPtr; + }; + + /// A job barrier keeps track of a number of jobs and allows waiting until they are all completed. + class Barrier : public NonCopyable + { + public: + JPH_OVERRIDE_NEW_DELETE + + /// Add a job to this barrier + /// Note that jobs can keep being added to the barrier while waiting for the barrier + virtual void AddJob(const JobHandle &inJob) = 0; + + /// Add multiple jobs to this barrier + /// Note that jobs can keep being added to the barrier while waiting for the barrier + virtual void AddJobs(const JobHandle *inHandles, uint inNumHandles) = 0; + + protected: + /// Job needs to be able to call OnJobFinished + friend class Job; + + /// Destructor, you should call JobSystem::DestroyBarrier instead of destructing this object directly + virtual ~Barrier() = default; + + /// Called by a Job to mark that it is finished + virtual void OnJobFinished(Job *inJob) = 0; + }; + + /// Main function of the job + using JobFunction = function; + + /// Destructor + virtual ~JobSystem() = default; + + /// Get maximum number of concurrently executing jobs + virtual int GetMaxConcurrency() const = 0; + + /// Create a new job, the job is started immediately if inNumDependencies == 0 otherwise it starts when + /// RemoveDependency causes the dependency counter to reach 0. + virtual JobHandle CreateJob(const char *inName, ColorArg inColor, const JobFunction &inJobFunction, uint32 inNumDependencies = 0) = 0; + + /// Create a new barrier, used to wait on jobs + virtual Barrier * CreateBarrier() = 0; + + /// Destroy a barrier when it is no longer used. The barrier should be empty at this point. + virtual void DestroyBarrier(Barrier *inBarrier) = 0; + + /// Wait for a set of jobs to be finished, note that only 1 thread can be waiting on a barrier at a time + virtual void WaitForJobs(Barrier *inBarrier) = 0; + +protected: + /// A class that contains information for a single unit of work + class Job + { + public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + Job([[maybe_unused]] const char *inJobName, [[maybe_unused]] ColorArg inColor, JobSystem *inJobSystem, const JobFunction &inJobFunction, uint32 inNumDependencies) : + #if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + mJobName(inJobName), + mColor(inColor), + #endif // defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + mJobSystem(inJobSystem), + mJobFunction(inJobFunction), + mNumDependencies(inNumDependencies) + { + } + + /// Get the jobs system to which this job belongs + inline JobSystem * GetJobSystem() { return mJobSystem; } + + /// Add or release a reference to this object + inline void AddRef() + { + // Adding a reference can use relaxed memory ordering + mReferenceCount.fetch_add(1, memory_order_relaxed); + } + inline void Release() + { + #ifndef JPH_TSAN_ENABLED + // Releasing a reference must use release semantics... + if (mReferenceCount.fetch_sub(1, memory_order_release) == 1) + { + // ... so that we can use acquire to ensure that we see any updates from other threads that released a ref before freeing the job + atomic_thread_fence(memory_order_acquire); + mJobSystem->FreeJob(this); + } + #else + // But under TSAN, we cannot use atomic_thread_fence, so we use an acq_rel operation unconditionally instead + if (mReferenceCount.fetch_sub(1, memory_order_acq_rel) == 1) + mJobSystem->FreeJob(this); + #endif + } + + /// Add to the dependency counter. + inline void AddDependency(int inCount); + + /// Remove from the dependency counter. Returns true whenever the dependency counter reaches zero + /// and if it does it is no longer valid to call the AddDependency/RemoveDependency functions. + inline bool RemoveDependency(int inCount); + + /// Remove from the dependency counter. Job will be queued whenever the dependency counter reaches zero + /// and if it does it is no longer valid to call the AddDependency/RemoveDependency functions. + inline void RemoveDependencyAndQueue(int inCount); + + /// Set the job barrier that this job belongs to and returns false if this was not possible because the job already finished + inline bool SetBarrier(Barrier *inBarrier) + { + intptr_t barrier = 0; + if (mBarrier.compare_exchange_strong(barrier, reinterpret_cast(inBarrier), memory_order_relaxed)) + return true; + JPH_ASSERT(barrier == cBarrierDoneState, "A job can only belong to 1 barrier"); + return false; + } + + /// Run the job function, returns the number of dependencies that this job still has or cExecutingState or cDoneState + inline uint32 Execute() + { + // Transition job to executing state + uint32 state = 0; // We can only start running with a dependency counter of 0 + if (!mNumDependencies.compare_exchange_strong(state, cExecutingState, memory_order_acquire)) + return state; // state is updated by compare_exchange_strong to the current value + + // Run the job function + { + JPH_PROFILE(mJobName, mColor.GetUInt32()); + mJobFunction(); + } + + // Fetch the barrier pointer and exchange it for the done state, so we're sure that no barrier gets set after we want to call the callback + intptr_t barrier = mBarrier.load(memory_order_relaxed); + for (;;) + { + if (mBarrier.compare_exchange_weak(barrier, cBarrierDoneState, memory_order_relaxed)) + break; + } + JPH_ASSERT(barrier != cBarrierDoneState); + + // Mark job as done + state = cExecutingState; + mNumDependencies.compare_exchange_strong(state, cDoneState, memory_order_relaxed); + JPH_ASSERT(state == cExecutingState); + + // Notify the barrier after we've changed the job to the done state so that any thread reading the state after receiving the callback will see that the job has finished + if (barrier != 0) + reinterpret_cast(barrier)->OnJobFinished(this); + + return cDoneState; + } + + /// Test if the job can be executed + inline bool CanBeExecuted() const { return mNumDependencies.load(memory_order_relaxed) == 0; } + + /// Test if the job finished executing + inline bool IsDone() const { return mNumDependencies.load(memory_order_relaxed) == cDoneState; } + + #if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + /// Get the name of the job + const char * GetName() const { return mJobName; } + #endif // defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + + static constexpr uint32 cExecutingState = 0xe0e0e0e0; ///< Value of mNumDependencies when job is executing + static constexpr uint32 cDoneState = 0xd0d0d0d0; ///< Value of mNumDependencies when job is done executing + + static constexpr intptr_t cBarrierDoneState = ~intptr_t(0); ///< Value to use when the barrier has been triggered + +private: + #if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + const char * mJobName; ///< Name of the job + Color mColor; ///< Color of the job in the profiler + #endif // defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + JobSystem * mJobSystem; ///< The job system we belong to + atomic mBarrier = 0; ///< Barrier that this job is associated with (is a Barrier pointer) + JobFunction mJobFunction; ///< Main job function + atomic mReferenceCount = 0; ///< Amount of JobHandles pointing to this job + atomic mNumDependencies; ///< Amount of jobs that need to complete before this job can run + }; + + /// Adds a job to the job queue + virtual void QueueJob(Job *inJob) = 0; + + /// Adds a number of jobs at once to the job queue + virtual void QueueJobs(Job **inJobs, uint inNumJobs) = 0; + + /// Frees a job + virtual void FreeJob(Job *inJob) = 0; +}; + +using JobHandle = JobSystem::JobHandle; + +JPH_NAMESPACE_END + +#include "JobSystem.inl" diff --git a/thirdparty/jolt_physics/Jolt/Core/JobSystem.inl b/thirdparty/jolt_physics/Jolt/Core/JobSystem.inl new file mode 100644 index 000000000000..bee4f13b5055 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/JobSystem.inl @@ -0,0 +1,56 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +JPH_NAMESPACE_BEGIN + +void JobSystem::Job::AddDependency(int inCount) +{ + JPH_IF_ENABLE_ASSERTS(uint32 old_value =) mNumDependencies.fetch_add(inCount, memory_order_relaxed); + JPH_ASSERT(old_value > 0 && old_value != cExecutingState && old_value != cDoneState, "Job is queued, running or done, it is not allowed to add a dependency to a running job"); +} + +bool JobSystem::Job::RemoveDependency(int inCount) +{ + uint32 old_value = mNumDependencies.fetch_sub(inCount, memory_order_release); + JPH_ASSERT(old_value != cExecutingState && old_value != cDoneState, "Job is running or done, it is not allowed to add a dependency to a running job"); + uint32 new_value = old_value - inCount; + JPH_ASSERT(old_value > new_value, "Test wrap around, this is a logic error"); + return new_value == 0; +} + +void JobSystem::Job::RemoveDependencyAndQueue(int inCount) +{ + if (RemoveDependency(inCount)) + mJobSystem->QueueJob(this); +} + +void JobSystem::JobHandle::sRemoveDependencies(const JobHandle *inHandles, uint inNumHandles, int inCount) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inNumHandles > 0); + + // Get the job system, all jobs should be part of the same job system + JobSystem *job_system = inHandles->GetPtr()->GetJobSystem(); + + // Allocate a buffer to store the jobs that need to be queued + Job **jobs_to_queue = (Job **)JPH_STACK_ALLOC(inNumHandles * sizeof(Job *)); + Job **next_job = jobs_to_queue; + + // Remove the dependencies on all jobs + for (const JobHandle *handle = inHandles, *handle_end = inHandles + inNumHandles; handle < handle_end; ++handle) + { + Job *job = handle->GetPtr(); + JPH_ASSERT(job->GetJobSystem() == job_system); // All jobs should belong to the same job system + if (job->RemoveDependency(inCount)) + *(next_job++) = job; + } + + // If any jobs need to be scheduled, schedule them as a batch + uint num_jobs_to_queue = uint(next_job - jobs_to_queue); + if (num_jobs_to_queue != 0) + job_system->QueueJobs(jobs_to_queue, num_jobs_to_queue); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/JobSystemSingleThreaded.cpp b/thirdparty/jolt_physics/Jolt/Core/JobSystemSingleThreaded.cpp new file mode 100644 index 000000000000..5f09d1a04cea --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/JobSystemSingleThreaded.cpp @@ -0,0 +1,65 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +void JobSystemSingleThreaded::Init(uint inMaxJobs) +{ + mJobs.Init(inMaxJobs, inMaxJobs); +} + +JobHandle JobSystemSingleThreaded::CreateJob(const char *inJobName, ColorArg inColor, const JobFunction &inJobFunction, uint32 inNumDependencies) +{ + // Construct an object + uint32 index = mJobs.ConstructObject(inJobName, inColor, this, inJobFunction, inNumDependencies); + JPH_ASSERT(index != AvailableJobs::cInvalidObjectIndex); + Job *job = &mJobs.Get(index); + + // Construct handle to keep a reference, the job is queued below and will immediately complete + JobHandle handle(job); + + // If there are no dependencies, queue the job now + if (inNumDependencies == 0) + QueueJob(job); + + // Return the handle + return handle; +} + +void JobSystemSingleThreaded::FreeJob(Job *inJob) +{ + mJobs.DestructObject(inJob); +} + +void JobSystemSingleThreaded::QueueJob(Job *inJob) +{ + inJob->Execute(); +} + +void JobSystemSingleThreaded::QueueJobs(Job **inJobs, uint inNumJobs) +{ + for (uint i = 0; i < inNumJobs; ++i) + QueueJob(inJobs[i]); +} + +JobSystem::Barrier *JobSystemSingleThreaded::CreateBarrier() +{ + return &mDummyBarrier; +} + +void JobSystemSingleThreaded::DestroyBarrier(Barrier *inBarrier) +{ + // There's nothing to do here, the barrier is just a dummy +} + +void JobSystemSingleThreaded::WaitForJobs(Barrier *inBarrier) +{ + // There's nothing to do here, the barrier is just a dummy, we just execute the jobs immediately +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/JobSystemSingleThreaded.h b/thirdparty/jolt_physics/Jolt/Core/JobSystemSingleThreaded.h new file mode 100644 index 000000000000..4db599b3135f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/JobSystemSingleThreaded.h @@ -0,0 +1,62 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Implementation of a JobSystem without threads, runs jobs as soon as they are added +class JPH_EXPORT JobSystemSingleThreaded final : public JobSystem +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + JobSystemSingleThreaded() = default; + explicit JobSystemSingleThreaded(uint inMaxJobs) { Init(inMaxJobs); } + + /// Initialize the job system + /// @param inMaxJobs Max number of jobs that can be allocated at any time + void Init(uint inMaxJobs); + + // See JobSystem + virtual int GetMaxConcurrency() const override { return 1; } + virtual JobHandle CreateJob(const char *inName, ColorArg inColor, const JobFunction &inJobFunction, uint32 inNumDependencies = 0) override; + virtual Barrier * CreateBarrier() override; + virtual void DestroyBarrier(Barrier *inBarrier) override; + virtual void WaitForJobs(Barrier *inBarrier) override; + +protected: + // Dummy implementation of Barrier, all jobs are executed immediately + class BarrierImpl : public Barrier + { + public: + JPH_OVERRIDE_NEW_DELETE + + // See Barrier + virtual void AddJob(const JobHandle &inJob) override { /* We don't need to track jobs */ } + virtual void AddJobs(const JobHandle *inHandles, uint inNumHandles) override { /* We don't need to track jobs */ } + + protected: + /// Called by a Job to mark that it is finished + virtual void OnJobFinished(Job *inJob) override { /* We don't need to track jobs */ } + }; + + // See JobSystem + virtual void QueueJob(Job *inJob) override; + virtual void QueueJobs(Job **inJobs, uint inNumJobs) override; + virtual void FreeJob(Job *inJob) override; + + /// Shared barrier since the barrier implementation does nothing + BarrierImpl mDummyBarrier; + + /// Array of jobs (fixed size) + using AvailableJobs = FixedSizeFreeList; + AvailableJobs mJobs; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/JobSystemThreadPool.cpp b/thirdparty/jolt_physics/Jolt/Core/JobSystemThreadPool.cpp new file mode 100644 index 000000000000..6d4dc10f502e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/JobSystemThreadPool.cpp @@ -0,0 +1,364 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include + +#ifdef JPH_PLATFORM_WINDOWS + JPH_SUPPRESS_WARNING_PUSH + JPH_MSVC_SUPPRESS_WARNING(5039) // winbase.h(13179): warning C5039: 'TpSetCallbackCleanupGroup': pointer or reference to potentially throwing function passed to 'extern "C"' function under -EHc. Undefined behavior may occur if this function throws an exception. + #ifndef WIN32_LEAN_AND_MEAN + #define WIN32_LEAN_AND_MEAN + #endif +#ifndef JPH_COMPILER_MINGW + #include +#else + #include +#endif + + JPH_SUPPRESS_WARNING_POP +#endif +#ifdef JPH_PLATFORM_LINUX + #include +#endif + +JPH_NAMESPACE_BEGIN + +void JobSystemThreadPool::Init(uint inMaxJobs, uint inMaxBarriers, int inNumThreads) +{ + JobSystemWithBarrier::Init(inMaxBarriers); + + // Init freelist of jobs + mJobs.Init(inMaxJobs, inMaxJobs); + + // Init queue + for (atomic &j : mQueue) + j = nullptr; + + // Start the worker threads + StartThreads(inNumThreads); +} + +JobSystemThreadPool::JobSystemThreadPool(uint inMaxJobs, uint inMaxBarriers, int inNumThreads) +{ + Init(inMaxJobs, inMaxBarriers, inNumThreads); +} + +void JobSystemThreadPool::StartThreads([[maybe_unused]] int inNumThreads) +{ +#if !defined(JPH_CPU_WASM) || defined(__EMSCRIPTEN_PTHREADS__) // If we're running without threads support we cannot create threads and we ignore the inNumThreads parameter + // Auto detect number of threads + if (inNumThreads < 0) + inNumThreads = thread::hardware_concurrency() - 1; + + // If no threads are requested we're done + if (inNumThreads == 0) + return; + + // Don't quit the threads + mQuit = false; + + // Allocate heads + mHeads = reinterpret_cast *>(Allocate(sizeof(atomic) * inNumThreads)); + for (int i = 0; i < inNumThreads; ++i) + mHeads[i] = 0; + + // Start running threads + JPH_ASSERT(mThreads.empty()); + mThreads.reserve(inNumThreads); + for (int i = 0; i < inNumThreads; ++i) + mThreads.emplace_back([this, i] { ThreadMain(i); }); +#endif +} + +JobSystemThreadPool::~JobSystemThreadPool() +{ + // Stop all worker threads + StopThreads(); +} + +void JobSystemThreadPool::StopThreads() +{ + if (mThreads.empty()) + return; + + // Signal threads that we want to stop and wake them up + mQuit = true; + mSemaphore.Release((uint)mThreads.size()); + + // Wait for all threads to finish + for (thread &t : mThreads) + if (t.joinable()) + t.join(); + + // Delete all threads + mThreads.clear(); + + // Ensure that there are no lingering jobs in the queue + for (uint head = 0; head != mTail; ++head) + { + // Fetch job + Job *job_ptr = mQueue[head & (cQueueLength - 1)].exchange(nullptr); + if (job_ptr != nullptr) + { + // And execute it + job_ptr->Execute(); + job_ptr->Release(); + } + } + + // Destroy heads and reset tail + Free(mHeads); + mHeads = nullptr; + mTail = 0; +} + +JobHandle JobSystemThreadPool::CreateJob(const char *inJobName, ColorArg inColor, const JobFunction &inJobFunction, uint32 inNumDependencies) +{ + JPH_PROFILE_FUNCTION(); + + // Loop until we can get a job from the free list + uint32 index; + for (;;) + { + index = mJobs.ConstructObject(inJobName, inColor, this, inJobFunction, inNumDependencies); + if (index != AvailableJobs::cInvalidObjectIndex) + break; + JPH_ASSERT(false, "No jobs available!"); + std::this_thread::sleep_for(std::chrono::microseconds(100)); + } + Job *job = &mJobs.Get(index); + + // Construct handle to keep a reference, the job is queued below and may immediately complete + JobHandle handle(job); + + // If there are no dependencies, queue the job now + if (inNumDependencies == 0) + QueueJob(job); + + // Return the handle + return handle; +} + +void JobSystemThreadPool::FreeJob(Job *inJob) +{ + mJobs.DestructObject(inJob); +} + +uint JobSystemThreadPool::GetHead() const +{ + // Find the minimal value across all threads + uint head = mTail; + for (size_t i = 0; i < mThreads.size(); ++i) + head = min(head, mHeads[i].load()); + return head; +} + +void JobSystemThreadPool::QueueJobInternal(Job *inJob) +{ + // Add reference to job because we're adding the job to the queue + inJob->AddRef(); + + // Need to read head first because otherwise the tail can already have passed the head + // We read the head outside of the loop since it involves iterating over all threads and we only need to update + // it if there's not enough space in the queue. + uint head = GetHead(); + + for (;;) + { + // Check if there's space in the queue + uint old_value = mTail; + if (old_value - head >= cQueueLength) + { + // We calculated the head outside of the loop, update head (and we also need to update tail to prevent it from passing head) + head = GetHead(); + old_value = mTail; + + // Second check if there's space in the queue + if (old_value - head >= cQueueLength) + { + // Wake up all threads in order to ensure that they can clear any nullptrs they may not have processed yet + mSemaphore.Release((uint)mThreads.size()); + + // Sleep a little (we have to wait for other threads to update their head pointer in order for us to be able to continue) + std::this_thread::sleep_for(std::chrono::microseconds(100)); + continue; + } + } + + // Write the job pointer if the slot is empty + Job *expected_job = nullptr; + bool success = mQueue[old_value & (cQueueLength - 1)].compare_exchange_strong(expected_job, inJob); + + // Regardless of who wrote the slot, we will update the tail (if the successful thread got scheduled out + // after writing the pointer we still want to be able to continue) + mTail.compare_exchange_strong(old_value, old_value + 1); + + // If we successfully added our job we're done + if (success) + break; + } +} + +void JobSystemThreadPool::QueueJob(Job *inJob) +{ + JPH_PROFILE_FUNCTION(); + + // If we have no worker threads, we can't queue the job either. We assume in this case that the job will be added to a barrier and that the barrier will execute the job when it's Wait() function is called. + if (mThreads.empty()) + return; + + // Queue the job + QueueJobInternal(inJob); + + // Wake up thread + mSemaphore.Release(); +} + +void JobSystemThreadPool::QueueJobs(Job **inJobs, uint inNumJobs) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inNumJobs > 0); + + // If we have no worker threads, we can't queue the job either. We assume in this case that the job will be added to a barrier and that the barrier will execute the job when it's Wait() function is called. + if (mThreads.empty()) + return; + + // Queue all jobs + for (Job **job = inJobs, **job_end = inJobs + inNumJobs; job < job_end; ++job) + QueueJobInternal(*job); + + // Wake up threads + mSemaphore.Release(min(inNumJobs, (uint)mThreads.size())); +} + +#if defined(JPH_PLATFORM_WINDOWS) + +#if !defined(JPH_COMPILER_MINGW) // MinGW doesn't support __try/__except) + // Sets the current thread name in MSVC debugger + static void RaiseThreadNameException(const char *inName) + { + #pragma pack(push, 8) + + struct THREADNAME_INFO + { + DWORD dwType; // Must be 0x1000. + LPCSTR szName; // Pointer to name (in user addr space). + DWORD dwThreadID; // Thread ID (-1=caller thread). + DWORD dwFlags; // Reserved for future use, must be zero. + }; + + #pragma pack(pop) + + THREADNAME_INFO info; + info.dwType = 0x1000; + info.szName = inName; + info.dwThreadID = (DWORD)-1; + info.dwFlags = 0; + + __try + { + RaiseException(0x406D1388, 0, sizeof(info) / sizeof(ULONG_PTR), (ULONG_PTR *)&info); + } + __except(EXCEPTION_EXECUTE_HANDLER) + { + } + } +#endif // !JPH_COMPILER_MINGW + + static void SetThreadName(const char* inName) + { + JPH_SUPPRESS_WARNING_PUSH + + // Suppress casting warning, it's fine here as GetProcAddress doesn't really return a FARPROC + JPH_CLANG_SUPPRESS_WARNING("-Wcast-function-type") // error : cast from 'FARPROC' (aka 'long long (*)()') to 'SetThreadDescriptionFunc' (aka 'long (*)(void *, const wchar_t *)') converts to incompatible function type + JPH_CLANG_SUPPRESS_WARNING("-Wcast-function-type-strict") // error : cast from 'FARPROC' (aka 'long long (*)()') to 'SetThreadDescriptionFunc' (aka 'long (*)(void *, const wchar_t *)') converts to incompatible function type + JPH_MSVC_SUPPRESS_WARNING(4191) // reinterpret_cast' : unsafe conversion from 'FARPROC' to 'SetThreadDescriptionFunc'. Calling this function through the result pointer may cause your program to fail + + using SetThreadDescriptionFunc = HRESULT(WINAPI*)(HANDLE hThread, PCWSTR lpThreadDescription); + static SetThreadDescriptionFunc SetThreadDescription = reinterpret_cast(GetProcAddress(GetModuleHandleW(L"Kernel32.dll"), "SetThreadDescription")); + + JPH_SUPPRESS_WARNING_POP + + if (SetThreadDescription) + { + wchar_t name_buffer[64] = { 0 }; + if (MultiByteToWideChar(CP_UTF8, 0, inName, -1, name_buffer, sizeof(name_buffer) / sizeof(wchar_t) - 1) == 0) + return; + + SetThreadDescription(GetCurrentThread(), name_buffer); + } +#if !defined(JPH_COMPILER_MINGW) + else if (IsDebuggerPresent()) + RaiseThreadNameException(inName); +#endif // !JPH_COMPILER_MINGW + } +#elif defined(JPH_PLATFORM_LINUX) + static void SetThreadName(const char *inName) + { + JPH_ASSERT(strlen(inName) < 16); // String will be truncated if it is longer + prctl(PR_SET_NAME, inName, 0, 0, 0); + } +#endif // JPH_PLATFORM_LINUX + +void JobSystemThreadPool::ThreadMain(int inThreadIndex) +{ + // Name the thread + char name[64]; + snprintf(name, sizeof(name), "Worker %d", int(inThreadIndex + 1)); + +#if defined(JPH_PLATFORM_WINDOWS) || defined(JPH_PLATFORM_LINUX) + SetThreadName(name); +#endif // JPH_PLATFORM_WINDOWS && !JPH_COMPILER_MINGW + + // Enable floating point exceptions + FPExceptionsEnable enable_exceptions; + JPH_UNUSED(enable_exceptions); + + JPH_PROFILE_THREAD_START(name); + + // Call the thread init function + mThreadInitFunction(inThreadIndex); + + atomic &head = mHeads[inThreadIndex]; + + while (!mQuit) + { + // Wait for jobs + mSemaphore.Acquire(); + + { + JPH_PROFILE("Executing Jobs"); + + // Loop over the queue + while (head != mTail) + { + // Exchange any job pointer we find with a nullptr + atomic &job = mQueue[head & (cQueueLength - 1)]; + if (job.load() != nullptr) + { + Job *job_ptr = job.exchange(nullptr); + if (job_ptr != nullptr) + { + // And execute it + job_ptr->Execute(); + job_ptr->Release(); + } + } + head++; + } + } + } + + // Call the thread exit function + mThreadExitFunction(inThreadIndex); + + JPH_PROFILE_THREAD_END(); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/JobSystemThreadPool.h b/thirdparty/jolt_physics/Jolt/Core/JobSystemThreadPool.h new file mode 100644 index 000000000000..2e44cb69e0d5 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/JobSystemThreadPool.h @@ -0,0 +1,101 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +// Things we're using from STL +using std::thread; + +/// Implementation of a JobSystem using a thread pool +/// +/// Note that this is considered an example implementation. It is expected that when you integrate +/// the physics engine into your own project that you'll provide your own implementation of the +/// JobSystem built on top of whatever job system your project uses. +class JPH_EXPORT JobSystemThreadPool final : public JobSystemWithBarrier +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Creates a thread pool. + /// @see JobSystemThreadPool::Init + JobSystemThreadPool(uint inMaxJobs, uint inMaxBarriers, int inNumThreads = -1); + JobSystemThreadPool() = default; + virtual ~JobSystemThreadPool() override; + + /// Functions to call when a thread is initialized or exits, must be set before calling Init() + using InitExitFunction = function; + void SetThreadInitFunction(const InitExitFunction &inInitFunction) { mThreadInitFunction = inInitFunction; } + void SetThreadExitFunction(const InitExitFunction &inExitFunction) { mThreadExitFunction = inExitFunction; } + + /// Initialize the thread pool + /// @param inMaxJobs Max number of jobs that can be allocated at any time + /// @param inMaxBarriers Max number of barriers that can be allocated at any time + /// @param inNumThreads Number of threads to start (the number of concurrent jobs is 1 more because the main thread will also run jobs while waiting for a barrier to complete). Use -1 to auto detect the amount of CPU's. + void Init(uint inMaxJobs, uint inMaxBarriers, int inNumThreads = -1); + + // See JobSystem + virtual int GetMaxConcurrency() const override { return int(mThreads.size()) + 1; } + virtual JobHandle CreateJob(const char *inName, ColorArg inColor, const JobFunction &inJobFunction, uint32 inNumDependencies = 0) override; + + /// Change the max concurrency after initialization + void SetNumThreads(int inNumThreads) { StopThreads(); StartThreads(inNumThreads); } + +protected: + // See JobSystem + virtual void QueueJob(Job *inJob) override; + virtual void QueueJobs(Job **inJobs, uint inNumJobs) override; + virtual void FreeJob(Job *inJob) override; + +private: + /// Start/stop the worker threads + void StartThreads(int inNumThreads); + void StopThreads(); + + /// Entry point for a thread + void ThreadMain(int inThreadIndex); + + /// Get the head of the thread that has processed the least amount of jobs + inline uint GetHead() const; + + /// Internal helper function to queue a job + inline void QueueJobInternal(Job *inJob); + + /// Functions to call when initializing or exiting a thread + InitExitFunction mThreadInitFunction = [](int) { }; + InitExitFunction mThreadExitFunction = [](int) { }; + + /// Array of jobs (fixed size) + using AvailableJobs = FixedSizeFreeList; + AvailableJobs mJobs; + + /// Threads running jobs + Array mThreads; + + // The job queue + static constexpr uint32 cQueueLength = 1024; + static_assert(IsPowerOf2(cQueueLength)); // We do bit operations and require queue length to be a power of 2 + atomic mQueue[cQueueLength]; + + // Head and tail of the queue, do this value modulo cQueueLength - 1 to get the element in the mQueue array + atomic * mHeads = nullptr; ///< Per executing thread the head of the current queue + alignas(JPH_CACHE_LINE_SIZE) atomic mTail = 0; ///< Tail (write end) of the queue + + // Semaphore used to signal worker threads that there is new work + Semaphore mSemaphore; + + /// Boolean to indicate that we want to stop the job system + atomic mQuit = false; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/JobSystemWithBarrier.cpp b/thirdparty/jolt_physics/Jolt/Core/JobSystemWithBarrier.cpp new file mode 100644 index 000000000000..3d4c7268b48c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/JobSystemWithBarrier.cpp @@ -0,0 +1,230 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +JobSystemWithBarrier::BarrierImpl::BarrierImpl() +{ + for (atomic &j : mJobs) + j = nullptr; +} + +JobSystemWithBarrier::BarrierImpl::~BarrierImpl() +{ + JPH_ASSERT(IsEmpty()); +} + +void JobSystemWithBarrier::BarrierImpl::AddJob(const JobHandle &inJob) +{ + JPH_PROFILE_FUNCTION(); + + bool release_semaphore = false; + + // Set the barrier on the job, this returns true if the barrier was successfully set (otherwise the job is already done and we don't need to add it to our list) + Job *job = inJob.GetPtr(); + if (job->SetBarrier(this)) + { + // If the job can be executed we want to release the semaphore an extra time to allow the waiting thread to start executing it + mNumToAcquire++; + if (job->CanBeExecuted()) + { + release_semaphore = true; + mNumToAcquire++; + } + + // Add the job to our job list + job->AddRef(); + uint write_index = mJobWriteIndex++; + while (write_index - mJobReadIndex >= cMaxJobs) + { + JPH_ASSERT(false, "Barrier full, stalling!"); + std::this_thread::sleep_for(std::chrono::microseconds(100)); + } + mJobs[write_index & (cMaxJobs - 1)] = job; + } + + // Notify waiting thread that a new executable job is available + if (release_semaphore) + mSemaphore.Release(); +} + +void JobSystemWithBarrier::BarrierImpl::AddJobs(const JobHandle *inHandles, uint inNumHandles) +{ + JPH_PROFILE_FUNCTION(); + + bool release_semaphore = false; + + for (const JobHandle *handle = inHandles, *handles_end = inHandles + inNumHandles; handle < handles_end; ++handle) + { + // Set the barrier on the job, this returns true if the barrier was successfully set (otherwise the job is already done and we don't need to add it to our list) + Job *job = handle->GetPtr(); + if (job->SetBarrier(this)) + { + // If the job can be executed we want to release the semaphore an extra time to allow the waiting thread to start executing it + mNumToAcquire++; + if (!release_semaphore && job->CanBeExecuted()) + { + release_semaphore = true; + mNumToAcquire++; + } + + // Add the job to our job list + job->AddRef(); + uint write_index = mJobWriteIndex++; + while (write_index - mJobReadIndex >= cMaxJobs) + { + JPH_ASSERT(false, "Barrier full, stalling!"); + std::this_thread::sleep_for(std::chrono::microseconds(100)); + } + mJobs[write_index & (cMaxJobs - 1)] = job; + } + } + + // Notify waiting thread that a new executable job is available + if (release_semaphore) + mSemaphore.Release(); +} + +void JobSystemWithBarrier::BarrierImpl::OnJobFinished(Job *inJob) +{ + JPH_PROFILE_FUNCTION(); + + mSemaphore.Release(); +} + +void JobSystemWithBarrier::BarrierImpl::Wait() +{ + while (mNumToAcquire > 0) + { + { + JPH_PROFILE("Execute Jobs"); + + // Go through all jobs + bool has_executed; + do + { + has_executed = false; + + // Loop through the jobs and erase jobs from the beginning of the list that are done + while (mJobReadIndex < mJobWriteIndex) + { + atomic &job = mJobs[mJobReadIndex & (cMaxJobs - 1)]; + Job *job_ptr = job.load(); + if (job_ptr == nullptr || !job_ptr->IsDone()) + break; + + // Job is finished, release it + job_ptr->Release(); + job = nullptr; + ++mJobReadIndex; + } + + // Loop through the jobs and execute the first executable job + for (uint index = mJobReadIndex; index < mJobWriteIndex; ++index) + { + const atomic &job = mJobs[index & (cMaxJobs - 1)]; + Job *job_ptr = job.load(); + if (job_ptr != nullptr && job_ptr->CanBeExecuted()) + { + // This will only execute the job if it has not already executed + job_ptr->Execute(); + has_executed = true; + break; + } + } + + } while (has_executed); + } + + // Wait for another thread to wake us when either there is more work to do or when all jobs have completed. + // When there have been multiple releases, we acquire them all at the same time to avoid needlessly spinning on executing jobs. + // Note that using GetValue is inherently unsafe since we can read a stale value, but this is not an issue here as this is the only + // place where we acquire the semaphore. Other threads only release it, so we can only read a value that is lower or equal to the actual value. + int num_to_acquire = max(1, mSemaphore.GetValue()); + mSemaphore.Acquire(num_to_acquire); + mNumToAcquire -= num_to_acquire; + } + + // All jobs should be done now, release them + while (mJobReadIndex < mJobWriteIndex) + { + atomic &job = mJobs[mJobReadIndex & (cMaxJobs - 1)]; + Job *job_ptr = job.load(); + JPH_ASSERT(job_ptr != nullptr && job_ptr->IsDone()); + job_ptr->Release(); + job = nullptr; + ++mJobReadIndex; + } +} + +void JobSystemWithBarrier::Init(uint inMaxBarriers) +{ + JPH_ASSERT(mBarriers == nullptr); // Already initialized? + + // Init freelist of barriers + mMaxBarriers = inMaxBarriers; + mBarriers = new BarrierImpl [inMaxBarriers]; +} + +JobSystemWithBarrier::JobSystemWithBarrier(uint inMaxBarriers) +{ + Init(inMaxBarriers); +} + +JobSystemWithBarrier::~JobSystemWithBarrier() +{ + // Ensure that none of the barriers are used +#ifdef JPH_ENABLE_ASSERTS + for (const BarrierImpl *b = mBarriers, *b_end = mBarriers + mMaxBarriers; b < b_end; ++b) + JPH_ASSERT(!b->mInUse); +#endif // JPH_ENABLE_ASSERTS + delete [] mBarriers; +} + +JobSystem::Barrier *JobSystemWithBarrier::CreateBarrier() +{ + JPH_PROFILE_FUNCTION(); + + // Find the first unused barrier + for (uint32 index = 0; index < mMaxBarriers; ++index) + { + bool expected = false; + if (mBarriers[index].mInUse.compare_exchange_strong(expected, true)) + return &mBarriers[index]; + } + + return nullptr; +} + +void JobSystemWithBarrier::DestroyBarrier(Barrier *inBarrier) +{ + JPH_PROFILE_FUNCTION(); + + // Check that no jobs are in the barrier + JPH_ASSERT(static_cast(inBarrier)->IsEmpty()); + + // Flag the barrier as unused + bool expected = true; + static_cast(inBarrier)->mInUse.compare_exchange_strong(expected, false); + JPH_ASSERT(expected); +} + +void JobSystemWithBarrier::WaitForJobs(Barrier *inBarrier) +{ + JPH_PROFILE_FUNCTION(); + + // Let our barrier implementation wait for the jobs + static_cast(inBarrier)->Wait(); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/JobSystemWithBarrier.h b/thirdparty/jolt_physics/Jolt/Core/JobSystemWithBarrier.h new file mode 100644 index 000000000000..0fc663330ea8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/JobSystemWithBarrier.h @@ -0,0 +1,85 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Implementation of the Barrier class for a JobSystem +/// +/// This class can be used to make it easier to create a new JobSystem implementation that integrates with your own job system. +/// It will implement all functionality relating to barriers, so the only functions that are left to be implemented are: +/// +/// * JobSystem::GetMaxConcurrency +/// * JobSystem::CreateJob +/// * JobSystem::FreeJob +/// * JobSystem::QueueJob/QueueJobs +/// +/// See instructions in JobSystem for more information on how to implement these. +class JPH_EXPORT JobSystemWithBarrier : public JobSystem +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructs barriers + /// @see JobSystemWithBarrier::Init + explicit JobSystemWithBarrier(uint inMaxBarriers); + JobSystemWithBarrier() = default; + virtual ~JobSystemWithBarrier() override; + + /// Initialize the barriers + /// @param inMaxBarriers Max number of barriers that can be allocated at any time + void Init(uint inMaxBarriers); + + // See JobSystem + virtual Barrier * CreateBarrier() override; + virtual void DestroyBarrier(Barrier *inBarrier) override; + virtual void WaitForJobs(Barrier *inBarrier) override; + +private: + class BarrierImpl : public Barrier + { + public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + BarrierImpl(); + virtual ~BarrierImpl() override; + + // See Barrier + virtual void AddJob(const JobHandle &inJob) override; + virtual void AddJobs(const JobHandle *inHandles, uint inNumHandles) override; + + /// Check if there are any jobs in the job barrier + inline bool IsEmpty() const { return mJobReadIndex == mJobWriteIndex; } + + /// Wait for all jobs in this job barrier, while waiting, execute jobs that are part of this barrier on the current thread + void Wait(); + + /// Flag to indicate if a barrier has been handed out + atomic mInUse { false }; + + protected: + /// Called by a Job to mark that it is finished + virtual void OnJobFinished(Job *inJob) override; + + /// Jobs queue for the barrier + static constexpr uint cMaxJobs = 2048; + static_assert(IsPowerOf2(cMaxJobs)); // We do bit operations and require max jobs to be a power of 2 + atomic mJobs[cMaxJobs]; ///< List of jobs that are part of this barrier, nullptrs for empty slots + alignas(JPH_CACHE_LINE_SIZE) atomic mJobReadIndex { 0 }; ///< First job that could be valid (modulo cMaxJobs), can be nullptr if other thread is still working on adding the job + alignas(JPH_CACHE_LINE_SIZE) atomic mJobWriteIndex { 0 }; ///< First job that can be written (modulo cMaxJobs) + atomic mNumToAcquire { 0 }; ///< Number of times the semaphore has been released, the barrier should acquire the semaphore this many times (written at the same time as mJobWriteIndex so ok to put in same cache line) + Semaphore mSemaphore; ///< Semaphore used by finishing jobs to signal the barrier that they're done + }; + + /// Array of barriers (we keep them constructed all the time since constructing a semaphore/mutex is not cheap) + uint mMaxBarriers = 0; ///< Max amount of barriers + BarrierImpl * mBarriers = nullptr; ///< List of the actual barriers +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/LinearCurve.cpp b/thirdparty/jolt_physics/Jolt/Core/LinearCurve.cpp new file mode 100644 index 000000000000..8c8f7682ec2a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/LinearCurve.cpp @@ -0,0 +1,51 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(LinearCurve::Point) +{ + JPH_ADD_ATTRIBUTE(Point, mX) + JPH_ADD_ATTRIBUTE(Point, mY) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(LinearCurve) +{ + JPH_ADD_ATTRIBUTE(LinearCurve, mPoints) +} + +float LinearCurve::GetValue(float inX) const +{ + if (mPoints.empty()) + return 0.0f; + + Points::const_iterator i2 = std::lower_bound(mPoints.begin(), mPoints.end(), inX, [](const Point &inPoint, float inValue) { return inPoint.mX < inValue; }); + + if (i2 == mPoints.begin()) + return mPoints.front().mY; + else if (i2 == mPoints.end()) + return mPoints.back().mY; + + Points::const_iterator i1 = i2 - 1; + return i1->mY + (inX - i1->mX) * (i2->mY - i1->mY) / (i2->mX - i1->mX); +} + +void LinearCurve::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mPoints); +} + +void LinearCurve::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mPoints); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/LinearCurve.h b/thirdparty/jolt_physics/Jolt/Core/LinearCurve.h new file mode 100644 index 000000000000..870014406882 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/LinearCurve.h @@ -0,0 +1,67 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class StreamOut; +class StreamIn; + +// A set of points (x, y) that form a linear curve +class JPH_EXPORT LinearCurve +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, LinearCurve) + +public: + /// A point on the curve + class Point + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Point) + + public: + float mX = 0.0f; + float mY = 0.0f; + }; + + /// Remove all points + void Clear() { mPoints.clear(); } + + /// Reserve memory for inNumPoints points + void Reserve(uint inNumPoints) { mPoints.reserve(inNumPoints); } + + /// Add a point to the curve. Points must be inserted in ascending X or Sort() needs to be called when all points have been added. + /// @param inX X value + /// @param inY Y value + void AddPoint(float inX, float inY) { mPoints.push_back({ inX, inY }); } + + /// Sort the points on X ascending + void Sort() { QuickSort(mPoints.begin(), mPoints.end(), [](const Point &inLHS, const Point &inRHS) { return inLHS.mX < inRHS.mX; }); } + + /// Get the lowest X value + float GetMinX() const { return mPoints.empty()? 0.0f : mPoints.front().mX; } + + /// Get the highest X value + float GetMaxX() const { return mPoints.empty()? 0.0f : mPoints.back().mX; } + + /// Sample value on the curve + /// @param inX X value to sample at + /// @return Interpolated Y value + float GetValue(float inX) const; + + /// Saves the state of this object in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restore the state of this object from inStream. + void RestoreBinaryState(StreamIn &inStream); + + /// The points on the curve, should be sorted ascending by x + using Points = Array; + Points mPoints; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/LockFreeHashMap.h b/thirdparty/jolt_physics/Jolt/Core/LockFreeHashMap.h new file mode 100644 index 000000000000..ac1557b39a4b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/LockFreeHashMap.h @@ -0,0 +1,182 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Allocator for a lock free hash map +class LFHMAllocator : public NonCopyable +{ +public: + /// Destructor + inline ~LFHMAllocator(); + + /// Initialize the allocator + /// @param inObjectStoreSizeBytes Number of bytes to reserve for all key value pairs + inline void Init(uint inObjectStoreSizeBytes); + + /// Clear all allocations + inline void Clear(); + + /// Allocate a new block of data + /// @param inBlockSize Size of block to allocate (will potentially return a smaller block if memory is full). + /// @param ioBegin Should be the start of the first free byte in current memory block on input, will contain the start of the first free byte in allocated block on return. + /// @param ioEnd Should be the byte beyond the current memory block on input, will contain the byte beyond the allocated block on return. + inline void Allocate(uint32 inBlockSize, uint32 &ioBegin, uint32 &ioEnd); + + /// Convert a pointer to an offset + template + inline uint32 ToOffset(const T *inData) const; + + /// Convert an offset to a pointer + template + inline T * FromOffset(uint32 inOffset) const; + +private: + uint8 * mObjectStore = nullptr; ///< This contains a contiguous list of objects (possibly of varying size) + uint32 mObjectStoreSizeBytes = 0; ///< The size of mObjectStore in bytes + atomic mWriteOffset { 0 }; ///< Next offset to write to in mObjectStore +}; + +/// Allocator context object for a lock free hash map that allocates a larger memory block at once and hands it out in smaller portions. +/// This avoids contention on the atomic LFHMAllocator::mWriteOffset. +class LFHMAllocatorContext : public NonCopyable +{ +public: + /// Construct a new allocator context + inline LFHMAllocatorContext(LFHMAllocator &inAllocator, uint32 inBlockSize); + + /// @brief Allocate data block + /// @param inSize Size of block to allocate. + /// @param inAlignment Alignment of block to allocate. + /// @param outWriteOffset Offset in buffer where block is located + /// @return True if allocation succeeded + inline bool Allocate(uint32 inSize, uint32 inAlignment, uint32 &outWriteOffset); + +private: + LFHMAllocator & mAllocator; + uint32 mBlockSize; + uint32 mBegin = 0; + uint32 mEnd = 0; +}; + +/// Very simple lock free hash map that only allows insertion, retrieval and provides a fixed amount of buckets and fixed storage. +/// Note: This class currently assumes key and value are simple types that need no calls to the destructor. +template +class LockFreeHashMap : public NonCopyable +{ +public: + using MapType = LockFreeHashMap; + + /// Destructor + explicit LockFreeHashMap(LFHMAllocator &inAllocator) : mAllocator(inAllocator) { } + ~LockFreeHashMap(); + + /// Initialization + /// @param inMaxBuckets Max amount of buckets to use in the hashmap. Must be power of 2. + void Init(uint32 inMaxBuckets); + + /// Remove all elements. + /// Note that this cannot happen simultaneously with adding new elements. + void Clear(); + + /// Get the current amount of buckets that the map is using + uint32 GetNumBuckets() const { return mNumBuckets; } + + /// Get the maximum amount of buckets that this map supports + uint32 GetMaxBuckets() const { return mMaxBuckets; } + + /// Update the number of buckets. This must be done after clearing the map and cannot be done concurrently with any other operations on the map. + /// Note that the number of buckets can never become bigger than the specified max buckets during initialization and that it must be a power of 2. + void SetNumBuckets(uint32 inNumBuckets); + + /// A key / value pair that is inserted in the map + class KeyValue + { + public: + const Key & GetKey() const { return mKey; } + Value & GetValue() { return mValue; } + const Value & GetValue() const { return mValue; } + + private: + template friend class LockFreeHashMap; + + Key mKey; ///< Key for this entry + uint32 mNextOffset; ///< Offset in mObjectStore of next KeyValue entry with same hash + Value mValue; ///< Value for this entry + optionally extra bytes + }; + + /// Insert a new element, returns null if map full. + /// Multiple threads can be inserting in the map at the same time. + template + inline KeyValue * Create(LFHMAllocatorContext &ioContext, const Key &inKey, uint64 inKeyHash, int inExtraBytes, Params &&... inConstructorParams); + + /// Find an element, returns null if not found + inline const KeyValue * Find(const Key &inKey, uint64 inKeyHash) const; + + /// Value of an invalid handle + const static uint32 cInvalidHandle = uint32(-1); + + /// Get convert key value pair to uint32 handle + inline uint32 ToHandle(const KeyValue *inKeyValue) const; + + /// Convert uint32 handle back to key and value + inline const KeyValue * FromHandle(uint32 inHandle) const; + +#ifdef JPH_ENABLE_ASSERTS + /// Get the number of key value pairs that this map currently contains. + /// Available only when asserts are enabled because adding elements creates contention on this atomic and negatively affects performance. + inline uint32 GetNumKeyValues() const { return mNumKeyValues; } +#endif // JPH_ENABLE_ASSERTS + + /// Get all key/value pairs + inline void GetAllKeyValues(Array &outAll) const; + + /// Non-const iterator + struct Iterator + { + /// Comparison + bool operator == (const Iterator &inRHS) const { return mMap == inRHS.mMap && mBucket == inRHS.mBucket && mOffset == inRHS.mOffset; } + bool operator != (const Iterator &inRHS) const { return !(*this == inRHS); } + + /// Convert to key value pair + KeyValue & operator * (); + + /// Next item + Iterator & operator ++ (); + + MapType * mMap; + uint32 mBucket; + uint32 mOffset; + }; + + /// Iterate over the map, note that it is not safe to do this in parallel to Clear(). + /// It is safe to do this while adding elements to the map, but newly added elements may or may not be returned by the iterator. + Iterator begin(); + Iterator end(); + +#ifdef JPH_DEBUG + /// Output stats about this map to the log + void TraceStats() const; +#endif + +private: + LFHMAllocator & mAllocator; ///< Allocator used to allocate key value pairs + +#ifdef JPH_ENABLE_ASSERTS + atomic mNumKeyValues = 0; ///< Number of key value pairs in the store +#endif // JPH_ENABLE_ASSERTS + + atomic * mBuckets = nullptr; ///< This contains the offset in mObjectStore of the first object with a particular hash + uint32 mNumBuckets = 0; ///< Current number of buckets + uint32 mMaxBuckets = 0; ///< Maximum number of buckets +}; + +JPH_NAMESPACE_END + +#include "LockFreeHashMap.inl" diff --git a/thirdparty/jolt_physics/Jolt/Core/LockFreeHashMap.inl b/thirdparty/jolt_physics/Jolt/Core/LockFreeHashMap.inl new file mode 100644 index 000000000000..aa4b875e9c7b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/LockFreeHashMap.inl @@ -0,0 +1,351 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/////////////////////////////////////////////////////////////////////////////////// +// LFHMAllocator +/////////////////////////////////////////////////////////////////////////////////// + +inline LFHMAllocator::~LFHMAllocator() +{ + AlignedFree(mObjectStore); +} + +inline void LFHMAllocator::Init(uint inObjectStoreSizeBytes) +{ + JPH_ASSERT(mObjectStore == nullptr); + + mObjectStoreSizeBytes = inObjectStoreSizeBytes; + mObjectStore = reinterpret_cast(JPH::AlignedAllocate(inObjectStoreSizeBytes, 16)); +} + +inline void LFHMAllocator::Clear() +{ + mWriteOffset = 0; +} + +inline void LFHMAllocator::Allocate(uint32 inBlockSize, uint32 &ioBegin, uint32 &ioEnd) +{ + // If we're already beyond the end of our buffer then don't do an atomic add. + // It's possible that many keys are inserted after the allocator is full, making it possible + // for mWriteOffset (uint32) to wrap around to zero. When this happens, there will be a memory corruption. + // This way, we will be able to progress the write offset beyond the size of the buffer + // worst case by max * inBlockSize. + if (mWriteOffset.load(memory_order_relaxed) >= mObjectStoreSizeBytes) + return; + + // Atomically fetch a block from the pool + uint32 begin = mWriteOffset.fetch_add(inBlockSize, memory_order_relaxed); + uint32 end = min(begin + inBlockSize, mObjectStoreSizeBytes); + + if (ioEnd == begin) + { + // Block is allocated straight after our previous block + begin = ioBegin; + } + else + { + // Block is a new block + begin = min(begin, mObjectStoreSizeBytes); + } + + // Store the begin and end of the resulting block + ioBegin = begin; + ioEnd = end; +} + +template +inline uint32 LFHMAllocator::ToOffset(const T *inData) const +{ + const uint8 *data = reinterpret_cast(inData); + JPH_ASSERT(data >= mObjectStore && data < mObjectStore + mObjectStoreSizeBytes); + return uint32(data - mObjectStore); +} + +template +inline T *LFHMAllocator::FromOffset(uint32 inOffset) const +{ + JPH_ASSERT(inOffset < mObjectStoreSizeBytes); + return reinterpret_cast(mObjectStore + inOffset); +} + +/////////////////////////////////////////////////////////////////////////////////// +// LFHMAllocatorContext +/////////////////////////////////////////////////////////////////////////////////// + +inline LFHMAllocatorContext::LFHMAllocatorContext(LFHMAllocator &inAllocator, uint32 inBlockSize) : + mAllocator(inAllocator), + mBlockSize(inBlockSize) +{ +} + +inline bool LFHMAllocatorContext::Allocate(uint32 inSize, uint32 inAlignment, uint32 &outWriteOffset) +{ + // Calculate needed bytes for alignment + JPH_ASSERT(IsPowerOf2(inAlignment)); + uint32 alignment_mask = inAlignment - 1; + uint32 alignment = (inAlignment - (mBegin & alignment_mask)) & alignment_mask; + + // Check if we have space + if (mEnd - mBegin < inSize + alignment) + { + // Allocate a new block + mAllocator.Allocate(mBlockSize, mBegin, mEnd); + + // Update alignment + alignment = (inAlignment - (mBegin & alignment_mask)) & alignment_mask; + + // Check if we have space again + if (mEnd - mBegin < inSize + alignment) + return false; + } + + // Make the allocation + mBegin += alignment; + outWriteOffset = mBegin; + mBegin += inSize; + return true; +} + +/////////////////////////////////////////////////////////////////////////////////// +// LockFreeHashMap +/////////////////////////////////////////////////////////////////////////////////// + +template +void LockFreeHashMap::Init(uint32 inMaxBuckets) +{ + JPH_ASSERT(inMaxBuckets >= 4 && IsPowerOf2(inMaxBuckets)); + JPH_ASSERT(mBuckets == nullptr); + + mNumBuckets = inMaxBuckets; + mMaxBuckets = inMaxBuckets; + + mBuckets = reinterpret_cast *>(AlignedAllocate(inMaxBuckets * sizeof(atomic), 16)); + + Clear(); +} + +template +LockFreeHashMap::~LockFreeHashMap() +{ + AlignedFree(mBuckets); +} + +template +void LockFreeHashMap::Clear() +{ +#ifdef JPH_ENABLE_ASSERTS + // Reset number of key value pairs + mNumKeyValues = 0; +#endif // JPH_ENABLE_ASSERTS + + // Reset buckets 4 at a time + static_assert(sizeof(atomic) == sizeof(uint32)); + UVec4 invalid_handle = UVec4::sReplicate(cInvalidHandle); + uint32 *start = reinterpret_cast(mBuckets); + const uint32 *end = start + mNumBuckets; + JPH_ASSERT(IsAligned(start, 16)); + while (start < end) + { + invalid_handle.StoreInt4Aligned(start); + start += 4; + } +} + +template +void LockFreeHashMap::SetNumBuckets(uint32 inNumBuckets) +{ + JPH_ASSERT(mNumKeyValues == 0); + JPH_ASSERT(inNumBuckets <= mMaxBuckets); + JPH_ASSERT(inNumBuckets >= 4 && IsPowerOf2(inNumBuckets)); + + mNumBuckets = inNumBuckets; +} + +template +template +inline typename LockFreeHashMap::KeyValue *LockFreeHashMap::Create(LFHMAllocatorContext &ioContext, const Key &inKey, uint64 inKeyHash, int inExtraBytes, Params &&... inConstructorParams) +{ + // This is not a multi map, test the key hasn't been inserted yet + JPH_ASSERT(Find(inKey, inKeyHash) == nullptr); + + // Calculate total size + uint size = sizeof(KeyValue) + inExtraBytes; + + // Get the write offset for this key value pair + uint32 write_offset; + if (!ioContext.Allocate(size, alignof(KeyValue), write_offset)) + return nullptr; + +#ifdef JPH_ENABLE_ASSERTS + // Increment amount of entries in map + mNumKeyValues.fetch_add(1, memory_order_relaxed); +#endif // JPH_ENABLE_ASSERTS + + // Construct the key/value pair + KeyValue *kv = mAllocator.template FromOffset(write_offset); + JPH_ASSERT(intptr_t(kv) % alignof(KeyValue) == 0); +#ifdef JPH_DEBUG + memset(kv, 0xcd, size); +#endif + kv->mKey = inKey; + new (&kv->mValue) Value(std::forward(inConstructorParams)...); + + // Get the offset to the first object from the bucket with corresponding hash + atomic &offset = mBuckets[inKeyHash & (mNumBuckets - 1)]; + + // Add this entry as the first element in the linked list + uint32 old_offset = offset.load(memory_order_relaxed); + for (;;) + { + kv->mNextOffset = old_offset; + if (offset.compare_exchange_weak(old_offset, write_offset, memory_order_release)) + break; + } + + return kv; +} + +template +inline const typename LockFreeHashMap::KeyValue *LockFreeHashMap::Find(const Key &inKey, uint64 inKeyHash) const +{ + // Get the offset to the keyvalue object from the bucket with corresponding hash + uint32 offset = mBuckets[inKeyHash & (mNumBuckets - 1)].load(memory_order_acquire); + while (offset != cInvalidHandle) + { + // Loop through linked list of values until the right one is found + const KeyValue *kv = mAllocator.template FromOffset(offset); + if (kv->mKey == inKey) + return kv; + offset = kv->mNextOffset; + } + + // Not found + return nullptr; +} + +template +inline uint32 LockFreeHashMap::ToHandle(const KeyValue *inKeyValue) const +{ + return mAllocator.ToOffset(inKeyValue); +} + +template +inline const typename LockFreeHashMap::KeyValue *LockFreeHashMap::FromHandle(uint32 inHandle) const +{ + return mAllocator.template FromOffset(inHandle); +} + +template +inline void LockFreeHashMap::GetAllKeyValues(Array &outAll) const +{ + for (const atomic *bucket = mBuckets; bucket < mBuckets + mNumBuckets; ++bucket) + { + uint32 offset = *bucket; + while (offset != cInvalidHandle) + { + const KeyValue *kv = mAllocator.template FromOffset(offset); + outAll.push_back(kv); + offset = kv->mNextOffset; + } + } +} + +template +typename LockFreeHashMap::Iterator LockFreeHashMap::begin() +{ + // Start with the first bucket + Iterator it { this, 0, mBuckets[0] }; + + // If it doesn't contain a valid entry, use the ++ operator to find the first valid entry + if (it.mOffset == cInvalidHandle) + ++it; + + return it; +} + +template +typename LockFreeHashMap::Iterator LockFreeHashMap::end() +{ + return { this, mNumBuckets, cInvalidHandle }; +} + +template +typename LockFreeHashMap::KeyValue &LockFreeHashMap::Iterator::operator* () +{ + JPH_ASSERT(mOffset != cInvalidHandle); + + return *mMap->mAllocator.template FromOffset(mOffset); +} + +template +typename LockFreeHashMap::Iterator &LockFreeHashMap::Iterator::operator++ () +{ + JPH_ASSERT(mBucket < mMap->mNumBuckets); + + // Find the next key value in this bucket + if (mOffset != cInvalidHandle) + { + const KeyValue *kv = mMap->mAllocator.template FromOffset(mOffset); + mOffset = kv->mNextOffset; + if (mOffset != cInvalidHandle) + return *this; + } + + // Loop over next buckets + for (;;) + { + // Next bucket + ++mBucket; + if (mBucket >= mMap->mNumBuckets) + return *this; + + // Fetch the first entry in the bucket + mOffset = mMap->mBuckets[mBucket]; + if (mOffset != cInvalidHandle) + return *this; + } +} + +#ifdef JPH_DEBUG + +template +void LockFreeHashMap::TraceStats() const +{ + const int cMaxPerBucket = 256; + + int max_objects_per_bucket = 0; + int num_objects = 0; + int histogram[cMaxPerBucket]; + for (int i = 0; i < cMaxPerBucket; ++i) + histogram[i] = 0; + + for (atomic *bucket = mBuckets, *bucket_end = mBuckets + mNumBuckets; bucket < bucket_end; ++bucket) + { + int objects_in_bucket = 0; + uint32 offset = *bucket; + while (offset != cInvalidHandle) + { + const KeyValue *kv = mAllocator.template FromOffset(offset); + offset = kv->mNextOffset; + ++objects_in_bucket; + ++num_objects; + } + max_objects_per_bucket = max(objects_in_bucket, max_objects_per_bucket); + histogram[min(objects_in_bucket, cMaxPerBucket - 1)]++; + } + + Trace("max_objects_per_bucket = %d, num_buckets = %u, num_objects = %d", max_objects_per_bucket, mNumBuckets, num_objects); + + for (int i = 0; i < cMaxPerBucket; ++i) + if (histogram[i] != 0) + Trace("%d: %d", i, histogram[i]); +} + +#endif + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/Memory.cpp b/thirdparty/jolt_physics/Jolt/Core/Memory.cpp new file mode 100644 index 000000000000..6e642c4ea828 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/Memory.cpp @@ -0,0 +1,85 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END +#include + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_DISABLE_CUSTOM_ALLOCATOR + #define JPH_ALLOC_FN(x) x + #define JPH_ALLOC_SCOPE +#else + #define JPH_ALLOC_FN(x) x##Impl + #define JPH_ALLOC_SCOPE static +#endif + +JPH_ALLOC_SCOPE void *JPH_ALLOC_FN(Allocate)(size_t inSize) +{ + JPH_ASSERT(inSize > 0); + return malloc(inSize); +} + +JPH_ALLOC_SCOPE void *JPH_ALLOC_FN(Reallocate)(void *inBlock, [[maybe_unused]] size_t inOldSize, size_t inNewSize) +{ + JPH_ASSERT(inNewSize > 0); + return realloc(inBlock, inNewSize); +} + +JPH_ALLOC_SCOPE void JPH_ALLOC_FN(Free)(void *inBlock) +{ + free(inBlock); +} + +JPH_ALLOC_SCOPE void *JPH_ALLOC_FN(AlignedAllocate)(size_t inSize, size_t inAlignment) +{ + JPH_ASSERT(inSize > 0 && inAlignment > 0); + +#if defined(JPH_PLATFORM_WINDOWS) + // Microsoft doesn't implement posix_memalign + return _aligned_malloc(inSize, inAlignment); +#else + void *block = nullptr; + JPH_SUPPRESS_WARNING_PUSH + JPH_GCC_SUPPRESS_WARNING("-Wunused-result") + JPH_CLANG_SUPPRESS_WARNING("-Wunused-result") + posix_memalign(&block, inAlignment, inSize); + JPH_SUPPRESS_WARNING_POP + return block; +#endif +} + +JPH_ALLOC_SCOPE void JPH_ALLOC_FN(AlignedFree)(void *inBlock) +{ +#if defined(JPH_PLATFORM_WINDOWS) + _aligned_free(inBlock); +#else + free(inBlock); +#endif +} + +#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR + +AllocateFunction Allocate = nullptr; +ReallocateFunction Reallocate = nullptr; +FreeFunction Free = nullptr; +AlignedAllocateFunction AlignedAllocate = nullptr; +AlignedFreeFunction AlignedFree = nullptr; + +void RegisterDefaultAllocator() +{ + Allocate = AllocateImpl; + Reallocate = ReallocateImpl; + Free = FreeImpl; + AlignedAllocate = AlignedAllocateImpl; + AlignedFree = AlignedFreeImpl; +} + +#endif // JPH_DISABLE_CUSTOM_ALLOCATOR + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/Memory.h b/thirdparty/jolt_physics/Jolt/Core/Memory.h new file mode 100644 index 000000000000..d5b7c6a308be --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/Memory.h @@ -0,0 +1,58 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR + +// Normal memory allocation, must be at least 8 byte aligned on 32 bit platform and 16 byte aligned on 64 bit platform +using AllocateFunction = void *(*)(size_t inSize); +using ReallocateFunction = void *(*)(void *inBlock, size_t inOldSize, size_t inNewSize); +using FreeFunction = void (*)(void *inBlock); + +// Aligned memory allocation +using AlignedAllocateFunction = void *(*)(size_t inSize, size_t inAlignment); +using AlignedFreeFunction = void (*)(void *inBlock); + +// User defined allocation / free functions +JPH_EXPORT extern AllocateFunction Allocate; +JPH_EXPORT extern ReallocateFunction Reallocate; +JPH_EXPORT extern FreeFunction Free; +JPH_EXPORT extern AlignedAllocateFunction AlignedAllocate; +JPH_EXPORT extern AlignedFreeFunction AlignedFree; + +/// Register platform default allocation / free functions +JPH_EXPORT void RegisterDefaultAllocator(); + +/// Macro to override the new and delete functions +#define JPH_OVERRIDE_NEW_DELETE \ + JPH_INLINE void *operator new (size_t inCount) { return JPH::Allocate(inCount); } \ + JPH_INLINE void operator delete (void *inPointer) noexcept { JPH::Free(inPointer); } \ + JPH_INLINE void *operator new[] (size_t inCount) { return JPH::Allocate(inCount); } \ + JPH_INLINE void operator delete[] (void *inPointer) noexcept { JPH::Free(inPointer); } \ + JPH_INLINE void *operator new (size_t inCount, std::align_val_t inAlignment) { return JPH::AlignedAllocate(inCount, static_cast(inAlignment)); } \ + JPH_INLINE void operator delete (void *inPointer, [[maybe_unused]] std::align_val_t inAlignment) noexcept { JPH::AlignedFree(inPointer); } \ + JPH_INLINE void *operator new[] (size_t inCount, std::align_val_t inAlignment) { return JPH::AlignedAllocate(inCount, static_cast(inAlignment)); } \ + JPH_INLINE void operator delete[] (void *inPointer, [[maybe_unused]] std::align_val_t inAlignment) noexcept { JPH::AlignedFree(inPointer); } + +#else + +// Directly define the allocation functions +JPH_EXPORT void *Allocate(size_t inSize); +JPH_EXPORT void *Reallocate(void *inBlock, size_t inOldSize, size_t inNewSize); +JPH_EXPORT void Free(void *inBlock); +JPH_EXPORT void *AlignedAllocate(size_t inSize, size_t inAlignment); +JPH_EXPORT void AlignedFree(void *inBlock); + +// Don't implement allocator registering +inline void RegisterDefaultAllocator() { } + +// Don't override new/delete +#define JPH_OVERRIDE_NEW_DELETE + +#endif // !JPH_DISABLE_CUSTOM_ALLOCATOR + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/Mutex.h b/thirdparty/jolt_physics/Jolt/Core/Mutex.h new file mode 100644 index 000000000000..6969aa45d0a8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/Mutex.h @@ -0,0 +1,223 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +#include +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +// Things we're using from STL +using std::mutex; +using std::shared_mutex; +using std::thread; +using std::lock_guard; +using std::shared_lock; +using std::unique_lock; + +#ifdef JPH_PLATFORM_BLUE + +// On Platform Blue the mutex class is not very fast so we implement it using the official APIs +class MutexBase : public NonCopyable +{ +public: + MutexBase() + { + JPH_PLATFORM_BLUE_MUTEX_INIT(mMutex); + } + + ~MutexBase() + { + JPH_PLATFORM_BLUE_MUTEX_DESTROY(mMutex); + } + + inline bool try_lock() + { + return JPH_PLATFORM_BLUE_MUTEX_TRYLOCK(mMutex); + } + + inline void lock() + { + JPH_PLATFORM_BLUE_MUTEX_LOCK(mMutex); + } + + inline void unlock() + { + JPH_PLATFORM_BLUE_MUTEX_UNLOCK(mMutex); + } + +private: + JPH_PLATFORM_BLUE_MUTEX mMutex; +}; + +// On Platform Blue the shared_mutex class is not very fast so we implement it using the official APIs +class SharedMutexBase : public NonCopyable +{ +public: + SharedMutexBase() + { + JPH_PLATFORM_BLUE_RWLOCK_INIT(mRWLock); + } + + ~SharedMutexBase() + { + JPH_PLATFORM_BLUE_RWLOCK_DESTROY(mRWLock); + } + + inline bool try_lock() + { + return JPH_PLATFORM_BLUE_RWLOCK_TRYWLOCK(mRWLock); + } + + inline bool try_lock_shared() + { + return JPH_PLATFORM_BLUE_RWLOCK_TRYRLOCK(mRWLock); + } + + inline void lock() + { + JPH_PLATFORM_BLUE_RWLOCK_WLOCK(mRWLock); + } + + inline void unlock() + { + JPH_PLATFORM_BLUE_RWLOCK_WUNLOCK(mRWLock); + } + + inline void lock_shared() + { + JPH_PLATFORM_BLUE_RWLOCK_RLOCK(mRWLock); + } + + inline void unlock_shared() + { + JPH_PLATFORM_BLUE_RWLOCK_RUNLOCK(mRWLock); + } + +private: + JPH_PLATFORM_BLUE_RWLOCK mRWLock; +}; + +#else + +// On other platforms just use the STL implementation +using MutexBase = mutex; +using SharedMutexBase = shared_mutex; + +#endif // JPH_PLATFORM_BLUE + +#if defined(JPH_ENABLE_ASSERTS) || defined(JPH_PROFILE_ENABLED) || defined(JPH_EXTERNAL_PROFILE) + +/// Very simple wrapper around MutexBase which tracks lock contention in the profiler +/// and asserts that locks/unlocks take place on the same thread +class Mutex : public MutexBase +{ +public: + inline bool try_lock() + { + JPH_ASSERT(mLockedThreadID != std::this_thread::get_id()); + if (MutexBase::try_lock()) + { + JPH_IF_ENABLE_ASSERTS(mLockedThreadID = std::this_thread::get_id();) + return true; + } + return false; + } + + inline void lock() + { + if (!try_lock()) + { + JPH_PROFILE("Lock", 0xff00ffff); + MutexBase::lock(); + JPH_IF_ENABLE_ASSERTS(mLockedThreadID = std::this_thread::get_id();) + } + } + + inline void unlock() + { + JPH_ASSERT(mLockedThreadID == std::this_thread::get_id()); + JPH_IF_ENABLE_ASSERTS(mLockedThreadID = thread::id();) + MutexBase::unlock(); + } + +#ifdef JPH_ENABLE_ASSERTS + inline bool is_locked() + { + return mLockedThreadID != thread::id(); + } +#endif // JPH_ENABLE_ASSERTS + +private: + JPH_IF_ENABLE_ASSERTS(thread::id mLockedThreadID;) +}; + +/// Very simple wrapper around SharedMutexBase which tracks lock contention in the profiler +/// and asserts that locks/unlocks take place on the same thread +class SharedMutex : public SharedMutexBase +{ +public: + inline bool try_lock() + { + JPH_ASSERT(mLockedThreadID != std::this_thread::get_id()); + if (SharedMutexBase::try_lock()) + { + JPH_IF_ENABLE_ASSERTS(mLockedThreadID = std::this_thread::get_id();) + return true; + } + return false; + } + + inline void lock() + { + if (!try_lock()) + { + JPH_PROFILE("WLock", 0xff00ffff); + SharedMutexBase::lock(); + JPH_IF_ENABLE_ASSERTS(mLockedThreadID = std::this_thread::get_id();) + } + } + + inline void unlock() + { + JPH_ASSERT(mLockedThreadID == std::this_thread::get_id()); + JPH_IF_ENABLE_ASSERTS(mLockedThreadID = thread::id();) + SharedMutexBase::unlock(); + } + +#ifdef JPH_ENABLE_ASSERTS + inline bool is_locked() + { + return mLockedThreadID != thread::id(); + } +#endif // JPH_ENABLE_ASSERTS + + inline void lock_shared() + { + if (!try_lock_shared()) + { + JPH_PROFILE("RLock", 0xff00ffff); + SharedMutexBase::lock_shared(); + } + } + +private: + JPH_IF_ENABLE_ASSERTS(thread::id mLockedThreadID;) +}; + +#else + +using Mutex = MutexBase; +using SharedMutex = SharedMutexBase; + +#endif + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/MutexArray.h b/thirdparty/jolt_physics/Jolt/Core/MutexArray.h new file mode 100644 index 000000000000..3a0b5586ae92 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/MutexArray.h @@ -0,0 +1,98 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// A mutex array protects a number of resources with a limited amount of mutexes. +/// It uses hashing to find the mutex of a particular object. +/// The idea is that if the amount of threads is much smaller than the amount of mutexes +/// that there is a relatively small chance that two different objects map to the same mutex. +template +class MutexArray : public NonCopyable +{ +public: + /// Constructor, constructs an empty mutex array that you need to initialize with Init() + MutexArray() = default; + + /// Constructor, constructs an array with inNumMutexes entries + explicit MutexArray(uint inNumMutexes) { Init(inNumMutexes); } + + /// Destructor + ~MutexArray() { delete [] mMutexStorage; } + + /// Initialization + /// @param inNumMutexes The amount of mutexes to allocate + void Init(uint inNumMutexes) + { + JPH_ASSERT(mMutexStorage == nullptr); + JPH_ASSERT(inNumMutexes > 0 && IsPowerOf2(inNumMutexes)); + + mMutexStorage = new MutexStorage[inNumMutexes]; + mNumMutexes = inNumMutexes; + } + + /// Get the number of mutexes that were allocated + inline uint GetNumMutexes() const + { + return mNumMutexes; + } + + /// Convert an object index to a mutex index + inline uint32 GetMutexIndex(uint32 inObjectIndex) const + { + Hash hasher; + return hasher(inObjectIndex) & (mNumMutexes - 1); + } + + /// Get the mutex belonging to a certain object by index + inline MutexType & GetMutexByObjectIndex(uint32 inObjectIndex) + { + return mMutexStorage[GetMutexIndex(inObjectIndex)].mMutex; + } + + /// Get a mutex by index in the array + inline MutexType & GetMutexByIndex(uint32 inMutexIndex) + { + return mMutexStorage[inMutexIndex].mMutex; + } + + /// Lock all mutexes + void LockAll() + { + JPH_PROFILE_FUNCTION(); + + MutexStorage *end = mMutexStorage + mNumMutexes; + for (MutexStorage *m = mMutexStorage; m < end; ++m) + m->mMutex.lock(); + } + + /// Unlock all mutexes + void UnlockAll() + { + JPH_PROFILE_FUNCTION(); + + MutexStorage *end = mMutexStorage + mNumMutexes; + for (MutexStorage *m = mMutexStorage; m < end; ++m) + m->mMutex.unlock(); + } + +private: + /// Align the mutex to a cache line to ensure there is no false sharing (this is platform dependent, we do this to be safe) + struct alignas(JPH_CACHE_LINE_SIZE) MutexStorage + { + JPH_OVERRIDE_NEW_DELETE + + MutexType mMutex; + }; + + MutexStorage * mMutexStorage = nullptr; + uint mNumMutexes = 0; +}; + +JPH_NAMESPACE_END + diff --git a/thirdparty/jolt_physics/Jolt/Core/NonCopyable.h b/thirdparty/jolt_physics/Jolt/Core/NonCopyable.h new file mode 100644 index 000000000000..18b431e925a3 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/NonCopyable.h @@ -0,0 +1,18 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Class that makes another class non-copyable. Usage: Inherit from NonCopyable. +class JPH_EXPORT NonCopyable +{ +public: + NonCopyable() = default; + NonCopyable(const NonCopyable &) = delete; + void operator = (const NonCopyable &) = delete; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/Profiler.cpp b/thirdparty/jolt_physics/Jolt/Core/Profiler.cpp new file mode 100644 index 000000000000..39152b10b4d7 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/Profiler.cpp @@ -0,0 +1,354 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +#if defined(JPH_EXTERNAL_PROFILE) && defined(JPH_SHARED_LIBRARY) + +ProfileStartMeasurementFunction ProfileStartMeasurement = [](const char *, uint32, uint8 *) { }; +ProfileEndMeasurementFunction ProfileEndMeasurement = [](uint8 *) { }; + +#elif defined(JPH_PROFILE_ENABLED) + +////////////////////////////////////////////////////////////////////////////////////////// +// Profiler +////////////////////////////////////////////////////////////////////////////////////////// + +Profiler *Profiler::sInstance = nullptr; + +#ifdef JPH_SHARED_LIBRARY + static thread_local ProfileThread *sInstance = nullptr; + + ProfileThread *ProfileThread::sGetInstance() + { + return sInstance; + } + + void ProfileThread::sSetInstance(ProfileThread *inInstance) + { + sInstance = inInstance; + } +#else + thread_local ProfileThread *ProfileThread::sInstance = nullptr; +#endif + +bool ProfileMeasurement::sOutOfSamplesReported = false; + +void Profiler::UpdateReferenceTime() +{ + mReferenceTick = GetProcessorTickCount(); + mReferenceTime = std::chrono::high_resolution_clock::now(); +} + +uint64 Profiler::GetProcessorTicksPerSecond() const +{ + uint64 ticks = GetProcessorTickCount(); + std::chrono::high_resolution_clock::time_point time = std::chrono::high_resolution_clock::now(); + + return (ticks - mReferenceTick) * 1000000000ULL / std::chrono::duration_cast(time - mReferenceTime).count(); +} + +// This function assumes that none of the threads are active while we're dumping the profile, +// otherwise there will be a race condition on mCurrentSample and the profile data. +JPH_TSAN_NO_SANITIZE +void Profiler::NextFrame() +{ + std::lock_guard lock(mLock); + + if (mDump) + { + DumpInternal(); + mDump = false; + } + + for (ProfileThread *t : mThreads) + t->mCurrentSample = 0; + + UpdateReferenceTime(); +} + +void Profiler::Dump(const string_view &inTag) +{ + mDump = true; + mDumpTag = inTag; +} + +void Profiler::AddThread(ProfileThread *inThread) +{ + std::lock_guard lock(mLock); + + mThreads.push_back(inThread); +} + +void Profiler::RemoveThread(ProfileThread *inThread) +{ + std::lock_guard lock(mLock); + + Array::iterator i = std::find(mThreads.begin(), mThreads.end(), inThread); + JPH_ASSERT(i != mThreads.end()); + mThreads.erase(i); +} + +void Profiler::sAggregate(int inDepth, uint32 inColor, ProfileSample *&ioSample, const ProfileSample *inEnd, Aggregators &ioAggregators, KeyToAggregator &ioKeyToAggregator) +{ + // Store depth + ioSample->mDepth = uint8(min(255, inDepth)); + + // Update color + if (ioSample->mColor == 0) + ioSample->mColor = inColor; + else + inColor = ioSample->mColor; + + // Start accumulating totals + uint64 cycles_this_with_children = ioSample->mEndCycle - ioSample->mStartCycle; + + // Loop over following samples until we find a sample that starts on or after our end + ProfileSample *sample; + for (sample = ioSample + 1; sample < inEnd && sample->mStartCycle < ioSample->mEndCycle; ++sample) + { + JPH_ASSERT(sample[-1].mStartCycle <= sample->mStartCycle); + JPH_ASSERT(sample->mStartCycle >= ioSample->mStartCycle); + JPH_ASSERT(sample->mEndCycle <= ioSample->mEndCycle); + + // Recurse and skip over the children of this child + sAggregate(inDepth + 1, inColor, sample, inEnd, ioAggregators, ioKeyToAggregator); + } + + // Find the aggregator for this name / filename pair + Aggregator *aggregator; + KeyToAggregator::iterator aggregator_idx = ioKeyToAggregator.find(ioSample->mName); + if (aggregator_idx == ioKeyToAggregator.end()) + { + // Not found, add to map and insert in array + ioKeyToAggregator.try_emplace(ioSample->mName, ioAggregators.size()); + ioAggregators.emplace_back(ioSample->mName); + aggregator = &ioAggregators.back(); + } + else + { + // Found + aggregator = &ioAggregators[aggregator_idx->second]; + } + + // Add the measurement to the aggregator + aggregator->AccumulateMeasurement(cycles_this_with_children); + + // Update ioSample to the last child of ioSample + JPH_ASSERT(sample[-1].mStartCycle <= ioSample->mEndCycle); + JPH_ASSERT(sample >= inEnd || sample->mStartCycle >= ioSample->mEndCycle); + ioSample = sample - 1; +} + +void Profiler::DumpInternal() +{ + // Freeze data from threads + // Note that this is not completely thread safe: As a profile sample is added mCurrentSample is incremented + // but the data is not written until the sample finishes. So if we dump the profile information while + // some other thread is running, we may get some garbage information from the previous frame + Threads threads; + for (ProfileThread *t : mThreads) + threads.push_back({ t->mThreadName, t->mSamples, t->mSamples + t->mCurrentSample }); + + // Shift all samples so that the first sample is at zero + uint64 min_cycle = 0xffffffffffffffffUL; + for (const ThreadSamples &t : threads) + if (t.mSamplesBegin < t.mSamplesEnd) + min_cycle = min(min_cycle, t.mSamplesBegin[0].mStartCycle); + for (const ThreadSamples &t : threads) + for (ProfileSample *s = t.mSamplesBegin, *end = t.mSamplesEnd; s < end; ++s) + { + s->mStartCycle -= min_cycle; + s->mEndCycle -= min_cycle; + } + + // Determine tag of this profile + String tag; + if (mDumpTag.empty()) + { + // Next sequence number + static int number = 0; + ++number; + tag = ConvertToString(number); + } + else + { + // Take provided tag + tag = mDumpTag; + mDumpTag.clear(); + } + + // Aggregate data across threads + Aggregators aggregators; + KeyToAggregator key_to_aggregators; + for (const ThreadSamples &t : threads) + for (ProfileSample *s = t.mSamplesBegin, *end = t.mSamplesEnd; s < end; ++s) + sAggregate(0, Color::sGetDistinctColor(0).GetUInt32(), s, end, aggregators, key_to_aggregators); + + // Dump as chart + DumpChart(tag.c_str(), threads, key_to_aggregators, aggregators); +} + +static String sHTMLEncode(const char *inString) +{ + String str(inString); + StringReplace(str, "<", "<"); + StringReplace(str, ">", ">"); + return str; +} + +void Profiler::DumpChart(const char *inTag, const Threads &inThreads, const KeyToAggregator &inKeyToAggregators, const Aggregators &inAggregators) +{ + // Open file + std::ofstream f; + f.open(StringFormat("profile_chart_%s.html", inTag).c_str(), std::ofstream::out | std::ofstream::trunc); + if (!f.is_open()) + return; + + // Write header + f << R"( + + + Profile Chart + + + + + + + +
+ +)"; +} + +#endif // JPH_PROFILE_ENABLED + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/Profiler.h b/thirdparty/jolt_physics/Jolt/Core/Profiler.h new file mode 100644 index 000000000000..878865313ba2 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/Profiler.h @@ -0,0 +1,301 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +#include +JPH_SUPPRESS_WARNINGS_STD_END + +#include +#include +#include + +#if defined(JPH_EXTERNAL_PROFILE) + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_SHARED_LIBRARY +/// Functions called when a profiler measurement starts or stops, need to be overridden by the user. +using ProfileStartMeasurementFunction = void (*)(const char *inName, uint32 inColor, uint8 *ioUserData); +using ProfileEndMeasurementFunction = void (*)(uint8 *ioUserData); + +JPH_EXPORT extern ProfileStartMeasurementFunction ProfileStartMeasurement; +JPH_EXPORT extern ProfileEndMeasurementFunction ProfileEndMeasurement; +#endif // JPH_SHARED_LIBRARY + +/// Create this class on the stack to start sampling timing information of a particular scope. +/// +/// For statically linked builds, this is left unimplemented intentionally. Needs to be implemented by the user of the library. +/// On construction a measurement should start, on destruction it should be stopped. +/// For dynamically linked builds, the user should override the ProfileStartMeasurement and ProfileEndMeasurement functions. +class alignas(16) ExternalProfileMeasurement : public NonCopyable +{ +public: + /// Constructor +#ifdef JPH_SHARED_LIBRARY + JPH_INLINE ExternalProfileMeasurement(const char *inName, uint32 inColor = 0) { ProfileStartMeasurement(inName, inColor, mUserData); } + JPH_INLINE ~ExternalProfileMeasurement() { ProfileEndMeasurement(mUserData); } +#else + ExternalProfileMeasurement(const char *inName, uint32 inColor = 0); + ~ExternalProfileMeasurement(); +#endif + +private: + uint8 mUserData[64]; +}; + +JPH_NAMESPACE_END + +////////////////////////////////////////////////////////////////////////////////////////// +// Macros to do the actual profiling +////////////////////////////////////////////////////////////////////////////////////////// + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic") + +// Dummy implementations +#define JPH_PROFILE_START(name) +#define JPH_PROFILE_END() +#define JPH_PROFILE_THREAD_START(name) +#define JPH_PROFILE_THREAD_END() +#define JPH_PROFILE_NEXTFRAME() +#define JPH_PROFILE_DUMP(...) + +// Scope profiling measurement +#define JPH_PROFILE_TAG2(line) profile##line +#define JPH_PROFILE_TAG(line) JPH_PROFILE_TAG2(line) + +/// Macro to collect profiling information. +/// +/// Usage: +/// +/// { +/// JPH_PROFILE("Operation"); +/// do operation; +/// } +/// +#define JPH_PROFILE(...) ExternalProfileMeasurement JPH_PROFILE_TAG(__LINE__)(__VA_ARGS__) + +// Scope profiling for function +#define JPH_PROFILE_FUNCTION() JPH_PROFILE(JPH_FUNCTION_NAME) + +JPH_SUPPRESS_WARNING_POP + +#elif defined(JPH_PROFILE_ENABLED) + +JPH_NAMESPACE_BEGIN + +class ProfileSample; +class ProfileThread; + +/// Singleton class for managing profiling information +class JPH_EXPORT Profiler : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + Profiler() { UpdateReferenceTime(); } + + /// Increments the frame counter to provide statistics per frame + void NextFrame(); + + /// Dump profiling statistics at the start of the next frame + /// @param inTag If not empty, this overrides the auto incrementing number in the filename of the dump file + void Dump(const string_view &inTag = string_view()); + + /// Add a thread to be instrumented + void AddThread(ProfileThread *inThread); + + /// Remove a thread from being instrumented + void RemoveThread(ProfileThread *inThread); + + /// Singleton instance + static Profiler * sInstance; + +private: + /// Helper class to freeze ProfileSamples per thread while processing them + struct ThreadSamples + { + String mThreadName; + ProfileSample * mSamplesBegin; + ProfileSample * mSamplesEnd; + }; + + /// Helper class to aggregate ProfileSamples + class Aggregator + { + public: + /// Constructor + Aggregator(const char *inName) : mName(inName) { } + + /// Accumulate results for a measurement + void AccumulateMeasurement(uint64 inCyclesInCallWithChildren) + { + mCallCounter++; + mTotalCyclesInCallWithChildren += inCyclesInCallWithChildren; + mMinCyclesInCallWithChildren = min(inCyclesInCallWithChildren, mMinCyclesInCallWithChildren); + mMaxCyclesInCallWithChildren = max(inCyclesInCallWithChildren, mMaxCyclesInCallWithChildren); + } + + /// Sort descending by total cycles + bool operator < (const Aggregator &inRHS) const + { + return mTotalCyclesInCallWithChildren > inRHS.mTotalCyclesInCallWithChildren; + } + + /// Identification + const char * mName; ///< User defined name of this item + + /// Statistics + uint32 mCallCounter = 0; ///< Number of times AccumulateMeasurement was called + uint64 mTotalCyclesInCallWithChildren = 0; ///< Total amount of cycles spent in this scope + uint64 mMinCyclesInCallWithChildren = 0xffffffffffffffffUL; ///< Minimum amount of cycles spent per call + uint64 mMaxCyclesInCallWithChildren = 0; ///< Maximum amount of cycles spent per call + }; + + using Threads = Array; + using Aggregators = Array; + using KeyToAggregator = UnorderedMap; + + /// Helper function to aggregate profile sample data + static void sAggregate(int inDepth, uint32 inColor, ProfileSample *&ioSample, const ProfileSample *inEnd, Aggregators &ioAggregators, KeyToAggregator &ioKeyToAggregator); + + /// We measure the amount of ticks per second, this function resets the reference time point + void UpdateReferenceTime(); + + /// Get the amount of ticks per second, note that this number will never be fully accurate as the amount of ticks per second may vary with CPU load, so this number is only to be used to give an indication of time for profiling purposes + uint64 GetProcessorTicksPerSecond() const; + + /// Dump profiling statistics + void DumpInternal(); + void DumpChart(const char *inTag, const Threads &inThreads, const KeyToAggregator &inKeyToAggregators, const Aggregators &inAggregators); + + std::mutex mLock; ///< Lock that protects mThreads + uint64 mReferenceTick; ///< Tick count at the start of the frame + std::chrono::high_resolution_clock::time_point mReferenceTime; ///< Time at the start of the frame + Array mThreads; ///< List of all active threads + bool mDump = false; ///< When true, the samples are dumped next frame + String mDumpTag; ///< When not empty, this overrides the auto incrementing number of the dump filename +}; + +// Class that contains the information of a single scoped measurement +class alignas(16) JPH_EXPORT_GCC_BUG_WORKAROUND ProfileSample : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + const char * mName; ///< User defined name of this item + uint32 mColor; ///< Color to use for this sample + uint8 mDepth; ///< Calculated depth + uint8 mUnused[3]; + uint64 mStartCycle; ///< Cycle counter at start of measurement + uint64 mEndCycle; ///< Cycle counter at end of measurement +}; + +/// Collects all samples of a single thread +class ProfileThread : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + inline ProfileThread(const string_view &inThreadName); + inline ~ProfileThread(); + + static const uint cMaxSamples = 65536; + + String mThreadName; ///< Name of the thread that we're collecting information for + ProfileSample mSamples[cMaxSamples]; ///< Buffer of samples + uint mCurrentSample = 0; ///< Next position to write a sample to + +#ifdef JPH_SHARED_LIBRARY + JPH_EXPORT static void sSetInstance(ProfileThread *inInstance); + JPH_EXPORT static ProfileThread *sGetInstance(); +#else + static inline void sSetInstance(ProfileThread *inInstance) { sInstance = inInstance; } + static inline ProfileThread *sGetInstance() { return sInstance; } + +private: + static thread_local ProfileThread *sInstance; +#endif +}; + +/// Create this class on the stack to start sampling timing information of a particular scope +class JPH_EXPORT ProfileMeasurement : public NonCopyable +{ +public: + /// Constructor + inline ProfileMeasurement(const char *inName, uint32 inColor = 0); + inline ~ProfileMeasurement(); + +private: + ProfileSample * mSample; + ProfileSample mTemp; + + static bool sOutOfSamplesReported; +}; + +JPH_NAMESPACE_END + +#include "Profiler.inl" + +////////////////////////////////////////////////////////////////////////////////////////// +// Macros to do the actual profiling +////////////////////////////////////////////////////////////////////////////////////////// + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic") + +/// Start instrumenting program +#define JPH_PROFILE_START(name) do { Profiler::sInstance = new Profiler; JPH_PROFILE_THREAD_START(name); } while (false) + +/// End instrumenting program +#define JPH_PROFILE_END() do { JPH_PROFILE_THREAD_END(); delete Profiler::sInstance; Profiler::sInstance = nullptr; } while (false) + +/// Start instrumenting a thread +#define JPH_PROFILE_THREAD_START(name) do { if (Profiler::sInstance) ProfileThread::sSetInstance(new ProfileThread(name)); } while (false) + +/// End instrumenting a thread +#define JPH_PROFILE_THREAD_END() do { delete ProfileThread::sGetInstance(); ProfileThread::sSetInstance(nullptr); } while (false) + +/// Scope profiling measurement +#define JPH_PROFILE_TAG2(line) profile##line +#define JPH_PROFILE_TAG(line) JPH_PROFILE_TAG2(line) +#define JPH_PROFILE(...) ProfileMeasurement JPH_PROFILE_TAG(__LINE__)(__VA_ARGS__) + +/// Scope profiling for function +#define JPH_PROFILE_FUNCTION() JPH_PROFILE(JPH_FUNCTION_NAME) + +/// Update frame counter +#define JPH_PROFILE_NEXTFRAME() Profiler::sInstance->NextFrame() + +/// Dump profiling info +#define JPH_PROFILE_DUMP(...) Profiler::sInstance->Dump(__VA_ARGS__) + +JPH_SUPPRESS_WARNING_POP + +#else + +////////////////////////////////////////////////////////////////////////////////////////// +// Dummy profiling instructions +////////////////////////////////////////////////////////////////////////////////////////// + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic") + +#define JPH_PROFILE_START(name) +#define JPH_PROFILE_END() +#define JPH_PROFILE_THREAD_START(name) +#define JPH_PROFILE_THREAD_END() +#define JPH_PROFILE(...) +#define JPH_PROFILE_FUNCTION() +#define JPH_PROFILE_NEXTFRAME() +#define JPH_PROFILE_DUMP(...) + +JPH_SUPPRESS_WARNING_POP + +#endif diff --git a/thirdparty/jolt_physics/Jolt/Core/Profiler.inl b/thirdparty/jolt_physics/Jolt/Core/Profiler.inl new file mode 100644 index 000000000000..912ba8383379 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/Profiler.inl @@ -0,0 +1,90 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +JPH_NAMESPACE_BEGIN + +////////////////////////////////////////////////////////////////////////////////////////// +// ProfileThread +////////////////////////////////////////////////////////////////////////////////////////// + +ProfileThread::ProfileThread(const string_view &inThreadName) : + mThreadName(inThreadName) +{ + Profiler::sInstance->AddThread(this); +} + +ProfileThread::~ProfileThread() +{ + Profiler::sInstance->RemoveThread(this); +} + +////////////////////////////////////////////////////////////////////////////////////////// +// ProfileMeasurement +////////////////////////////////////////////////////////////////////////////////////////// + +JPH_TSAN_NO_SANITIZE // TSAN reports a race on sOutOfSamplesReported, however the worst case is that we report the out of samples message multiple times +ProfileMeasurement::ProfileMeasurement(const char *inName, uint32 inColor) +{ + ProfileThread *current_thread = ProfileThread::sGetInstance(); + if (current_thread == nullptr) + { + // Thread not instrumented + mSample = nullptr; + } + else if (current_thread->mCurrentSample < ProfileThread::cMaxSamples) + { + // Get pointer to write data to + mSample = ¤t_thread->mSamples[current_thread->mCurrentSample++]; + + // Start constructing sample (will end up on stack) + mTemp.mName = inName; + mTemp.mColor = inColor; + + // Collect start sample last + mTemp.mStartCycle = GetProcessorTickCount(); + } + else + { + // Out of samples + if (!sOutOfSamplesReported) + { + sOutOfSamplesReported = true; + Trace("ProfileMeasurement: Too many samples, some data will be lost!"); + } + mSample = nullptr; + } +} + +ProfileMeasurement::~ProfileMeasurement() +{ + if (mSample != nullptr) + { + // Finalize sample + mTemp.mEndCycle = GetProcessorTickCount(); + + // Write it to the memory buffer bypassing the cache + static_assert(sizeof(ProfileSample) == 32, "Assume 32 bytes"); + static_assert(alignof(ProfileSample) == 16, "Assume 16 byte alignment"); + #if defined(JPH_USE_SSE) + const __m128i *src = reinterpret_cast(&mTemp); + __m128i *dst = reinterpret_cast<__m128i *>(mSample); + __m128i val = _mm_loadu_si128(src); + _mm_stream_si128(dst, val); + val = _mm_loadu_si128(src + 1); + _mm_stream_si128(dst + 1, val); + #elif defined(JPH_USE_NEON) + const int *src = reinterpret_cast(&mTemp); + int *dst = reinterpret_cast(mSample); + int32x4_t val = vld1q_s32(src); + vst1q_s32(dst, val); + val = vld1q_s32(src + 4); + vst1q_s32(dst + 4, val); + #else + memcpy(mSample, &mTemp, sizeof(ProfileSample)); + #endif + mSample = nullptr; + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/QuickSort.h b/thirdparty/jolt_physics/Jolt/Core/QuickSort.h new file mode 100644 index 000000000000..bd12a3b339db --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/QuickSort.h @@ -0,0 +1,137 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Helper function for QuickSort, will move the pivot element to inMiddle. +template +inline void QuickSortMedianOfThree(Iterator inFirst, Iterator inMiddle, Iterator inLast, Compare inCompare) +{ + // This should be guaranteed because we switch over to insertion sort when there's 32 or less elements + JPH_ASSERT(inFirst != inMiddle && inMiddle != inLast); + + if (inCompare(*inMiddle, *inFirst)) + std::swap(*inFirst, *inMiddle); + + if (inCompare(*inLast, *inFirst)) + std::swap(*inFirst, *inLast); + + if (inCompare(*inLast, *inMiddle)) + std::swap(*inMiddle, *inLast); +} + +/// Helper function for QuickSort using the Ninther method, will move the pivot element to inMiddle. +template +inline void QuickSortNinther(Iterator inFirst, Iterator inMiddle, Iterator inLast, Compare inCompare) +{ + // Divide the range in 8 equal parts (this means there are 9 points) + auto diff = (inLast - inFirst) >> 3; + auto two_diff = diff << 1; + + // Median of first 3 points + Iterator mid1 = inFirst + diff; + QuickSortMedianOfThree(inFirst, mid1, inFirst + two_diff, inCompare); + + // Median of second 3 points + QuickSortMedianOfThree(inMiddle - diff, inMiddle, inMiddle + diff, inCompare); + + // Median of third 3 points + Iterator mid3 = inLast - diff; + QuickSortMedianOfThree(inLast - two_diff, mid3, inLast, inCompare); + + // Determine the median of the 3 medians + QuickSortMedianOfThree(mid1, inMiddle, mid3, inCompare); +} + +/// Implementation of the quick sort algorithm. The STL version implementation is not consistent across platforms. +template +inline void QuickSort(Iterator inBegin, Iterator inEnd, Compare inCompare) +{ + // Implementation based on https://en.wikipedia.org/wiki/Quicksort using Hoare's partition scheme + + // Loop so that we only need to do 1 recursive call instead of 2. + for (;;) + { + // If there's less than 2 elements we're done + auto num_elements = inEnd - inBegin; + if (num_elements < 2) + return; + + // Fall back to insertion sort if there are too few elements + if (num_elements <= 32) + { + InsertionSort(inBegin, inEnd, inCompare); + return; + } + + // Determine pivot + Iterator pivot_iterator = inBegin + ((num_elements - 1) >> 1); + QuickSortNinther(inBegin, pivot_iterator, inEnd - 1, inCompare); + auto pivot = *pivot_iterator; + + // Left and right iterators + Iterator i = inBegin; + Iterator j = inEnd; + + for (;;) + { + // Find the first element that is bigger than the pivot + while (inCompare(*i, pivot)) + i++; + + // Find the last element that is smaller than the pivot + do + --j; + while (inCompare(pivot, *j)); + + // If the two iterators crossed, we're done + if (i >= j) + break; + + // Swap the elements + std::swap(*i, *j); + + // Note that the first while loop in this function should + // have been do i++ while (...) but since we cannot decrement + // the iterator from inBegin we left that out, so we need to do + // it here. + ++i; + } + + // Include the middle element on the left side + j++; + + // Check which partition is smaller + if (j - inBegin < inEnd - j) + { + // Left side is smaller, recurse to left first + QuickSort(inBegin, j, inCompare); + + // Loop again with the right side to avoid a call + inBegin = j; + } + else + { + // Right side is smaller, recurse to right first + QuickSort(j, inEnd, inCompare); + + // Loop again with the left side to avoid a call + inEnd = j; + } + } +} + +/// Implementation of quick sort algorithm without comparator. +template +inline void QuickSort(Iterator inBegin, Iterator inEnd) +{ + std::less<> compare; + QuickSort(inBegin, inEnd, compare); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/RTTI.cpp b/thirdparty/jolt_physics/Jolt/Core/RTTI.cpp new file mode 100644 index 000000000000..91235745adc1 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/RTTI.cpp @@ -0,0 +1,149 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +////////////////////////////////////////////////////////////////////////////////////////// +// RTTI +////////////////////////////////////////////////////////////////////////////////////////// + +RTTI::RTTI(const char *inName, int inSize, pCreateObjectFunction inCreateObject, pDestructObjectFunction inDestructObject) : + mName(inName), + mSize(inSize), + mCreate(inCreateObject), + mDestruct(inDestructObject) +{ + JPH_ASSERT(inDestructObject != nullptr, "Object cannot be destructed"); +} + +RTTI::RTTI(const char *inName, int inSize, pCreateObjectFunction inCreateObject, pDestructObjectFunction inDestructObject, pCreateRTTIFunction inCreateRTTI) : + mName(inName), + mSize(inSize), + mCreate(inCreateObject), + mDestruct(inDestructObject) +{ + JPH_ASSERT(inDestructObject != nullptr, "Object cannot be destructed"); + + inCreateRTTI(*this); +} + +int RTTI::GetBaseClassCount() const +{ + return (int)mBaseClasses.size(); +} + +const RTTI *RTTI::GetBaseClass(int inIdx) const +{ + return mBaseClasses[inIdx].mRTTI; +} + +uint32 RTTI::GetHash() const +{ + // Perform diffusion step to get from 64 to 32 bits (see https://en.wikipedia.org/wiki/Fowler%E2%80%93Noll%E2%80%93Vo_hash_function) + uint64 hash = HashString(mName); + return (uint32)(hash ^ (hash >> 32)); +} + +void *RTTI::CreateObject() const +{ + return IsAbstract()? nullptr : mCreate(); +} + +void RTTI::DestructObject(void *inObject) const +{ + mDestruct(inObject); +} + +void RTTI::AddBaseClass(const RTTI *inRTTI, int inOffset) +{ + JPH_ASSERT(inOffset >= 0 && inOffset < mSize, "Base class not contained in derived class"); + + // Add base class + BaseClass base; + base.mRTTI = inRTTI; + base.mOffset = inOffset; + mBaseClasses.push_back(base); + +#ifdef JPH_OBJECT_STREAM + // Add attributes of base class + for (const SerializableAttribute &a : inRTTI->mAttributes) + mAttributes.push_back(SerializableAttribute(a, inOffset)); +#endif // JPH_OBJECT_STREAM +} + +bool RTTI::operator == (const RTTI &inRHS) const +{ + // Compare addresses + if (this == &inRHS) + return true; + + // Check that the names differ (if that is the case we probably have two instances + // of the same attribute info across the program, probably the second is in a DLL) + JPH_ASSERT(strcmp(mName, inRHS.mName) != 0); + return false; +} + +bool RTTI::IsKindOf(const RTTI *inRTTI) const +{ + // Check if this is the same type + if (this == inRTTI) + return true; + + // Check all base classes + for (const BaseClass &b : mBaseClasses) + if (b.mRTTI->IsKindOf(inRTTI)) + return true; + + return false; +} + +const void *RTTI::CastTo(const void *inObject, const RTTI *inRTTI) const +{ + JPH_ASSERT(inObject != nullptr); + + // Check if this is the same type + if (this == inRTTI) + return inObject; + + // Check all base classes + for (const BaseClass &b : mBaseClasses) + { + // Cast the pointer to the base class + const void *casted = (const void *)(((const uint8 *)inObject) + b.mOffset); + + // Test base class + const void *rv = b.mRTTI->CastTo(casted, inRTTI); + if (rv != nullptr) + return rv; + } + + // Not possible to cast + return nullptr; +} + +#ifdef JPH_OBJECT_STREAM + +void RTTI::AddAttribute(const SerializableAttribute &inAttribute) +{ + mAttributes.push_back(inAttribute); +} + +int RTTI::GetAttributeCount() const +{ + return (int)mAttributes.size(); +} + +const SerializableAttribute &RTTI::GetAttribute(int inIdx) const +{ + return mAttributes[inIdx]; +} + +#endif // JPH_OBJECT_STREAM + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/RTTI.h b/thirdparty/jolt_physics/Jolt/Core/RTTI.h new file mode 100644 index 000000000000..90f358cbd580 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/RTTI.h @@ -0,0 +1,436 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +////////////////////////////////////////////////////////////////////////////////////////// +// RTTI +////////////////////////////////////////////////////////////////////////////////////////// + +/// Light weight runtime type information system. This way we don't need to turn +/// on the default RTTI system of the compiler (introducing a possible overhead for every +/// class) +/// +/// Notes: +/// - An extra virtual member function is added. This adds 8 bytes to the size of +/// an instance of the class (unless you are already using virtual functions). +/// +/// To use RTTI on a specific class use: +/// +/// Header file: +/// +/// class Foo +/// { +/// JPH_DECLARE_RTTI_VIRTUAL_BASE(Foo) +/// } +/// +/// class Bar : public Foo +/// { +/// JPH_DECLARE_RTTI_VIRTUAL(Bar) +/// }; +/// +/// Implementation file: +/// +/// JPH_IMPLEMENT_RTTI_VIRTUAL_BASE(Foo) +/// { +/// } +/// +/// JPH_IMPLEMENT_RTTI_VIRTUAL(Bar) +/// { +/// JPH_ADD_BASE_CLASS(Bar, Foo) // Multiple inheritance is allowed, just do JPH_ADD_BASE_CLASS for every base class +/// } +/// +/// For abstract classes use: +/// +/// Header file: +/// +/// class Foo +/// { +/// JPH_DECLARE_RTTI_ABSTRACT_BASE(Foo) +/// +/// public: +/// virtual void AbstractFunction() = 0; +/// } +/// +/// class Bar : public Foo +/// { +/// JPH_DECLARE_RTTI_VIRTUAL(Bar) +/// +/// public: +/// virtual void AbstractFunction() { } // Function is now implemented so this class is no longer abstract +/// }; +/// +/// Implementation file: +/// +/// JPH_IMPLEMENT_RTTI_ABSTRACT_BASE(Foo) +/// { +/// } +/// +/// JPH_IMPLEMENT_RTTI_VIRTUAL(Bar) +/// { +/// JPH_ADD_BASE_CLASS(Bar, Foo) +/// } +/// +/// Example of usage in a program: +/// +/// Foo *foo_ptr = new Foo; +/// Foo *bar_ptr = new Bar; +/// +/// IsType(foo_ptr, RTTI(Bar)) returns false +/// IsType(bar_ptr, RTTI(Bar)) returns true +/// +/// IsKindOf(foo_ptr, RTTI(Bar)) returns false +/// IsKindOf(bar_ptr, RTTI(Foo)) returns true +/// IsKindOf(bar_ptr, RTTI(Bar)) returns true +/// +/// StaticCast(foo_ptr) asserts and returns foo_ptr casted to Bar * +/// StaticCast(bar_ptr) returns bar_ptr casted to Bar * +/// +/// DynamicCast(foo_ptr) returns nullptr +/// DynamicCast(bar_ptr) returns bar_ptr casted to Bar * +/// +/// Other feature of DynamicCast: +/// +/// class A { int data[5]; }; +/// class B { int data[7]; }; +/// class C : public A, public B { int data[9]; }; +/// +/// C *c = new C; +/// A *a = c; +/// +/// Note that: +/// +/// B *b = (B *)a; +/// +/// generates an invalid pointer, +/// +/// B *b = StaticCast(a); +/// +/// doesn't compile, and +/// +/// B *b = DynamicCast(a); +/// +/// does the correct cast +class JPH_EXPORT RTTI +{ +public: + /// Function to create an object + using pCreateObjectFunction = void *(*)(); + + /// Function to destroy an object + using pDestructObjectFunction = void (*)(void *inObject); + + /// Function to initialize the runtime type info structure + using pCreateRTTIFunction = void (*)(RTTI &inRTTI); + + /// Constructor + RTTI(const char *inName, int inSize, pCreateObjectFunction inCreateObject, pDestructObjectFunction inDestructObject); + RTTI(const char *inName, int inSize, pCreateObjectFunction inCreateObject, pDestructObjectFunction inDestructObject, pCreateRTTIFunction inCreateRTTI); + + // Properties + inline const char * GetName() const { return mName; } + void SetName(const char *inName) { mName = inName; } + inline int GetSize() const { return mSize; } + bool IsAbstract() const { return mCreate == nullptr || mDestruct == nullptr; } + int GetBaseClassCount() const; + const RTTI * GetBaseClass(int inIdx) const; + uint32 GetHash() const; + + /// Create an object of this type (returns nullptr if the object is abstract) + void * CreateObject() const; + + /// Destruct object of this type (does nothing if the object is abstract) + void DestructObject(void *inObject) const; + + /// Add base class + void AddBaseClass(const RTTI *inRTTI, int inOffset); + + /// Equality operators + bool operator == (const RTTI &inRHS) const; + bool operator != (const RTTI &inRHS) const { return !(*this == inRHS); } + + /// Test if this class is derived from class of type inRTTI + bool IsKindOf(const RTTI *inRTTI) const; + + /// Cast inObject of this type to object of type inRTTI, returns nullptr if the cast is unsuccessful + const void * CastTo(const void *inObject, const RTTI *inRTTI) const; + +#ifdef JPH_OBJECT_STREAM + /// Attribute access + void AddAttribute(const SerializableAttribute &inAttribute); + int GetAttributeCount() const; + const SerializableAttribute & GetAttribute(int inIdx) const; +#endif // JPH_OBJECT_STREAM + +protected: + /// Base class information + struct BaseClass + { + const RTTI * mRTTI; + int mOffset; + }; + + const char * mName; ///< Class name + int mSize; ///< Class size + StaticArray mBaseClasses; ///< Names of base classes + pCreateObjectFunction mCreate; ///< Pointer to a function that will create a new instance of this class + pDestructObjectFunction mDestruct; ///< Pointer to a function that will destruct an object of this class +#ifdef JPH_OBJECT_STREAM + StaticArray mAttributes; ///< All attributes of this class +#endif // JPH_OBJECT_STREAM +}; + +////////////////////////////////////////////////////////////////////////////////////////// +// Add run time type info to types that don't have virtual functions +////////////////////////////////////////////////////////////////////////////////////////// + +// JPH_DECLARE_RTTI_NON_VIRTUAL +#define JPH_DECLARE_RTTI_NON_VIRTUAL(linkage, class_name) \ +public: \ + JPH_OVERRIDE_NEW_DELETE \ + friend linkage RTTI * GetRTTIOfType(class_name *); \ + friend inline const RTTI * GetRTTI([[maybe_unused]] const class_name *inObject) { return GetRTTIOfType(static_cast(nullptr)); }\ + static void sCreateRTTI(RTTI &inRTTI); \ + +// JPH_IMPLEMENT_RTTI_NON_VIRTUAL +#define JPH_IMPLEMENT_RTTI_NON_VIRTUAL(class_name) \ + RTTI * GetRTTIOfType(class_name *) \ + { \ + static RTTI rtti(#class_name, sizeof(class_name), []() -> void * { return new class_name; }, [](void *inObject) { delete (class_name *)inObject; }, &class_name::sCreateRTTI); \ + return &rtti; \ + } \ + void class_name::sCreateRTTI(RTTI &inRTTI) \ + +////////////////////////////////////////////////////////////////////////////////////////// +// Same as above, but when you cannot insert the declaration in the class +// itself, for example for templates and third party classes +////////////////////////////////////////////////////////////////////////////////////////// + +// JPH_DECLARE_RTTI_OUTSIDE_CLASS +#define JPH_DECLARE_RTTI_OUTSIDE_CLASS(linkage, class_name) \ + linkage RTTI * GetRTTIOfType(class_name *); \ + inline const RTTI * GetRTTI(const class_name *inObject) { return GetRTTIOfType((class_name *)nullptr); }\ + void CreateRTTI##class_name(RTTI &inRTTI); \ + +// JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS +#define JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(class_name) \ + RTTI * GetRTTIOfType(class_name *) \ + { \ + static RTTI rtti((const char *)#class_name, sizeof(class_name), []() -> void * { return new class_name; }, [](void *inObject) { delete (class_name *)inObject; }, &CreateRTTI##class_name); \ + return &rtti; \ + } \ + void CreateRTTI##class_name(RTTI &inRTTI) + +////////////////////////////////////////////////////////////////////////////////////////// +// Same as above, but for classes that have virtual functions +////////////////////////////////////////////////////////////////////////////////////////// + +#define JPH_DECLARE_RTTI_HELPER(linkage, class_name, modifier) \ +public: \ + JPH_OVERRIDE_NEW_DELETE \ + friend linkage RTTI * GetRTTIOfType(class_name *); \ + friend inline const RTTI * GetRTTI(const class_name *inObject) { return inObject->GetRTTI(); } \ + virtual const RTTI * GetRTTI() const modifier; \ + virtual const void * CastTo(const RTTI *inRTTI) const modifier; \ + static void sCreateRTTI(RTTI &inRTTI); \ + +// JPH_DECLARE_RTTI_VIRTUAL - for derived classes with RTTI +#define JPH_DECLARE_RTTI_VIRTUAL(linkage, class_name) \ + JPH_DECLARE_RTTI_HELPER(linkage, class_name, override) + +// JPH_IMPLEMENT_RTTI_VIRTUAL +#define JPH_IMPLEMENT_RTTI_VIRTUAL(class_name) \ + RTTI * GetRTTIOfType(class_name *) \ + { \ + static RTTI rtti(#class_name, sizeof(class_name), []() -> void * { return new class_name; }, [](void *inObject) { delete (class_name *)inObject; }, &class_name::sCreateRTTI); \ + return &rtti; \ + } \ + const RTTI * class_name::GetRTTI() const \ + { \ + return JPH_RTTI(class_name); \ + } \ + const void * class_name::CastTo(const RTTI *inRTTI) const \ + { \ + return JPH_RTTI(class_name)->CastTo((const void *)this, inRTTI); \ + } \ + void class_name::sCreateRTTI(RTTI &inRTTI) \ + +// JPH_DECLARE_RTTI_VIRTUAL_BASE - for concrete base class that has RTTI +#define JPH_DECLARE_RTTI_VIRTUAL_BASE(linkage, class_name) \ + JPH_DECLARE_RTTI_HELPER(linkage, class_name, ) + +// JPH_IMPLEMENT_RTTI_VIRTUAL_BASE +#define JPH_IMPLEMENT_RTTI_VIRTUAL_BASE(class_name) \ + JPH_IMPLEMENT_RTTI_VIRTUAL(class_name) + +// JPH_DECLARE_RTTI_ABSTRACT - for derived abstract class that have RTTI +#define JPH_DECLARE_RTTI_ABSTRACT(linkage, class_name) \ + JPH_DECLARE_RTTI_HELPER(linkage, class_name, override) + +// JPH_IMPLEMENT_RTTI_ABSTRACT +#define JPH_IMPLEMENT_RTTI_ABSTRACT(class_name) \ + RTTI * GetRTTIOfType(class_name *) \ + { \ + static RTTI rtti(#class_name, sizeof(class_name), nullptr, [](void *inObject) { delete (class_name *)inObject; }, &class_name::sCreateRTTI); \ + return &rtti; \ + } \ + const RTTI * class_name::GetRTTI() const \ + { \ + return JPH_RTTI(class_name); \ + } \ + const void * class_name::CastTo(const RTTI *inRTTI) const \ + { \ + return JPH_RTTI(class_name)->CastTo((const void *)this, inRTTI); \ + } \ + void class_name::sCreateRTTI(RTTI &inRTTI) \ + +// JPH_DECLARE_RTTI_ABSTRACT_BASE - for abstract base class that has RTTI +#define JPH_DECLARE_RTTI_ABSTRACT_BASE(linkage, class_name) \ + JPH_DECLARE_RTTI_HELPER(linkage, class_name, ) + +// JPH_IMPLEMENT_RTTI_ABSTRACT_BASE +#define JPH_IMPLEMENT_RTTI_ABSTRACT_BASE(class_name) \ + JPH_IMPLEMENT_RTTI_ABSTRACT(class_name) + +////////////////////////////////////////////////////////////////////////////////////////// +// Declare an RTTI class for registering with the factory +////////////////////////////////////////////////////////////////////////////////////////// + +#define JPH_DECLARE_RTTI_FOR_FACTORY(linkage, class_name) \ + linkage RTTI * GetRTTIOfType(class class_name *); + +#define JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(linkage, name_space, class_name) \ + namespace name_space { \ + class class_name; \ + linkage RTTI * GetRTTIOfType(class class_name *); \ + } + +////////////////////////////////////////////////////////////////////////////////////////// +// Find the RTTI of a class +////////////////////////////////////////////////////////////////////////////////////////// + +#define JPH_RTTI(class_name) GetRTTIOfType(static_cast(nullptr)) + +////////////////////////////////////////////////////////////////////////////////////////// +// Macro to rename a class, useful for embedded classes: +// +// class A { class B { }; } +// +// Now use JPH_RENAME_CLASS(B, A::B) to avoid conflicts with other classes named B +////////////////////////////////////////////////////////////////////////////////////////// + +// JPH_RENAME_CLASS +#define JPH_RENAME_CLASS(class_name, new_name) \ + inRTTI.SetName(#new_name); + +////////////////////////////////////////////////////////////////////////////////////////// +// Macro to add base classes +////////////////////////////////////////////////////////////////////////////////////////// + +/// Define very dirty macro to get the offset of a baseclass into a class +#define JPH_BASE_CLASS_OFFSET(inClass, inBaseClass) ((int(uint64((inBaseClass *)((inClass *)0x10000))))-0x10000) + +// JPH_ADD_BASE_CLASS +#define JPH_ADD_BASE_CLASS(class_name, base_class_name) \ + inRTTI.AddBaseClass(JPH_RTTI(base_class_name), JPH_BASE_CLASS_OFFSET(class_name, base_class_name)); + +////////////////////////////////////////////////////////////////////////////////////////// +// Macros and templates to identify a class +////////////////////////////////////////////////////////////////////////////////////////// + +/// Check if inObject is of DstType +template +inline bool IsType(const Type *inObject, const RTTI *inRTTI) +{ + return inObject == nullptr || *inObject->GetRTTI() == *inRTTI; +} + +template +inline bool IsType(const RefConst &inObject, const RTTI *inRTTI) +{ + return inObject == nullptr || *inObject->GetRTTI() == *inRTTI; +} + +template +inline bool IsType(const Ref &inObject, const RTTI *inRTTI) +{ + return inObject == nullptr || *inObject->GetRTTI() == *inRTTI; +} + +/// Check if inObject is or is derived from DstType +template +inline bool IsKindOf(const Type *inObject, const RTTI *inRTTI) +{ + return inObject == nullptr || inObject->GetRTTI()->IsKindOf(inRTTI); +} + +template +inline bool IsKindOf(const RefConst &inObject, const RTTI *inRTTI) +{ + return inObject == nullptr || inObject->GetRTTI()->IsKindOf(inRTTI); +} + +template +inline bool IsKindOf(const Ref &inObject, const RTTI *inRTTI) +{ + return inObject == nullptr || inObject->GetRTTI()->IsKindOf(inRTTI); +} + +/// Cast inObject to DstType, asserts on failure +template || std::is_base_of_v, bool> = true> +inline const DstType *StaticCast(const SrcType *inObject) +{ + return static_cast(inObject); +} + +template || std::is_base_of_v, bool> = true> +inline DstType *StaticCast(SrcType *inObject) +{ + return static_cast(inObject); +} + +template || std::is_base_of_v, bool> = true> +inline const DstType *StaticCast(const RefConst &inObject) +{ + return static_cast(inObject.GetPtr()); +} + +template || std::is_base_of_v, bool> = true> +inline DstType *StaticCast(const Ref &inObject) +{ + return static_cast(inObject.GetPtr()); +} + +/// Cast inObject to DstType, returns nullptr on failure +template +inline const DstType *DynamicCast(const SrcType *inObject) +{ + return inObject != nullptr? reinterpret_cast(inObject->CastTo(JPH_RTTI(DstType))) : nullptr; +} + +template +inline DstType *DynamicCast(SrcType *inObject) +{ + return inObject != nullptr? const_cast(reinterpret_cast(inObject->CastTo(JPH_RTTI(DstType)))) : nullptr; +} + +template +inline const DstType *DynamicCast(const RefConst &inObject) +{ + return inObject != nullptr? reinterpret_cast(inObject->CastTo(JPH_RTTI(DstType))) : nullptr; +} + +template +inline DstType *DynamicCast(const Ref &inObject) +{ + return inObject != nullptr? const_cast(reinterpret_cast(inObject->CastTo(JPH_RTTI(DstType)))) : nullptr; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/Reference.h b/thirdparty/jolt_physics/Jolt/Core/Reference.h new file mode 100644 index 000000000000..8a2de696865d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/Reference.h @@ -0,0 +1,244 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +// Forward declares +template class Ref; +template class RefConst; + +/// Simple class to facilitate reference counting / releasing +/// Derive your class from RefTarget and you can reference it by using Ref or RefConst +/// +/// Reference counting classes keep an integer which indicates how many references +/// to the object are active. Reference counting objects are derived from RefTarget +/// and staT & their life with a reference count of zero. They can then be assigned +/// to equivalents of pointers (Ref) which will increase the reference count immediately. +/// If the destructor of Ref is called or another object is assigned to the reference +/// counting pointer it will decrease the reference count of the object again. If this +/// reference count becomes zero, the object is destroyed. +/// +/// This provides a very powerful mechanism to prevent memory leaks, but also gives +/// some responsibility to the programmer. The most notable point is that you cannot +/// have one object reference another and have the other reference the first one +/// back, because this way the reference count of both objects will never become +/// lower than 1, resulting in a memory leak. By carefully designing your classes +/// (and particularly identifying who owns who in the class hierarchy) you can avoid +/// these problems. +template +class RefTarget +{ +public: + /// Constructor + inline RefTarget() = default; + inline RefTarget(const RefTarget &) { /* Do not copy refcount */ } + inline ~RefTarget() { JPH_IF_ENABLE_ASSERTS(uint32 value = mRefCount.load(memory_order_relaxed);) JPH_ASSERT(value == 0 || value == cEmbedded); } ///< assert no one is referencing us + + /// Mark this class as embedded, this means the type can be used in a compound or constructed on the stack. + /// The Release function will never destruct the object, it is assumed the destructor will be called by whoever allocated + /// the object and at that point in time it is checked that no references are left to the structure. + inline void SetEmbedded() const { JPH_IF_ENABLE_ASSERTS(uint32 old = ) mRefCount.fetch_add(cEmbedded, memory_order_relaxed); JPH_ASSERT(old < cEmbedded); } + + /// Assignment operator + inline RefTarget & operator = (const RefTarget &) { /* Don't copy refcount */ return *this; } + + /// Get current refcount of this object + uint32 GetRefCount() const { return mRefCount.load(memory_order_relaxed); } + + /// Add or release a reference to this object + inline void AddRef() const + { + // Adding a reference can use relaxed memory ordering + mRefCount.fetch_add(1, memory_order_relaxed); + } + + inline void Release() const + { + #ifndef JPH_TSAN_ENABLED + // Releasing a reference must use release semantics... + if (mRefCount.fetch_sub(1, memory_order_release) == 1) + { + // ... so that we can use acquire to ensure that we see any updates from other threads that released a ref before deleting the object + atomic_thread_fence(memory_order_acquire); + delete static_cast(this); + } + #else + // But under TSAN, we cannot use atomic_thread_fence, so we use an acq_rel operation unconditionally instead + if (mRefCount.fetch_sub(1, memory_order_acq_rel) == 1) + delete static_cast(this); + #endif + } + + /// INTERNAL HELPER FUNCTION USED BY SERIALIZATION + static int sInternalGetRefCountOffset() { return offsetof(T, mRefCount); } + +protected: + static constexpr uint32 cEmbedded = 0x0ebedded; ///< A large value that gets added to the refcount to mark the object as embedded + + mutable atomic mRefCount = 0; ///< Current reference count +}; + +/// Pure virtual version of RefTarget +class JPH_EXPORT RefTargetVirtual +{ +public: + /// Virtual destructor + virtual ~RefTargetVirtual() = default; + + /// Virtual add reference + virtual void AddRef() = 0; + + /// Virtual release reference + virtual void Release() = 0; +}; + +/// Class for automatic referencing, this is the equivalent of a pointer to type T +/// if you assign a value to this class it will increment the reference count by one +/// of this object, and if you assign something else it will decrease the reference +/// count of the first object again. If it reaches a reference count of zero it will +/// be deleted +template +class Ref +{ +public: + /// Constructor + inline Ref() : mPtr(nullptr) { } + inline Ref(T *inRHS) : mPtr(inRHS) { AddRef(); } + inline Ref(const Ref &inRHS) : mPtr(inRHS.mPtr) { AddRef(); } + inline Ref(Ref &&inRHS) noexcept : mPtr(inRHS.mPtr) { inRHS.mPtr = nullptr; } + inline ~Ref() { Release(); } + + /// Assignment operators + inline Ref & operator = (T *inRHS) { if (mPtr != inRHS) { Release(); mPtr = inRHS; AddRef(); } return *this; } + inline Ref & operator = (const Ref &inRHS) { if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; AddRef(); } return *this; } + inline Ref & operator = (Ref &&inRHS) noexcept { if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; inRHS.mPtr = nullptr; } return *this; } + + /// Casting operators + inline operator T *() const { return mPtr; } + + /// Access like a normal pointer + inline T * operator -> () const { return mPtr; } + inline T & operator * () const { return *mPtr; } + + /// Comparison + inline bool operator == (const T * inRHS) const { return mPtr == inRHS; } + inline bool operator == (const Ref &inRHS) const { return mPtr == inRHS.mPtr; } + inline bool operator != (const T * inRHS) const { return mPtr != inRHS; } + inline bool operator != (const Ref &inRHS) const { return mPtr != inRHS.mPtr; } + + /// Get pointer + inline T * GetPtr() const { return mPtr; } + + /// Get hash for this object + uint64 GetHash() const + { + return Hash { } (mPtr); + } + + /// INTERNAL HELPER FUNCTION USED BY SERIALIZATION + void ** InternalGetPointer() { return reinterpret_cast(&mPtr); } + +private: + template friend class RefConst; + + /// Use "variable = nullptr;" to release an object, do not call these functions + inline void AddRef() { if (mPtr != nullptr) mPtr->AddRef(); } + inline void Release() { if (mPtr != nullptr) mPtr->Release(); } + + T * mPtr; ///< Pointer to object that we are reference counting +}; + +/// Class for automatic referencing, this is the equivalent of a CONST pointer to type T +/// if you assign a value to this class it will increment the reference count by one +/// of this object, and if you assign something else it will decrease the reference +/// count of the first object again. If it reaches a reference count of zero it will +/// be deleted +template +class RefConst +{ +public: + /// Constructor + inline RefConst() : mPtr(nullptr) { } + inline RefConst(const T * inRHS) : mPtr(inRHS) { AddRef(); } + inline RefConst(const RefConst &inRHS) : mPtr(inRHS.mPtr) { AddRef(); } + inline RefConst(RefConst &&inRHS) noexcept : mPtr(inRHS.mPtr) { inRHS.mPtr = nullptr; } + inline RefConst(const Ref &inRHS) : mPtr(inRHS.mPtr) { AddRef(); } + inline RefConst(Ref &&inRHS) noexcept : mPtr(inRHS.mPtr) { inRHS.mPtr = nullptr; } + inline ~RefConst() { Release(); } + + /// Assignment operators + inline RefConst & operator = (const T * inRHS) { if (mPtr != inRHS) { Release(); mPtr = inRHS; AddRef(); } return *this; } + inline RefConst & operator = (const RefConst &inRHS) { if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; AddRef(); } return *this; } + inline RefConst & operator = (RefConst &&inRHS) noexcept { if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; inRHS.mPtr = nullptr; } return *this; } + inline RefConst & operator = (const Ref &inRHS) { if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; AddRef(); } return *this; } + inline RefConst & operator = (Ref &&inRHS) noexcept { if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; inRHS.mPtr = nullptr; } return *this; } + + /// Casting operators + inline operator const T * () const { return mPtr; } + + /// Access like a normal pointer + inline const T * operator -> () const { return mPtr; } + inline const T & operator * () const { return *mPtr; } + + /// Comparison + inline bool operator == (const T * inRHS) const { return mPtr == inRHS; } + inline bool operator == (const RefConst &inRHS) const { return mPtr == inRHS.mPtr; } + inline bool operator == (const Ref &inRHS) const { return mPtr == inRHS.mPtr; } + inline bool operator != (const T * inRHS) const { return mPtr != inRHS; } + inline bool operator != (const RefConst &inRHS) const { return mPtr != inRHS.mPtr; } + inline bool operator != (const Ref &inRHS) const { return mPtr != inRHS.mPtr; } + + /// Get pointer + inline const T * GetPtr() const { return mPtr; } + + /// Get hash for this object + uint64 GetHash() const + { + return Hash { } (mPtr); + } + + /// INTERNAL HELPER FUNCTION USED BY SERIALIZATION + void ** InternalGetPointer() { return const_cast(reinterpret_cast(&mPtr)); } + +private: + /// Use "variable = nullptr;" to release an object, do not call these functions + inline void AddRef() { if (mPtr != nullptr) mPtr->AddRef(); } + inline void Release() { if (mPtr != nullptr) mPtr->Release(); } + + const T * mPtr; ///< Pointer to object that we are reference counting +}; + +JPH_NAMESPACE_END + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat") + +namespace std +{ + /// Declare std::hash for Ref + template + struct hash> + { + size_t operator () (const JPH::Ref &inRHS) const + { + return size_t(inRHS.GetHash()); + } + }; + + /// Declare std::hash for RefConst + template + struct hash> + { + size_t operator () (const JPH::RefConst &inRHS) const + { + return size_t(inRHS.GetHash()); + } + }; +} + +JPH_SUPPRESS_WARNING_POP diff --git a/thirdparty/jolt_physics/Jolt/Core/Result.h b/thirdparty/jolt_physics/Jolt/Core/Result.h new file mode 100644 index 000000000000..ad2eab164bdf --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/Result.h @@ -0,0 +1,174 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Helper class that either contains a valid result or an error +template +class Result +{ +public: + /// Default constructor + Result() { } + + /// Copy constructor + Result(const Result &inRHS) : + mState(inRHS.mState) + { + switch (inRHS.mState) + { + case EState::Valid: + ::new (&mResult) Type (inRHS.mResult); + break; + + case EState::Error: + ::new (&mError) String(inRHS.mError); + break; + + case EState::Invalid: + break; + } + } + + /// Move constructor + Result(Result &&inRHS) noexcept : + mState(inRHS.mState) + { + switch (inRHS.mState) + { + case EState::Valid: + ::new (&mResult) Type (std::move(inRHS.mResult)); + break; + + case EState::Error: + ::new (&mError) String(std::move(inRHS.mError)); + break; + + case EState::Invalid: + break; + } + + // Don't reset the state of inRHS, the destructors still need to be called after a move operation + } + + /// Destructor + ~Result() { Clear(); } + + /// Copy assignment + Result & operator = (const Result &inRHS) + { + Clear(); + + mState = inRHS.mState; + + switch (inRHS.mState) + { + case EState::Valid: + ::new (&mResult) Type (inRHS.mResult); + break; + + case EState::Error: + ::new (&mError) String(inRHS.mError); + break; + + case EState::Invalid: + break; + } + + return *this; + } + + /// Move assignment + Result & operator = (Result &&inRHS) noexcept + { + Clear(); + + mState = inRHS.mState; + + switch (inRHS.mState) + { + case EState::Valid: + ::new (&mResult) Type (std::move(inRHS.mResult)); + break; + + case EState::Error: + ::new (&mError) String(std::move(inRHS.mError)); + break; + + case EState::Invalid: + break; + } + + // Don't reset the state of inRHS, the destructors still need to be called after a move operation + + return *this; + } + + /// Clear result or error + void Clear() + { + switch (mState) + { + case EState::Valid: + mResult.~Type(); + break; + + case EState::Error: + mError.~String(); + break; + + case EState::Invalid: + break; + } + + mState = EState::Invalid; + } + + /// Checks if the result is still uninitialized + bool IsEmpty() const { return mState == EState::Invalid; } + + /// Checks if the result is valid + bool IsValid() const { return mState == EState::Valid; } + + /// Get the result value + const Type & Get() const { JPH_ASSERT(IsValid()); return mResult; } + + /// Set the result value + void Set(const Type &inResult) { Clear(); ::new (&mResult) Type(inResult); mState = EState::Valid; } + + /// Set the result value (move value) + void Set(Type &&inResult) { Clear(); ::new (&mResult) Type(std::move(inResult)); mState = EState::Valid; } + + /// Check if we had an error + bool HasError() const { return mState == EState::Error; } + + /// Get the error value + const String & GetError() const { JPH_ASSERT(HasError()); return mError; } + + /// Set an error value + void SetError(const char *inError) { Clear(); ::new (&mError) String(inError); mState = EState::Error; } + void SetError(const string_view &inError) { Clear(); ::new (&mError) String(inError); mState = EState::Error; } + void SetError(String &&inError) { Clear(); ::new (&mError) String(std::move(inError)); mState = EState::Error; } + +private: + union + { + Type mResult; ///< The actual result object + String mError; ///< The error description if the result failed + }; + + /// State of the result + enum class EState : uint8 + { + Invalid, + Valid, + Error + }; + + EState mState = EState::Invalid; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/STLAlignedAllocator.h b/thirdparty/jolt_physics/Jolt/Core/STLAlignedAllocator.h new file mode 100644 index 000000000000..60e3ad4873dc --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/STLAlignedAllocator.h @@ -0,0 +1,72 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// STL allocator that takes care that memory is aligned to N bytes +template +class STLAlignedAllocator +{ +public: + using value_type = T; + + /// Pointer to type + using pointer = T *; + using const_pointer = const T *; + + /// Reference to type. + /// Can be removed in C++20. + using reference = T &; + using const_reference = const T &; + + using size_type = size_t; + using difference_type = ptrdiff_t; + + /// The allocator is stateless + using is_always_equal = std::true_type; + + /// Allocator supports moving + using propagate_on_container_move_assignment = std::true_type; + + /// Constructor + inline STLAlignedAllocator() = default; + + /// Constructor from other allocator + template + inline explicit STLAlignedAllocator(const STLAlignedAllocator &) { } + + /// Allocate memory + inline pointer allocate(size_type inN) + { + return (pointer)AlignedAllocate(inN * sizeof(value_type), N); + } + + /// Free memory + inline void deallocate(pointer inPointer, size_type) + { + AlignedFree(inPointer); + } + + /// Allocators are stateless so assumed to be equal + inline bool operator == (const STLAlignedAllocator &) const + { + return true; + } + + inline bool operator != (const STLAlignedAllocator &) const + { + return false; + } + + /// Converting to allocator for other type + template + struct rebind + { + using other = STLAlignedAllocator; + }; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/STLAllocator.h b/thirdparty/jolt_physics/Jolt/Core/STLAllocator.h new file mode 100644 index 000000000000..f9573279ad61 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/STLAllocator.h @@ -0,0 +1,127 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Default implementation of AllocatorHasReallocate which tells if an allocator has a reallocate function +template struct AllocatorHasReallocate { static constexpr bool sValue = false; }; + +#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR + +/// STL allocator that forwards to our allocation functions +template +class STLAllocator +{ +public: + using value_type = T; + + /// Pointer to type + using pointer = T *; + using const_pointer = const T *; + + /// Reference to type. + /// Can be removed in C++20. + using reference = T &; + using const_reference = const T &; + + using size_type = size_t; + using difference_type = ptrdiff_t; + + /// The allocator is stateless + using is_always_equal = std::true_type; + + /// Allocator supports moving + using propagate_on_container_move_assignment = std::true_type; + + /// Constructor + inline STLAllocator() = default; + + /// Constructor from other allocator + template + inline STLAllocator(const STLAllocator &) { } + + /// If this allocator needs to fall back to aligned allocations because the type requires it + static constexpr bool needs_aligned_allocate = alignof(T) > (JPH_CPU_ADDRESS_BITS == 32? 8 : 16); + + /// Allocate memory + inline pointer allocate(size_type inN) + { + if constexpr (needs_aligned_allocate) + return pointer(AlignedAllocate(inN * sizeof(value_type), alignof(T))); + else + return pointer(Allocate(inN * sizeof(value_type))); + } + + /// Should we expose a reallocate function? + static constexpr bool has_reallocate = std::is_trivially_copyable() && !needs_aligned_allocate; + + /// Reallocate memory + template > + inline pointer reallocate(pointer inOldPointer, size_type inOldSize, size_type inNewSize) + { + JPH_ASSERT(inNewSize > 0); // Reallocating to zero size is implementation dependent, so we don't allow it + return pointer(Reallocate(inOldPointer, inOldSize * sizeof(value_type), inNewSize * sizeof(value_type))); + } + + /// Free memory + inline void deallocate(pointer inPointer, size_type) + { + if constexpr (needs_aligned_allocate) + AlignedFree(inPointer); + else + Free(inPointer); + } + + /// Allocators are stateless so assumed to be equal + inline bool operator == (const STLAllocator &) const + { + return true; + } + + inline bool operator != (const STLAllocator &) const + { + return false; + } + + /// Converting to allocator for other type + template + struct rebind + { + using other = STLAllocator; + }; +}; + +/// The STLAllocator implements the reallocate function if the alignment of the class is smaller or equal to the default alignment for the platform +template struct AllocatorHasReallocate> { static constexpr bool sValue = STLAllocator::has_reallocate; }; + +#else + +template using STLAllocator = std::allocator; + +#endif // !JPH_DISABLE_CUSTOM_ALLOCATOR + +// Declare STL containers that use our allocator +using String = std::basic_string, STLAllocator>; +using IStringStream = std::basic_istringstream, STLAllocator>; + +JPH_NAMESPACE_END + +#if (!defined(JPH_PLATFORM_WINDOWS) || defined(JPH_COMPILER_MINGW)) && !defined(JPH_DISABLE_CUSTOM_ALLOCATOR) + +namespace std +{ + /// Declare std::hash for String, for some reason on Linux based platforms template deduction takes the wrong variant + template <> + struct hash + { + inline size_t operator () (const JPH::String &inRHS) const + { + return hash { } (inRHS); + } + }; +} + +#endif // (!JPH_PLATFORM_WINDOWS || JPH_COMPILER_MINGW) && !JPH_DISABLE_CUSTOM_ALLOCATOR diff --git a/thirdparty/jolt_physics/Jolt/Core/STLTempAllocator.h b/thirdparty/jolt_physics/Jolt/Core/STLTempAllocator.h new file mode 100644 index 000000000000..cf7c39d45550 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/STLTempAllocator.h @@ -0,0 +1,80 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// STL allocator that wraps around TempAllocator +template +class STLTempAllocator +{ +public: + using value_type = T; + + /// Pointer to type + using pointer = T *; + using const_pointer = const T *; + + /// Reference to type. + /// Can be removed in C++20. + using reference = T &; + using const_reference = const T &; + + using size_type = size_t; + using difference_type = ptrdiff_t; + + /// The allocator is not stateless (depends on the temp allocator) + using is_always_equal = std::false_type; + + /// Constructor + inline STLTempAllocator(TempAllocator &inAllocator) : mAllocator(inAllocator) { } + + /// Constructor from other allocator + template + inline explicit STLTempAllocator(const STLTempAllocator &inRHS) : mAllocator(inRHS.GetAllocator()) { } + + /// Allocate memory + inline pointer allocate(size_type inN) + { + return pointer(mAllocator.Allocate(uint(inN * sizeof(value_type)))); + } + + /// Free memory + inline void deallocate(pointer inPointer, size_type inN) + { + mAllocator.Free(inPointer, uint(inN * sizeof(value_type))); + } + + /// Allocators are not-stateless, assume if allocator address matches that the allocators are the same + inline bool operator == (const STLTempAllocator &inRHS) const + { + return &mAllocator == &inRHS.mAllocator; + } + + inline bool operator != (const STLTempAllocator &inRHS) const + { + return &mAllocator != &inRHS.mAllocator; + } + + /// Converting to allocator for other type + template + struct rebind + { + using other = STLTempAllocator; + }; + + /// Get our temp allocator + TempAllocator & GetAllocator() const + { + return mAllocator; + } + +private: + TempAllocator & mAllocator; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/ScopeExit.h b/thirdparty/jolt_physics/Jolt/Core/ScopeExit.h new file mode 100644 index 000000000000..613838421319 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/ScopeExit.h @@ -0,0 +1,49 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that calls a function when it goes out of scope +template +class ScopeExit : public NonCopyable +{ +public: + /// Constructor specifies the exit function + JPH_INLINE explicit ScopeExit(F &&inFunction) : mFunction(std::move(inFunction)) { } + + /// Destructor calls the exit function + JPH_INLINE ~ScopeExit() { if (!mInvoked) mFunction(); } + + /// Call the exit function now instead of when going out of scope + JPH_INLINE void Invoke() + { + if (!mInvoked) + { + mFunction(); + mInvoked = true; + } + } + + /// No longer call the exit function when going out of scope + JPH_INLINE void Release() + { + mInvoked = true; + } + +private: + F mFunction; + bool mInvoked = false; +}; + +#define JPH_SCOPE_EXIT_TAG2(line) scope_exit##line +#define JPH_SCOPE_EXIT_TAG(line) JPH_SCOPE_EXIT_TAG2(line) + +/// Usage: JPH_SCOPE_EXIT([]{ code to call on scope exit }); +#define JPH_SCOPE_EXIT(...) ScopeExit JPH_SCOPE_EXIT_TAG(__LINE__)(__VA_ARGS__) + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/Semaphore.cpp b/thirdparty/jolt_physics/Jolt/Core/Semaphore.cpp new file mode 100644 index 000000000000..27bb591bce52 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/Semaphore.cpp @@ -0,0 +1,82 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +#ifdef JPH_PLATFORM_WINDOWS + JPH_SUPPRESS_WARNING_PUSH + JPH_MSVC_SUPPRESS_WARNING(5039) // winbase.h(13179): warning C5039: 'TpSetCallbackCleanupGroup': pointer or reference to potentially throwing function passed to 'extern "C"' function under -EHc. Undefined behavior may occur if this function throws an exception. + #ifndef WIN32_LEAN_AND_MEAN + #define WIN32_LEAN_AND_MEAN + #endif +#ifndef JPH_COMPILER_MINGW + #include +#else + #include +#endif + + JPH_SUPPRESS_WARNING_POP +#endif + +JPH_NAMESPACE_BEGIN + +Semaphore::Semaphore() +{ +#ifdef JPH_PLATFORM_WINDOWS + mSemaphore = CreateSemaphore(nullptr, 0, INT_MAX, nullptr); +#endif +} + +Semaphore::~Semaphore() +{ +#ifdef JPH_PLATFORM_WINDOWS + CloseHandle(mSemaphore); +#endif +} + +void Semaphore::Release(uint inNumber) +{ + JPH_ASSERT(inNumber > 0); + +#ifdef JPH_PLATFORM_WINDOWS + int old_value = mCount.fetch_add(inNumber); + if (old_value < 0) + { + int new_value = old_value + (int)inNumber; + int num_to_release = min(new_value, 0) - old_value; + ::ReleaseSemaphore(mSemaphore, num_to_release, nullptr); + } +#else + std::lock_guard lock(mLock); + mCount.fetch_add(inNumber, std::memory_order_relaxed); + if (inNumber > 1) + mWaitVariable.notify_all(); + else + mWaitVariable.notify_one(); +#endif +} + +void Semaphore::Acquire(uint inNumber) +{ + JPH_ASSERT(inNumber > 0); + +#ifdef JPH_PLATFORM_WINDOWS + int old_value = mCount.fetch_sub(inNumber); + int new_value = old_value - (int)inNumber; + if (new_value < 0) + { + int num_to_acquire = min(old_value, 0) - new_value; + for (int i = 0; i < num_to_acquire; ++i) + WaitForSingleObject(mSemaphore, INFINITE); + } +#else + std::unique_lock lock(mLock); + mCount.fetch_sub(inNumber, std::memory_order_relaxed); + mWaitVariable.wait(lock, [this]() { return mCount >= 0; }); +#endif +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/Semaphore.h b/thirdparty/jolt_physics/Jolt/Core/Semaphore.h new file mode 100644 index 000000000000..091d7d5b0aec --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/Semaphore.h @@ -0,0 +1,51 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +#include +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +// Things we're using from STL +using std::atomic; +using std::mutex; +using std::condition_variable; + +/// Implements a semaphore +/// When we switch to C++20 we can use counting_semaphore to unify this +class JPH_EXPORT Semaphore +{ +public: + /// Constructor + Semaphore(); + ~Semaphore(); + + /// Release the semaphore, signaling the thread waiting on the barrier that there may be work + void Release(uint inNumber = 1); + + /// Acquire the semaphore inNumber times + void Acquire(uint inNumber = 1); + + /// Get the current value of the semaphore + inline int GetValue() const { return mCount.load(std::memory_order_relaxed); } + +private: +#ifdef JPH_PLATFORM_WINDOWS + // On windows we use a semaphore object since it is more efficient than a lock and a condition variable + alignas(JPH_CACHE_LINE_SIZE) atomic mCount { 0 }; ///< We increment mCount for every release, to acquire we decrement the count. If the count is negative we know that we are waiting on the actual semaphore. + void * mSemaphore; ///< The semaphore is an expensive construct so we only acquire/release it if we know that we need to wait/have waiting threads +#else + // Other platforms: Emulate a semaphore using a mutex, condition variable and count + mutex mLock; + condition_variable mWaitVariable; + atomic mCount { 0 }; +#endif +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/StaticArray.h b/thirdparty/jolt_physics/Jolt/Core/StaticArray.h new file mode 100644 index 000000000000..fddaaa37a2c2 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/StaticArray.h @@ -0,0 +1,329 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Simple variable length array backed by a fixed size buffer +template +class [[nodiscard]] StaticArray +{ +public: + using value_type = T; + + using size_type = uint; + + static constexpr uint Capacity = N; + + /// Default constructor + StaticArray() = default; + + /// Constructor from initializer list + explicit StaticArray(std::initializer_list inList) + { + JPH_ASSERT(inList.size() <= N); + for (const T &v : inList) + ::new (reinterpret_cast(&mElements[mSize++])) T(v); + } + + /// Copy constructor + StaticArray(const StaticArray &inRHS) + { + while (mSize < inRHS.mSize) + { + ::new (&mElements[mSize]) T(inRHS[mSize]); + ++mSize; + } + } + + /// Destruct all elements + ~StaticArray() + { + if constexpr (!std::is_trivially_destructible()) + for (T *e = reinterpret_cast(mElements), *end = e + mSize; e < end; ++e) + e->~T(); + } + + /// Destruct all elements and set length to zero + void clear() + { + if constexpr (!std::is_trivially_destructible()) + for (T *e = reinterpret_cast(mElements), *end = e + mSize; e < end; ++e) + e->~T(); + mSize = 0; + } + + /// Add element to the back of the array + void push_back(const T &inElement) + { + JPH_ASSERT(mSize < N); + ::new (&mElements[mSize++]) T(inElement); + } + + /// Construct element at the back of the array + template + void emplace_back(A &&... inElement) + { + JPH_ASSERT(mSize < N); + ::new (&mElements[mSize++]) T(std::forward(inElement)...); + } + + /// Remove element from the back of the array + void pop_back() + { + JPH_ASSERT(mSize > 0); + reinterpret_cast(mElements[--mSize]).~T(); + } + + /// Returns true if there are no elements in the array + bool empty() const + { + return mSize == 0; + } + + /// Returns amount of elements in the array + size_type size() const + { + return mSize; + } + + /// Returns maximum amount of elements the array can hold + size_type capacity() const + { + return N; + } + + /// Resize array to new length + void resize(size_type inNewSize) + { + JPH_ASSERT(inNewSize <= N); + if constexpr (!std::is_trivially_constructible()) + for (T *element = reinterpret_cast(mElements) + mSize, *element_end = reinterpret_cast(mElements) + inNewSize; element < element_end; ++element) + ::new (element) T; + if constexpr (!std::is_trivially_destructible()) + for (T *element = reinterpret_cast(mElements) + inNewSize, *element_end = reinterpret_cast(mElements) + mSize; element < element_end; ++element) + element->~T(); + mSize = inNewSize; + } + + using const_iterator = const T *; + + /// Iterators + const_iterator begin() const + { + return reinterpret_cast(mElements); + } + + const_iterator end() const + { + return reinterpret_cast(mElements + mSize); + } + + using iterator = T *; + + iterator begin() + { + return reinterpret_cast(mElements); + } + + iterator end() + { + return reinterpret_cast(mElements + mSize); + } + + const T * data() const + { + return reinterpret_cast(mElements); + } + + T * data() + { + return reinterpret_cast(mElements); + } + + /// Access element + T & operator [] (size_type inIdx) + { + JPH_ASSERT(inIdx < mSize); + return reinterpret_cast(mElements[inIdx]); + } + + const T & operator [] (size_type inIdx) const + { + JPH_ASSERT(inIdx < mSize); + return reinterpret_cast(mElements[inIdx]); + } + + /// Access element + T & at(size_type inIdx) + { + JPH_ASSERT(inIdx < mSize); + return reinterpret_cast(mElements[inIdx]); + } + + const T & at(size_type inIdx) const + { + JPH_ASSERT(inIdx < mSize); + return reinterpret_cast(mElements[inIdx]); + } + + /// First element in the array + const T & front() const + { + JPH_ASSERT(mSize > 0); + return reinterpret_cast(mElements[0]); + } + + T & front() + { + JPH_ASSERT(mSize > 0); + return reinterpret_cast(mElements[0]); + } + + /// Last element in the array + const T & back() const + { + JPH_ASSERT(mSize > 0); + return reinterpret_cast(mElements[mSize - 1]); + } + + T & back() + { + JPH_ASSERT(mSize > 0); + return reinterpret_cast(mElements[mSize - 1]); + } + + /// Remove one element from the array + void erase(const_iterator inIter) + { + size_type p = size_type(inIter - begin()); + JPH_ASSERT(p < mSize); + reinterpret_cast(mElements[p]).~T(); + if (p + 1 < mSize) + memmove(mElements + p, mElements + p + 1, (mSize - p - 1) * sizeof(T)); + --mSize; + } + + /// Remove multiple element from the array + void erase(const_iterator inBegin, const_iterator inEnd) + { + size_type p = size_type(inBegin - begin()); + size_type n = size_type(inEnd - inBegin); + JPH_ASSERT(inEnd <= end()); + for (size_type i = 0; i < n; ++i) + reinterpret_cast(mElements[p + i]).~T(); + if (p + n < mSize) + memmove(mElements + p, mElements + p + n, (mSize - p - n) * sizeof(T)); + mSize -= n; + } + + /// Assignment operator + StaticArray & operator = (const StaticArray &inRHS) + { + size_type rhs_size = inRHS.size(); + + if (static_cast(this) != static_cast(&inRHS)) + { + clear(); + + while (mSize < rhs_size) + { + ::new (&mElements[mSize]) T(inRHS[mSize]); + ++mSize; + } + } + + return *this; + } + + /// Assignment operator with static array of different max length + template + StaticArray & operator = (const StaticArray &inRHS) + { + size_type rhs_size = inRHS.size(); + JPH_ASSERT(rhs_size <= N); + + if (static_cast(this) != static_cast(&inRHS)) + { + clear(); + + while (mSize < rhs_size) + { + ::new (&mElements[mSize]) T(inRHS[mSize]); + ++mSize; + } + } + + return *this; + } + + /// Comparing arrays + bool operator == (const StaticArray &inRHS) const + { + if (mSize != inRHS.mSize) + return false; + for (size_type i = 0; i < mSize; ++i) + if (!(reinterpret_cast(mElements[i]) == reinterpret_cast(inRHS.mElements[i]))) + return false; + return true; + } + + bool operator != (const StaticArray &inRHS) const + { + if (mSize != inRHS.mSize) + return true; + for (size_type i = 0; i < mSize; ++i) + if (reinterpret_cast(mElements[i]) != reinterpret_cast(inRHS.mElements[i])) + return true; + return false; + } + + /// Get hash for this array + uint64 GetHash() const + { + // Hash length first + uint64 ret = Hash { } (uint32(size())); + + // Then hash elements + for (const T *element = reinterpret_cast(mElements), *element_end = reinterpret_cast(mElements) + mSize; element < element_end; ++element) + HashCombine(ret, *element); + + return ret; + } + +protected: + struct alignas(T) Storage + { + uint8 mData[sizeof(T)]; + }; + + static_assert(sizeof(T) == sizeof(Storage), "Mismatch in size"); + static_assert(alignof(T) == alignof(Storage), "Mismatch in alignment"); + + size_type mSize = 0; + Storage mElements[N]; +}; + +JPH_NAMESPACE_END + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat") + +namespace std +{ + /// Declare std::hash for StaticArray + template + struct hash> + { + size_t operator () (const JPH::StaticArray &inRHS) const + { + return std::size_t(inRHS.GetHash()); + } + }; +} + +JPH_SUPPRESS_WARNING_POP diff --git a/thirdparty/jolt_physics/Jolt/Core/StreamIn.h b/thirdparty/jolt_physics/Jolt/Core/StreamIn.h new file mode 100644 index 000000000000..4de299e6d1cf --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/StreamIn.h @@ -0,0 +1,119 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Simple binary input stream +class JPH_EXPORT StreamIn : public NonCopyable +{ +public: + /// Virtual destructor + virtual ~StreamIn() = default; + + /// Read a string of bytes from the binary stream + virtual void ReadBytes(void *outData, size_t inNumBytes) = 0; + + /// Returns true when an attempt has been made to read past the end of the file + virtual bool IsEOF() const = 0; + + /// Returns true if there was an IO failure + virtual bool IsFailed() const = 0; + + /// Read a primitive (e.g. float, int, etc.) from the binary stream + template , bool> = true> + void Read(T &outT) + { + ReadBytes(&outT, sizeof(outT)); + } + + /// Read a vector of primitives from the binary stream + template , bool> = true> + void Read(Array &outT) + { + uint32 len = uint32(outT.size()); // Initialize to previous array size, this is used for validation in the StateRecorder class + Read(len); + if (!IsEOF() && !IsFailed()) + { + outT.resize(len); + if constexpr (std::is_same_v || std::is_same_v || std::is_same_v) + { + // These types have unused components that we don't want to read + for (typename Array::size_type i = 0; i < len; ++i) + Read(outT[i]); + } + else + { + // Read all elements at once + ReadBytes(outT.data(), len * sizeof(T)); + } + } + else + outT.clear(); + } + + /// Read a string from the binary stream (reads the number of characters and then the characters) + template + void Read(std::basic_string &outString) + { + uint32 len = 0; + Read(len); + if (!IsEOF() && !IsFailed()) + { + outString.resize(len); + ReadBytes(outString.data(), len * sizeof(Type)); + } + else + outString.clear(); + } + + /// Read a vector of primitives from the binary stream using a custom function to read the elements + template + void Read(Array &outT, const F &inReadElement) + { + uint32 len = uint32(outT.size()); // Initialize to previous array size, this is used for validation in the StateRecorder class + Read(len); + if (!IsEOF() && !IsFailed()) + { + outT.resize(len); + for (typename Array::size_type i = 0; i < len; ++i) + inReadElement(*this, outT[i]); + } + else + outT.clear(); + } + + /// Read a Vec3 (don't read W) + void Read(Vec3 &outVec) + { + ReadBytes(&outVec, 3 * sizeof(float)); + outVec = Vec3::sFixW(outVec.mValue); + } + + /// Read a DVec3 (don't read W) + void Read(DVec3 &outVec) + { + ReadBytes(&outVec, 3 * sizeof(double)); + outVec = DVec3::sFixW(outVec.mValue); + } + + /// Read a DMat44 (don't read W component of translation) + void Read(DMat44 &outVec) + { + Vec4 x, y, z; + Read(x); + Read(y); + Read(z); + + DVec3 t; + Read(t); + + outVec = DMat44(x, y, z, t); + } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/StreamOut.h b/thirdparty/jolt_physics/Jolt/Core/StreamOut.h new file mode 100644 index 000000000000..566f8ec8da32 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/StreamOut.h @@ -0,0 +1,97 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Simple binary output stream +class JPH_EXPORT StreamOut : public NonCopyable +{ +public: + /// Virtual destructor + virtual ~StreamOut() = default; + + /// Write a string of bytes to the binary stream + virtual void WriteBytes(const void *inData, size_t inNumBytes) = 0; + + /// Returns true if there was an IO failure + virtual bool IsFailed() const = 0; + + /// Write a primitive (e.g. float, int, etc.) to the binary stream + template , bool> = true> + void Write(const T &inT) + { + WriteBytes(&inT, sizeof(inT)); + } + + /// Write a vector of primitives to the binary stream + template , bool> = true> + void Write(const Array &inT) + { + uint32 len = uint32(inT.size()); + Write(len); + if (!IsFailed()) + { + if constexpr (std::is_same_v || std::is_same_v || std::is_same_v) + { + // These types have unused components that we don't want to write + for (typename Array::size_type i = 0; i < len; ++i) + Write(inT[i]); + } + else + { + // Write all elements at once + WriteBytes(inT.data(), len * sizeof(T)); + } + } + } + + /// Write a string to the binary stream (writes the number of characters and then the characters) + template + void Write(const std::basic_string &inString) + { + uint32 len = uint32(inString.size()); + Write(len); + if (!IsFailed()) + WriteBytes(inString.data(), len * sizeof(Type)); + } + + /// Write a vector of primitives to the binary stream using a custom write function + template + void Write(const Array &inT, const F &inWriteElement) + { + uint32 len = uint32(inT.size()); + Write(len); + if (!IsFailed()) + for (typename Array::size_type i = 0; i < len; ++i) + inWriteElement(inT[i], *this); + } + + /// Write a Vec3 (don't write W) + void Write(const Vec3 &inVec) + { + WriteBytes(&inVec, 3 * sizeof(float)); + } + + /// Write a DVec3 (don't write W) + void Write(const DVec3 &inVec) + { + WriteBytes(&inVec, 3 * sizeof(double)); + } + + /// Write a DMat44 (don't write W component of translation) + void Write(const DMat44 &inVec) + { + Write(inVec.GetColumn4(0)); + Write(inVec.GetColumn4(1)); + Write(inVec.GetColumn4(2)); + + Write(inVec.GetTranslation()); + } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/StreamUtils.h b/thirdparty/jolt_physics/Jolt/Core/StreamUtils.h new file mode 100644 index 000000000000..1feb232f350c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/StreamUtils.h @@ -0,0 +1,168 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +namespace StreamUtils { + +template +using ObjectToIDMap = UnorderedMap; + +template +using IDToObjectMap = Array>; + +// Restore a single object by reading the hash of the type, constructing it and then calling the restore function +template +Result> RestoreObject(StreamIn &inStream, void (Type::*inRestoreBinaryStateFunction)(StreamIn &)) +{ + Result> result; + + // Read the hash of the type + uint32 hash; + inStream.Read(hash); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to read type hash"); + return result; + } + + // Get the RTTI for the type + const RTTI *rtti = Factory::sInstance->Find(hash); + if (rtti == nullptr) + { + result.SetError("Failed to create instance of type"); + return result; + } + + // Construct and read the data of the type + Ref object = reinterpret_cast(rtti->CreateObject()); + (object->*inRestoreBinaryStateFunction)(inStream); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to restore object"); + return result; + } + + result.Set(object); + return result; +} + +/// Save an object reference to a stream. Uses a map to map objects to IDs which is also used to prevent writing duplicates. +template +void SaveObjectReference(StreamOut &inStream, const Type *inObject, ObjectToIDMap *ioObjectToIDMap) +{ + if (ioObjectToIDMap == nullptr || inObject == nullptr) + { + // Write null ID + inStream.Write(~uint32(0)); + } + else + { + typename ObjectToIDMap::const_iterator id = ioObjectToIDMap->find(inObject); + if (id != ioObjectToIDMap->end()) + { + // Existing object, write ID + inStream.Write(id->second); + } + else + { + // New object, write the ID + uint32 new_id = uint32(ioObjectToIDMap->size()); + (*ioObjectToIDMap)[inObject] = new_id; + inStream.Write(new_id); + + // Write the object + inObject->SaveBinaryState(inStream); + } + } +} + +/// Restore an object reference from stream. +template +Result> RestoreObjectReference(StreamIn &inStream, IDToObjectMap &ioIDToObjectMap) +{ + Result> result; + + // Read id + uint32 id = ~uint32(0); + inStream.Read(id); + + // Check null + if (id == ~uint32(0)) + { + result.Set(nullptr); + return result; + } + + // Check if it already exists + if (id >= ioIDToObjectMap.size()) + { + // New object, restore it + result = Type::sRestoreFromBinaryState(inStream); + if (result.HasError()) + return result; + JPH_ASSERT(id == ioIDToObjectMap.size()); + ioIDToObjectMap.push_back(result.Get()); + } + else + { + // Existing object filter + result.Set(ioIDToObjectMap[id].GetPtr()); + } + + return result; +} + +// Save an array of objects to a stream. +template +void SaveObjectArray(StreamOut &inStream, const ArrayType &inArray, ObjectToIDMap *ioObjectToIDMap) +{ + uint32 len = uint32(inArray.size()); + inStream.Write(len); + for (const ValueType *value: inArray) + SaveObjectReference(inStream, value, ioObjectToIDMap); +} + +// Restore an array of objects from a stream. +template +Result RestoreObjectArray(StreamIn &inStream, IDToObjectMap &ioIDToObjectMap) +{ + Result result; + + uint32 len; + inStream.Read(len); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to read stream"); + return result; + } + + ArrayType values; + values.reserve(len); + for (size_t i = 0; i < len; ++i) + { + Result value = RestoreObjectReference(inStream, ioIDToObjectMap); + if (value.HasError()) + { + result.SetError(value.GetError()); + return result; + } + values.push_back(std::move(value.Get())); + } + + result.Set(values); + return result; +} + +} // StreamUtils + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/StreamWrapper.h b/thirdparty/jolt_physics/Jolt/Core/StreamWrapper.h new file mode 100644 index 000000000000..66a36b0571ec --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/StreamWrapper.h @@ -0,0 +1,53 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +/// Wrapper around std::ostream +class StreamOutWrapper : public StreamOut +{ +public: + /// Constructor + StreamOutWrapper(ostream &ioWrapped) : mWrapped(ioWrapped) { } + + /// Write a string of bytes to the binary stream + virtual void WriteBytes(const void *inData, size_t inNumBytes) override { mWrapped.write((const char *)inData, inNumBytes); } + + /// Returns true if there was an IO failure + virtual bool IsFailed() const override { return mWrapped.fail(); } + +private: + ostream & mWrapped; +}; + +/// Wrapper around std::istream +class StreamInWrapper : public StreamIn +{ +public: + /// Constructor + StreamInWrapper(istream &ioWrapped) : mWrapped(ioWrapped) { } + + /// Write a string of bytes to the binary stream + virtual void ReadBytes(void *outData, size_t inNumBytes) override { mWrapped.read((char *)outData, inNumBytes); } + + /// Returns true when an attempt has been made to read past the end of the file + virtual bool IsEOF() const override { return mWrapped.eof(); } + + /// Returns true if there was an IO failure + virtual bool IsFailed() const override { return mWrapped.fail(); } + +private: + istream & mWrapped; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/StridedPtr.h b/thirdparty/jolt_physics/Jolt/Core/StridedPtr.h new file mode 100644 index 000000000000..1cf97fa67831 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/StridedPtr.h @@ -0,0 +1,63 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// A strided pointer behaves exactly like a normal pointer except that the +/// elements that the pointer points to can be part of a larger structure. +/// The stride gives the number of bytes from one element to the next. +template +class JPH_EXPORT StridedPtr +{ +public: + using value_type = T; + + /// Constructors + StridedPtr() = default; + StridedPtr(const StridedPtr &inRHS) = default; + StridedPtr(T *inPtr, int inStride = sizeof(T)) : mPtr(const_cast(reinterpret_cast(inPtr))), mStride(inStride) { } + + /// Assignment + inline StridedPtr & operator = (const StridedPtr &inRHS) = default; + + /// Incrementing / decrementing + inline StridedPtr & operator ++ () { mPtr += mStride; return *this; } + inline StridedPtr & operator -- () { mPtr -= mStride; return *this; } + inline StridedPtr operator ++ (int) { StridedPtr old_ptr(*this); mPtr += mStride; return old_ptr; } + inline StridedPtr operator -- (int) { StridedPtr old_ptr(*this); mPtr -= mStride; return old_ptr; } + inline StridedPtr operator + (int inOffset) const { StridedPtr new_ptr(*this); new_ptr.mPtr += inOffset * mStride; return new_ptr; } + inline StridedPtr operator - (int inOffset) const { StridedPtr new_ptr(*this); new_ptr.mPtr -= inOffset * mStride; return new_ptr; } + inline void operator += (int inOffset) { mPtr += inOffset * mStride; } + inline void operator -= (int inOffset) { mPtr -= inOffset * mStride; } + + /// Distance between two pointers in elements + inline int operator - (const StridedPtr &inRHS) const { JPH_ASSERT(inRHS.mStride == mStride); return (mPtr - inRHS.mPtr) / mStride; } + + /// Comparison operators + inline bool operator == (const StridedPtr &inRHS) const { return mPtr == inRHS.mPtr; } + inline bool operator != (const StridedPtr &inRHS) const { return mPtr != inRHS.mPtr; } + inline bool operator <= (const StridedPtr &inRHS) const { return mPtr <= inRHS.mPtr; } + inline bool operator >= (const StridedPtr &inRHS) const { return mPtr >= inRHS.mPtr; } + inline bool operator < (const StridedPtr &inRHS) const { return mPtr < inRHS.mPtr; } + inline bool operator > (const StridedPtr &inRHS) const { return mPtr > inRHS.mPtr; } + + /// Access value + inline T & operator * () const { return *reinterpret_cast(mPtr); } + inline T * operator -> () const { return reinterpret_cast(mPtr); } + inline T & operator [] (int inOffset) const { uint8 *ptr = mPtr + inOffset * mStride; return *reinterpret_cast(ptr); } + + /// Explicit conversion + inline T * GetPtr() const { return reinterpret_cast(mPtr); } + + /// Get stride in bytes + inline int GetStride() const { return mStride; } + +private: + uint8 * mPtr = nullptr; /// Pointer to element + int mStride = 0; /// Stride (number of bytes) between elements +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/StringTools.cpp b/thirdparty/jolt_physics/Jolt/Core/StringTools.cpp new file mode 100644 index 000000000000..6eae982cc6bf --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/StringTools.cpp @@ -0,0 +1,101 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +String StringFormat(const char *inFMT, ...) +{ + char buffer[1024]; + + // Format the string + va_list list; + va_start(list, inFMT); + vsnprintf(buffer, sizeof(buffer), inFMT, list); + va_end(list); + + return String(buffer); +} + +void StringReplace(String &ioString, const string_view &inSearch, const string_view &inReplace) +{ + size_t index = 0; + for (;;) + { + index = ioString.find(inSearch, index); + if (index == String::npos) + break; + + ioString.replace(index, inSearch.size(), inReplace); + + index += inReplace.size(); + } +} + +void StringToVector(const string_view &inString, Array &outVector, const string_view &inDelimiter, bool inClearVector) +{ + JPH_ASSERT(inDelimiter.size() > 0); + + // Ensure vector empty + if (inClearVector) + outVector.clear(); + + // No string? no elements + if (inString.empty()) + return; + + // Start with initial string + String s(inString); + + // Add to vector while we have a delimiter + size_t i; + while (!s.empty() && (i = s.find(inDelimiter)) != String::npos) + { + outVector.push_back(s.substr(0, i)); + s.erase(0, i + inDelimiter.length()); + } + + // Add final element + outVector.push_back(s); +} + +void VectorToString(const Array &inVector, String &outString, const string_view &inDelimiter) +{ + // Ensure string empty + outString.clear(); + + for (const String &s : inVector) + { + // Add delimiter if not first element + if (!outString.empty()) + outString.append(inDelimiter); + + // Add element + outString.append(s); + } +} + +String ToLower(const string_view &inString) +{ + String out; + out.reserve(inString.length()); + for (char c : inString) + out.push_back((char)tolower(c)); + return out; +} + +const char *NibbleToBinary(uint32 inNibble) +{ + static const char *nibbles[] = { "0000", "0001", "0010", "0011", "0100", "0101", "0110", "0111", "1000", "1001", "1010", "1011", "1100", "1101", "1110", "1111" }; + return nibbles[inNibble & 0xf]; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/StringTools.h b/thirdparty/jolt_physics/Jolt/Core/StringTools.h new file mode 100644 index 000000000000..378278b15d99 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/StringTools.h @@ -0,0 +1,38 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Create a formatted text string for debugging purposes. +/// Note that this function has an internal buffer of 1024 characters, so long strings will be trimmed. +JPH_EXPORT String StringFormat(const char *inFMT, ...); + +/// Convert type to string +template +String ConvertToString(const T &inValue) +{ + using OStringStream = std::basic_ostringstream, STLAllocator>; + OStringStream oss; + oss << inValue; + return oss.str(); +} + +/// Replace substring with other string +JPH_EXPORT void StringReplace(String &ioString, const string_view &inSearch, const string_view &inReplace); + +/// Convert a delimited string to an array of strings +JPH_EXPORT void StringToVector(const string_view &inString, Array &outVector, const string_view &inDelimiter = ",", bool inClearVector = true); + +/// Convert an array strings to a delimited string +JPH_EXPORT void VectorToString(const Array &inVector, String &outString, const string_view &inDelimiter = ","); + +/// Convert a string to lower case +JPH_EXPORT String ToLower(const string_view &inString); + +/// Converts the lower 4 bits of inNibble to a string that represents the number in binary format +JPH_EXPORT const char *NibbleToBinary(uint32 inNibble); + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/TempAllocator.h b/thirdparty/jolt_physics/Jolt/Core/TempAllocator.h new file mode 100644 index 000000000000..99586bde646f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/TempAllocator.h @@ -0,0 +1,188 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Allocator for temporary allocations. +/// This allocator works as a stack: The blocks must always be freed in the reverse order as they are allocated. +/// Note that allocations and frees can take place from different threads, but the order is guaranteed though +/// job dependencies, so it is not needed to use any form of locking. +class JPH_EXPORT TempAllocator : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Destructor + virtual ~TempAllocator() = default; + + /// Allocates inSize bytes of memory, returned memory address must be JPH_RVECTOR_ALIGNMENT byte aligned + virtual void * Allocate(uint inSize) = 0; + + /// Frees inSize bytes of memory located at inAddress + virtual void Free(void *inAddress, uint inSize) = 0; +}; + +/// Default implementation of the temp allocator that allocates a large block through malloc upfront +class JPH_EXPORT TempAllocatorImpl final : public TempAllocator +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructs the allocator with a maximum allocatable size of inSize + explicit TempAllocatorImpl(uint inSize) : + mBase(static_cast(AlignedAllocate(inSize, JPH_RVECTOR_ALIGNMENT))), + mSize(inSize) + { + } + + /// Destructor, frees the block + virtual ~TempAllocatorImpl() override + { + JPH_ASSERT(mTop == 0); + AlignedFree(mBase); + } + + // See: TempAllocator + virtual void * Allocate(uint inSize) override + { + if (inSize == 0) + { + return nullptr; + } + else + { + uint new_top = mTop + AlignUp(inSize, JPH_RVECTOR_ALIGNMENT); + if (new_top > mSize) + { + Trace("TempAllocator: Out of memory"); + std::abort(); + } + void *address = mBase + mTop; + mTop = new_top; + return address; + } + } + + // See: TempAllocator + virtual void Free(void *inAddress, uint inSize) override + { + if (inAddress == nullptr) + { + JPH_ASSERT(inSize == 0); + } + else + { + mTop -= AlignUp(inSize, JPH_RVECTOR_ALIGNMENT); + if (mBase + mTop != inAddress) + { + Trace("TempAllocator: Freeing in the wrong order"); + std::abort(); + } + } + } + + /// Check if no allocations have been made + bool IsEmpty() const + { + return mTop == 0; + } + + /// Get the total size of the fixed buffer + uint GetSize() const + { + return mSize; + } + + /// Get current usage in bytes of the buffer + uint GetUsage() const + { + return mTop; + } + + /// Check if an allocation of inSize can be made in this fixed buffer allocator + bool CanAllocate(uint inSize) const + { + return mTop + AlignUp(inSize, JPH_RVECTOR_ALIGNMENT) <= mSize; + } + + /// Check if memory block at inAddress is owned by this allocator + bool OwnsMemory(const void *inAddress) const + { + return inAddress >= mBase && inAddress < mBase + mSize; + } + +private: + uint8 * mBase; ///< Base address of the memory block + uint mSize; ///< Size of the memory block + uint mTop = 0; ///< End of currently allocated area +}; + +/// Implementation of the TempAllocator that just falls back to malloc/free +/// Note: This can be quite slow when running in the debugger as large memory blocks need to be initialized with 0xcd +class JPH_EXPORT TempAllocatorMalloc final : public TempAllocator +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // See: TempAllocator + virtual void * Allocate(uint inSize) override + { + return inSize > 0? AlignedAllocate(inSize, JPH_RVECTOR_ALIGNMENT) : nullptr; + } + + // See: TempAllocator + virtual void Free(void *inAddress, [[maybe_unused]] uint inSize) override + { + if (inAddress != nullptr) + AlignedFree(inAddress); + } +}; + +/// Implementation of the TempAllocator that tries to allocate from a large preallocated block, but falls back to malloc when it is exhausted +class JPH_EXPORT TempAllocatorImplWithMallocFallback final : public TempAllocator +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructs the allocator with an initial fixed block if inSize + explicit TempAllocatorImplWithMallocFallback(uint inSize) : + mAllocator(inSize) + { + } + + // See: TempAllocator + virtual void * Allocate(uint inSize) override + { + if (mAllocator.CanAllocate(inSize)) + return mAllocator.Allocate(inSize); + else + return mFallbackAllocator.Allocate(inSize); + } + + // See: TempAllocator + virtual void Free(void *inAddress, uint inSize) override + { + if (inAddress == nullptr) + { + JPH_ASSERT(inSize == 0); + } + else + { + if (mAllocator.OwnsMemory(inAddress)) + mAllocator.Free(inAddress, inSize); + else + mFallbackAllocator.Free(inAddress, inSize); + } + } + +private: + TempAllocatorImpl mAllocator; + TempAllocatorMalloc mFallbackAllocator; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/TickCounter.cpp b/thirdparty/jolt_physics/Jolt/Core/TickCounter.cpp new file mode 100644 index 000000000000..d08140091e92 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/TickCounter.cpp @@ -0,0 +1,36 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +#if defined(JPH_PLATFORM_WINDOWS) + JPH_SUPPRESS_WARNING_PUSH + JPH_MSVC_SUPPRESS_WARNING(5039) // winbase.h(13179): warning C5039: 'TpSetCallbackCleanupGroup': pointer or reference to potentially throwing function passed to 'extern "C"' function under -EHc. Undefined behavior may occur if this function throws an exception. + #ifndef WIN32_LEAN_AND_MEAN + #define WIN32_LEAN_AND_MEAN + #endif +#ifndef JPH_COMPILER_MINGW + #include +#else + #include +#endif + JPH_SUPPRESS_WARNING_POP +#endif + +JPH_NAMESPACE_BEGIN + +#if defined(JPH_PLATFORM_WINDOWS_UWP) || (defined(JPH_PLATFORM_WINDOWS) && defined(JPH_CPU_ARM)) + +uint64 GetProcessorTickCount() +{ + LARGE_INTEGER count; + QueryPerformanceCounter(&count); + return uint64(count.QuadPart); +} + +#endif // JPH_PLATFORM_WINDOWS_UWP || (JPH_PLATFORM_WINDOWS && JPH_CPU_ARM) + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/TickCounter.h b/thirdparty/jolt_physics/Jolt/Core/TickCounter.h new file mode 100644 index 000000000000..2b5410e3d9f9 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/TickCounter.h @@ -0,0 +1,49 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +// Include for __rdtsc +#if defined(JPH_PLATFORM_WINDOWS) + #include +#elif defined(JPH_CPU_X86) && defined(JPH_COMPILER_GCC) + #include +#elif defined(JPH_CPU_E2K) + #include +#endif + +JPH_NAMESPACE_BEGIN + +#if defined(JPH_PLATFORM_WINDOWS_UWP) || (defined(JPH_PLATFORM_WINDOWS) && defined(JPH_CPU_ARM)) + +/// Functionality to get the processors cycle counter +uint64 GetProcessorTickCount(); // Not inline to avoid having to include Windows.h + +#else + +/// Functionality to get the processors cycle counter +JPH_INLINE uint64 GetProcessorTickCount() +{ +#if defined(JPH_PLATFORM_BLUE) + return JPH_PLATFORM_BLUE_GET_TICKS(); +#elif defined(JPH_CPU_X86) + return __rdtsc(); +#elif defined(JPH_CPU_E2K) + return __rdtsc(); +#elif defined(JPH_CPU_ARM) && defined(JPH_USE_NEON) + uint64 val; + asm volatile("mrs %0, cntvct_el0" : "=r" (val)); + return val; +#elif defined(JPH_CPU_ARM) + return 0; // Not supported +#elif defined(JPH_CPU_WASM) + return 0; // Not supported +#else + #error Undefined +#endif +} + +#endif // JPH_PLATFORM_WINDOWS_UWP || (JPH_PLATFORM_WINDOWS && JPH_CPU_ARM) + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/UnorderedMap.h b/thirdparty/jolt_physics/Jolt/Core/UnorderedMap.h new file mode 100644 index 000000000000..f876e2849e20 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/UnorderedMap.h @@ -0,0 +1,80 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Internal helper class to provide context for UnorderedMap +template +class UnorderedMapDetail +{ +public: + /// Get key from key value pair + static const Key & sGetKey(const std::pair &inKeyValue) + { + return inKeyValue.first; + } +}; + +/// Hash Map class +/// @tparam Key Key type +/// @tparam Value Value type +/// @tparam Hash Hash function (note should be 64-bits) +/// @tparam KeyEqual Equality comparison function +template , class KeyEqual = std::equal_to> +class UnorderedMap : public HashTable, UnorderedMapDetail, Hash, KeyEqual> +{ + using Base = HashTable, UnorderedMapDetail, Hash, KeyEqual>; + +public: + using size_type = typename Base::size_type; + using iterator = typename Base::iterator; + using const_iterator = typename Base::const_iterator; + using value_type = typename Base::value_type; + + Value & operator [] (const Key &inKey) + { + size_type index; + bool inserted = this->InsertKey(inKey, index); + value_type &key_value = this->GetElement(index); + if (inserted) + ::new (&key_value) value_type(inKey, Value()); + return key_value.second; + } + + template + std::pair try_emplace(const Key &inKey, Args &&...inArgs) + { + size_type index; + bool inserted = this->InsertKey(inKey, index); + if (inserted) + ::new (&this->GetElement(index)) value_type(std::piecewise_construct, std::forward_as_tuple(inKey), std::forward_as_tuple(std::forward(inArgs)...)); + return std::make_pair(iterator(this, index), inserted); + } + + template + std::pair try_emplace(Key &&inKey, Args &&...inArgs) + { + size_type index; + bool inserted = this->InsertKey(inKey, index); + if (inserted) + ::new (&this->GetElement(index)) value_type(std::piecewise_construct, std::forward_as_tuple(std::move(inKey)), std::forward_as_tuple(std::forward(inArgs)...)); + return std::make_pair(iterator(this, index), inserted); + } + + /// Const version of find + using Base::find; + + /// Non-const version of find + iterator find(const Key &inKey) + { + const_iterator it = Base::find(inKey); + return iterator(this, it.mIndex); + } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/UnorderedSet.h b/thirdparty/jolt_physics/Jolt/Core/UnorderedSet.h new file mode 100644 index 000000000000..a3202554435e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/UnorderedSet.h @@ -0,0 +1,32 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Internal helper class to provide context for UnorderedSet +template +class UnorderedSetDetail +{ +public: + /// The key is the key, just return it + static const Key & sGetKey(const Key &inKey) + { + return inKey; + } +}; + +/// Hash Set class +/// @tparam Key Key type +/// @tparam Hash Hash function (note should be 64-bits) +/// @tparam KeyEqual Equality comparison function +template , class KeyEqual = std::equal_to> +class UnorderedSet : public HashTable, Hash, KeyEqual> +{ +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/AABox.h b/thirdparty/jolt_physics/Jolt/Geometry/AABox.h new file mode 100644 index 000000000000..65e353aff919 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/AABox.h @@ -0,0 +1,312 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Axis aligned box +class [[nodiscard]] AABox +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + AABox() : mMin(Vec3::sReplicate(FLT_MAX)), mMax(Vec3::sReplicate(-FLT_MAX)) { } + AABox(Vec3Arg inMin, Vec3Arg inMax) : mMin(inMin), mMax(inMax) { } + AABox(DVec3Arg inMin, DVec3Arg inMax) : mMin(inMin.ToVec3RoundDown()), mMax(inMax.ToVec3RoundUp()) { } + AABox(Vec3Arg inCenter, float inRadius) : mMin(inCenter - Vec3::sReplicate(inRadius)), mMax(inCenter + Vec3::sReplicate(inRadius)) { } + + /// Create box from 2 points + static AABox sFromTwoPoints(Vec3Arg inP1, Vec3Arg inP2) { return AABox(Vec3::sMin(inP1, inP2), Vec3::sMax(inP1, inP2)); } + + /// Create box from indexed triangle + static AABox sFromTriangle(const VertexList &inVertices, const IndexedTriangle &inTriangle) + { + AABox box = sFromTwoPoints(Vec3(inVertices[inTriangle.mIdx[0]]), Vec3(inVertices[inTriangle.mIdx[1]])); + box.Encapsulate(Vec3(inVertices[inTriangle.mIdx[2]])); + return box; + } + + /// Get bounding box of size 2 * FLT_MAX + static AABox sBiggest() + { + return AABox(Vec3::sReplicate(-FLT_MAX), Vec3::sReplicate(FLT_MAX)); + } + + /// Comparison operators + bool operator == (const AABox &inRHS) const { return mMin == inRHS.mMin && mMax == inRHS.mMax; } + bool operator != (const AABox &inRHS) const { return mMin != inRHS.mMin || mMax != inRHS.mMax; } + + /// Reset the bounding box to an empty bounding box + void SetEmpty() + { + mMin = Vec3::sReplicate(FLT_MAX); + mMax = Vec3::sReplicate(-FLT_MAX); + } + + /// Check if the bounding box is valid (max >= min) + bool IsValid() const + { + return mMin.GetX() <= mMax.GetX() && mMin.GetY() <= mMax.GetY() && mMin.GetZ() <= mMax.GetZ(); + } + + /// Encapsulate point in bounding box + void Encapsulate(Vec3Arg inPos) + { + mMin = Vec3::sMin(mMin, inPos); + mMax = Vec3::sMax(mMax, inPos); + } + + /// Encapsulate bounding box in bounding box + void Encapsulate(const AABox &inRHS) + { + mMin = Vec3::sMin(mMin, inRHS.mMin); + mMax = Vec3::sMax(mMax, inRHS.mMax); + } + + /// Encapsulate triangle in bounding box + void Encapsulate(const Triangle &inRHS) + { + Vec3 v = Vec3::sLoadFloat3Unsafe(inRHS.mV[0]); + Encapsulate(v); + v = Vec3::sLoadFloat3Unsafe(inRHS.mV[1]); + Encapsulate(v); + v = Vec3::sLoadFloat3Unsafe(inRHS.mV[2]); + Encapsulate(v); + } + + /// Encapsulate triangle in bounding box + void Encapsulate(const VertexList &inVertices, const IndexedTriangle &inTriangle) + { + for (uint32 idx : inTriangle.mIdx) + Encapsulate(Vec3(inVertices[idx])); + } + + /// Intersect this bounding box with inOther, returns the intersection + AABox Intersect(const AABox &inOther) const + { + return AABox(Vec3::sMax(mMin, inOther.mMin), Vec3::sMin(mMax, inOther.mMax)); + } + + /// Make sure that each edge of the bounding box has a minimal length + void EnsureMinimalEdgeLength(float inMinEdgeLength) + { + Vec3 min_length = Vec3::sReplicate(inMinEdgeLength); + mMax = Vec3::sSelect(mMax, mMin + min_length, Vec3::sLess(mMax - mMin, min_length)); + } + + /// Widen the box on both sides by inVector + void ExpandBy(Vec3Arg inVector) + { + mMin -= inVector; + mMax += inVector; + } + + /// Get center of bounding box + Vec3 GetCenter() const + { + return 0.5f * (mMin + mMax); + } + + /// Get extent of bounding box (half of the size) + Vec3 GetExtent() const + { + return 0.5f * (mMax - mMin); + } + + /// Get size of bounding box + Vec3 GetSize() const + { + return mMax - mMin; + } + + /// Get surface area of bounding box + float GetSurfaceArea() const + { + Vec3 extent = mMax - mMin; + return 2.0f * (extent.GetX() * extent.GetY() + extent.GetX() * extent.GetZ() + extent.GetY() * extent.GetZ()); + } + + /// Get volume of bounding box + float GetVolume() const + { + Vec3 extent = mMax - mMin; + return extent.GetX() * extent.GetY() * extent.GetZ(); + } + + /// Check if this box contains another box + bool Contains(const AABox &inOther) const + { + return UVec4::sAnd(Vec3::sLessOrEqual(mMin, inOther.mMin), Vec3::sGreaterOrEqual(mMax, inOther.mMax)).TestAllXYZTrue(); + } + + /// Check if this box contains a point + bool Contains(Vec3Arg inOther) const + { + return UVec4::sAnd(Vec3::sLessOrEqual(mMin, inOther), Vec3::sGreaterOrEqual(mMax, inOther)).TestAllXYZTrue(); + } + + /// Check if this box contains a point + bool Contains(DVec3Arg inOther) const + { + return Contains(Vec3(inOther)); + } + + /// Check if this box overlaps with another box + bool Overlaps(const AABox &inOther) const + { + return !UVec4::sOr(Vec3::sGreater(mMin, inOther.mMax), Vec3::sLess(mMax, inOther.mMin)).TestAnyXYZTrue(); + } + + /// Check if this box overlaps with a plane + bool Overlaps(const Plane &inPlane) const + { + Vec3 normal = inPlane.GetNormal(); + float dist_normal = inPlane.SignedDistance(GetSupport(normal)); + float dist_min_normal = inPlane.SignedDistance(GetSupport(-normal)); + return dist_normal * dist_min_normal <= 0.0f; // If both support points are on the same side of the plane we don't overlap + } + + /// Translate bounding box + void Translate(Vec3Arg inTranslation) + { + mMin += inTranslation; + mMax += inTranslation; + } + + /// Translate bounding box + void Translate(DVec3Arg inTranslation) + { + mMin = (DVec3(mMin) + inTranslation).ToVec3RoundDown(); + mMax = (DVec3(mMax) + inTranslation).ToVec3RoundUp(); + } + + /// Transform bounding box + AABox Transformed(Mat44Arg inMatrix) const + { + // Start with the translation of the matrix + Vec3 new_min, new_max; + new_min = new_max = inMatrix.GetTranslation(); + + // Now find the extreme points by considering the product of the min and max with each column of inMatrix + for (int c = 0; c < 3; ++c) + { + Vec3 col = inMatrix.GetColumn3(c); + + Vec3 a = col * mMin[c]; + Vec3 b = col * mMax[c]; + + new_min += Vec3::sMin(a, b); + new_max += Vec3::sMax(a, b); + } + + // Return the new bounding box + return AABox(new_min, new_max); + } + + /// Transform bounding box + AABox Transformed(DMat44Arg inMatrix) const + { + AABox transformed = Transformed(inMatrix.GetRotation()); + transformed.Translate(inMatrix.GetTranslation()); + return transformed; + } + + /// Scale this bounding box, can handle non-uniform and negative scaling + AABox Scaled(Vec3Arg inScale) const + { + return AABox::sFromTwoPoints(mMin * inScale, mMax * inScale); + } + + /// Calculate the support vector for this convex shape. + Vec3 GetSupport(Vec3Arg inDirection) const + { + return Vec3::sSelect(mMax, mMin, Vec3::sLess(inDirection, Vec3::sZero())); + } + + /// Get the vertices of the face that faces inDirection the most + template + void GetSupportingFace(Vec3Arg inDirection, VERTEX_ARRAY &outVertices) const + { + outVertices.resize(4); + + int axis = inDirection.Abs().GetHighestComponentIndex(); + if (inDirection[axis] < 0.0f) + { + switch (axis) + { + case 0: + outVertices[0] = Vec3(mMax.GetX(), mMin.GetY(), mMin.GetZ()); + outVertices[1] = Vec3(mMax.GetX(), mMax.GetY(), mMin.GetZ()); + outVertices[2] = Vec3(mMax.GetX(), mMax.GetY(), mMax.GetZ()); + outVertices[3] = Vec3(mMax.GetX(), mMin.GetY(), mMax.GetZ()); + break; + + case 1: + outVertices[0] = Vec3(mMin.GetX(), mMax.GetY(), mMin.GetZ()); + outVertices[1] = Vec3(mMin.GetX(), mMax.GetY(), mMax.GetZ()); + outVertices[2] = Vec3(mMax.GetX(), mMax.GetY(), mMax.GetZ()); + outVertices[3] = Vec3(mMax.GetX(), mMax.GetY(), mMin.GetZ()); + break; + + case 2: + outVertices[0] = Vec3(mMin.GetX(), mMin.GetY(), mMax.GetZ()); + outVertices[1] = Vec3(mMax.GetX(), mMin.GetY(), mMax.GetZ()); + outVertices[2] = Vec3(mMax.GetX(), mMax.GetY(), mMax.GetZ()); + outVertices[3] = Vec3(mMin.GetX(), mMax.GetY(), mMax.GetZ()); + break; + } + } + else + { + switch (axis) + { + case 0: + outVertices[0] = Vec3(mMin.GetX(), mMin.GetY(), mMin.GetZ()); + outVertices[1] = Vec3(mMin.GetX(), mMin.GetY(), mMax.GetZ()); + outVertices[2] = Vec3(mMin.GetX(), mMax.GetY(), mMax.GetZ()); + outVertices[3] = Vec3(mMin.GetX(), mMax.GetY(), mMin.GetZ()); + break; + + case 1: + outVertices[0] = Vec3(mMin.GetX(), mMin.GetY(), mMin.GetZ()); + outVertices[1] = Vec3(mMax.GetX(), mMin.GetY(), mMin.GetZ()); + outVertices[2] = Vec3(mMax.GetX(), mMin.GetY(), mMax.GetZ()); + outVertices[3] = Vec3(mMin.GetX(), mMin.GetY(), mMax.GetZ()); + break; + + case 2: + outVertices[0] = Vec3(mMin.GetX(), mMin.GetY(), mMin.GetZ()); + outVertices[1] = Vec3(mMin.GetX(), mMax.GetY(), mMin.GetZ()); + outVertices[2] = Vec3(mMax.GetX(), mMax.GetY(), mMin.GetZ()); + outVertices[3] = Vec3(mMax.GetX(), mMin.GetY(), mMin.GetZ()); + break; + } + } + } + + /// Get the closest point on or in this box to inPoint + Vec3 GetClosestPoint(Vec3Arg inPoint) const + { + return Vec3::sMin(Vec3::sMax(inPoint, mMin), mMax); + } + + /// Get the squared distance between inPoint and this box (will be 0 if in Point is inside the box) + inline float GetSqDistanceTo(Vec3Arg inPoint) const + { + return (GetClosestPoint(inPoint) - inPoint).LengthSq(); + } + + /// Bounding box min and max + Vec3 mMin; + Vec3 mMax; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/AABox4.h b/thirdparty/jolt_physics/Jolt/Geometry/AABox4.h new file mode 100644 index 000000000000..4465d4dabe5a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/AABox4.h @@ -0,0 +1,224 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Helper functions that process 4 axis aligned boxes at the same time using SIMD +/// Test if 4 bounding boxes overlap with 1 bounding box, splat 1 box +JPH_INLINE UVec4 AABox4VsBox(const AABox &inBox1, Vec4Arg inBox2MinX, Vec4Arg inBox2MinY, Vec4Arg inBox2MinZ, Vec4Arg inBox2MaxX, Vec4Arg inBox2MaxY, Vec4Arg inBox2MaxZ) +{ + // Splat values of box 1 + Vec4 box1_minx = inBox1.mMin.SplatX(); + Vec4 box1_miny = inBox1.mMin.SplatY(); + Vec4 box1_minz = inBox1.mMin.SplatZ(); + Vec4 box1_maxx = inBox1.mMax.SplatX(); + Vec4 box1_maxy = inBox1.mMax.SplatY(); + Vec4 box1_maxz = inBox1.mMax.SplatZ(); + + // Test separation over each axis + UVec4 nooverlapx = UVec4::sOr(Vec4::sGreater(box1_minx, inBox2MaxX), Vec4::sGreater(inBox2MinX, box1_maxx)); + UVec4 nooverlapy = UVec4::sOr(Vec4::sGreater(box1_miny, inBox2MaxY), Vec4::sGreater(inBox2MinY, box1_maxy)); + UVec4 nooverlapz = UVec4::sOr(Vec4::sGreater(box1_minz, inBox2MaxZ), Vec4::sGreater(inBox2MinZ, box1_maxz)); + + // Return overlap + return UVec4::sNot(UVec4::sOr(UVec4::sOr(nooverlapx, nooverlapy), nooverlapz)); +} + +/// Scale 4 axis aligned boxes +JPH_INLINE void AABox4Scale(Vec3Arg inScale, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ, Vec4 &outBoundsMinX, Vec4 &outBoundsMinY, Vec4 &outBoundsMinZ, Vec4 &outBoundsMaxX, Vec4 &outBoundsMaxY, Vec4 &outBoundsMaxZ) +{ + Vec4 scale_x = inScale.SplatX(); + Vec4 scaled_min_x = scale_x * inBoxMinX; + Vec4 scaled_max_x = scale_x * inBoxMaxX; + outBoundsMinX = Vec4::sMin(scaled_min_x, scaled_max_x); // Negative scale can flip min and max + outBoundsMaxX = Vec4::sMax(scaled_min_x, scaled_max_x); + + Vec4 scale_y = inScale.SplatY(); + Vec4 scaled_min_y = scale_y * inBoxMinY; + Vec4 scaled_max_y = scale_y * inBoxMaxY; + outBoundsMinY = Vec4::sMin(scaled_min_y, scaled_max_y); + outBoundsMaxY = Vec4::sMax(scaled_min_y, scaled_max_y); + + Vec4 scale_z = inScale.SplatZ(); + Vec4 scaled_min_z = scale_z * inBoxMinZ; + Vec4 scaled_max_z = scale_z * inBoxMaxZ; + outBoundsMinZ = Vec4::sMin(scaled_min_z, scaled_max_z); + outBoundsMaxZ = Vec4::sMax(scaled_min_z, scaled_max_z); +} + +/// Enlarge 4 bounding boxes with extent (add to both sides) +JPH_INLINE void AABox4EnlargeWithExtent(Vec3Arg inExtent, Vec4 &ioBoundsMinX, Vec4 &ioBoundsMinY, Vec4 &ioBoundsMinZ, Vec4 &ioBoundsMaxX, Vec4 &ioBoundsMaxY, Vec4 &ioBoundsMaxZ) +{ + Vec4 extent_x = inExtent.SplatX(); + ioBoundsMinX -= extent_x; + ioBoundsMaxX += extent_x; + + Vec4 extent_y = inExtent.SplatY(); + ioBoundsMinY -= extent_y; + ioBoundsMaxY += extent_y; + + Vec4 extent_z = inExtent.SplatZ(); + ioBoundsMinZ -= extent_z; + ioBoundsMaxZ += extent_z; +} + +/// Test if 4 bounding boxes overlap with a point +JPH_INLINE UVec4 AABox4VsPoint(Vec3Arg inPoint, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ) +{ + // Splat point to 4 component vectors + Vec4 point_x = Vec4(inPoint).SplatX(); + Vec4 point_y = Vec4(inPoint).SplatY(); + Vec4 point_z = Vec4(inPoint).SplatZ(); + + // Test if point overlaps with box + UVec4 overlapx = UVec4::sAnd(Vec4::sGreaterOrEqual(point_x, inBoxMinX), Vec4::sLessOrEqual(point_x, inBoxMaxX)); + UVec4 overlapy = UVec4::sAnd(Vec4::sGreaterOrEqual(point_y, inBoxMinY), Vec4::sLessOrEqual(point_y, inBoxMaxY)); + UVec4 overlapz = UVec4::sAnd(Vec4::sGreaterOrEqual(point_z, inBoxMinZ), Vec4::sLessOrEqual(point_z, inBoxMaxZ)); + + // Test if all are overlapping + return UVec4::sAnd(UVec4::sAnd(overlapx, overlapy), overlapz); +} + +/// Test if 4 bounding boxes overlap with an oriented box +JPH_INLINE UVec4 AABox4VsBox(Mat44Arg inOrientation, Vec3Arg inHalfExtents, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ, float inEpsilon = 1.0e-6f) +{ + // Taken from: Real Time Collision Detection - Christer Ericson + // Chapter 4.4.1, page 103-105. + // Note that the code is swapped around: A is the aabox and B is the oriented box (this saves us from having to invert the orientation of the oriented box) + + // Compute translation vector t (the translation of B in the space of A) + Vec4 t[3] { + inOrientation.GetTranslation().SplatX() - 0.5f * (inBoxMinX + inBoxMaxX), + inOrientation.GetTranslation().SplatY() - 0.5f * (inBoxMinY + inBoxMaxY), + inOrientation.GetTranslation().SplatZ() - 0.5f * (inBoxMinZ + inBoxMaxZ) }; + + // Compute common subexpressions. Add in an epsilon term to + // counteract arithmetic errors when two edges are parallel and + // their cross product is (near) null (see text for details) + Vec3 epsilon = Vec3::sReplicate(inEpsilon); + Vec3 abs_r[3] { inOrientation.GetAxisX().Abs() + epsilon, inOrientation.GetAxisY().Abs() + epsilon, inOrientation.GetAxisZ().Abs() + epsilon }; + + // Half extents for a + Vec4 a_half_extents[3] { + 0.5f * (inBoxMaxX - inBoxMinX), + 0.5f * (inBoxMaxY - inBoxMinY), + 0.5f * (inBoxMaxZ - inBoxMinZ) }; + + // Half extents of b + Vec4 b_half_extents_x = inHalfExtents.SplatX(); + Vec4 b_half_extents_y = inHalfExtents.SplatY(); + Vec4 b_half_extents_z = inHalfExtents.SplatZ(); + + // Each component corresponds to 1 overlapping OBB vs ABB + UVec4 overlaps = UVec4(0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff); + + // Test axes L = A0, L = A1, L = A2 + Vec4 ra, rb; + for (int i = 0; i < 3; i++) + { + ra = a_half_extents[i]; + rb = b_half_extents_x * abs_r[0][i] + b_half_extents_y * abs_r[1][i] + b_half_extents_z * abs_r[2][i]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual(t[i].Abs(), ra + rb)); + } + + // Test axes L = B0, L = B1, L = B2 + for (int i = 0; i < 3; i++) + { + ra = a_half_extents[0] * abs_r[i][0] + a_half_extents[1] * abs_r[i][1] + a_half_extents[2] * abs_r[i][2]; + rb = Vec4::sReplicate(inHalfExtents[i]); + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(0, i) + t[1] * inOrientation(1, i) + t[2] * inOrientation(2, i)).Abs(), ra + rb)); + } + + // Test axis L = A0 x B0 + ra = a_half_extents[1] * abs_r[0][2] + a_half_extents[2] * abs_r[0][1]; + rb = b_half_extents_y * abs_r[2][0] + b_half_extents_z * abs_r[1][0]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[2] * inOrientation(1, 0) - t[1] * inOrientation(2, 0)).Abs(), ra + rb)); + + // Test axis L = A0 x B1 + ra = a_half_extents[1] * abs_r[1][2] + a_half_extents[2] * abs_r[1][1]; + rb = b_half_extents_x * abs_r[2][0] + b_half_extents_z * abs_r[0][0]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[2] * inOrientation(1, 1) - t[1] * inOrientation(2, 1)).Abs(), ra + rb)); + + // Test axis L = A0 x B2 + ra = a_half_extents[1] * abs_r[2][2] + a_half_extents[2] * abs_r[2][1]; + rb = b_half_extents_x * abs_r[1][0] + b_half_extents_y * abs_r[0][0]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[2] * inOrientation(1, 2) - t[1] * inOrientation(2, 2)).Abs(), ra + rb)); + + // Test axis L = A1 x B0 + ra = a_half_extents[0] * abs_r[0][2] + a_half_extents[2] * abs_r[0][0]; + rb = b_half_extents_y * abs_r[2][1] + b_half_extents_z * abs_r[1][1]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(2, 0) - t[2] * inOrientation(0, 0)).Abs(), ra + rb)); + + // Test axis L = A1 x B1 + ra = a_half_extents[0] * abs_r[1][2] + a_half_extents[2] * abs_r[1][0]; + rb = b_half_extents_x * abs_r[2][1] + b_half_extents_z * abs_r[0][1]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(2, 1) - t[2] * inOrientation(0, 1)).Abs(), ra + rb)); + + // Test axis L = A1 x B2 + ra = a_half_extents[0] * abs_r[2][2] + a_half_extents[2] * abs_r[2][0]; + rb = b_half_extents_x * abs_r[1][1] + b_half_extents_y * abs_r[0][1]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(2, 2) - t[2] * inOrientation(0, 2)).Abs(), ra + rb)); + + // Test axis L = A2 x B0 + ra = a_half_extents[0] * abs_r[0][1] + a_half_extents[1] * abs_r[0][0]; + rb = b_half_extents_y * abs_r[2][2] + b_half_extents_z * abs_r[1][2]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[1] * inOrientation(0, 0) - t[0] * inOrientation(1, 0)).Abs(), ra + rb)); + + // Test axis L = A2 x B1 + ra = a_half_extents[0] * abs_r[1][1] + a_half_extents[1] * abs_r[1][0]; + rb = b_half_extents_x * abs_r[2][2] + b_half_extents_z * abs_r[0][2]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[1] * inOrientation(0, 1) - t[0] * inOrientation(1, 1)).Abs(), ra + rb)); + + // Test axis L = A2 x B2 + ra = a_half_extents[0] * abs_r[2][1] + a_half_extents[1] * abs_r[2][0]; + rb = b_half_extents_x * abs_r[1][2] + b_half_extents_y * abs_r[0][2]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[1] * inOrientation(0, 2) - t[0] * inOrientation(1, 2)).Abs(), ra + rb)); + + // Return if the OBB vs AABBs are intersecting + return overlaps; +} + +/// Convenience function that tests 4 AABoxes vs OrientedBox +JPH_INLINE UVec4 AABox4VsBox(const OrientedBox &inBox, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ, float inEpsilon = 1.0e-6f) +{ + return AABox4VsBox(inBox.mOrientation, inBox.mHalfExtents, inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ, inEpsilon); +} + +/// Get the squared distance between 4 AABoxes and a point +JPH_INLINE Vec4 AABox4DistanceSqToPoint(Vec4Arg inPointX, Vec4Arg inPointY, Vec4Arg inPointZ, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ) +{ + // Get closest point on box + Vec4 closest_x = Vec4::sMin(Vec4::sMax(inPointX, inBoxMinX), inBoxMaxX); + Vec4 closest_y = Vec4::sMin(Vec4::sMax(inPointY, inBoxMinY), inBoxMaxY); + Vec4 closest_z = Vec4::sMin(Vec4::sMax(inPointZ, inBoxMinZ), inBoxMaxZ); + + // Return the squared distance between the box and point + return Square(closest_x - inPointX) + Square(closest_y - inPointY) + Square(closest_z - inPointZ); +} + +/// Get the squared distance between 4 AABoxes and a point +JPH_INLINE Vec4 AABox4DistanceSqToPoint(Vec3 inPoint, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ) +{ + return AABox4DistanceSqToPoint(inPoint.SplatX(), inPoint.SplatY(), inPoint.SplatZ(), inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ); +} + +/// Test 4 AABoxes vs a sphere +JPH_INLINE UVec4 AABox4VsSphere(Vec4Arg inCenterX, Vec4Arg inCenterY, Vec4Arg inCenterZ, Vec4Arg inRadiusSq, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ) +{ + // Test the distance from the center of the sphere to the box is smaller than the radius + Vec4 distance_sq = AABox4DistanceSqToPoint(inCenterX, inCenterY, inCenterZ, inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ); + return Vec4::sLessOrEqual(distance_sq, inRadiusSq); +} + +/// Test 4 AABoxes vs a sphere +JPH_INLINE UVec4 AABox4VsSphere(Vec3Arg inCenter, float inRadiusSq, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ) +{ + return AABox4VsSphere(inCenter.SplatX(), inCenter.SplatY(), inCenter.SplatZ(), Vec4::sReplicate(inRadiusSq), inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/ClipPoly.h b/thirdparty/jolt_physics/Jolt/Geometry/ClipPoly.h new file mode 100644 index 000000000000..7301d8e6f0a5 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/ClipPoly.h @@ -0,0 +1,200 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Clip inPolygonToClip against the positive halfspace of plane defined by inPlaneOrigin and inPlaneNormal. +/// inPlaneNormal does not need to be normalized. +template +void ClipPolyVsPlane(const VERTEX_ARRAY &inPolygonToClip, Vec3Arg inPlaneOrigin, Vec3Arg inPlaneNormal, VERTEX_ARRAY &outClippedPolygon) +{ + JPH_ASSERT(inPolygonToClip.size() >= 2); + JPH_ASSERT(outClippedPolygon.empty()); + + // Determine state of last point + Vec3 e1 = inPolygonToClip[inPolygonToClip.size() - 1]; + float prev_num = (inPlaneOrigin - e1).Dot(inPlaneNormal); + bool prev_inside = prev_num < 0.0f; + + // Loop through all vertices + for (typename VERTEX_ARRAY::size_type j = 0; j < inPolygonToClip.size(); ++j) + { + // Check if second point is inside + Vec3Arg e2 = inPolygonToClip[j]; + float num = (inPlaneOrigin - e2).Dot(inPlaneNormal); + bool cur_inside = num < 0.0f; + + // In -> Out or Out -> In: Add point on clipping plane + if (cur_inside != prev_inside) + { + // Solve: (X - inPlaneOrigin) . inPlaneNormal = 0 and X = e1 + t * (e2 - e1) for X + Vec3 e12 = e2 - e1; + float denom = e12.Dot(inPlaneNormal); + if (denom != 0.0f) + outClippedPolygon.push_back(e1 + (prev_num / denom) * e12); + else + cur_inside = prev_inside; // Edge is parallel to plane, treat point as if it were on the same side as the last point + } + + // Point inside, add it + if (cur_inside) + outClippedPolygon.push_back(e2); + + // Update previous state + prev_num = num; + prev_inside = cur_inside; + e1 = e2; + } +} + +/// Clip polygon versus polygon. +/// Both polygons are assumed to be in counter clockwise order. +/// @param inClippingPolygonNormal is used to create planes of all edges in inClippingPolygon against which inPolygonToClip is clipped, inClippingPolygonNormal does not need to be normalized +/// @param inClippingPolygon is the polygon which inClippedPolygon is clipped against +/// @param inPolygonToClip is the polygon that is clipped +/// @param outClippedPolygon will contain clipped polygon when function returns +template +void ClipPolyVsPoly(const VERTEX_ARRAY &inPolygonToClip, const VERTEX_ARRAY &inClippingPolygon, Vec3Arg inClippingPolygonNormal, VERTEX_ARRAY &outClippedPolygon) +{ + JPH_ASSERT(inPolygonToClip.size() >= 2); + JPH_ASSERT(inClippingPolygon.size() >= 3); + + VERTEX_ARRAY tmp_vertices[2]; + int tmp_vertices_idx = 0; + + for (typename VERTEX_ARRAY::size_type i = 0; i < inClippingPolygon.size(); ++i) + { + // Get edge to clip against + Vec3 clip_e1 = inClippingPolygon[i]; + Vec3 clip_e2 = inClippingPolygon[(i + 1) % inClippingPolygon.size()]; + Vec3 clip_normal = inClippingPolygonNormal.Cross(clip_e2 - clip_e1); // Pointing inward to the clipping polygon + + // Get source and target polygon + const VERTEX_ARRAY &src_polygon = (i == 0)? inPolygonToClip : tmp_vertices[tmp_vertices_idx]; + tmp_vertices_idx ^= 1; + VERTEX_ARRAY &tgt_polygon = (i == inClippingPolygon.size() - 1)? outClippedPolygon : tmp_vertices[tmp_vertices_idx]; + tgt_polygon.clear(); + + // Clip against the edge + ClipPolyVsPlane(src_polygon, clip_e1, clip_normal, tgt_polygon); + + // Break out if no polygon left + if (tgt_polygon.size() < 3) + { + outClippedPolygon.clear(); + break; + } + } +} + +/// Clip inPolygonToClip against an edge, the edge is projected on inPolygonToClip using inClippingEdgeNormal. +/// The positive half space (the side on the edge in the direction of inClippingEdgeNormal) is cut away. +template +void ClipPolyVsEdge(const VERTEX_ARRAY &inPolygonToClip, Vec3Arg inEdgeVertex1, Vec3Arg inEdgeVertex2, Vec3Arg inClippingEdgeNormal, VERTEX_ARRAY &outClippedPolygon) +{ + JPH_ASSERT(inPolygonToClip.size() >= 3); + JPH_ASSERT(outClippedPolygon.empty()); + + // Get normal that is perpendicular to the edge and the clipping edge normal + Vec3 edge = inEdgeVertex2 - inEdgeVertex1; + Vec3 edge_normal = inClippingEdgeNormal.Cross(edge); + + // Project vertices of edge on inPolygonToClip + Vec3 polygon_normal = (inPolygonToClip[2] - inPolygonToClip[0]).Cross(inPolygonToClip[1] - inPolygonToClip[0]); + float polygon_normal_len_sq = polygon_normal.LengthSq(); + Vec3 v1 = inEdgeVertex1 + polygon_normal.Dot(inPolygonToClip[0] - inEdgeVertex1) * polygon_normal / polygon_normal_len_sq; + Vec3 v2 = inEdgeVertex2 + polygon_normal.Dot(inPolygonToClip[0] - inEdgeVertex2) * polygon_normal / polygon_normal_len_sq; + Vec3 v12 = v2 - v1; + float v12_len_sq = v12.LengthSq(); + + // Determine state of last point + Vec3 e1 = inPolygonToClip[inPolygonToClip.size() - 1]; + float prev_num = (inEdgeVertex1 - e1).Dot(edge_normal); + bool prev_inside = prev_num < 0.0f; + + // Loop through all vertices + for (typename VERTEX_ARRAY::size_type j = 0; j < inPolygonToClip.size(); ++j) + { + // Check if second point is inside + Vec3 e2 = inPolygonToClip[j]; + float num = (inEdgeVertex1 - e2).Dot(edge_normal); + bool cur_inside = num < 0.0f; + + // In -> Out or Out -> In: Add point on clipping plane + if (cur_inside != prev_inside) + { + // Solve: (inEdgeVertex1 - X) . edge_normal = 0 and X = e1 + t * (e2 - e1) for X + Vec3 e12 = e2 - e1; + float denom = e12.Dot(edge_normal); + Vec3 clipped_point = denom != 0.0f? e1 + (prev_num / denom) * e12 : e1; + + // Project point on line segment v1, v2 so see if it falls outside if the edge + float projection = (clipped_point - v1).Dot(v12); + if (projection < 0.0f) + outClippedPolygon.push_back(v1); + else if (projection > v12_len_sq) + outClippedPolygon.push_back(v2); + else + outClippedPolygon.push_back(clipped_point); + } + + // Update previous state + prev_num = num; + prev_inside = cur_inside; + e1 = e2; + } +} + +/// Clip polygon vs axis aligned box, inPolygonToClip is assume to be in counter clockwise order. +/// Output will be stored in outClippedPolygon. Everything inside inAABox will be kept. +template +void ClipPolyVsAABox(const VERTEX_ARRAY &inPolygonToClip, const AABox &inAABox, VERTEX_ARRAY &outClippedPolygon) +{ + JPH_ASSERT(inPolygonToClip.size() >= 2); + + VERTEX_ARRAY tmp_vertices[2]; + int tmp_vertices_idx = 0; + + for (int coord = 0; coord < 3; ++coord) + for (int side = 0; side < 2; ++side) + { + // Get plane to clip against + Vec3 origin = Vec3::sZero(), normal = Vec3::sZero(); + if (side == 0) + { + normal.SetComponent(coord, 1.0f); + origin.SetComponent(coord, inAABox.mMin[coord]); + } + else + { + normal.SetComponent(coord, -1.0f); + origin.SetComponent(coord, inAABox.mMax[coord]); + } + + // Get source and target polygon + const VERTEX_ARRAY &src_polygon = tmp_vertices_idx == 0? inPolygonToClip : tmp_vertices[tmp_vertices_idx & 1]; + tmp_vertices_idx++; + VERTEX_ARRAY &tgt_polygon = tmp_vertices_idx == 6? outClippedPolygon : tmp_vertices[tmp_vertices_idx & 1]; + tgt_polygon.clear(); + + // Clip against the edge + ClipPolyVsPlane(src_polygon, origin, normal, tgt_polygon); + + // Break out if no polygon left + if (tgt_polygon.size() < 3) + { + outClippedPolygon.clear(); + return; + } + + // Flip normal + normal = -normal; + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/ClosestPoint.h b/thirdparty/jolt_physics/Jolt/Geometry/ClosestPoint.h new file mode 100644 index 000000000000..a437763f5744 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/ClosestPoint.h @@ -0,0 +1,498 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +// Turn off fused multiply add instruction because it makes the equations of the form a * b - c * d inaccurate below +JPH_PRECISE_MATH_ON + +/// Helper utils to find the closest point to a line segment, triangle or tetrahedron +namespace ClosestPoint +{ + /// Compute barycentric coordinates of closest point to origin for infinite line defined by (inA, inB) + /// Point can then be computed as inA * outU + inB * outV + /// Returns false if the points inA, inB do not form a line (are at the same point) + inline bool GetBaryCentricCoordinates(Vec3Arg inA, Vec3Arg inB, float &outU, float &outV) + { + Vec3 ab = inB - inA; + float denominator = ab.LengthSq(); + if (denominator < Square(FLT_EPSILON)) + { + // Degenerate line segment, fallback to points + if (inA.LengthSq() < inB.LengthSq()) + { + // A closest + outU = 1.0f; + outV = 0.0f; + } + else + { + // B closest + outU = 0.0f; + outV = 1.0f; + } + return false; + } + else + { + outV = -inA.Dot(ab) / denominator; + outU = 1.0f - outV; + } + return true; + } + + /// Compute barycentric coordinates of closest point to origin for plane defined by (inA, inB, inC) + /// Point can then be computed as inA * outU + inB * outV + inC * outW + /// Returns false if the points inA, inB, inC do not form a plane (are on the same line or at the same point) + inline bool GetBaryCentricCoordinates(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, float &outU, float &outV, float &outW) + { + // Taken from: Real-Time Collision Detection - Christer Ericson (Section: Barycentric Coordinates) + // With p = 0 + // Adjusted to always include the shortest edge of the triangle in the calculation to improve numerical accuracy + + // First calculate the three edges + Vec3 v0 = inB - inA; + Vec3 v1 = inC - inA; + Vec3 v2 = inC - inB; + + // Make sure that the shortest edge is included in the calculation to keep the products a * b - c * d as small as possible to preserve accuracy + float d00 = v0.LengthSq(); + float d11 = v1.LengthSq(); + float d22 = v2.LengthSq(); + if (d00 <= d22) + { + // Use v0 and v1 to calculate barycentric coordinates + float d01 = v0.Dot(v1); + + // Denominator must be positive: + // |v0|^2 * |v1|^2 - (v0 . v1)^2 = |v0|^2 * |v1|^2 * (1 - cos(angle)^2) >= 0 + float denominator = d00 * d11 - d01 * d01; + if (denominator < 1.0e-12f) + { + // Degenerate triangle, return coordinates along longest edge + if (d00 > d11) + { + GetBaryCentricCoordinates(inA, inB, outU, outV); + outW = 0.0f; + } + else + { + GetBaryCentricCoordinates(inA, inC, outU, outW); + outV = 0.0f; + } + return false; + } + else + { + float a0 = inA.Dot(v0); + float a1 = inA.Dot(v1); + outV = (d01 * a1 - d11 * a0) / denominator; + outW = (d01 * a0 - d00 * a1) / denominator; + outU = 1.0f - outV - outW; + } + } + else + { + // Use v1 and v2 to calculate barycentric coordinates + float d12 = v1.Dot(v2); + + float denominator = d11 * d22 - d12 * d12; + if (denominator < 1.0e-12f) + { + // Degenerate triangle, return coordinates along longest edge + if (d11 > d22) + { + GetBaryCentricCoordinates(inA, inC, outU, outW); + outV = 0.0f; + } + else + { + GetBaryCentricCoordinates(inB, inC, outV, outW); + outU = 0.0f; + } + return false; + } + else + { + float c1 = inC.Dot(v1); + float c2 = inC.Dot(v2); + outU = (d22 * c1 - d12 * c2) / denominator; + outV = (d11 * c2 - d12 * c1) / denominator; + outW = 1.0f - outU - outV; + } + } + return true; + } + + /// Get the closest point to the origin of line (inA, inB) + /// outSet describes which features are closest: 1 = a, 2 = b, 3 = line segment ab + inline Vec3 GetClosestPointOnLine(Vec3Arg inA, Vec3Arg inB, uint32 &outSet) + { + float u, v; + GetBaryCentricCoordinates(inA, inB, u, v); + if (v <= 0.0f) + { + // inA is closest point + outSet = 0b0001; + return inA; + } + else if (u <= 0.0f) + { + // inB is closest point + outSet = 0b0010; + return inB; + } + else + { + // Closest point lies on line inA inB + outSet = 0b0011; + return u * inA + v * inB; + } + } + + /// Get the closest point to the origin of triangle (inA, inB, inC) + /// outSet describes which features are closest: 1 = a, 2 = b, 4 = c, 5 = line segment ac, 7 = triangle interior etc. + /// If MustIncludeC is true, the function assumes that C is part of the closest feature (vertex, edge, face) and does less work, if the assumption is not true then a closest point to the other features is returned. + template + inline Vec3 GetClosestPointOnTriangle(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, uint32 &outSet) + { + // Taken from: Real-Time Collision Detection - Christer Ericson (Section: Closest Point on Triangle to Point) + // With p = 0 + + // The most accurate normal is calculated by using the two shortest edges + // See: https://box2d.org/posts/2014/01/troublesome-triangle/ + // The difference in normals is most pronounced when one edge is much smaller than the others (in which case the other 2 must have roughly the same length). + // Therefore we can suffice by just picking the shortest from 2 edges and use that with the 3rd edge to calculate the normal. + // We first check which of the edges is shorter and if bc is shorter than ac then we swap a with c to a is always on the shortest edge + UVec4 swap_ac; + { + Vec3 ac = inC - inA; + Vec3 bc = inC - inB; + swap_ac = Vec4::sLess(bc.DotV4(bc), ac.DotV4(ac)); + } + Vec3 a = Vec3::sSelect(inA, inC, swap_ac); + Vec3 c = Vec3::sSelect(inC, inA, swap_ac); + + // Calculate normal + Vec3 ab = inB - a; + Vec3 ac = c - a; + Vec3 n = ab.Cross(ac); + float n_len_sq = n.LengthSq(); + + // Check degenerate + if (n_len_sq < 1.0e-10f) // Square(FLT_EPSILON) was too small and caused numerical problems, see test case TestCollideParallelTriangleVsCapsule + { + // Degenerate, fallback to vertices and edges + + // Start with vertex C being the closest + uint32 closest_set = 0b0100; + Vec3 closest_point = inC; + float best_dist_sq = inC.LengthSq(); + + // If the closest point must include C then A or B cannot be closest + // Note that we test vertices first because we want to prefer a closest vertex over a closest edge (this results in an outSet with fewer bits set) + if constexpr (!MustIncludeC) + { + // Try vertex A + float a_len_sq = inA.LengthSq(); + if (a_len_sq < best_dist_sq) + { + closest_set = 0b0001; + closest_point = inA; + best_dist_sq = a_len_sq; + } + + // Try vertex B + float b_len_sq = inB.LengthSq(); + if (b_len_sq < best_dist_sq) + { + closest_set = 0b0010; + closest_point = inB; + best_dist_sq = b_len_sq; + } + } + + // Edge AC + float ac_len_sq = ac.LengthSq(); + if (ac_len_sq > Square(FLT_EPSILON)) + { + float v = Clamp(-a.Dot(ac) / ac_len_sq, 0.0f, 1.0f); + Vec3 q = a + v * ac; + float dist_sq = q.LengthSq(); + if (dist_sq < best_dist_sq) + { + closest_set = 0b0101; + closest_point = q; + best_dist_sq = dist_sq; + } + } + + // Edge BC + Vec3 bc = inC - inB; + float bc_len_sq = bc.LengthSq(); + if (bc_len_sq > Square(FLT_EPSILON)) + { + float v = Clamp(-inB.Dot(bc) / bc_len_sq, 0.0f, 1.0f); + Vec3 q = inB + v * bc; + float dist_sq = q.LengthSq(); + if (dist_sq < best_dist_sq) + { + closest_set = 0b0110; + closest_point = q; + best_dist_sq = dist_sq; + } + } + + // If the closest point must include C then AB cannot be closest + if constexpr (!MustIncludeC) + { + // Edge AB + ab = inB - inA; + float ab_len_sq = ab.LengthSq(); + if (ab_len_sq > Square(FLT_EPSILON)) + { + float v = Clamp(-inA.Dot(ab) / ab_len_sq, 0.0f, 1.0f); + Vec3 q = inA + v * ab; + float dist_sq = q.LengthSq(); + if (dist_sq < best_dist_sq) + { + closest_set = 0b0011; + closest_point = q; + best_dist_sq = dist_sq; + } + } + } + + outSet = closest_set; + return closest_point; + } + + // Check if P in vertex region outside A + Vec3 ap = -a; + float d1 = ab.Dot(ap); + float d2 = ac.Dot(ap); + if (d1 <= 0.0f && d2 <= 0.0f) + { + outSet = swap_ac.GetX()? 0b0100 : 0b0001; + return a; // barycentric coordinates (1,0,0) + } + + // Check if P in vertex region outside B + Vec3 bp = -inB; + float d3 = ab.Dot(bp); + float d4 = ac.Dot(bp); + if (d3 >= 0.0f && d4 <= d3) + { + outSet = 0b0010; + return inB; // barycentric coordinates (0,1,0) + } + + // Check if P in edge region of AB, if so return projection of P onto AB + if (d1 * d4 <= d3 * d2 && d1 >= 0.0f && d3 <= 0.0f) + { + float v = d1 / (d1 - d3); + outSet = swap_ac.GetX()? 0b0110 : 0b0011; + return a + v * ab; // barycentric coordinates (1-v,v,0) + } + + // Check if P in vertex region outside C + Vec3 cp = -c; + float d5 = ab.Dot(cp); + float d6 = ac.Dot(cp); + if (d6 >= 0.0f && d5 <= d6) + { + outSet = swap_ac.GetX()? 0b0001 : 0b0100; + return c; // barycentric coordinates (0,0,1) + } + + // Check if P in edge region of AC, if so return projection of P onto AC + if (d5 * d2 <= d1 * d6 && d2 >= 0.0f && d6 <= 0.0f) + { + float w = d2 / (d2 - d6); + outSet = 0b0101; + return a + w * ac; // barycentric coordinates (1-w,0,w) + } + + // Check if P in edge region of BC, if so return projection of P onto BC + float d4_d3 = d4 - d3; + float d5_d6 = d5 - d6; + if (d3 * d6 <= d5 * d4 && d4_d3 >= 0.0f && d5_d6 >= 0.0f) + { + float w = d4_d3 / (d4_d3 + d5_d6); + outSet = swap_ac.GetX()? 0b0011 : 0b0110; + return inB + w * (c - inB); // barycentric coordinates (0,1-w,w) + } + + // P inside face region. + // Here we deviate from Christer Ericson's article to improve accuracy. + // Determine distance between triangle and origin: distance = (centroid - origin) . normal / |normal| + // Closest point to origin is then: distance . normal / |normal| + // Note that this way of calculating the closest point is much more accurate than first calculating barycentric coordinates + // and then calculating the closest point based on those coordinates. + outSet = 0b0111; + return n * (a + inB + c).Dot(n) / (3.0f * n_len_sq); + } + + /// Check if the origin is outside the plane of triangle (inA, inB, inC). inD specifies the front side of the plane. + inline bool OriginOutsideOfPlane(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inD) + { + // Taken from: Real-Time Collision Detection - Christer Ericson (Section: Closest Point on Tetrahedron to Point) + // With p = 0 + + // Test if point p and d lie on opposite sides of plane through abc + Vec3 n = (inB - inA).Cross(inC - inA); + float signp = inA.Dot(n); // [AP AB AC] + float signd = (inD - inA).Dot(n); // [AD AB AC] + + // Points on opposite sides if expression signs are the same + // Note that we left out the minus sign in signp so we need to check > 0 instead of < 0 as in Christer's book + // We compare against a small negative value to allow for a little bit of slop in the calculations + return signp * signd > -FLT_EPSILON; + } + + /// Returns for each of the planes of the tetrahedron if the origin is inside it + /// Roughly equivalent to: + /// [OriginOutsideOfPlane(inA, inB, inC, inD), + /// OriginOutsideOfPlane(inA, inC, inD, inB), + /// OriginOutsideOfPlane(inA, inD, inB, inC), + /// OriginOutsideOfPlane(inB, inD, inC, inA)] + inline UVec4 OriginOutsideOfTetrahedronPlanes(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inD) + { + Vec3 ab = inB - inA; + Vec3 ac = inC - inA; + Vec3 ad = inD - inA; + Vec3 bd = inD - inB; + Vec3 bc = inC - inB; + + Vec3 ab_cross_ac = ab.Cross(ac); + Vec3 ac_cross_ad = ac.Cross(ad); + Vec3 ad_cross_ab = ad.Cross(ab); + Vec3 bd_cross_bc = bd.Cross(bc); + + // For each plane get the side on which the origin is + float signp0 = inA.Dot(ab_cross_ac); // ABC + float signp1 = inA.Dot(ac_cross_ad); // ACD + float signp2 = inA.Dot(ad_cross_ab); // ADB + float signp3 = inB.Dot(bd_cross_bc); // BDC + Vec4 signp(signp0, signp1, signp2, signp3); + + // For each plane get the side that is outside (determined by the 4th point) + float signd0 = ad.Dot(ab_cross_ac); // D + float signd1 = ab.Dot(ac_cross_ad); // B + float signd2 = ac.Dot(ad_cross_ab); // C + float signd3 = -ab.Dot(bd_cross_bc); // A + Vec4 signd(signd0, signd1, signd2, signd3); + + // The winding of all triangles has been chosen so that signd should have the + // same sign for all components. If this is not the case the tetrahedron + // is degenerate and we return that the origin is in front of all sides + int sign_bits = signd.GetSignBits(); + switch (sign_bits) + { + case 0: + // All positive + return Vec4::sGreaterOrEqual(signp, Vec4::sReplicate(-FLT_EPSILON)); + + case 0xf: + // All negative + return Vec4::sLessOrEqual(signp, Vec4::sReplicate(FLT_EPSILON)); + + default: + // Mixed signs, degenerate tetrahedron + return UVec4::sReplicate(0xffffffff); + } + } + + /// Get the closest point between tetrahedron (inA, inB, inC, inD) to the origin + /// outSet specifies which feature was closest, 1 = a, 2 = b, 4 = c, 8 = d. Edges have 2 bits set, triangles 3 and if the point is in the interior 4 bits are set. + /// If MustIncludeD is true, the function assumes that D is part of the closest feature (vertex, edge, face, tetrahedron) and does less work, if the assumption is not true then a closest point to the other features is returned. + template + inline Vec3 GetClosestPointOnTetrahedron(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inD, uint32 &outSet) + { + // Taken from: Real-Time Collision Detection - Christer Ericson (Section: Closest Point on Tetrahedron to Point) + // With p = 0 + + // Start out assuming point inside all halfspaces, so closest to itself + uint32 closest_set = 0b1111; + Vec3 closest_point = Vec3::sZero(); + float best_dist_sq = FLT_MAX; + + // Determine for each of the faces of the tetrahedron if the origin is in front of the plane + UVec4 origin_out_of_planes = OriginOutsideOfTetrahedronPlanes(inA, inB, inC, inD); + + // If point outside face abc then compute closest point on abc + if (origin_out_of_planes.GetX()) // OriginOutsideOfPlane(inA, inB, inC, inD) + { + if constexpr (MustIncludeD) + { + // If the closest point must include D then ABC cannot be closest but the closest point + // cannot be an interior point either so we return A as closest point + closest_set = 0b0001; + closest_point = inA; + } + else + { + // Test the face normally + closest_point = GetClosestPointOnTriangle(inA, inB, inC, closest_set); + } + best_dist_sq = closest_point.LengthSq(); + } + + // Repeat test for face acd + if (origin_out_of_planes.GetY()) // OriginOutsideOfPlane(inA, inC, inD, inB) + { + uint32 set; + Vec3 q = GetClosestPointOnTriangle(inA, inC, inD, set); + float dist_sq = q.LengthSq(); + if (dist_sq < best_dist_sq) + { + best_dist_sq = dist_sq; + closest_point = q; + closest_set = (set & 0b0001) + ((set & 0b0110) << 1); + } + } + + // Repeat test for face adb + if (origin_out_of_planes.GetZ()) // OriginOutsideOfPlane(inA, inD, inB, inC) + { + // Keep original vertex order, it doesn't matter if the triangle is facing inward or outward + // and it improves consistency for GJK which will always add a new vertex D and keep the closest + // feature from the previous iteration in ABC + uint32 set; + Vec3 q = GetClosestPointOnTriangle(inA, inB, inD, set); + float dist_sq = q.LengthSq(); + if (dist_sq < best_dist_sq) + { + best_dist_sq = dist_sq; + closest_point = q; + closest_set = (set & 0b0011) + ((set & 0b0100) << 1); + } + } + + // Repeat test for face bdc + if (origin_out_of_planes.GetW()) // OriginOutsideOfPlane(inB, inD, inC, inA) + { + // Keep original vertex order, it doesn't matter if the triangle is facing inward or outward + // and it improves consistency for GJK which will always add a new vertex D and keep the closest + // feature from the previous iteration in ABC + uint32 set; + Vec3 q = GetClosestPointOnTriangle(inB, inC, inD, set); + float dist_sq = q.LengthSq(); + if (dist_sq < best_dist_sq) + { + closest_point = q; + closest_set = set << 1; + } + } + + outSet = closest_set; + return closest_point; + } +}; + +JPH_PRECISE_MATH_OFF + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder.cpp b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder.cpp new file mode 100644 index 000000000000..e15c0912ffd2 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder.cpp @@ -0,0 +1,1467 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include + +#ifdef JPH_CONVEX_BUILDER_DUMP_SHAPE +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END +#endif // JPH_CONVEX_BUILDER_DUMP_SHAPE + +#ifdef JPH_CONVEX_BUILDER_DEBUG + #include +#endif + +JPH_NAMESPACE_BEGIN + +ConvexHullBuilder::Face::~Face() +{ + // Free all edges + Edge *e = mFirstEdge; + if (e != nullptr) + { + do + { + Edge *next = e->mNextEdge; + delete e; + e = next; + } while (e != mFirstEdge); + } +} + +void ConvexHullBuilder::Face::CalculateNormalAndCentroid(const Vec3 *inPositions) +{ + // Get point that we use to construct a triangle fan + Edge *e = mFirstEdge; + Vec3 y0 = inPositions[e->mStartIdx]; + + // Get the 2nd point + e = e->mNextEdge; + Vec3 y1 = inPositions[e->mStartIdx]; + + // Start accumulating the centroid + mCentroid = y0 + y1; + int n = 2; + + // Start accumulating the normal + mNormal = Vec3::sZero(); + + // Loop over remaining edges accumulating normals in a triangle fan fashion + for (e = e->mNextEdge; e != mFirstEdge; e = e->mNextEdge) + { + // Get the 3rd point + Vec3 y2 = inPositions[e->mStartIdx]; + + // Calculate edges (counter clockwise) + Vec3 e0 = y1 - y0; + Vec3 e1 = y2 - y1; + Vec3 e2 = y0 - y2; + + // The best normal is calculated by using the two shortest edges + // See: https://box2d.org/posts/2014/01/troublesome-triangle/ + // The difference in normals is most pronounced when one edge is much smaller than the others (in which case the others must have roughly the same length). + // Therefore we can suffice by just picking the shortest from 2 edges and use that with the 3rd edge to calculate the normal. + // We first check which of the edges is shorter: e1 or e2 + UVec4 e1_shorter_than_e2 = Vec4::sLess(e1.DotV4(e1), e2.DotV4(e2)); + + // We calculate both normals and then select the one that had the shortest edge for our normal (this avoids branching) + Vec3 normal_e01 = e0.Cross(e1); + Vec3 normal_e02 = e2.Cross(e0); + mNormal += Vec3::sSelect(normal_e02, normal_e01, e1_shorter_than_e2); + + // Accumulate centroid + mCentroid += y2; + n++; + + // Update y1 for next triangle + y1 = y2; + } + + // Finalize centroid + mCentroid /= float(n); +} + +void ConvexHullBuilder::Face::Initialize(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions) +{ + JPH_ASSERT(mFirstEdge == nullptr); + JPH_ASSERT(inIdx0 != inIdx1 && inIdx0 != inIdx2 && inIdx1 != inIdx2); + + // Create 3 edges + Edge *e0 = new Edge(this, inIdx0); + Edge *e1 = new Edge(this, inIdx1); + Edge *e2 = new Edge(this, inIdx2); + + // Link edges + e0->mNextEdge = e1; + e1->mNextEdge = e2; + e2->mNextEdge = e0; + mFirstEdge = e0; + + CalculateNormalAndCentroid(inPositions); +} + +ConvexHullBuilder::ConvexHullBuilder(const Positions &inPositions) : + mPositions(inPositions) +{ +#ifdef JPH_CONVEX_BUILDER_DEBUG + mIteration = 0; + + // Center the drawing of the first hull around the origin and calculate the delta offset between states + mOffset = RVec3::sZero(); + if (mPositions.empty()) + { + // No hull will be generated + mDelta = Vec3::sZero(); + } + else + { + Vec3 maxv = Vec3::sReplicate(-FLT_MAX), minv = Vec3::sReplicate(FLT_MAX); + for (Vec3 v : mPositions) + { + minv = Vec3::sMin(minv, v); + maxv = Vec3::sMax(maxv, v); + mOffset -= v; + } + mOffset /= Real(mPositions.size()); + mDelta = Vec3((maxv - minv).GetX() + 0.5f, 0, 0); + mOffset += mDelta; // Don't start at origin, we're already drawing the final hull there + } +#endif +} + +void ConvexHullBuilder::FreeFaces() +{ + for (Face *f : mFaces) + delete f; + mFaces.clear(); +} + +void ConvexHullBuilder::GetFaceForPoint(Vec3Arg inPoint, const Faces &inFaces, Face *&outFace, float &outDistSq) const +{ + outFace = nullptr; + outDistSq = 0.0f; + + for (Face *f : inFaces) + if (!f->mRemoved) + { + // Determine distance to face + float dot = f->mNormal.Dot(inPoint - f->mCentroid); + if (dot > 0.0f) + { + float dist_sq = dot * dot / f->mNormal.LengthSq(); + if (dist_sq > outDistSq) + { + outFace = f; + outDistSq = dist_sq; + } + } + } +} + +float ConvexHullBuilder::GetDistanceToEdgeSq(Vec3Arg inPoint, const Face *inFace) const +{ + bool all_inside = true; + float edge_dist_sq = FLT_MAX; + + // Test if it is inside the edges of the polygon + Edge *edge = inFace->mFirstEdge; + Vec3 p1 = mPositions[edge->GetPreviousEdge()->mStartIdx]; + do + { + Vec3 p2 = mPositions[edge->mStartIdx]; + if ((p2 - p1).Cross(inPoint - p1).Dot(inFace->mNormal) < 0.0f) + { + // It is outside + all_inside = false; + + // Measure distance to this edge + uint32 s; + edge_dist_sq = min(edge_dist_sq, ClosestPoint::GetClosestPointOnLine(p1 - inPoint, p2 - inPoint, s).LengthSq()); + } + p1 = p2; + edge = edge->mNextEdge; + } while (edge != inFace->mFirstEdge); + + return all_inside? 0.0f : edge_dist_sq; +} + +bool ConvexHullBuilder::AssignPointToFace(int inPositionIdx, const Faces &inFaces, float inToleranceSq) +{ + Vec3 point = mPositions[inPositionIdx]; + + // Find the face for which the point is furthest away + Face *best_face; + float best_dist_sq; + GetFaceForPoint(point, inFaces, best_face, best_dist_sq); + + if (best_face != nullptr) + { + // Check if this point is within the tolerance margin to the plane + if (best_dist_sq <= inToleranceSq) + { + // Check distance to edges + float dist_to_edge_sq = GetDistanceToEdgeSq(point, best_face); + if (dist_to_edge_sq > inToleranceSq) + { + // Point is outside of the face and too far away to discard + mCoplanarList.push_back({ inPositionIdx, dist_to_edge_sq }); + } + } + else + { + // This point is in front of the face, add it to the conflict list + if (best_dist_sq > best_face->mFurthestPointDistanceSq) + { + // This point is further away than any others, update the distance and add point as last point + best_face->mFurthestPointDistanceSq = best_dist_sq; + best_face->mConflictList.push_back(inPositionIdx); + } + else + { + // Not the furthest point, add it as the before last point + best_face->mConflictList.insert(best_face->mConflictList.begin() + best_face->mConflictList.size() - 1, inPositionIdx); + } + + return true; + } + } + + return false; +} + +float ConvexHullBuilder::DetermineCoplanarDistance() const +{ + // Formula as per: Implementing Quickhull - Dirk Gregorius. + Vec3 vmax = Vec3::sZero(); + for (Vec3 v : mPositions) + vmax = Vec3::sMax(vmax, v.Abs()); + return 3.0f * FLT_EPSILON * (vmax.GetX() + vmax.GetY() + vmax.GetZ()); +} + +int ConvexHullBuilder::GetNumVerticesUsed() const +{ + UnorderedSet used_verts; + used_verts.reserve(UnorderedSet::size_type(mPositions.size())); + for (Face *f : mFaces) + { + Edge *e = f->mFirstEdge; + do + { + used_verts.insert(e->mStartIdx); + e = e->mNextEdge; + } while (e != f->mFirstEdge); + } + return (int)used_verts.size(); +} + +bool ConvexHullBuilder::ContainsFace(const Array &inIndices) const +{ + for (Face *f : mFaces) + { + Edge *e = f->mFirstEdge; + Array::const_iterator index = std::find(inIndices.begin(), inIndices.end(), e->mStartIdx); + if (index != inIndices.end()) + { + size_t matches = 0; + + do + { + // Check if index matches + if (*index != e->mStartIdx) + break; + + // Increment number of matches + matches++; + + // Next index in list of inIndices + index++; + if (index == inIndices.end()) + index = inIndices.begin(); + + // Next edge + e = e->mNextEdge; + } while (e != f->mFirstEdge); + + if (matches == inIndices.size()) + return true; + } + } + + return false; +} + +ConvexHullBuilder::EResult ConvexHullBuilder::Initialize(int inMaxVertices, float inTolerance, const char *&outError) +{ + // Free the faces possibly left over from an earlier hull + FreeFaces(); + + // Test that we have at least 3 points + if (mPositions.size() < 3) + { + outError = "Need at least 3 points to make a hull"; + return EResult::TooFewPoints; + } + + // Determine a suitable tolerance for detecting that points are coplanar + float coplanar_tolerance_sq = Square(DetermineCoplanarDistance()); + + // Increase desired tolerance if accuracy doesn't allow it + float tolerance_sq = max(coplanar_tolerance_sq, Square(inTolerance)); + + // Find point furthest from the origin + int idx1 = -1; + float max_dist_sq = -1.0f; + for (int i = 0; i < (int)mPositions.size(); ++i) + { + float dist_sq = mPositions[i].LengthSq(); + if (dist_sq > max_dist_sq) + { + max_dist_sq = dist_sq; + idx1 = i; + } + } + JPH_ASSERT(idx1 >= 0); + + // Find point that is furthest away from this point + int idx2 = -1; + max_dist_sq = -1.0f; + for (int i = 0; i < (int)mPositions.size(); ++i) + if (i != idx1) + { + float dist_sq = (mPositions[i] - mPositions[idx1]).LengthSq(); + if (dist_sq > max_dist_sq) + { + max_dist_sq = dist_sq; + idx2 = i; + } + } + JPH_ASSERT(idx2 >= 0); + + // Find point that forms the biggest triangle + int idx3 = -1; + float best_triangle_area_sq = -1.0f; + for (int i = 0; i < (int)mPositions.size(); ++i) + if (i != idx1 && i != idx2) + { + float triangle_area_sq = (mPositions[idx1] - mPositions[i]).Cross(mPositions[idx2] - mPositions[i]).LengthSq(); + if (triangle_area_sq > best_triangle_area_sq) + { + best_triangle_area_sq = triangle_area_sq; + idx3 = i; + } + } + JPH_ASSERT(idx3 >= 0); + if (best_triangle_area_sq < cMinTriangleAreaSq) + { + outError = "Could not find a suitable initial triangle because its area was too small"; + return EResult::Degenerate; + } + + // Check if we have only 3 vertices + if (mPositions.size() == 3) + { + // Create two triangles (back to back) + Face *t1 = CreateTriangle(idx1, idx2, idx3); + Face *t2 = CreateTriangle(idx1, idx3, idx2); + + // Link faces edges + sLinkFace(t1->mFirstEdge, t2->mFirstEdge->mNextEdge->mNextEdge); + sLinkFace(t1->mFirstEdge->mNextEdge, t2->mFirstEdge->mNextEdge); + sLinkFace(t1->mFirstEdge->mNextEdge->mNextEdge, t2->mFirstEdge); + +#ifdef JPH_CONVEX_BUILDER_DEBUG + // Draw current state + DrawState(); +#endif + + return EResult::Success; + } + + // Find point that forms the biggest tetrahedron + Vec3 initial_plane_normal = (mPositions[idx2] - mPositions[idx1]).Cross(mPositions[idx3] - mPositions[idx1]).Normalized(); + Vec3 initial_plane_centroid = (mPositions[idx1] + mPositions[idx2] + mPositions[idx3]) / 3.0f; + int idx4 = -1; + float max_dist = 0.0f; + for (int i = 0; i < (int)mPositions.size(); ++i) + if (i != idx1 && i != idx2 && i != idx3) + { + float dist = (mPositions[i] - initial_plane_centroid).Dot(initial_plane_normal); + if (abs(dist) > abs(max_dist)) + { + max_dist = dist; + idx4 = i; + } + } + + // Check if the hull is coplanar + if (Square(max_dist) <= 25.0f * coplanar_tolerance_sq) + { + // First project all points in 2D space + Vec3 base1 = initial_plane_normal.GetNormalizedPerpendicular(); + Vec3 base2 = initial_plane_normal.Cross(base1); + Array positions_2d; + positions_2d.reserve(mPositions.size()); + for (Vec3 v : mPositions) + positions_2d.emplace_back(base1.Dot(v), base2.Dot(v), 0.0f); + + // Build hull + Array edges_2d; + ConvexHullBuilder2D builder_2d(positions_2d); + ConvexHullBuilder2D::EResult result = builder_2d.Initialize(idx1, idx2, idx3, inMaxVertices, inTolerance, edges_2d); + + // Create faces (back to back) + Face *f1 = CreateFace(); + Face *f2 = CreateFace(); + + // Create edges for face 1 + Array edges_f1; + edges_f1.reserve(edges_2d.size()); + for (int start_idx : edges_2d) + { + Edge *edge = new Edge(f1, start_idx); + if (edges_f1.empty()) + f1->mFirstEdge = edge; + else + edges_f1.back()->mNextEdge = edge; + edges_f1.push_back(edge); + } + edges_f1.back()->mNextEdge = f1->mFirstEdge; + + // Create edges for face 2 + Array edges_f2; + edges_f2.reserve(edges_2d.size()); + for (int i = (int)edges_2d.size() - 1; i >= 0; --i) + { + Edge *edge = new Edge(f2, edges_2d[i]); + if (edges_f2.empty()) + f2->mFirstEdge = edge; + else + edges_f2.back()->mNextEdge = edge; + edges_f2.push_back(edge); + } + edges_f2.back()->mNextEdge = f2->mFirstEdge; + + // Link edges + for (size_t i = 0; i < edges_2d.size(); ++i) + sLinkFace(edges_f1[i], edges_f2[(2 * edges_2d.size() - 2 - i) % edges_2d.size()]); + + // Calculate the plane for both faces + f1->CalculateNormalAndCentroid(mPositions.data()); + f2->mNormal = -f1->mNormal; + f2->mCentroid = f1->mCentroid; + +#ifdef JPH_CONVEX_BUILDER_DEBUG + // Draw current state + DrawState(); +#endif + + return result == ConvexHullBuilder2D::EResult::MaxVerticesReached? EResult::MaxVerticesReached : EResult::Success; + } + + // Ensure the planes are facing outwards + if (max_dist < 0.0f) + std::swap(idx2, idx3); + + // Create tetrahedron + Face *t1 = CreateTriangle(idx1, idx2, idx4); + Face *t2 = CreateTriangle(idx2, idx3, idx4); + Face *t3 = CreateTriangle(idx3, idx1, idx4); + Face *t4 = CreateTriangle(idx1, idx3, idx2); + + // Link face edges + sLinkFace(t1->mFirstEdge, t4->mFirstEdge->mNextEdge->mNextEdge); + sLinkFace(t1->mFirstEdge->mNextEdge, t2->mFirstEdge->mNextEdge->mNextEdge); + sLinkFace(t1->mFirstEdge->mNextEdge->mNextEdge, t3->mFirstEdge->mNextEdge); + sLinkFace(t2->mFirstEdge, t4->mFirstEdge->mNextEdge); + sLinkFace(t2->mFirstEdge->mNextEdge, t3->mFirstEdge->mNextEdge->mNextEdge); + sLinkFace(t3->mFirstEdge, t4->mFirstEdge); + + // Build the initial conflict lists + Faces faces { t1, t2, t3, t4 }; + for (int idx = 0; idx < (int)mPositions.size(); ++idx) + if (idx != idx1 && idx != idx2 && idx != idx3 && idx != idx4) + AssignPointToFace(idx, faces, tolerance_sq); + +#ifdef JPH_CONVEX_BUILDER_DEBUG + // Draw current state including conflict list + DrawState(true); + + // Increment iteration counter + ++mIteration; +#endif + + // Overestimate of the actual amount of vertices we use, for limiting the amount of vertices in the hull + int num_vertices_used = 4; + + // Loop through the remainder of the points and add them + for (;;) + { + // Find the face with the furthest point on it + Face *face_with_furthest_point = nullptr; + float furthest_dist_sq = 0.0f; + for (Face *f : mFaces) + if (f->mFurthestPointDistanceSq > furthest_dist_sq) + { + furthest_dist_sq = f->mFurthestPointDistanceSq; + face_with_furthest_point = f; + } + + int furthest_point_idx; + if (face_with_furthest_point != nullptr) + { + // Take the furthest point + furthest_point_idx = face_with_furthest_point->mConflictList.back(); + face_with_furthest_point->mConflictList.pop_back(); + } + else if (!mCoplanarList.empty()) + { + // Try to assign points to faces (this also recalculates the distance to the hull for the coplanar vertices) + CoplanarList coplanar; + mCoplanarList.swap(coplanar); + bool added = false; + for (const Coplanar &c : coplanar) + added |= AssignPointToFace(c.mPositionIdx, mFaces, tolerance_sq); + + // If we were able to assign a point, loop again to pick it up + if (added) + continue; + + // If the coplanar list is empty, there are no points left and we're done + if (mCoplanarList.empty()) + break; + + do + { + // Find the vertex that is furthest from the hull + CoplanarList::size_type best_idx = 0; + float best_dist_sq = mCoplanarList.front().mDistanceSq; + for (CoplanarList::size_type idx = 1; idx < mCoplanarList.size(); ++idx) + { + const Coplanar &c = mCoplanarList[idx]; + if (c.mDistanceSq > best_dist_sq) + { + best_idx = idx; + best_dist_sq = c.mDistanceSq; + } + } + + // Swap it to the end + std::swap(mCoplanarList[best_idx], mCoplanarList.back()); + + // Remove it + furthest_point_idx = mCoplanarList.back().mPositionIdx; + mCoplanarList.pop_back(); + + // Find the face for which the point is furthest away + GetFaceForPoint(mPositions[furthest_point_idx], mFaces, face_with_furthest_point, best_dist_sq); + } while (!mCoplanarList.empty() && face_with_furthest_point == nullptr); + + if (face_with_furthest_point == nullptr) + break; + } + else + { + // If there are no more vertices, we're done + break; + } + + // Check if we have a limit on the max vertices that we should produce + if (num_vertices_used >= inMaxVertices) + { + // Count the actual amount of used vertices (we did not take the removal of any vertices into account) + num_vertices_used = GetNumVerticesUsed(); + + // Check if there are too many + if (num_vertices_used >= inMaxVertices) + return EResult::MaxVerticesReached; + } + + // We're about to add another vertex + ++num_vertices_used; + + // Add the point to the hull + Faces new_faces; + AddPoint(face_with_furthest_point, furthest_point_idx, coplanar_tolerance_sq, new_faces); + + // Redistribute points on conflict lists belonging to removed faces + for (const Face *face : mFaces) + if (face->mRemoved) + for (int idx : face->mConflictList) + AssignPointToFace(idx, new_faces, tolerance_sq); + + // Permanently delete faces that we removed in AddPoint() + GarbageCollectFaces(); + +#ifdef JPH_CONVEX_BUILDER_DEBUG + // Draw state at the end of this step including conflict list + DrawState(true); + + // Increment iteration counter + ++mIteration; +#endif + } + + // Check if we are left with a hull. It is possible that hull building fails if the points are nearly coplanar. + if (mFaces.size() < 2) + { + outError = "Too few faces in hull"; + return EResult::TooFewFaces; + } + + return EResult::Success; +} + +void ConvexHullBuilder::AddPoint(Face *inFacingFace, int inIdx, float inCoplanarToleranceSq, Faces &outNewFaces) +{ + // Get position + Vec3 pos = mPositions[inIdx]; + +#ifdef JPH_CONVEX_BUILDER_DEBUG + // Draw point to be added + DebugRenderer::sInstance->DrawMarker(cDrawScale * (mOffset + pos), Color::sYellow, 0.1f); + DebugRenderer::sInstance->DrawText3D(cDrawScale * (mOffset + pos), ConvertToString(inIdx), Color::sWhite); +#endif + +#ifdef JPH_ENABLE_ASSERTS + // Check if structure is intact + ValidateFaces(); +#endif + + // Find edge of convex hull of faces that are not facing the new vertex + FullEdges edges; + FindEdge(inFacingFace, pos, edges); + JPH_ASSERT(edges.size() >= 3); + + // Create new faces + outNewFaces.reserve(edges.size()); + for (const FullEdge &e : edges) + { + JPH_ASSERT(e.mStartIdx != e.mEndIdx); + Face *f = CreateTriangle(e.mStartIdx, e.mEndIdx, inIdx); + outNewFaces.push_back(f); + } + + // Link edges + for (Faces::size_type i = 0; i < outNewFaces.size(); ++i) + { + sLinkFace(outNewFaces[i]->mFirstEdge, edges[i].mNeighbourEdge); + sLinkFace(outNewFaces[i]->mFirstEdge->mNextEdge, outNewFaces[(i + 1) % outNewFaces.size()]->mFirstEdge->mNextEdge->mNextEdge); + } + + // Loop on faces that were modified until nothing needs to be checked anymore + Faces affected_faces = outNewFaces; + while (!affected_faces.empty()) + { + // Take the next face + Face *face = affected_faces.back(); + affected_faces.pop_back(); + + if (!face->mRemoved) + { + // Merge with neighbour if this is a degenerate face + MergeDegenerateFace(face, affected_faces); + + // Merge with coplanar neighbours (or when the neighbour forms a concave edge) + if (!face->mRemoved) + MergeCoplanarOrConcaveFaces(face, inCoplanarToleranceSq, affected_faces); + } + } + +#ifdef JPH_ENABLE_ASSERTS + // Check if structure is intact + ValidateFaces(); +#endif +} + +void ConvexHullBuilder::GarbageCollectFaces() +{ + for (int i = (int)mFaces.size() - 1; i >= 0; --i) + { + Face *f = mFaces[i]; + if (f->mRemoved) + { + FreeFace(f); + mFaces.erase(mFaces.begin() + i); + } + } +} + +ConvexHullBuilder::Face *ConvexHullBuilder::CreateFace() +{ + // Call provider to create face + Face *f = new Face(); + +#ifdef JPH_CONVEX_BUILDER_DEBUG + // Remember iteration counter + f->mIteration = mIteration; +#endif + + // Add to list + mFaces.push_back(f); + return f; +} + +ConvexHullBuilder::Face *ConvexHullBuilder::CreateTriangle(int inIdx1, int inIdx2, int inIdx3) +{ + Face *f = CreateFace(); + f->Initialize(inIdx1, inIdx2, inIdx3, mPositions.data()); + return f; +} + +void ConvexHullBuilder::FreeFace(Face *inFace) +{ + JPH_ASSERT(inFace->mRemoved); + +#ifdef JPH_ENABLE_ASSERTS + // Make sure that this face is not connected + Edge *e = inFace->mFirstEdge; + if (e != nullptr) + do + { + JPH_ASSERT(e->mNeighbourEdge == nullptr); + e = e->mNextEdge; + } while (e != inFace->mFirstEdge); +#endif + + // Free the face + delete inFace; +} + +void ConvexHullBuilder::sLinkFace(Edge *inEdge1, Edge *inEdge2) +{ + // Check not connected yet + JPH_ASSERT(inEdge1->mNeighbourEdge == nullptr); + JPH_ASSERT(inEdge2->mNeighbourEdge == nullptr); + JPH_ASSERT(inEdge1->mFace != inEdge2->mFace); + + // Check vertices match + JPH_ASSERT(inEdge1->mStartIdx == inEdge2->mNextEdge->mStartIdx); + JPH_ASSERT(inEdge2->mStartIdx == inEdge1->mNextEdge->mStartIdx); + + // Link up + inEdge1->mNeighbourEdge = inEdge2; + inEdge2->mNeighbourEdge = inEdge1; +} + +void ConvexHullBuilder::sUnlinkFace(Face *inFace) +{ + // Unlink from neighbours + Edge *e = inFace->mFirstEdge; + do + { + if (e->mNeighbourEdge != nullptr) + { + // Validate that neighbour points to us + JPH_ASSERT(e->mNeighbourEdge->mNeighbourEdge == e); + + // Unlink + e->mNeighbourEdge->mNeighbourEdge = nullptr; + e->mNeighbourEdge = nullptr; + } + e = e->mNextEdge; + } while (e != inFace->mFirstEdge); +} + +void ConvexHullBuilder::FindEdge(Face *inFacingFace, Vec3Arg inVertex, FullEdges &outEdges) const +{ + // Assert that we were given an empty array + JPH_ASSERT(outEdges.empty()); + + // Should start with a facing face + JPH_ASSERT(inFacingFace->IsFacing(inVertex)); + + // Flag as removed + inFacingFace->mRemoved = true; + + // Instead of recursing, we build our own stack with the information we need + struct StackEntry + { + Edge * mFirstEdge; + Edge * mCurrentEdge; + }; + constexpr int cMaxEdgeLength = 128; + StackEntry stack[cMaxEdgeLength]; + int cur_stack_pos = 0; + + static_assert(alignof(Edge) >= 2, "Need lowest bit to indicate to tell if we completed the loop"); + + // Start with the face / edge provided + stack[0].mFirstEdge = inFacingFace->mFirstEdge; + stack[0].mCurrentEdge = reinterpret_cast(reinterpret_cast(inFacingFace->mFirstEdge) | 1); // Set lowest bit of pointer to make it different from the first edge + + for (;;) + { + StackEntry &cur_entry = stack[cur_stack_pos]; + + // Next edge + Edge *raw_e = cur_entry.mCurrentEdge; + Edge *e = reinterpret_cast(reinterpret_cast(raw_e) & ~uintptr_t(1)); // Remove the lowest bit which was used to indicate that this is the first edge we're testing + cur_entry.mCurrentEdge = e->mNextEdge; + + // If we're back at the first edge we've completed the face and we're done + if (raw_e == cur_entry.mFirstEdge) + { + // This face needs to be removed, unlink it now, caller will free + sUnlinkFace(e->mFace); + + // Pop from stack + if (--cur_stack_pos < 0) + break; + } + else + { + // Visit neighbour face + Edge *ne = e->mNeighbourEdge; + if (ne != nullptr) + { + Face *n = ne->mFace; + if (!n->mRemoved) + { + // Check if vertex is on the front side of this face + if (n->IsFacing(inVertex)) + { + // Vertex on front, this face needs to be removed + n->mRemoved = true; + + // Add element to the stack of elements to visit + cur_stack_pos++; + JPH_ASSERT(cur_stack_pos < cMaxEdgeLength); + StackEntry &new_entry = stack[cur_stack_pos]; + new_entry.mFirstEdge = ne; + new_entry.mCurrentEdge = ne->mNextEdge; // We don't need to test this edge again since we came from it + } + else + { + // Vertex behind, keep edge + FullEdge full; + full.mNeighbourEdge = ne; + full.mStartIdx = e->mStartIdx; + full.mEndIdx = ne->mStartIdx; + outEdges.push_back(full); + } + } + } + } + } + + // Assert that we have a fully connected loop +#ifdef JPH_ENABLE_ASSERTS + for (int i = 0; i < (int)outEdges.size(); ++i) + JPH_ASSERT(outEdges[i].mEndIdx == outEdges[(i + 1) % outEdges.size()].mStartIdx); +#endif + +#ifdef JPH_CONVEX_BUILDER_DEBUG + // Draw edge of facing faces + for (int i = 0; i < (int)outEdges.size(); ++i) + DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + mPositions[outEdges[i].mStartIdx]), cDrawScale * (mOffset + mPositions[outEdges[i].mEndIdx]), Color::sWhite, 0.01f); + DrawState(); +#endif +} + +void ConvexHullBuilder::MergeFaces(Edge *inEdge) +{ + // Get the face + Face *face = inEdge->mFace; + + // Find the previous and next edge + Edge *next_edge = inEdge->mNextEdge; + Edge *prev_edge = inEdge->GetPreviousEdge(); + + // Get the other face + Edge *other_edge = inEdge->mNeighbourEdge; + Face *other_face = other_edge->mFace; + + // Check if attempting to merge with self + JPH_ASSERT(face != other_face); + +#ifdef JPH_CONVEX_BUILDER_DEBUG + DrawWireFace(face, Color::sGreen); + DrawWireFace(other_face, Color::sRed); + DrawState(); +#endif + + // Loop over the edges of the other face and make them belong to inFace + Edge *edge = other_edge->mNextEdge; + prev_edge->mNextEdge = edge; + for (;;) + { + edge->mFace = face; + if (edge->mNextEdge == other_edge) + { + // Terminate when we are back at other_edge + edge->mNextEdge = next_edge; + break; + } + edge = edge->mNextEdge; + } + + // If the first edge happens to be inEdge we need to fix it because this edge is no longer part of the face. + // Note that we replace it with the first edge of the merged face so that if the MergeFace function is called + // from a loop that loops around the face that it will still terminate after visiting all edges once. + if (face->mFirstEdge == inEdge) + face->mFirstEdge = prev_edge->mNextEdge; + + // Free the edges + delete inEdge; + delete other_edge; + + // Mark the other face as removed + other_face->mFirstEdge = nullptr; + other_face->mRemoved = true; + + // Recalculate plane + face->CalculateNormalAndCentroid(mPositions.data()); + + // Merge conflict lists + if (face->mFurthestPointDistanceSq > other_face->mFurthestPointDistanceSq) + { + // This face has a point that's further away, make sure it remains the last one as we add the other points to this faces list + face->mConflictList.insert(face->mConflictList.end() - 1, other_face->mConflictList.begin(), other_face->mConflictList.end()); + } + else + { + // The other face has a point that's furthest away, add that list at the end. + face->mConflictList.insert(face->mConflictList.end(), other_face->mConflictList.begin(), other_face->mConflictList.end()); + face->mFurthestPointDistanceSq = other_face->mFurthestPointDistanceSq; + } + other_face->mConflictList.clear(); + +#ifdef JPH_CONVEX_BUILDER_DEBUG + DrawWireFace(face, Color::sWhite); + DrawState(); +#endif +} + +void ConvexHullBuilder::MergeDegenerateFace(Face *inFace, Faces &ioAffectedFaces) +{ + // Check area of face + if (inFace->mNormal.LengthSq() < cMinTriangleAreaSq) + { + // Find longest edge, since this face is a sliver this should keep the face convex + float max_length_sq = 0.0f; + Edge *longest_edge = nullptr; + Edge *e = inFace->mFirstEdge; + Vec3 p1 = mPositions[e->mStartIdx]; + do + { + Edge *next = e->mNextEdge; + Vec3 p2 = mPositions[next->mStartIdx]; + float length_sq = (p2 - p1).LengthSq(); + if (length_sq >= max_length_sq) + { + max_length_sq = length_sq; + longest_edge = e; + } + p1 = p2; + e = next; + } while (e != inFace->mFirstEdge); + + // Merge with face on longest edge + MergeFaces(longest_edge); + + // Remove any invalid edges + RemoveInvalidEdges(inFace, ioAffectedFaces); + } +} + +void ConvexHullBuilder::MergeCoplanarOrConcaveFaces(Face *inFace, float inCoplanarToleranceSq, Faces &ioAffectedFaces) +{ + bool merged = false; + + Edge *edge = inFace->mFirstEdge; + do + { + // Store next edge since this edge can be removed + Edge *next_edge = edge->mNextEdge; + + // Test if centroid of one face is above plane of the other face by inCoplanarToleranceSq. + // If so we need to merge other face into inFace. + const Face *other_face = edge->mNeighbourEdge->mFace; + Vec3 delta_centroid = other_face->mCentroid - inFace->mCentroid; + float dist_other_face_centroid = inFace->mNormal.Dot(delta_centroid); + float signed_dist_other_face_centroid_sq = abs(dist_other_face_centroid) * dist_other_face_centroid; + float dist_face_centroid = -other_face->mNormal.Dot(delta_centroid); + float signed_dist_face_centroid_sq = abs(dist_face_centroid) * dist_face_centroid; + float face_normal_len_sq = inFace->mNormal.LengthSq(); + float other_face_normal_len_sq = other_face->mNormal.LengthSq(); + if ((signed_dist_other_face_centroid_sq > -inCoplanarToleranceSq * face_normal_len_sq + || signed_dist_face_centroid_sq > -inCoplanarToleranceSq * other_face_normal_len_sq) + && inFace->mNormal.Dot(other_face->mNormal) > 0.0f) // Never merge faces that are back to back + { + MergeFaces(edge); + merged = true; + } + + edge = next_edge; + } while (edge != inFace->mFirstEdge); + + if (merged) + RemoveInvalidEdges(inFace, ioAffectedFaces); +} + +void ConvexHullBuilder::sMarkAffected(Face *inFace, Faces &ioAffectedFaces) +{ + if (std::find(ioAffectedFaces.begin(), ioAffectedFaces.end(), inFace) == ioAffectedFaces.end()) + ioAffectedFaces.push_back(inFace); +} + +void ConvexHullBuilder::RemoveInvalidEdges(Face *inFace, Faces &ioAffectedFaces) +{ + // This marks that the plane needs to be recalculated (we delay this until the end of the + // function since we don't use the plane and we want to avoid calculating it multiple times) + bool recalculate_plane = false; + + // We keep going through this loop until no more edges were removed + bool removed; + do + { + removed = false; + + // Loop over all edges in this face + Edge *edge = inFace->mFirstEdge; + Face *neighbour_face = edge->mNeighbourEdge->mFace; + do + { + Edge *next_edge = edge->mNextEdge; + Face *next_neighbour_face = next_edge->mNeighbourEdge->mFace; + + if (neighbour_face == inFace) + { + // We only remove 1 edge at a time, check if this edge's next edge is our neighbour. + // If this check fails, we will continue to scan along the edge until we find an edge where this is the case. + if (edge->mNeighbourEdge == next_edge) + { + // This edge leads back to the starting point, this means the edge is interior and needs to be removed +#ifdef JPH_CONVEX_BUILDER_DEBUG + DrawWireFace(inFace, Color::sBlue); + DrawState(); +#endif + + // Remove edge + Edge *prev_edge = edge->GetPreviousEdge(); + prev_edge->mNextEdge = next_edge->mNextEdge; + if (inFace->mFirstEdge == edge || inFace->mFirstEdge == next_edge) + inFace->mFirstEdge = prev_edge; + delete edge; + delete next_edge; + +#ifdef JPH_CONVEX_BUILDER_DEBUG + DrawWireFace(inFace, Color::sGreen); + DrawState(); +#endif + + // Check if inFace now has only 2 edges left + if (RemoveTwoEdgeFace(inFace, ioAffectedFaces)) + return; // Bail if face no longer exists + + // Restart the loop + recalculate_plane = true; + removed = true; + break; + } + } + else if (neighbour_face == next_neighbour_face) + { + // There are two edges that connect to the same face, we will remove the second one +#ifdef JPH_CONVEX_BUILDER_DEBUG + DrawWireFace(inFace, Color::sYellow); + DrawWireFace(neighbour_face, Color::sRed); + DrawState(); +#endif + + // First merge the neighbours edges + Edge *neighbour_edge = next_edge->mNeighbourEdge; + Edge *next_neighbour_edge = neighbour_edge->mNextEdge; + if (neighbour_face->mFirstEdge == next_neighbour_edge) + neighbour_face->mFirstEdge = neighbour_edge; + neighbour_edge->mNextEdge = next_neighbour_edge->mNextEdge; + neighbour_edge->mNeighbourEdge = edge; + delete next_neighbour_edge; + + // Then merge my own edges + if (inFace->mFirstEdge == next_edge) + inFace->mFirstEdge = edge; + edge->mNextEdge = next_edge->mNextEdge; + edge->mNeighbourEdge = neighbour_edge; + delete next_edge; + +#ifdef JPH_CONVEX_BUILDER_DEBUG + DrawWireFace(inFace, Color::sYellow); + DrawWireFace(neighbour_face, Color::sGreen); + DrawState(); +#endif + + // Check if neighbour has only 2 edges left + if (!RemoveTwoEdgeFace(neighbour_face, ioAffectedFaces)) + { + // No, we need to recalculate its plane + neighbour_face->CalculateNormalAndCentroid(mPositions.data()); + + // Mark neighbour face as affected + sMarkAffected(neighbour_face, ioAffectedFaces); + } + + // Check if inFace now has only 2 edges left + if (RemoveTwoEdgeFace(inFace, ioAffectedFaces)) + return; // Bail if face no longer exists + + // Restart loop + recalculate_plane = true; + removed = true; + break; + } + + // This edge is ok, go to the next edge + edge = next_edge; + neighbour_face = next_neighbour_face; + + } while (edge != inFace->mFirstEdge); + } while (removed); + + // Recalculate plane? + if (recalculate_plane) + inFace->CalculateNormalAndCentroid(mPositions.data()); +} + +bool ConvexHullBuilder::RemoveTwoEdgeFace(Face *inFace, Faces &ioAffectedFaces) const +{ + // Check if this face contains only 2 edges + Edge *edge = inFace->mFirstEdge; + Edge *next_edge = edge->mNextEdge; + JPH_ASSERT(edge != next_edge); // 1 edge faces should not exist + if (next_edge->mNextEdge == edge) + { +#ifdef JPH_CONVEX_BUILDER_DEBUG + DrawWireFace(inFace, Color::sRed); + DrawState(); +#endif + + // Schedule both neighbours for re-checking + Edge *neighbour_edge = edge->mNeighbourEdge; + Face *neighbour_face = neighbour_edge->mFace; + Edge *next_neighbour_edge = next_edge->mNeighbourEdge; + Face *next_neighbour_face = next_neighbour_edge->mFace; + sMarkAffected(neighbour_face, ioAffectedFaces); + sMarkAffected(next_neighbour_face, ioAffectedFaces); + + // Link my neighbours to each other + neighbour_edge->mNeighbourEdge = next_neighbour_edge; + next_neighbour_edge->mNeighbourEdge = neighbour_edge; + + // Unlink my edges + edge->mNeighbourEdge = nullptr; + next_edge->mNeighbourEdge = nullptr; + + // Mark this face as removed + inFace->mRemoved = true; + + return true; + } + + return false; +} + +#ifdef JPH_ENABLE_ASSERTS + +void ConvexHullBuilder::DumpFace(const Face *inFace) const +{ + Trace("f:0x%p", inFace); + + const Edge *e = inFace->mFirstEdge; + do + { + Trace("\te:0x%p { i:%d e:0x%p f:0x%p }", e, e->mStartIdx, e->mNeighbourEdge, e->mNeighbourEdge->mFace); + e = e->mNextEdge; + } while (e != inFace->mFirstEdge); +} + +void ConvexHullBuilder::DumpFaces() const +{ + Trace("Dump Faces:"); + + for (const Face *f : mFaces) + if (!f->mRemoved) + DumpFace(f); +} + +void ConvexHullBuilder::ValidateFace(const Face *inFace) const +{ + if (inFace->mRemoved) + { + const Edge *e = inFace->mFirstEdge; + if (e != nullptr) + do + { + JPH_ASSERT(e->mNeighbourEdge == nullptr); + e = e->mNextEdge; + } while (e != inFace->mFirstEdge); + } + else + { + int edge_count = 0; + + const Edge *e = inFace->mFirstEdge; + do + { + // Count edge + ++edge_count; + + // Validate that adjacent faces are all different + if (mFaces.size() > 2) + for (const Edge *other_edge = e->mNextEdge; other_edge != inFace->mFirstEdge; other_edge = other_edge->mNextEdge) + JPH_ASSERT(e->mNeighbourEdge->mFace != other_edge->mNeighbourEdge->mFace); + + // Assert that the face is correct + JPH_ASSERT(e->mFace == inFace); + + // Assert that we have a neighbour + const Edge *nb_edge = e->mNeighbourEdge; + JPH_ASSERT(nb_edge != nullptr); + if (nb_edge != nullptr) + { + // Assert that our neighbours edge points to us + JPH_ASSERT(nb_edge->mNeighbourEdge == e); + + // Assert that it belongs to a different face + JPH_ASSERT(nb_edge->mFace != inFace); + + // Assert that the next edge of the neighbour points to the same vertex as this edge's vertex + JPH_ASSERT(nb_edge->mNextEdge->mStartIdx == e->mStartIdx); + + // Assert that my next edge points to the same vertex as my neighbours vertex + JPH_ASSERT(e->mNextEdge->mStartIdx == nb_edge->mStartIdx); + } + e = e->mNextEdge; + } while (e != inFace->mFirstEdge); + + // Assert that we have 3 or more edges + JPH_ASSERT(edge_count >= 3); + } +} + +void ConvexHullBuilder::ValidateFaces() const +{ + for (const Face *f : mFaces) + ValidateFace(f); +} + +#endif // JPH_ENABLE_ASSERTS + +void ConvexHullBuilder::GetCenterOfMassAndVolume(Vec3 &outCenterOfMass, float &outVolume) const +{ + // Fourth point is the average of all face centroids + Vec3 v4 = Vec3::sZero(); + for (const Face *f : mFaces) + v4 += f->mCentroid; + v4 /= float(mFaces.size()); + + // Calculate mass and center of mass of this convex hull by summing all tetrahedrons + outVolume = 0.0f; + outCenterOfMass = Vec3::sZero(); + for (const Face *f : mFaces) + { + // Get the first vertex that we'll use to create a triangle fan + Edge *e = f->mFirstEdge; + Vec3 v1 = mPositions[e->mStartIdx]; + + // Get the second vertex + e = e->mNextEdge; + Vec3 v2 = mPositions[e->mStartIdx]; + + for (e = e->mNextEdge; e != f->mFirstEdge; e = e->mNextEdge) + { + // Fetch the last point of the triangle + Vec3 v3 = mPositions[e->mStartIdx]; + + // Calculate center of mass and mass of this tetrahedron, + // see: https://en.wikipedia.org/wiki/Tetrahedron#Volume + float volume_tetrahedron = (v1 - v4).Dot((v2 - v4).Cross(v3 - v4)); // Needs to be divided by 6, postpone this until the end of the loop + Vec3 center_of_mass_tetrahedron = v1 + v2 + v3 + v4; // Needs to be divided by 4, postpone this until the end of the loop + + // Accumulate results + outVolume += volume_tetrahedron; + outCenterOfMass += volume_tetrahedron * center_of_mass_tetrahedron; + + // Update v2 for next triangle + v2 = v3; + } + } + + // Calculate center of mass, fall back to average point in case there is no volume (everything is on a plane in this case) + if (outVolume > FLT_EPSILON) + outCenterOfMass /= 4.0f * outVolume; + else + outCenterOfMass = v4; + + outVolume /= 6.0f; +} + +void ConvexHullBuilder::DetermineMaxError(Face *&outFaceWithMaxError, float &outMaxError, int &outMaxErrorPositionIdx, float &outCoplanarDistance) const +{ + outCoplanarDistance = DetermineCoplanarDistance(); + + // This measures the distance from a polygon to the furthest point outside of the hull + float max_error = 0.0f; + Face *max_error_face = nullptr; + int max_error_point = -1; + + for (int i = 0; i < (int)mPositions.size(); ++i) + { + Vec3 v = mPositions[i]; + + // This measures the closest edge from all faces to point v + // Note that we take the min of all faces since there may be multiple near coplanar faces so if we were to test this per face + // we may find that a point is outside of a polygon and mark it as an error, while it is actually inside a nearly coplanar + // polygon. + float min_edge_dist_sq = FLT_MAX; + Face *min_edge_dist_face = nullptr; + + for (Face *f : mFaces) + { + // Check if point is on or in front of plane + float normal_len = f->mNormal.Length(); + JPH_ASSERT(normal_len > 0.0f); + float plane_dist = f->mNormal.Dot(v - f->mCentroid) / normal_len; + if (plane_dist > -outCoplanarDistance) + { + // Check distance to the edges of this face + float edge_dist_sq = GetDistanceToEdgeSq(v, f); + if (edge_dist_sq < min_edge_dist_sq) + { + min_edge_dist_sq = edge_dist_sq; + min_edge_dist_face = f; + } + + // If the point is inside the polygon and the point is in front of the plane, measure the distance + if (edge_dist_sq == 0.0f && plane_dist > max_error) + { + max_error = plane_dist; + max_error_face = f; + max_error_point = i; + } + } + } + + // If the minimum distance to an edge is further than our current max error, we use that as max error + float min_edge_dist = sqrt(min_edge_dist_sq); + if (min_edge_dist_face != nullptr && min_edge_dist > max_error) + { + max_error = min_edge_dist; + max_error_face = min_edge_dist_face; + max_error_point = i; + } + } + + outFaceWithMaxError = max_error_face; + outMaxError = max_error; + outMaxErrorPositionIdx = max_error_point; +} + +#ifdef JPH_CONVEX_BUILDER_DEBUG + +void ConvexHullBuilder::DrawState(bool inDrawConflictList) const +{ + // Draw origin + DebugRenderer::sInstance->DrawMarker(cDrawScale * mOffset, Color::sRed, 0.2f); + + int face_idx = 0; + + // Draw faces + for (const Face *f : mFaces) + if (!f->mRemoved) + { + Color iteration_color = Color::sGetDistinctColor(f->mIteration); + Color face_color = Color::sGetDistinctColor(face_idx++); + + // First point + const Edge *e = f->mFirstEdge; + RVec3 p1 = cDrawScale * (mOffset + mPositions[e->mStartIdx]); + + // Second point + e = e->mNextEdge; + RVec3 p2 = cDrawScale * (mOffset + mPositions[e->mStartIdx]); + + // First line + DebugRenderer::sInstance->DrawLine(p1, p2, Color::sGrey); + + do + { + // Third point + e = e->mNextEdge; + RVec3 p3 = cDrawScale * (mOffset + mPositions[e->mStartIdx]); + + DebugRenderer::sInstance->DrawTriangle(p1, p2, p3, iteration_color); + + DebugRenderer::sInstance->DrawLine(p2, p3, Color::sGrey); + + p2 = p3; + } + while (e != f->mFirstEdge); + + // Draw normal + RVec3 centroid = cDrawScale * (mOffset + f->mCentroid); + DebugRenderer::sInstance->DrawArrow(centroid, centroid + f->mNormal.NormalizedOr(Vec3::sZero()), face_color, 0.01f); + + // Draw conflict list + if (inDrawConflictList) + for (int idx : f->mConflictList) + DebugRenderer::sInstance->DrawMarker(cDrawScale * (mOffset + mPositions[idx]), face_color, 0.05f); + } + + // Offset to the right + mOffset += mDelta; +} + +void ConvexHullBuilder::DrawWireFace(const Face *inFace, ColorArg inColor) const +{ + const Edge *e = inFace->mFirstEdge; + RVec3 prev = cDrawScale * (mOffset + mPositions[e->mStartIdx]); + do + { + const Edge *next = e->mNextEdge; + RVec3 cur = cDrawScale * (mOffset + mPositions[next->mStartIdx]); + DebugRenderer::sInstance->DrawArrow(prev, cur, inColor, 0.01f); + DebugRenderer::sInstance->DrawText3D(prev, ConvertToString(e->mStartIdx), inColor); + e = next; + prev = cur; + } while (e != inFace->mFirstEdge); +} + +void ConvexHullBuilder::DrawEdge(const Edge *inEdge, ColorArg inColor) const +{ + RVec3 p1 = cDrawScale * (mOffset + mPositions[inEdge->mStartIdx]); + RVec3 p2 = cDrawScale * (mOffset + mPositions[inEdge->mNextEdge->mStartIdx]); + DebugRenderer::sInstance->DrawArrow(p1, p2, inColor, 0.01f); +} + +#endif // JPH_CONVEX_BUILDER_DEBUG + +#ifdef JPH_CONVEX_BUILDER_DUMP_SHAPE + +void ConvexHullBuilder::DumpShape() const +{ + static atomic sShapeNo = 1; + int shape_no = sShapeNo++; + + std::ofstream f; + f.open(StringFormat("dumped_shape%d.cpp", shape_no).c_str(), std::ofstream::out | std::ofstream::trunc); + if (!f.is_open()) + return; + + f << "{\n"; + for (Vec3 v : mPositions) + f << StringFormat("\tVec3(%.9gf, %.9gf, %.9gf),\n", (double)v.GetX(), (double)v.GetY(), (double)v.GetZ()); + f << "},\n"; +} + +#endif // JPH_CONVEX_BUILDER_DUMP_SHAPE + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder.h b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder.h new file mode 100644 index 000000000000..db1ef358d160 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder.h @@ -0,0 +1,276 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +//#define JPH_CONVEX_BUILDER_DEBUG +//#define JPH_CONVEX_BUILDER_DUMP_SHAPE + +#ifdef JPH_CONVEX_BUILDER_DEBUG + #include +#endif + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// A convex hull builder that tries to create hulls as accurately as possible. Used for offline processing. +class JPH_EXPORT ConvexHullBuilder : public NonCopyable +{ +public: + // Forward declare + class Face; + + /// Class that holds the information of an edge + class Edge : public NonCopyable + { + public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + Edge(Face *inFace, int inStartIdx) : mFace(inFace), mStartIdx(inStartIdx) { } + + /// Get the previous edge + inline Edge * GetPreviousEdge() + { + Edge *prev_edge = this; + while (prev_edge->mNextEdge != this) + prev_edge = prev_edge->mNextEdge; + return prev_edge; + } + + Face * mFace; ///< Face that this edge belongs to + Edge * mNextEdge = nullptr; ///< Next edge of this face + Edge * mNeighbourEdge = nullptr; ///< Edge that this edge is connected to + int mStartIdx; ///< Vertex index in mPositions that indicates the start vertex of this edge + }; + + using ConflictList = Array; + + /// Class that holds the information of one face + class Face : public NonCopyable + { + public: + JPH_OVERRIDE_NEW_DELETE + + /// Destructor + ~Face(); + + /// Initialize a face with three indices + void Initialize(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions); + + /// Calculates the centroid and normal for this face + void CalculateNormalAndCentroid(const Vec3 *inPositions); + + /// Check if face inFace is facing inPosition + inline bool IsFacing(Vec3Arg inPosition) const + { + JPH_ASSERT(!mRemoved); + return mNormal.Dot(inPosition - mCentroid) > 0.0f; + } + + Vec3 mNormal; ///< Normal of this face, length is 2 times area of face + Vec3 mCentroid; ///< Center of the face + ConflictList mConflictList; ///< Positions associated with this edge (that are closest to this edge). The last position in the list is the point that is furthest away from the face. + Edge * mFirstEdge = nullptr; ///< First edge of this face + float mFurthestPointDistanceSq = 0.0f; ///< Squared distance of furthest point from the conflict list to the face + bool mRemoved = false; ///< Flag that indicates that face has been removed (face will be freed later) +#ifdef JPH_CONVEX_BUILDER_DEBUG + int mIteration; ///< Iteration that this face was created +#endif + }; + + // Typedefs + using Positions = Array; + using Faces = Array; + + /// Constructor + explicit ConvexHullBuilder(const Positions &inPositions); + + /// Destructor + ~ConvexHullBuilder() { FreeFaces(); } + + /// Result enum that indicates how the hull got created + enum class EResult + { + Success, ///< Hull building finished successfully + MaxVerticesReached, ///< Hull building finished successfully, but the desired accuracy was not reached because the max vertices limit was reached + TooFewPoints, ///< Too few points to create a hull + TooFewFaces, ///< Too few faces in the created hull (signifies precision errors during building) + Degenerate, ///< Degenerate hull detected + }; + + /// Takes all positions as provided by the constructor and use them to build a hull + /// Any points that are closer to the hull than inTolerance will be discarded + /// @param inMaxVertices Max vertices to allow in the hull. Specify INT_MAX if there is no limit. + /// @param inTolerance Max distance that a point is allowed to be outside of the hull + /// @param outError Error message when building fails + /// @return Status code that reports if the hull was created or not + EResult Initialize(int inMaxVertices, float inTolerance, const char *&outError); + + /// Returns the amount of vertices that are currently used by the hull + int GetNumVerticesUsed() const; + + /// Returns true if the hull contains a polygon with inIndices (counter clockwise indices in mPositions) + bool ContainsFace(const Array &inIndices) const; + + /// Calculate the center of mass and the volume of the current convex hull + void GetCenterOfMassAndVolume(Vec3 &outCenterOfMass, float &outVolume) const; + + /// Determines the point that is furthest outside of the hull and reports how far it is outside of the hull (which indicates a failure during hull building) + /// @param outFaceWithMaxError The face that caused the error + /// @param outMaxError The maximum distance of a point to the hull + /// @param outMaxErrorPositionIdx The index of the point that had this distance + /// @param outCoplanarDistance Points that are less than this distance from the hull are considered on the hull. This should be used as a lowerbound for the allowed error. + void DetermineMaxError(Face *&outFaceWithMaxError, float &outMaxError, int &outMaxErrorPositionIdx, float &outCoplanarDistance) const; + + /// Access to the created faces. Memory is owned by the convex hull builder. + const Faces & GetFaces() const { return mFaces; } + +private: + /// Minimal square area of a triangle (used for merging and checking if a triangle is degenerate) + static constexpr float cMinTriangleAreaSq = 1.0e-12f; + +#ifdef JPH_CONVEX_BUILDER_DEBUG + /// Factor to scale convex hull when debug drawing the construction process + static constexpr Real cDrawScale = 10; +#endif + + /// Class that holds an edge including start and end index + class FullEdge + { + public: + Edge * mNeighbourEdge; ///< Edge that this edge is connected to + int mStartIdx; ///< Vertex index in mPositions that indicates the start vertex of this edge + int mEndIdx; ///< Vertex index in mPosition that indicates the end vertex of this edge + }; + + // Private typedefs + using FullEdges = Array; + + // Determine a suitable tolerance for detecting that points are coplanar + float DetermineCoplanarDistance() const; + + /// Find the face for which inPoint is furthest to the front + /// @param inPoint Point to test + /// @param inFaces List of faces to test + /// @param outFace Returns the best face + /// @param outDistSq Returns the squared distance how much inPoint is in front of the plane of the face + void GetFaceForPoint(Vec3Arg inPoint, const Faces &inFaces, Face *&outFace, float &outDistSq) const; + + /// @brief Calculates the distance between inPoint and inFace + /// @param inFace Face to test + /// @param inPoint Point to test + /// @return If the projection of the point on the plane is interior to the face 0, otherwise the squared distance to the closest edge + float GetDistanceToEdgeSq(Vec3Arg inPoint, const Face *inFace) const; + + /// Assigns a position to one of the supplied faces based on which face is closest. + /// @param inPositionIdx Index of the position to add + /// @param inFaces List of faces to consider + /// @param inToleranceSq Tolerance of the hull, if the point is closer to the face than this, we ignore it + /// @return True if point was assigned, false if it was discarded or added to the coplanar list + bool AssignPointToFace(int inPositionIdx, const Faces &inFaces, float inToleranceSq); + + /// Add a new point to the convex hull + void AddPoint(Face *inFacingFace, int inIdx, float inToleranceSq, Faces &outNewFaces); + + /// Remove all faces that have been marked 'removed' from mFaces list + void GarbageCollectFaces(); + + /// Create a new face + Face * CreateFace(); + + /// Create a new triangle + Face * CreateTriangle(int inIdx1, int inIdx2, int inIdx3); + + /// Delete a face (checking that it is not connected to any other faces) + void FreeFace(Face *inFace); + + /// Release all faces and edges + void FreeFaces(); + + /// Link face edge to other face edge + static void sLinkFace(Edge *inEdge1, Edge *inEdge2); + + /// Unlink this face from all of its neighbours + static void sUnlinkFace(Face *inFace); + + /// Given one face that faces inVertex, find the edges of the faces that are not facing inVertex. + /// Will flag all those faces for removal. + void FindEdge(Face *inFacingFace, Vec3Arg inVertex, FullEdges &outEdges) const; + + /// Merges the two faces that share inEdge into the face inEdge->mFace + void MergeFaces(Edge *inEdge); + + /// Merges inFace with a neighbour if it is degenerate (a sliver) + void MergeDegenerateFace(Face *inFace, Faces &ioAffectedFaces); + + /// Merges any coplanar as well as neighbours that form a non-convex edge into inFace. + /// Faces are considered coplanar if the distance^2 of the other face's centroid is smaller than inToleranceSq. + void MergeCoplanarOrConcaveFaces(Face *inFace, float inToleranceSq, Faces &ioAffectedFaces); + + /// Mark face as affected if it is not already in the list + static void sMarkAffected(Face *inFace, Faces &ioAffectedFaces); + + /// Removes all invalid edges. + /// 1. Merges inFace with faces that share two edges with it since this means inFace or the other face cannot be convex or the edge is colinear. + /// 2. Removes edges that are interior to inFace (that have inFace on both sides) + /// Any faces that need to be checked for validity will be added to ioAffectedFaces. + void RemoveInvalidEdges(Face *inFace, Faces &ioAffectedFaces); + + /// Removes inFace if it consists of only 2 edges, linking its neighbouring faces together + /// Any faces that need to be checked for validity will be added to ioAffectedFaces. + /// @return True if face was removed. + bool RemoveTwoEdgeFace(Face *inFace, Faces &ioAffectedFaces) const; + +#ifdef JPH_ENABLE_ASSERTS + /// Dumps the text representation of a face to the TTY + void DumpFace(const Face *inFace) const; + + /// Dumps the text representation of all faces to the TTY + void DumpFaces() const; + + /// Check consistency of 1 face + void ValidateFace(const Face *inFace) const; + + /// Check consistency of all faces + void ValidateFaces() const; +#endif + +#ifdef JPH_CONVEX_BUILDER_DEBUG + /// Draw state of algorithm + void DrawState(bool inDrawConflictList = false) const; + + /// Draw a face for debugging purposes + void DrawWireFace(const Face *inFace, ColorArg inColor) const; + + /// Draw an edge for debugging purposes + void DrawEdge(const Edge *inEdge, ColorArg inColor) const; +#endif + +#ifdef JPH_CONVEX_BUILDER_DUMP_SHAPE + void DumpShape() const; +#endif + + const Positions & mPositions; ///< List of positions (some of them are part of the hull) + Faces mFaces; ///< List of faces that are part of the hull (if !mRemoved) + + struct Coplanar + { + int mPositionIdx; ///< Index in mPositions + float mDistanceSq; ///< Distance to the edge of closest face (should be > 0) + }; + using CoplanarList = Array; + + CoplanarList mCoplanarList; ///< List of positions that are coplanar to a face but outside of the face, these are added to the hull at the end + +#ifdef JPH_CONVEX_BUILDER_DEBUG + int mIteration; ///< Number of iterations we've had so far (for debug purposes) + mutable RVec3 mOffset; ///< Offset to use for state drawing + Vec3 mDelta; ///< Delta offset between next states +#endif +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder2D.cpp b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder2D.cpp new file mode 100644 index 000000000000..c821f92eef77 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder2D.cpp @@ -0,0 +1,335 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +#ifdef JPH_CONVEX_BUILDER_2D_DEBUG + #include +#endif + +JPH_NAMESPACE_BEGIN + +void ConvexHullBuilder2D::Edge::CalculateNormalAndCenter(const Vec3 *inPositions) +{ + Vec3 p1 = inPositions[mStartIdx]; + Vec3 p2 = inPositions[mNextEdge->mStartIdx]; + + // Center of edge + mCenter = 0.5f * (p1 + p2); + + // Create outward pointing normal. + // We have two choices for the normal (which satisfies normal . edge = 0): + // normal1 = (-edge.y, edge.x, 0) + // normal2 = (edge.y, -edge.x, 0) + // We want (normal x edge).z > 0 so that the normal points out of the polygon. Only normal2 satisfies this condition. + Vec3 edge = p2 - p1; + mNormal = Vec3(edge.GetY(), -edge.GetX(), 0); +} + +ConvexHullBuilder2D::ConvexHullBuilder2D(const Positions &inPositions) : + mPositions(inPositions) +{ +#ifdef JPH_CONVEX_BUILDER_2D_DEBUG + // Center the drawing of the first hull around the origin and calculate the delta offset between states + mOffset = RVec3::sZero(); + if (mPositions.empty()) + { + // No hull will be generated + mDelta = Vec3::sZero(); + } + else + { + Vec3 maxv = Vec3::sReplicate(-FLT_MAX), minv = Vec3::sReplicate(FLT_MAX); + for (Vec3 v : mPositions) + { + minv = Vec3::sMin(minv, v); + maxv = Vec3::sMax(maxv, v); + mOffset -= v; + } + mOffset /= Real(mPositions.size()); + mDelta = Vec3((maxv - minv).GetX() + 0.5f, 0, 0); + mOffset += mDelta; // Don't start at origin, we're already drawing the final hull there + } +#endif +} + +ConvexHullBuilder2D::~ConvexHullBuilder2D() +{ + FreeEdges(); +} + +void ConvexHullBuilder2D::FreeEdges() +{ + if (mFirstEdge == nullptr) + return; + + Edge *edge = mFirstEdge; + do + { + Edge *next = edge->mNextEdge; + delete edge; + edge = next; + } while (edge != mFirstEdge); + + mFirstEdge = nullptr; + mNumEdges = 0; +} + +#ifdef JPH_ENABLE_ASSERTS + +void ConvexHullBuilder2D::ValidateEdges() const +{ + if (mFirstEdge == nullptr) + { + JPH_ASSERT(mNumEdges == 0); + return; + } + + int count = 0; + + Edge *edge = mFirstEdge; + do + { + // Validate connectivity + JPH_ASSERT(edge->mNextEdge->mPrevEdge == edge); + JPH_ASSERT(edge->mPrevEdge->mNextEdge == edge); + + ++count; + edge = edge->mNextEdge; + } while (edge != mFirstEdge); + + // Validate that count matches + JPH_ASSERT(count == mNumEdges); +} + +#endif // JPH_ENABLE_ASSERTS + +void ConvexHullBuilder2D::AssignPointToEdge(int inPositionIdx, const Array &inEdges) const +{ + Vec3 point = mPositions[inPositionIdx]; + + Edge *best_edge = nullptr; + float best_dist_sq = 0.0f; + + // Test against all edges + for (Edge *edge : inEdges) + { + // Determine distance to edge + float dot = edge->mNormal.Dot(point - edge->mCenter); + if (dot > 0.0f) + { + float dist_sq = dot * dot / edge->mNormal.LengthSq(); + if (dist_sq > best_dist_sq) + { + best_edge = edge; + best_dist_sq = dist_sq; + } + } + } + + // If this point is in front of the edge, add it to the conflict list + if (best_edge != nullptr) + { + if (best_dist_sq > best_edge->mFurthestPointDistanceSq) + { + // This point is further away than any others, update the distance and add point as last point + best_edge->mFurthestPointDistanceSq = best_dist_sq; + best_edge->mConflictList.push_back(inPositionIdx); + } + else + { + // Not the furthest point, add it as the before last point + best_edge->mConflictList.insert(best_edge->mConflictList.begin() + best_edge->mConflictList.size() - 1, inPositionIdx); + } + } +} + +ConvexHullBuilder2D::EResult ConvexHullBuilder2D::Initialize(int inIdx1, int inIdx2, int inIdx3, int inMaxVertices, float inTolerance, Edges &outEdges) +{ + // Clear any leftovers + FreeEdges(); + outEdges.clear(); + + // Reset flag + EResult result = EResult::Success; + + // Determine a suitable tolerance for detecting that points are colinear + // Formula as per: Implementing Quickhull - Dirk Gregorius. + Vec3 vmax = Vec3::sZero(); + for (Vec3 v : mPositions) + vmax = Vec3::sMax(vmax, v.Abs()); + float colinear_tolerance_sq = Square(2.0f * FLT_EPSILON * (vmax.GetX() + vmax.GetY())); + + // Increase desired tolerance if accuracy doesn't allow it + float tolerance_sq = max(colinear_tolerance_sq, Square(inTolerance)); + + // Start with the initial indices in counter clockwise order + float z = (mPositions[inIdx2] - mPositions[inIdx1]).Cross(mPositions[inIdx3] - mPositions[inIdx1]).GetZ(); + if (z < 0.0f) + std::swap(inIdx1, inIdx2); + + // Create and link edges + Edge *e1 = new Edge(inIdx1); + Edge *e2 = new Edge(inIdx2); + Edge *e3 = new Edge(inIdx3); + e1->mNextEdge = e2; + e1->mPrevEdge = e3; + e2->mNextEdge = e3; + e2->mPrevEdge = e1; + e3->mNextEdge = e1; + e3->mPrevEdge = e2; + mFirstEdge = e1; + mNumEdges = 3; + + // Build the initial conflict lists + Array edges { e1, e2, e3 }; + for (Edge *edge : edges) + edge->CalculateNormalAndCenter(mPositions.data()); + for (int idx = 0; idx < (int)mPositions.size(); ++idx) + if (idx != inIdx1 && idx != inIdx2 && idx != inIdx3) + AssignPointToEdge(idx, edges); + + JPH_IF_ENABLE_ASSERTS(ValidateEdges();) +#ifdef JPH_CONVEX_BUILDER_2D_DEBUG + DrawState(); +#endif + + // Add the remaining points to the hull + for (;;) + { + // Check if we've reached the max amount of vertices that are allowed + if (mNumEdges >= inMaxVertices) + { + result = EResult::MaxVerticesReached; + break; + } + + // Find the edge with the furthest point on it + Edge *edge_with_furthest_point = nullptr; + float furthest_dist_sq = 0.0f; + Edge *edge = mFirstEdge; + do + { + if (edge->mFurthestPointDistanceSq > furthest_dist_sq) + { + furthest_dist_sq = edge->mFurthestPointDistanceSq; + edge_with_furthest_point = edge; + } + edge = edge->mNextEdge; + } while (edge != mFirstEdge); + + // If there is none closer than our tolerance value, we're done + if (edge_with_furthest_point == nullptr || furthest_dist_sq < tolerance_sq) + break; + + // Take the furthest point + int furthest_point_idx = edge_with_furthest_point->mConflictList.back(); + edge_with_furthest_point->mConflictList.pop_back(); + Vec3 furthest_point = mPositions[furthest_point_idx]; + + // Find the horizon of edges that need to be removed + Edge *first_edge = edge_with_furthest_point; + do + { + Edge *prev = first_edge->mPrevEdge; + if (!prev->IsFacing(furthest_point)) + break; + first_edge = prev; + } while (first_edge != edge_with_furthest_point); + + Edge *last_edge = edge_with_furthest_point; + do + { + Edge *next = last_edge->mNextEdge; + if (!next->IsFacing(furthest_point)) + break; + last_edge = next; + } while (last_edge != edge_with_furthest_point); + + // Create new edges + e1 = new Edge(first_edge->mStartIdx); + e2 = new Edge(furthest_point_idx); + e1->mNextEdge = e2; + e1->mPrevEdge = first_edge->mPrevEdge; + e2->mPrevEdge = e1; + e2->mNextEdge = last_edge->mNextEdge; + e1->mPrevEdge->mNextEdge = e1; + e2->mNextEdge->mPrevEdge = e2; + mFirstEdge = e1; // We could delete mFirstEdge so just update it to the newly created edge + mNumEdges += 2; + + // Calculate normals + Array new_edges { e1, e2 }; + for (Edge *new_edge : new_edges) + new_edge->CalculateNormalAndCenter(mPositions.data()); + + // Delete the old edges + for (;;) + { + Edge *next = first_edge->mNextEdge; + + // Redistribute points in conflict list + for (int idx : first_edge->mConflictList) + AssignPointToEdge(idx, new_edges); + + // Delete the old edge + delete first_edge; + --mNumEdges; + + if (first_edge == last_edge) + break; + first_edge = next; + } + + JPH_IF_ENABLE_ASSERTS(ValidateEdges();) + #ifdef JPH_CONVEX_BUILDER_2D_DEBUG + DrawState(); + #endif + } + + // Convert the edge list to a list of indices + outEdges.reserve(mNumEdges); + Edge *edge = mFirstEdge; + do + { + outEdges.push_back(edge->mStartIdx); + edge = edge->mNextEdge; + } while (edge != mFirstEdge); + + return result; +} + +#ifdef JPH_CONVEX_BUILDER_2D_DEBUG + +void ConvexHullBuilder2D::DrawState() +{ + int color_idx = 0; + + const Edge *edge = mFirstEdge; + do + { + const Edge *next = edge->mNextEdge; + + // Get unique color per edge + Color color = Color::sGetDistinctColor(color_idx++); + + // Draw edge and normal + DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + mPositions[edge->mStartIdx]), cDrawScale * (mOffset + mPositions[next->mStartIdx]), color, 0.1f); + DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + edge->mCenter), cDrawScale * (mOffset + edge->mCenter) + edge->mNormal.NormalizedOr(Vec3::sZero()), Color::sGreen, 0.1f); + + // Draw points that belong to this edge in the same color + for (int idx : edge->mConflictList) + DebugRenderer::sInstance->DrawMarker(cDrawScale * (mOffset + mPositions[idx]), color, 0.05f); + + edge = next; + } while (edge != mFirstEdge); + + mOffset += mDelta; +} + +#endif + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder2D.h b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder2D.h new file mode 100644 index 000000000000..ff06a3403e0b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder2D.h @@ -0,0 +1,105 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +//#define JPH_CONVEX_BUILDER_2D_DEBUG + +JPH_NAMESPACE_BEGIN + +/// A convex hull builder that tries to create 2D hulls as accurately as possible. Used for offline processing. +class JPH_EXPORT ConvexHullBuilder2D : public NonCopyable +{ +public: + using Positions = Array; + using Edges = Array; + + /// Constructor + /// @param inPositions Positions used to make the hull. Uses X and Y component of Vec3 only! + explicit ConvexHullBuilder2D(const Positions &inPositions); + + /// Destructor + ~ConvexHullBuilder2D(); + + /// Result enum that indicates how the hull got created + enum class EResult + { + Success, ///< Hull building finished successfully + MaxVerticesReached, ///< Hull building finished successfully, but the desired accuracy was not reached because the max vertices limit was reached + }; + + /// Takes all positions as provided by the constructor and use them to build a hull + /// Any points that are closer to the hull than inTolerance will be discarded + /// @param inIdx1 , inIdx2 , inIdx3 The indices to use as initial hull (in any order) + /// @param inMaxVertices Max vertices to allow in the hull. Specify INT_MAX if there is no limit. + /// @param inTolerance Max distance that a point is allowed to be outside of the hull + /// @param outEdges On success this will contain the list of indices that form the hull (counter clockwise) + /// @return Status code that reports if the hull was created or not + EResult Initialize(int inIdx1, int inIdx2, int inIdx3, int inMaxVertices, float inTolerance, Edges &outEdges); + +private: +#ifdef JPH_CONVEX_BUILDER_2D_DEBUG + /// Factor to scale convex hull when debug drawing the construction process + static constexpr Real cDrawScale = 10; +#endif + + class Edge; + + /// Frees all edges + void FreeEdges(); + + /// Assigns a position to one of the supplied edges based on which edge is closest. + /// @param inPositionIdx Index of the position to add + /// @param inEdges List of edges to consider + void AssignPointToEdge(int inPositionIdx, const Array &inEdges) const; + +#ifdef JPH_CONVEX_BUILDER_2D_DEBUG + /// Draw state of algorithm + void DrawState(); +#endif + +#ifdef JPH_ENABLE_ASSERTS + /// Validate that the edge structure is intact + void ValidateEdges() const; +#endif + + using ConflictList = Array; + + /// Linked list of edges + class Edge + { + public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit Edge(int inStartIdx) : mStartIdx(inStartIdx) { } + + /// Calculate the center of the edge and the edge normal + void CalculateNormalAndCenter(const Vec3 *inPositions); + + /// Check if this edge is facing inPosition + inline bool IsFacing(Vec3Arg inPosition) const { return mNormal.Dot(inPosition - mCenter) > 0.0f; } + + Vec3 mNormal; ///< Normal of the edge (not normalized) + Vec3 mCenter; ///< Center of the edge + ConflictList mConflictList; ///< Positions associated with this edge (that are closest to this edge). Last entry is the one furthest away from the edge, remainder is unsorted. + Edge * mPrevEdge = nullptr; ///< Previous edge in circular list + Edge * mNextEdge = nullptr; ///< Next edge in circular list + int mStartIdx; ///< Position index of start of this edge + float mFurthestPointDistanceSq = 0.0f; ///< Squared distance of furthest point from the conflict list to the edge + }; + + const Positions & mPositions; ///< List of positions (some of them are part of the hull) + Edge * mFirstEdge = nullptr; ///< First edge of the hull + int mNumEdges = 0; ///< Number of edges in hull + +#ifdef JPH_CONVEX_BUILDER_2D_DEBUG + RVec3 mOffset; ///< Offset to use for state drawing + Vec3 mDelta; ///< Delta offset between next states +#endif +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/ConvexSupport.h b/thirdparty/jolt_physics/Jolt/Geometry/ConvexSupport.h new file mode 100644 index 000000000000..3ba2c935db27 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/ConvexSupport.h @@ -0,0 +1,188 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Helper functions to get the support point for a convex object +/// Structure that transforms a convex object (supports only uniform scaling) +template +struct TransformedConvexObject +{ + /// Create transformed convex object. + TransformedConvexObject(Mat44Arg inTransform, const ConvexObject &inObject) : + mTransform(inTransform), + mObject(inObject) + { + } + + /// Calculate the support vector for this convex shape. + Vec3 GetSupport(Vec3Arg inDirection) const + { + return mTransform * mObject.GetSupport(mTransform.Multiply3x3Transposed(inDirection)); + } + + /// Get the vertices of the face that faces inDirection the most + template + void GetSupportingFace(Vec3Arg inDirection, VERTEX_ARRAY &outVertices) const + { + mObject.GetSupportingFace(mTransform.Multiply3x3Transposed(inDirection), outVertices); + + for (Vec3 &v : outVertices) + v = mTransform * v; + } + + Mat44 mTransform; + const ConvexObject & mObject; +}; + +/// Structure that adds a convex radius +template +struct AddConvexRadius +{ + AddConvexRadius(const ConvexObject &inObject, float inRadius) : + mObject(inObject), + mRadius(inRadius) + { + } + + /// Calculate the support vector for this convex shape. + Vec3 GetSupport(Vec3Arg inDirection) const + { + float length = inDirection.Length(); + return length > 0.0f ? mObject.GetSupport(inDirection) + (mRadius / length) * inDirection : mObject.GetSupport(inDirection); + } + + const ConvexObject & mObject; + float mRadius; +}; + +/// Structure that performs a Minkowski difference A - B +template +struct MinkowskiDifference +{ + MinkowskiDifference(const ConvexObjectA &inObjectA, const ConvexObjectB &inObjectB) : + mObjectA(inObjectA), + mObjectB(inObjectB) + { + } + + /// Calculate the support vector for this convex shape. + Vec3 GetSupport(Vec3Arg inDirection) const + { + return mObjectA.GetSupport(inDirection) - mObjectB.GetSupport(-inDirection); + } + + const ConvexObjectA & mObjectA; + const ConvexObjectB & mObjectB; +}; + +/// Class that wraps a point so that it can be used with convex collision detection +struct PointConvexSupport +{ + /// Calculate the support vector for this convex shape. + Vec3 GetSupport([[maybe_unused]] Vec3Arg inDirection) const + { + return mPoint; + } + + Vec3 mPoint; +}; + +/// Class that wraps a triangle so that it can used with convex collision detection +struct TriangleConvexSupport +{ + /// Constructor + TriangleConvexSupport(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3) : + mV1(inV1), + mV2(inV2), + mV3(inV3) + { + } + + /// Calculate the support vector for this convex shape. + Vec3 GetSupport(Vec3Arg inDirection) const + { + // Project vertices on inDirection + float d1 = mV1.Dot(inDirection); + float d2 = mV2.Dot(inDirection); + float d3 = mV3.Dot(inDirection); + + // Return vertex with biggest projection + if (d1 > d2) + { + if (d1 > d3) + return mV1; + else + return mV3; + } + else + { + if (d2 > d3) + return mV2; + else + return mV3; + } + } + + /// Get the vertices of the face that faces inDirection the most + template + void GetSupportingFace([[maybe_unused]] Vec3Arg inDirection, VERTEX_ARRAY &outVertices) const + { + outVertices.push_back(mV1); + outVertices.push_back(mV2); + outVertices.push_back(mV3); + } + + /// The three vertices of the triangle + Vec3 mV1; + Vec3 mV2; + Vec3 mV3; +}; + +/// Class that wraps a polygon so that it can used with convex collision detection +template +struct PolygonConvexSupport +{ + /// Constructor + explicit PolygonConvexSupport(const VERTEX_ARRAY &inVertices) : + mVertices(inVertices) + { + } + + /// Calculate the support vector for this convex shape. + Vec3 GetSupport(Vec3Arg inDirection) const + { + Vec3 support_point = mVertices[0]; + float best_dot = mVertices[0].Dot(inDirection); + + for (typename VERTEX_ARRAY::const_iterator v = mVertices.begin() + 1; v < mVertices.end(); ++v) + { + float dot = v->Dot(inDirection); + if (dot > best_dot) + { + best_dot = dot; + support_point = *v; + } + } + + return support_point; + } + + /// Get the vertices of the face that faces inDirection the most + template + void GetSupportingFace([[maybe_unused]] Vec3Arg inDirection, VERTEX_ARRAY_ARG &outVertices) const + { + for (Vec3 v : mVertices) + outVertices.push_back(v); + } + + /// The vertices of the polygon + const VERTEX_ARRAY & mVertices; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/EPAConvexHullBuilder.h b/thirdparty/jolt_physics/Jolt/Geometry/EPAConvexHullBuilder.h new file mode 100644 index 000000000000..15d61b068bd0 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/EPAConvexHullBuilder.h @@ -0,0 +1,845 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +// Define to validate the integrity of the hull structure +//#define JPH_EPA_CONVEX_BUILDER_VALIDATE + +// Define to draw the building of the hull for debugging purposes +//#define JPH_EPA_CONVEX_BUILDER_DRAW + +#include +#include + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + #include + #include +#endif + +JPH_NAMESPACE_BEGIN + +/// A convex hull builder specifically made for the EPA penetration depth calculation. It trades accuracy for speed and will simply abort of the hull forms defects due to numerical precision problems. +class EPAConvexHullBuilder : public NonCopyable +{ +private: +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + /// Factor to scale convex hull when debug drawing the construction process + static constexpr Real cDrawScale = 10; +#endif + +public: + // Due to the Euler characteristic (https://en.wikipedia.org/wiki/Euler_characteristic) we know that Vertices - Edges + Faces = 2 + // In our case we only have triangles and they are always fully connected, so each edge is shared exactly between 2 faces: Edges = Faces * 3 / 2 + // Substituting: Vertices = Faces / 2 + 2 which is approximately Faces / 2. + static constexpr int cMaxTriangles = 256; ///< Max triangles in hull + static constexpr int cMaxPoints = cMaxTriangles / 2; ///< Max number of points in hull + + // Constants + static constexpr int cMaxEdgeLength = 128; ///< Max number of edges in FindEdge + static constexpr float cMinTriangleArea = 1.0e-10f; ///< Minimum area of a triangle before, if smaller than this it will not be added to the priority queue + static constexpr float cBarycentricEpsilon = 1.0e-3f; ///< Epsilon value used to determine if a point is in the interior of a triangle + + // Forward declare + class Triangle; + + /// Class that holds the information of an edge + class Edge + { + public: + /// Information about neighbouring triangle + Triangle * mNeighbourTriangle; ///< Triangle that neighbours this triangle + int mNeighbourEdge; ///< Index in mEdge that specifies edge that this Edge is connected to + + int mStartIdx; ///< Vertex index in mPositions that indicates the start vertex of this edge + }; + + using Edges = StaticArray; + using NewTriangles = StaticArray; + + /// Class that holds the information of one triangle + class Triangle : public NonCopyable + { + public: + /// Constructor + inline Triangle(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions); + + /// Check if triangle is facing inPosition + inline bool IsFacing(Vec3Arg inPosition) const + { + JPH_ASSERT(!mRemoved); + return mNormal.Dot(inPosition - mCentroid) > 0.0f; + } + + /// Check if triangle is facing the origin + inline bool IsFacingOrigin() const + { + JPH_ASSERT(!mRemoved); + return mNormal.Dot(mCentroid) < 0.0f; + } + + /// Get the next edge of edge inIndex + inline const Edge & GetNextEdge(int inIndex) const + { + return mEdge[(inIndex + 1) % 3]; + } + + Edge mEdge[3]; ///< 3 edges of this triangle + Vec3 mNormal; ///< Normal of this triangle, length is 2 times area of triangle + Vec3 mCentroid; ///< Center of the triangle + float mClosestLenSq = FLT_MAX; ///< Closest distance^2 from origin to triangle + float mLambda[2]; ///< Barycentric coordinates of closest point to origin on triangle + bool mLambdaRelativeTo0; ///< How to calculate the closest point, true: y0 + l0 * (y1 - y0) + l1 * (y2 - y0), false: y1 + l0 * (y0 - y1) + l1 * (y2 - y1) + bool mClosestPointInterior = false; ///< Flag that indicates that the closest point from this triangle to the origin is an interior point + bool mRemoved = false; ///< Flag that indicates that triangle has been removed + bool mInQueue = false; ///< Flag that indicates that this triangle was placed in the sorted heap (stays true after it is popped because the triangle is freed by the main EPA algorithm loop) +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + int mIteration; ///< Iteration that this triangle was created +#endif + }; + + /// Factory that creates triangles in a fixed size buffer + class TriangleFactory : public NonCopyable + { + private: + /// Struct that stores both a triangle or a next pointer in case the triangle is unused + union alignas(Triangle) Block + { + uint8 mTriangle[sizeof(Triangle)]; + Block * mNextFree; + }; + + /// Storage for triangle data + Block mTriangles[cMaxTriangles]; ///< Storage for triangles + Block * mNextFree = nullptr; ///< List of free triangles + int mHighWatermark = 0; ///< High water mark for used triangles (if mNextFree == nullptr we can take one from here) + + public: + /// Return all triangles to the free pool + void Clear() + { + mNextFree = nullptr; + mHighWatermark = 0; + } + + /// Allocate a new triangle with 3 indexes + Triangle * CreateTriangle(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions) + { + Triangle *t; + if (mNextFree != nullptr) + { + // Entry available from the free list + t = reinterpret_cast(&mNextFree->mTriangle); + mNextFree = mNextFree->mNextFree; + } + else + { + // Allocate from never used before triangle store + if (mHighWatermark >= cMaxTriangles) + return nullptr; // Buffer full + t = reinterpret_cast(&mTriangles[mHighWatermark].mTriangle); + ++mHighWatermark; + } + + // Call constructor + new (t) Triangle(inIdx0, inIdx1, inIdx2, inPositions); + + return t; + } + + /// Free a triangle + void FreeTriangle(Triangle *inT) + { + // Destruct triangle + inT->~Triangle(); +#ifdef JPH_DEBUG + memset(inT, 0xcd, sizeof(Triangle)); +#endif + + // Add triangle to the free list + Block *tu = reinterpret_cast(inT); + tu->mNextFree = mNextFree; + mNextFree = tu; + } + }; + + // Typedefs + using PointsBase = StaticArray; + using Triangles = StaticArray; + + /// Specialized points list that allows direct access to the size + class Points : public PointsBase + { + public: + size_type & GetSizeRef() + { + return mSize; + } + }; + + /// Specialized triangles list that keeps them sorted on closest distance to origin + class TriangleQueue : public Triangles + { + public: + /// Function to sort triangles on closest distance to origin + static bool sTriangleSorter(const Triangle *inT1, const Triangle *inT2) + { + return inT1->mClosestLenSq > inT2->mClosestLenSq; + } + + /// Add triangle to the list + void push_back(Triangle *inT) + { + // Add to base + Triangles::push_back(inT); + + // Mark in queue + inT->mInQueue = true; + + // Resort heap + BinaryHeapPush(begin(), end(), sTriangleSorter); + } + + /// Peek the next closest triangle without removing it + Triangle * PeekClosest() + { + return front(); + } + + /// Get next closest triangle + Triangle * PopClosest() + { + // Move closest to end + BinaryHeapPop(begin(), end(), sTriangleSorter); + + // Remove last triangle + Triangle *t = back(); + pop_back(); + return t; + } + }; + + /// Constructor + explicit EPAConvexHullBuilder(const Points &inPositions) : + mPositions(inPositions) + { +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + mIteration = 0; + mOffset = RVec3::sZero(); +#endif + } + + /// Initialize the hull with 3 points + void Initialize(int inIdx1, int inIdx2, int inIdx3) + { + // Release triangles + mFactory.Clear(); + + // Create triangles (back to back) + Triangle *t1 = CreateTriangle(inIdx1, inIdx2, inIdx3); + Triangle *t2 = CreateTriangle(inIdx1, inIdx3, inIdx2); + + // Link triangles edges + sLinkTriangle(t1, 0, t2, 2); + sLinkTriangle(t1, 1, t2, 1); + sLinkTriangle(t1, 2, t2, 0); + + // Always add both triangles to the priority queue + mTriangleQueue.push_back(t1); + mTriangleQueue.push_back(t2); + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + // Draw current state + DrawState(); + + // Increment iteration counter + ++mIteration; +#endif + } + + /// Check if there's another triangle to process from the queue + bool HasNextTriangle() const + { + return !mTriangleQueue.empty(); + } + + /// Access to the next closest triangle to the origin (won't remove it from the queue). + Triangle * PeekClosestTriangleInQueue() + { + return mTriangleQueue.PeekClosest(); + } + + /// Access to the next closest triangle to the origin and remove it from the queue. + Triangle * PopClosestTriangleFromQueue() + { + return mTriangleQueue.PopClosest(); + } + + /// Find the triangle on which inPosition is the furthest to the front + /// Note this function works as long as all points added have been added with AddPoint(..., FLT_MAX). + Triangle * FindFacingTriangle(Vec3Arg inPosition, float &outBestDistSq) + { + Triangle *best = nullptr; + float best_dist_sq = 0.0f; + + for (Triangle *t : mTriangleQueue) + if (!t->mRemoved) + { + float dot = t->mNormal.Dot(inPosition - t->mCentroid); + if (dot > 0.0f) + { + float dist_sq = dot * dot / t->mNormal.LengthSq(); + if (dist_sq > best_dist_sq) + { + best = t; + best_dist_sq = dist_sq; + } + } + } + + outBestDistSq = best_dist_sq; + return best; + } + + /// Add a new point to the convex hull + bool AddPoint(Triangle *inFacingTriangle, int inIdx, float inClosestDistSq, NewTriangles &outTriangles) + { + // Get position + Vec3 pos = mPositions[inIdx]; + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + // Draw new support point + DrawMarker(pos, Color::sYellow, 1.0f); +#endif + +#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE + // Check if structure is intact + ValidateTriangles(); +#endif + + // Find edge of convex hull of triangles that are not facing the new vertex w + Edges edges; + if (!FindEdge(inFacingTriangle, pos, edges)) + return false; + + // Create new triangles + int num_edges = edges.size(); + for (int i = 0; i < num_edges; ++i) + { + // Create new triangle + Triangle *nt = CreateTriangle(edges[i].mStartIdx, edges[(i + 1) % num_edges].mStartIdx, inIdx); + if (nt == nullptr) + return false; + outTriangles.push_back(nt); + + // Check if we need to put this triangle in the priority queue + if ((nt->mClosestPointInterior && nt->mClosestLenSq < inClosestDistSq) // For the main algorithm + || nt->mClosestLenSq < 0.0f) // For when the origin is not inside the hull yet + mTriangleQueue.push_back(nt); + } + + // Link edges + for (int i = 0; i < num_edges; ++i) + { + sLinkTriangle(outTriangles[i], 0, edges[i].mNeighbourTriangle, edges[i].mNeighbourEdge); + sLinkTriangle(outTriangles[i], 1, outTriangles[(i + 1) % num_edges], 2); + } + +#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE + // Check if structure is intact + ValidateTriangles(); +#endif + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + // Draw state of the hull + DrawState(); + + // Increment iteration counter + ++mIteration; +#endif + + return true; + } + + /// Free a triangle + void FreeTriangle(Triangle *inT) + { +#ifdef JPH_ENABLE_ASSERTS + // Make sure that this triangle is not connected + JPH_ASSERT(inT->mRemoved); + for (const Edge &e : inT->mEdge) + JPH_ASSERT(e.mNeighbourTriangle == nullptr); +#endif + +#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW) + // Remove from list of all triangles + Triangles::iterator i = std::find(mTriangles.begin(), mTriangles.end(), inT); + JPH_ASSERT(i != mTriangles.end()); + mTriangles.erase(i); +#endif + + mFactory.FreeTriangle(inT); + } + +private: + /// Create a new triangle + Triangle * CreateTriangle(int inIdx1, int inIdx2, int inIdx3) + { + // Call provider to create triangle + Triangle *t = mFactory.CreateTriangle(inIdx1, inIdx2, inIdx3, mPositions.data()); + if (t == nullptr) + return nullptr; + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + // Remember iteration counter + t->mIteration = mIteration; +#endif + +#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW) + // Add to list of triangles for debugging purposes + mTriangles.push_back(t); +#endif + + return t; + } + + /// Link triangle edge to other triangle edge + static void sLinkTriangle(Triangle *inT1, int inEdge1, Triangle *inT2, int inEdge2) + { + JPH_ASSERT(inEdge1 >= 0 && inEdge1 < 3); + JPH_ASSERT(inEdge2 >= 0 && inEdge2 < 3); + Edge &e1 = inT1->mEdge[inEdge1]; + Edge &e2 = inT2->mEdge[inEdge2]; + + // Check not connected yet + JPH_ASSERT(e1.mNeighbourTriangle == nullptr); + JPH_ASSERT(e2.mNeighbourTriangle == nullptr); + + // Check vertices match + JPH_ASSERT(e1.mStartIdx == inT2->GetNextEdge(inEdge2).mStartIdx); + JPH_ASSERT(e2.mStartIdx == inT1->GetNextEdge(inEdge1).mStartIdx); + + // Link up + e1.mNeighbourTriangle = inT2; + e1.mNeighbourEdge = inEdge2; + e2.mNeighbourTriangle = inT1; + e2.mNeighbourEdge = inEdge1; + } + + /// Unlink this triangle + void UnlinkTriangle(Triangle *inT) + { + // Unlink from neighbours + for (int i = 0; i < 3; ++i) + { + Edge &edge = inT->mEdge[i]; + if (edge.mNeighbourTriangle != nullptr) + { + Edge &neighbour_edge = edge.mNeighbourTriangle->mEdge[edge.mNeighbourEdge]; + + // Validate that neighbour points to us + JPH_ASSERT(neighbour_edge.mNeighbourTriangle == inT); + JPH_ASSERT(neighbour_edge.mNeighbourEdge == i); + + // Unlink + neighbour_edge.mNeighbourTriangle = nullptr; + edge.mNeighbourTriangle = nullptr; + } + } + + // If this triangle is not in the priority queue, we can delete it now + if (!inT->mInQueue) + FreeTriangle(inT); + } + + /// Given one triangle that faces inVertex, find the edges of the triangles that are not facing inVertex. + /// Will flag all those triangles for removal. + bool FindEdge(Triangle *inFacingTriangle, Vec3Arg inVertex, Edges &outEdges) + { + // Assert that we were given an empty array + JPH_ASSERT(outEdges.empty()); + + // Should start with a facing triangle + JPH_ASSERT(inFacingTriangle->IsFacing(inVertex)); + + // Flag as removed + inFacingTriangle->mRemoved = true; + + // Instead of recursing, we build our own stack with the information we need + struct StackEntry + { + Triangle * mTriangle; + int mEdge; + int mIter; + }; + StackEntry stack[cMaxEdgeLength]; + int cur_stack_pos = 0; + + // Start with the triangle / edge provided + stack[0].mTriangle = inFacingTriangle; + stack[0].mEdge = 0; + stack[0].mIter = -1; // Start with edge 0 (is incremented below before use) + + // Next index that we expect to find, if we don't then there are 'islands' + int next_expected_start_idx = -1; + + for (;;) + { + StackEntry &cur_entry = stack[cur_stack_pos]; + + // Next iteration + if (++cur_entry.mIter >= 3) + { + // This triangle needs to be removed, unlink it now + UnlinkTriangle(cur_entry.mTriangle); + + // Pop from stack + if (--cur_stack_pos < 0) + break; + } + else + { + // Visit neighbour + Edge &e = cur_entry.mTriangle->mEdge[(cur_entry.mEdge + cur_entry.mIter) % 3]; + Triangle *n = e.mNeighbourTriangle; + if (n != nullptr && !n->mRemoved) + { + // Check if vertex is on the front side of this triangle + if (n->IsFacing(inVertex)) + { + // Vertex on front, this triangle needs to be removed + n->mRemoved = true; + + // Add element to the stack of elements to visit + cur_stack_pos++; + JPH_ASSERT(cur_stack_pos < cMaxEdgeLength); + StackEntry &new_entry = stack[cur_stack_pos]; + new_entry.mTriangle = n; + new_entry.mEdge = e.mNeighbourEdge; + new_entry.mIter = 0; // Is incremented before use, we don't need to test this edge again since we came from it + } + else + { + // Detect if edge doesn't connect to previous edge, if this happens we have found and 'island' which means + // the newly added point is so close to the triangles of the hull that we classified some (nearly) coplanar + // triangles as before and some behind the point. At this point we just abort adding the point because + // we've reached numerical precision. + // Note that we do not need to test if the first and last edge connect, since when there are islands + // there should be at least 2 disconnects. + if (e.mStartIdx != next_expected_start_idx && next_expected_start_idx != -1) + return false; + + // Next expected index is the start index of our neighbour's edge + next_expected_start_idx = n->mEdge[e.mNeighbourEdge].mStartIdx; + + // Vertex behind, keep edge + outEdges.push_back(e); + } + } + } + } + + // Assert that we have a fully connected loop + JPH_ASSERT(outEdges.empty() || outEdges[0].mStartIdx == next_expected_start_idx); + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + // Draw edge of facing triangles + for (int i = 0; i < (int)outEdges.size(); ++i) + { + RVec3 edge_start = cDrawScale * (mOffset + mPositions[outEdges[i].mStartIdx]); + DebugRenderer::sInstance->DrawArrow(edge_start, cDrawScale * (mOffset + mPositions[outEdges[(i + 1) % outEdges.size()].mStartIdx]), Color::sYellow, 0.01f); + DebugRenderer::sInstance->DrawText3D(edge_start, ConvertToString(outEdges[i].mStartIdx), Color::sWhite); + } + + // Draw the state with the facing triangles removed + DrawState(); +#endif + + // When we start with two triangles facing away from each other and adding a point that is on the plane, + // sometimes we consider the point in front of both causing both triangles to be removed resulting in an empty edge list. + // In this case we fail to add the point which will result in no collision reported (the shapes are contacting in 1 point so there's 0 penetration) + return outEdges.size() >= 3; + } + +#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE + /// Check consistency of 1 triangle + void ValidateTriangle(const Triangle *inT) const + { + if (inT->mRemoved) + { + // Validate that removed triangles are not connected to anything + for (const Edge &my_edge : inT->mEdge) + JPH_ASSERT(my_edge.mNeighbourTriangle == nullptr); + } + else + { + for (int i = 0; i < 3; ++i) + { + const Edge &my_edge = inT->mEdge[i]; + + // Assert that we have a neighbour + const Triangle *nb = my_edge.mNeighbourTriangle; + JPH_ASSERT(nb != nullptr); + + if (nb != nullptr) + { + // Assert that our neighbours edge points to us + const Edge &nb_edge = nb->mEdge[my_edge.mNeighbourEdge]; + JPH_ASSERT(nb_edge.mNeighbourTriangle == inT); + JPH_ASSERT(nb_edge.mNeighbourEdge == i); + + // Assert that the next edge of the neighbour points to the same vertex as this edge's vertex + const Edge &nb_next_edge = nb->GetNextEdge(my_edge.mNeighbourEdge); + JPH_ASSERT(nb_next_edge.mStartIdx == my_edge.mStartIdx); + + // Assert that my next edge points to the same vertex as my neighbours vertex + const Edge &my_next_edge = inT->GetNextEdge(i); + JPH_ASSERT(my_next_edge.mStartIdx == nb_edge.mStartIdx); + } + } + } + } + + /// Check consistency of all triangles + void ValidateTriangles() const + { + for (const Triangle *t : mTriangles) + ValidateTriangle(t); + } +#endif + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW +public: + /// Draw state of algorithm + void DrawState() + { + // Draw origin + DebugRenderer::sInstance->DrawCoordinateSystem(RMat44::sTranslation(cDrawScale * mOffset), 1.0f); + + // Draw triangles + for (const Triangle *t : mTriangles) + if (!t->mRemoved) + { + // Calculate the triangle vertices + RVec3 p1 = cDrawScale * (mOffset + mPositions[t->mEdge[0].mStartIdx]); + RVec3 p2 = cDrawScale * (mOffset + mPositions[t->mEdge[1].mStartIdx]); + RVec3 p3 = cDrawScale * (mOffset + mPositions[t->mEdge[2].mStartIdx]); + + // Draw triangle + DebugRenderer::sInstance->DrawTriangle(p1, p2, p3, Color::sGetDistinctColor(t->mIteration)); + DebugRenderer::sInstance->DrawWireTriangle(p1, p2, p3, Color::sGrey); + + // Draw normal + RVec3 centroid = cDrawScale * (mOffset + t->mCentroid); + float len = t->mNormal.Length(); + if (len > 0.0f) + DebugRenderer::sInstance->DrawArrow(centroid, centroid + t->mNormal / len, Color::sDarkGreen, 0.01f); + } + + // Determine max position + float min_x = FLT_MAX; + float max_x = -FLT_MAX; + for (Vec3 p : mPositions) + { + min_x = min(min_x, p.GetX()); + max_x = max(max_x, p.GetX()); + } + + // Offset to the right + mOffset += Vec3(max_x - min_x + 0.5f, 0.0f, 0.0f); + } + + /// Draw a label to indicate the next stage in the algorithm + void DrawLabel(const string_view &inText) + { + DebugRenderer::sInstance->DrawText3D(cDrawScale * mOffset, inText, Color::sWhite, 0.1f * cDrawScale); + + mOffset += Vec3(5.0f, 0.0f, 0.0f); + } + + /// Draw geometry for debugging purposes + void DrawGeometry(const DebugRenderer::GeometryRef &inGeometry, ColorArg inColor) + { + RMat44 origin = RMat44::sScale(Vec3::sReplicate(cDrawScale)) * RMat44::sTranslation(mOffset); + DebugRenderer::sInstance->DrawGeometry(origin, inGeometry->mBounds.Transformed(origin), inGeometry->mBounds.GetExtent().LengthSq(), inColor, inGeometry); + + mOffset += Vec3(inGeometry->mBounds.GetSize().GetX(), 0, 0); + } + + /// Draw a triangle for debugging purposes + void DrawWireTriangle(const Triangle &inTriangle, ColorArg inColor) + { + RVec3 prev = cDrawScale * (mOffset + mPositions[inTriangle.mEdge[2].mStartIdx]); + for (const Edge &edge : inTriangle.mEdge) + { + RVec3 cur = cDrawScale * (mOffset + mPositions[edge.mStartIdx]); + DebugRenderer::sInstance->DrawArrow(prev, cur, inColor, 0.01f); + prev = cur; + } + } + + /// Draw a marker for debugging purposes + void DrawMarker(Vec3Arg inPosition, ColorArg inColor, float inSize) + { + DebugRenderer::sInstance->DrawMarker(cDrawScale * (mOffset + inPosition), inColor, inSize); + } + + /// Draw an arrow for debugging purposes + void DrawArrow(Vec3Arg inFrom, Vec3Arg inTo, ColorArg inColor, float inArrowSize) + { + DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + inFrom), cDrawScale * (mOffset + inTo), inColor, inArrowSize); + } +#endif + +private: + TriangleFactory mFactory; ///< Factory to create new triangles and remove old ones + const Points & mPositions; ///< List of positions (some of them are part of the hull) + TriangleQueue mTriangleQueue; ///< List of triangles that are part of the hull that still need to be checked (if !mRemoved) + +#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW) + Triangles mTriangles; ///< The list of all triangles in this hull (for debug purposes) +#endif + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + int mIteration; ///< Number of iterations we've had so far (for debug purposes) + RVec3 mOffset; ///< Offset to use for state drawing +#endif +}; + +// The determinant that is calculated in the Triangle constructor is really sensitive +// to numerical round off, disable the fmadd instructions to maintain precision. +JPH_PRECISE_MATH_ON + +EPAConvexHullBuilder::Triangle::Triangle(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions) +{ + // Fill in indexes + JPH_ASSERT(inIdx0 != inIdx1 && inIdx0 != inIdx2 && inIdx1 != inIdx2); + mEdge[0].mStartIdx = inIdx0; + mEdge[1].mStartIdx = inIdx1; + mEdge[2].mStartIdx = inIdx2; + + // Clear links + mEdge[0].mNeighbourTriangle = nullptr; + mEdge[1].mNeighbourTriangle = nullptr; + mEdge[2].mNeighbourTriangle = nullptr; + + // Get vertex positions + Vec3 y0 = inPositions[inIdx0]; + Vec3 y1 = inPositions[inIdx1]; + Vec3 y2 = inPositions[inIdx2]; + + // Calculate centroid + mCentroid = (y0 + y1 + y2) / 3.0f; + + // Calculate edges + Vec3 y10 = y1 - y0; + Vec3 y20 = y2 - y0; + Vec3 y21 = y2 - y1; + + // The most accurate normal is calculated by using the two shortest edges + // See: https://box2d.org/posts/2014/01/troublesome-triangle/ + // The difference in normals is most pronounced when one edge is much smaller than the others (in which case the other 2 must have roughly the same length). + // Therefore we can suffice by just picking the shortest from 2 edges and use that with the 3rd edge to calculate the normal. + // We first check which of the edges is shorter. + float y20_dot_y20 = y20.Dot(y20); + float y21_dot_y21 = y21.Dot(y21); + if (y20_dot_y20 < y21_dot_y21) + { + // We select the edges y10 and y20 + mNormal = y10.Cross(y20); + + // Check if triangle is degenerate + float normal_len_sq = mNormal.LengthSq(); + if (normal_len_sq > cMinTriangleArea) + { + // Determine distance between triangle and origin: distance = (centroid - origin) . normal / |normal| + // Note that this way of calculating the closest point is much more accurate than first calculating barycentric coordinates and then calculating the closest + // point based on those coordinates. Note that we preserve the sign of the distance to check on which side the origin is. + float c_dot_n = mCentroid.Dot(mNormal); + mClosestLenSq = abs(c_dot_n) * c_dot_n / normal_len_sq; + + // Calculate closest point to origin using barycentric coordinates: + // + // v = y0 + l0 * (y1 - y0) + l1 * (y2 - y0) + // v . (y1 - y0) = 0 + // v . (y2 - y0) = 0 + // + // Written in matrix form: + // + // | y10.y10 y20.y10 | | l0 | = | -y0.y10 | + // | y10.y20 y20.y20 | | l1 | | -y0.y20 | + // + // (y10 = y1 - y0 etc.) + // + // Cramers rule to invert matrix: + float y10_dot_y10 = y10.LengthSq(); + float y10_dot_y20 = y10.Dot(y20); + float determinant = y10_dot_y10 * y20_dot_y20 - y10_dot_y20 * y10_dot_y20; + if (determinant > 0.0f) // If determinant == 0 then the system is linearly dependent and the triangle is degenerate, since y10.10 * y20.y20 > y10.y20^2 it should also be > 0 + { + float y0_dot_y10 = y0.Dot(y10); + float y0_dot_y20 = y0.Dot(y20); + float l0 = (y10_dot_y20 * y0_dot_y20 - y20_dot_y20 * y0_dot_y10) / determinant; + float l1 = (y10_dot_y20 * y0_dot_y10 - y10_dot_y10 * y0_dot_y20) / determinant; + mLambda[0] = l0; + mLambda[1] = l1; + mLambdaRelativeTo0 = true; + + // Check if closest point is interior to the triangle. For a convex hull which contains the origin each face must contain the origin, but because + // our faces are triangles, we can have multiple coplanar triangles and only 1 will have the origin as an interior point. We want to use this triangle + // to calculate the contact points because it gives the most accurate results, so we will only add these triangles to the priority queue. + if (l0 > -cBarycentricEpsilon && l1 > -cBarycentricEpsilon && l0 + l1 < 1.0f + cBarycentricEpsilon) + mClosestPointInterior = true; + } + } + } + else + { + // We select the edges y10 and y21 + mNormal = y10.Cross(y21); + + // Check if triangle is degenerate + float normal_len_sq = mNormal.LengthSq(); + if (normal_len_sq > cMinTriangleArea) + { + // Again calculate distance between triangle and origin + float c_dot_n = mCentroid.Dot(mNormal); + mClosestLenSq = abs(c_dot_n) * c_dot_n / normal_len_sq; + + // Calculate closest point to origin using barycentric coordinates but this time using y1 as the reference vertex + // + // v = y1 + l0 * (y0 - y1) + l1 * (y2 - y1) + // v . (y0 - y1) = 0 + // v . (y2 - y1) = 0 + // + // Written in matrix form: + // + // | y10.y10 -y21.y10 | | l0 | = | y1.y10 | + // | -y10.y21 y21.y21 | | l1 | | -y1.y21 | + // + // Cramers rule to invert matrix: + float y10_dot_y10 = y10.LengthSq(); + float y10_dot_y21 = y10.Dot(y21); + float determinant = y10_dot_y10 * y21_dot_y21 - y10_dot_y21 * y10_dot_y21; + if (determinant > 0.0f) + { + float y1_dot_y10 = y1.Dot(y10); + float y1_dot_y21 = y1.Dot(y21); + float l0 = (y21_dot_y21 * y1_dot_y10 - y10_dot_y21 * y1_dot_y21) / determinant; + float l1 = (y10_dot_y21 * y1_dot_y10 - y10_dot_y10 * y1_dot_y21) / determinant; + mLambda[0] = l0; + mLambda[1] = l1; + mLambdaRelativeTo0 = false; + + // Again check if the closest point is inside the triangle + if (l0 > -cBarycentricEpsilon && l1 > -cBarycentricEpsilon && l0 + l1 < 1.0f + cBarycentricEpsilon) + mClosestPointInterior = true; + } + } + } +} + +JPH_PRECISE_MATH_OFF + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/EPAPenetrationDepth.h b/thirdparty/jolt_physics/Jolt/Geometry/EPAPenetrationDepth.h new file mode 100644 index 000000000000..ffa0c39127a4 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/EPAPenetrationDepth.h @@ -0,0 +1,559 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +//#define JPH_EPA_PENETRATION_DEPTH_DEBUG + +JPH_NAMESPACE_BEGIN + +/// Implementation of Expanding Polytope Algorithm as described in: +/// +/// Proximity Queries and Penetration Depth Computation on 3D Game Objects - Gino van den Bergen +/// +/// The implementation of this algorithm does not completely follow the article, instead of splitting +/// triangles at each edge as in fig. 7 in the article, we build a convex hull (removing any triangles that +/// are facing the new point, thereby avoiding the problem of getting really oblong triangles as mentioned in +/// the article). +/// +/// The algorithm roughly works like: +/// +/// - Start with a simplex of the Minkowski sum (difference) of two objects that was calculated by GJK +/// - This simplex should contain the origin (or else GJK would have reported: no collision) +/// - In cases where the simplex consists of 1 - 3 points, find some extra support points (of the Minkowski sum) to get to at least 4 points +/// - Convert this into a convex hull with non-zero volume (which includes the origin) +/// - A: Calculate the closest point to the origin for all triangles of the hull and take the closest one +/// - Calculate a new support point (of the Minkowski sum) in this direction and add this point to the convex hull +/// - This will remove all faces that are facing the new point and will create new triangles to fill up the hole +/// - Loop to A until no closer point found +/// - The closest point indicates the position / direction of least penetration +class EPAPenetrationDepth +{ +private: + // Typedefs + static constexpr int cMaxPoints = EPAConvexHullBuilder::cMaxPoints; + static constexpr int cMaxPointsToIncludeOriginInHull = 32; + static_assert(cMaxPointsToIncludeOriginInHull < cMaxPoints); + + using Triangle = EPAConvexHullBuilder::Triangle; + using Points = EPAConvexHullBuilder::Points; + + /// The GJK algorithm, used to start the EPA algorithm + GJKClosestPoint mGJK; + +#ifdef JPH_ENABLE_ASSERTS + /// Tolerance as passed to the GJK algorithm, used for asserting. + float mGJKTolerance = 0.0f; +#endif // JPH_ENABLE_ASSERTS + + /// A list of support points for the EPA algorithm + class SupportPoints + { + public: + /// List of support points + Points mY; + Vec3 mP[cMaxPoints]; + Vec3 mQ[cMaxPoints]; + + /// Calculate and add new support point to the list of points + template + Vec3 Add(const A &inA, const B &inB, Vec3Arg inDirection, int &outIndex) + { + // Get support point of the minkowski sum A - B + Vec3 p = inA.GetSupport(inDirection); + Vec3 q = inB.GetSupport(-inDirection); + Vec3 w = p - q; + + // Store new point + outIndex = mY.size(); + mY.push_back(w); + mP[outIndex] = p; + mQ[outIndex] = q; + + return w; + } + }; + +public: + /// Return code for GetPenetrationDepthStepGJK + enum class EStatus + { + NotColliding, ///< Returned if the objects don't collide, in this case outPointA/outPointB are invalid + Colliding, ///< Returned if the objects penetrate + Indeterminate ///< Returned if the objects penetrate further than the convex radius. In this case you need to call GetPenetrationDepthStepEPA to get the actual penetration depth. + }; + + /// Calculates penetration depth between two objects, first step of two (the GJK step) + /// + /// @param inAExcludingConvexRadius Object A without convex radius. + /// @param inBExcludingConvexRadius Object B without convex radius. + /// @param inConvexRadiusA Convex radius for A. + /// @param inConvexRadiusB Convex radius for B. + /// @param ioV Pass in previously returned value or (1, 0, 0). On return this value is changed to direction to move B out of collision along the shortest path (magnitude is meaningless). + /// @param inTolerance Minimal distance before A and B are considered colliding. + /// @param outPointA Position on A that has the least amount of penetration. + /// @param outPointB Position on B that has the least amount of penetration. + /// Use |outPointB - outPointA| to get the distance of penetration. + 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. + // + // Note that if the assert below triggers, it is very likely that you have a MeshShape that contains a degenerate triangle (e.g. a sliver). + // Go up a couple of levels in the call stack to see if we're indeed testing a triangle and if it is degenerate. + // If this is the case then fix the triangles you supply to the MeshShape. + JPH_ASSERT(!ioV.IsNearZero()); + + // Get closest points + float combined_radius = inConvexRadiusA + inConvexRadiusB; + float combined_radius_sq = combined_radius * combined_radius; + float closest_points_dist_sq = mGJK.GetClosestPoints(inAExcludingConvexRadius, inBExcludingConvexRadius, inTolerance, combined_radius_sq, ioV, outPointA, outPointB); + if (closest_points_dist_sq > combined_radius_sq) + { + // No collision + return EStatus::NotColliding; + } + if (closest_points_dist_sq > 0.0f) + { + // Collision within convex radius, adjust points for convex radius + float v_len = sqrt(closest_points_dist_sq); // GetClosestPoints function returns |ioV|^2 when return value < FLT_MAX + outPointA += ioV * (inConvexRadiusA / v_len); + outPointB -= ioV * (inConvexRadiusB / v_len); + return EStatus::Colliding; + } + + return EStatus::Indeterminate; + } + + /// Calculates penetration depth between two objects, second step (the EPA step) + /// + /// @param inAIncludingConvexRadius Object A with convex radius + /// @param inBIncludingConvexRadius Object B with convex radius + /// @param inTolerance A factor that determines the accuracy of the result. If the change of the squared distance is less than inTolerance * current_penetration_depth^2 the algorithm will terminate. Should be bigger or equal to FLT_EPSILON. + /// @param outV Direction to move B out of collision along the shortest path (magnitude is meaningless) + /// @param outPointA Position on A that has the least amount of penetration + /// @param outPointB Position on B that has the least amount of penetration + /// Use |outPointB - outPointA| to get the distance of penetration + /// + /// @return False if the objects don't collide, in this case outPointA/outPointB are invalid. + /// True if the objects penetrate + template + bool GetPenetrationDepthStepEPA(const AI &inAIncludingConvexRadius, const BI &inBIncludingConvexRadius, float inTolerance, Vec3 &outV, Vec3 &outPointA, Vec3 &outPointB) + { + JPH_PROFILE_FUNCTION(); + + // Check that the tolerance makes sense (smaller value than this will just result in needless iterations) + JPH_ASSERT(inTolerance >= FLT_EPSILON); + + // Fetch the simplex from GJK algorithm + SupportPoints support_points; + mGJK.GetClosestPointsSimplex(support_points.mY.data(), support_points.mP, support_points.mQ, support_points.mY.GetSizeRef()); + + // Fill up the amount of support points to 4 + switch (support_points.mY.size()) + { + case 1: + { + // 1 vertex, which must be at the origin, which is useless for our purpose + JPH_ASSERT(support_points.mY[0].IsNearZero(Square(mGJKTolerance))); + support_points.mY.pop_back(); + + // Add support points in 4 directions to form a tetrahedron around the origin + int p1, p2, p3, p4; + (void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, Vec3(0, 1, 0), p1); + (void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, Vec3(-1, -1, -1), p2); + (void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, Vec3(1, -1, -1), p3); + (void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, Vec3(0, -1, 1), p4); + JPH_ASSERT(p1 == 0); + JPH_ASSERT(p2 == 1); + JPH_ASSERT(p3 == 2); + JPH_ASSERT(p4 == 3); + break; + } + + case 2: + { + // Two vertices, create 3 extra by taking perpendicular axis and rotating it around in 120 degree increments + Vec3 axis = (support_points.mY[1] - support_points.mY[0]).Normalized(); + Mat44 rotation = Mat44::sRotation(axis, DegreesToRadians(120.0f)); + Vec3 dir1 = axis.GetNormalizedPerpendicular(); + Vec3 dir2 = rotation * dir1; + Vec3 dir3 = rotation * dir2; + int p1, p2, p3; + (void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, dir1, p1); + (void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, dir2, p2); + (void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, dir3, p3); + JPH_ASSERT(p1 == 2); + JPH_ASSERT(p2 == 3); + JPH_ASSERT(p3 == 4); + break; + } + + case 3: + case 4: + // We already have enough points + break; + } + + // Create hull out of the initial points + JPH_ASSERT(support_points.mY.size() >= 3); + EPAConvexHullBuilder hull(support_points.mY); +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + hull.DrawLabel("Build initial hull"); +#endif +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("Init: num_points = %u", (uint)support_points.mY.size()); +#endif + hull.Initialize(0, 1, 2); + for (typename Points::size_type i = 3; i < support_points.mY.size(); ++i) + { + float dist_sq; + Triangle *t = hull.FindFacingTriangle(support_points.mY[i], dist_sq); + if (t != nullptr) + { + EPAConvexHullBuilder::NewTriangles new_triangles; + if (!hull.AddPoint(t, i, FLT_MAX, new_triangles)) + { + // We can't recover from a failure to add a point to the hull because the old triangles have been unlinked already. + // Assume no collision. This can happen if the shapes touch in 1 point (or plane) in which case the hull is degenerate. + return false; + } + } + } + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + hull.DrawLabel("Complete hull"); + + // Generate the hull of the Minkowski difference for visualization + MinkowskiDifference diff(inAIncludingConvexRadius, inBIncludingConvexRadius); + DebugRenderer::GeometryRef geometry = DebugRenderer::sInstance->CreateTriangleGeometryForConvex([&diff](Vec3Arg inDirection) { return diff.GetSupport(inDirection); }); + hull.DrawGeometry(geometry, Color::sYellow); + + hull.DrawLabel("Ensure origin in hull"); +#endif + + // Loop until we are sure that the origin is inside the hull + for (;;) + { + // Get the next closest triangle + Triangle *t = hull.PeekClosestTriangleInQueue(); + + // Don't process removed triangles, just free them (because they're in a heap we don't remove them earlier since we would have to rebuild the sorted heap) + if (t->mRemoved) + { + hull.PopClosestTriangleFromQueue(); + + // If we run out of triangles, we couldn't include the origin in the hull so there must be very little penetration and we report no collision. + if (!hull.HasNextTriangle()) + return false; + + hull.FreeTriangle(t); + continue; + } + + // If the closest to the triangle is zero or positive, the origin is in the hull and we can proceed to the main algorithm + if (t->mClosestLenSq >= 0.0f) + break; + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + hull.DrawLabel("Next iteration"); +#endif +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("EncapsulateOrigin: verts = (%d, %d, %d), closest_dist_sq = %g, centroid = (%g, %g, %g), normal = (%g, %g, %g)", + t->mEdge[0].mStartIdx, t->mEdge[1].mStartIdx, t->mEdge[2].mStartIdx, + t->mClosestLenSq, + t->mCentroid.GetX(), t->mCentroid.GetY(), t->mCentroid.GetZ(), + t->mNormal.GetX(), t->mNormal.GetY(), t->mNormal.GetZ()); +#endif + + // Remove the triangle from the queue before we start adding new ones (which may result in a new closest triangle at the front of the queue) + hull.PopClosestTriangleFromQueue(); + + // Add a support point to get the origin inside the hull + int new_index; + Vec3 w = support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, t->mNormal, new_index); + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + // Draw the point that we're adding + hull.DrawMarker(w, Color::sRed, 1.0f); + hull.DrawWireTriangle(*t, Color::sRed); + hull.DrawState(); +#endif + + // Add the point to the hull, if we fail we terminate and report no collision + EPAConvexHullBuilder::NewTriangles new_triangles; + if (!t->IsFacing(w) || !hull.AddPoint(t, new_index, FLT_MAX, new_triangles)) + return false; + + // The triangle is facing the support point "w" and can now be safely removed + JPH_ASSERT(t->mRemoved); + hull.FreeTriangle(t); + + // If we run out of triangles or points, we couldn't include the origin in the hull so there must be very little penetration and we report no collision. + if (!hull.HasNextTriangle() || support_points.mY.size() >= cMaxPointsToIncludeOriginInHull) + return false; + } + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + hull.DrawLabel("Main algorithm"); +#endif + + // Current closest distance to origin + float closest_dist_sq = FLT_MAX; + + // Remember last good triangle + Triangle *last = nullptr; + + // If we want to flip the penetration depth + bool flip_v_sign = false; + + // Loop until closest point found + do + { + // Get closest triangle to the origin + Triangle *t = hull.PopClosestTriangleFromQueue(); + + // Don't process removed triangles, just free them (because they're in a heap we don't remove them earlier since we would have to rebuild the sorted heap) + if (t->mRemoved) + { + hull.FreeTriangle(t); + continue; + } + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + hull.DrawLabel("Next iteration"); +#endif +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("FindClosest: verts = (%d, %d, %d), closest_len_sq = %g, centroid = (%g, %g, %g), normal = (%g, %g, %g)", + t->mEdge[0].mStartIdx, t->mEdge[1].mStartIdx, t->mEdge[2].mStartIdx, + t->mClosestLenSq, + t->mCentroid.GetX(), t->mCentroid.GetY(), t->mCentroid.GetZ(), + t->mNormal.GetX(), t->mNormal.GetY(), t->mNormal.GetZ()); +#endif + // Check if next triangle is further away than closest point, we've found the closest point + if (t->mClosestLenSq >= closest_dist_sq) + break; + + // Replace last good with this triangle + if (last != nullptr) + hull.FreeTriangle(last); + last = t; + + // Add support point in direction of normal of the plane + // Note that the article uses the closest point between the origin and plane, but this always has the exact same direction as the normal (if the origin is behind the plane) + // and this way we do less calculations and lose less precision + int new_index; + Vec3 w = support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, t->mNormal, new_index); + + // Project w onto the triangle normal + float dot = t->mNormal.Dot(w); + + // Check if we just found a separating axis. This can happen if the shape shrunk by convex radius and then expanded by + // convex radius is bigger then the original shape due to inaccuracies in the shrinking process. + if (dot < 0.0f) + return false; + + // Get the distance squared (along normal) to the support point + float dist_sq = Square(dot) / t->mNormal.LengthSq(); + +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("FindClosest: w = (%g, %g, %g), dot = %g, dist_sq = %g", + w.GetX(), w.GetY(), w.GetZ(), + dot, dist_sq); +#endif +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + // Draw the point that we're adding + hull.DrawMarker(w, Color::sPurple, 1.0f); + hull.DrawWireTriangle(*t, Color::sPurple); + hull.DrawState(); +#endif + + // If the error became small enough, we've converged + if (dist_sq - t->mClosestLenSq < t->mClosestLenSq * inTolerance) + { +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("Converged"); +#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG + break; + } + + // Keep track of the minimum distance + closest_dist_sq = min(closest_dist_sq, dist_sq); + + // If the triangle thinks this point is not front facing, we've reached numerical precision and we're done + if (!t->IsFacing(w)) + { +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("Not facing triangle"); +#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG + break; + } + + // Add point to hull + EPAConvexHullBuilder::NewTriangles new_triangles; + if (!hull.AddPoint(t, new_index, closest_dist_sq, new_triangles)) + { +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("Could not add point"); +#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG + break; + } + + // If the hull is starting to form defects then we're reaching numerical precision and we have to stop + bool has_defect = false; + for (const Triangle *nt : new_triangles) + if (nt->IsFacingOrigin()) + { + has_defect = true; + break; + } + if (has_defect) + { +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("Has defect"); +#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG + // When the hull has defects it is possible that the origin has been classified on the wrong side of the triangle + // so we do an additional check to see if the penetration in the -triangle normal direction is smaller than + // the penetration in the triangle normal direction. If so we must flip the sign of the penetration depth. + Vec3 w2 = inAIncludingConvexRadius.GetSupport(-t->mNormal) - inBIncludingConvexRadius.GetSupport(t->mNormal); + float dot2 = -t->mNormal.Dot(w2); + if (dot2 < dot) + flip_v_sign = true; + break; + } + } + while (hull.HasNextTriangle() && support_points.mY.size() < cMaxPoints); + + // Determine closest points, if last == null it means the hull was a plane so there's no penetration + if (last == nullptr) + return false; + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + hull.DrawLabel("Closest found"); + hull.DrawWireTriangle(*last, Color::sWhite); + hull.DrawArrow(last->mCentroid, last->mCentroid + last->mNormal.NormalizedOr(Vec3::sZero()), Color::sWhite, 0.1f); + hull.DrawState(); +#endif + + // Calculate penetration by getting the vector from the origin to the closest point on the triangle: + // distance = (centroid - origin) . normal / |normal|, closest = origin + distance * normal / |normal| + outV = (last->mCentroid.Dot(last->mNormal) / last->mNormal.LengthSq()) * last->mNormal; + + // If penetration is near zero, treat this as a non collision since we cannot find a good normal + if (outV.IsNearZero()) + return false; + + // Check if we have to flip the sign of the penetration depth + if (flip_v_sign) + outV = -outV; + + // Use the barycentric coordinates for the closest point to the origin to find the contact points on A and B + Vec3 p0 = support_points.mP[last->mEdge[0].mStartIdx]; + Vec3 p1 = support_points.mP[last->mEdge[1].mStartIdx]; + Vec3 p2 = support_points.mP[last->mEdge[2].mStartIdx]; + + Vec3 q0 = support_points.mQ[last->mEdge[0].mStartIdx]; + Vec3 q1 = support_points.mQ[last->mEdge[1].mStartIdx]; + Vec3 q2 = support_points.mQ[last->mEdge[2].mStartIdx]; + + if (last->mLambdaRelativeTo0) + { + // y0 was the reference vertex + outPointA = p0 + last->mLambda[0] * (p1 - p0) + last->mLambda[1] * (p2 - p0); + outPointB = q0 + last->mLambda[0] * (q1 - q0) + last->mLambda[1] * (q2 - q0); + } + else + { + // y1 was the reference vertex + outPointA = p1 + last->mLambda[0] * (p0 - p1) + last->mLambda[1] * (p2 - p1); + outPointB = q1 + last->mLambda[0] * (q0 - q1) + last->mLambda[1] * (q2 - q1); + } + + return true; + } + + /// This function combines the GJK and EPA steps and is provided as a convenience function. + /// Note: less performant since you're providing all support functions in one go + /// Note 2: You need to initialize ioV, see documentation at GetPenetrationDepthStepGJK! + template + bool GetPenetrationDepth(const AE &inAExcludingConvexRadius, const AI &inAIncludingConvexRadius, float inConvexRadiusA, const BE &inBExcludingConvexRadius, const BI &inBIncludingConvexRadius, float inConvexRadiusB, float inCollisionToleranceSq, float inPenetrationTolerance, Vec3 &ioV, Vec3 &outPointA, Vec3 &outPointB) + { + // Check result of collision detection + switch (GetPenetrationDepthStepGJK(inAExcludingConvexRadius, inConvexRadiusA, inBExcludingConvexRadius, inConvexRadiusB, inCollisionToleranceSq, ioV, outPointA, outPointB)) + { + case EPAPenetrationDepth::EStatus::Colliding: + return true; + + case EPAPenetrationDepth::EStatus::NotColliding: + return false; + + case EPAPenetrationDepth::EStatus::Indeterminate: + return GetPenetrationDepthStepEPA(inAIncludingConvexRadius, inBIncludingConvexRadius, inPenetrationTolerance, ioV, outPointA, outPointB); + } + + JPH_ASSERT(false); + return false; + } + + /// Test if a cast shape inA moving from inStart to lambda * inStart.GetTranslation() + inDirection where lambda e [0, ioLambda> intersects inB + /// + /// @param inStart Start position and orientation of the convex object + /// @param inDirection Direction of the sweep (ioLambda * inDirection determines length) + /// @param inCollisionTolerance The minimal distance between A and B before they are considered colliding + /// @param inPenetrationTolerance A factor that determines the accuracy of the result. If the change of the squared distance is less than inTolerance * current_penetration_depth^2 the algorithm will terminate. Should be bigger or equal to FLT_EPSILON. + /// @param inA The convex object A, must support the GetSupport(Vec3) function. + /// @param inB The convex object B, must support the GetSupport(Vec3) function. + /// @param inConvexRadiusA The convex radius of A, this will be added on all sides to pad A. + /// @param inConvexRadiusB The convex radius of B, this will be added on all sides to pad B. + /// @param inReturnDeepestPoint If the shapes are initially intersecting this determines if the EPA algorithm will run to find the deepest point + /// @param ioLambda The max fraction along the sweep, on output updated with the actual collision fraction. + /// @param outPointA is the contact point on A + /// @param outPointB is the contact point on B + /// @param outContactNormal is either the contact normal when the objects are touching or the penetration axis when the objects are penetrating at the start of the sweep (pointing from A to B, length will not be 1) + /// + /// @return true if the a hit was found, in which case ioLambda, outPointA, outPointB and outSurfaceNormal are updated. + template + bool CastShape(Mat44Arg inStart, Vec3Arg inDirection, float inCollisionTolerance, float inPenetrationTolerance, const A &inA, const B &inB, float inConvexRadiusA, float inConvexRadiusB, bool inReturnDeepestPoint, float &ioLambda, Vec3 &outPointA, Vec3 &outPointB, Vec3 &outContactNormal) + { + JPH_IF_ENABLE_ASSERTS(mGJKTolerance = inCollisionTolerance;) + + // First determine if there's a collision at all + if (!mGJK.CastShape(inStart, inDirection, inCollisionTolerance, inA, inB, inConvexRadiusA, inConvexRadiusB, ioLambda, outPointA, outPointB, outContactNormal)) + return false; + + // When our contact normal is too small, we don't have an accurate result + bool contact_normal_invalid = outContactNormal.IsNearZero(Square(inCollisionTolerance)); + + if (inReturnDeepestPoint + && ioLambda == 0.0f // Only when lambda = 0 we can have the bodies overlap + && (inConvexRadiusA + inConvexRadiusB == 0.0f // When no convex radius was provided we can never trust contact points at lambda = 0 + || contact_normal_invalid)) + { + // If we're initially intersecting, we need to run the EPA algorithm in order to find the deepest contact point + AddConvexRadius add_convex_a(inA, inConvexRadiusA); + AddConvexRadius add_convex_b(inB, inConvexRadiusB); + TransformedConvexObject> transformed_a(inStart, add_convex_a); + if (!GetPenetrationDepthStepEPA(transformed_a, add_convex_b, inPenetrationTolerance, outContactNormal, outPointA, outPointB)) + return false; + } + else if (contact_normal_invalid) + { + // If we weren't able to calculate a contact normal, use the cast direction instead + outContactNormal = inDirection; + } + + return true; + } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/Ellipse.h b/thirdparty/jolt_physics/Jolt/Geometry/Ellipse.h new file mode 100644 index 000000000000..bfa508faae86 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/Ellipse.h @@ -0,0 +1,77 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Ellipse centered around the origin +/// @see https://en.wikipedia.org/wiki/Ellipse +class Ellipse +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct ellipse with radius A along the X-axis and B along the Y-axis + Ellipse(float inA, float inB) : mA(inA), mB(inB) { JPH_ASSERT(inA > 0.0f); JPH_ASSERT(inB > 0.0f); } + + /// Check if inPoint is inside the ellipse + bool IsInside(const Float2 &inPoint) const + { + return Square(inPoint.x / mA) + Square(inPoint.y / mB) <= 1.0f; + } + + /// Get the closest point on the ellipse to inPoint + /// Assumes inPoint is outside the ellipse + /// @see Rotation Joint Limits in Quaternion Space by Gino van den Bergen, section 10.1 in Game Engine Gems 3. + Float2 GetClosestPoint(const Float2 &inPoint) const + { + float a_sq = Square(mA); + float b_sq = Square(mB); + + // Equation of ellipse: f(x, y) = (x/a)^2 + (y/b)^2 - 1 = 0 [1] + // Normal on surface: (df/dx, df/dy) = (2 x / a^2, 2 y / b^2) + // Closest point (x', y') on ellipse to point (x, y): (x', y') + t (x / a^2, y / b^2) = (x, y) + // <=> (x', y') = (a^2 x / (t + a^2), b^2 y / (t + b^2)) + // Requiring point to be on ellipse (substituting into [1]): g(t) = (a x / (t + a^2))^2 + (b y / (t + b^2))^2 - 1 = 0 + + // Newton raphson iteration, starting at t = 0 + float t = 0.0f; + for (;;) + { + // Calculate g(t) + float t_plus_a_sq = t + a_sq; + float t_plus_b_sq = t + b_sq; + float gt = Square(mA * inPoint.x / t_plus_a_sq) + Square(mB * inPoint.y / t_plus_b_sq) - 1.0f; + + // Check if g(t) it is close enough to zero + if (abs(gt) < 1.0e-6f) + return Float2(a_sq * inPoint.x / t_plus_a_sq, b_sq * inPoint.y / t_plus_b_sq); + + // Get derivative dg/dt = g'(t) = -2 (b^2 y^2 / (t + b^2)^3 + a^2 x^2 / (t + a^2)^3) + float gt_accent = -2.0f * + (a_sq * Square(inPoint.x) / Cubed(t_plus_a_sq) + + b_sq * Square(inPoint.y) / Cubed(t_plus_b_sq)); + + // Calculate t for next iteration: tn+1 = tn - g(t) / g'(t) + float tn = t - gt / gt_accent; + t = tn; + } + } + + /// Get normal at point inPoint (non-normalized vector) + Float2 GetNormal(const Float2 &inPoint) const + { + // Calculated by [d/dx f(x, y), d/dy f(x, y)], where f(x, y) is the ellipse equation from above + return Float2(inPoint.x / Square(mA), inPoint.y / Square(mB)); + } + +private: + float mA; ///< Radius along X-axis + float mB; ///< Radius along Y-axis +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/GJKClosestPoint.h b/thirdparty/jolt_physics/Jolt/Geometry/GJKClosestPoint.h new file mode 100644 index 000000000000..e5f49b65a0be --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/GJKClosestPoint.h @@ -0,0 +1,946 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +//#define JPH_GJK_DEBUG +#ifdef JPH_GJK_DEBUG + #include + #include +#endif + +JPH_NAMESPACE_BEGIN + +/// Convex vs convex collision detection +/// Based on: A Fast and Robust GJK Implementation for Collision Detection of Convex Objects - Gino van den Bergen +class GJKClosestPoint : public NonCopyable +{ +private: + /// Get new closest point to origin given simplex mY of mNumPoints points + /// + /// @param inPrevVLenSq Length of |outV|^2 from the previous iteration, used as a maximum value when selecting a new closest point. + /// @param outV Closest point + /// @param outVLenSq |outV|^2 + /// @param outSet Set of points that form the new simplex closest to the origin (bit 1 = mY[0], bit 2 = mY[1], ...) + /// + /// If LastPointPartOfClosestFeature is true then the last point added will be assumed to be part of the closest feature and the function will do less work. + /// + /// @return True if new closest point was found. + /// False if the function failed, in this case the output variables are not modified + template + bool GetClosest(float inPrevVLenSq, Vec3 &outV, float &outVLenSq, uint32 &outSet) const + { +#ifdef JPH_GJK_DEBUG + for (int i = 0; i < mNumPoints; ++i) + Trace("y[%d] = [%s], |y[%d]| = %g", i, ConvertToString(mY[i]).c_str(), i, (double)mY[i].Length()); +#endif + + uint32 set; + Vec3 v; + + switch (mNumPoints) + { + case 1: + // Single point + set = 0b0001; + v = mY[0]; + break; + + case 2: + // Line segment + v = ClosestPoint::GetClosestPointOnLine(mY[0], mY[1], set); + break; + + case 3: + // Triangle + v = ClosestPoint::GetClosestPointOnTriangle(mY[0], mY[1], mY[2], set); + break; + + case 4: + // Tetrahedron + v = ClosestPoint::GetClosestPointOnTetrahedron(mY[0], mY[1], mY[2], mY[3], set); + break; + + default: + JPH_ASSERT(false); + return false; + } + +#ifdef JPH_GJK_DEBUG + Trace("GetClosest: set = 0b%s, v = [%s], |v| = %g", NibbleToBinary(set), ConvertToString(v).c_str(), (double)v.Length()); +#endif + + float v_len_sq = v.LengthSq(); + if (v_len_sq < inPrevVLenSq) // Note, comparison order important: If v_len_sq is NaN then this expression will be false so we will return false + { + // Return closest point + outV = v; + outVLenSq = v_len_sq; + outSet = set; + return true; + } + + // No better match found +#ifdef JPH_GJK_DEBUG + Trace("New closer point is further away, failed to converge"); +#endif + return false; + } + + // Get max(|Y_0|^2 .. |Y_n|^2) + float GetMaxYLengthSq() const + { + float y_len_sq = mY[0].LengthSq(); + for (int i = 1; i < mNumPoints; ++i) + y_len_sq = max(y_len_sq, mY[i].LengthSq()); + return y_len_sq; + } + + // Remove points that are not in the set, only updates mY + void UpdatePointSetY(uint32 inSet) + { + int num_points = 0; + for (int i = 0; i < mNumPoints; ++i) + if ((inSet & (1 << i)) != 0) + { + mY[num_points] = mY[i]; + ++num_points; + } + mNumPoints = num_points; + } + + // Remove points that are not in the set, only updates mP + void UpdatePointSetP(uint32 inSet) + { + int num_points = 0; + for (int i = 0; i < mNumPoints; ++i) + if ((inSet & (1 << i)) != 0) + { + mP[num_points] = mP[i]; + ++num_points; + } + mNumPoints = num_points; + } + + // Remove points that are not in the set, only updates mP and mQ + void UpdatePointSetPQ(uint32 inSet) + { + int num_points = 0; + for (int i = 0; i < mNumPoints; ++i) + if ((inSet & (1 << i)) != 0) + { + mP[num_points] = mP[i]; + mQ[num_points] = mQ[i]; + ++num_points; + } + mNumPoints = num_points; + } + + // Remove points that are not in the set, updates mY, mP and mQ + void UpdatePointSetYPQ(uint32 inSet) + { + int num_points = 0; + for (int i = 0; i < mNumPoints; ++i) + if ((inSet & (1 << i)) != 0) + { + mY[num_points] = mY[i]; + mP[num_points] = mP[i]; + mQ[num_points] = mQ[i]; + ++num_points; + } + mNumPoints = num_points; + } + + // Calculate closest points on A and B + void CalculatePointAAndB(Vec3 &outPointA, Vec3 &outPointB) const + { + switch (mNumPoints) + { + case 1: + outPointA = mP[0]; + outPointB = mQ[0]; + break; + + case 2: + { + float u, v; + ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], u, v); + outPointA = u * mP[0] + v * mP[1]; + outPointB = u * mQ[0] + v * mQ[1]; + } + break; + + case 3: + { + float u, v, w; + ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], mY[2], u, v, w); + outPointA = u * mP[0] + v * mP[1] + w * mP[2]; + outPointB = u * mQ[0] + v * mQ[1] + w * mQ[2]; + } + break; + + case 4: + #ifdef JPH_DEBUG + memset(&outPointA, 0xcd, sizeof(outPointA)); + memset(&outPointB, 0xcd, sizeof(outPointB)); + #endif + break; + } + } + +public: + /// Test if inA and inB intersect + /// + /// @param inA The convex object A, must support the GetSupport(Vec3) function. + /// @param inB The convex object B, must support the GetSupport(Vec3) function. + /// @param inTolerance Minimal distance between objects when the objects are considered to be colliding + /// @param ioV is used as initial separating axis (provide a zero vector if you don't know yet) + /// + /// @return True if they intersect (in which case ioV = (0, 0, 0)). + /// False if they don't intersect in which case ioV is a separating axis in the direction from A to B (magnitude is meaningless) + template + bool Intersects(const A &inA, const B &inB, float inTolerance, Vec3 &ioV) + { + float tolerance_sq = Square(inTolerance); + + // Reset state + mNumPoints = 0; + +#ifdef JPH_GJK_DEBUG + for (int i = 0; i < 4; ++i) + mY[i] = Vec3::sZero(); +#endif + + // Previous length^2 of v + float prev_v_len_sq = FLT_MAX; + + for (;;) + { +#ifdef JPH_GJK_DEBUG + Trace("v = [%s], num_points = %d", ConvertToString(ioV).c_str(), mNumPoints); +#endif + + // Get support points for shape A and B in the direction of v + Vec3 p = inA.GetSupport(ioV); + Vec3 q = inB.GetSupport(-ioV); + + // Get support point of the minkowski sum A - B of v + Vec3 w = p - q; + + // If the support point sA-B(v) is in the opposite direction as v, then we have found a separating axis and there is no intersection + if (ioV.Dot(w) < 0.0f) + { + // Separating axis found +#ifdef JPH_GJK_DEBUG + Trace("Separating axis"); +#endif + return false; + } + + // Store the point for later use + mY[mNumPoints] = w; + ++mNumPoints; + +#ifdef JPH_GJK_DEBUG + Trace("w = [%s]", ConvertToString(w).c_str()); +#endif + + // Determine the new closest point + float v_len_sq; // Length^2 of v + uint32 set; // Set of points that form the new simplex + if (!GetClosest(prev_v_len_sq, ioV, v_len_sq, set)) + return false; + + // If there are 4 points, the origin is inside the tetrahedron and we're done + if (set == 0xf) + { +#ifdef JPH_GJK_DEBUG + Trace("Full simplex"); +#endif + ioV = Vec3::sZero(); + return true; + } + + // If v is very close to zero, we consider this a collision + if (v_len_sq <= tolerance_sq) + { +#ifdef JPH_GJK_DEBUG + Trace("Distance zero"); +#endif + ioV = Vec3::sZero(); + return true; + } + + // If v is very small compared to the length of y, we also consider this a collision + if (v_len_sq <= FLT_EPSILON * GetMaxYLengthSq()) + { +#ifdef JPH_GJK_DEBUG + Trace("Machine precision reached"); +#endif + ioV = Vec3::sZero(); + return true; + } + + // The next separation axis to test is the negative of the closest point of the Minkowski sum to the origin + // Note: This must be done before terminating as converged since the separating axis is -v + ioV = -ioV; + + // If the squared length of v is not changing enough, we've converged and there is no collision + JPH_ASSERT(prev_v_len_sq >= v_len_sq); + if (prev_v_len_sq - v_len_sq <= FLT_EPSILON * prev_v_len_sq) + { + // v is a separating axis +#ifdef JPH_GJK_DEBUG + Trace("Converged"); +#endif + return false; + } + prev_v_len_sq = v_len_sq; + + // Update the points of the simplex + UpdatePointSetY(set); + } + } + + /// Get closest points between inA and inB + /// + /// @param inA The convex object A, must support the GetSupport(Vec3) function. + /// @param inB The convex object B, must support the GetSupport(Vec3) function. + /// @param inTolerance The minimal distance between A and B before the objects are considered colliding and processing is terminated. + /// @param inMaxDistSq The maximum squared distance between A and B before the objects are considered infinitely far away and processing is terminated. + /// @param ioV Initial guess for the separating axis. Start with any non-zero vector if you don't know. + /// If return value is 0, ioV = (0, 0, 0). + /// If the return value is bigger than 0 but smaller than FLT_MAX, ioV will be the separating axis in the direction from A to B and its length the squared distance between A and B. + /// If the return value is FLT_MAX, ioV will be the separating axis in the direction from A to B and the magnitude of the vector is meaningless. + /// @param outPointA , outPointB + /// If the return value is 0 the points are invalid. + /// If the return value is bigger than 0 but smaller than FLT_MAX these will contain the closest point on A and B. + /// If the return value is FLT_MAX the points are invalid. + /// + /// @return The squared distance between A and B or FLT_MAX when they are further away than inMaxDistSq. + template + float GetClosestPoints(const A &inA, const B &inB, float inTolerance, float inMaxDistSq, Vec3 &ioV, Vec3 &outPointA, Vec3 &outPointB) + { + float tolerance_sq = Square(inTolerance); + + // Reset state + mNumPoints = 0; + +#ifdef JPH_GJK_DEBUG + // Generate the hull of the Minkowski difference for visualization + MinkowskiDifference diff(inA, inB); + mGeometry = DebugRenderer::sInstance->CreateTriangleGeometryForConvex([&diff](Vec3Arg inDirection) { return diff.GetSupport(inDirection); }); + + for (int i = 0; i < 4; ++i) + { + mY[i] = Vec3::sZero(); + mP[i] = Vec3::sZero(); + mQ[i] = Vec3::sZero(); + } +#endif + + // Length^2 of v + float v_len_sq = ioV.LengthSq(); + + // Previous length^2 of v + float prev_v_len_sq = FLT_MAX; + + for (;;) + { +#ifdef JPH_GJK_DEBUG + Trace("v = [%s], num_points = %d", ConvertToString(ioV).c_str(), mNumPoints); +#endif + + // Get support points for shape A and B in the direction of v + Vec3 p = inA.GetSupport(ioV); + Vec3 q = inB.GetSupport(-ioV); + + // Get support point of the minkowski sum A - B of v + Vec3 w = p - q; + + float dot = ioV.Dot(w); + +#ifdef JPH_GJK_DEBUG + // Draw -ioV to show the closest point to the origin from the previous simplex + DebugRenderer::sInstance->DrawArrow(mOffset, mOffset - ioV, Color::sOrange, 0.05f); + + // Draw ioV to show where we're probing next + DebugRenderer::sInstance->DrawArrow(mOffset, mOffset + ioV, Color::sCyan, 0.05f); + + // Draw w, the support point + DebugRenderer::sInstance->DrawArrow(mOffset, mOffset + w, Color::sGreen, 0.05f); + DebugRenderer::sInstance->DrawMarker(mOffset + w, Color::sGreen, 1.0f); + + // Draw the simplex and the Minkowski difference around it + DrawState(); +#endif + + // Test if we have a separation of more than inMaxDistSq, in which case we terminate early + if (dot < 0.0f && dot * dot > v_len_sq * inMaxDistSq) + { +#ifdef JPH_GJK_DEBUG + Trace("Distance bigger than max"); +#endif +#ifdef JPH_DEBUG + memset(&outPointA, 0xcd, sizeof(outPointA)); + memset(&outPointB, 0xcd, sizeof(outPointB)); +#endif + return FLT_MAX; + } + + // Store the point for later use + mY[mNumPoints] = w; + mP[mNumPoints] = p; + mQ[mNumPoints] = q; + ++mNumPoints; + +#ifdef JPH_GJK_DEBUG + Trace("w = [%s]", ConvertToString(w).c_str()); +#endif + + uint32 set; + if (!GetClosest(prev_v_len_sq, ioV, v_len_sq, set)) + { + --mNumPoints; // Undo add last point + break; + } + + // If there are 4 points, the origin is inside the tetrahedron and we're done + if (set == 0xf) + { +#ifdef JPH_GJK_DEBUG + Trace("Full simplex"); +#endif + ioV = Vec3::sZero(); + v_len_sq = 0.0f; + break; + } + + // Update the points of the simplex + UpdatePointSetYPQ(set); + + // If v is very close to zero, we consider this a collision + if (v_len_sq <= tolerance_sq) + { +#ifdef JPH_GJK_DEBUG + Trace("Distance zero"); +#endif + ioV = Vec3::sZero(); + v_len_sq = 0.0f; + break; + } + + // If v is very small compared to the length of y, we also consider this a collision +#ifdef JPH_GJK_DEBUG + Trace("Check v small compared to y: %g <= %g", (double)v_len_sq, (double)(FLT_EPSILON * GetMaxYLengthSq())); +#endif + if (v_len_sq <= FLT_EPSILON * GetMaxYLengthSq()) + { +#ifdef JPH_GJK_DEBUG + Trace("Machine precision reached"); +#endif + ioV = Vec3::sZero(); + v_len_sq = 0.0f; + break; + } + + // The next separation axis to test is the negative of the closest point of the Minkowski sum to the origin + // Note: This must be done before terminating as converged since the separating axis is -v + ioV = -ioV; + + // If the squared length of v is not changing enough, we've converged and there is no collision +#ifdef JPH_GJK_DEBUG + Trace("Check v not changing enough: %g <= %g", (double)(prev_v_len_sq - v_len_sq), (double)(FLT_EPSILON * prev_v_len_sq)); +#endif + JPH_ASSERT(prev_v_len_sq >= v_len_sq); + if (prev_v_len_sq - v_len_sq <= FLT_EPSILON * prev_v_len_sq) + { + // v is a separating axis +#ifdef JPH_GJK_DEBUG + Trace("Converged"); +#endif + break; + } + prev_v_len_sq = v_len_sq; + } + + // Get the closest points + CalculatePointAAndB(outPointA, outPointB); + +#ifdef JPH_GJK_DEBUG + Trace("Return: v = [%s], |v| = %g", ConvertToString(ioV).c_str(), (double)ioV.Length()); + + // Draw -ioV to show the closest point to the origin from the previous simplex + DebugRenderer::sInstance->DrawArrow(mOffset, mOffset - ioV, Color::sOrange, 0.05f); + + // Draw the closest points + DebugRenderer::sInstance->DrawMarker(mOffset + outPointA, Color::sGreen, 1.0f); + DebugRenderer::sInstance->DrawMarker(mOffset + outPointB, Color::sPurple, 1.0f); + + // Draw the simplex and the Minkowski difference around it + DrawState(); +#endif + + JPH_ASSERT(ioV.LengthSq() == v_len_sq); + return v_len_sq; + } + + /// Get the resulting simplex after the GetClosestPoints algorithm finishes. + /// If it returned a squared distance of 0, the origin will be contained in the simplex. + void GetClosestPointsSimplex(Vec3 *outY, Vec3 *outP, Vec3 *outQ, uint &outNumPoints) const + { + uint size = sizeof(Vec3) * mNumPoints; + memcpy(outY, mY, size); + memcpy(outP, mP, size); + memcpy(outQ, mQ, size); + outNumPoints = mNumPoints; + } + + /// Test if a ray inRayOrigin + lambda * inRayDirection for lambda e [0, ioLambda> intersects inA + /// + /// Code based upon: Ray Casting against General Convex Objects with Application to Continuous Collision Detection - Gino van den Bergen + /// + /// @param inRayOrigin Origin of the ray + /// @param inRayDirection Direction of the ray (ioLambda * inDirection determines length) + /// @param inTolerance The minimal distance between the ray and A before it is considered colliding + /// @param inA A convex object that has the GetSupport(Vec3) function + /// @param ioLambda The max fraction along the ray, on output updated with the actual collision fraction. + /// + /// @return true if a hit was found, ioLambda is the solution for lambda. + template + bool CastRay(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, float inTolerance, const A &inA, float &ioLambda) + { + float tolerance_sq = Square(inTolerance); + + // Reset state + mNumPoints = 0; + + float lambda = 0.0f; + Vec3 x = inRayOrigin; + Vec3 v = x - inA.GetSupport(Vec3::sZero()); + float v_len_sq = FLT_MAX; + bool allow_restart = false; + + for (;;) + { +#ifdef JPH_GJK_DEBUG + Trace("v = [%s], num_points = %d", ConvertToString(v).c_str(), mNumPoints); +#endif + + // Get new support point + Vec3 p = inA.GetSupport(v); + Vec3 w = x - p; + +#ifdef JPH_GJK_DEBUG + Trace("w = [%s]", ConvertToString(w).c_str()); +#endif + + float v_dot_w = v.Dot(w); +#ifdef JPH_GJK_DEBUG + Trace("v . w = %g", (double)v_dot_w); +#endif + if (v_dot_w > 0.0f) + { + // If ray and normal are in the same direction, we've passed A and there's no collision + float v_dot_r = v.Dot(inRayDirection); +#ifdef JPH_GJK_DEBUG + Trace("v . r = %g", (double)v_dot_r); +#endif + if (v_dot_r >= 0.0f) + return false; + + // Update the lower bound for lambda + float delta = v_dot_w / v_dot_r; + float old_lambda = lambda; + lambda -= delta; +#ifdef JPH_GJK_DEBUG + Trace("lambda = %g, delta = %g", (double)lambda, (double)delta); +#endif + + // If lambda didn't change, we cannot converge any further and we assume a hit + if (old_lambda == lambda) + break; + + // If lambda is bigger or equal than max, we don't have a hit + if (lambda >= ioLambda) + return false; + + // Update x to new closest point on the ray + x = inRayOrigin + lambda * inRayDirection; + + // We've shifted x, so reset v_len_sq so that it is not used as early out for GetClosest + v_len_sq = FLT_MAX; + + // We allow rebuilding the simplex once after x changes because the simplex was built + // for another x and numerical round off builds up as you keep adding points to an + // existing simplex + allow_restart = true; + } + + // Add p to set P: P = P U {p} + mP[mNumPoints] = p; + ++mNumPoints; + + // Calculate Y = {x} - P + for (int i = 0; i < mNumPoints; ++i) + mY[i] = x - mP[i]; + + // Determine the new closest point from Y to origin + uint32 set; // Set of points that form the new simplex + if (!GetClosest(v_len_sq, v, v_len_sq, set)) + { +#ifdef JPH_GJK_DEBUG + Trace("Failed to converge"); +#endif + + // Only allow 1 restart, if we still can't get a closest point + // we're so close that we return this as a hit + if (!allow_restart) + break; + + // If we fail to converge, we start again with the last point as simplex +#ifdef JPH_GJK_DEBUG + Trace("Restarting"); +#endif + allow_restart = false; + mP[0] = p; + mNumPoints = 1; + v = x - p; + v_len_sq = FLT_MAX; + continue; + } + else if (set == 0xf) + { +#ifdef JPH_GJK_DEBUG + Trace("Full simplex"); +#endif + + // We're inside the tetrahedron, we have a hit (verify that length of v is 0) + JPH_ASSERT(v_len_sq == 0.0f); + break; + } + + // Update the points P to form the new simplex + // Note: We're not updating Y as Y will shift with x so we have to calculate it every iteration + UpdatePointSetP(set); + + // Check if x is close enough to inA + if (v_len_sq <= tolerance_sq) + { +#ifdef JPH_GJK_DEBUG + Trace("Converged"); +#endif + break; + } + } + + // Store hit fraction + ioLambda = lambda; + return true; + } + + /// Test if a cast shape inA moving from inStart to lambda * inStart.GetTranslation() + inDirection where lambda e [0, ioLambda> intersects inB + /// + /// @param inStart Start position and orientation of the convex object + /// @param inDirection Direction of the sweep (ioLambda * inDirection determines length) + /// @param inTolerance The minimal distance between A and B before they are considered colliding + /// @param inA The convex object A, must support the GetSupport(Vec3) function. + /// @param inB The convex object B, must support the GetSupport(Vec3) function. + /// @param ioLambda The max fraction along the sweep, on output updated with the actual collision fraction. + /// + /// @return true if a hit was found, ioLambda is the solution for lambda. + template + bool CastShape(Mat44Arg inStart, Vec3Arg inDirection, float inTolerance, const A &inA, const B &inB, float &ioLambda) + { + // Transform the shape to be cast to the starting position + TransformedConvexObject transformed_a(inStart, inA); + + // Calculate the minkowski difference inB - inA + // inA is moving, so we need to add the back side of inB to the front side of inA + MinkowskiDifference difference(inB, transformed_a); + + // Do a raycast against the Minkowski difference + return CastRay(Vec3::sZero(), inDirection, inTolerance, difference, ioLambda); + } + + /// Test if a cast shape inA moving from inStart to lambda * inStart.GetTranslation() + inDirection where lambda e [0, ioLambda> intersects inB + /// + /// @param inStart Start position and orientation of the convex object + /// @param inDirection Direction of the sweep (ioLambda * inDirection determines length) + /// @param inTolerance The minimal distance between A and B before they are considered colliding + /// @param inA The convex object A, must support the GetSupport(Vec3) function. + /// @param inB The convex object B, must support the GetSupport(Vec3) function. + /// @param inConvexRadiusA The convex radius of A, this will be added on all sides to pad A. + /// @param inConvexRadiusB The convex radius of B, this will be added on all sides to pad B. + /// @param ioLambda The max fraction along the sweep, on output updated with the actual collision fraction. + /// @param outPointA is the contact point on A (if outSeparatingAxis is near zero, this may not be not the deepest point) + /// @param outPointB is the contact point on B (if outSeparatingAxis is near zero, this may not be not the deepest point) + /// @param outSeparatingAxis On return this will contain a vector that points from A to B along the smallest distance of separation. + /// The length of this vector indicates the separation of A and B without their convex radius. + /// If it is near zero, the direction may not be accurate as the bodies may overlap when lambda = 0. + /// + /// @return true if a hit was found, ioLambda is the solution for lambda and outPoint and outSeparatingAxis are valid. + template + bool CastShape(Mat44Arg inStart, Vec3Arg inDirection, float inTolerance, const A &inA, const B &inB, float inConvexRadiusA, float inConvexRadiusB, float &ioLambda, Vec3 &outPointA, Vec3 &outPointB, Vec3 &outSeparatingAxis) + { + float tolerance_sq = Square(inTolerance); + + // Calculate how close A and B (without their convex radius) need to be to each other in order for us to consider this a collision + float sum_convex_radius = inConvexRadiusA + inConvexRadiusB; + + // Transform the shape to be cast to the starting position + TransformedConvexObject transformed_a(inStart, inA); + + // Reset state + mNumPoints = 0; + + float lambda = 0.0f; + Vec3 x = Vec3::sZero(); // Since A is already transformed we can start the cast from zero + Vec3 v = -inB.GetSupport(Vec3::sZero()) + transformed_a.GetSupport(Vec3::sZero()); // See CastRay: v = x - inA.GetSupport(Vec3::sZero()) where inA is the Minkowski difference inB - transformed_a (see CastShape above) and x is zero + float v_len_sq = FLT_MAX; + bool allow_restart = false; + + // Keeps track of separating axis of the previous iteration. + // Initialized at zero as we don't know if our first v is actually a separating axis. + Vec3 prev_v = Vec3::sZero(); + + for (;;) + { +#ifdef JPH_GJK_DEBUG + Trace("v = [%s], num_points = %d", ConvertToString(v).c_str(), mNumPoints); +#endif + + // Calculate the minkowski difference inB - inA + // inA is moving, so we need to add the back side of inB to the front side of inA + // Keep the support points on A and B separate so that in the end we can calculate a contact point + Vec3 p = transformed_a.GetSupport(-v); + Vec3 q = inB.GetSupport(v); + Vec3 w = x - (q - p); + +#ifdef JPH_GJK_DEBUG + Trace("w = [%s]", ConvertToString(w).c_str()); +#endif + + // Difference from article to this code: + // We did not include the convex radius in p and q in order to be able to calculate a good separating axis at the end of the algorithm. + // However when moving forward along inDirection we do need to take this into account so that we keep A and B separated by the sum of their convex radii. + // From p we have to subtract: inConvexRadiusA * v / |v| + // To q we have to add: inConvexRadiusB * v / |v| + // This means that to w we have to add: -(inConvexRadiusA + inConvexRadiusB) * v / |v| + // So to v . w we have to add: v . (-(inConvexRadiusA + inConvexRadiusB) * v / |v|) = -(inConvexRadiusA + inConvexRadiusB) * |v| + float v_dot_w = v.Dot(w) - sum_convex_radius * v.Length(); +#ifdef JPH_GJK_DEBUG + Trace("v . w = %g", (double)v_dot_w); +#endif + if (v_dot_w > 0.0f) + { + // If ray and normal are in the same direction, we've passed A and there's no collision + float v_dot_r = v.Dot(inDirection); +#ifdef JPH_GJK_DEBUG + Trace("v . r = %g", (double)v_dot_r); +#endif + if (v_dot_r >= 0.0f) + return false; + + // Update the lower bound for lambda + float delta = v_dot_w / v_dot_r; + float old_lambda = lambda; + lambda -= delta; +#ifdef JPH_GJK_DEBUG + Trace("lambda = %g, delta = %g", (double)lambda, (double)delta); +#endif + + // If lambda didn't change, we cannot converge any further and we assume a hit + if (old_lambda == lambda) + break; + + // If lambda is bigger or equal than max, we don't have a hit + if (lambda >= ioLambda) + return false; + + // Update x to new closest point on the ray + x = lambda * inDirection; + + // We've shifted x, so reset v_len_sq so that it is not used as early out when GetClosest returns false + v_len_sq = FLT_MAX; + + // Now that we've moved, we know that A and B are not intersecting at lambda = 0, so we can update our tolerance to stop iterating + // as soon as A and B are inConvexRadiusA + inConvexRadiusB apart + tolerance_sq = Square(inTolerance + sum_convex_radius); + + // We allow rebuilding the simplex once after x changes because the simplex was built + // for another x and numerical round off builds up as you keep adding points to an + // existing simplex + allow_restart = true; + } + + // Add p to set P, q to set Q: P = P U {p}, Q = Q U {q} + mP[mNumPoints] = p; + mQ[mNumPoints] = q; + ++mNumPoints; + + // Calculate Y = {x} - (Q - P) + for (int i = 0; i < mNumPoints; ++i) + mY[i] = x - (mQ[i] - mP[i]); + + // Determine the new closest point from Y to origin + uint32 set; // Set of points that form the new simplex + if (!GetClosest(v_len_sq, v, v_len_sq, set)) + { +#ifdef JPH_GJK_DEBUG + Trace("Failed to converge"); +#endif + + // Only allow 1 restart, if we still can't get a closest point + // we're so close that we return this as a hit + if (!allow_restart) + break; + + // If we fail to converge, we start again with the last point as simplex +#ifdef JPH_GJK_DEBUG + Trace("Restarting"); +#endif + allow_restart = false; + mP[0] = p; + mQ[0] = q; + mNumPoints = 1; + v = x - q; + v_len_sq = FLT_MAX; + continue; + } + else if (set == 0xf) + { +#ifdef JPH_GJK_DEBUG + Trace("Full simplex"); +#endif + + // We're inside the tetrahedron, we have a hit (verify that length of v is 0) + JPH_ASSERT(v_len_sq == 0.0f); + break; + } + + // Update the points P and Q to form the new simplex + // Note: We're not updating Y as Y will shift with x so we have to calculate it every iteration + UpdatePointSetPQ(set); + + // Check if A and B are touching according to our tolerance + if (v_len_sq <= tolerance_sq) + { +#ifdef JPH_GJK_DEBUG + Trace("Converged"); +#endif + break; + } + + // Store our v to return as separating axis + prev_v = v; + } + + // Calculate Y = {x} - (Q - P) again so we can calculate the contact points + for (int i = 0; i < mNumPoints; ++i) + mY[i] = x - (mQ[i] - mP[i]); + + // Calculate the offset we need to apply to A and B to correct for the convex radius + Vec3 normalized_v = v.NormalizedOr(Vec3::sZero()); + Vec3 convex_radius_a = inConvexRadiusA * normalized_v; + Vec3 convex_radius_b = inConvexRadiusB * normalized_v; + + // Get the contact point + // Note that A and B will coincide when lambda > 0. In this case we calculate only B as it is more accurate as it contains less terms. + switch (mNumPoints) + { + case 1: + outPointB = mQ[0] + convex_radius_b; + outPointA = lambda > 0.0f? outPointB : mP[0] - convex_radius_a; + break; + + case 2: + { + float bu, bv; + ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], bu, bv); + outPointB = bu * mQ[0] + bv * mQ[1] + convex_radius_b; + outPointA = lambda > 0.0f? outPointB : bu * mP[0] + bv * mP[1] - convex_radius_a; + } + break; + + case 3: + case 4: // A full simplex, we can't properly determine a contact point! As contact point we take the closest point of the previous iteration. + { + float bu, bv, bw; + ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], mY[2], bu, bv, bw); + outPointB = bu * mQ[0] + bv * mQ[1] + bw * mQ[2] + convex_radius_b; + outPointA = lambda > 0.0f? outPointB : bu * mP[0] + bv * mP[1] + bw * mP[2] - convex_radius_a; + } + break; + } + + // Store separating axis, in case we have a convex radius we can just return v, + // otherwise v will be very small and we resort to returning previous v as an approximation. + outSeparatingAxis = sum_convex_radius > 0.0f? -v : -prev_v; + + // Store hit fraction + ioLambda = lambda; + return true; + } + +private: +#ifdef JPH_GJK_DEBUG + /// Draw state of algorithm + void DrawState() + { + RMat44 origin = RMat44::sTranslation(mOffset); + + // Draw origin + DebugRenderer::sInstance->DrawCoordinateSystem(origin, 1.0f); + + // Draw the hull + DebugRenderer::sInstance->DrawGeometry(origin, mGeometry->mBounds.Transformed(origin), mGeometry->mBounds.GetExtent().LengthSq(), Color::sYellow, mGeometry); + + // Draw Y + for (int i = 0; i < mNumPoints; ++i) + { + // Draw support point + RVec3 y_i = origin * mY[i]; + DebugRenderer::sInstance->DrawMarker(y_i, Color::sRed, 1.0f); + for (int j = i + 1; j < mNumPoints; ++j) + { + // Draw edge + RVec3 y_j = origin * mY[j]; + DebugRenderer::sInstance->DrawLine(y_i, y_j, Color::sRed); + for (int k = j + 1; k < mNumPoints; ++k) + { + // Make sure triangle faces the origin + RVec3 y_k = origin * mY[k]; + RVec3 center = (y_i + y_j + y_k) / Real(3); + RVec3 normal = (y_j - y_i).Cross(y_k - y_i); + if (normal.Dot(center) < Real(0)) + DebugRenderer::sInstance->DrawTriangle(y_i, y_j, y_k, Color::sLightGrey); + else + DebugRenderer::sInstance->DrawTriangle(y_i, y_k, y_j, Color::sLightGrey); + } + } + } + + // Offset to the right + mOffset += Vec3(mGeometry->mBounds.GetSize().GetX() + 2.0f, 0, 0); + } +#endif // JPH_GJK_DEBUG + + Vec3 mY[4]; ///< Support points on A - B + Vec3 mP[4]; ///< Support point on A + Vec3 mQ[4]; ///< Support point on B + int mNumPoints = 0; ///< Number of points in mY, mP and mQ that are valid + +#ifdef JPH_GJK_DEBUG + DebugRenderer::GeometryRef mGeometry; ///< A visualization of the minkowski difference for state drawing + RVec3 mOffset = RVec3::sZero(); ///< Offset to use for state drawing +#endif +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/IndexedTriangle.h b/thirdparty/jolt_physics/Jolt/Geometry/IndexedTriangle.h new file mode 100644 index 000000000000..7ebffc29c514 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/IndexedTriangle.h @@ -0,0 +1,116 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Triangle with 32-bit indices +class IndexedTriangleNoMaterial +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + IndexedTriangleNoMaterial() = default; + constexpr IndexedTriangleNoMaterial(uint32 inI1, uint32 inI2, uint32 inI3) : mIdx { inI1, inI2, inI3 } { } + + /// Check if two triangles are identical + bool operator == (const IndexedTriangleNoMaterial &inRHS) const + { + return mIdx[0] == inRHS.mIdx[0] && mIdx[1] == inRHS.mIdx[1] && mIdx[2] == inRHS.mIdx[2]; + } + + /// Check if two triangles are equivalent (using the same vertices) + bool IsEquivalent(const IndexedTriangleNoMaterial &inRHS) const + { + return (mIdx[0] == inRHS.mIdx[0] && mIdx[1] == inRHS.mIdx[1] && mIdx[2] == inRHS.mIdx[2]) + || (mIdx[0] == inRHS.mIdx[1] && mIdx[1] == inRHS.mIdx[2] && mIdx[2] == inRHS.mIdx[0]) + || (mIdx[0] == inRHS.mIdx[2] && mIdx[1] == inRHS.mIdx[0] && mIdx[2] == inRHS.mIdx[1]); + } + + /// Check if two triangles are opposite (using the same vertices but in opposing order) + bool IsOpposite(const IndexedTriangleNoMaterial &inRHS) const + { + return (mIdx[0] == inRHS.mIdx[0] && mIdx[1] == inRHS.mIdx[2] && mIdx[2] == inRHS.mIdx[1]) + || (mIdx[0] == inRHS.mIdx[1] && mIdx[1] == inRHS.mIdx[0] && mIdx[2] == inRHS.mIdx[2]) + || (mIdx[0] == inRHS.mIdx[2] && mIdx[1] == inRHS.mIdx[1] && mIdx[2] == inRHS.mIdx[0]); + } + + /// Check if triangle is degenerate + bool IsDegenerate(const VertexList &inVertices) const + { + Vec3 v0(inVertices[mIdx[0]]); + Vec3 v1(inVertices[mIdx[1]]); + Vec3 v2(inVertices[mIdx[2]]); + + return (v1 - v0).Cross(v2 - v0).IsNearZero(); + } + + /// Rotate the vertices so that the second vertex becomes first etc. This does not change the represented triangle. + void Rotate() + { + uint32 tmp = mIdx[0]; + mIdx[0] = mIdx[1]; + mIdx[1] = mIdx[2]; + mIdx[2] = tmp; + } + + /// Get center of triangle + Vec3 GetCentroid(const VertexList &inVertices) const + { + return (Vec3(inVertices[mIdx[0]]) + Vec3(inVertices[mIdx[1]]) + Vec3(inVertices[mIdx[2]])) / 3.0f; + } + + uint32 mIdx[3]; +}; + +/// Triangle with 32-bit indices and material index +class IndexedTriangle : public IndexedTriangleNoMaterial +{ +public: + using IndexedTriangleNoMaterial::IndexedTriangleNoMaterial; + + /// Constructor + constexpr IndexedTriangle(uint32 inI1, uint32 inI2, uint32 inI3, uint32 inMaterialIndex, uint inUserData = 0) : IndexedTriangleNoMaterial(inI1, inI2, inI3), mMaterialIndex(inMaterialIndex), mUserData(inUserData) { } + + /// Check if two triangles are identical + bool operator == (const IndexedTriangle &inRHS) const + { + return mMaterialIndex == inRHS.mMaterialIndex && mUserData == inRHS.mUserData && IndexedTriangleNoMaterial::operator==(inRHS); + } + + /// Rotate the vertices so that the lowest vertex becomes the first. This does not change the represented triangle. + IndexedTriangle GetLowestIndexFirst() const + { + if (mIdx[0] < mIdx[1]) + { + if (mIdx[0] < mIdx[2]) + return IndexedTriangle(mIdx[0], mIdx[1], mIdx[2], mMaterialIndex, mUserData); // 0 is smallest + else + return IndexedTriangle(mIdx[2], mIdx[0], mIdx[1], mMaterialIndex, mUserData); // 2 is smallest + } + else + { + if (mIdx[1] < mIdx[2]) + return IndexedTriangle(mIdx[1], mIdx[2], mIdx[0], mMaterialIndex, mUserData); // 1 is smallest + else + return IndexedTriangle(mIdx[2], mIdx[0], mIdx[1], mMaterialIndex, mUserData); // 2 is smallest + } + } + + uint32 mMaterialIndex = 0; + uint32 mUserData = 0; ///< User data that can be used for anything by the application, e.g. for tracking the original index of the triangle +}; + +using IndexedTriangleNoMaterialList = Array; +using IndexedTriangleList = Array; + +JPH_NAMESPACE_END + +// Create a std::hash/JPH::Hash for IndexedTriangleNoMaterial and IndexedTriangle +JPH_MAKE_HASHABLE(JPH::IndexedTriangleNoMaterial, t.mIdx[0], t.mIdx[1], t.mIdx[2]) +JPH_MAKE_HASHABLE(JPH::IndexedTriangle, t.mIdx[0], t.mIdx[1], t.mIdx[2], t.mMaterialIndex, t.mUserData) diff --git a/thirdparty/jolt_physics/Jolt/Geometry/Indexify.cpp b/thirdparty/jolt_physics/Jolt/Geometry/Indexify.cpp new file mode 100644 index 000000000000..019dbeeda40e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/Indexify.cpp @@ -0,0 +1,222 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +static JPH_INLINE const Float3 &sIndexifyGetFloat3(const TriangleList &inTriangles, uint32 inVertexIndex) +{ + return inTriangles[inVertexIndex / 3].mV[inVertexIndex % 3]; +} + +static JPH_INLINE Vec3 sIndexifyGetVec3(const TriangleList &inTriangles, uint32 inVertexIndex) +{ + return Vec3::sLoadFloat3Unsafe(sIndexifyGetFloat3(inTriangles, inVertexIndex)); +} + +static void sIndexifyVerticesBruteForce(const TriangleList &inTriangles, const uint32 *inVertexIndices, const uint32 *inVertexIndicesEnd, Array &ioWeldedVertices, float inVertexWeldDistance) +{ + float weld_dist_sq = Square(inVertexWeldDistance); + + // Compare every vertex + for (const uint32 *v1_idx = inVertexIndices; v1_idx < inVertexIndicesEnd; ++v1_idx) + { + Vec3 v1 = sIndexifyGetVec3(inTriangles, *v1_idx); + + // with every other vertex... + for (const uint32 *v2_idx = v1_idx + 1; v2_idx < inVertexIndicesEnd; ++v2_idx) + { + Vec3 v2 = sIndexifyGetVec3(inTriangles, *v2_idx); + + // If they're weldable + if ((v2 - v1).LengthSq() <= weld_dist_sq) + { + // Find the lowest indices both indices link to + uint32 idx1 = *v1_idx; + for (;;) + { + uint32 new_idx1 = ioWeldedVertices[idx1]; + if (new_idx1 >= idx1) + break; + idx1 = new_idx1; + } + uint32 idx2 = *v2_idx; + for (;;) + { + uint32 new_idx2 = ioWeldedVertices[idx2]; + if (new_idx2 >= idx2) + break; + idx2 = new_idx2; + } + + // Order the vertices + uint32 lowest = min(idx1, idx2); + uint32 highest = max(idx1, idx2); + + // Link highest to lowest + ioWeldedVertices[highest] = lowest; + + // Also update the vertices we started from to avoid creating long chains + ioWeldedVertices[*v1_idx] = lowest; + ioWeldedVertices[*v2_idx] = lowest; + break; + } + } + } +} + +static void sIndexifyVerticesRecursively(const TriangleList &inTriangles, uint32 *ioVertexIndices, uint inNumVertices, uint32 *ioScratch, Array &ioWeldedVertices, float inVertexWeldDistance, uint inMaxRecursion) +{ + // Check if we have few enough vertices to do a brute force search + // Or if we've recursed too deep (this means we chipped off a few vertices each iteration because all points are very close) + if (inNumVertices <= 8 || inMaxRecursion == 0) + { + sIndexifyVerticesBruteForce(inTriangles, ioVertexIndices, ioVertexIndices + inNumVertices, ioWeldedVertices, inVertexWeldDistance); + return; + } + + // Calculate bounds + AABox bounds; + for (const uint32 *v = ioVertexIndices, *v_end = ioVertexIndices + inNumVertices; v < v_end; ++v) + bounds.Encapsulate(sIndexifyGetVec3(inTriangles, *v)); + + // Determine split plane + int split_axis = bounds.GetExtent().GetHighestComponentIndex(); + float split_value = bounds.GetCenter()[split_axis]; + + // Partition vertices + uint32 *v_read = ioVertexIndices, *v_write = ioVertexIndices, *v_end = ioVertexIndices + inNumVertices; + uint32 *scratch = ioScratch; + while (v_read < v_end) + { + // Calculate distance to plane + float distance_to_split_plane = sIndexifyGetFloat3(inTriangles, *v_read)[split_axis] - split_value; + if (distance_to_split_plane < -inVertexWeldDistance) + { + // Vertex is on the right side + *v_write = *v_read; + ++v_read; + ++v_write; + } + else if (distance_to_split_plane > inVertexWeldDistance) + { + // Vertex is on the wrong side, swap with the last vertex + --v_end; + std::swap(*v_read, *v_end); + } + else + { + // Vertex is too close to the split plane, it goes on both sides + *scratch++ = *v_read++; + } + } + + // Check if we made any progress + uint num_vertices_on_both_sides = (uint)(scratch - ioScratch); + if (num_vertices_on_both_sides == inNumVertices) + { + sIndexifyVerticesBruteForce(inTriangles, ioVertexIndices, ioVertexIndices + inNumVertices, ioWeldedVertices, inVertexWeldDistance); + return; + } + + // Calculate how we classified the vertices + uint num_vertices_left = (uint)(v_write - ioVertexIndices); + uint num_vertices_right = (uint)(ioVertexIndices + inNumVertices - v_end); + JPH_ASSERT(num_vertices_left + num_vertices_right + num_vertices_on_both_sides == inNumVertices); + memcpy(v_write, ioScratch, num_vertices_on_both_sides * sizeof(uint32)); + + // Recurse + uint max_recursion = inMaxRecursion - 1; + sIndexifyVerticesRecursively(inTriangles, ioVertexIndices, num_vertices_left + num_vertices_on_both_sides, ioScratch, ioWeldedVertices, inVertexWeldDistance, max_recursion); + sIndexifyVerticesRecursively(inTriangles, ioVertexIndices + num_vertices_left, num_vertices_right + num_vertices_on_both_sides, ioScratch, ioWeldedVertices, inVertexWeldDistance, max_recursion); +} + +void Indexify(const TriangleList &inTriangles, VertexList &outVertices, IndexedTriangleList &outTriangles, float inVertexWeldDistance) +{ + uint num_triangles = (uint)inTriangles.size(); + uint num_vertices = num_triangles * 3; + + // Create a list of all vertex indices + Array vertex_indices; + vertex_indices.resize(num_vertices); + for (uint i = 0; i < num_vertices; ++i) + vertex_indices[i] = i; + + // Link each vertex to itself + Array welded_vertices; + welded_vertices.resize(num_vertices); + for (uint i = 0; i < num_vertices; ++i) + welded_vertices[i] = i; + + // A scope to free memory used by the scratch array + { + // Some scratch memory, used for the vertices that fall in both partitions + Array scratch; + scratch.resize(num_vertices); + + // Recursively split the vertices + sIndexifyVerticesRecursively(inTriangles, vertex_indices.data(), num_vertices, scratch.data(), welded_vertices, inVertexWeldDistance, 32); + } + + // Do a pass to complete the welding, linking each vertex to the vertex it is welded to + // (and since we're going from 0 to N we can be sure that the vertex we're linking to is already linked to the lowest vertex) + uint num_resulting_vertices = 0; + for (uint i = 0; i < num_vertices; ++i) + { + JPH_ASSERT(welded_vertices[welded_vertices[i]] <= welded_vertices[i]); + welded_vertices[i] = welded_vertices[welded_vertices[i]]; + if (welded_vertices[i] == i) + ++num_resulting_vertices; + } + + // Collect the vertices + outVertices.clear(); + outVertices.reserve(num_resulting_vertices); + for (uint i = 0; i < num_vertices; ++i) + if (welded_vertices[i] == i) + { + // New vertex + welded_vertices[i] = (uint32)outVertices.size(); + outVertices.push_back(sIndexifyGetFloat3(inTriangles, i)); + } + else + { + // Reused vertex, remap index + welded_vertices[i] = welded_vertices[welded_vertices[i]]; + } + + // Create indexed triangles + outTriangles.clear(); + outTriangles.reserve(num_triangles); + for (uint t = 0; t < num_triangles; ++t) + { + IndexedTriangle it; + it.mMaterialIndex = inTriangles[t].mMaterialIndex; + it.mUserData = inTriangles[t].mUserData; + for (int v = 0; v < 3; ++v) + it.mIdx[v] = welded_vertices[t * 3 + v]; + if (!it.IsDegenerate(outVertices)) + outTriangles.push_back(it); + } +} + +void Deindexify(const VertexList &inVertices, const IndexedTriangleList &inTriangles, TriangleList &outTriangles) +{ + outTriangles.resize(inTriangles.size()); + for (size_t t = 0; t < inTriangles.size(); ++t) + { + const IndexedTriangle &in = inTriangles[t]; + Triangle &out = outTriangles[t]; + out.mMaterialIndex = in.mMaterialIndex; + out.mUserData = in.mUserData; + for (int v = 0; v < 3; ++v) + out.mV[v] = inVertices[in.mIdx[v]]; + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/Indexify.h b/thirdparty/jolt_physics/Jolt/Geometry/Indexify.h new file mode 100644 index 000000000000..01fb805305de --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/Indexify.h @@ -0,0 +1,19 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Take a list of triangles and get the unique set of vertices and use them to create indexed triangles. +/// Vertices that are less than inVertexWeldDistance apart will be combined to a single vertex. +JPH_EXPORT void Indexify(const TriangleList &inTriangles, VertexList &outVertices, IndexedTriangleList &outTriangles, float inVertexWeldDistance = 1.0e-4f); + +/// Take a list of indexed triangles and unpack them +JPH_EXPORT void Deindexify(const VertexList &inVertices, const IndexedTriangleList &inTriangles, TriangleList &outTriangles); + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/MortonCode.h b/thirdparty/jolt_physics/Jolt/Geometry/MortonCode.h new file mode 100644 index 000000000000..e750d7e6d1ba --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/MortonCode.h @@ -0,0 +1,40 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class MortonCode +{ +public: + /// First converts a floating point value in the range [0, 1] to a 10 bit fixed point integer. + /// Then expands a 10-bit integer into 30 bits by inserting 2 zeros after each bit. + static uint32 sExpandBits(float inV) + { + JPH_ASSERT(inV >= 0.0f && inV <= 1.0f); + uint32 v = uint32(inV * 1023.0f + 0.5f); + JPH_ASSERT(v < 1024); + v = (v * 0x00010001u) & 0xFF0000FFu; + v = (v * 0x00000101u) & 0x0F00F00Fu; + v = (v * 0x00000011u) & 0xC30C30C3u; + v = (v * 0x00000005u) & 0x49249249u; + return v; + } + + /// Calculate the morton code for inVector, given that all vectors lie in inVectorBounds + static uint32 sGetMortonCode(Vec3Arg inVector, const AABox &inVectorBounds) + { + // Convert to 10 bit fixed point + Vec3 scaled = (inVector - inVectorBounds.mMin) / inVectorBounds.GetSize(); + uint x = sExpandBits(scaled.GetX()); + uint y = sExpandBits(scaled.GetY()); + uint z = sExpandBits(scaled.GetZ()); + return (x << 2) + (y << 1) + z; + } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/OrientedBox.cpp b/thirdparty/jolt_physics/Jolt/Geometry/OrientedBox.cpp new file mode 100644 index 000000000000..31c38c877e81 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/OrientedBox.cpp @@ -0,0 +1,178 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +bool OrientedBox::Overlaps(const AABox &inBox, float inEpsilon) const +{ + // Taken from: Real Time Collision Detection - Christer Ericson + // Chapter 4.4.1, page 103-105. + // Note that the code is swapped around: A is the aabox and B is the oriented box (this saves us from having to invert the orientation of the oriented box) + + // Convert AABox to center / extent representation + Vec3 a_center = inBox.GetCenter(); + Vec3 a_half_extents = inBox.GetExtent(); + + // Compute rotation matrix expressing b in a's coordinate frame + Mat44 rot(mOrientation.GetColumn4(0), mOrientation.GetColumn4(1), mOrientation.GetColumn4(2), mOrientation.GetColumn4(3) - Vec4(a_center, 0)); + + // Compute common subexpressions. Add in an epsilon term to + // counteract arithmetic errors when two edges are parallel and + // their cross product is (near) null (see text for details) + Vec3 epsilon = Vec3::sReplicate(inEpsilon); + Vec3 abs_r[3] { rot.GetAxisX().Abs() + epsilon, rot.GetAxisY().Abs() + epsilon, rot.GetAxisZ().Abs() + epsilon }; + + // Test axes L = A0, L = A1, L = A2 + float ra, rb; + for (int i = 0; i < 3; i++) + { + ra = a_half_extents[i]; + rb = mHalfExtents[0] * abs_r[0][i] + mHalfExtents[1] * abs_r[1][i] + mHalfExtents[2] * abs_r[2][i]; + if (abs(rot(i, 3)) > ra + rb) return false; + } + + // Test axes L = B0, L = B1, L = B2 + for (int i = 0; i < 3; i++) + { + ra = a_half_extents.Dot(abs_r[i]); + rb = mHalfExtents[i]; + if (abs(rot.GetTranslation().Dot(rot.GetColumn3(i))) > ra + rb) return false; + } + + // Test axis L = A0 x B0 + ra = a_half_extents[1] * abs_r[0][2] + a_half_extents[2] * abs_r[0][1]; + rb = mHalfExtents[1] * abs_r[2][0] + mHalfExtents[2] * abs_r[1][0]; + if (abs(rot(2, 3) * rot(1, 0) - rot(1, 3) * rot(2, 0)) > ra + rb) return false; + + // Test axis L = A0 x B1 + ra = a_half_extents[1] * abs_r[1][2] + a_half_extents[2] * abs_r[1][1]; + rb = mHalfExtents[0] * abs_r[2][0] + mHalfExtents[2] * abs_r[0][0]; + if (abs(rot(2, 3) * rot(1, 1) - rot(1, 3) * rot(2, 1)) > ra + rb) return false; + + // Test axis L = A0 x B2 + ra = a_half_extents[1] * abs_r[2][2] + a_half_extents[2] * abs_r[2][1]; + rb = mHalfExtents[0] * abs_r[1][0] + mHalfExtents[1] * abs_r[0][0]; + if (abs(rot(2, 3) * rot(1, 2) - rot(1, 3) * rot(2, 2)) > ra + rb) return false; + + // Test axis L = A1 x B0 + ra = a_half_extents[0] * abs_r[0][2] + a_half_extents[2] * abs_r[0][0]; + rb = mHalfExtents[1] * abs_r[2][1] + mHalfExtents[2] * abs_r[1][1]; + if (abs(rot(0, 3) * rot(2, 0) - rot(2, 3) * rot(0, 0)) > ra + rb) return false; + + // Test axis L = A1 x B1 + ra = a_half_extents[0] * abs_r[1][2] + a_half_extents[2] * abs_r[1][0]; + rb = mHalfExtents[0] * abs_r[2][1] + mHalfExtents[2] * abs_r[0][1]; + if (abs(rot(0, 3) * rot(2, 1) - rot(2, 3) * rot(0, 1)) > ra + rb) return false; + + // Test axis L = A1 x B2 + ra = a_half_extents[0] * abs_r[2][2] + a_half_extents[2] * abs_r[2][0]; + rb = mHalfExtents[0] * abs_r[1][1] + mHalfExtents[1] * abs_r[0][1]; + if (abs(rot(0, 3) * rot(2, 2) - rot(2, 3) * rot(0, 2)) > ra + rb) return false; + + // Test axis L = A2 x B0 + ra = a_half_extents[0] * abs_r[0][1] + a_half_extents[1] * abs_r[0][0]; + rb = mHalfExtents[1] * abs_r[2][2] + mHalfExtents[2] * abs_r[1][2]; + if (abs(rot(1, 3) * rot(0, 0) - rot(0, 3) * rot(1, 0)) > ra + rb) return false; + + // Test axis L = A2 x B1 + ra = a_half_extents[0] * abs_r[1][1] + a_half_extents[1] * abs_r[1][0]; + rb = mHalfExtents[0] * abs_r[2][2] + mHalfExtents[2] * abs_r[0][2]; + if (abs(rot(1, 3) * rot(0, 1) - rot(0, 3) * rot(1, 1)) > ra + rb) return false; + + // Test axis L = A2 x B2 + ra = a_half_extents[0] * abs_r[2][1] + a_half_extents[1] * abs_r[2][0]; + rb = mHalfExtents[0] * abs_r[1][2] + mHalfExtents[1] * abs_r[0][2]; + if (abs(rot(1, 3) * rot(0, 2) - rot(0, 3) * rot(1, 2)) > ra + rb) return false; + + // Since no separating axis is found, the OBB and AAB must be intersecting + return true; +} + +bool OrientedBox::Overlaps(const OrientedBox &inBox, float inEpsilon) const +{ + // Taken from: Real Time Collision Detection - Christer Ericson + // Chapter 4.4.1, page 103-105. + // Note that A is this, B is inBox + + // Compute rotation matrix expressing b in a's coordinate frame + Mat44 rot = mOrientation.InversedRotationTranslation() * inBox.mOrientation; + + // Compute common subexpressions. Add in an epsilon term to + // counteract arithmetic errors when two edges are parallel and + // their cross product is (near) null (see text for details) + Vec3 epsilon = Vec3::sReplicate(inEpsilon); + Vec3 abs_r[3] { rot.GetAxisX().Abs() + epsilon, rot.GetAxisY().Abs() + epsilon, rot.GetAxisZ().Abs() + epsilon }; + + // Test axes L = A0, L = A1, L = A2 + float ra, rb; + for (int i = 0; i < 3; i++) + { + ra = mHalfExtents[i]; + rb = inBox.mHalfExtents[0] * abs_r[0][i] + inBox.mHalfExtents[1] * abs_r[1][i] + inBox.mHalfExtents[2] * abs_r[2][i]; + if (abs(rot(i, 3)) > ra + rb) return false; + } + + // Test axes L = B0, L = B1, L = B2 + for (int i = 0; i < 3; i++) + { + ra = mHalfExtents.Dot(abs_r[i]); + rb = inBox.mHalfExtents[i]; + if (abs(rot.GetTranslation().Dot(rot.GetColumn3(i))) > ra + rb) return false; + } + + // Test axis L = A0 x B0 + ra = mHalfExtents[1] * abs_r[0][2] + mHalfExtents[2] * abs_r[0][1]; + rb = inBox.mHalfExtents[1] * abs_r[2][0] + inBox.mHalfExtents[2] * abs_r[1][0]; + if (abs(rot(2, 3) * rot(1, 0) - rot(1, 3) * rot(2, 0)) > ra + rb) return false; + + // Test axis L = A0 x B1 + ra = mHalfExtents[1] * abs_r[1][2] + mHalfExtents[2] * abs_r[1][1]; + rb = inBox.mHalfExtents[0] * abs_r[2][0] + inBox.mHalfExtents[2] * abs_r[0][0]; + if (abs(rot(2, 3) * rot(1, 1) - rot(1, 3) * rot(2, 1)) > ra + rb) return false; + + // Test axis L = A0 x B2 + ra = mHalfExtents[1] * abs_r[2][2] + mHalfExtents[2] * abs_r[2][1]; + rb = inBox.mHalfExtents[0] * abs_r[1][0] + inBox.mHalfExtents[1] * abs_r[0][0]; + if (abs(rot(2, 3) * rot(1, 2) - rot(1, 3) * rot(2, 2)) > ra + rb) return false; + + // Test axis L = A1 x B0 + ra = mHalfExtents[0] * abs_r[0][2] + mHalfExtents[2] * abs_r[0][0]; + rb = inBox.mHalfExtents[1] * abs_r[2][1] + inBox.mHalfExtents[2] * abs_r[1][1]; + if (abs(rot(0, 3) * rot(2, 0) - rot(2, 3) * rot(0, 0)) > ra + rb) return false; + + // Test axis L = A1 x B1 + ra = mHalfExtents[0] * abs_r[1][2] + mHalfExtents[2] * abs_r[1][0]; + rb = inBox.mHalfExtents[0] * abs_r[2][1] + inBox.mHalfExtents[2] * abs_r[0][1]; + if (abs(rot(0, 3) * rot(2, 1) - rot(2, 3) * rot(0, 1)) > ra + rb) return false; + + // Test axis L = A1 x B2 + ra = mHalfExtents[0] * abs_r[2][2] + mHalfExtents[2] * abs_r[2][0]; + rb = inBox.mHalfExtents[0] * abs_r[1][1] + inBox.mHalfExtents[1] * abs_r[0][1]; + if (abs(rot(0, 3) * rot(2, 2) - rot(2, 3) * rot(0, 2)) > ra + rb) return false; + + // Test axis L = A2 x B0 + ra = mHalfExtents[0] * abs_r[0][1] + mHalfExtents[1] * abs_r[0][0]; + rb = inBox.mHalfExtents[1] * abs_r[2][2] + inBox.mHalfExtents[2] * abs_r[1][2]; + if (abs(rot(1, 3) * rot(0, 0) - rot(0, 3) * rot(1, 0)) > ra + rb) return false; + + // Test axis L = A2 x B1 + ra = mHalfExtents[0] * abs_r[1][1] + mHalfExtents[1] * abs_r[1][0]; + rb = inBox.mHalfExtents[0] * abs_r[2][2] + inBox.mHalfExtents[2] * abs_r[0][2]; + if (abs(rot(1, 3) * rot(0, 1) - rot(0, 3) * rot(1, 1)) > ra + rb) return false; + + // Test axis L = A2 x B2 + ra = mHalfExtents[0] * abs_r[2][1] + mHalfExtents[1] * abs_r[2][0]; + rb = inBox.mHalfExtents[0] * abs_r[1][2] + inBox.mHalfExtents[1] * abs_r[0][2]; + if (abs(rot(1, 3) * rot(0, 2) - rot(0, 3) * rot(1, 2)) > ra + rb) return false; + + // Since no separating axis is found, the OBBs must be intersecting + return true; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/OrientedBox.h b/thirdparty/jolt_physics/Jolt/Geometry/OrientedBox.h new file mode 100644 index 000000000000..c5c2a0e16c84 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/OrientedBox.h @@ -0,0 +1,39 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class AABox; + +/// Oriented box +class [[nodiscard]] JPH_EXPORT_GCC_BUG_WORKAROUND OrientedBox +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + OrientedBox() = default; + OrientedBox(Mat44Arg inOrientation, Vec3Arg inHalfExtents) : mOrientation(inOrientation), mHalfExtents(inHalfExtents) { } + + /// Construct from axis aligned box and transform. Only works for rotation/translation matrix (no scaling / shearing). + OrientedBox(Mat44Arg inOrientation, const AABox &inBox) : OrientedBox(inOrientation.PreTranslated(inBox.GetCenter()), inBox.GetExtent()) { } + + /// Test if oriented box overlaps with axis aligned box each other + bool Overlaps(const AABox &inBox, float inEpsilon = 1.0e-6f) const; + + /// Test if two oriented boxes overlap each other + bool Overlaps(const OrientedBox &inBox, float inEpsilon = 1.0e-6f) const; + + Mat44 mOrientation; ///< Transform that positions and rotates the local space axis aligned box into world space + Vec3 mHalfExtents; ///< Half extents (half the size of the edge) of the local space axis aligned box +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/Plane.h b/thirdparty/jolt_physics/Jolt/Geometry/Plane.h new file mode 100644 index 000000000000..7e3b8a8c4fed --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/Plane.h @@ -0,0 +1,101 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// An infinite plane described by the formula X . Normal + Constant = 0. +class [[nodiscard]] Plane +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + Plane() = default; + explicit Plane(Vec4Arg inNormalAndConstant) : mNormalAndConstant(inNormalAndConstant) { } + Plane(Vec3Arg inNormal, float inConstant) : mNormalAndConstant(inNormal, inConstant) { } + + /// Create from point and normal + static Plane sFromPointAndNormal(Vec3Arg inPoint, Vec3Arg inNormal) { return Plane(Vec4(inNormal, -inNormal.Dot(inPoint))); } + + /// Create from point and normal, double precision version that more accurately calculates the plane constant + static Plane sFromPointAndNormal(DVec3Arg inPoint, Vec3Arg inNormal) { return Plane(Vec4(inNormal, -float(DVec3(inNormal).Dot(inPoint)))); } + + /// Create from 3 counter clockwise points + static Plane sFromPointsCCW(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3) { return sFromPointAndNormal(inV1, (inV2 - inV1).Cross(inV3 - inV1).Normalized()); } + + // Properties + Vec3 GetNormal() const { return Vec3(mNormalAndConstant); } + void SetNormal(Vec3Arg inNormal) { mNormalAndConstant = Vec4(inNormal, mNormalAndConstant.GetW()); } + float GetConstant() const { return mNormalAndConstant.GetW(); } + void SetConstant(float inConstant) { mNormalAndConstant.SetW(inConstant); } + + /// 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)); } + + /// Transform the plane by a matrix + inline Plane GetTransformed(Mat44Arg inTransform) const + { + Vec3 transformed_normal = inTransform.Multiply3x3(GetNormal()); + return Plane(transformed_normal, GetConstant() - inTransform.GetTranslation().Dot(transformed_normal)); + } + + /// Scale the plane, can handle non-uniform and negative scaling + inline Plane Scaled(Vec3Arg inScale) const + { + Vec3 scaled_normal = GetNormal() / inScale; + float scaled_normal_length = scaled_normal.Length(); + return Plane(scaled_normal / scaled_normal_length, GetConstant() / scaled_normal_length); + } + + /// Distance point to plane + float SignedDistance(Vec3Arg inPoint) const { return inPoint.Dot(GetNormal()) + GetConstant(); } + + /// Project inPoint onto the plane + Vec3 ProjectPointOnPlane(Vec3Arg inPoint) const { return inPoint - GetNormal() * SignedDistance(inPoint); } + + /// Returns intersection point between 3 planes + static bool sIntersectPlanes(const Plane &inP1, const Plane &inP2, const Plane &inP3, Vec3 &outPoint) + { + // We solve the equation: + // |ax, ay, az, aw| | x | | 0 | + // |bx, by, bz, bw| * | y | = | 0 | + // |cx, cy, cz, cw| | z | | 0 | + // | 0, 0, 0, 1| | 1 | | 1 | + // Where normal of plane 1 = (ax, ay, az), plane constant of 1 = aw, normal of plane 2 = (bx, by, bz) etc. + // This involves inverting the matrix and multiplying it with [0, 0, 0, 1] + + // Fetch the normals and plane constants for the three planes + Vec4 a = inP1.mNormalAndConstant; + Vec4 b = inP2.mNormalAndConstant; + Vec4 c = inP3.mNormalAndConstant; + + // Result is a vector that we have to divide by: + float denominator = Vec3(a).Dot(Vec3(b).Cross(Vec3(c))); + if (denominator == 0.0f) + return false; + + // The numerator is: + // [aw*(bz*cy-by*cz)+ay*(bw*cz-bz*cw)+az*(by*cw-bw*cy)] + // [aw*(bx*cz-bz*cx)+ax*(bz*cw-bw*cz)+az*(bw*cx-bx*cw)] + // [aw*(by*cx-bx*cy)+ax*(bw*cy-by*cw)+ay*(bx*cw-bw*cx)] + Vec4 numerator = + a.SplatW() * (b.Swizzle() * c.Swizzle() - b.Swizzle() * c.Swizzle()) + + a.Swizzle() * (b.Swizzle() * c.Swizzle() - b.Swizzle() * c.Swizzle()) + + a.Swizzle() * (b.Swizzle() * c.Swizzle() - b.Swizzle() * c.Swizzle()); + + outPoint = Vec3(numerator) / denominator; + return true; + } + +private: +#ifdef JPH_OBJECT_STREAM + friend void CreateRTTIPlane(class RTTI &); // For JPH_IMPLEMENT_SERIALIZABLE_OUTSIDE_CLASS +#endif + + Vec4 mNormalAndConstant; ///< XYZ = normal, W = constant, plane: x . normal + constant = 0 +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/RayAABox.h b/thirdparty/jolt_physics/Jolt/Geometry/RayAABox.h new file mode 100644 index 000000000000..4506fadbb75b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/RayAABox.h @@ -0,0 +1,241 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Helper structure holding the reciprocal of a ray for Ray vs AABox testing +class RayInvDirection +{ +public: + /// Constructors + inline RayInvDirection() = default; + inline explicit RayInvDirection(Vec3Arg inDirection) { Set(inDirection); } + + /// Set reciprocal from ray direction + inline void Set(Vec3Arg inDirection) + { + // if (abs(inDirection) <= Epsilon) the ray is nearly parallel to the slab. + mIsParallel = Vec3::sLessOrEqual(inDirection.Abs(), Vec3::sReplicate(1.0e-20f)); + + // Calculate 1 / direction while avoiding division by zero + mInvDirection = Vec3::sSelect(inDirection, Vec3::sReplicate(1.0f), mIsParallel).Reciprocal(); + } + + Vec3 mInvDirection; ///< 1 / ray direction + UVec4 mIsParallel; ///< for each component if it is parallel to the coordinate axis +}; + +/// Intersect AABB with ray, returns minimal distance along ray or FLT_MAX if no hit +/// Note: Can return negative value if ray starts in box +JPH_INLINE float RayAABox(Vec3Arg inOrigin, const RayInvDirection &inInvDirection, Vec3Arg inBoundsMin, Vec3Arg inBoundsMax) +{ + // Constants + Vec3 flt_min = Vec3::sReplicate(-FLT_MAX); + Vec3 flt_max = Vec3::sReplicate(FLT_MAX); + + // Test against all three axii simultaneously. + Vec3 t1 = (inBoundsMin - inOrigin) * inInvDirection.mInvDirection; + Vec3 t2 = (inBoundsMax - inOrigin) * inInvDirection.mInvDirection; + + // Compute the max of min(t1,t2) and the min of max(t1,t2) ensuring we don't + // use the results from any directions parallel to the slab. + Vec3 t_min = Vec3::sSelect(Vec3::sMin(t1, t2), flt_min, inInvDirection.mIsParallel); + Vec3 t_max = Vec3::sSelect(Vec3::sMax(t1, t2), flt_max, inInvDirection.mIsParallel); + + // t_min.xyz = maximum(t_min.x, t_min.y, t_min.z); + t_min = Vec3::sMax(t_min, t_min.Swizzle()); + t_min = Vec3::sMax(t_min, t_min.Swizzle()); + + // t_max.xyz = minimum(t_max.x, t_max.y, t_max.z); + t_max = Vec3::sMin(t_max, t_max.Swizzle()); + t_max = Vec3::sMin(t_max, t_max.Swizzle()); + + // if (t_min > t_max) return FLT_MAX; + UVec4 no_intersection = Vec3::sGreater(t_min, t_max); + + // if (t_max < 0.0f) return FLT_MAX; + no_intersection = UVec4::sOr(no_intersection, Vec3::sLess(t_max, Vec3::sZero())); + + // if (inInvDirection.mIsParallel && !(Min <= inOrigin && inOrigin <= Max)) return FLT_MAX; else return t_min; + UVec4 no_parallel_overlap = UVec4::sOr(Vec3::sLess(inOrigin, inBoundsMin), Vec3::sGreater(inOrigin, inBoundsMax)); + no_intersection = UVec4::sOr(no_intersection, UVec4::sAnd(inInvDirection.mIsParallel, no_parallel_overlap)); + no_intersection = UVec4::sOr(no_intersection, no_intersection.SplatY()); + no_intersection = UVec4::sOr(no_intersection, no_intersection.SplatZ()); + return Vec3::sSelect(t_min, flt_max, no_intersection).GetX(); +} + +/// Intersect 4 AABBs with ray, returns minimal distance along ray or FLT_MAX if no hit +/// Note: Can return negative value if ray starts in box +JPH_INLINE Vec4 RayAABox4(Vec3Arg inOrigin, const RayInvDirection &inInvDirection, Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) +{ + // Constants + Vec4 flt_min = Vec4::sReplicate(-FLT_MAX); + Vec4 flt_max = Vec4::sReplicate(FLT_MAX); + + // Origin + Vec4 originx = inOrigin.SplatX(); + Vec4 originy = inOrigin.SplatY(); + Vec4 originz = inOrigin.SplatZ(); + + // Parallel + UVec4 parallelx = inInvDirection.mIsParallel.SplatX(); + UVec4 parallely = inInvDirection.mIsParallel.SplatY(); + UVec4 parallelz = inInvDirection.mIsParallel.SplatZ(); + + // Inverse direction + Vec4 invdirx = inInvDirection.mInvDirection.SplatX(); + Vec4 invdiry = inInvDirection.mInvDirection.SplatY(); + Vec4 invdirz = inInvDirection.mInvDirection.SplatZ(); + + // Test against all three axii simultaneously. + Vec4 t1x = (inBoundsMinX - originx) * invdirx; + Vec4 t1y = (inBoundsMinY - originy) * invdiry; + Vec4 t1z = (inBoundsMinZ - originz) * invdirz; + Vec4 t2x = (inBoundsMaxX - originx) * invdirx; + Vec4 t2y = (inBoundsMaxY - originy) * invdiry; + Vec4 t2z = (inBoundsMaxZ - originz) * invdirz; + + // Compute the max of min(t1,t2) and the min of max(t1,t2) ensuring we don't + // use the results from any directions parallel to the slab. + Vec4 t_minx = Vec4::sSelect(Vec4::sMin(t1x, t2x), flt_min, parallelx); + Vec4 t_miny = Vec4::sSelect(Vec4::sMin(t1y, t2y), flt_min, parallely); + Vec4 t_minz = Vec4::sSelect(Vec4::sMin(t1z, t2z), flt_min, parallelz); + Vec4 t_maxx = Vec4::sSelect(Vec4::sMax(t1x, t2x), flt_max, parallelx); + Vec4 t_maxy = Vec4::sSelect(Vec4::sMax(t1y, t2y), flt_max, parallely); + Vec4 t_maxz = Vec4::sSelect(Vec4::sMax(t1z, t2z), flt_max, parallelz); + + // t_min.xyz = maximum(t_min.x, t_min.y, t_min.z); + Vec4 t_min = Vec4::sMax(Vec4::sMax(t_minx, t_miny), t_minz); + + // t_max.xyz = minimum(t_max.x, t_max.y, t_max.z); + Vec4 t_max = Vec4::sMin(Vec4::sMin(t_maxx, t_maxy), t_maxz); + + // if (t_min > t_max) return FLT_MAX; + UVec4 no_intersection = Vec4::sGreater(t_min, t_max); + + // if (t_max < 0.0f) return FLT_MAX; + no_intersection = UVec4::sOr(no_intersection, Vec4::sLess(t_max, Vec4::sZero())); + + // if bounds are invalid return FLOAT_MAX; + UVec4 bounds_invalid = UVec4::sOr(UVec4::sOr(Vec4::sGreater(inBoundsMinX, inBoundsMaxX), Vec4::sGreater(inBoundsMinY, inBoundsMaxY)), Vec4::sGreater(inBoundsMinZ, inBoundsMaxZ)); + no_intersection = UVec4::sOr(no_intersection, bounds_invalid); + + // if (inInvDirection.mIsParallel && !(Min <= inOrigin && inOrigin <= Max)) return FLT_MAX; else return t_min; + UVec4 no_parallel_overlapx = UVec4::sAnd(parallelx, UVec4::sOr(Vec4::sLess(originx, inBoundsMinX), Vec4::sGreater(originx, inBoundsMaxX))); + UVec4 no_parallel_overlapy = UVec4::sAnd(parallely, UVec4::sOr(Vec4::sLess(originy, inBoundsMinY), Vec4::sGreater(originy, inBoundsMaxY))); + UVec4 no_parallel_overlapz = UVec4::sAnd(parallelz, UVec4::sOr(Vec4::sLess(originz, inBoundsMinZ), Vec4::sGreater(originz, inBoundsMaxZ))); + no_intersection = UVec4::sOr(no_intersection, UVec4::sOr(UVec4::sOr(no_parallel_overlapx, no_parallel_overlapy), no_parallel_overlapz)); + return Vec4::sSelect(t_min, flt_max, no_intersection); +} + +/// Intersect AABB with ray, returns minimal and maximal distance along ray or FLT_MAX, -FLT_MAX if no hit +/// Note: Can return negative value for outMin if ray starts in box +JPH_INLINE void RayAABox(Vec3Arg inOrigin, const RayInvDirection &inInvDirection, Vec3Arg inBoundsMin, Vec3Arg inBoundsMax, float &outMin, float &outMax) +{ + // Constants + Vec3 flt_min = Vec3::sReplicate(-FLT_MAX); + Vec3 flt_max = Vec3::sReplicate(FLT_MAX); + + // Test against all three axii simultaneously. + Vec3 t1 = (inBoundsMin - inOrigin) * inInvDirection.mInvDirection; + Vec3 t2 = (inBoundsMax - inOrigin) * inInvDirection.mInvDirection; + + // Compute the max of min(t1,t2) and the min of max(t1,t2) ensuring we don't + // use the results from any directions parallel to the slab. + Vec3 t_min = Vec3::sSelect(Vec3::sMin(t1, t2), flt_min, inInvDirection.mIsParallel); + Vec3 t_max = Vec3::sSelect(Vec3::sMax(t1, t2), flt_max, inInvDirection.mIsParallel); + + // t_min.xyz = maximum(t_min.x, t_min.y, t_min.z); + t_min = Vec3::sMax(t_min, t_min.Swizzle()); + t_min = Vec3::sMax(t_min, t_min.Swizzle()); + + // t_max.xyz = minimum(t_max.x, t_max.y, t_max.z); + t_max = Vec3::sMin(t_max, t_max.Swizzle()); + t_max = Vec3::sMin(t_max, t_max.Swizzle()); + + // if (t_min > t_max) return FLT_MAX; + UVec4 no_intersection = Vec3::sGreater(t_min, t_max); + + // if (t_max < 0.0f) return FLT_MAX; + no_intersection = UVec4::sOr(no_intersection, Vec3::sLess(t_max, Vec3::sZero())); + + // if (inInvDirection.mIsParallel && !(Min <= inOrigin && inOrigin <= Max)) return FLT_MAX; else return t_min; + UVec4 no_parallel_overlap = UVec4::sOr(Vec3::sLess(inOrigin, inBoundsMin), Vec3::sGreater(inOrigin, inBoundsMax)); + no_intersection = UVec4::sOr(no_intersection, UVec4::sAnd(inInvDirection.mIsParallel, no_parallel_overlap)); + no_intersection = UVec4::sOr(no_intersection, no_intersection.SplatY()); + no_intersection = UVec4::sOr(no_intersection, no_intersection.SplatZ()); + outMin = Vec3::sSelect(t_min, flt_max, no_intersection).GetX(); + outMax = Vec3::sSelect(t_max, flt_min, no_intersection).GetX(); +} + +/// Intersect AABB with ray, returns true if there is a hit closer than inClosest +JPH_INLINE bool RayAABoxHits(Vec3Arg inOrigin, const RayInvDirection &inInvDirection, Vec3Arg inBoundsMin, Vec3Arg inBoundsMax, float inClosest) +{ + // Constants + Vec3 flt_min = Vec3::sReplicate(-FLT_MAX); + Vec3 flt_max = Vec3::sReplicate(FLT_MAX); + + // Test against all three axii simultaneously. + Vec3 t1 = (inBoundsMin - inOrigin) * inInvDirection.mInvDirection; + Vec3 t2 = (inBoundsMax - inOrigin) * inInvDirection.mInvDirection; + + // Compute the max of min(t1,t2) and the min of max(t1,t2) ensuring we don't + // use the results from any directions parallel to the slab. + Vec3 t_min = Vec3::sSelect(Vec3::sMin(t1, t2), flt_min, inInvDirection.mIsParallel); + Vec3 t_max = Vec3::sSelect(Vec3::sMax(t1, t2), flt_max, inInvDirection.mIsParallel); + + // t_min.xyz = maximum(t_min.x, t_min.y, t_min.z); + t_min = Vec3::sMax(t_min, t_min.Swizzle()); + t_min = Vec3::sMax(t_min, t_min.Swizzle()); + + // t_max.xyz = minimum(t_max.x, t_max.y, t_max.z); + t_max = Vec3::sMin(t_max, t_max.Swizzle()); + t_max = Vec3::sMin(t_max, t_max.Swizzle()); + + // if (t_min > t_max) return false; + UVec4 no_intersection = Vec3::sGreater(t_min, t_max); + + // if (t_max < 0.0f) return false; + no_intersection = UVec4::sOr(no_intersection, Vec3::sLess(t_max, Vec3::sZero())); + + // if (t_min > inClosest) return false; + no_intersection = UVec4::sOr(no_intersection, Vec3::sGreater(t_min, Vec3::sReplicate(inClosest))); + + // if (inInvDirection.mIsParallel && !(Min <= inOrigin && inOrigin <= Max)) return false; else return true; + UVec4 no_parallel_overlap = UVec4::sOr(Vec3::sLess(inOrigin, inBoundsMin), Vec3::sGreater(inOrigin, inBoundsMax)); + no_intersection = UVec4::sOr(no_intersection, UVec4::sAnd(inInvDirection.mIsParallel, no_parallel_overlap)); + + return !no_intersection.TestAnyXYZTrue(); +} + +/// Intersect AABB with ray without hit fraction, based on separating axis test +/// @see http://www.codercorner.com/RayAABB.cpp +JPH_INLINE bool RayAABoxHits(Vec3Arg inOrigin, Vec3Arg inDirection, Vec3Arg inBoundsMin, Vec3Arg inBoundsMax) +{ + Vec3 extents = inBoundsMax - inBoundsMin; + + Vec3 diff = 2.0f * inOrigin - inBoundsMin - inBoundsMax; + Vec3 abs_diff = diff.Abs(); + + UVec4 no_intersection = UVec4::sAnd(Vec3::sGreater(abs_diff, extents), Vec3::sGreaterOrEqual(diff * inDirection, Vec3::sZero())); + + Vec3 abs_dir = inDirection.Abs(); + Vec3 abs_dir_yzz = abs_dir.Swizzle(); + Vec3 abs_dir_xyx = abs_dir.Swizzle(); + + Vec3 extents_yzz = extents.Swizzle(); + Vec3 extents_xyx = extents.Swizzle(); + + Vec3 diff_yzx = diff.Swizzle(); + + Vec3 dir_yzx = inDirection.Swizzle(); + + no_intersection = UVec4::sOr(no_intersection, Vec3::sGreater((inDirection * diff_yzx - dir_yzx * diff).Abs(), extents_xyx * abs_dir_yzz + extents_yzz * abs_dir_xyx)); + + return !no_intersection.TestAnyXYZTrue(); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/RayCapsule.h b/thirdparty/jolt_physics/Jolt/Geometry/RayCapsule.h new file mode 100644 index 000000000000..4862931b685d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/RayCapsule.h @@ -0,0 +1,37 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Tests a ray starting at inRayOrigin and extending infinitely in inRayDirection +/// against a capsule centered around the origin with its axis along the Y axis and half height specified. +/// @return FLT_MAX if there is no intersection, otherwise the fraction along the ray. +/// @param inRayDirection Ray direction. Does not need to be normalized. +/// @param inRayOrigin Origin of the ray. If the ray starts inside the capsule, the returned fraction will be 0. +/// @param inCapsuleHalfHeight Distance from the origin to the center of the top sphere (or that of the bottom) +/// @param inCapsuleRadius Radius of the top/bottom sphere +JPH_INLINE float RayCapsule(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, float inCapsuleHalfHeight, float inCapsuleRadius) +{ + // Test infinite cylinder + float cylinder = RayCylinder(inRayOrigin, inRayDirection, inCapsuleRadius); + if (cylinder == FLT_MAX) + return FLT_MAX; + + // If this hit is in the finite cylinder we have our fraction + if (abs(inRayOrigin.GetY() + cylinder * inRayDirection.GetY()) <= inCapsuleHalfHeight) + return cylinder; + + // Test upper and lower sphere + Vec3 sphere_center(0, inCapsuleHalfHeight, 0); + float upper = RaySphere(inRayOrigin, inRayDirection, sphere_center, inCapsuleRadius); + float lower = RaySphere(inRayOrigin, inRayDirection, -sphere_center, inCapsuleRadius); + return min(upper, lower); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/RayCylinder.h b/thirdparty/jolt_physics/Jolt/Geometry/RayCylinder.h new file mode 100644 index 000000000000..cabed0680a22 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/RayCylinder.h @@ -0,0 +1,101 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Tests a ray starting at inRayOrigin and extending infinitely in inRayDirection +/// against an infinite cylinder centered along the Y axis +/// @return FLT_MAX if there is no intersection, otherwise the fraction along the ray. +/// @param inRayDirection Direction of the ray. Does not need to be normalized. +/// @param inRayOrigin Origin of the ray. If the ray starts inside the cylinder, the returned fraction will be 0. +/// @param inCylinderRadius Radius of the infinite cylinder +JPH_INLINE float RayCylinder(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, float inCylinderRadius) +{ + // Remove Y component of ray to see of ray intersects with infinite cylinder + UVec4 mask_y = UVec4(0, 0xffffffff, 0, 0); + Vec3 origin_xz = Vec3::sSelect(inRayOrigin, Vec3::sZero(), mask_y); + float origin_xz_len_sq = origin_xz.LengthSq(); + float r_sq = Square(inCylinderRadius); + if (origin_xz_len_sq > r_sq) + { + // Ray starts outside of the infinite cylinder + // Solve: |RayOrigin_xz + fraction * RayDirection_xz|^2 = r^2 to find fraction + Vec3 direction_xz = Vec3::sSelect(inRayDirection, Vec3::sZero(), mask_y); + float a = direction_xz.LengthSq(); + float b = 2.0f * origin_xz.Dot(direction_xz); + float c = origin_xz_len_sq - r_sq; + float fraction1, fraction2; + if (FindRoot(a, b, c, fraction1, fraction2) == 0) + return FLT_MAX; // No intersection with infinite cylinder + + // Get fraction corresponding to the ray entering the circle + float fraction = min(fraction1, fraction2); + if (fraction >= 0.0f) + return fraction; + } + else + { + // Ray starts inside the infinite cylinder + return 0.0f; + } + + // No collision + return FLT_MAX; +} + +/// Test a ray against a cylinder centered around the origin with its axis along the Y axis and half height specified. +/// @return FLT_MAX if there is no intersection, otherwise the fraction along the ray. +/// @param inRayDirection Ray direction. Does not need to be normalized. +/// @param inRayOrigin Origin of the ray. If the ray starts inside the cylinder, the returned fraction will be 0. +/// @param inCylinderRadius Radius of the cylinder +/// @param inCylinderHalfHeight Distance from the origin to the top (or bottom) of the cylinder +JPH_INLINE float RayCylinder(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, float inCylinderHalfHeight, float inCylinderRadius) +{ + // Test infinite cylinder + float fraction = RayCylinder(inRayOrigin, inRayDirection, inCylinderRadius); + if (fraction == FLT_MAX) + return FLT_MAX; + + // If this hit is in the finite cylinder we have our fraction + if (abs(inRayOrigin.GetY() + fraction * inRayDirection.GetY()) <= inCylinderHalfHeight) + return fraction; + + // Check if ray could hit the top or bottom plane of the cylinder + float direction_y = inRayDirection.GetY(); + if (direction_y != 0.0f) + { + // Solving line equation: x = ray_origin + fraction * ray_direction + // and plane equation: plane_normal . x + plane_constant = 0 + // fraction = (-plane_constant - plane_normal . ray_origin) / (plane_normal . ray_direction) + // when the ray_direction.y < 0: + // plane_constant = -cylinder_half_height, plane_normal = (0, 1, 0) + // else + // plane_constant = -cylinder_half_height, plane_normal = (0, -1, 0) + float origin_y = inRayOrigin.GetY(); + float plane_fraction; + if (direction_y < 0.0f) + plane_fraction = (inCylinderHalfHeight - origin_y) / direction_y; + else + plane_fraction = -(inCylinderHalfHeight + origin_y) / direction_y; + + // Check if the hit is in front of the ray + if (plane_fraction >= 0.0f) + { + // Test if this hit is inside the cylinder + Vec3 point = inRayOrigin + plane_fraction * inRayDirection; + float dist_sq = Square(point.GetX()) + Square(point.GetZ()); + if (dist_sq <= Square(inCylinderRadius)) + return plane_fraction; + } + } + + // No collision + return FLT_MAX; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/RaySphere.h b/thirdparty/jolt_physics/Jolt/Geometry/RaySphere.h new file mode 100644 index 000000000000..93ad2682800b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/RaySphere.h @@ -0,0 +1,96 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Tests a ray starting at inRayOrigin and extending infinitely in inRayDirection against a sphere, +/// @return FLT_MAX if there is no intersection, otherwise the fraction along the ray. +/// @param inRayOrigin Ray origin. If the ray starts inside the sphere, the returned fraction will be 0. +/// @param inRayDirection Ray direction. Does not need to be normalized. +/// @param inSphereCenter Position of the center of the sphere +/// @param inSphereRadius Radius of the sphere +JPH_INLINE float RaySphere(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, Vec3Arg inSphereCenter, float inSphereRadius) +{ + // Solve: |RayOrigin + fraction * RayDirection - SphereCenter|^2 = SphereRadius^2 for fraction + Vec3 center_origin = inRayOrigin - inSphereCenter; + float a = inRayDirection.LengthSq(); + float b = 2.0f * inRayDirection.Dot(center_origin); + float c = center_origin.LengthSq() - inSphereRadius * inSphereRadius; + float fraction1, fraction2; + if (FindRoot(a, b, c, fraction1, fraction2) == 0) + return c <= 0.0f? 0.0f : FLT_MAX; // Return if origin is inside the sphere + + // Sort so that the smallest is first + if (fraction1 > fraction2) + std::swap(fraction1, fraction2); + + // Test solution with lowest fraction, this will be the ray entering the sphere + if (fraction1 >= 0.0f) + return fraction1; // Sphere is before the ray start + + // Test solution with highest fraction, this will be the ray leaving the sphere + if (fraction2 >= 0.0f) + return 0.0f; // We start inside the sphere + + // No solution + return FLT_MAX; +} + +/// Tests a ray starting at inRayOrigin and extending infinitely in inRayDirection against a sphere. +/// Outputs entry and exit points (outMinFraction and outMaxFraction) along the ray (which could be negative if the hit point is before the start of the ray). +/// @param inRayOrigin Ray origin. If the ray starts inside the sphere, the returned fraction will be 0. +/// @param inRayDirection Ray direction. Does not need to be normalized. +/// @param inSphereCenter Position of the center of the sphere. +/// @param inSphereRadius Radius of the sphere. +/// @param outMinFraction Returned lowest intersection fraction +/// @param outMaxFraction Returned highest intersection fraction +/// @return The amount of intersections with the sphere. +/// If 1 intersection is returned outMinFraction will be equal to outMaxFraction +JPH_INLINE int RaySphere(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, Vec3Arg inSphereCenter, float inSphereRadius, float &outMinFraction, float &outMaxFraction) +{ + // Solve: |RayOrigin + fraction * RayDirection - SphereCenter|^2 = SphereRadius^2 for fraction + Vec3 center_origin = inRayOrigin - inSphereCenter; + float a = inRayDirection.LengthSq(); + float b = 2.0f * inRayDirection.Dot(center_origin); + float c = center_origin.LengthSq() - inSphereRadius * inSphereRadius; + float fraction1, fraction2; + switch (FindRoot(a, b, c, fraction1, fraction2)) + { + case 0: + if (c <= 0.0f) + { + // Origin inside sphere + outMinFraction = outMaxFraction = 0.0f; + return 1; + } + else + { + // Origin outside of the sphere + return 0; + } + break; + + case 1: + // Ray is touching the sphere + outMinFraction = outMaxFraction = fraction1; + return 1; + + default: + // Ray enters and exits the sphere + + // Sort so that the smallest is first + if (fraction1 > fraction2) + std::swap(fraction1, fraction2); + + outMinFraction = fraction1; + outMaxFraction = fraction2; + return 2; + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/RayTriangle.h b/thirdparty/jolt_physics/Jolt/Geometry/RayTriangle.h new file mode 100644 index 000000000000..dabd0275cc0c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/RayTriangle.h @@ -0,0 +1,158 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Intersect ray with triangle, returns closest point or FLT_MAX if no hit (branch less version) +/// Adapted from: http://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm +JPH_INLINE float RayTriangle(Vec3Arg inOrigin, Vec3Arg inDirection, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) +{ + // Epsilon + Vec3 epsilon = Vec3::sReplicate(1.0e-12f); + + // Zero & one + Vec3 zero = Vec3::sZero(); + Vec3 one = Vec3::sReplicate(1.0f); + + // Find vectors for two edges sharing inV0 + Vec3 e1 = inV1 - inV0; + Vec3 e2 = inV2 - inV0; + + // Begin calculating determinant - also used to calculate u parameter + Vec3 p = inDirection.Cross(e2); + + // if determinant is near zero, ray lies in plane of triangle + Vec3 det = Vec3::sReplicate(e1.Dot(p)); + + // Check if determinant is near zero + UVec4 det_near_zero = Vec3::sLess(det.Abs(), epsilon); + + // When the determinant is near zero, set it to one to avoid dividing by zero + det = Vec3::sSelect(det, Vec3::sReplicate(1.0f), det_near_zero); + + // Calculate distance from inV0 to ray origin + Vec3 s = inOrigin - inV0; + + // Calculate u parameter + Vec3 u = Vec3::sReplicate(s.Dot(p)) / det; + + // Prepare to test v parameter + Vec3 q = s.Cross(e1); + + // Calculate v parameter + Vec3 v = Vec3::sReplicate(inDirection.Dot(q)) / det; + + // Get intersection point + Vec3 t = Vec3::sReplicate(e2.Dot(q)) / det; + + // Check if there is an intersection + UVec4 no_intersection = + UVec4::sOr + ( + UVec4::sOr + ( + UVec4::sOr + ( + det_near_zero, + Vec3::sLess(u, zero) + ), + UVec4::sOr + ( + Vec3::sLess(v, zero), + Vec3::sGreater(u + v, one) + ) + ), + Vec3::sLess(t, zero) + ); + + // Select intersection point or FLT_MAX based on if there is an intersection or not + return Vec3::sSelect(t, Vec3::sReplicate(FLT_MAX), no_intersection).GetX(); +} + +/// Intersect ray with 4 triangles in SOA format, returns 4 vector of closest points or FLT_MAX if no hit (uses bit tricks to do less divisions) +JPH_INLINE Vec4 RayTriangle4(Vec3Arg inOrigin, Vec3Arg inDirection, Vec4Arg inV0X, Vec4Arg inV0Y, Vec4Arg inV0Z, Vec4Arg inV1X, Vec4Arg inV1Y, Vec4Arg inV1Z, Vec4Arg inV2X, Vec4Arg inV2Y, Vec4Arg inV2Z) +{ + // Epsilon + Vec4 epsilon = Vec4::sReplicate(1.0e-12f); + + // Zero + Vec4 zero = Vec4::sZero(); + + // Find vectors for two edges sharing inV0 + Vec4 e1x = inV1X - inV0X; + Vec4 e1y = inV1Y - inV0Y; + Vec4 e1z = inV1Z - inV0Z; + Vec4 e2x = inV2X - inV0X; + Vec4 e2y = inV2Y - inV0Y; + Vec4 e2z = inV2Z - inV0Z; + + // Get direction vector components + Vec4 dx = inDirection.SplatX(); + Vec4 dy = inDirection.SplatY(); + Vec4 dz = inDirection.SplatZ(); + + // Begin calculating determinant - also used to calculate u parameter + Vec4 px = dy * e2z - dz * e2y; + Vec4 py = dz * e2x - dx * e2z; + Vec4 pz = dx * e2y - dy * e2x; + + // if determinant is near zero, ray lies in plane of triangle + Vec4 det = e1x * px + e1y * py + e1z * pz; + + // Get sign bit for determinant and make positive + Vec4 det_sign = Vec4::sAnd(det, UVec4::sReplicate(0x80000000).ReinterpretAsFloat()); + det = Vec4::sXor(det, det_sign); + + // Check which determinants are near zero + UVec4 det_near_zero = Vec4::sLess(det, epsilon); + + // Set components of the determinant to 1 that are near zero to avoid dividing by zero + det = Vec4::sSelect(det, Vec4::sReplicate(1.0f), det_near_zero); + + // Calculate distance from inV0 to ray origin + Vec4 sx = inOrigin.SplatX() - inV0X; + Vec4 sy = inOrigin.SplatY() - inV0Y; + Vec4 sz = inOrigin.SplatZ() - inV0Z; + + // Calculate u parameter and flip sign if determinant was negative + Vec4 u = Vec4::sXor(sx * px + sy * py + sz * pz, det_sign); + + // Prepare to test v parameter + Vec4 qx = sy * e1z - sz * e1y; + Vec4 qy = sz * e1x - sx * e1z; + Vec4 qz = sx * e1y - sy * e1x; + + // Calculate v parameter and flip sign if determinant was negative + Vec4 v = Vec4::sXor(dx * qx + dy * qy + dz * qz, det_sign); + + // Get intersection point and flip sign if determinant was negative + Vec4 t = Vec4::sXor(e2x * qx + e2y * qy + e2z * qz, det_sign); + + // Check if there is an intersection + UVec4 no_intersection = + UVec4::sOr + ( + UVec4::sOr + ( + UVec4::sOr + ( + det_near_zero, + Vec4::sLess(u, zero) + ), + UVec4::sOr + ( + Vec4::sLess(v, zero), + Vec4::sGreater(u + v, det) + ) + ), + Vec4::sLess(t, zero) + ); + + // Select intersection point or FLT_MAX based on if there is an intersection or not + return Vec4::sSelect(t / det, Vec4::sReplicate(FLT_MAX), no_intersection); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/Sphere.h b/thirdparty/jolt_physics/Jolt/Geometry/Sphere.h new file mode 100644 index 000000000000..05f7dd14f37d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/Sphere.h @@ -0,0 +1,72 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class [[nodiscard]] Sphere +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + inline Sphere() = default; + inline Sphere(const Float3 &inCenter, float inRadius) : mCenter(inCenter), mRadius(inRadius) { } + inline Sphere(Vec3Arg inCenter, float inRadius) : mRadius(inRadius) { inCenter.StoreFloat3(&mCenter); } + + /// Calculate the support vector for this convex shape. + inline Vec3 GetSupport(Vec3Arg inDirection) const + { + float length = inDirection.Length(); + return length > 0.0f ? Vec3::sLoadFloat3Unsafe(mCenter) + (mRadius/ length) * inDirection : Vec3::sLoadFloat3Unsafe(mCenter); + } + + // Properties + inline Vec3 GetCenter() const { return Vec3::sLoadFloat3Unsafe(mCenter); } + inline float GetRadius() const { return mRadius; } + + /// Test if two spheres overlap + inline bool Overlaps(const Sphere &inB) const + { + return (Vec3::sLoadFloat3Unsafe(mCenter) - Vec3::sLoadFloat3Unsafe(inB.mCenter)).LengthSq() <= Square(mRadius + inB.mRadius); + } + + /// Check if this sphere overlaps with a box + inline bool Overlaps(const AABox &inOther) const + { + return inOther.GetSqDistanceTo(GetCenter()) <= Square(mRadius); + } + + /// Create the minimal sphere that encapsulates this sphere and inPoint + inline void EncapsulatePoint(Vec3Arg inPoint) + { + // Calculate distance between point and center + Vec3 center = GetCenter(); + Vec3 d_vec = inPoint - center; + float d_sq = d_vec.LengthSq(); + if (d_sq > Square(mRadius)) + { + // It is further away than radius, we need to widen the sphere + // The diameter of the new sphere is radius + d, so the new radius is half of that + float d = sqrt(d_sq); + float radius = 0.5f * (mRadius + d); + + // The center needs to shift by new radius - old radius in the direction of d + center += (radius - mRadius) / d * d_vec; + + // Store new sphere + center.StoreFloat3(&mCenter); + mRadius = radius; + } + } + +private: + Float3 mCenter; + float mRadius; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/Triangle.h b/thirdparty/jolt_physics/Jolt/Geometry/Triangle.h new file mode 100644 index 000000000000..7ad718fc2a8b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/Triangle.h @@ -0,0 +1,34 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// A simple triangle and its material +class Triangle +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + Triangle() = default; + Triangle(const Float3 &inV1, const Float3 &inV2, const Float3 &inV3, uint32 inMaterialIndex = 0, uint32 inUserData = 0) : mV { inV1, inV2, inV3 }, mMaterialIndex(inMaterialIndex), mUserData(inUserData) { } + Triangle(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, uint32 inMaterialIndex = 0, uint32 inUserData = 0) : mMaterialIndex(inMaterialIndex), mUserData(inUserData) { inV1.StoreFloat3(&mV[0]); inV2.StoreFloat3(&mV[1]); inV3.StoreFloat3(&mV[2]); } + + /// Get center of triangle + Vec3 GetCentroid() const + { + return (Vec3::sLoadFloat3Unsafe(mV[0]) + Vec3::sLoadFloat3Unsafe(mV[1]) + Vec3::sLoadFloat3Unsafe(mV[2])) * (1.0f / 3.0f); + } + + /// Vertices + Float3 mV[3]; + uint32 mMaterialIndex = 0; ///< Follows mV[3] so that we can read mV as 4 vectors + uint32 mUserData = 0; ///< User data that can be used for anything by the application, e.g. for tracking the original index of the triangle +}; + +using TriangleList = Array; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Jolt.h b/thirdparty/jolt_physics/Jolt/Jolt.h new file mode 100644 index 000000000000..acc400ce9e3c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Jolt.h @@ -0,0 +1,16 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +// Project includes +#include +#include +#include +#include +#include +#include +#include +#include +#include diff --git a/thirdparty/jolt_physics/Jolt/Jolt.natvis b/thirdparty/jolt_physics/Jolt/Jolt.natvis new file mode 100644 index 000000000000..bebafa1adcdd --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Jolt.natvis @@ -0,0 +1,116 @@ + + + + r={(int)r}, g={(int)g}, b={(int)b}, a={(int)a} + + + {x}, {y} + + + {x}, {y}, {z} + + + {x}, {y}, {z}, {w} + + + {mF32[0]}, {mF32[1]}, {mF32[2]}, L^2={mF32[0]*mF32[0]+mF32[1]*mF32[1]+mF32[2]*mF32[2]} + + + {mF64[0]}, {mF64[1]}, {mF64[2]}, L^2={mF64[0]*mF64[0]+mF64[1]*mF64[1]+mF64[2]*mF64[2]} + + + {mF32[0]}, {mF32[1]}, {mF32[2]}, {mF32[3]}, L^2={mF32[0]*mF32[0]+mF32[1]*mF32[1]+mF32[2]*mF32[2]+mF32[3]*mF32[3]} + + + {mU32[0]}, {mU32[1]}, {mU32[2]}, {mU32[3]} + + + {uint(mU8[0])}, {uint(mU8[1])}, {uint(mU8[2])}, {uint(mU8[3])}, {uint(mU8[4])}, {uint(mU8[5])}, {uint(mU8[6])}, {uint(mU8[7])}, {uint(mU8[8])}, {uint(mU8[9])}, {uint(mU8[10])}, {uint(mU8[11])}, {uint(mU8[12])}, {uint(mU8[13])}, {uint(mU8[14])}, {uint(mU8[15])} + + + {mValue} + + + {mCol[0].mF32[0]}, {mCol[1].mF32[0]}, {mCol[2].mF32[0]}, {mCol[3].mF32[0]} | {mCol[0].mF32[1]}, {mCol[1].mF32[1]}, {mCol[2].mF32[1]}, {mCol[3].mF32[1]} | {mCol[0].mF32[2]}, {mCol[1].mF32[2]}, {mCol[2].mF32[2]}, {mCol[3].mF32[2]} + + + {mCol[0].mF32[0]}, {mCol[1].mF32[0]}, {mCol[2].mF32[0]}, {mCol[3].mF32[0]} + + + {mCol[0].mF32[1]}, {mCol[1].mF32[1]}, {mCol[2].mF32[1]}, {mCol[3].mF32[1]} + + + {mCol[0].mF32[2]}, {mCol[1].mF32[2]}, {mCol[2].mF32[2]}, {mCol[3].mF32[2]} + + + {mCol[0].mF32[3]}, {mCol[1].mF32[3]}, {mCol[2].mF32[3]}, {mCol[3].mF32[3]} + + + + + {mCol[0].mF32[0]}, {mCol[1].mF32[0]}, {mCol[2].mF32[0]}, {mCol3.mF64[0]} | {mCol[0].mF32[1]}, {mCol[1].mF32[1]}, {mCol[2].mF32[1]}, {mCol3.mF64[1]} | {mCol[0].mF32[2]}, {mCol[1].mF32[2]}, {mCol[2].mF32[2]}, {mCol3.mF64[2]} + + + {mCol[0].mF32[0]}, {mCol[1].mF32[0]}, {mCol[2].mF32[0]}, {mCol3.mF64[0]} + + + {mCol[0].mF32[1]}, {mCol[1].mF32[1]}, {mCol[2].mF32[1]}, {mCol3.mF64[1]} + + + {mCol[0].mF32[2]}, {mCol[1].mF32[2]}, {mCol[2].mF32[2]}, {mCol3.mF64[2]} + + + {mCol[0].mF32[3]}, {mCol[1].mF32[3]}, {mCol[2].mF32[3]}, 1} + + + + + min=({mMin}), max=({mMax}) + + + {mID} + + + {mDebugName}: p=({mPosition.mF32[0],g}, {mPosition.mF32[1],g}, {mPosition.mF32[2],g}), r=({mRotation.mValue.mF32[0],g}, {mRotation.mValue.mF32[1],g}, {mRotation.mValue.mF32[2],g}, {mRotation.mValue.mF32[3],g}), v=({mLinearVelocity.mF32[0],g}, {mLinearVelocity.mF32[1],g}, {mLinearVelocity.mF32[2],g}), w=({mAngularVelocity.mF32[0],g}, {mAngularVelocity.mF32[1],g}, {mAngularVelocity.mF32[2],g}) + + + bodies={mBodies._Mypair._Myval2._Mylast - mBodies._Mypair._Myval2._Myfirst}, active={mActiveBodies._Mypair._Myval2._Mylast - mActiveBodies._Mypair._Myval2._Myfirst} + + + size={mSize} + + mSize + + mSize + (value_type *)mElements + + + + + size={mSize} + + mSize + mCapacity + + mSize + mElements + + + + + size={mSize} + + mSize + mMaxSize + + mMaxSize + mData[$i] + "--Empty--" + "--Deleted--" + + + + + {(value_type *)mPtr}, stride={mStride} + + diff --git a/thirdparty/jolt_physics/Jolt/Math/BVec16.h b/thirdparty/jolt_physics/Jolt/Math/BVec16.h new file mode 100644 index 000000000000..a4ac12bf79d6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/BVec16.h @@ -0,0 +1,99 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// A vector consisting of 16 bytes +class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) BVec16 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // Underlying vector type +#if defined(JPH_USE_SSE) + using Type = __m128i; +#elif defined(JPH_USE_NEON) + using Type = uint8x16_t; +#else + using Type = struct { uint64 mData[2]; }; +#endif + + /// Constructor + BVec16() = default; ///< Intentionally not initialized for performance reasons + BVec16(const BVec16 &inRHS) = default; + BVec16 & operator = (const BVec16 &inRHS) = default; + JPH_INLINE BVec16(Type inRHS) : mValue(inRHS) { } + + /// Create a vector from 16 bytes + JPH_INLINE BVec16(uint8 inB0, uint8 inB1, uint8 inB2, uint8 inB3, uint8 inB4, uint8 inB5, uint8 inB6, uint8 inB7, uint8 inB8, uint8 inB9, uint8 inB10, uint8 inB11, uint8 inB12, uint8 inB13, uint8 inB14, uint8 inB15); + + /// Create a vector from two uint64's + JPH_INLINE BVec16(uint64 inV0, uint64 inV1); + + /// Comparison + JPH_INLINE bool operator == (BVec16Arg inV2) const; + JPH_INLINE bool operator != (BVec16Arg inV2) const { return !(*this == inV2); } + + /// Vector with all zeros + static JPH_INLINE BVec16 sZero(); + + /// Replicate int inV across all components + static JPH_INLINE BVec16 sReplicate(uint8 inV); + + /// Load 16 bytes from memory + static JPH_INLINE BVec16 sLoadByte16(const uint8 *inV); + + /// Equals (component wise), highest bit of each component that is set is considered true + static JPH_INLINE BVec16 sEquals(BVec16Arg inV1, BVec16Arg inV2); + + /// Logical or (component wise) + static JPH_INLINE BVec16 sOr(BVec16Arg inV1, BVec16Arg inV2); + + /// Logical xor (component wise) + static JPH_INLINE BVec16 sXor(BVec16Arg inV1, BVec16Arg inV2); + + /// Logical and (component wise) + static JPH_INLINE BVec16 sAnd(BVec16Arg inV1, BVec16Arg inV2); + + /// Logical not (component wise) + static JPH_INLINE BVec16 sNot(BVec16Arg inV1); + + /// Get component by index + JPH_INLINE uint8 operator [] (uint inCoordinate) const { JPH_ASSERT(inCoordinate < 16); return mU8[inCoordinate]; } + JPH_INLINE uint8 & operator [] (uint inCoordinate) { JPH_ASSERT(inCoordinate < 16); return mU8[inCoordinate]; } + + /// Test if any of the components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAnyTrue() const; + + /// Test if all components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAllTrue() const; + + /// Store if mU8[0] is true in bit 0, mU8[1] in bit 1, etc. (true is when highest bit of component is set) + JPH_INLINE int GetTrues() const; + + /// To String + friend ostream & operator << (ostream &inStream, BVec16Arg inV) + { + inStream << uint(inV.mU8[0]) << ", " << uint(inV.mU8[1]) << ", " << uint(inV.mU8[2]) << ", " << uint(inV.mU8[3]) << ", " + << uint(inV.mU8[4]) << ", " << uint(inV.mU8[5]) << ", " << uint(inV.mU8[6]) << ", " << uint(inV.mU8[7]) << ", " + << uint(inV.mU8[8]) << ", " << uint(inV.mU8[9]) << ", " << uint(inV.mU8[10]) << ", " << uint(inV.mU8[11]) << ", " + << uint(inV.mU8[12]) << ", " << uint(inV.mU8[13]) << ", " << uint(inV.mU8[14]) << ", " << uint(inV.mU8[15]); + return inStream; + } + + union + { + Type mValue; + uint8 mU8[16]; + uint64 mU64[2]; + }; +}; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "BVec16.inl" diff --git a/thirdparty/jolt_physics/Jolt/Math/BVec16.inl b/thirdparty/jolt_physics/Jolt/Math/BVec16.inl new file mode 100644 index 000000000000..91e063a0134c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/BVec16.inl @@ -0,0 +1,177 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +JPH_NAMESPACE_BEGIN + +BVec16::BVec16(uint8 inB0, uint8 inB1, uint8 inB2, uint8 inB3, uint8 inB4, uint8 inB5, uint8 inB6, uint8 inB7, uint8 inB8, uint8 inB9, uint8 inB10, uint8 inB11, uint8 inB12, uint8 inB13, uint8 inB14, uint8 inB15) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_set_epi8(char(inB15), char(inB14), char(inB13), char(inB12), char(inB11), char(inB10), char(inB9), char(inB8), char(inB7), char(inB6), char(inB5), char(inB4), char(inB3), char(inB2), char(inB1), char(inB0)); +#elif defined(JPH_USE_NEON) + uint8x8_t v1 = vcreate_u8(uint64(inB0) | (uint64(inB1) << 8) | (uint64(inB2) << 16) | (uint64(inB3) << 24) | (uint64(inB4) << 32) | (uint64(inB5) << 40) | (uint64(inB6) << 48) | (uint64(inB7) << 56)); + uint8x8_t v2 = vcreate_u8(uint64(inB8) | (uint64(inB9) << 8) | (uint64(inB10) << 16) | (uint64(inB11) << 24) | (uint64(inB12) << 32) | (uint64(inB13) << 40) | (uint64(inB14) << 48) | (uint64(inB15) << 56)); + mValue = vcombine_u8(v1, v2); +#else + mU8[0] = inB0; + mU8[1] = inB1; + mU8[2] = inB2; + mU8[3] = inB3; + mU8[4] = inB4; + mU8[5] = inB5; + mU8[6] = inB6; + mU8[7] = inB7; + mU8[8] = inB8; + mU8[9] = inB9; + mU8[10] = inB10; + mU8[11] = inB11; + mU8[12] = inB12; + mU8[13] = inB13; + mU8[14] = inB14; + mU8[15] = inB15; +#endif +} + +BVec16::BVec16(uint64 inV0, uint64 inV1) +{ + mU64[0] = inV0; + mU64[1] = inV1; +} + +bool BVec16::operator == (BVec16Arg inV2) const +{ + return sEquals(*this, inV2).TestAllTrue(); +} + +BVec16 BVec16::sZero() +{ +#if defined(JPH_USE_SSE) + return _mm_setzero_si128(); +#elif defined(JPH_USE_NEON) + return vdupq_n_u8(0); +#else + return BVec16(0, 0); +#endif +} + +BVec16 BVec16::sReplicate(uint8 inV) +{ +#if defined(JPH_USE_SSE) + return _mm_set1_epi8(char(inV)); +#elif defined(JPH_USE_NEON) + return vdupq_n_u8(inV); +#else + uint64 v(inV); + v |= v << 8; + v |= v << 16; + v |= v << 32; + return BVec16(v, v); +#endif +} + +BVec16 BVec16::sLoadByte16(const uint8 *inV) +{ +#if defined(JPH_USE_SSE) + return _mm_loadu_si128(reinterpret_cast(inV)); +#elif defined(JPH_USE_NEON) + return vld1q_u8(inV); +#else + return BVec16(inV[0], inV[1], inV[2], inV[3], inV[4], inV[5], inV[6], inV[7], inV[8], inV[9], inV[10], inV[11], inV[12], inV[13], inV[14], inV[15]); +#endif +} + +BVec16 BVec16::sEquals(BVec16Arg inV1, BVec16Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_cmpeq_epi8(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vceqq_u8(inV1.mValue, inV2.mValue); +#else + auto equals = [](uint64 inV1, uint64 inV2) { + uint64 r = inV1 ^ ~inV2; // Bits that are equal are 1 + r &= r << 1; // Combine bit 0 through 1 + r &= r << 2; // Combine bit 0 through 3 + r &= r << 4; // Combine bit 0 through 7 + r &= 0x8080808080808080UL; // Keep only the highest bit of each byte + return r; + }; + return BVec16(equals(inV1.mU64[0], inV2.mU64[0]), equals(inV1.mU64[1], inV2.mU64[1])); +#endif +} + +BVec16 BVec16::sOr(BVec16Arg inV1, BVec16Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_or_si128(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vorrq_u8(inV1.mValue, inV2.mValue); +#else + return BVec16(inV1.mU64[0] | inV2.mU64[0], inV1.mU64[1] | inV2.mU64[1]); +#endif +} + +BVec16 BVec16::sXor(BVec16Arg inV1, BVec16Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_xor_si128(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return veorq_u8(inV1.mValue, inV2.mValue); +#else + return BVec16(inV1.mU64[0] ^ inV2.mU64[0], inV1.mU64[1] ^ inV2.mU64[1]); +#endif +} + +BVec16 BVec16::sAnd(BVec16Arg inV1, BVec16Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_and_si128(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vandq_u8(inV1.mValue, inV2.mValue); +#else + return BVec16(inV1.mU64[0] & inV2.mU64[0], inV1.mU64[1] & inV2.mU64[1]); +#endif +} + + +BVec16 BVec16::sNot(BVec16Arg inV1) +{ +#if defined(JPH_USE_SSE) + return sXor(inV1, sReplicate(0xff)); +#elif defined(JPH_USE_NEON) + return vmvnq_u8(inV1.mValue); +#else + return BVec16(~inV1.mU64[0], ~inV1.mU64[1]); +#endif +} + +int BVec16::GetTrues() const +{ +#if defined(JPH_USE_SSE) + return _mm_movemask_epi8(mValue); +#else + int result = 0; + for (int i = 0; i < 16; ++i) + result |= int(mU8[i] >> 7) << i; + return result; +#endif +} + +bool BVec16::TestAnyTrue() const +{ +#if defined(JPH_USE_SSE) + return _mm_movemask_epi8(mValue) != 0; +#else + return ((mU64[0] | mU64[1]) & 0x8080808080808080UL) != 0; +#endif +} + +bool BVec16::TestAllTrue() const +{ +#if defined(JPH_USE_SSE) + return _mm_movemask_epi8(mValue) == 0b1111111111111111; +#else + return ((mU64[0] & mU64[1]) & 0x8080808080808080UL) == 0x8080808080808080UL; +#endif +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/DMat44.h b/thirdparty/jolt_physics/Jolt/Math/DMat44.h new file mode 100644 index 000000000000..cd8042107ada --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/DMat44.h @@ -0,0 +1,158 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Holds a 4x4 matrix of floats with the last column consisting of doubles +class [[nodiscard]] alignas(JPH_DVECTOR_ALIGNMENT) DMat44 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // Underlying column type + using Type = Vec4::Type; + using DType = DVec3::Type; + using DTypeArg = DVec3::TypeArg; + + // Argument type + using ArgType = DMat44Arg; + + /// Constructor + DMat44() = default; ///< Intentionally not initialized for performance reasons + JPH_INLINE DMat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, DVec3Arg inC4); + DMat44(const DMat44 &inM2) = default; + DMat44 & operator = (const DMat44 &inM2) = default; + JPH_INLINE explicit DMat44(Mat44Arg inM); + JPH_INLINE DMat44(Mat44Arg inRot, DVec3Arg inT); + JPH_INLINE DMat44(Type inC1, Type inC2, Type inC3, DTypeArg inC4); + + /// Zero matrix + static JPH_INLINE DMat44 sZero(); + + /// Identity matrix + static JPH_INLINE DMat44 sIdentity(); + + /// Rotate from quaternion + static JPH_INLINE DMat44 sRotation(QuatArg inQuat) { return DMat44(Mat44::sRotation(inQuat), DVec3::sZero()); } + + /// Get matrix that translates + static JPH_INLINE DMat44 sTranslation(DVec3Arg inV) { return DMat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), inV); } + + /// Get matrix that rotates and translates + static JPH_INLINE DMat44 sRotationTranslation(QuatArg inR, DVec3Arg inT) { return DMat44(Mat44::sRotation(inR), inT); } + + /// Get inverse matrix of sRotationTranslation + static JPH_INLINE DMat44 sInverseRotationTranslation(QuatArg inR, DVec3Arg inT); + + /// Get matrix that scales (produces a matrix with (inV, 1) on its diagonal) + static JPH_INLINE DMat44 sScale(Vec3Arg inV) { return DMat44(Mat44::sScale(inV), DVec3::sZero()); } + + /// Convert to Mat44 rounding to nearest + JPH_INLINE Mat44 ToMat44() const { return Mat44(mCol[0], mCol[1], mCol[2], Vec3(mCol3)); } + + /// Comparison + JPH_INLINE bool operator == (DMat44Arg inM2) const; + JPH_INLINE bool operator != (DMat44Arg inM2) const { return !(*this == inM2); } + + /// Test if two matrices are close + JPH_INLINE bool IsClose(DMat44Arg inM2, float inMaxDistSq = 1.0e-12f) const; + + /// Multiply matrix by matrix + JPH_INLINE DMat44 operator * (Mat44Arg inM) const; + + /// Multiply matrix by matrix + JPH_INLINE DMat44 operator * (DMat44Arg inM) const; + + /// Multiply vector by matrix + JPH_INLINE DVec3 operator * (Vec3Arg inV) const; + + /// Multiply vector by matrix + JPH_INLINE DVec3 operator * (DVec3Arg inV) const; + + /// Multiply vector by only 3x3 part of the matrix + JPH_INLINE Vec3 Multiply3x3(Vec3Arg inV) const { return GetRotation().Multiply3x3(inV); } + + /// Multiply vector by only 3x3 part of the matrix + JPH_INLINE DVec3 Multiply3x3(DVec3Arg inV) const; + + /// Multiply vector by only 3x3 part of the transpose of the matrix (\f$result = this^T \: inV\f$) + JPH_INLINE Vec3 Multiply3x3Transposed(Vec3Arg inV) const { return GetRotation().Multiply3x3Transposed(inV); } + + /// Scale a matrix: result = this * Mat44::sScale(inScale) + JPH_INLINE DMat44 PreScaled(Vec3Arg inScale) const; + + /// Scale a matrix: result = Mat44::sScale(inScale) * this + JPH_INLINE DMat44 PostScaled(Vec3Arg inScale) const; + + /// Pre multiply by translation matrix: result = this * Mat44::sTranslation(inTranslation) + JPH_INLINE DMat44 PreTranslated(Vec3Arg inTranslation) const; + + /// Pre multiply by translation matrix: result = this * Mat44::sTranslation(inTranslation) + JPH_INLINE DMat44 PreTranslated(DVec3Arg inTranslation) const; + + /// Post multiply by translation matrix: result = Mat44::sTranslation(inTranslation) * this (i.e. add inTranslation to the 4-th column) + JPH_INLINE DMat44 PostTranslated(Vec3Arg inTranslation) const; + + /// Post multiply by translation matrix: result = Mat44::sTranslation(inTranslation) * this (i.e. add inTranslation to the 4-th column) + JPH_INLINE DMat44 PostTranslated(DVec3Arg inTranslation) const; + + /// Access to the columns + JPH_INLINE Vec3 GetAxisX() const { return Vec3(mCol[0]); } + JPH_INLINE void SetAxisX(Vec3Arg inV) { mCol[0] = Vec4(inV, 0.0f); } + JPH_INLINE Vec3 GetAxisY() const { return Vec3(mCol[1]); } + JPH_INLINE void SetAxisY(Vec3Arg inV) { mCol[1] = Vec4(inV, 0.0f); } + JPH_INLINE Vec3 GetAxisZ() const { return Vec3(mCol[2]); } + JPH_INLINE void SetAxisZ(Vec3Arg inV) { mCol[2] = Vec4(inV, 0.0f); } + JPH_INLINE DVec3 GetTranslation() const { return mCol3; } + JPH_INLINE void SetTranslation(DVec3Arg inV) { mCol3 = inV; } + JPH_INLINE Vec3 GetColumn3(uint inCol) const { JPH_ASSERT(inCol < 3); return Vec3(mCol[inCol]); } + JPH_INLINE void SetColumn3(uint inCol, Vec3Arg inV) { JPH_ASSERT(inCol < 3); mCol[inCol] = Vec4(inV, 0.0f); } + JPH_INLINE Vec4 GetColumn4(uint inCol) const { JPH_ASSERT(inCol < 3); return mCol[inCol]; } + JPH_INLINE void SetColumn4(uint inCol, Vec4Arg inV) { JPH_ASSERT(inCol < 3); mCol[inCol] = inV; } + + /// Transpose 3x3 subpart of matrix + JPH_INLINE Mat44 Transposed3x3() const { return GetRotation().Transposed3x3(); } + + /// Inverse 4x4 matrix + JPH_INLINE DMat44 Inversed() const; + + /// Inverse 4x4 matrix when it only contains rotation and translation + JPH_INLINE DMat44 InversedRotationTranslation() const; + + /// Get rotation part only (note: retains the first 3 values from the bottom row) + JPH_INLINE Mat44 GetRotation() const { return Mat44(mCol[0], mCol[1], mCol[2], Vec4(0, 0, 0, 1)); } + + /// Updates the rotation part of this matrix (the first 3 columns) + JPH_INLINE void SetRotation(Mat44Arg inRotation); + + /// Convert to quaternion + JPH_INLINE Quat GetQuaternion() const { return GetRotation().GetQuaternion(); } + + /// Get matrix that transforms a direction with the same transform as this matrix (length is not preserved) + JPH_INLINE Mat44 GetDirectionPreservingMatrix() const { return GetRotation().Inversed3x3().Transposed3x3(); } + + /// Works identical to Mat44::Decompose + JPH_INLINE DMat44 Decompose(Vec3 &outScale) const { return DMat44(GetRotation().Decompose(outScale), mCol3); } + + /// To String + friend ostream & operator << (ostream &inStream, DMat44Arg inM) + { + inStream << inM.mCol[0] << ", " << inM.mCol[1] << ", " << inM.mCol[2] << ", " << inM.mCol3; + return inStream; + } + +private: + Vec4 mCol[3]; ///< Rotation columns + DVec3 mCol3; ///< Translation column, 4th element is assumed to be 1 +}; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "DMat44.inl" diff --git a/thirdparty/jolt_physics/Jolt/Math/DMat44.inl b/thirdparty/jolt_physics/Jolt/Math/DMat44.inl new file mode 100644 index 000000000000..462cf79114c3 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/DMat44.inl @@ -0,0 +1,310 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +DMat44::DMat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, DVec3Arg inC4) : + mCol { inC1, inC2, inC3 }, + mCol3(inC4) +{ +} + +DMat44::DMat44(Type inC1, Type inC2, Type inC3, DTypeArg inC4) : + mCol { inC1, inC2, inC3 }, + mCol3(inC4) +{ +} + +DMat44::DMat44(Mat44Arg inM) : + mCol { inM.GetColumn4(0), inM.GetColumn4(1), inM.GetColumn4(2) }, + mCol3(inM.GetTranslation()) +{ +} + +DMat44::DMat44(Mat44Arg inRot, DVec3Arg inT) : + mCol { inRot.GetColumn4(0), inRot.GetColumn4(1), inRot.GetColumn4(2) }, + mCol3(inT) +{ +} + +DMat44 DMat44::sZero() +{ + return DMat44(Vec4::sZero(), Vec4::sZero(), Vec4::sZero(), DVec3::sZero()); +} + +DMat44 DMat44::sIdentity() +{ + return DMat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), DVec3::sZero()); +} + +DMat44 DMat44::sInverseRotationTranslation(QuatArg inR, DVec3Arg inT) +{ + Mat44 m = Mat44::sRotation(inR.Conjugated()); + DMat44 dm(m, DVec3::sZero()); + dm.SetTranslation(-dm.Multiply3x3(inT)); + return dm; +} + +bool DMat44::operator == (DMat44Arg inM2) const +{ + return mCol[0] == inM2.mCol[0] + && mCol[1] == inM2.mCol[1] + && mCol[2] == inM2.mCol[2] + && mCol3 == inM2.mCol3; +} + +bool DMat44::IsClose(DMat44Arg inM2, float inMaxDistSq) const +{ + for (int i = 0; i < 3; ++i) + if (!mCol[i].IsClose(inM2.mCol[i], inMaxDistSq)) + return false; + return mCol3.IsClose(inM2.mCol3, double(inMaxDistSq)); +} + +DVec3 DMat44::operator * (Vec3Arg inV) const +{ +#if defined(JPH_USE_AVX) + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2)))); + return DVec3::sFixW(_mm256_add_pd(mCol3.mValue, _mm256_cvtps_pd(t))); +#elif defined(JPH_USE_SSE) + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2)))); + __m128d low = _mm_add_pd(mCol3.mValue.mLow, _mm_cvtps_pd(t)); + __m128d high = _mm_add_pd(mCol3.mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(t, t, _MM_SHUFFLE(2, 2, 2, 2)))); + return DVec3({ low, high }); +#elif defined(JPH_USE_NEON) + float32x4_t t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2)); + float64x2_t low = vaddq_f64(mCol3.mValue.val[0], vcvt_f64_f32(vget_low_f32(t))); + float64x2_t high = vaddq_f64(mCol3.mValue.val[1], vcvt_high_f64_f32(t)); + return DVec3::sFixW({ low, high }); +#else + return DVec3( + mCol3.mF64[0] + double(mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2]), + mCol3.mF64[1] + double(mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2]), + mCol3.mF64[2] + double(mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2])); +#endif +} + +DVec3 DMat44::operator * (DVec3Arg inV) const +{ +#if defined(JPH_USE_AVX) + __m256d t = _mm256_add_pd(mCol3.mValue, _mm256_mul_pd(_mm256_cvtps_pd(mCol[0].mValue), _mm256_set1_pd(inV.mF64[0]))); + t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[1].mValue), _mm256_set1_pd(inV.mF64[1]))); + t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[2].mValue), _mm256_set1_pd(inV.mF64[2]))); + return DVec3::sFixW(t); +#elif defined(JPH_USE_SSE) + __m128d xxxx = _mm_set1_pd(inV.mF64[0]); + __m128d yyyy = _mm_set1_pd(inV.mF64[1]); + __m128d zzzz = _mm_set1_pd(inV.mF64[2]); + __m128 col0 = mCol[0].mValue; + __m128 col1 = mCol[1].mValue; + __m128 col2 = mCol[2].mValue; + __m128d t_low = _mm_add_pd(mCol3.mValue.mLow, _mm_mul_pd(_mm_cvtps_pd(col0), xxxx)); + t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col1), yyyy)); + t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col2), zzzz)); + __m128d t_high = _mm_add_pd(mCol3.mValue.mHigh, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col0, col0, _MM_SHUFFLE(2, 2, 2, 2))), xxxx)); + t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col1, col1, _MM_SHUFFLE(2, 2, 2, 2))), yyyy)); + t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col2, col2, _MM_SHUFFLE(2, 2, 2, 2))), zzzz)); + return DVec3({ t_low, t_high }); +#elif defined(JPH_USE_NEON) + float64x2_t xxxx = vdupq_laneq_f64(inV.mValue.val[0], 0); + float64x2_t yyyy = vdupq_laneq_f64(inV.mValue.val[0], 1); + float64x2_t zzzz = vdupq_laneq_f64(inV.mValue.val[1], 0); + float32x4_t col0 = mCol[0].mValue; + float32x4_t col1 = mCol[1].mValue; + float32x4_t col2 = mCol[2].mValue; + float64x2_t t_low = vaddq_f64(mCol3.mValue.val[0], vmulq_f64(vcvt_f64_f32(vget_low_f32(col0)), xxxx)); + t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col1)), yyyy)); + t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col2)), zzzz)); + float64x2_t t_high = vaddq_f64(mCol3.mValue.val[1], vmulq_f64(vcvt_high_f64_f32(col0), xxxx)); + t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col1), yyyy)); + t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col2), zzzz)); + return DVec3::sFixW({ t_low, t_high }); +#else + return DVec3( + mCol3.mF64[0] + double(mCol[0].mF32[0]) * inV.mF64[0] + double(mCol[1].mF32[0]) * inV.mF64[1] + double(mCol[2].mF32[0]) * inV.mF64[2], + mCol3.mF64[1] + double(mCol[0].mF32[1]) * inV.mF64[0] + double(mCol[1].mF32[1]) * inV.mF64[1] + double(mCol[2].mF32[1]) * inV.mF64[2], + mCol3.mF64[2] + double(mCol[0].mF32[2]) * inV.mF64[0] + double(mCol[1].mF32[2]) * inV.mF64[1] + double(mCol[2].mF32[2]) * inV.mF64[2]); +#endif +} + +DVec3 DMat44::Multiply3x3(DVec3Arg inV) const +{ +#if defined(JPH_USE_AVX) + __m256d t = _mm256_mul_pd(_mm256_cvtps_pd(mCol[0].mValue), _mm256_set1_pd(inV.mF64[0])); + t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[1].mValue), _mm256_set1_pd(inV.mF64[1]))); + t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[2].mValue), _mm256_set1_pd(inV.mF64[2]))); + return DVec3::sFixW(t); +#elif defined(JPH_USE_SSE) + __m128d xxxx = _mm_set1_pd(inV.mF64[0]); + __m128d yyyy = _mm_set1_pd(inV.mF64[1]); + __m128d zzzz = _mm_set1_pd(inV.mF64[2]); + __m128 col0 = mCol[0].mValue; + __m128 col1 = mCol[1].mValue; + __m128 col2 = mCol[2].mValue; + __m128d t_low = _mm_mul_pd(_mm_cvtps_pd(col0), xxxx); + t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col1), yyyy)); + t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col2), zzzz)); + __m128d t_high = _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col0, col0, _MM_SHUFFLE(2, 2, 2, 2))), xxxx); + t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col1, col1, _MM_SHUFFLE(2, 2, 2, 2))), yyyy)); + t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col2, col2, _MM_SHUFFLE(2, 2, 2, 2))), zzzz)); + return DVec3({ t_low, t_high }); +#elif defined(JPH_USE_NEON) + float64x2_t xxxx = vdupq_laneq_f64(inV.mValue.val[0], 0); + float64x2_t yyyy = vdupq_laneq_f64(inV.mValue.val[0], 1); + float64x2_t zzzz = vdupq_laneq_f64(inV.mValue.val[1], 0); + float32x4_t col0 = mCol[0].mValue; + float32x4_t col1 = mCol[1].mValue; + float32x4_t col2 = mCol[2].mValue; + float64x2_t t_low = vmulq_f64(vcvt_f64_f32(vget_low_f32(col0)), xxxx); + t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col1)), yyyy)); + t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col2)), zzzz)); + float64x2_t t_high = vmulq_f64(vcvt_high_f64_f32(col0), xxxx); + t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col1), yyyy)); + t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col2), zzzz)); + return DVec3::sFixW({ t_low, t_high }); +#else + return DVec3( + double(mCol[0].mF32[0]) * inV.mF64[0] + double(mCol[1].mF32[0]) * inV.mF64[1] + double(mCol[2].mF32[0]) * inV.mF64[2], + double(mCol[0].mF32[1]) * inV.mF64[0] + double(mCol[1].mF32[1]) * inV.mF64[1] + double(mCol[2].mF32[1]) * inV.mF64[2], + double(mCol[0].mF32[2]) * inV.mF64[0] + double(mCol[1].mF32[2]) * inV.mF64[1] + double(mCol[2].mF32[2]) * inV.mF64[2]); +#endif +} + +DMat44 DMat44::operator * (Mat44Arg inM) const +{ + DMat44 result; + + // Rotation part +#if defined(JPH_USE_SSE) + for (int i = 0; i < 3; ++i) + { + __m128 c = inM.GetColumn4(i).mValue; + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2)))); + result.mCol[i].mValue = t; + } +#elif defined(JPH_USE_NEON) + for (int i = 0; i < 3; ++i) + { + Type c = inM.GetColumn4(i).mValue; + Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2)); + result.mCol[i].mValue = t; + } +#else + for (int i = 0; i < 3; ++i) + { + Vec4 coli = inM.GetColumn4(i); + result.mCol[i] = mCol[0] * coli.mF32[0] + mCol[1] * coli.mF32[1] + mCol[2] * coli.mF32[2]; + } +#endif + + // Translation part + result.mCol3 = *this * inM.GetTranslation(); + + return result; +} + +DMat44 DMat44::operator * (DMat44Arg inM) const +{ + DMat44 result; + + // Rotation part +#if defined(JPH_USE_SSE) + for (int i = 0; i < 3; ++i) + { + __m128 c = inM.mCol[i].mValue; + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2)))); + result.mCol[i].mValue = t; + } +#elif defined(JPH_USE_NEON) + for (int i = 0; i < 3; ++i) + { + Type c = inM.GetColumn4(i).mValue; + Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2)); + result.mCol[i].mValue = t; + } +#else + for (int i = 0; i < 3; ++i) + { + Vec4 coli = inM.mCol[i]; + result.mCol[i] = mCol[0] * coli.mF32[0] + mCol[1] * coli.mF32[1] + mCol[2] * coli.mF32[2]; + } +#endif + + // Translation part + result.mCol3 = *this * inM.GetTranslation(); + + return result; +} + +void DMat44::SetRotation(Mat44Arg inRotation) +{ + mCol[0] = inRotation.GetColumn4(0); + mCol[1] = inRotation.GetColumn4(1); + mCol[2] = inRotation.GetColumn4(2); +} + +DMat44 DMat44::PreScaled(Vec3Arg inScale) const +{ + return DMat44(inScale.GetX() * mCol[0], inScale.GetY() * mCol[1], inScale.GetZ() * mCol[2], mCol3); +} + +DMat44 DMat44::PostScaled(Vec3Arg inScale) const +{ + Vec4 scale(inScale, 1); + return DMat44(scale * mCol[0], scale * mCol[1], scale * mCol[2], DVec3(scale) * mCol3); +} + +DMat44 DMat44::PreTranslated(Vec3Arg inTranslation) const +{ + return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + Multiply3x3(inTranslation)); +} + +DMat44 DMat44::PreTranslated(DVec3Arg inTranslation) const +{ + return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + Multiply3x3(inTranslation)); +} + +DMat44 DMat44::PostTranslated(Vec3Arg inTranslation) const +{ + return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + inTranslation); +} + +DMat44 DMat44::PostTranslated(DVec3Arg inTranslation) const +{ + return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + inTranslation); +} + +DMat44 DMat44::Inversed() const +{ + DMat44 m(GetRotation().Inversed3x3()); + m.mCol3 = -m.Multiply3x3(mCol3); + return m; +} + +DMat44 DMat44::InversedRotationTranslation() const +{ + DMat44 m(GetRotation().Transposed3x3()); + m.mCol3 = -m.Multiply3x3(mCol3); + return m; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/DVec3.h b/thirdparty/jolt_physics/Jolt/Math/DVec3.h new file mode 100644 index 000000000000..74d209b2a849 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/DVec3.h @@ -0,0 +1,288 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// 3 component vector of doubles (stored as 4 vectors). +/// Note that we keep the 4th component the same as the 3rd component to avoid divisions by zero when JPH_FLOATING_POINT_EXCEPTIONS_ENABLED defined +class [[nodiscard]] alignas(JPH_DVECTOR_ALIGNMENT) DVec3 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // Underlying vector type +#if defined(JPH_USE_AVX) + using Type = __m256d; + using TypeArg = __m256d; +#elif defined(JPH_USE_SSE) + using Type = struct { __m128d mLow, mHigh; }; + using TypeArg = const Type &; +#elif defined(JPH_USE_NEON) + using Type = float64x2x2_t; + using TypeArg = const Type &; +#else + using Type = struct { double mData[4]; }; + using TypeArg = const Type &; +#endif + + // Argument type + using ArgType = DVec3Arg; + + /// Constructor + DVec3() = default; ///< Intentionally not initialized for performance reasons + DVec3(const DVec3 &inRHS) = default; + DVec3 & operator = (const DVec3 &inRHS) = default; + JPH_INLINE explicit DVec3(Vec3Arg inRHS); + JPH_INLINE explicit DVec3(Vec4Arg inRHS); + JPH_INLINE DVec3(TypeArg inRHS) : mValue(inRHS) { CheckW(); } + + /// Create a vector from 3 components + JPH_INLINE DVec3(double inX, double inY, double inZ); + + /// Load 3 doubles from memory + explicit JPH_INLINE DVec3(const Double3 &inV); + + /// Vector with all zeros + static JPH_INLINE DVec3 sZero(); + + /// Vectors with the principal axis + static JPH_INLINE DVec3 sAxisX() { return DVec3(1, 0, 0); } + static JPH_INLINE DVec3 sAxisY() { return DVec3(0, 1, 0); } + static JPH_INLINE DVec3 sAxisZ() { return DVec3(0, 0, 1); } + + /// Replicate inV across all components + static JPH_INLINE DVec3 sReplicate(double inV); + + /// Vector with all NaN's + static JPH_INLINE DVec3 sNaN(); + + /// Load 3 doubles from memory (reads 64 bits extra which it doesn't use) + static JPH_INLINE DVec3 sLoadDouble3Unsafe(const Double3 &inV); + + /// Store 3 doubles to memory + JPH_INLINE void StoreDouble3(Double3 *outV) const; + + /// Convert to float vector 3 rounding to nearest + JPH_INLINE explicit operator Vec3() const; + + /// Prepare to convert to float vector 3 rounding towards zero (returns DVec3 that can be converted to a Vec3 to get the rounding) + JPH_INLINE DVec3 PrepareRoundToZero() const; + + /// Prepare to convert to float vector 3 rounding towards positive/negative inf (returns DVec3 that can be converted to a Vec3 to get the rounding) + JPH_INLINE DVec3 PrepareRoundToInf() const; + + /// Convert to float vector 3 rounding down + JPH_INLINE Vec3 ToVec3RoundDown() const; + + /// Convert to float vector 3 rounding up + JPH_INLINE Vec3 ToVec3RoundUp() const; + + /// Return the minimum value of each of the components + static JPH_INLINE DVec3 sMin(DVec3Arg inV1, DVec3Arg inV2); + + /// Return the maximum of each of the components + static JPH_INLINE DVec3 sMax(DVec3Arg inV1, DVec3Arg inV2); + + /// Clamp a vector between min and max (component wise) + static JPH_INLINE DVec3 sClamp(DVec3Arg inV, DVec3Arg inMin, DVec3Arg inMax); + + /// Equals (component wise) + static JPH_INLINE DVec3 sEquals(DVec3Arg inV1, DVec3Arg inV2); + + /// Less than (component wise) + static JPH_INLINE DVec3 sLess(DVec3Arg inV1, DVec3Arg inV2); + + /// Less than or equal (component wise) + static JPH_INLINE DVec3 sLessOrEqual(DVec3Arg inV1, DVec3Arg inV2); + + /// Greater than (component wise) + static JPH_INLINE DVec3 sGreater(DVec3Arg inV1, DVec3Arg inV2); + + /// Greater than or equal (component wise) + static JPH_INLINE DVec3 sGreaterOrEqual(DVec3Arg inV1, DVec3Arg inV2); + + /// Calculates inMul1 * inMul2 + inAdd + static JPH_INLINE DVec3 sFusedMultiplyAdd(DVec3Arg inMul1, DVec3Arg inMul2, DVec3Arg inAdd); + + /// Component wise select, returns inNotSet when highest bit of inControl = 0 and inSet when highest bit of inControl = 1 + static JPH_INLINE DVec3 sSelect(DVec3Arg inNotSet, DVec3Arg inSet, DVec3Arg inControl); + + /// Logical or (component wise) + static JPH_INLINE DVec3 sOr(DVec3Arg inV1, DVec3Arg inV2); + + /// Logical xor (component wise) + static JPH_INLINE DVec3 sXor(DVec3Arg inV1, DVec3Arg inV2); + + /// Logical and (component wise) + static JPH_INLINE DVec3 sAnd(DVec3Arg inV1, DVec3Arg inV2); + + /// Store if X is true in bit 0, Y in bit 1, Z in bit 2 and W in bit 3 (true is when highest bit of component is set) + JPH_INLINE int GetTrues() const; + + /// Test if any of the components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAnyTrue() const; + + /// Test if all components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAllTrue() const; + + /// Get individual components +#if defined(JPH_USE_AVX) + JPH_INLINE double GetX() const { return _mm_cvtsd_f64(_mm256_castpd256_pd128(mValue)); } + JPH_INLINE double GetY() const { return mF64[1]; } + JPH_INLINE double GetZ() const { return mF64[2]; } +#elif defined(JPH_USE_SSE) + JPH_INLINE double GetX() const { return _mm_cvtsd_f64(mValue.mLow); } + JPH_INLINE double GetY() const { return mF64[1]; } + JPH_INLINE double GetZ() const { return _mm_cvtsd_f64(mValue.mHigh); } +#elif defined(JPH_USE_NEON) + JPH_INLINE double GetX() const { return vgetq_lane_f64(mValue.val[0], 0); } + JPH_INLINE double GetY() const { return vgetq_lane_f64(mValue.val[0], 1); } + JPH_INLINE double GetZ() const { return vgetq_lane_f64(mValue.val[1], 0); } +#else + JPH_INLINE double GetX() const { return mF64[0]; } + JPH_INLINE double GetY() const { return mF64[1]; } + JPH_INLINE double GetZ() const { return mF64[2]; } +#endif + + /// Set individual components + JPH_INLINE void SetX(double inX) { mF64[0] = inX; } + JPH_INLINE void SetY(double inY) { mF64[1] = inY; } + JPH_INLINE void SetZ(double inZ) { mF64[2] = mF64[3] = inZ; } // Assure Z and W are the same + + /// Set all components + JPH_INLINE void Set(double inX, double inY, double inZ) { *this = DVec3(inX, inY, inZ); } + + /// Get double component by index + JPH_INLINE double operator [] (uint inCoordinate) const { JPH_ASSERT(inCoordinate < 3); return mF64[inCoordinate]; } + + /// Set double component by index + JPH_INLINE void SetComponent(uint inCoordinate, double inValue) { JPH_ASSERT(inCoordinate < 3); mF64[inCoordinate] = inValue; mValue = sFixW(mValue); } // Assure Z and W are the same + + /// Comparison + JPH_INLINE bool operator == (DVec3Arg inV2) const; + JPH_INLINE bool operator != (DVec3Arg inV2) const { return !(*this == inV2); } + + /// Test if two vectors are close + JPH_INLINE bool IsClose(DVec3Arg inV2, double inMaxDistSq = 1.0e-24) const; + + /// Test if vector is near zero + JPH_INLINE bool IsNearZero(double inMaxDistSq = 1.0e-24) const; + + /// Test if vector is normalized + JPH_INLINE bool IsNormalized(double inTolerance = 1.0e-12) const; + + /// Test if vector contains NaN elements + JPH_INLINE bool IsNaN() const; + + /// Multiply two double vectors (component wise) + JPH_INLINE DVec3 operator * (DVec3Arg inV2) const; + + /// Multiply vector with double + JPH_INLINE DVec3 operator * (double inV2) const; + + /// Multiply vector with double + friend JPH_INLINE DVec3 operator * (double inV1, DVec3Arg inV2); + + /// Divide vector by double + JPH_INLINE DVec3 operator / (double inV2) const; + + /// Multiply vector with double + JPH_INLINE DVec3 & operator *= (double inV2); + + /// Multiply vector with vector + JPH_INLINE DVec3 & operator *= (DVec3Arg inV2); + + /// Divide vector by double + JPH_INLINE DVec3 & operator /= (double inV2); + + /// Add two vectors (component wise) + JPH_INLINE DVec3 operator + (Vec3Arg inV2) const; + + /// Add two double vectors (component wise) + JPH_INLINE DVec3 operator + (DVec3Arg inV2) const; + + /// Add two vectors (component wise) + JPH_INLINE DVec3 & operator += (Vec3Arg inV2); + + /// Add two double vectors (component wise) + JPH_INLINE DVec3 & operator += (DVec3Arg inV2); + + /// Negate + JPH_INLINE DVec3 operator - () const; + + /// Subtract two vectors (component wise) + JPH_INLINE DVec3 operator - (Vec3Arg inV2) const; + + /// Subtract two double vectors (component wise) + JPH_INLINE DVec3 operator - (DVec3Arg inV2) const; + + /// Subtract two vectors (component wise) + JPH_INLINE DVec3 & operator -= (Vec3Arg inV2); + + /// Subtract two vectors (component wise) + JPH_INLINE DVec3 & operator -= (DVec3Arg inV2); + + /// Divide (component wise) + JPH_INLINE DVec3 operator / (DVec3Arg inV2) const; + + /// Return the absolute value of each of the components + JPH_INLINE DVec3 Abs() const; + + /// Reciprocal vector (1 / value) for each of the components + JPH_INLINE DVec3 Reciprocal() const; + + /// Cross product + JPH_INLINE DVec3 Cross(DVec3Arg inV2) const; + + /// Dot product + JPH_INLINE double Dot(DVec3Arg inV2) const; + + /// Squared length of vector + JPH_INLINE double LengthSq() const; + + /// Length of vector + JPH_INLINE double Length() const; + + /// Normalize vector + JPH_INLINE DVec3 Normalized() const; + + /// Component wise square root + JPH_INLINE DVec3 Sqrt() const; + + /// Get vector that contains the sign of each element (returns 1 if positive, -1 if negative) + JPH_INLINE DVec3 GetSign() const; + + /// To String + friend ostream & operator << (ostream &inStream, DVec3Arg inV) + { + inStream << inV.mF64[0] << ", " << inV.mF64[1] << ", " << inV.mF64[2]; + return inStream; + } + + /// Internal helper function that checks that W is equal to Z, so e.g. dividing by it should not generate div by 0 + JPH_INLINE void CheckW() const; + + /// Internal helper function that ensures that the Z component is replicated to the W component to prevent divisions by zero + static JPH_INLINE Type sFixW(TypeArg inValue); + + /// Representations of true and false for boolean operations + inline static const double cTrue = BitCast(~uint64(0)); + inline static const double cFalse = 0.0; + + union + { + Type mValue; + double mF64[4]; + }; +}; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "DVec3.inl" diff --git a/thirdparty/jolt_physics/Jolt/Math/DVec3.inl b/thirdparty/jolt_physics/Jolt/Math/DVec3.inl new file mode 100644 index 000000000000..eb0bcf4393e8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/DVec3.inl @@ -0,0 +1,936 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +// Create a std::hash/JPH::Hash for DVec3 +JPH_MAKE_HASHABLE(JPH::DVec3, t.GetX(), t.GetY(), t.GetZ()) + +JPH_NAMESPACE_BEGIN + +DVec3::DVec3(Vec3Arg inRHS) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_cvtps_pd(inRHS.mValue); +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_cvtps_pd(inRHS.mValue); + mValue.mHigh = _mm_cvtps_pd(_mm_shuffle_ps(inRHS.mValue, inRHS.mValue, _MM_SHUFFLE(2, 2, 2, 2))); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vcvt_f64_f32(vget_low_f32(inRHS.mValue)); + mValue.val[1] = vcvt_high_f64_f32(inRHS.mValue); +#else + mF64[0] = (double)inRHS.GetX(); + mF64[1] = (double)inRHS.GetY(); + mF64[2] = (double)inRHS.GetZ(); + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif +} + +DVec3::DVec3(Vec4Arg inRHS) : + DVec3(Vec3(inRHS)) +{ +} + +DVec3::DVec3(double inX, double inY, double inZ) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_set_pd(inZ, inZ, inY, inX); // Assure Z and W are the same +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_set_pd(inY, inX); + mValue.mHigh = _mm_set1_pd(inZ); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vcombine_f64(vcreate_f64(BitCast(inX)), vcreate_f64(BitCast(inY))); + mValue.val[1] = vdupq_n_f64(inZ); +#else + mF64[0] = inX; + mF64[1] = inY; + mF64[2] = inZ; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif +} + +DVec3::DVec3(const Double3 &inV) +{ +#if defined(JPH_USE_AVX) + Type x = _mm256_castpd128_pd256(_mm_load_sd(&inV.x)); + Type y = _mm256_castpd128_pd256(_mm_load_sd(&inV.y)); + Type z = _mm256_broadcast_sd(&inV.z); + Type xy = _mm256_unpacklo_pd(x, y); + mValue = _mm256_blend_pd(xy, z, 0b1100); // Assure Z and W are the same +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_loadu_pd(&inV.x); + mValue.mHigh = _mm_set1_pd(inV.z); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vld1q_f64(&inV.x); + mValue.val[1] = vdupq_n_f64(inV.z); +#else + mF64[0] = inV.x; + mF64[1] = inV.y; + mF64[2] = inV.z; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif +} + +void DVec3::CheckW() const +{ +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + // Avoid asserts when both components are NaN + JPH_ASSERT(reinterpret_cast(mF64)[2] == reinterpret_cast(mF64)[3]); +#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED +} + +/// Internal helper function that ensures that the Z component is replicated to the W component to prevent divisions by zero +DVec3::Type DVec3::sFixW(TypeArg inValue) +{ +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + #if defined(JPH_USE_AVX) + return _mm256_shuffle_pd(inValue, inValue, 2); + #elif defined(JPH_USE_SSE) + Type value; + value.mLow = inValue.mLow; + value.mHigh = _mm_shuffle_pd(inValue.mHigh, inValue.mHigh, 0); + return value; + #elif defined(JPH_USE_NEON) + Type value; + value.val[0] = inValue.val[0]; + value.val[1] = vdupq_laneq_f64(inValue.val[1], 0); + return value; + #else + Type value; + value.mData[0] = inValue.mData[0]; + value.mData[1] = inValue.mData[1]; + value.mData[2] = inValue.mData[2]; + value.mData[3] = inValue.mData[2]; + return value; + #endif +#else + return inValue; +#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED +} + +DVec3 DVec3::sZero() +{ +#if defined(JPH_USE_AVX) + return _mm256_setzero_pd(); +#elif defined(JPH_USE_SSE) + __m128d zero = _mm_setzero_pd(); + return DVec3({ zero, zero }); +#elif defined(JPH_USE_NEON) + float64x2_t zero = vdupq_n_f64(0.0); + return DVec3({ zero, zero }); +#else + return DVec3(0, 0, 0); +#endif +} + +DVec3 DVec3::sReplicate(double inV) +{ +#if defined(JPH_USE_AVX) + return _mm256_set1_pd(inV); +#elif defined(JPH_USE_SSE) + __m128d value = _mm_set1_pd(inV); + return DVec3({ value, value }); +#elif defined(JPH_USE_NEON) + float64x2_t value = vdupq_n_f64(inV); + return DVec3({ value, value }); +#else + return DVec3(inV, inV, inV); +#endif +} + +DVec3 DVec3::sNaN() +{ + return sReplicate(numeric_limits::quiet_NaN()); +} + +DVec3 DVec3::sLoadDouble3Unsafe(const Double3 &inV) +{ +#if defined(JPH_USE_AVX) + Type v = _mm256_loadu_pd(&inV.x); +#elif defined(JPH_USE_SSE) + Type v; + v.mLow = _mm_loadu_pd(&inV.x); + v.mHigh = _mm_set1_pd(inV.z); +#elif defined(JPH_USE_NEON) + Type v = vld1q_f64_x2(&inV.x); +#else + Type v = { inV.x, inV.y, inV.z }; +#endif + return sFixW(v); +} + +void DVec3::StoreDouble3(Double3 *outV) const +{ + outV->x = mF64[0]; + outV->y = mF64[1]; + outV->z = mF64[2]; +} + +DVec3::operator Vec3() const +{ +#if defined(JPH_USE_AVX) + return _mm256_cvtpd_ps(mValue); +#elif defined(JPH_USE_SSE) + __m128 low = _mm_cvtpd_ps(mValue.mLow); + __m128 high = _mm_cvtpd_ps(mValue.mHigh); + return _mm_shuffle_ps(low, high, _MM_SHUFFLE(1, 0, 1, 0)); +#elif defined(JPH_USE_NEON) + return vcvt_high_f32_f64(vcvtx_f32_f64(mValue.val[0]), mValue.val[1]); +#else + return Vec3((float)GetX(), (float)GetY(), (float)GetZ()); +#endif +} + +DVec3 DVec3::sMin(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_min_pd(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_min_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_min_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vminq_f64(inV1.mValue.val[0], inV2.mValue.val[0]), vminq_f64(inV1.mValue.val[1], inV2.mValue.val[1]) }); +#else + return DVec3(min(inV1.mF64[0], inV2.mF64[0]), + min(inV1.mF64[1], inV2.mF64[1]), + min(inV1.mF64[2], inV2.mF64[2])); +#endif +} + +DVec3 DVec3::sMax(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_max_pd(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_max_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_max_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vmaxq_f64(inV1.mValue.val[0], inV2.mValue.val[0]), vmaxq_f64(inV1.mValue.val[1], inV2.mValue.val[1]) }); +#else + return DVec3(max(inV1.mF64[0], inV2.mF64[0]), + max(inV1.mF64[1], inV2.mF64[1]), + max(inV1.mF64[2], inV2.mF64[2])); +#endif +} + +DVec3 DVec3::sClamp(DVec3Arg inV, DVec3Arg inMin, DVec3Arg inMax) +{ + return sMax(sMin(inV, inMax), inMin); +} + +DVec3 DVec3::sEquals(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_EQ_OQ); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_cmpeq_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmpeq_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vreinterpretq_f64_u64(vceqq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_f64_u64(vceqq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) }); +#else + return DVec3(inV1.mF64[0] == inV2.mF64[0]? cTrue : cFalse, + inV1.mF64[1] == inV2.mF64[1]? cTrue : cFalse, + inV1.mF64[2] == inV2.mF64[2]? cTrue : cFalse); +#endif +} + +DVec3 DVec3::sLess(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_LT_OQ); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_cmplt_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmplt_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vreinterpretq_f64_u64(vcltq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_f64_u64(vcltq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) }); +#else + return DVec3(inV1.mF64[0] < inV2.mF64[0]? cTrue : cFalse, + inV1.mF64[1] < inV2.mF64[1]? cTrue : cFalse, + inV1.mF64[2] < inV2.mF64[2]? cTrue : cFalse); +#endif +} + +DVec3 DVec3::sLessOrEqual(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_LE_OQ); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_cmple_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmple_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vreinterpretq_f64_u64(vcleq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_f64_u64(vcleq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) }); +#else + return DVec3(inV1.mF64[0] <= inV2.mF64[0]? cTrue : cFalse, + inV1.mF64[1] <= inV2.mF64[1]? cTrue : cFalse, + inV1.mF64[2] <= inV2.mF64[2]? cTrue : cFalse); +#endif +} + +DVec3 DVec3::sGreater(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_GT_OQ); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_cmpgt_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmpgt_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vreinterpretq_f64_u64(vcgtq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_f64_u64(vcgtq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) }); +#else + return DVec3(inV1.mF64[0] > inV2.mF64[0]? cTrue : cFalse, + inV1.mF64[1] > inV2.mF64[1]? cTrue : cFalse, + inV1.mF64[2] > inV2.mF64[2]? cTrue : cFalse); +#endif +} + +DVec3 DVec3::sGreaterOrEqual(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_GE_OQ); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_cmpge_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmpge_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vreinterpretq_f64_u64(vcgeq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_f64_u64(vcgeq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) }); +#else + return DVec3(inV1.mF64[0] >= inV2.mF64[0]? cTrue : cFalse, + inV1.mF64[1] >= inV2.mF64[1]? cTrue : cFalse, + inV1.mF64[2] >= inV2.mF64[2]? cTrue : cFalse); +#endif +} + +DVec3 DVec3::sFusedMultiplyAdd(DVec3Arg inMul1, DVec3Arg inMul2, DVec3Arg inAdd) +{ +#if defined(JPH_USE_AVX) + #ifdef JPH_USE_FMADD + return _mm256_fmadd_pd(inMul1.mValue, inMul2.mValue, inAdd.mValue); + #else + return _mm256_add_pd(_mm256_mul_pd(inMul1.mValue, inMul2.mValue), inAdd.mValue); + #endif +#elif defined(JPH_USE_NEON) + return DVec3({ vmlaq_f64(inAdd.mValue.val[0], inMul1.mValue.val[0], inMul2.mValue.val[0]), vmlaq_f64(inAdd.mValue.val[1], inMul1.mValue.val[1], inMul2.mValue.val[1]) }); +#else + return inMul1 * inMul2 + inAdd; +#endif +} + +DVec3 DVec3::sSelect(DVec3Arg inNotSet, DVec3Arg inSet, DVec3Arg inControl) +{ +#if defined(JPH_USE_AVX) + return _mm256_blendv_pd(inNotSet.mValue, inSet.mValue, inControl.mValue); +#elif defined(JPH_USE_SSE4_1) + Type v = { _mm_blendv_pd(inNotSet.mValue.mLow, inSet.mValue.mLow, inControl.mValue.mLow), _mm_blendv_pd(inNotSet.mValue.mHigh, inSet.mValue.mHigh, inControl.mValue.mHigh) }; + return sFixW(v); +#elif defined(JPH_USE_NEON) + Type v = { vbslq_f64(vreinterpretq_u64_s64(vshrq_n_s64(vreinterpretq_s64_f64(inControl.mValue.val[0]), 63)), inSet.mValue.val[0], inNotSet.mValue.val[0]), + vbslq_f64(vreinterpretq_u64_s64(vshrq_n_s64(vreinterpretq_s64_f64(inControl.mValue.val[1]), 63)), inSet.mValue.val[1], inNotSet.mValue.val[1]) }; + return sFixW(v); +#else + DVec3 result; + for (int i = 0; i < 3; i++) + result.mF64[i] = (BitCast(inControl.mF64[i]) & (uint64(1) << 63))? inSet.mF64[i] : inNotSet.mF64[i]; +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + result.mF64[3] = result.mF64[2]; +#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + return result; +#endif +} + +DVec3 DVec3::sOr(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_or_pd(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_or_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_or_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(inV1.mValue.val[0]), vreinterpretq_u64_f64(inV2.mValue.val[0]))), + vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(inV1.mValue.val[1]), vreinterpretq_u64_f64(inV2.mValue.val[1]))) }); +#else + return DVec3(BitCast(BitCast(inV1.mF64[0]) | BitCast(inV2.mF64[0])), + BitCast(BitCast(inV1.mF64[1]) | BitCast(inV2.mF64[1])), + BitCast(BitCast(inV1.mF64[2]) | BitCast(inV2.mF64[2]))); +#endif +} + +DVec3 DVec3::sXor(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_xor_pd(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_xor_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_xor_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vreinterpretq_f64_u64(veorq_u64(vreinterpretq_u64_f64(inV1.mValue.val[0]), vreinterpretq_u64_f64(inV2.mValue.val[0]))), + vreinterpretq_f64_u64(veorq_u64(vreinterpretq_u64_f64(inV1.mValue.val[1]), vreinterpretq_u64_f64(inV2.mValue.val[1]))) }); +#else + return DVec3(BitCast(BitCast(inV1.mF64[0]) ^ BitCast(inV2.mF64[0])), + BitCast(BitCast(inV1.mF64[1]) ^ BitCast(inV2.mF64[1])), + BitCast(BitCast(inV1.mF64[2]) ^ BitCast(inV2.mF64[2]))); +#endif +} + +DVec3 DVec3::sAnd(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_and_pd(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_and_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_and_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(inV1.mValue.val[0]), vreinterpretq_u64_f64(inV2.mValue.val[0]))), + vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(inV1.mValue.val[1]), vreinterpretq_u64_f64(inV2.mValue.val[1]))) }); +#else + return DVec3(BitCast(BitCast(inV1.mF64[0]) & BitCast(inV2.mF64[0])), + BitCast(BitCast(inV1.mF64[1]) & BitCast(inV2.mF64[1])), + BitCast(BitCast(inV1.mF64[2]) & BitCast(inV2.mF64[2]))); +#endif +} + +int DVec3::GetTrues() const +{ +#if defined(JPH_USE_AVX) + return _mm256_movemask_pd(mValue) & 0x7; +#elif defined(JPH_USE_SSE) + return (_mm_movemask_pd(mValue.mLow) + (_mm_movemask_pd(mValue.mHigh) << 2)) & 0x7; +#else + return int((BitCast(mF64[0]) >> 63) | ((BitCast(mF64[1]) >> 63) << 1) | ((BitCast(mF64[2]) >> 63) << 2)); +#endif +} + +bool DVec3::TestAnyTrue() const +{ + return GetTrues() != 0; +} + +bool DVec3::TestAllTrue() const +{ + return GetTrues() == 0x7; +} + +bool DVec3::operator == (DVec3Arg inV2) const +{ + return sEquals(*this, inV2).TestAllTrue(); +} + +bool DVec3::IsClose(DVec3Arg inV2, double inMaxDistSq) const +{ + return (inV2 - *this).LengthSq() <= inMaxDistSq; +} + +bool DVec3::IsNearZero(double inMaxDistSq) const +{ + return LengthSq() <= inMaxDistSq; +} + +DVec3 DVec3::operator * (DVec3Arg inV2) const +{ +#if defined(JPH_USE_AVX) + return _mm256_mul_pd(mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_mul_pd(mValue.mLow, inV2.mValue.mLow), _mm_mul_pd(mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vmulq_f64(mValue.val[0], inV2.mValue.val[0]), vmulq_f64(mValue.val[1], inV2.mValue.val[1]) }); +#else + return DVec3(mF64[0] * inV2.mF64[0], mF64[1] * inV2.mF64[1], mF64[2] * inV2.mF64[2]); +#endif +} + +DVec3 DVec3::operator * (double inV2) const +{ +#if defined(JPH_USE_AVX) + return _mm256_mul_pd(mValue, _mm256_set1_pd(inV2)); +#elif defined(JPH_USE_SSE) + __m128d v = _mm_set1_pd(inV2); + return DVec3({ _mm_mul_pd(mValue.mLow, v), _mm_mul_pd(mValue.mHigh, v) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vmulq_n_f64(mValue.val[0], inV2), vmulq_n_f64(mValue.val[1], inV2) }); +#else + return DVec3(mF64[0] * inV2, mF64[1] * inV2, mF64[2] * inV2); +#endif +} + +DVec3 operator * (double inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_mul_pd(_mm256_set1_pd(inV1), inV2.mValue); +#elif defined(JPH_USE_SSE) + __m128d v = _mm_set1_pd(inV1); + return DVec3({ _mm_mul_pd(v, inV2.mValue.mLow), _mm_mul_pd(v, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vmulq_n_f64(inV2.mValue.val[0], inV1), vmulq_n_f64(inV2.mValue.val[1], inV1) }); +#else + return DVec3(inV1 * inV2.mF64[0], inV1 * inV2.mF64[1], inV1 * inV2.mF64[2]); +#endif +} + +DVec3 DVec3::operator / (double inV2) const +{ +#if defined(JPH_USE_AVX) + return _mm256_div_pd(mValue, _mm256_set1_pd(inV2)); +#elif defined(JPH_USE_SSE) + __m128d v = _mm_set1_pd(inV2); + return DVec3({ _mm_div_pd(mValue.mLow, v), _mm_div_pd(mValue.mHigh, v) }); +#elif defined(JPH_USE_NEON) + float64x2_t v = vdupq_n_f64(inV2); + return DVec3({ vdivq_f64(mValue.val[0], v), vdivq_f64(mValue.val[1], v) }); +#else + return DVec3(mF64[0] / inV2, mF64[1] / inV2, mF64[2] / inV2); +#endif +} + +DVec3 &DVec3::operator *= (double inV2) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_mul_pd(mValue, _mm256_set1_pd(inV2)); +#elif defined(JPH_USE_SSE) + __m128d v = _mm_set1_pd(inV2); + mValue.mLow = _mm_mul_pd(mValue.mLow, v); + mValue.mHigh = _mm_mul_pd(mValue.mHigh, v); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vmulq_n_f64(mValue.val[0], inV2); + mValue.val[1] = vmulq_n_f64(mValue.val[1], inV2); +#else + for (int i = 0; i < 3; ++i) + mF64[i] *= inV2; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif + return *this; +} + +DVec3 &DVec3::operator *= (DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_mul_pd(mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_mul_pd(mValue.mLow, inV2.mValue.mLow); + mValue.mHigh = _mm_mul_pd(mValue.mHigh, inV2.mValue.mHigh); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vmulq_f64(mValue.val[0], inV2.mValue.val[0]); + mValue.val[1] = vmulq_f64(mValue.val[1], inV2.mValue.val[1]); +#else + for (int i = 0; i < 3; ++i) + mF64[i] *= inV2.mF64[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif + return *this; +} + +DVec3 &DVec3::operator /= (double inV2) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_div_pd(mValue, _mm256_set1_pd(inV2)); +#elif defined(JPH_USE_SSE) + __m128d v = _mm_set1_pd(inV2); + mValue.mLow = _mm_div_pd(mValue.mLow, v); + mValue.mHigh = _mm_div_pd(mValue.mHigh, v); +#elif defined(JPH_USE_NEON) + float64x2_t v = vdupq_n_f64(inV2); + mValue.val[0] = vdivq_f64(mValue.val[0], v); + mValue.val[1] = vdivq_f64(mValue.val[1], v); +#else + for (int i = 0; i < 3; ++i) + mF64[i] /= inV2; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif + return *this; +} + +DVec3 DVec3::operator + (Vec3Arg inV2) const +{ +#if defined(JPH_USE_AVX) + return _mm256_add_pd(mValue, _mm256_cvtps_pd(inV2.mValue)); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_add_pd(mValue.mLow, _mm_cvtps_pd(inV2.mValue)), _mm_add_pd(mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(2, 2, 2, 2)))) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vaddq_f64(mValue.val[0], vcvt_f64_f32(vget_low_f32(inV2.mValue))), vaddq_f64(mValue.val[1], vcvt_high_f64_f32(inV2.mValue)) }); +#else + return DVec3(mF64[0] + inV2.mF32[0], mF64[1] + inV2.mF32[1], mF64[2] + inV2.mF32[2]); +#endif +} + +DVec3 DVec3::operator + (DVec3Arg inV2) const +{ +#if defined(JPH_USE_AVX) + return _mm256_add_pd(mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_add_pd(mValue.mLow, inV2.mValue.mLow), _mm_add_pd(mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vaddq_f64(mValue.val[0], inV2.mValue.val[0]), vaddq_f64(mValue.val[1], inV2.mValue.val[1]) }); +#else + return DVec3(mF64[0] + inV2.mF64[0], mF64[1] + inV2.mF64[1], mF64[2] + inV2.mF64[2]); +#endif +} + +DVec3 &DVec3::operator += (Vec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_add_pd(mValue, _mm256_cvtps_pd(inV2.mValue)); +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_add_pd(mValue.mLow, _mm_cvtps_pd(inV2.mValue)); + mValue.mHigh = _mm_add_pd(mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(2, 2, 2, 2)))); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vaddq_f64(mValue.val[0], vcvt_f64_f32(vget_low_f32(inV2.mValue))); + mValue.val[1] = vaddq_f64(mValue.val[1], vcvt_high_f64_f32(inV2.mValue)); +#else + for (int i = 0; i < 3; ++i) + mF64[i] += inV2.mF32[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif + return *this; +} + +DVec3 &DVec3::operator += (DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_add_pd(mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_add_pd(mValue.mLow, inV2.mValue.mLow); + mValue.mHigh = _mm_add_pd(mValue.mHigh, inV2.mValue.mHigh); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vaddq_f64(mValue.val[0], inV2.mValue.val[0]); + mValue.val[1] = vaddq_f64(mValue.val[1], inV2.mValue.val[1]); +#else + for (int i = 0; i < 3; ++i) + mF64[i] += inV2.mF64[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif + return *this; +} + +DVec3 DVec3::operator - () const +{ +#if defined(JPH_USE_AVX) + return _mm256_sub_pd(_mm256_setzero_pd(), mValue); +#elif defined(JPH_USE_SSE) + __m128d zero = _mm_setzero_pd(); + return DVec3({ _mm_sub_pd(zero, mValue.mLow), _mm_sub_pd(zero, mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + #ifdef JPH_CROSS_PLATFORM_DETERMINISTIC + float64x2_t zero = vdupq_n_f64(0); + return DVec3({ vsubq_f64(zero, mValue.val[0]), vsubq_f64(zero, mValue.val[1]) }); + #else + return DVec3({ vnegq_f64(mValue.val[0]), vnegq_f64(mValue.val[1]) }); + #endif +#else + #ifdef JPH_CROSS_PLATFORM_DETERMINISTIC + return DVec3(0.0 - mF64[0], 0.0 - mF64[1], 0.0 - mF64[2]); + #else + return DVec3(-mF64[0], -mF64[1], -mF64[2]); + #endif +#endif +} + +DVec3 DVec3::operator - (Vec3Arg inV2) const +{ +#if defined(JPH_USE_AVX) + return _mm256_sub_pd(mValue, _mm256_cvtps_pd(inV2.mValue)); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_sub_pd(mValue.mLow, _mm_cvtps_pd(inV2.mValue)), _mm_sub_pd(mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(2, 2, 2, 2)))) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vsubq_f64(mValue.val[0], vcvt_f64_f32(vget_low_f32(inV2.mValue))), vsubq_f64(mValue.val[1], vcvt_high_f64_f32(inV2.mValue)) }); +#else + return DVec3(mF64[0] - inV2.mF32[0], mF64[1] - inV2.mF32[1], mF64[2] - inV2.mF32[2]); +#endif +} + +DVec3 DVec3::operator - (DVec3Arg inV2) const +{ +#if defined(JPH_USE_AVX) + return _mm256_sub_pd(mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_sub_pd(mValue.mLow, inV2.mValue.mLow), _mm_sub_pd(mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vsubq_f64(mValue.val[0], inV2.mValue.val[0]), vsubq_f64(mValue.val[1], inV2.mValue.val[1]) }); +#else + return DVec3(mF64[0] - inV2.mF64[0], mF64[1] - inV2.mF64[1], mF64[2] - inV2.mF64[2]); +#endif +} + +DVec3 &DVec3::operator -= (Vec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_sub_pd(mValue, _mm256_cvtps_pd(inV2.mValue)); +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_sub_pd(mValue.mLow, _mm_cvtps_pd(inV2.mValue)); + mValue.mHigh = _mm_sub_pd(mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(2, 2, 2, 2)))); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vsubq_f64(mValue.val[0], vcvt_f64_f32(vget_low_f32(inV2.mValue))); + mValue.val[1] = vsubq_f64(mValue.val[1], vcvt_high_f64_f32(inV2.mValue)); +#else + for (int i = 0; i < 3; ++i) + mF64[i] -= inV2.mF32[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif + return *this; +} + +DVec3 &DVec3::operator -= (DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_sub_pd(mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_sub_pd(mValue.mLow, inV2.mValue.mLow); + mValue.mHigh = _mm_sub_pd(mValue.mHigh, inV2.mValue.mHigh); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vsubq_f64(mValue.val[0], inV2.mValue.val[0]); + mValue.val[1] = vsubq_f64(mValue.val[1], inV2.mValue.val[1]); +#else + for (int i = 0; i < 3; ++i) + mF64[i] -= inV2.mF64[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif + return *this; +} + +DVec3 DVec3::operator / (DVec3Arg inV2) const +{ + inV2.CheckW(); +#if defined(JPH_USE_AVX) + return _mm256_div_pd(mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_div_pd(mValue.mLow, inV2.mValue.mLow), _mm_div_pd(mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vdivq_f64(mValue.val[0], inV2.mValue.val[0]), vdivq_f64(mValue.val[1], inV2.mValue.val[1]) }); +#else + return DVec3(mF64[0] / inV2.mF64[0], mF64[1] / inV2.mF64[1], mF64[2] / inV2.mF64[2]); +#endif +} + +DVec3 DVec3::Abs() const +{ +#if defined(JPH_USE_AVX512) + return _mm256_range_pd(mValue, mValue, 0b1000); +#elif defined(JPH_USE_AVX) + return _mm256_max_pd(_mm256_sub_pd(_mm256_setzero_pd(), mValue), mValue); +#elif defined(JPH_USE_SSE) + __m128d zero = _mm_setzero_pd(); + return DVec3({ _mm_max_pd(_mm_sub_pd(zero, mValue.mLow), mValue.mLow), _mm_max_pd(_mm_sub_pd(zero, mValue.mHigh), mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vabsq_f64(mValue.val[0]), vabsq_f64(mValue.val[1]) }); +#else + return DVec3(abs(mF64[0]), abs(mF64[1]), abs(mF64[2])); +#endif +} + +DVec3 DVec3::Reciprocal() const +{ + return sReplicate(1.0) / mValue; +} + +DVec3 DVec3::Cross(DVec3Arg inV2) const +{ +#if defined(JPH_USE_AVX2) + __m256d t1 = _mm256_permute4x64_pd(inV2.mValue, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same + t1 = _mm256_mul_pd(t1, mValue); + __m256d t2 = _mm256_permute4x64_pd(mValue, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same + t2 = _mm256_mul_pd(t2, inV2.mValue); + __m256d t3 = _mm256_sub_pd(t1, t2); + return _mm256_permute4x64_pd(t3, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same +#else + return DVec3(mF64[1] * inV2.mF64[2] - mF64[2] * inV2.mF64[1], + mF64[2] * inV2.mF64[0] - mF64[0] * inV2.mF64[2], + mF64[0] * inV2.mF64[1] - mF64[1] * inV2.mF64[0]); +#endif +} + +double DVec3::Dot(DVec3Arg inV2) const +{ +#if defined(JPH_USE_AVX) + __m256d mul = _mm256_mul_pd(mValue, inV2.mValue); + __m128d xy = _mm256_castpd256_pd128(mul); + __m128d yx = _mm_shuffle_pd(xy, xy, 1); + __m128d sum = _mm_add_pd(xy, yx); + __m128d zw = _mm256_extractf128_pd(mul, 1); + sum = _mm_add_pd(sum, zw); + return _mm_cvtsd_f64(sum); +#elif defined(JPH_USE_SSE) + __m128d xy = _mm_mul_pd(mValue.mLow, inV2.mValue.mLow); + __m128d yx = _mm_shuffle_pd(xy, xy, 1); + __m128d sum = _mm_add_pd(xy, yx); + __m128d z = _mm_mul_sd(mValue.mHigh, inV2.mValue.mHigh); + sum = _mm_add_pd(sum, z); + return _mm_cvtsd_f64(sum); +#elif defined(JPH_USE_NEON) + float64x2_t mul_low = vmulq_f64(mValue.val[0], inV2.mValue.val[0]); + float64x2_t mul_high = vmulq_f64(mValue.val[1], inV2.mValue.val[1]); + return vaddvq_f64(mul_low) + vgetq_lane_f64(mul_high, 0); +#else + double dot = 0.0; + for (int i = 0; i < 3; i++) + dot += mF64[i] * inV2.mF64[i]; + return dot; +#endif +} + +double DVec3::LengthSq() const +{ + return Dot(*this); +} + +DVec3 DVec3::Sqrt() const +{ +#if defined(JPH_USE_AVX) + return _mm256_sqrt_pd(mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_sqrt_pd(mValue.mLow), _mm_sqrt_pd(mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vsqrtq_f64(mValue.val[0]), vsqrtq_f64(mValue.val[1]) }); +#else + return DVec3(sqrt(mF64[0]), sqrt(mF64[1]), sqrt(mF64[2])); +#endif +} + +double DVec3::Length() const +{ + return sqrt(Dot(*this)); +} + +DVec3 DVec3::Normalized() const +{ + return *this / Length(); +} + +bool DVec3::IsNormalized(double inTolerance) const +{ + return abs(LengthSq() - 1.0) <= inTolerance; +} + +bool DVec3::IsNaN() const +{ +#if defined(JPH_USE_AVX512) + return (_mm256_fpclass_pd_mask(mValue, 0b10000001) & 0x7) != 0; +#elif defined(JPH_USE_AVX) + return (_mm256_movemask_pd(_mm256_cmp_pd(mValue, mValue, _CMP_UNORD_Q)) & 0x7) != 0; +#elif defined(JPH_USE_SSE) + return ((_mm_movemask_pd(_mm_cmpunord_pd(mValue.mLow, mValue.mLow)) + (_mm_movemask_pd(_mm_cmpunord_pd(mValue.mHigh, mValue.mHigh)) << 2)) & 0x7) != 0; +#else + return isnan(mF64[0]) || isnan(mF64[1]) || isnan(mF64[2]); +#endif +} + +DVec3 DVec3::GetSign() const +{ +#if defined(JPH_USE_AVX512) + return _mm256_fixupimm_pd(mValue, mValue, _mm256_set1_epi32(0xA9A90A00), 0); +#elif defined(JPH_USE_AVX) + __m256d minus_one = _mm256_set1_pd(-1.0); + __m256d one = _mm256_set1_pd(1.0); + return _mm256_or_pd(_mm256_and_pd(mValue, minus_one), one); +#elif defined(JPH_USE_SSE) + __m128d minus_one = _mm_set1_pd(-1.0); + __m128d one = _mm_set1_pd(1.0); + return DVec3({ _mm_or_pd(_mm_and_pd(mValue.mLow, minus_one), one), _mm_or_pd(_mm_and_pd(mValue.mHigh, minus_one), one) }); +#elif defined(JPH_USE_NEON) + uint64x2_t minus_one = vreinterpretq_u64_f64(vdupq_n_f64(-1.0f)); + uint64x2_t one = vreinterpretq_u64_f64(vdupq_n_f64(1.0f)); + return DVec3({ vreinterpretq_f64_u64(vorrq_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[0]), minus_one), one)), + vreinterpretq_f64_u64(vorrq_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[1]), minus_one), one)) }); +#else + return DVec3(std::signbit(mF64[0])? -1.0 : 1.0, + std::signbit(mF64[1])? -1.0 : 1.0, + std::signbit(mF64[2])? -1.0 : 1.0); +#endif +} + +DVec3 DVec3::PrepareRoundToZero() const +{ + // Float has 23 bit mantissa, double 52 bit mantissa => we lose 29 bits when converting from double to float + constexpr uint64 cDoubleToFloatMantissaLoss = (1U << 29) - 1; + +#if defined(JPH_USE_AVX) + return _mm256_and_pd(mValue, _mm256_castsi256_pd(_mm256_set1_epi64x(int64_t(~cDoubleToFloatMantissaLoss)))); +#elif defined(JPH_USE_SSE) + __m128d mask = _mm_castsi128_pd(_mm_set1_epi64x(int64_t(~cDoubleToFloatMantissaLoss))); + return DVec3({ _mm_and_pd(mValue.mLow, mask), _mm_and_pd(mValue.mHigh, mask) }); +#elif defined(JPH_USE_NEON) + uint64x2_t mask = vdupq_n_u64(~cDoubleToFloatMantissaLoss); + return DVec3({ vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[0]), mask)), + vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[1]), mask)) }); +#else + double x = BitCast(BitCast(mF64[0]) & ~cDoubleToFloatMantissaLoss); + double y = BitCast(BitCast(mF64[1]) & ~cDoubleToFloatMantissaLoss); + double z = BitCast(BitCast(mF64[2]) & ~cDoubleToFloatMantissaLoss); + + return DVec3(x, y, z); +#endif +} + +DVec3 DVec3::PrepareRoundToInf() const +{ + // Float has 23 bit mantissa, double 52 bit mantissa => we lose 29 bits when converting from double to float + constexpr uint64 cDoubleToFloatMantissaLoss = (1U << 29) - 1; + +#if defined(JPH_USE_AVX512) + __m256i mantissa_loss = _mm256_set1_epi64x(cDoubleToFloatMantissaLoss); + __mmask8 is_zero = _mm256_testn_epi64_mask(_mm256_castpd_si256(mValue), mantissa_loss); + __m256d value_or_mantissa_loss = _mm256_or_pd(mValue, _mm256_castsi256_pd(mantissa_loss)); + return _mm256_mask_blend_pd(is_zero, value_or_mantissa_loss, mValue); +#elif defined(JPH_USE_AVX) + __m256i mantissa_loss = _mm256_set1_epi64x(cDoubleToFloatMantissaLoss); + __m256d value_and_mantissa_loss = _mm256_and_pd(mValue, _mm256_castsi256_pd(mantissa_loss)); + __m256d is_zero = _mm256_cmp_pd(value_and_mantissa_loss, _mm256_setzero_pd(), _CMP_EQ_OQ); + __m256d value_or_mantissa_loss = _mm256_or_pd(mValue, _mm256_castsi256_pd(mantissa_loss)); + return _mm256_blendv_pd(value_or_mantissa_loss, mValue, is_zero); +#elif defined(JPH_USE_SSE4_1) + __m128i mantissa_loss = _mm_set1_epi64x(cDoubleToFloatMantissaLoss); + __m128d zero = _mm_setzero_pd(); + __m128d value_and_mantissa_loss_low = _mm_and_pd(mValue.mLow, _mm_castsi128_pd(mantissa_loss)); + __m128d is_zero_low = _mm_cmpeq_pd(value_and_mantissa_loss_low, zero); + __m128d value_or_mantissa_loss_low = _mm_or_pd(mValue.mLow, _mm_castsi128_pd(mantissa_loss)); + __m128d value_and_mantissa_loss_high = _mm_and_pd(mValue.mHigh, _mm_castsi128_pd(mantissa_loss)); + __m128d is_zero_high = _mm_cmpeq_pd(value_and_mantissa_loss_high, zero); + __m128d value_or_mantissa_loss_high = _mm_or_pd(mValue.mHigh, _mm_castsi128_pd(mantissa_loss)); + return DVec3({ _mm_blendv_pd(value_or_mantissa_loss_low, mValue.mLow, is_zero_low), _mm_blendv_pd(value_or_mantissa_loss_high, mValue.mHigh, is_zero_high) }); +#elif defined(JPH_USE_NEON) + uint64x2_t mantissa_loss = vdupq_n_u64(cDoubleToFloatMantissaLoss); + float64x2_t zero = vdupq_n_f64(0.0); + float64x2_t value_and_mantissa_loss_low = vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[0]), mantissa_loss)); + uint64x2_t is_zero_low = vceqq_f64(value_and_mantissa_loss_low, zero); + float64x2_t value_or_mantissa_loss_low = vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(mValue.val[0]), mantissa_loss)); + float64x2_t value_and_mantissa_loss_high = vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[1]), mantissa_loss)); + float64x2_t value_low = vbslq_f64(is_zero_low, mValue.val[0], value_or_mantissa_loss_low); + uint64x2_t is_zero_high = vceqq_f64(value_and_mantissa_loss_high, zero); + float64x2_t value_or_mantissa_loss_high = vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(mValue.val[1]), mantissa_loss)); + float64x2_t value_high = vbslq_f64(is_zero_high, mValue.val[1], value_or_mantissa_loss_high); + return DVec3({ value_low, value_high }); +#else + uint64 ux = BitCast(mF64[0]); + uint64 uy = BitCast(mF64[1]); + uint64 uz = BitCast(mF64[2]); + + double x = BitCast((ux & cDoubleToFloatMantissaLoss) == 0? ux : (ux | cDoubleToFloatMantissaLoss)); + double y = BitCast((uy & cDoubleToFloatMantissaLoss) == 0? uy : (uy | cDoubleToFloatMantissaLoss)); + double z = BitCast((uz & cDoubleToFloatMantissaLoss) == 0? uz : (uz | cDoubleToFloatMantissaLoss)); + + return DVec3(x, y, z); +#endif +} + +Vec3 DVec3::ToVec3RoundDown() const +{ + DVec3 to_zero = PrepareRoundToZero(); + DVec3 to_inf = PrepareRoundToInf(); + return Vec3(DVec3::sSelect(to_zero, to_inf, DVec3::sLess(*this, DVec3::sZero()))); +} + +Vec3 DVec3::ToVec3RoundUp() const +{ + DVec3 to_zero = PrepareRoundToZero(); + DVec3 to_inf = PrepareRoundToInf(); + return Vec3(DVec3::sSelect(to_inf, to_zero, DVec3::sLess(*this, DVec3::sZero()))); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Double3.h b/thirdparty/jolt_physics/Jolt/Math/Double3.h new file mode 100644 index 000000000000..9b738ff8f8f8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Double3.h @@ -0,0 +1,48 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that holds 3 doubles. Used as a storage class. Convert to DVec3 for calculations. +class [[nodiscard]] Double3 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + Double3() = default; ///< Intentionally not initialized for performance reasons + Double3(const Double3 &inRHS) = default; + Double3 & operator = (const Double3 &inRHS) = default; + Double3(double inX, double inY, double inZ) : x(inX), y(inY), z(inZ) { } + + double operator [] (int inCoordinate) const + { + JPH_ASSERT(inCoordinate < 3); + return *(&x + inCoordinate); + } + + bool operator == (const Double3 &inRHS) const + { + return x == inRHS.x && y == inRHS.y && z == inRHS.z; + } + + bool operator != (const Double3 &inRHS) const + { + return x != inRHS.x || y != inRHS.y || z != inRHS.z; + } + + double x; + double y; + double z; +}; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +// Create a std::hash/JPH::Hash for Double3 +JPH_MAKE_HASHABLE(JPH::Double3, t.x, t.y, t.z) diff --git a/thirdparty/jolt_physics/Jolt/Math/DynMatrix.h b/thirdparty/jolt_physics/Jolt/Math/DynMatrix.h new file mode 100644 index 000000000000..76db294c782c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/DynMatrix.h @@ -0,0 +1,31 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Dynamic resizable matrix class +class [[nodiscard]] DynMatrix +{ +public: + /// Constructor + DynMatrix(const DynMatrix &) = default; + DynMatrix(uint inRows, uint inCols) : mRows(inRows), mCols(inCols) { mElements.resize(inRows * inCols); } + + /// Access an element + float operator () (uint inRow, uint inCol) const { JPH_ASSERT(inRow < mRows && inCol < mCols); return mElements[inRow * mCols + inCol]; } + float & operator () (uint inRow, uint inCol) { JPH_ASSERT(inRow < mRows && inCol < mCols); return mElements[inRow * mCols + inCol]; } + + /// Get dimensions + uint GetCols() const { return mCols; } + uint GetRows() const { return mRows; } + +private: + uint mRows; + uint mCols; + Array mElements; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/EigenValueSymmetric.h b/thirdparty/jolt_physics/Jolt/Math/EigenValueSymmetric.h new file mode 100644 index 000000000000..920ddb9c9e72 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/EigenValueSymmetric.h @@ -0,0 +1,177 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Function to determine the eigen vectors and values of a N x N real symmetric matrix +/// by Jacobi transformations. This method is most suitable for N < 10. +/// +/// Taken and adapted from Numerical Recipies paragraph 11.1 +/// +/// An eigen vector is a vector v for which \f$A \: v = \lambda \: v\f$ +/// +/// Where: +/// A: A square matrix. +/// \f$\lambda\f$: a non-zero constant value. +/// +/// @see https://en.wikipedia.org/wiki/Eigenvalues_and_eigenvectors +/// +/// Matrix is a matrix type, which has dimensions N x N. +/// @param inMatrix is the matrix of which to return the eigenvalues and vectors +/// @param outEigVec will contain a matrix whose columns contain the normalized eigenvectors (must be identity before call) +/// @param outEigVal will contain the eigenvalues +template +bool EigenValueSymmetric(const Matrix &inMatrix, Matrix &outEigVec, Vector &outEigVal) +{ + // This algorithm can generate infinite values, see comment below + FPExceptionDisableInvalid disable_invalid; + (void)disable_invalid; + + // Maximum number of sweeps to make + const int cMaxSweeps = 50; + + // Get problem dimension + const uint n = inMatrix.GetRows(); + + // Make sure the dimensions are right + JPH_ASSERT(inMatrix.GetRows() == n); + JPH_ASSERT(inMatrix.GetCols() == n); + JPH_ASSERT(outEigVec.GetRows() == n); + JPH_ASSERT(outEigVec.GetCols() == n); + JPH_ASSERT(outEigVal.GetRows() == n); + JPH_ASSERT(outEigVec.IsIdentity()); + + // Get the matrix in a so we can mess with it + Matrix a = inMatrix; + + Vector b, z; + + for (uint ip = 0; ip < n; ++ip) + { + // Initialize b to diagonal of a + b[ip] = a(ip, ip); + + // Initialize output to diagonal of a + outEigVal[ip] = a(ip, ip); + + // Reset z + z[ip] = 0.0f; + } + + for (int sweep = 0; sweep < cMaxSweeps; ++sweep) + { + // Get the sum of the off-diagonal elements of a + float sm = 0.0f; + for (uint ip = 0; ip < n - 1; ++ip) + for (uint iq = ip + 1; iq < n; ++iq) + sm += abs(a(ip, iq)); + float avg_sm = sm / Square(n); + + // Normal return, convergence to machine underflow + if (avg_sm < FLT_MIN) // Original code: sm == 0.0f, when the average is denormal, we also consider it machine underflow + { + // Sanity checks + #ifdef JPH_ENABLE_ASSERTS + for (uint c = 0; c < n; ++c) + { + // Check if the eigenvector is normalized + JPH_ASSERT(outEigVec.GetColumn(c).IsNormalized()); + + // Check if inMatrix * eigen_vector = eigen_value * eigen_vector + Vector mat_eigvec = inMatrix * outEigVec.GetColumn(c); + Vector eigval_eigvec = outEigVal[c] * outEigVec.GetColumn(c); + JPH_ASSERT(mat_eigvec.IsClose(eigval_eigvec, max(mat_eigvec.LengthSq(), eigval_eigvec.LengthSq()) * 1.0e-6f)); + } + #endif + + // Success + return true; + } + + // On the first three sweeps use a fraction of the sum of the off diagonal elements as threshold + // Note that we pick a minimum threshold of FLT_MIN because dividing by a denormalized number is likely to result in infinity. + float tresh = sweep < 4? 0.2f * avg_sm : FLT_MIN; // Original code: 0.0f instead of FLT_MIN + + for (uint ip = 0; ip < n - 1; ++ip) + for (uint iq = ip + 1; iq < n; ++iq) + { + float &a_pq = a(ip, iq); + float &eigval_p = outEigVal[ip]; + float &eigval_q = outEigVal[iq]; + + float abs_a_pq = abs(a_pq); + float g = 100.0f * abs_a_pq; + + // After four sweeps, skip the rotation if the off-diagonal element is small + if (sweep > 4 + && abs(eigval_p) + g == abs(eigval_p) + && abs(eigval_q) + g == abs(eigval_q)) + { + a_pq = 0.0f; + } + else if (abs_a_pq > tresh) + { + float h = eigval_q - eigval_p; + float abs_h = abs(h); + + float t; + if (abs_h + g == abs_h) + { + t = a_pq / h; + } + else + { + float theta = 0.5f * h / a_pq; // Warning: Can become infinite if a(ip, iq) is very small which may trigger an invalid float exception + t = 1.0f / (abs(theta) + sqrt(1.0f + theta * theta)); // If theta becomes inf, t will be 0 so the infinite is not a problem for the algorithm + if (theta < 0.0f) t = -t; + } + + float c = 1.0f / sqrt(1.0f + t * t); + float s = t * c; + float tau = s / (1.0f + c); + h = t * a_pq; + + a_pq = 0.0f; + + z[ip] -= h; + z[iq] += h; + + eigval_p -= h; + eigval_q += h; + + #define JPH_EVS_ROTATE(a, i, j, k, l) \ + g = a(i, j), \ + h = a(k, l), \ + a(i, j) = g - s * (h + g * tau), \ + a(k, l) = h + s * (g - h * tau) + + uint j; + for (j = 0; j < ip; ++j) JPH_EVS_ROTATE(a, j, ip, j, iq); + for (j = ip + 1; j < iq; ++j) JPH_EVS_ROTATE(a, ip, j, j, iq); + for (j = iq + 1; j < n; ++j) JPH_EVS_ROTATE(a, ip, j, iq, j); + for (j = 0; j < n; ++j) JPH_EVS_ROTATE(outEigVec, j, ip, j, iq); + + #undef JPH_EVS_ROTATE + } + } + + // Update eigenvalues with the sum of ta_pq and reinitialize z + for (uint ip = 0; ip < n; ++ip) + { + b[ip] += z[ip]; + outEigVal[ip] = b[ip]; + z[ip] = 0.0f; + } + } + + // Failure + JPH_ASSERT(false, "Too many iterations"); + return false; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/FindRoot.h b/thirdparty/jolt_physics/Jolt/Math/FindRoot.h new file mode 100644 index 000000000000..21fef9f61f08 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/FindRoot.h @@ -0,0 +1,42 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Find the roots of \f$inA \: x^2 + inB \: x + inC = 0\f$. +/// @return The number of roots, actual roots in outX1 and outX2. +/// If number of roots returned is 1 then outX1 == outX2. +template +inline int FindRoot(const T inA, const T inB, const T inC, T &outX1, T &outX2) +{ + // Check if this is a linear equation + if (inA == T(0)) + { + // Check if this is a constant equation + if (inB == T(0)) + return 0; + + // Linear equation with 1 solution + outX1 = outX2 = -inC / inB; + return 1; + } + + // See Numerical Recipes in C, Chapter 5.6 Quadratic and Cubic Equations + T det = Square(inB) - T(4) * inA * inC; + if (det < T(0)) + return 0; + T q = (inB + Sign(inB) * sqrt(det)) / T(-2); + outX1 = q / inA; + if (q == T(0)) + { + outX2 = outX1; + return 1; + } + outX2 = inC / q; + return 2; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Float2.h b/thirdparty/jolt_physics/Jolt/Math/Float2.h new file mode 100644 index 000000000000..3e16e8c6c048 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Float2.h @@ -0,0 +1,36 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Class that holds 2 floats, used as a storage class mainly. +class [[nodiscard]] Float2 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + Float2() = default; ///< Intentionally not initialized for performance reasons + Float2(const Float2 &inRHS) = default; + Float2 & operator = (const Float2 &inRHS) = default; + Float2(float inX, float inY) : x(inX), y(inY) { } + + bool operator == (const Float2 &inRHS) const { return x == inRHS.x && y == inRHS.y; } + bool operator != (const Float2 &inRHS) const { return x != inRHS.x || y != inRHS.y; } + + /// To String + friend ostream & operator << (ostream &inStream, const Float2 &inV) + { + inStream << inV.x << ", " << inV.y; + return inStream; + } + + float x; + float y; +}; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Float3.h b/thirdparty/jolt_physics/Jolt/Math/Float3.h new file mode 100644 index 000000000000..1355e9ca0f81 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Float3.h @@ -0,0 +1,50 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that holds 3 floats. Used as a storage class. Convert to Vec3 for calculations. +class [[nodiscard]] Float3 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + Float3() = default; ///< Intentionally not initialized for performance reasons + Float3(const Float3 &inRHS) = default; + Float3 & operator = (const Float3 &inRHS) = default; + constexpr Float3(float inX, float inY, float inZ) : x(inX), y(inY), z(inZ) { } + + float operator [] (int inCoordinate) const + { + JPH_ASSERT(inCoordinate < 3); + return *(&x + inCoordinate); + } + + bool operator == (const Float3 &inRHS) const + { + return x == inRHS.x && y == inRHS.y && z == inRHS.z; + } + + bool operator != (const Float3 &inRHS) const + { + return x != inRHS.x || y != inRHS.y || z != inRHS.z; + } + + float x; + float y; + float z; +}; + +using VertexList = Array; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +// Create a std::hash/JPH::Hash for Float3 +JPH_MAKE_HASHABLE(JPH::Float3, t.x, t.y, t.z) diff --git a/thirdparty/jolt_physics/Jolt/Math/Float4.h b/thirdparty/jolt_physics/Jolt/Math/Float4.h new file mode 100644 index 000000000000..ca292b3ac3e4 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Float4.h @@ -0,0 +1,33 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Class that holds 4 float values. Convert to Vec4 to perform calculations. +class [[nodiscard]] Float4 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + 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) { } + + float operator [] (int inCoordinate) const + { + JPH_ASSERT(inCoordinate < 4); + return *(&x + inCoordinate); + } + + float x; + float y; + float z; + float w; +}; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/GaussianElimination.h b/thirdparty/jolt_physics/Jolt/Math/GaussianElimination.h new file mode 100644 index 000000000000..f986cf3b3b07 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/GaussianElimination.h @@ -0,0 +1,102 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// This function performs Gauss-Jordan elimination to solve a matrix equation. +/// A must be an NxN matrix and B must be an NxM matrix forming the equation A * x = B +/// on output B will contain x and A will be destroyed. +/// +/// This code can be used for example to compute the inverse of a matrix. +/// Set A to the matrix to invert, set B to identity and let GaussianElimination solve +/// the equation, on return B will be the inverse of A. And A is destroyed. +/// +/// Taken and adapted from Numerical Recipies in C paragraph 2.1 +template +bool GaussianElimination(MatrixA &ioA, MatrixB &ioB, float inTolerance = 1.0e-16f) +{ + // Get problem dimensions + const uint n = ioA.GetCols(); + const uint m = ioB.GetCols(); + + // Check matrix requirement + JPH_ASSERT(ioA.GetRows() == n); + JPH_ASSERT(ioB.GetRows() == n); + + // Create array for bookkeeping on pivoting + int *ipiv = (int *)JPH_STACK_ALLOC(n * sizeof(int)); + memset(ipiv, 0, n * sizeof(int)); + + for (uint i = 0; i < n; ++i) + { + // Initialize pivot element as the diagonal + uint pivot_row = i, pivot_col = i; + + // Determine pivot element + float largest_element = 0.0f; + for (uint j = 0; j < n; ++j) + if (ipiv[j] != 1) + for (uint k = 0; k < n; ++k) + { + if (ipiv[k] == 0) + { + float element = abs(ioA(j, k)); + if (element >= largest_element) + { + largest_element = element; + pivot_row = j; + pivot_col = k; + } + } + else if (ipiv[k] > 1) + { + return false; + } + } + + // Mark this column as used + ++ipiv[pivot_col]; + + // Exchange rows when needed so that the pivot element is at ioA(pivot_col, pivot_col) instead of at ioA(pivot_row, pivot_col) + if (pivot_row != pivot_col) + { + for (uint j = 0; j < n; ++j) + std::swap(ioA(pivot_row, j), ioA(pivot_col, j)); + for (uint j = 0; j < m; ++j) + std::swap(ioB(pivot_row, j), ioB(pivot_col, j)); + } + + // Get diagonal element that we are about to set to 1 + float diagonal_element = ioA(pivot_col, pivot_col); + if (abs(diagonal_element) < inTolerance) + return false; + + // Divide the whole row by the pivot element, making ioA(pivot_col, pivot_col) = 1 + for (uint j = 0; j < n; ++j) + ioA(pivot_col, j) /= diagonal_element; + for (uint j = 0; j < m; ++j) + ioB(pivot_col, j) /= diagonal_element; + ioA(pivot_col, pivot_col) = 1.0f; + + // Next reduce the rows, except for the pivot one, + // after this step the pivot_col column is zero except for the pivot element which is 1 + for (uint j = 0; j < n; ++j) + if (j != pivot_col) + { + float element = ioA(j, pivot_col); + for (uint k = 0; k < n; ++k) + ioA(j, k) -= ioA(pivot_col, k) * element; + for (uint k = 0; k < m; ++k) + ioB(j, k) -= ioB(pivot_col, k) * element; + ioA(j, pivot_col) = 0.0f; + } + } + + // Success + return true; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/HalfFloat.h b/thirdparty/jolt_physics/Jolt/Math/HalfFloat.h new file mode 100644 index 000000000000..5b36cb095177 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/HalfFloat.h @@ -0,0 +1,204 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +using HalfFloat = uint16; + +// Define half float constant values +static constexpr HalfFloat HALF_FLT_MAX = 0x7bff; +static constexpr HalfFloat HALF_FLT_MAX_NEGATIVE = 0xfbff; +static constexpr HalfFloat HALF_FLT_INF = 0x7c00; +static constexpr HalfFloat HALF_FLT_INF_NEGATIVE = 0xfc00; +static constexpr HalfFloat HALF_FLT_NANQ = 0x7e00; +static constexpr HalfFloat HALF_FLT_NANQ_NEGATIVE = 0xfe00; + +namespace HalfFloatConversion { + +// Layout of a float +static constexpr int FLOAT_SIGN_POS = 31; +static constexpr int FLOAT_EXPONENT_POS = 23; +static constexpr int FLOAT_EXPONENT_BITS = 8; +static constexpr int FLOAT_EXPONENT_MASK = (1 << FLOAT_EXPONENT_BITS) - 1; +static constexpr int FLOAT_EXPONENT_BIAS = 127; +static constexpr int FLOAT_MANTISSA_BITS = 23; +static constexpr int FLOAT_MANTISSA_MASK = (1 << FLOAT_MANTISSA_BITS) - 1; +static constexpr int FLOAT_EXPONENT_AND_MANTISSA_MASK = FLOAT_MANTISSA_MASK + (FLOAT_EXPONENT_MASK << FLOAT_EXPONENT_POS); + +// Layout of half float +static constexpr int HALF_FLT_SIGN_POS = 15; +static constexpr int HALF_FLT_EXPONENT_POS = 10; +static constexpr int HALF_FLT_EXPONENT_BITS = 5; +static constexpr int HALF_FLT_EXPONENT_MASK = (1 << HALF_FLT_EXPONENT_BITS) - 1; +static constexpr int HALF_FLT_EXPONENT_BIAS = 15; +static constexpr int HALF_FLT_MANTISSA_BITS = 10; +static constexpr int HALF_FLT_MANTISSA_MASK = (1 << HALF_FLT_MANTISSA_BITS) - 1; +static constexpr int HALF_FLT_EXPONENT_AND_MANTISSA_MASK = HALF_FLT_MANTISSA_MASK + (HALF_FLT_EXPONENT_MASK << HALF_FLT_EXPONENT_POS); + +/// Define half-float rounding modes +enum ERoundingMode +{ + ROUND_TO_NEG_INF, ///< Round to negative infinity + ROUND_TO_POS_INF, ///< Round to positive infinity + ROUND_TO_NEAREST, ///< Round to nearest value +}; + +/// Convert a float (32-bits) to a half float (16-bits), fallback version when no intrinsics available +template +inline HalfFloat FromFloatFallback(float inV) +{ + // Reinterpret the float as an uint32 + uint32 value = BitCast(inV); + + // Extract exponent + uint32 exponent = (value >> FLOAT_EXPONENT_POS) & FLOAT_EXPONENT_MASK; + + // Extract mantissa + uint32 mantissa = value & FLOAT_MANTISSA_MASK; + + // Extract the sign and move it into the right spot for the half float (so we can just or it in at the end) + HalfFloat hf_sign = HalfFloat(value >> (FLOAT_SIGN_POS - HALF_FLT_SIGN_POS)) & (1 << HALF_FLT_SIGN_POS); + + // Check NaN or INF + if (exponent == FLOAT_EXPONENT_MASK) // NaN or INF + return hf_sign | (mantissa == 0? HALF_FLT_INF : HALF_FLT_NANQ); + + // Rebias the exponent for half floats + int rebiased_exponent = int(exponent) - FLOAT_EXPONENT_BIAS + HALF_FLT_EXPONENT_BIAS; + + // Check overflow to infinity + if (rebiased_exponent >= HALF_FLT_EXPONENT_MASK) + { + bool round_up = RoundingMode == ROUND_TO_NEAREST || (hf_sign == 0) == (RoundingMode == ROUND_TO_POS_INF); + return hf_sign | (round_up? HALF_FLT_INF : HALF_FLT_MAX); + } + + // Check underflow to zero + if (rebiased_exponent < -HALF_FLT_MANTISSA_BITS) + { + bool round_up = RoundingMode != ROUND_TO_NEAREST && (hf_sign == 0) == (RoundingMode == ROUND_TO_POS_INF) && (value & FLOAT_EXPONENT_AND_MANTISSA_MASK) != 0; + return hf_sign | (round_up? 1 : 0); + } + + HalfFloat hf_exponent; + int shift; + if (rebiased_exponent <= 0) + { + // Underflow to denormalized number + hf_exponent = 0; + mantissa |= 1 << FLOAT_MANTISSA_BITS; // Add the implicit 1 bit to the mantissa + shift = FLOAT_MANTISSA_BITS - HALF_FLT_MANTISSA_BITS + 1 - rebiased_exponent; + } + else + { + // Normal half float + hf_exponent = HalfFloat(rebiased_exponent << HALF_FLT_EXPONENT_POS); + shift = FLOAT_MANTISSA_BITS - HALF_FLT_MANTISSA_BITS; + } + + // Compose the half float + HalfFloat hf_mantissa = HalfFloat(mantissa >> shift); + HalfFloat hf = hf_sign | hf_exponent | hf_mantissa; + + // Calculate the remaining bits that we're discarding + uint remainder = mantissa & ((1 << shift) - 1); + + if constexpr (RoundingMode == ROUND_TO_NEAREST) + { + // Round to nearest + uint round_threshold = 1 << (shift - 1); + if (remainder > round_threshold // Above threshold, we must always round + || (remainder == round_threshold && (hf_mantissa & 1))) // When equal, round to nearest even + hf++; // May overflow to infinity + } + else + { + // Round up or down (truncate) depending on the rounding mode + bool round_up = (hf_sign == 0) == (RoundingMode == ROUND_TO_POS_INF) && remainder != 0; + if (round_up) + hf++; // May overflow to infinity + } + + return hf; +} + +/// Convert a float (32-bits) to a half float (16-bits) +template +JPH_INLINE HalfFloat FromFloat(float inV) +{ +#ifdef JPH_USE_F16C + union + { + __m128i u128; + HalfFloat u16[8]; + } hf; + __m128 val = _mm_load_ss(&inV); + switch (RoundingMode) + { + case ROUND_TO_NEG_INF: + hf.u128 = _mm_cvtps_ph(val, _MM_FROUND_TO_NEG_INF); + break; + case ROUND_TO_POS_INF: + hf.u128 = _mm_cvtps_ph(val, _MM_FROUND_TO_POS_INF); + break; + case ROUND_TO_NEAREST: + hf.u128 = _mm_cvtps_ph(val, _MM_FROUND_TO_NEAREST_INT); + break; + } + return hf.u16[0]; +#else + return FromFloatFallback(inV); +#endif +} + +/// Convert 4 half floats (lower 64 bits) to floats, fallback version when no intrinsics available +inline Vec4 ToFloatFallback(UVec4Arg inValue) +{ + // Unpack half floats to 4 uint32's + UVec4 value = inValue.Expand4Uint16Lo(); + + // Normal half float path, extract the exponent and mantissa, shift them into place and update the exponent bias + UVec4 exponent_mantissa = UVec4::sAnd(value, UVec4::sReplicate(HALF_FLT_EXPONENT_AND_MANTISSA_MASK)).LogicalShiftLeft() + UVec4::sReplicate((FLOAT_EXPONENT_BIAS - HALF_FLT_EXPONENT_BIAS) << FLOAT_EXPONENT_POS); + + // Denormalized half float path, renormalize the float + UVec4 exponent_mantissa_denormalized = ((exponent_mantissa + UVec4::sReplicate(1 << FLOAT_EXPONENT_POS)).ReinterpretAsFloat() - UVec4::sReplicate((FLOAT_EXPONENT_BIAS - HALF_FLT_EXPONENT_BIAS + 1) << FLOAT_EXPONENT_POS).ReinterpretAsFloat()).ReinterpretAsInt(); + + // NaN / INF path, set all exponent bits + UVec4 exponent_mantissa_nan_inf = UVec4::sOr(exponent_mantissa, UVec4::sReplicate(FLOAT_EXPONENT_MASK << FLOAT_EXPONENT_POS)); + + // Get the exponent to determine which of the paths we should take + UVec4 exponent_mask = UVec4::sReplicate(HALF_FLT_EXPONENT_MASK << HALF_FLT_EXPONENT_POS); + UVec4 exponent = UVec4::sAnd(value, exponent_mask); + UVec4 is_denormalized = UVec4::sEquals(exponent, UVec4::sZero()); + UVec4 is_nan_inf = UVec4::sEquals(exponent, exponent_mask); + + // Select the correct result + UVec4 result_exponent_mantissa = UVec4::sSelect(UVec4::sSelect(exponent_mantissa, exponent_mantissa_nan_inf, is_nan_inf), exponent_mantissa_denormalized, is_denormalized); + + // Extract the sign bit and shift it to the left + UVec4 sign = UVec4::sAnd(value, UVec4::sReplicate(1 << HALF_FLT_SIGN_POS)).LogicalShiftLeft(); + + // Construct the float + return UVec4::sOr(sign, result_exponent_mantissa).ReinterpretAsFloat(); +} + +/// Convert 4 half floats (lower 64 bits) to floats +JPH_INLINE Vec4 ToFloat(UVec4Arg inValue) +{ +#if defined(JPH_USE_F16C) + return _mm_cvtph_ps(inValue.mValue); +#elif defined(JPH_USE_NEON) + return vcvt_f32_f16(vreinterpret_f16_u32(vget_low_u32(inValue.mValue))); +#else + return ToFloatFallback(inValue); +#endif +} + +} // HalfFloatConversion + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Mat44.h b/thirdparty/jolt_physics/Jolt/Math/Mat44.h new file mode 100644 index 000000000000..4774935efcc7 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Mat44.h @@ -0,0 +1,243 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Holds a 4x4 matrix of floats, but supports also operations on the 3x3 upper left part of the matrix. +class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Mat44 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // Underlying column type + using Type = Vec4::Type; + + // Argument type + using ArgType = Mat44Arg; + + /// Constructor + Mat44() = default; ///< Intentionally not initialized for performance reasons + JPH_INLINE Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec4Arg inC4); + JPH_INLINE Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec3Arg inC4); + Mat44(const Mat44 &inM2) = default; + Mat44 & operator = (const Mat44 &inM2) = default; + JPH_INLINE Mat44(Type inC1, Type inC2, Type inC3, Type inC4); + + /// Zero matrix + static JPH_INLINE Mat44 sZero(); + + /// Identity matrix + static JPH_INLINE Mat44 sIdentity(); + + /// Matrix filled with NaN's + static JPH_INLINE Mat44 sNaN(); + + /// Load 16 floats from memory + static JPH_INLINE Mat44 sLoadFloat4x4(const Float4 *inV); + + /// Load 16 floats from memory, 16 bytes aligned + static JPH_INLINE Mat44 sLoadFloat4x4Aligned(const Float4 *inV); + + /// Rotate around X, Y or Z axis (angle in radians) + static JPH_INLINE Mat44 sRotationX(float inX); + static JPH_INLINE Mat44 sRotationY(float inY); + static JPH_INLINE Mat44 sRotationZ(float inZ); + + /// Rotate around arbitrary axis + static JPH_INLINE Mat44 sRotation(Vec3Arg inAxis, float inAngle); + + /// Rotate from quaternion + static JPH_INLINE Mat44 sRotation(QuatArg inQuat); + + /// Get matrix that translates + static JPH_INLINE Mat44 sTranslation(Vec3Arg inV); + + /// Get matrix that rotates and translates + static JPH_INLINE Mat44 sRotationTranslation(QuatArg inR, Vec3Arg inT); + + /// Get inverse matrix of sRotationTranslation + static JPH_INLINE Mat44 sInverseRotationTranslation(QuatArg inR, Vec3Arg inT); + + /// Get matrix that scales uniformly + static JPH_INLINE Mat44 sScale(float inScale); + + /// Get matrix that scales (produces a matrix with (inV, 1) on its diagonal) + static JPH_INLINE Mat44 sScale(Vec3Arg inV); + + /// Get outer product of inV and inV2 (equivalent to \f$inV1 \otimes inV2\f$) + static JPH_INLINE Mat44 sOuterProduct(Vec3Arg inV1, Vec3Arg inV2); + + /// Get matrix that represents a cross product \f$A \times B = \text{sCrossProduct}(A) \: B\f$ + static JPH_INLINE Mat44 sCrossProduct(Vec3Arg inV); + + /// Returns matrix ML so that \f$ML(q) \: p = q \: p\f$ (where p and q are quaternions) + static JPH_INLINE Mat44 sQuatLeftMultiply(QuatArg inQ); + + /// Returns matrix MR so that \f$MR(q) \: p = p \: q\f$ (where p and q are quaternions) + static JPH_INLINE Mat44 sQuatRightMultiply(QuatArg inQ); + + /// Returns a look at matrix that transforms from world space to view space + /// @param inPos Position of the camera + /// @param inTarget Target of the camera + /// @param inUp Up vector + static JPH_INLINE Mat44 sLookAt(Vec3Arg inPos, Vec3Arg inTarget, Vec3Arg inUp); + + /// Returns a right-handed perspective projection matrix + static JPH_INLINE Mat44 sPerspective(float inFovY, float inAspect, float inNear, float inFar); + + /// Get float component by element index + JPH_INLINE float operator () (uint inRow, uint inColumn) const { JPH_ASSERT(inRow < 4); JPH_ASSERT(inColumn < 4); return mCol[inColumn].mF32[inRow]; } + JPH_INLINE float & operator () (uint inRow, uint inColumn) { JPH_ASSERT(inRow < 4); JPH_ASSERT(inColumn < 4); return mCol[inColumn].mF32[inRow]; } + + /// Comparison + JPH_INLINE bool operator == (Mat44Arg inM2) const; + JPH_INLINE bool operator != (Mat44Arg inM2) const { return !(*this == inM2); } + + /// Test if two matrices are close + JPH_INLINE bool IsClose(Mat44Arg inM2, float inMaxDistSq = 1.0e-12f) const; + + /// Multiply matrix by matrix + JPH_INLINE Mat44 operator * (Mat44Arg inM) const; + + /// Multiply vector by matrix + JPH_INLINE Vec3 operator * (Vec3Arg inV) const; + JPH_INLINE Vec4 operator * (Vec4Arg inV) const; + + /// Multiply vector by only 3x3 part of the matrix + JPH_INLINE Vec3 Multiply3x3(Vec3Arg inV) const; + + /// Multiply vector by only 3x3 part of the transpose of the matrix (\f$result = this^T \: inV\f$) + JPH_INLINE Vec3 Multiply3x3Transposed(Vec3Arg inV) const; + + /// Multiply 3x3 matrix by 3x3 matrix + JPH_INLINE Mat44 Multiply3x3(Mat44Arg inM) const; + + /// Multiply transpose of 3x3 matrix by 3x3 matrix (\f$result = this^T \: inM\f$) + JPH_INLINE Mat44 Multiply3x3LeftTransposed(Mat44Arg inM) const; + + /// Multiply 3x3 matrix by the transpose of a 3x3 matrix (\f$result = this \: inM^T\f$) + JPH_INLINE Mat44 Multiply3x3RightTransposed(Mat44Arg inM) const; + + /// Multiply matrix with float + JPH_INLINE Mat44 operator * (float inV) const; + friend JPH_INLINE Mat44 operator * (float inV, Mat44Arg inM) { return inM * inV; } + + /// Multiply matrix with float + JPH_INLINE Mat44 & operator *= (float inV); + + /// Per element addition of matrix + JPH_INLINE Mat44 operator + (Mat44Arg inM) const; + + /// Negate + JPH_INLINE Mat44 operator - () const; + + /// Per element subtraction of matrix + JPH_INLINE Mat44 operator - (Mat44Arg inM) const; + + /// Per element addition of matrix + JPH_INLINE Mat44 & operator += (Mat44Arg inM); + + /// Access to the columns + JPH_INLINE Vec3 GetAxisX() const { return Vec3(mCol[0]); } + JPH_INLINE void SetAxisX(Vec3Arg inV) { mCol[0] = Vec4(inV, 0.0f); } + JPH_INLINE Vec3 GetAxisY() const { return Vec3(mCol[1]); } + JPH_INLINE void SetAxisY(Vec3Arg inV) { mCol[1] = Vec4(inV, 0.0f); } + JPH_INLINE Vec3 GetAxisZ() const { return Vec3(mCol[2]); } + JPH_INLINE void SetAxisZ(Vec3Arg inV) { mCol[2] = Vec4(inV, 0.0f); } + JPH_INLINE Vec3 GetTranslation() const { return Vec3(mCol[3]); } + JPH_INLINE void SetTranslation(Vec3Arg inV) { mCol[3] = Vec4(inV, 1.0f); } + JPH_INLINE Vec3 GetDiagonal3() const { return Vec3(mCol[0][0], mCol[1][1], mCol[2][2]); } + JPH_INLINE void SetDiagonal3(Vec3Arg inV) { mCol[0][0] = inV.GetX(); mCol[1][1] = inV.GetY(); mCol[2][2] = inV.GetZ(); } + JPH_INLINE Vec4 GetDiagonal4() const { return Vec4(mCol[0][0], mCol[1][1], mCol[2][2], mCol[3][3]); } + JPH_INLINE void SetDiagonal4(Vec4Arg inV) { mCol[0][0] = inV.GetX(); mCol[1][1] = inV.GetY(); mCol[2][2] = inV.GetZ(); mCol[3][3] = inV.GetW(); } + JPH_INLINE Vec3 GetColumn3(uint inCol) const { JPH_ASSERT(inCol < 4); return Vec3(mCol[inCol]); } + JPH_INLINE void SetColumn3(uint inCol, Vec3Arg inV) { JPH_ASSERT(inCol < 4); mCol[inCol] = Vec4(inV, inCol == 3? 1.0f : 0.0f); } + JPH_INLINE Vec4 GetColumn4(uint inCol) const { JPH_ASSERT(inCol < 4); return mCol[inCol]; } + JPH_INLINE void SetColumn4(uint inCol, Vec4Arg inV) { JPH_ASSERT(inCol < 4); mCol[inCol] = inV; } + + /// Store matrix to memory + JPH_INLINE void StoreFloat4x4(Float4 *outV) const; + + /// Transpose matrix + JPH_INLINE Mat44 Transposed() const; + + /// Transpose 3x3 subpart of matrix + JPH_INLINE Mat44 Transposed3x3() const; + + /// Inverse 4x4 matrix + JPH_INLINE Mat44 Inversed() const; + + /// Inverse 4x4 matrix when it only contains rotation and translation + JPH_INLINE Mat44 InversedRotationTranslation() const; + + /// Get the determinant of a 3x3 matrix + JPH_INLINE float GetDeterminant3x3() const; + + /// Get the adjoint of a 3x3 matrix + JPH_INLINE Mat44 Adjointed3x3() const; + + /// Inverse 3x3 matrix + JPH_INLINE Mat44 Inversed3x3() const; + + /// *this = inM.Inversed3x3(), returns false if the matrix is singular in which case *this is unchanged + JPH_INLINE bool SetInversed3x3(Mat44Arg inM); + + /// Get rotation part only (note: retains the first 3 values from the bottom row) + JPH_INLINE Mat44 GetRotation() const; + + /// Get rotation part only (note: also clears the bottom row) + JPH_INLINE Mat44 GetRotationSafe() const; + + /// Updates the rotation part of this matrix (the first 3 columns) + JPH_INLINE void SetRotation(Mat44Arg inRotation); + + /// Convert to quaternion + JPH_INLINE Quat GetQuaternion() const; + + /// Get matrix that transforms a direction with the same transform as this matrix (length is not preserved) + JPH_INLINE Mat44 GetDirectionPreservingMatrix() const { return GetRotation().Inversed3x3().Transposed3x3(); } + + /// Pre multiply by translation matrix: result = this * Mat44::sTranslation(inTranslation) + JPH_INLINE Mat44 PreTranslated(Vec3Arg inTranslation) const; + + /// Post multiply by translation matrix: result = Mat44::sTranslation(inTranslation) * this (i.e. add inTranslation to the 4-th column) + JPH_INLINE Mat44 PostTranslated(Vec3Arg inTranslation) const; + + /// Scale a matrix: result = this * Mat44::sScale(inScale) + JPH_INLINE Mat44 PreScaled(Vec3Arg inScale) const; + + /// Scale a matrix: result = Mat44::sScale(inScale) * this + JPH_INLINE Mat44 PostScaled(Vec3Arg inScale) const; + + /// Decompose a matrix into a rotation & translation part and into a scale part so that: + /// this = return_value * Mat44::sScale(outScale). + /// This equation only holds when the matrix is orthogonal, if it is not the returned matrix + /// will be made orthogonal using the modified Gram-Schmidt algorithm (see: https://en.wikipedia.org/wiki/Gram%E2%80%93Schmidt_process) + JPH_INLINE Mat44 Decompose(Vec3 &outScale) const; + +#ifndef JPH_DOUBLE_PRECISION + /// In single precision mode just return the matrix itself + JPH_INLINE Mat44 ToMat44() const { return *this; } +#endif // !JPH_DOUBLE_PRECISION + + /// To String + friend ostream & operator << (ostream &inStream, Mat44Arg inM) + { + inStream << inM.mCol[0] << ", " << inM.mCol[1] << ", " << inM.mCol[2] << ", " << inM.mCol[3]; + return inStream; + } + +private: + Vec4 mCol[4]; ///< Column +}; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "Mat44.inl" diff --git a/thirdparty/jolt_physics/Jolt/Math/Mat44.inl b/thirdparty/jolt_physics/Jolt/Math/Mat44.inl new file mode 100644 index 000000000000..76577b7153a6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Mat44.inl @@ -0,0 +1,952 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +#define JPH_EL(r, c) mCol[c].mF32[r] + +Mat44::Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec4Arg inC4) : + mCol { inC1, inC2, inC3, inC4 } +{ +} + +Mat44::Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec3Arg inC4) : + mCol { inC1, inC2, inC3, Vec4(inC4, 1.0f) } +{ +} + +Mat44::Mat44(Type inC1, Type inC2, Type inC3, Type inC4) : + mCol { inC1, inC2, inC3, inC4 } +{ +} + +Mat44 Mat44::sZero() +{ + return Mat44(Vec4::sZero(), Vec4::sZero(), Vec4::sZero(), Vec4::sZero()); +} + +Mat44 Mat44::sIdentity() +{ + return Mat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::sNaN() +{ + return Mat44(Vec4::sNaN(), Vec4::sNaN(), Vec4::sNaN(), Vec4::sNaN()); +} + +Mat44 Mat44::sLoadFloat4x4(const Float4 *inV) +{ + Mat44 result; + for (int c = 0; c < 4; ++c) + result.mCol[c] = Vec4::sLoadFloat4(inV + c); + return result; +} + +Mat44 Mat44::sLoadFloat4x4Aligned(const Float4 *inV) +{ + Mat44 result; + for (int c = 0; c < 4; ++c) + result.mCol[c] = Vec4::sLoadFloat4Aligned(inV + c); + return result; +} + +Mat44 Mat44::sRotationX(float inX) +{ + Vec4 sv, cv; + Vec4::sReplicate(inX).SinCos(sv, cv); + float s = sv.GetX(), c = cv.GetX(); + return Mat44(Vec4(1, 0, 0, 0), Vec4(0, c, s, 0), Vec4(0, -s, c, 0), Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::sRotationY(float inY) +{ + Vec4 sv, cv; + Vec4::sReplicate(inY).SinCos(sv, cv); + float s = sv.GetX(), c = cv.GetX(); + return Mat44(Vec4(c, 0, -s, 0), Vec4(0, 1, 0, 0), Vec4(s, 0, c, 0), Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::sRotationZ(float inZ) +{ + Vec4 sv, cv; + Vec4::sReplicate(inZ).SinCos(sv, cv); + float s = sv.GetX(), c = cv.GetX(); + return Mat44(Vec4(c, s, 0, 0), Vec4(-s, c, 0, 0), Vec4(0, 0, 1, 0), Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::sRotation(QuatArg inQuat) +{ + JPH_ASSERT(inQuat.IsNormalized()); + + // See: https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation section 'Quaternion-derived rotation matrix' +#ifdef JPH_USE_SSE4_1 + __m128 xyzw = inQuat.mValue.mValue; + __m128 two_xyzw = _mm_add_ps(xyzw, xyzw); + __m128 yzxw = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 0, 2, 1)); + __m128 two_yzxw = _mm_add_ps(yzxw, yzxw); + __m128 zxyw = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 1, 0, 2)); + __m128 two_zxyw = _mm_add_ps(zxyw, zxyw); + __m128 wwww = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 3, 3, 3)); + __m128 diagonal = _mm_sub_ps(_mm_sub_ps(_mm_set1_ps(1.0f), _mm_mul_ps(two_yzxw, yzxw)), _mm_mul_ps(two_zxyw, zxyw)); // (1 - 2 y^2 - 2 z^2, 1 - 2 x^2 - 2 z^2, 1 - 2 x^2 - 2 y^2, 1 - 4 w^2) + __m128 plus = _mm_add_ps(_mm_mul_ps(two_xyzw, zxyw), _mm_mul_ps(two_yzxw, wwww)); // 2 * (xz + yw, xy + zw, yz + xw, ww) + __m128 minus = _mm_sub_ps(_mm_mul_ps(two_yzxw, xyzw), _mm_mul_ps(two_zxyw, wwww)); // 2 * (xy - zw, yz - xw, xz - yw, 0) + + // Workaround for compiler changing _mm_sub_ps(_mm_mul_ps(...), ...) into a fused multiply sub instruction, resulting in w not being 0 + // There doesn't appear to be a reliable way to turn this off in Clang + minus = _mm_insert_ps(minus, minus, 0b1000); + + __m128 col0 = _mm_blend_ps(_mm_blend_ps(plus, diagonal, 0b0001), minus, 0b1100); // (1 - 2 y^2 - 2 z^2, 2 xy + 2 zw, 2 xz - 2 yw, 0) + __m128 col1 = _mm_blend_ps(_mm_blend_ps(diagonal, minus, 0b1001), plus, 0b0100); // (2 xy - 2 zw, 1 - 2 x^2 - 2 z^2, 2 yz + 2 xw, 0) + __m128 col2 = _mm_blend_ps(_mm_blend_ps(minus, plus, 0b0001), diagonal, 0b0100); // (2 xz + 2 yw, 2 yz - 2 xw, 1 - 2 x^2 - 2 y^2, 0) + __m128 col3 = _mm_set_ps(1, 0, 0, 0); + + return Mat44(col0, col1, col2, col3); +#else + float x = inQuat.GetX(); + float y = inQuat.GetY(); + float z = inQuat.GetZ(); + float w = inQuat.GetW(); + + float tx = x + x; // Note: Using x + x instead of 2.0f * x to force this function to return the same value as the SSE4.1 version across platforms. + float ty = y + y; + float tz = z + z; + + float xx = tx * x; + float yy = ty * y; + float zz = tz * z; + float xy = tx * y; + float xz = tx * z; + float xw = tx * w; + float yz = ty * z; + float yw = ty * w; + float zw = tz * w; + + return Mat44(Vec4((1.0f - yy) - zz, xy + zw, xz - yw, 0.0f), // Note: Added extra brackets to force this function to return the same value as the SSE4.1 version across platforms. + Vec4(xy - zw, (1.0f - zz) - xx, yz + xw, 0.0f), + Vec4(xz + yw, yz - xw, (1.0f - xx) - yy, 0.0f), + Vec4(0.0f, 0.0f, 0.0f, 1.0f)); +#endif +} + +Mat44 Mat44::sRotation(Vec3Arg inAxis, float inAngle) +{ + return sRotation(Quat::sRotation(inAxis, inAngle)); +} + +Mat44 Mat44::sTranslation(Vec3Arg inV) +{ + return Mat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), Vec4(inV, 1)); +} + +Mat44 Mat44::sRotationTranslation(QuatArg inR, Vec3Arg inT) +{ + Mat44 m = sRotation(inR); + m.SetTranslation(inT); + return m; +} + +Mat44 Mat44::sInverseRotationTranslation(QuatArg inR, Vec3Arg inT) +{ + Mat44 m = sRotation(inR.Conjugated()); + m.SetTranslation(-m.Multiply3x3(inT)); + return m; +} + +Mat44 Mat44::sScale(float inScale) +{ + return Mat44(Vec4(inScale, 0, 0, 0), Vec4(0, inScale, 0, 0), Vec4(0, 0, inScale, 0), Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::sScale(Vec3Arg inV) +{ + return Mat44(Vec4(inV.GetX(), 0, 0, 0), Vec4(0, inV.GetY(), 0, 0), Vec4(0, 0, inV.GetZ(), 0), Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::sOuterProduct(Vec3Arg inV1, Vec3Arg inV2) +{ + Vec4 v1(inV1, 0); + return Mat44(v1 * inV2.SplatX(), v1 * inV2.SplatY(), v1 * inV2.SplatZ(), Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::sCrossProduct(Vec3Arg inV) +{ +#ifdef JPH_USE_SSE4_1 + // Zero out the W component + __m128 zero = _mm_setzero_ps(); + __m128 v = _mm_blend_ps(inV.mValue, zero, 0b1000); + + // Negate + __m128 min_v = _mm_sub_ps(zero, v); + + return Mat44( + _mm_shuffle_ps(v, min_v, _MM_SHUFFLE(3, 1, 2, 3)), // [0, z, -y, 0] + _mm_shuffle_ps(min_v, v, _MM_SHUFFLE(3, 0, 3, 2)), // [-z, 0, x, 0] + _mm_blend_ps(_mm_shuffle_ps(v, v, _MM_SHUFFLE(3, 3, 3, 1)), _mm_shuffle_ps(min_v, min_v, _MM_SHUFFLE(3, 3, 0, 3)), 0b0010), // [y, -x, 0, 0] + Vec4(0, 0, 0, 1)); +#else + float x = inV.GetX(); + float y = inV.GetY(); + float z = inV.GetZ(); + + return Mat44( + Vec4(0, z, -y, 0), + Vec4(-z, 0, x, 0), + Vec4(y, -x, 0, 0), + Vec4(0, 0, 0, 1)); +#endif +} + +Mat44 Mat44::sLookAt(Vec3Arg inPos, Vec3Arg inTarget, Vec3Arg inUp) +{ + Vec3 direction = (inTarget - inPos).NormalizedOr(-Vec3::sAxisZ()); + Vec3 right = direction.Cross(inUp).NormalizedOr(Vec3::sAxisX()); + Vec3 up = right.Cross(direction); + + return Mat44(Vec4(right, 0), Vec4(up, 0), Vec4(-direction, 0), Vec4(inPos, 1)).InversedRotationTranslation(); +} + +Mat44 Mat44::sPerspective(float inFovY, float inAspect, float inNear, float inFar) +{ + float height = 1.0f / Tan(0.5f * inFovY); + float width = height / inAspect; + float range = inFar / (inNear - inFar); + + return Mat44(Vec4(width, 0.0f, 0.0f, 0.0f), Vec4(0.0f, height, 0.0f, 0.0f), Vec4(0.0f, 0.0f, range, -1.0f), Vec4(0.0f, 0.0f, range * inNear, 0.0f)); +} + +bool Mat44::operator == (Mat44Arg inM2) const +{ + return UVec4::sAnd( + UVec4::sAnd(Vec4::sEquals(mCol[0], inM2.mCol[0]), Vec4::sEquals(mCol[1], inM2.mCol[1])), + UVec4::sAnd(Vec4::sEquals(mCol[2], inM2.mCol[2]), Vec4::sEquals(mCol[3], inM2.mCol[3])) + ).TestAllTrue(); +} + +bool Mat44::IsClose(Mat44Arg inM2, float inMaxDistSq) const +{ + for (int i = 0; i < 4; ++i) + if (!mCol[i].IsClose(inM2.mCol[i], inMaxDistSq)) + return false; + return true; +} + +Mat44 Mat44::operator * (Mat44Arg inM) const +{ + Mat44 result; +#if defined(JPH_USE_SSE) + for (int i = 0; i < 4; ++i) + { + __m128 c = inM.mCol[i].mValue; + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[3].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(3, 3, 3, 3)))); + result.mCol[i].mValue = t; + } +#elif defined(JPH_USE_NEON) + for (int i = 0; i < 4; ++i) + { + Type c = inM.mCol[i].mValue; + Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2)); + t = vmlaq_f32(t, mCol[3].mValue, vdupq_laneq_f32(c, 3)); + result.mCol[i].mValue = t; + } +#else + for (int i = 0; i < 4; ++i) + result.mCol[i] = mCol[0] * inM.mCol[i].mF32[0] + mCol[1] * inM.mCol[i].mF32[1] + mCol[2] * inM.mCol[i].mF32[2] + mCol[3] * inM.mCol[i].mF32[3]; +#endif + return result; +} + +Vec3 Mat44::operator * (Vec3Arg inV) const +{ +#if defined(JPH_USE_SSE) + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2)))); + t = _mm_add_ps(t, mCol[3].mValue); + return Vec3::sFixW(t); +#elif defined(JPH_USE_NEON) + Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2)); + t = vaddq_f32(t, mCol[3].mValue); // Don't combine this with the first mul into a fused multiply add, causes precision issues + return Vec3::sFixW(t); +#else + return Vec3( + mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2] + mCol[3].mF32[0], + mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2] + mCol[3].mF32[1], + mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2] + mCol[3].mF32[2]); +#endif +} + +Vec4 Mat44::operator * (Vec4Arg inV) const +{ +#if defined(JPH_USE_SSE) + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[3].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(3, 3, 3, 3)))); + return t; +#elif defined(JPH_USE_NEON) + Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2)); + t = vmlaq_f32(t, mCol[3].mValue, vdupq_laneq_f32(inV.mValue, 3)); + return t; +#else + return Vec4( + mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2] + mCol[3].mF32[0] * inV.mF32[3], + mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2] + mCol[3].mF32[1] * inV.mF32[3], + mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2] + mCol[3].mF32[2] * inV.mF32[3], + mCol[0].mF32[3] * inV.mF32[0] + mCol[1].mF32[3] * inV.mF32[1] + mCol[2].mF32[3] * inV.mF32[2] + mCol[3].mF32[3] * inV.mF32[3]); +#endif +} + +Vec3 Mat44::Multiply3x3(Vec3Arg inV) const +{ +#if defined(JPH_USE_SSE) + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2)))); + return Vec3::sFixW(t); +#elif defined(JPH_USE_NEON) + Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2)); + return Vec3::sFixW(t); +#else + return Vec3( + mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2], + mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2], + mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2]); +#endif +} + +Vec3 Mat44::Multiply3x3Transposed(Vec3Arg inV) const +{ +#if defined(JPH_USE_SSE4_1) + __m128 x = _mm_dp_ps(mCol[0].mValue, inV.mValue, 0x7f); + __m128 y = _mm_dp_ps(mCol[1].mValue, inV.mValue, 0x7f); + __m128 xy = _mm_blend_ps(x, y, 0b0010); + __m128 z = _mm_dp_ps(mCol[2].mValue, inV.mValue, 0x7f); + __m128 xyzz = _mm_blend_ps(xy, z, 0b1100); + return xyzz; +#else + return Transposed3x3().Multiply3x3(inV); +#endif +} + +Mat44 Mat44::Multiply3x3(Mat44Arg inM) const +{ + JPH_ASSERT(mCol[0][3] == 0.0f); + JPH_ASSERT(mCol[1][3] == 0.0f); + JPH_ASSERT(mCol[2][3] == 0.0f); + + Mat44 result; +#if defined(JPH_USE_SSE) + for (int i = 0; i < 3; ++i) + { + __m128 c = inM.mCol[i].mValue; + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2)))); + result.mCol[i].mValue = t; + } +#elif defined(JPH_USE_NEON) + for (int i = 0; i < 3; ++i) + { + Type c = inM.mCol[i].mValue; + Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2)); + result.mCol[i].mValue = t; + } +#else + for (int i = 0; i < 3; ++i) + result.mCol[i] = mCol[0] * inM.mCol[i].mF32[0] + mCol[1] * inM.mCol[i].mF32[1] + mCol[2] * inM.mCol[i].mF32[2]; +#endif + result.mCol[3] = Vec4(0, 0, 0, 1); + return result; +} + +Mat44 Mat44::Multiply3x3LeftTransposed(Mat44Arg inM) const +{ + // Transpose left hand side + Mat44 trans = Transposed3x3(); + + // Do 3x3 matrix multiply + Mat44 result; + result.mCol[0] = trans.mCol[0] * inM.mCol[0].SplatX() + trans.mCol[1] * inM.mCol[0].SplatY() + trans.mCol[2] * inM.mCol[0].SplatZ(); + result.mCol[1] = trans.mCol[0] * inM.mCol[1].SplatX() + trans.mCol[1] * inM.mCol[1].SplatY() + trans.mCol[2] * inM.mCol[1].SplatZ(); + result.mCol[2] = trans.mCol[0] * inM.mCol[2].SplatX() + trans.mCol[1] * inM.mCol[2].SplatY() + trans.mCol[2] * inM.mCol[2].SplatZ(); + result.mCol[3] = Vec4(0, 0, 0, 1); + return result; +} + +Mat44 Mat44::Multiply3x3RightTransposed(Mat44Arg inM) const +{ + JPH_ASSERT(mCol[0][3] == 0.0f); + JPH_ASSERT(mCol[1][3] == 0.0f); + JPH_ASSERT(mCol[2][3] == 0.0f); + + Mat44 result; + result.mCol[0] = mCol[0] * inM.mCol[0].SplatX() + mCol[1] * inM.mCol[1].SplatX() + mCol[2] * inM.mCol[2].SplatX(); + result.mCol[1] = mCol[0] * inM.mCol[0].SplatY() + mCol[1] * inM.mCol[1].SplatY() + mCol[2] * inM.mCol[2].SplatY(); + result.mCol[2] = mCol[0] * inM.mCol[0].SplatZ() + mCol[1] * inM.mCol[1].SplatZ() + mCol[2] * inM.mCol[2].SplatZ(); + result.mCol[3] = Vec4(0, 0, 0, 1); + return result; +} + +Mat44 Mat44::operator * (float inV) const +{ + Vec4 multiplier = Vec4::sReplicate(inV); + + Mat44 result; + for (int c = 0; c < 4; ++c) + result.mCol[c] = mCol[c] * multiplier; + return result; +} + +Mat44 &Mat44::operator *= (float inV) +{ + for (int c = 0; c < 4; ++c) + mCol[c] *= inV; + + return *this; +} + +Mat44 Mat44::operator + (Mat44Arg inM) const +{ + Mat44 result; + for (int i = 0; i < 4; ++i) + result.mCol[i] = mCol[i] + inM.mCol[i]; + return result; +} + +Mat44 Mat44::operator - () const +{ + Mat44 result; + for (int i = 0; i < 4; ++i) + result.mCol[i] = -mCol[i]; + return result; +} + +Mat44 Mat44::operator - (Mat44Arg inM) const +{ + Mat44 result; + for (int i = 0; i < 4; ++i) + result.mCol[i] = mCol[i] - inM.mCol[i]; + return result; +} + +Mat44 &Mat44::operator += (Mat44Arg inM) +{ + for (int c = 0; c < 4; ++c) + mCol[c] += inM.mCol[c]; + + return *this; +} + +void Mat44::StoreFloat4x4(Float4 *outV) const +{ + for (int c = 0; c < 4; ++c) + mCol[c].StoreFloat4(outV + c); +} + +Mat44 Mat44::Transposed() const +{ +#if defined(JPH_USE_SSE) + __m128 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(1, 0, 1, 0)); + __m128 tmp3 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(3, 2, 3, 2)); + __m128 tmp2 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(1, 0, 1, 0)); + __m128 tmp4 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(3, 2, 3, 2)); + + Mat44 result; + result.mCol[0].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(2, 0, 2, 0)); + result.mCol[1].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(3, 1, 3, 1)); + result.mCol[2].mValue = _mm_shuffle_ps(tmp3, tmp4, _MM_SHUFFLE(2, 0, 2, 0)); + result.mCol[3].mValue = _mm_shuffle_ps(tmp3, tmp4, _MM_SHUFFLE(3, 1, 3, 1)); + return result; +#elif defined(JPH_USE_NEON) + float32x4x2_t tmp1 = vzipq_f32(mCol[0].mValue, mCol[2].mValue); + float32x4x2_t tmp2 = vzipq_f32(mCol[1].mValue, mCol[3].mValue); + float32x4x2_t tmp3 = vzipq_f32(tmp1.val[0], tmp2.val[0]); + float32x4x2_t tmp4 = vzipq_f32(tmp1.val[1], tmp2.val[1]); + + Mat44 result; + result.mCol[0].mValue = tmp3.val[0]; + result.mCol[1].mValue = tmp3.val[1]; + result.mCol[2].mValue = tmp4.val[0]; + result.mCol[3].mValue = tmp4.val[1]; + return result; +#else + Mat44 result; + for (int c = 0; c < 4; ++c) + for (int r = 0; r < 4; ++r) + result.mCol[r].mF32[c] = mCol[c].mF32[r]; + return result; +#endif +} + +Mat44 Mat44::Transposed3x3() const +{ +#if defined(JPH_USE_SSE) + __m128 zero = _mm_setzero_ps(); + __m128 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(1, 0, 1, 0)); + __m128 tmp3 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(3, 2, 3, 2)); + __m128 tmp2 = _mm_shuffle_ps(mCol[2].mValue, zero, _MM_SHUFFLE(1, 0, 1, 0)); + __m128 tmp4 = _mm_shuffle_ps(mCol[2].mValue, zero, _MM_SHUFFLE(3, 2, 3, 2)); + + Mat44 result; + result.mCol[0].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(2, 0, 2, 0)); + result.mCol[1].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(3, 1, 3, 1)); + result.mCol[2].mValue = _mm_shuffle_ps(tmp3, tmp4, _MM_SHUFFLE(2, 0, 2, 0)); +#elif defined(JPH_USE_NEON) + float32x4x2_t tmp1 = vzipq_f32(mCol[0].mValue, mCol[2].mValue); + float32x4x2_t tmp2 = vzipq_f32(mCol[1].mValue, vdupq_n_f32(0)); + float32x4x2_t tmp3 = vzipq_f32(tmp1.val[0], tmp2.val[0]); + float32x4x2_t tmp4 = vzipq_f32(tmp1.val[1], tmp2.val[1]); + + Mat44 result; + result.mCol[0].mValue = tmp3.val[0]; + result.mCol[1].mValue = tmp3.val[1]; + result.mCol[2].mValue = tmp4.val[0]; +#else + Mat44 result; + for (int c = 0; c < 3; ++c) + { + for (int r = 0; r < 3; ++r) + result.mCol[c].mF32[r] = mCol[r].mF32[c]; + result.mCol[c].mF32[3] = 0; + } +#endif + result.mCol[3] = Vec4(0, 0, 0, 1); + return result; +} + +Mat44 Mat44::Inversed() const +{ +#if defined(JPH_USE_SSE) + // Algorithm from: http://download.intel.com/design/PentiumIII/sml/24504301.pdf + // Streaming SIMD Extensions - Inverse of 4x4 Matrix + // Adapted to load data using _mm_shuffle_ps instead of loading from memory + // Replaced _mm_rcp_ps with _mm_div_ps for better accuracy + + __m128 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(1, 0, 1, 0)); + __m128 row1 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(1, 0, 1, 0)); + __m128 row0 = _mm_shuffle_ps(tmp1, row1, _MM_SHUFFLE(2, 0, 2, 0)); + row1 = _mm_shuffle_ps(row1, tmp1, _MM_SHUFFLE(3, 1, 3, 1)); + tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(3, 2, 3, 2)); + __m128 row3 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(3, 2, 3, 2)); + __m128 row2 = _mm_shuffle_ps(tmp1, row3, _MM_SHUFFLE(2, 0, 2, 0)); + row3 = _mm_shuffle_ps(row3, tmp1, _MM_SHUFFLE(3, 1, 3, 1)); + + tmp1 = _mm_mul_ps(row2, row3); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1)); + __m128 minor0 = _mm_mul_ps(row1, tmp1); + __m128 minor1 = _mm_mul_ps(row0, tmp1); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2)); + minor0 = _mm_sub_ps(_mm_mul_ps(row1, tmp1), minor0); + minor1 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor1); + minor1 = _mm_shuffle_ps(minor1, minor1, _MM_SHUFFLE(1, 0, 3, 2)); + + tmp1 = _mm_mul_ps(row1, row2); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1)); + minor0 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor0); + __m128 minor3 = _mm_mul_ps(row0, tmp1); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2)); + minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row3, tmp1)); + minor3 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor3); + minor3 = _mm_shuffle_ps(minor3, minor3, _MM_SHUFFLE(1, 0, 3, 2)); + + tmp1 = _mm_mul_ps(_mm_shuffle_ps(row1, row1, _MM_SHUFFLE(1, 0, 3, 2)), row3); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1)); + row2 = _mm_shuffle_ps(row2, row2, _MM_SHUFFLE(1, 0, 3, 2)); + minor0 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor0); + __m128 minor2 = _mm_mul_ps(row0, tmp1); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2)); + minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row2, tmp1)); + minor2 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor2); + minor2 = _mm_shuffle_ps(minor2, minor2, _MM_SHUFFLE(1, 0, 3, 2)); + + tmp1 = _mm_mul_ps(row0, row1); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1)); + minor2 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor2); + minor3 = _mm_sub_ps(_mm_mul_ps(row2, tmp1), minor3); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2)); + minor2 = _mm_sub_ps(_mm_mul_ps(row3, tmp1), minor2); + minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row2, tmp1)); + + tmp1 = _mm_mul_ps(row0, row3); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1)); + minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row2, tmp1)); + minor2 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor2); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2)); + minor1 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor1); + minor2 = _mm_sub_ps(minor2, _mm_mul_ps(row1, tmp1)); + + tmp1 = _mm_mul_ps(row0, row2); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1)); + minor1 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor1); + minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row1, tmp1)); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2)); + minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row3, tmp1)); + minor3 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor3); + + __m128 det = _mm_mul_ps(row0, minor0); + det = _mm_add_ps(_mm_shuffle_ps(det, det, _MM_SHUFFLE(2, 3, 0, 1)), det); // Original code did (x + z) + (y + w), changed to (x + y) + (z + w) to match the ARM code below and make the result cross platform deterministic + det = _mm_add_ss(_mm_shuffle_ps(det, det, _MM_SHUFFLE(1, 0, 3, 2)), det); + det = _mm_div_ss(_mm_set_ss(1.0f), det); + det = _mm_shuffle_ps(det, det, _MM_SHUFFLE(0, 0, 0, 0)); + + Mat44 result; + result.mCol[0].mValue = _mm_mul_ps(det, minor0); + result.mCol[1].mValue = _mm_mul_ps(det, minor1); + result.mCol[2].mValue = _mm_mul_ps(det, minor2); + result.mCol[3].mValue = _mm_mul_ps(det, minor3); + return result; +#elif defined(JPH_USE_NEON) + // Adapted from the SSE version, there's surprising few articles about efficient ways of calculating an inverse for ARM on the internet + Type tmp1 = JPH_NEON_SHUFFLE_F32x4(mCol[0].mValue, mCol[1].mValue, 0, 1, 4, 5); + Type row1 = JPH_NEON_SHUFFLE_F32x4(mCol[2].mValue, mCol[3].mValue, 0, 1, 4, 5); + Type row0 = JPH_NEON_SHUFFLE_F32x4(tmp1, row1, 0, 2, 4, 6); + row1 = JPH_NEON_SHUFFLE_F32x4(row1, tmp1, 1, 3, 5, 7); + tmp1 = JPH_NEON_SHUFFLE_F32x4(mCol[0].mValue, mCol[1].mValue, 2, 3, 6, 7); + Type row3 = JPH_NEON_SHUFFLE_F32x4(mCol[2].mValue, mCol[3].mValue, 2, 3, 6, 7); + Type row2 = JPH_NEON_SHUFFLE_F32x4(tmp1, row3, 0, 2, 4, 6); + row3 = JPH_NEON_SHUFFLE_F32x4(row3, tmp1, 1, 3, 5, 7); + + tmp1 = vmulq_f32(row2, row3); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2); + Type minor0 = vmulq_f32(row1, tmp1); + Type minor1 = vmulq_f32(row0, tmp1); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1); + minor0 = vsubq_f32(vmulq_f32(row1, tmp1), minor0); + minor1 = vsubq_f32(vmulq_f32(row0, tmp1), minor1); + minor1 = JPH_NEON_SHUFFLE_F32x4(minor1, minor1, 2, 3, 0, 1); + + tmp1 = vmulq_f32(row1, row2); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2); + minor0 = vaddq_f32(vmulq_f32(row3, tmp1), minor0); + Type minor3 = vmulq_f32(row0, tmp1); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1); + minor0 = vsubq_f32(minor0, vmulq_f32(row3, tmp1)); + minor3 = vsubq_f32(vmulq_f32(row0, tmp1), minor3); + minor3 = JPH_NEON_SHUFFLE_F32x4(minor3, minor3, 2, 3, 0, 1); + + tmp1 = JPH_NEON_SHUFFLE_F32x4(row1, row1, 2, 3, 0, 1); + tmp1 = vmulq_f32(tmp1, row3); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2); + row2 = JPH_NEON_SHUFFLE_F32x4(row2, row2, 2, 3, 0, 1); + minor0 = vaddq_f32(vmulq_f32(row2, tmp1), minor0); + Type minor2 = vmulq_f32(row0, tmp1); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1); + minor0 = vsubq_f32(minor0, vmulq_f32(row2, tmp1)); + minor2 = vsubq_f32(vmulq_f32(row0, tmp1), minor2); + minor2 = JPH_NEON_SHUFFLE_F32x4(minor2, minor2, 2, 3, 0, 1); + + tmp1 = vmulq_f32(row0, row1); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2); + minor2 = vaddq_f32(vmulq_f32(row3, tmp1), minor2); + minor3 = vsubq_f32(vmulq_f32(row2, tmp1), minor3); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1); + minor2 = vsubq_f32(vmulq_f32(row3, tmp1), minor2); + minor3 = vsubq_f32(minor3, vmulq_f32(row2, tmp1)); + + tmp1 = vmulq_f32(row0, row3); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2); + minor1 = vsubq_f32(minor1, vmulq_f32(row2, tmp1)); + minor2 = vaddq_f32(vmulq_f32(row1, tmp1), minor2); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1); + minor1 = vaddq_f32(vmulq_f32(row2, tmp1), minor1); + minor2 = vsubq_f32(minor2, vmulq_f32(row1, tmp1)); + + tmp1 = vmulq_f32(row0, row2); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2); + minor1 = vaddq_f32(vmulq_f32(row3, tmp1), minor1); + minor3 = vsubq_f32(minor3, vmulq_f32(row1, tmp1)); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1); + minor1 = vsubq_f32(minor1, vmulq_f32(row3, tmp1)); + minor3 = vaddq_f32(vmulq_f32(row1, tmp1), minor3); + + Type det = vmulq_f32(row0, minor0); + det = vdupq_n_f32(vaddvq_f32(det)); + det = vdivq_f32(vdupq_n_f32(1.0f), det); + + Mat44 result; + result.mCol[0].mValue = vmulq_f32(det, minor0); + result.mCol[1].mValue = vmulq_f32(det, minor1); + result.mCol[2].mValue = vmulq_f32(det, minor2); + result.mCol[3].mValue = vmulq_f32(det, minor3); + return result; +#else + float m00 = JPH_EL(0, 0), m10 = JPH_EL(1, 0), m20 = JPH_EL(2, 0), m30 = JPH_EL(3, 0); + float m01 = JPH_EL(0, 1), m11 = JPH_EL(1, 1), m21 = JPH_EL(2, 1), m31 = JPH_EL(3, 1); + float m02 = JPH_EL(0, 2), m12 = JPH_EL(1, 2), m22 = JPH_EL(2, 2), m32 = JPH_EL(3, 2); + float m03 = JPH_EL(0, 3), m13 = JPH_EL(1, 3), m23 = JPH_EL(2, 3), m33 = JPH_EL(3, 3); + + float m10211120 = m10 * m21 - m11 * m20; + float m10221220 = m10 * m22 - m12 * m20; + float m10231320 = m10 * m23 - m13 * m20; + float m10311130 = m10 * m31 - m11 * m30; + float m10321230 = m10 * m32 - m12 * m30; + float m10331330 = m10 * m33 - m13 * m30; + float m11221221 = m11 * m22 - m12 * m21; + float m11231321 = m11 * m23 - m13 * m21; + float m11321231 = m11 * m32 - m12 * m31; + float m11331331 = m11 * m33 - m13 * m31; + float m12231322 = m12 * m23 - m13 * m22; + float m12331332 = m12 * m33 - m13 * m32; + float m20312130 = m20 * m31 - m21 * m30; + float m20322230 = m20 * m32 - m22 * m30; + float m20332330 = m20 * m33 - m23 * m30; + float m21322231 = m21 * m32 - m22 * m31; + float m21332331 = m21 * m33 - m23 * m31; + float m22332332 = m22 * m33 - m23 * m32; + + Vec4 col0(m11 * m22332332 - m12 * m21332331 + m13 * m21322231, -m10 * m22332332 + m12 * m20332330 - m13 * m20322230, m10 * m21332331 - m11 * m20332330 + m13 * m20312130, -m10 * m21322231 + m11 * m20322230 - m12 * m20312130); + Vec4 col1(-m01 * m22332332 + m02 * m21332331 - m03 * m21322231, m00 * m22332332 - m02 * m20332330 + m03 * m20322230, -m00 * m21332331 + m01 * m20332330 - m03 * m20312130, m00 * m21322231 - m01 * m20322230 + m02 * m20312130); + Vec4 col2(m01 * m12331332 - m02 * m11331331 + m03 * m11321231, -m00 * m12331332 + m02 * m10331330 - m03 * m10321230, m00 * m11331331 - m01 * m10331330 + m03 * m10311130, -m00 * m11321231 + m01 * m10321230 - m02 * m10311130); + Vec4 col3(-m01 * m12231322 + m02 * m11231321 - m03 * m11221221, m00 * m12231322 - m02 * m10231320 + m03 * m10221220, -m00 * m11231321 + m01 * m10231320 - m03 * m10211120, m00 * m11221221 - m01 * m10221220 + m02 * m10211120); + + float det = m00 * col0.mF32[0] + m01 * col0.mF32[1] + m02 * col0.mF32[2] + m03 * col0.mF32[3]; + + return Mat44(col0 / det, col1 / det, col2 / det, col3 / det); +#endif +} + +Mat44 Mat44::InversedRotationTranslation() const +{ + Mat44 m = Transposed3x3(); + m.SetTranslation(-m.Multiply3x3(GetTranslation())); + return m; +} + +float Mat44::GetDeterminant3x3() const +{ + return GetAxisX().Dot(GetAxisY().Cross(GetAxisZ())); +} + +Mat44 Mat44::Adjointed3x3() const +{ + return Mat44( + Vec4(JPH_EL(1, 1), JPH_EL(1, 2), JPH_EL(1, 0), 0) * Vec4(JPH_EL(2, 2), JPH_EL(2, 0), JPH_EL(2, 1), 0) + - Vec4(JPH_EL(1, 2), JPH_EL(1, 0), JPH_EL(1, 1), 0) * Vec4(JPH_EL(2, 1), JPH_EL(2, 2), JPH_EL(2, 0), 0), + Vec4(JPH_EL(0, 2), JPH_EL(0, 0), JPH_EL(0, 1), 0) * Vec4(JPH_EL(2, 1), JPH_EL(2, 2), JPH_EL(2, 0), 0) + - Vec4(JPH_EL(0, 1), JPH_EL(0, 2), JPH_EL(0, 0), 0) * Vec4(JPH_EL(2, 2), JPH_EL(2, 0), JPH_EL(2, 1), 0), + Vec4(JPH_EL(0, 1), JPH_EL(0, 2), JPH_EL(0, 0), 0) * Vec4(JPH_EL(1, 2), JPH_EL(1, 0), JPH_EL(1, 1), 0) + - Vec4(JPH_EL(0, 2), JPH_EL(0, 0), JPH_EL(0, 1), 0) * Vec4(JPH_EL(1, 1), JPH_EL(1, 2), JPH_EL(1, 0), 0), + Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::Inversed3x3() const +{ + float det = GetDeterminant3x3(); + + return Mat44( + (Vec4(JPH_EL(1, 1), JPH_EL(1, 2), JPH_EL(1, 0), 0) * Vec4(JPH_EL(2, 2), JPH_EL(2, 0), JPH_EL(2, 1), 0) + - Vec4(JPH_EL(1, 2), JPH_EL(1, 0), JPH_EL(1, 1), 0) * Vec4(JPH_EL(2, 1), JPH_EL(2, 2), JPH_EL(2, 0), 0)) / det, + (Vec4(JPH_EL(0, 2), JPH_EL(0, 0), JPH_EL(0, 1), 0) * Vec4(JPH_EL(2, 1), JPH_EL(2, 2), JPH_EL(2, 0), 0) + - Vec4(JPH_EL(0, 1), JPH_EL(0, 2), JPH_EL(0, 0), 0) * Vec4(JPH_EL(2, 2), JPH_EL(2, 0), JPH_EL(2, 1), 0)) / det, + (Vec4(JPH_EL(0, 1), JPH_EL(0, 2), JPH_EL(0, 0), 0) * Vec4(JPH_EL(1, 2), JPH_EL(1, 0), JPH_EL(1, 1), 0) + - Vec4(JPH_EL(0, 2), JPH_EL(0, 0), JPH_EL(0, 1), 0) * Vec4(JPH_EL(1, 1), JPH_EL(1, 2), JPH_EL(1, 0), 0)) / det, + Vec4(0, 0, 0, 1)); +} + +bool Mat44::SetInversed3x3(Mat44Arg inM) +{ + float det = inM.GetDeterminant3x3(); + + // If the determinant is zero the matrix is singular and we return false + if (det == 0.0f) + return false; + + // Finish calculating the inverse + *this = inM.Adjointed3x3(); + mCol[0] /= det; + mCol[1] /= det; + mCol[2] /= det; + return true; +} + +Quat Mat44::GetQuaternion() const +{ + float tr = mCol[0].mF32[0] + mCol[1].mF32[1] + mCol[2].mF32[2]; + + if (tr >= 0.0f) + { + float s = sqrt(tr + 1.0f); + float is = 0.5f / s; + return Quat( + (mCol[1].mF32[2] - mCol[2].mF32[1]) * is, + (mCol[2].mF32[0] - mCol[0].mF32[2]) * is, + (mCol[0].mF32[1] - mCol[1].mF32[0]) * is, + 0.5f * s); + } + else + { + int i = 0; + if (mCol[1].mF32[1] > mCol[0].mF32[0]) i = 1; + if (mCol[2].mF32[2] > mCol[i].mF32[i]) i = 2; + + if (i == 0) + { + float s = sqrt(mCol[0].mF32[0] - (mCol[1].mF32[1] + mCol[2].mF32[2]) + 1); + float is = 0.5f / s; + return Quat( + 0.5f * s, + (mCol[1].mF32[0] + mCol[0].mF32[1]) * is, + (mCol[0].mF32[2] + mCol[2].mF32[0]) * is, + (mCol[1].mF32[2] - mCol[2].mF32[1]) * is); + } + else if (i == 1) + { + float s = sqrt(mCol[1].mF32[1] - (mCol[2].mF32[2] + mCol[0].mF32[0]) + 1); + float is = 0.5f / s; + return Quat( + (mCol[1].mF32[0] + mCol[0].mF32[1]) * is, + 0.5f * s, + (mCol[2].mF32[1] + mCol[1].mF32[2]) * is, + (mCol[2].mF32[0] - mCol[0].mF32[2]) * is); + } + else + { + JPH_ASSERT(i == 2); + + float s = sqrt(mCol[2].mF32[2] - (mCol[0].mF32[0] + mCol[1].mF32[1]) + 1); + float is = 0.5f / s; + return Quat( + (mCol[0].mF32[2] + mCol[2].mF32[0]) * is, + (mCol[2].mF32[1] + mCol[1].mF32[2]) * is, + 0.5f * s, + (mCol[0].mF32[1] - mCol[1].mF32[0]) * is); + } + } +} + +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); +} + +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); +} + +Mat44 Mat44::GetRotation() const +{ + JPH_ASSERT(mCol[0][3] == 0.0f); + JPH_ASSERT(mCol[1][3] == 0.0f); + JPH_ASSERT(mCol[2][3] == 0.0f); + + return Mat44(mCol[0], mCol[1], mCol[2], Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::GetRotationSafe() const +{ +#if defined(JPH_USE_AVX512) + return Mat44(_mm_maskz_mov_ps(0b0111, mCol[0].mValue), + _mm_maskz_mov_ps(0b0111, mCol[1].mValue), + _mm_maskz_mov_ps(0b0111, mCol[2].mValue), + Vec4(0, 0, 0, 1)); +#elif defined(JPH_USE_SSE4_1) + __m128 zero = _mm_setzero_ps(); + return Mat44(_mm_blend_ps(mCol[0].mValue, zero, 8), + _mm_blend_ps(mCol[1].mValue, zero, 8), + _mm_blend_ps(mCol[2].mValue, zero, 8), + Vec4(0, 0, 0, 1)); +#elif defined(JPH_USE_NEON) + return Mat44(vsetq_lane_f32(0, mCol[0].mValue, 3), + vsetq_lane_f32(0, mCol[1].mValue, 3), + vsetq_lane_f32(0, mCol[2].mValue, 3), + Vec4(0, 0, 0, 1)); +#else + return Mat44(Vec4(mCol[0].mF32[0], mCol[0].mF32[1], mCol[0].mF32[2], 0), + Vec4(mCol[1].mF32[0], mCol[1].mF32[1], mCol[1].mF32[2], 0), + Vec4(mCol[2].mF32[0], mCol[2].mF32[1], mCol[2].mF32[2], 0), + Vec4(0, 0, 0, 1)); +#endif +} + +void Mat44::SetRotation(Mat44Arg inRotation) +{ + mCol[0] = inRotation.mCol[0]; + mCol[1] = inRotation.mCol[1]; + mCol[2] = inRotation.mCol[2]; +} + +Mat44 Mat44::PreTranslated(Vec3Arg inTranslation) const +{ + return Mat44(mCol[0], mCol[1], mCol[2], Vec4(GetTranslation() + Multiply3x3(inTranslation), 1)); +} + +Mat44 Mat44::PostTranslated(Vec3Arg inTranslation) const +{ + return Mat44(mCol[0], mCol[1], mCol[2], Vec4(GetTranslation() + inTranslation, 1)); +} + +Mat44 Mat44::PreScaled(Vec3Arg inScale) const +{ + return Mat44(inScale.GetX() * mCol[0], inScale.GetY() * mCol[1], inScale.GetZ() * mCol[2], mCol[3]); +} + +Mat44 Mat44::PostScaled(Vec3Arg inScale) const +{ + Vec4 scale(inScale, 1); + return Mat44(scale * mCol[0], scale * mCol[1], scale * mCol[2], scale * mCol[3]); +} + +Mat44 Mat44::Decompose(Vec3 &outScale) const +{ + // Start the modified Gram-Schmidt algorithm + // X axis will just be normalized + Vec3 x = GetAxisX(); + + // Make Y axis perpendicular to X + Vec3 y = GetAxisY(); + float x_dot_x = x.LengthSq(); + y -= (x.Dot(y) / x_dot_x) * x; + + // Make Z axis perpendicular to X + Vec3 z = GetAxisZ(); + z -= (x.Dot(z) / x_dot_x) * x; + + // Make Z axis perpendicular to Y + float y_dot_y = y.LengthSq(); + z -= (y.Dot(z) / y_dot_y) * y; + + // Determine the scale + float z_dot_z = z.LengthSq(); + outScale = Vec3(x_dot_x, y_dot_y, z_dot_z).Sqrt(); + + // If the resulting x, y and z vectors don't form a right handed matrix, flip the z axis. + if (x.Cross(y).Dot(z) < 0.0f) + outScale.SetZ(-outScale.GetZ()); + + // Determine the rotation and translation + return Mat44(Vec4(x / outScale.GetX(), 0), Vec4(y / outScale.GetY(), 0), Vec4(z / outScale.GetZ(), 0), GetColumn4(3)); +} + +#undef JPH_EL + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Math.h b/thirdparty/jolt_physics/Jolt/Math/Math.h new file mode 100644 index 000000000000..729d5403e393 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Math.h @@ -0,0 +1,205 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// The constant \f$\pi\f$ +static constexpr float JPH_PI = 3.14159265358979323846f; + +/// Convert a value from degrees to radians +JPH_INLINE constexpr float DegreesToRadians(float inV) +{ + return inV * (JPH_PI / 180.0f); +} + +/// Convert a value from radians to degrees +JPH_INLINE constexpr float RadiansToDegrees(float inV) +{ + return inV * (180.0f / JPH_PI); +} + +/// Convert angle in radians to the range \f$[-\pi, \pi]\f$ +inline float CenterAngleAroundZero(float inV) +{ + if (inV < -JPH_PI) + { + do + inV += 2.0f * JPH_PI; + while (inV < -JPH_PI); + } + else if (inV > JPH_PI) + { + do + inV -= 2.0f * JPH_PI; + while (inV > JPH_PI); + } + JPH_ASSERT(inV >= -JPH_PI && inV <= JPH_PI); + return inV; +} + +/// Clamp a value between two values +template +JPH_INLINE constexpr T Clamp(T inV, T inMin, T inMax) +{ + return min(max(inV, inMin), inMax); +} + +/// Square a value +template +JPH_INLINE constexpr T Square(T inV) +{ + return inV * inV; +} + +/// Returns \f$inV^3\f$. +template +JPH_INLINE constexpr T Cubed(T inV) +{ + return inV * inV * inV; +} + +/// Get the sign of a value +template +JPH_INLINE constexpr T Sign(T inV) +{ + return inV < 0? T(-1) : T(1); +} + +/// Check if inV is a power of 2 +template +constexpr bool IsPowerOf2(T inV) +{ + return (inV & (inV - 1)) == 0; +} + +/// Align inV up to the next inAlignment bytes +template +inline T AlignUp(T inV, uint64 inAlignment) +{ + JPH_ASSERT(IsPowerOf2(inAlignment)); + return T((uint64(inV) + inAlignment - 1) & ~(inAlignment - 1)); +} + +/// Check if inV is inAlignment aligned +template +inline bool IsAligned(T inV, uint64 inAlignment) +{ + JPH_ASSERT(IsPowerOf2(inAlignment)); + return (uint64(inV) & (inAlignment - 1)) == 0; +} + +/// Compute number of trailing zero bits (how many low bits are zero) +inline uint CountTrailingZeros(uint32 inValue) +{ +#if defined(JPH_CPU_X86) || defined(JPH_CPU_WASM) + #if defined(JPH_USE_TZCNT) + return _tzcnt_u32(inValue); + #elif defined(JPH_COMPILER_MSVC) + if (inValue == 0) + return 32; + unsigned long result; + _BitScanForward(&result, inValue); + return result; + #else + if (inValue == 0) + return 32; + return __builtin_ctz(inValue); + #endif +#elif defined(JPH_CPU_ARM) + #if defined(JPH_COMPILER_MSVC) + if (inValue == 0) + return 32; + unsigned long result; + _BitScanForward(&result, inValue); + return result; + #else + if (inValue == 0) + return 32; + return __builtin_ctz(inValue); + #endif +#elif defined(JPH_CPU_E2K) + return inValue ? __builtin_ctz(inValue) : 32; +#else + #error Undefined +#endif +} + +/// Compute the number of leading zero bits (how many high bits are zero) +inline uint CountLeadingZeros(uint32 inValue) +{ +#if defined(JPH_CPU_X86) || defined(JPH_CPU_WASM) + #if defined(JPH_USE_LZCNT) + return _lzcnt_u32(inValue); + #elif defined(JPH_COMPILER_MSVC) + if (inValue == 0) + return 32; + unsigned long result; + _BitScanReverse(&result, inValue); + return 31 - result; + #else + if (inValue == 0) + return 32; + return __builtin_clz(inValue); + #endif +#elif defined(JPH_CPU_ARM) + #if defined(JPH_COMPILER_MSVC) + return _CountLeadingZeros(inValue); + #else + return __builtin_clz(inValue); + #endif +#elif defined(JPH_CPU_E2K) + return inValue ? __builtin_clz(inValue) : 32; +#else + #error Undefined +#endif +} + +/// Count the number of 1 bits in a value +inline uint CountBits(uint32 inValue) +{ +#if defined(JPH_COMPILER_CLANG) || defined(JPH_COMPILER_GCC) + return __builtin_popcount(inValue); +#elif defined(JPH_COMPILER_MSVC) + #if defined(JPH_USE_SSE4_2) + return _mm_popcnt_u32(inValue); + #elif defined(JPH_USE_NEON) && (_MSC_VER >= 1930) // _CountOneBits not available on MSVC2019 + return _CountOneBits(inValue); + #else + inValue = inValue - ((inValue >> 1) & 0x55555555); + inValue = (inValue & 0x33333333) + ((inValue >> 2) & 0x33333333); + inValue = (inValue + (inValue >> 4)) & 0x0F0F0F0F; + return (inValue * 0x01010101) >> 24; + #endif +#else + #error Undefined +#endif +} + +/// Get the next higher power of 2 of a value, or the value itself if the value is already a power of 2 +inline uint32 GetNextPowerOf2(uint32 inValue) +{ + return inValue <= 1? uint32(1) : uint32(1) << (32 - CountLeadingZeros(inValue - 1)); +} + +// Simple implementation of C++20 std::bit_cast (unfortunately not constexpr) +template +JPH_INLINE To BitCast(const From &inValue) +{ + static_assert(std::is_trivially_constructible_v); + static_assert(sizeof(From) == sizeof(To)); + + union FromTo + { + To mTo; + From mFrom; + }; + + FromTo convert; + convert.mFrom = inValue; + return convert.mTo; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/MathTypes.h b/thirdparty/jolt_physics/Jolt/Math/MathTypes.h new file mode 100644 index 000000000000..e58dc9dabec9 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/MathTypes.h @@ -0,0 +1,32 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +class Vec3; +class DVec3; +class Vec4; +class UVec4; +class BVec16; +class Quat; +class Mat44; +class DMat44; + +// Types to use for passing arguments to functions +using Vec3Arg = const Vec3; +#ifdef JPH_USE_AVX + using DVec3Arg = const DVec3; +#else + using DVec3Arg = const DVec3 &; +#endif +using Vec4Arg = const Vec4; +using UVec4Arg = const UVec4; +using BVec16Arg = const BVec16; +using QuatArg = const Quat; +using Mat44Arg = const Mat44 &; +using DMat44Arg = const DMat44 &; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Matrix.h b/thirdparty/jolt_physics/Jolt/Math/Matrix.h new file mode 100644 index 000000000000..031665bbe718 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Matrix.h @@ -0,0 +1,259 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Templatized matrix class +template +class [[nodiscard]] Matrix +{ +public: + /// Constructor + inline Matrix() = default; + inline Matrix(const Matrix &inM2) { *this = inM2; } + + /// Dimensions + inline uint GetRows() const { return Rows; } + inline uint GetCols() const { return Cols; } + + /// Zero matrix + inline void SetZero() + { + for (uint c = 0; c < Cols; ++c) + mCol[c].SetZero(); + } + + inline static Matrix sZero() { Matrix m; m.SetZero(); return m; } + + /// Check if this matrix consists of all zeros + inline bool IsZero() const + { + for (uint c = 0; c < Cols; ++c) + if (!mCol[c].IsZero()) + return false; + + return true; + } + + /// Identity matrix + inline void SetIdentity() + { + // Clear matrix + SetZero(); + + // Set diagonal to 1 + for (uint rc = 0, min_rc = min(Rows, Cols); rc < min_rc; ++rc) + mCol[rc].mF32[rc] = 1.0f; + } + + inline static Matrix sIdentity() { Matrix m; m.SetIdentity(); return m; } + + /// Check if this matrix is identity + bool IsIdentity() const { return *this == sIdentity(); } + + /// Diagonal matrix + inline void SetDiagonal(const Vector &inV) + { + // Clear matrix + SetZero(); + + // Set diagonal + for (uint rc = 0, min_rc = min(Rows, Cols); rc < min_rc; ++rc) + mCol[rc].mF32[rc] = inV[rc]; + } + + inline static Matrix sDiagonal(const Vector &inV) + { + Matrix m; + m.SetDiagonal(inV); + return m; + } + + /// Copy a (part) of another matrix into this matrix + template + void CopyPart(const OtherMatrix &inM, uint inSourceRow, uint inSourceCol, uint inNumRows, uint inNumCols, uint inDestRow, uint inDestCol) + { + for (uint c = 0; c < inNumCols; ++c) + for (uint r = 0; r < inNumRows; ++r) + mCol[inDestCol + c].mF32[inDestRow + r] = inM(inSourceRow + r, inSourceCol + c); + } + + /// Get float component by element index + inline float operator () (uint inRow, uint inColumn) const + { + JPH_ASSERT(inRow < Rows); + JPH_ASSERT(inColumn < Cols); + return mCol[inColumn].mF32[inRow]; + } + + inline float & operator () (uint inRow, uint inColumn) + { + JPH_ASSERT(inRow < Rows); + JPH_ASSERT(inColumn < Cols); + return mCol[inColumn].mF32[inRow]; + } + + /// Comparison + inline bool operator == (const Matrix &inM2) const + { + for (uint c = 0; c < Cols; ++c) + if (mCol[c] != inM2.mCol[c]) + return false; + return true; + } + + inline bool operator != (const Matrix &inM2) const + { + for (uint c = 0; c < Cols; ++c) + if (mCol[c] != inM2.mCol[c]) + return true; + return false; + } + + /// Assignment + inline Matrix & operator = (const Matrix &inM2) + { + for (uint c = 0; c < Cols; ++c) + mCol[c] = inM2.mCol[c]; + return *this; + } + + /// Multiply matrix by matrix + template + inline Matrix operator * (const Matrix &inM) const + { + Matrix m; + for (uint c = 0; c < OtherCols; ++c) + for (uint r = 0; r < Rows; ++r) + { + float dot = 0.0f; + for (uint i = 0; i < Cols; ++i) + dot += mCol[i].mF32[r] * inM.mCol[c].mF32[i]; + m.mCol[c].mF32[r] = dot; + } + return m; + } + + /// Multiply vector by matrix + inline Vector operator * (const Vector &inV) const + { + Vector v; + for (uint r = 0; r < Rows; ++r) + { + float dot = 0.0f; + for (uint c = 0; c < Cols; ++c) + dot += mCol[c].mF32[r] * inV.mF32[c]; + v.mF32[r] = dot; + } + return v; + } + + /// Multiply matrix with float + inline Matrix operator * (float inV) const + { + Matrix m; + for (uint c = 0; c < Cols; ++c) + m.mCol[c] = mCol[c] * inV; + return m; + } + + inline friend Matrix operator * (float inV, const Matrix &inM) + { + return inM * inV; + } + + /// Per element addition of matrix + inline Matrix operator + (const Matrix &inM) const + { + Matrix m; + for (uint c = 0; c < Cols; ++c) + m.mCol[c] = mCol[c] + inM.mCol[c]; + return m; + } + + /// Per element subtraction of matrix + inline Matrix operator - (const Matrix &inM) const + { + Matrix m; + for (uint c = 0; c < Cols; ++c) + m.mCol[c] = mCol[c] - inM.mCol[c]; + return m; + } + + /// Transpose matrix + inline Matrix Transposed() const + { + Matrix m; + for (uint r = 0; r < Rows; ++r) + for (uint c = 0; c < Cols; ++c) + m.mCol[r].mF32[c] = mCol[c].mF32[r]; + return m; + } + + /// Inverse matrix + bool SetInversed(const Matrix &inM) + { + if constexpr (Rows != Cols) JPH_ASSERT(false); + Matrix copy(inM); + SetIdentity(); + return GaussianElimination(copy, *this); + } + + inline Matrix Inversed() const + { + Matrix m; + m.SetInversed(*this); + return m; + } + + /// To String + friend ostream & operator << (ostream &inStream, const Matrix &inM) + { + for (uint i = 0; i < Cols - 1; ++i) + inStream << inM.mCol[i] << ", "; + inStream << inM.mCol[Cols - 1]; + return inStream; + } + + /// Column access + const Vector & GetColumn(int inIdx) const { return mCol[inIdx]; } + Vector & GetColumn(int inIdx) { return mCol[inIdx]; } + + Vector mCol[Cols]; ///< Column +}; + +// The template specialization doesn't sit well with Doxygen +#ifndef JPH_PLATFORM_DOXYGEN + +/// Specialization of SetInversed for 2x2 matrix +template <> +inline bool Matrix<2, 2>::SetInversed(const Matrix<2, 2> &inM) +{ + // Fetch elements + float a = inM.mCol[0].mF32[0]; + float b = inM.mCol[1].mF32[0]; + float c = inM.mCol[0].mF32[1]; + float d = inM.mCol[1].mF32[1]; + + // Calculate determinant + float det = a * d - b * c; + if (det == 0.0f) + return false; + + // Construct inverse + mCol[0].mF32[0] = d / det; + mCol[1].mF32[0] = -b / det; + mCol[0].mF32[1] = -c / det; + mCol[1].mF32[1] = a / det; + return true; +} + +#endif // !JPH_PLATFORM_DOXYGEN + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Quat.h b/thirdparty/jolt_physics/Jolt/Math/Quat.h new file mode 100644 index 000000000000..a68e45be294f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Quat.h @@ -0,0 +1,255 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Quaternion class, quaternions are 4 dimensional vectors which can describe rotations in 3 dimensional +/// space if their length is 1. +/// +/// They are written as: +/// +/// \f$q = w + x \: i + y \: j + z \: k\f$ +/// +/// or in vector notation: +/// +/// \f$q = [w, v] = [w, x, y, z]\f$ +/// +/// Where: +/// +/// w = the real part +/// v = the imaginary part, (x, y, z) +/// +/// Note that we store the quaternion in a Vec4 as [x, y, z, w] because that makes +/// it easy to extract the rotation axis of the quaternion: +/// +/// q = [cos(angle / 2), sin(angle / 2) * rotation_axis] +class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Quat +{ +public: + JPH_OVERRIDE_NEW_DELETE + + ///@name Constructors + ///@{ + inline Quat() = default; ///< Intentionally not initialized for performance reasons + 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(Vec4Arg inV) : mValue(inV) { } + ///@} + + ///@name Tests + ///@{ + + /// Check if two quaternions are exactly equal + inline bool operator == (QuatArg inRHS) const { return mValue == inRHS.mValue; } + + /// Check if two quaternions are different + inline bool operator != (QuatArg inRHS) const { return mValue != inRHS.mValue; } + + /// If this quaternion is close to inRHS. Note that q and -q represent the same rotation, this is not checked here. + inline bool IsClose(QuatArg inRHS, float inMaxDistSq = 1.0e-12f) const { return mValue.IsClose(inRHS.mValue, inMaxDistSq); } + + /// If the length of this quaternion is 1 +/- inTolerance + inline bool IsNormalized(float inTolerance = 1.0e-5f) const { return mValue.IsNormalized(inTolerance); } + + /// If any component of this quaternion is a NaN (not a number) + inline bool IsNaN() const { return mValue.IsNaN(); } + + ///@} + ///@name Get components + ///@{ + + /// Get X component (imaginary part i) + JPH_INLINE float GetX() const { return mValue.GetX(); } + + /// Get Y component (imaginary part j) + JPH_INLINE float GetY() const { return mValue.GetY(); } + + /// Get Z component (imaginary part k) + JPH_INLINE float GetZ() const { return mValue.GetZ(); } + + /// Get W component (real part) + JPH_INLINE float GetW() const { return mValue.GetW(); } + + /// Get the imaginary part of the quaternion + JPH_INLINE Vec3 GetXYZ() const { return Vec3(mValue); } + + /// Get the quaternion as a Vec4 + JPH_INLINE Vec4 GetXYZW() const { return mValue; } + + /// Set individual components + JPH_INLINE void SetX(float inX) { mValue.SetX(inX); } + JPH_INLINE void SetY(float inY) { mValue.SetY(inY); } + JPH_INLINE void SetZ(float inZ) { mValue.SetZ(inZ); } + JPH_INLINE void SetW(float inW) { mValue.SetW(inW); } + + /// Set all components + JPH_INLINE void Set(float inX, float inY, float inZ, float inW) { mValue.Set(inX, inY, inZ, inW); } + + ///@} + ///@name Default quaternions + ///@{ + + /// @return [0, 0, 0, 0] + JPH_INLINE static Quat sZero() { return Quat(Vec4::sZero()); } + + /// @return [1, 0, 0, 0] (or in storage format Quat(0, 0, 0, 1)) + JPH_INLINE static Quat sIdentity() { return Quat(0, 0, 0, 1); } + + ///@} + + /// Rotation from axis and angle + JPH_INLINE static Quat sRotation(Vec3Arg inAxis, float inAngle); + + /// Get axis and angle that represents this quaternion, outAngle will always be in the range \f$[0, \pi]\f$ + JPH_INLINE void GetAxisAngle(Vec3 &outAxis, float &outAngle) const; + + /// Create quaternion that rotates a vector from the direction of inFrom to the direction of inTo along the shortest path + /// @see https://www.euclideanspace.com/maths/algebra/vectors/angleBetween/index.htm + JPH_INLINE static Quat sFromTo(Vec3Arg inFrom, Vec3Arg inTo); + + /// Random unit quaternion + template + inline static Quat sRandom(Random &inRandom); + + /// Conversion from Euler angles. Rotation order is X then Y then Z (RotZ * RotY * RotX). Angles in radians. + inline static Quat sEulerAngles(Vec3Arg inAngles); + + /// Conversion to Euler angles. Rotation order is X then Y then Z (RotZ * RotY * RotX). Angles in radians. + inline Vec3 GetEulerAngles() const; + + ///@name Length / normalization operations + ///@{ + + /// Squared length of quaternion. + /// @return Squared length of quaternion (\f$|v|^2\f$) + JPH_INLINE float LengthSq() const { return mValue.LengthSq(); } + + /// Length of quaternion. + /// @return Length of quaternion (\f$|v|\f$) + JPH_INLINE float Length() const { return mValue.Length(); } + + /// Normalize the quaternion (make it length 1) + JPH_INLINE Quat Normalized() const { return Quat(mValue.Normalized()); } + + ///@} + ///@name Additions / multiplications + ///@{ + + JPH_INLINE void operator += (QuatArg inRHS) { mValue += inRHS.mValue; } + JPH_INLINE void operator -= (QuatArg inRHS) { mValue -= inRHS.mValue; } + JPH_INLINE void operator *= (float inValue) { mValue *= inValue; } + JPH_INLINE void operator /= (float inValue) { mValue /= inValue; } + JPH_INLINE Quat operator - () const { return Quat(-mValue); } + JPH_INLINE Quat operator + (QuatArg inRHS) const { return Quat(mValue + inRHS.mValue); } + JPH_INLINE Quat operator - (QuatArg inRHS) const { return Quat(mValue - inRHS.mValue); } + JPH_INLINE Quat operator * (QuatArg inRHS) const; + JPH_INLINE Quat operator * (float inValue) const { return Quat(mValue * inValue); } + inline friend Quat operator * (float inValue, QuatArg inRHS) { return Quat(inRHS.mValue * inValue); } + JPH_INLINE Quat operator / (float inValue) const { return Quat(mValue / inValue); } + + ///@} + + /// Rotate a vector by this quaternion + JPH_INLINE Vec3 operator * (Vec3Arg inValue) const; + + /// Rotate a vector by the inverse of this quaternion + JPH_INLINE Vec3 InverseRotate(Vec3Arg inValue) const; + + /// Rotate a the vector (1, 0, 0) with this quaternion + JPH_INLINE Vec3 RotateAxisX() const; + + /// Rotate a the vector (0, 1, 0) with this quaternion + JPH_INLINE Vec3 RotateAxisY() const; + + /// Rotate a the vector (0, 0, 1) with this quaternion + JPH_INLINE Vec3 RotateAxisZ() const; + + /// Dot product + 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())); } + + /// Get inverse quaternion + JPH_INLINE Quat Inversed() const { return Conjugated() / Length(); } + + /// Ensures that the W component is positive by negating the entire quaternion if it is not. This is useful when you want to store a quaternion as a 3 vector by discarding W and reconstructing it as sqrt(1 - x^2 - y^2 - z^2). + 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()); } + + /// 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()); } + + /// Swing Twist Decomposition: any quaternion can be split up as: + /// + /// \f[q = q_{swing} \: q_{twist}\f] + /// + /// where \f$q_{twist}\f$ rotates only around axis v. + /// + /// \f$q_{twist}\f$ is: + /// + /// \f[q_{twist} = \frac{[q_w, q_{ijk} \cdot v \: v]}{\left|[q_w, q_{ijk} \cdot v \: v]\right|}\f] + /// + /// where q_w is the real part of the quaternion and q_i the imaginary part (a 3 vector). + /// + /// The swing can then be calculated as: + /// + /// \f[q_{swing} = q \: q_{twist}^* \f] + /// + /// Where \f$q_{twist}^*\f$ = complex conjugate of \f$q_{twist}\f$ + JPH_INLINE Quat GetTwist(Vec3Arg inAxis) const; + + /// Decomposes quaternion into swing and twist component: + /// + /// \f$q = q_{swing} \: q_{twist}\f$ + /// + /// where \f$q_{swing} \: \hat{x} = q_{twist} \: \hat{y} = q_{twist} \: \hat{z} = 0\f$ + /// + /// In other words: + /// + /// - \f$q_{twist}\f$ only rotates around the X-axis. + /// - \f$q_{swing}\f$ only rotates around the Y and Z-axis. + /// + /// @see Gino van den Bergen - Rotational Joint Limits in Quaternion Space - GDC 2016 + JPH_INLINE void GetSwingTwist(Quat &outSwing, Quat &outTwist) const; + + /// Linear interpolation between two quaternions (for small steps). + /// @param inFraction is in the range [0, 1] + /// @param inDestination The destination quaternion + /// @return (1 - inFraction) * this + fraction * inDestination + JPH_INLINE Quat LERP(QuatArg inDestination, float inFraction) const; + + /// Spherical linear interpolation between two quaternions. + /// @param inFraction is in the range [0, 1] + /// @param inDestination The destination quaternion + /// @return When fraction is zero this quaternion is returned, when fraction is 1 inDestination is returned. + /// When fraction is between 0 and 1 an interpolation along the shortest path is returned. + JPH_INLINE Quat SLERP(QuatArg inDestination, float inFraction) const; + + /// 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) + JPH_INLINE void StoreFloat3(Float3 *outV) const; + + /// To String + friend ostream & operator << (ostream &inStream, QuatArg inQ) { inStream << inQ.mValue; return inStream; } + + /// 4 vector that stores [x, y, z, w] parts of the quaternion + Vec4 mValue; +}; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "Quat.inl" diff --git a/thirdparty/jolt_physics/Jolt/Math/Quat.inl b/thirdparty/jolt_physics/Jolt/Math/Quat.inl new file mode 100644 index 000000000000..201481929d55 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Quat.inl @@ -0,0 +1,328 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +JPH_NAMESPACE_BEGIN + +Quat Quat::operator * (QuatArg inRHS) const +{ +#if defined(JPH_USE_SSE4_1) + // Taken from: http://momchil-velikov.blogspot.nl/2013/10/fast-sse-quternion-multiplication.html + __m128 abcd = mValue.mValue; + __m128 xyzw = inRHS.mValue.mValue; + + __m128 t0 = _mm_shuffle_ps(abcd, abcd, _MM_SHUFFLE(3, 3, 3, 3)); + __m128 t1 = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(2, 3, 0, 1)); + + __m128 t3 = _mm_shuffle_ps(abcd, abcd, _MM_SHUFFLE(0, 0, 0, 0)); + __m128 t4 = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(1, 0, 3, 2)); + + __m128 t5 = _mm_shuffle_ps(abcd, abcd, _MM_SHUFFLE(1, 1, 1, 1)); + __m128 t6 = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(2, 0, 3, 1)); + + // [d,d,d,d] * [z,w,x,y] = [dz,dw,dx,dy] + __m128 m0 = _mm_mul_ps(t0, t1); + + // [a,a,a,a] * [y,x,w,z] = [ay,ax,aw,az] + __m128 m1 = _mm_mul_ps(t3, t4); + + // [b,b,b,b] * [z,x,w,y] = [bz,bx,bw,by] + __m128 m2 = _mm_mul_ps(t5, t6); + + // [c,c,c,c] * [w,z,x,y] = [cw,cz,cx,cy] + __m128 t7 = _mm_shuffle_ps(abcd, abcd, _MM_SHUFFLE(2, 2, 2, 2)); + __m128 t8 = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 2, 0, 1)); + __m128 m3 = _mm_mul_ps(t7, t8); + + // [dz,dw,dx,dy] + -[ay,ax,aw,az] = [dz+ay,dw-ax,dx+aw,dy-az] + __m128 e = _mm_addsub_ps(m0, m1); + + // [dx+aw,dz+ay,dy-az,dw-ax] + e = _mm_shuffle_ps(e, e, _MM_SHUFFLE(1, 3, 0, 2)); + + // [dx+aw,dz+ay,dy-az,dw-ax] + -[bz,bx,bw,by] = [dx+aw+bz,dz+ay-bx,dy-az+bw,dw-ax-by] + e = _mm_addsub_ps(e, m2); + + // [dz+ay-bx,dw-ax-by,dy-az+bw,dx+aw+bz] + e = _mm_shuffle_ps(e, e, _MM_SHUFFLE(2, 0, 1, 3)); + + // [dz+ay-bx,dw-ax-by,dy-az+bw,dx+aw+bz] + -[cw,cz,cx,cy] = [dz+ay-bx+cw,dw-ax-by-cz,dy-az+bw+cx,dx+aw+bz-cy] + e = _mm_addsub_ps(e, m3); + + // [dw-ax-by-cz,dz+ay-bx+cw,dy-az+bw+cx,dx+aw+bz-cy] + return Quat(Vec4(_mm_shuffle_ps(e, e, _MM_SHUFFLE(2, 3, 1, 0)))); +#else + float lx = mValue.GetX(); + float ly = mValue.GetY(); + float lz = mValue.GetZ(); + float lw = mValue.GetW(); + + float rx = inRHS.mValue.GetX(); + float ry = inRHS.mValue.GetY(); + float rz = inRHS.mValue.GetZ(); + float rw = inRHS.mValue.GetW(); + + float x = lw * rx + lx * rw + ly * rz - lz * ry; + float y = lw * ry - lx * rz + ly * rw + lz * rx; + float z = lw * rz + lx * ry - ly * rx + lz * rw; + float w = lw * rw - 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)] + JPH_ASSERT(inAxis.IsNormalized()); + Vec4 s, c; + Vec4::sReplicate(0.5f * inAngle).SinCos(s, c); + return Quat(Vec4::sSelect(Vec4(inAxis) * s, c, UVec4(0, 0, 0, 0xffffffffU))); +} + +void Quat::GetAxisAngle(Vec3 &outAxis, float &outAngle) const +{ + JPH_ASSERT(IsNormalized()); + Quat w_pos = EnsureWPositive(); + float abs_w = w_pos.GetW(); + if (abs_w >= 1.0f) + { + outAxis = Vec3::sZero(); + outAngle = 0.0f; + } + else + { + outAngle = 2.0f * ACos(abs_w); + outAxis = w_pos.GetXYZ().NormalizedOr(Vec3::sZero()); + } +} + +Quat Quat::sFromTo(Vec3Arg inFrom, Vec3Arg inTo) +{ + /* + Uses (inFrom = v1, inTo = v2): + + angle = arcos(v1 . v2 / |v1||v2|) + axis = normalize(v1 x v2) + + Quaternion is then: + + s = sin(angle / 2) + x = axis.x * s + y = axis.y * s + z = axis.z * s + w = cos(angle / 2) + + Using identities: + + sin(2 * a) = 2 * sin(a) * cos(a) + cos(2 * a) = cos(a)^2 - sin(a)^2 + sin(a)^2 + cos(a)^2 = 1 + + This reduces to: + + x = (v1 x v2).x + y = (v1 x v2).y + z = (v1 x v2).z + w = |v1||v2| + v1 . v2 + + which then needs to be normalized because the whole equation was multiplied by 2 cos(angle / 2) + */ + + float len_v1_v2 = sqrt(inFrom.LengthSq() * inTo.LengthSq()); + float w = len_v1_v2 + inFrom.Dot(inTo); + + if (w == 0.0f) + { + if (len_v1_v2 == 0.0f) + { + // If either of the vectors has zero length, there is no rotation and we return identity + return Quat::sIdentity(); + } + else + { + // If vectors are perpendicular, take one of the many 180 degree rotations that exist + return Quat(Vec4(inFrom.GetNormalizedPerpendicular(), 0)); + } + } + + Vec3 v = inFrom.Cross(inTo); + return Quat(Vec4(v, w)).Normalized(); +} + +template +Quat Quat::sRandom(Random &inRandom) +{ + std::uniform_real_distribution zero_to_one(0.0f, 1.0f); + float x0 = zero_to_one(inRandom); + float r1 = sqrt(1.0f - x0), r2 = sqrt(x0); + std::uniform_real_distribution zero_to_two_pi(0.0f, 2.0f * JPH_PI); + Vec4 s, c; + Vec4(zero_to_two_pi(inRandom), zero_to_two_pi(inRandom), 0, 0).SinCos(s, c); + return Quat(s.GetX() * r1, c.GetX() * r1, s.GetY() * r2, c.GetY() * r2); +} + +Quat Quat::sEulerAngles(Vec3Arg inAngles) +{ + Vec4 half(0.5f * inAngles); + Vec4 s, c; + half.SinCos(s, c); + + float cx = c.GetX(); + float sx = s.GetX(); + float cy = c.GetY(); + float sy = s.GetY(); + float cz = c.GetZ(); + float sz = s.GetZ(); + + return Quat( + cz * sx * cy - sz * cx * sy, + cz * cx * sy + sz * sx * cy, + sz * cx * cy - cz * sx * sy, + cz * cx * cy + sz * sx * sy); +} + +Vec3 Quat::GetEulerAngles() const +{ + float y_sq = GetY() * GetY(); + + // X + float t0 = 2.0f * (GetW() * GetX() + GetY() * GetZ()); + float t1 = 1.0f - 2.0f * (GetX() * GetX() + y_sq); + + // Y + float t2 = 2.0f * (GetW() * GetY() - GetZ() * GetX()); + t2 = t2 > 1.0f? 1.0f : t2; + t2 = t2 < -1.0f? -1.0f : t2; + + // Z + float t3 = 2.0f * (GetW() * GetZ() + GetX() * GetY()); + float t4 = 1.0f - 2.0f * (y_sq + GetZ() * GetZ()); + + return Vec3(ATan2(t0, t1), ASin(t2), ATan2(t3, t4)); +} + +Quat Quat::GetTwist(Vec3Arg inAxis) const +{ + Quat twist(Vec4(GetXYZ().Dot(inAxis) * inAxis, GetW())); + float twist_len = twist.LengthSq(); + if (twist_len != 0.0f) + return twist / sqrt(twist_len); + else + return Quat::sIdentity(); +} + +void Quat::GetSwingTwist(Quat &outSwing, Quat &outTwist) const +{ + float x = GetX(), y = GetY(), z = GetZ(), w = GetW(); + float s = sqrt(Square(w) + Square(x)); + if (s != 0.0f) + { + outTwist = Quat(x / s, 0, 0, w / s); + outSwing = Quat(0, (w * y - x * z) / s, (w * z + x * y) / s, s); + } + else + { + // If both x and w are zero, this must be a 180 degree rotation around either y or z + outTwist = Quat::sIdentity(); + outSwing = *this; + } +} + +Quat Quat::LERP(QuatArg inDestination, float inFraction) const +{ + float scale0 = 1.0f - inFraction; + return Quat(Vec4::sReplicate(scale0) * mValue + Vec4::sReplicate(inFraction) * inDestination.mValue); +} + +Quat Quat::SLERP(QuatArg inDestination, float inFraction) const +{ + // Difference at which to LERP instead of SLERP + const float delta = 0.0001f; + + // Calc cosine + float sign_scale1 = 1.0f; + float cos_omega = Dot(inDestination); + + // Adjust signs (if necessary) + if (cos_omega < 0.0f) + { + cos_omega = -cos_omega; + sign_scale1 = -1.0f; + } + + // Calculate coefficients + float scale0, scale1; + if (1.0f - cos_omega > delta) + { + // Standard case (slerp) + float omega = ACos(cos_omega); + float sin_omega = Sin(omega); + scale0 = Sin((1.0f - inFraction) * omega) / sin_omega; + scale1 = sign_scale1 * Sin(inFraction * omega) / sin_omega; + } + else + { + // Quaternions are very close so we can do a linear interpolation + scale0 = 1.0f - inFraction; + scale1 = sign_scale1 * inFraction; + } + + // Interpolate between the two quaternions + return Quat(Vec4::sReplicate(scale0) * mValue + Vec4::sReplicate(scale1) * inDestination.mValue).Normalized(); +} + +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) + JPH_ASSERT(IsNormalized()); + return Vec3((*this * Quat(Vec4(inValue, 0)) * Conjugated()).mValue); +} + +Vec3 Quat::InverseRotate(Vec3Arg inValue) const +{ + JPH_ASSERT(IsNormalized()); + return Vec3((Conjugated() * Quat(Vec4(inValue, 0)) * *this).mValue); +} + +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); +} + +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); +} + +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); +} + +void Quat::StoreFloat3(Float3 *outV) const +{ + JPH_ASSERT(IsNormalized()); + EnsureWPositive().GetXYZ().StoreFloat3(outV); +} + +Quat Quat::sLoadFloat3Unsafe(const Float3 &inV) +{ + Vec3 v = Vec3::sLoadFloat3Unsafe(inV); + float w = sqrt(max(1.0f - v.LengthSq(), 0.0f)); // It is possible that the length of v is a fraction above 1, and we don't want to introduce NaN's in that case so we clamp to 0 + return Quat(Vec4(v, w)); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Real.h b/thirdparty/jolt_physics/Jolt/Math/Real.h new file mode 100644 index 000000000000..ca8cf5049eb1 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Real.h @@ -0,0 +1,44 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_DOUBLE_PRECISION + +// Define real to double +using Real = double; +using Real3 = Double3; +using RVec3 = DVec3; +using RVec3Arg = DVec3Arg; +using RMat44 = DMat44; +using RMat44Arg = DMat44Arg; + +#define JPH_RVECTOR_ALIGNMENT JPH_DVECTOR_ALIGNMENT + +#else + +// Define real to float +using Real = float; +using Real3 = Float3; +using RVec3 = Vec3; +using RVec3Arg = Vec3Arg; +using RMat44 = Mat44; +using RMat44Arg = Mat44Arg; + +#define JPH_RVECTOR_ALIGNMENT JPH_VECTOR_ALIGNMENT + +#endif // JPH_DOUBLE_PRECISION + +// Put the 'real' operator in a namespace so that users can opt in to use it: +// using namespace JPH::literals; +namespace literals { + constexpr Real operator ""_r (long double inValue) { return Real(inValue); } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Swizzle.h b/thirdparty/jolt_physics/Jolt/Math/Swizzle.h new file mode 100644 index 000000000000..ad8dfbc144a5 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Swizzle.h @@ -0,0 +1,19 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Enum indicating which component to use when swizzling +enum +{ + SWIZZLE_X = 0, ///< Use the X component + SWIZZLE_Y = 1, ///< Use the Y component + SWIZZLE_Z = 2, ///< Use the Z component + SWIZZLE_W = 3, ///< Use the W component + SWIZZLE_UNUSED = 2, ///< We always use the Z component when we don't specifically want to initialize a value, this is consistent with what is done in Vec3(x, y, z), Vec3(Float3 &) and Vec3::sLoadFloat3Unsafe +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Trigonometry.h b/thirdparty/jolt_physics/Jolt/Math/Trigonometry.h new file mode 100644 index 000000000000..3503dd17bd49 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Trigonometry.h @@ -0,0 +1,79 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +// Note that this file exists because std::sin etc. are not platform independent and will lead to non-deterministic simulation + +/// Sine of x (input in radians) +JPH_INLINE float Sin(float inX) +{ + Vec4 s, c; + Vec4::sReplicate(inX).SinCos(s, c); + return s.GetX(); +} + +/// Cosine of x (input in radians) +JPH_INLINE float Cos(float inX) +{ + Vec4 s, c; + Vec4::sReplicate(inX).SinCos(s, c); + return c.GetX(); +} + +/// Tangent of x (input in radians) +JPH_INLINE float Tan(float inX) +{ + return Vec4::sReplicate(inX).Tan().GetX(); +} + +/// Arc sine of x (returns value in the range [-PI / 2, PI / 2]) +/// Note that all input values will be clamped to the range [-1, 1] and this function will not return NaNs like std::asin +JPH_INLINE float ASin(float inX) +{ + return Vec4::sReplicate(inX).ASin().GetX(); +} + +/// Arc cosine of x (returns value in the range [0, PI]) +/// Note that all input values will be clamped to the range [-1, 1] and this function will not return NaNs like std::acos +JPH_INLINE float ACos(float inX) +{ + return Vec4::sReplicate(inX).ACos().GetX(); +} + +/// An approximation of ACos, max error is 4.2e-3 over the entire range [-1, 1], is approximately 2.5x faster than ACos +JPH_INLINE float ACosApproximate(float inX) +{ + // See: https://www.johndcook.com/blog/2022/09/06/inverse-cosine-near-1/ + // See also: https://seblagarde.wordpress.com/2014/12/01/inverse-trigonometric-functions-gpu-optimization-for-amd-gcn-architecture/ + // Taylor of cos(x) = 1 - x^2 / 2 + ... + // Substitute x = sqrt(2 y) we get: cos(sqrt(2 y)) = 1 - y + // Substitute z = 1 - y we get: cos(sqrt(2 (1 - z))) = z <=> acos(z) = sqrt(2 (1 - z)) + // To avoid the discontinuity at 1, instead of using the Taylor expansion of acos(x) we use acos(x) / sqrt(2 (1 - x)) = 1 + (1 - x) / 12 + ... + // Since the approximation was made at 1, it has quite a large error at 0 meaning that if we want to extend to the + // range [-1, 1] by mirroring the range [0, 1], the value at 0+ is not the same as 0-. + // So we observe that the form of the Taylor expansion is f(x) = sqrt(1 - x) * (a + b x) and we fit the function so that f(0) = pi / 2 + // this gives us a = pi / 2. f(1) = 0 regardless of b. We search for a constant b that minimizes the error in the range [0, 1]. + float abs_x = min(abs(inX), 1.0f); // Ensure that we don't get a value larger than 1 + float val = sqrt(1.0f - abs_x) * (JPH_PI / 2 - 0.175394f * abs_x); + + // Our approximation is valid in the range [0, 1], extend it to the range [-1, 1] + return inX < 0? JPH_PI - val : val; +} + +/// Arc tangent of x (returns value in the range [-PI / 2, PI / 2]) +JPH_INLINE float ATan(float inX) +{ + return Vec4::sReplicate(inX).ATan().GetX(); +} + +/// Arc tangent of y / x using the signs of the arguments to determine the correct quadrant (returns value in the range [-PI, PI]) +JPH_INLINE float ATan2(float inY, float inX) +{ + return Vec4::sATan2(Vec4::sReplicate(inY), Vec4::sReplicate(inX)).GetX(); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/UVec4.h b/thirdparty/jolt_physics/Jolt/Math/UVec4.h new file mode 100644 index 000000000000..3f22d43ca6e1 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/UVec4.h @@ -0,0 +1,220 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) UVec4 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // Underlying vector type +#if defined(JPH_USE_SSE) + using Type = __m128i; +#elif defined(JPH_USE_NEON) + using Type = uint32x4_t; +#else + using Type = struct { uint32 mData[4]; }; +#endif + + /// Constructor + UVec4() = default; ///< Intentionally not initialized for performance reasons + UVec4(const UVec4 &inRHS) = default; + UVec4 & operator = (const UVec4 &inRHS) = default; + JPH_INLINE UVec4(Type inRHS) : mValue(inRHS) { } + + /// Create a vector from 4 integer components + JPH_INLINE UVec4(uint32 inX, uint32 inY, uint32 inZ, uint32 inW); + + /// Comparison + JPH_INLINE bool operator == (UVec4Arg inV2) const; + JPH_INLINE bool operator != (UVec4Arg inV2) const { return !(*this == inV2); } + + /// Swizzle the elements in inV + template + JPH_INLINE UVec4 Swizzle() const; + + /// Vector with all zeros + static JPH_INLINE UVec4 sZero(); + + /// Replicate int inV across all components + static JPH_INLINE UVec4 sReplicate(uint32 inV); + + /// Load 1 int from memory and place it in the X component, zeros Y, Z and W + static JPH_INLINE UVec4 sLoadInt(const uint32 *inV); + + /// Load 4 ints from memory + static JPH_INLINE UVec4 sLoadInt4(const uint32 *inV); + + /// Load 4 ints from memory, aligned to 16 bytes + static JPH_INLINE UVec4 sLoadInt4Aligned(const uint32 *inV); + + /// Gather 4 ints from memory at inBase + inOffsets[i] * Scale + template + static JPH_INLINE UVec4 sGatherInt4(const uint32 *inBase, UVec4Arg inOffsets); + + /// Return the minimum value of each of the components + static JPH_INLINE UVec4 sMin(UVec4Arg inV1, UVec4Arg inV2); + + /// Return the maximum of each of the components + static JPH_INLINE UVec4 sMax(UVec4Arg inV1, UVec4Arg inV2); + + /// Equals (component wise) + static JPH_INLINE UVec4 sEquals(UVec4Arg inV1, UVec4Arg inV2); + + /// Component wise select, returns inNotSet when highest bit of inControl = 0 and inSet when highest bit of inControl = 1 + static JPH_INLINE UVec4 sSelect(UVec4Arg inNotSet, UVec4Arg inSet, UVec4Arg inControl); + + /// Logical or (component wise) + static JPH_INLINE UVec4 sOr(UVec4Arg inV1, UVec4Arg inV2); + + /// Logical xor (component wise) + static JPH_INLINE UVec4 sXor(UVec4Arg inV1, UVec4Arg inV2); + + /// Logical and (component wise) + static JPH_INLINE UVec4 sAnd(UVec4Arg inV1, UVec4Arg inV2); + + /// Logical not (component wise) + static JPH_INLINE UVec4 sNot(UVec4Arg inV1); + + /// Sorts the elements in inIndex so that the values that correspond to trues in inValue are the first elements. + /// The remaining elements will be set to inValue.w. + /// I.e. if inValue = (true, false, true, false) and inIndex = (1, 2, 3, 4) the function returns (1, 3, 4, 4). + static JPH_INLINE UVec4 sSort4True(UVec4Arg inValue, UVec4Arg inIndex); + + /// Get individual components +#if defined(JPH_USE_SSE) + JPH_INLINE uint32 GetX() const { return uint32(_mm_cvtsi128_si32(mValue)); } + JPH_INLINE uint32 GetY() const { return mU32[1]; } + JPH_INLINE uint32 GetZ() const { return mU32[2]; } + JPH_INLINE uint32 GetW() const { return mU32[3]; } +#elif defined(JPH_USE_NEON) + JPH_INLINE uint32 GetX() const { return vgetq_lane_u32(mValue, 0); } + JPH_INLINE uint32 GetY() const { return vgetq_lane_u32(mValue, 1); } + JPH_INLINE uint32 GetZ() const { return vgetq_lane_u32(mValue, 2); } + JPH_INLINE uint32 GetW() const { return vgetq_lane_u32(mValue, 3); } +#else + JPH_INLINE uint32 GetX() const { return mU32[0]; } + JPH_INLINE uint32 GetY() const { return mU32[1]; } + JPH_INLINE uint32 GetZ() const { return mU32[2]; } + JPH_INLINE uint32 GetW() const { return mU32[3]; } +#endif + + /// Set individual components + JPH_INLINE void SetX(uint32 inX) { mU32[0] = inX; } + JPH_INLINE void SetY(uint32 inY) { mU32[1] = inY; } + JPH_INLINE void SetZ(uint32 inZ) { mU32[2] = inZ; } + JPH_INLINE void SetW(uint32 inW) { mU32[3] = inW; } + + /// Get component by index + 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) + 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); + + /// Replicate the X component to all components + JPH_INLINE UVec4 SplatX() const; + + /// Replicate the Y component to all components + JPH_INLINE UVec4 SplatY() const; + + /// Replicate the Z component to all components + JPH_INLINE UVec4 SplatZ() const; + + /// Replicate the W component to all components + JPH_INLINE UVec4 SplatW() const; + + /// Convert each component from an int to a float + JPH_INLINE Vec4 ToFloat() const; + + /// Reinterpret UVec4 as a Vec4 (doesn't change the bits) + JPH_INLINE Vec4 ReinterpretAsFloat() const; + + /// Store 4 ints to memory + JPH_INLINE void StoreInt4(uint32 *outV) const; + + /// Store 4 ints to memory, aligned to 16 bytes + JPH_INLINE void StoreInt4Aligned(uint32 *outV) const; + + /// Test if any of the components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAnyTrue() const; + + /// Test if any of X, Y or Z components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAnyXYZTrue() const; + + /// Test if all components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAllTrue() const; + + /// Test if X, Y and Z components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAllXYZTrue() const; + + /// Count the number of components that are true (true is when highest bit of component is set) + JPH_INLINE int CountTrues() const; + + /// Store if X is true in bit 0, Y in bit 1, Z in bit 2 and W in bit 3 (true is when highest bit of component is set) + JPH_INLINE int GetTrues() const; + + /// Shift all components by Count bits to the left (filling with zeros from the left) + template + JPH_INLINE UVec4 LogicalShiftLeft() const; + + /// Shift all components by Count bits to the right (filling with zeros from the right) + template + JPH_INLINE UVec4 LogicalShiftRight() const; + + /// Shift all components by Count bits to the right (shifting in the value of the highest bit) + template + JPH_INLINE UVec4 ArithmeticShiftRight() const; + + /// Takes the lower 4 16 bits and expands them to X, Y, Z and W + JPH_INLINE UVec4 Expand4Uint16Lo() const; + + /// Takes the upper 4 16 bits and expands them to X, Y, Z and W + JPH_INLINE UVec4 Expand4Uint16Hi() const; + + /// Takes byte 0 .. 3 and expands them to X, Y, Z and W + JPH_INLINE UVec4 Expand4Byte0() const; + + /// Takes byte 4 .. 7 and expands them to X, Y, Z and W + JPH_INLINE UVec4 Expand4Byte4() const; + + /// Takes byte 8 .. 11 and expands them to X, Y, Z and W + JPH_INLINE UVec4 Expand4Byte8() const; + + /// Takes byte 12 .. 15 and expands them to X, Y, Z and W + JPH_INLINE UVec4 Expand4Byte12() const; + + /// Shift vector components by 4 - Count floats to the left, so if Count = 1 the resulting vector is (W, 0, 0, 0), when Count = 3 the resulting vector is (Y, Z, W, 0) + JPH_INLINE UVec4 ShiftComponents4Minus(int inCount) const; + + /// To String + friend ostream & operator << (ostream &inStream, UVec4Arg inV) + { + inStream << inV.mU32[0] << ", " << inV.mU32[1] << ", " << inV.mU32[2] << ", " << inV.mU32[3]; + return inStream; + } + + union + { + Type mValue; + uint32 mU32[4]; + }; +}; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "UVec4.inl" diff --git a/thirdparty/jolt_physics/Jolt/Math/UVec4.inl b/thirdparty/jolt_physics/Jolt/Math/UVec4.inl new file mode 100644 index 000000000000..f9014acc28dc --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/UVec4.inl @@ -0,0 +1,581 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +JPH_NAMESPACE_BEGIN + +UVec4::UVec4(uint32 inX, uint32 inY, uint32 inZ, uint32 inW) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_set_epi32(int(inW), int(inZ), int(inY), int(inX)); +#elif defined(JPH_USE_NEON) + uint32x2_t xy = vcreate_u32(static_cast(inX) | (static_cast(inY) << 32)); + uint32x2_t zw = vcreate_u32(static_cast(inZ) | (static_cast(inW) << 32)); + mValue = vcombine_u32(xy, zw); +#else + mU32[0] = inX; + mU32[1] = inY; + mU32[2] = inZ; + mU32[3] = inW; +#endif +} + +bool UVec4::operator == (UVec4Arg inV2) const +{ + return sEquals(*this, inV2).TestAllTrue(); +} + +template +UVec4 UVec4::Swizzle() const +{ + static_assert(SwizzleX <= 3, "SwizzleX template parameter out of range"); + static_assert(SwizzleY <= 3, "SwizzleY template parameter out of range"); + static_assert(SwizzleZ <= 3, "SwizzleZ template parameter out of range"); + static_assert(SwizzleW <= 3, "SwizzleW template parameter out of range"); + +#if defined(JPH_USE_SSE) + return _mm_shuffle_epi32(mValue, _MM_SHUFFLE(SwizzleW, SwizzleZ, SwizzleY, SwizzleX)); +#elif defined(JPH_USE_NEON) + return JPH_NEON_SHUFFLE_U32x4(mValue, mValue, SwizzleX, SwizzleY, SwizzleZ, SwizzleW); +#else + return UVec4(mU32[SwizzleX], mU32[SwizzleY], mU32[SwizzleZ], mU32[SwizzleW]); +#endif +} + +UVec4 UVec4::sZero() +{ +#if defined(JPH_USE_SSE) + return _mm_setzero_si128(); +#elif defined(JPH_USE_NEON) + return vdupq_n_u32(0); +#else + return UVec4(0, 0, 0, 0); +#endif +} + +UVec4 UVec4::sReplicate(uint32 inV) +{ +#if defined(JPH_USE_SSE) + return _mm_set1_epi32(int(inV)); +#elif defined(JPH_USE_NEON) + return vdupq_n_u32(inV); +#else + return UVec4(inV, inV, inV, inV); +#endif +} + +UVec4 UVec4::sLoadInt(const uint32 *inV) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_load_ss(reinterpret_cast(inV))); +#elif defined(JPH_USE_NEON) + return vsetq_lane_u32(*inV, vdupq_n_u32(0), 0); +#else + return UVec4(*inV, 0, 0, 0); +#endif +} + +UVec4 UVec4::sLoadInt4(const uint32 *inV) +{ +#if defined(JPH_USE_SSE) + return _mm_loadu_si128(reinterpret_cast(inV)); +#elif defined(JPH_USE_NEON) + return vld1q_u32(inV); +#else + return UVec4(inV[0], inV[1], inV[2], inV[3]); +#endif +} + +UVec4 UVec4::sLoadInt4Aligned(const uint32 *inV) +{ +#if defined(JPH_USE_SSE) + return _mm_load_si128(reinterpret_cast(inV)); +#elif defined(JPH_USE_NEON) + return vld1q_u32(inV); // ARM doesn't make distinction between aligned or not +#else + return UVec4(inV[0], inV[1], inV[2], inV[3]); +#endif +} + +template +UVec4 UVec4::sGatherInt4(const uint32 *inBase, UVec4Arg inOffsets) +{ +#ifdef JPH_USE_AVX2 + return _mm_i32gather_epi32(reinterpret_cast(inBase), inOffsets.mValue, Scale); +#else + const uint8 *base = reinterpret_cast(inBase); + uint32 x = *reinterpret_cast(base + inOffsets.GetX() * Scale); + uint32 y = *reinterpret_cast(base + inOffsets.GetY() * Scale); + uint32 z = *reinterpret_cast(base + inOffsets.GetZ() * Scale); + uint32 w = *reinterpret_cast(base + inOffsets.GetW() * Scale); + return UVec4(x, y, z, w); +#endif +} + +UVec4 UVec4::sMin(UVec4Arg inV1, UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE4_1) + return _mm_min_epu32(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vminq_u32(inV1.mValue, inV2.mValue); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = min(inV1.mU32[i], inV2.mU32[i]); + return result; +#endif +} + +UVec4 UVec4::sMax(UVec4Arg inV1, UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE4_1) + return _mm_max_epu32(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmaxq_u32(inV1.mValue, inV2.mValue); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = max(inV1.mU32[i], inV2.mU32[i]); + return result; +#endif +} + +UVec4 UVec4::sEquals(UVec4Arg inV1, UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_cmpeq_epi32(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vceqq_u32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mU32[0] == inV2.mU32[0]? 0xffffffffu : 0, + inV1.mU32[1] == inV2.mU32[1]? 0xffffffffu : 0, + inV1.mU32[2] == inV2.mU32[2]? 0xffffffffu : 0, + inV1.mU32[3] == inV2.mU32[3]? 0xffffffffu : 0); +#endif +} + +UVec4 UVec4::sSelect(UVec4Arg inNotSet, UVec4Arg inSet, UVec4Arg inControl) +{ +#if defined(JPH_USE_SSE4_1) && !defined(JPH_PLATFORM_WASM) // _mm_blendv_ps has problems on FireFox + return _mm_castps_si128(_mm_blendv_ps(_mm_castsi128_ps(inNotSet.mValue), _mm_castsi128_ps(inSet.mValue), _mm_castsi128_ps(inControl.mValue))); +#elif defined(JPH_USE_SSE) + __m128 is_set = _mm_castsi128_ps(_mm_srai_epi32(inControl.mValue, 31)); + return _mm_castps_si128(_mm_or_ps(_mm_and_ps(is_set, _mm_castsi128_ps(inSet.mValue)), _mm_andnot_ps(is_set, _mm_castsi128_ps(inNotSet.mValue)))); +#elif defined(JPH_USE_NEON) + return vbslq_u32(vreinterpretq_u32_s32(vshrq_n_s32(vreinterpretq_s32_u32(inControl.mValue), 31)), inSet.mValue, inNotSet.mValue); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = (inControl.mU32[i] & 0x80000000u) ? inSet.mU32[i] : inNotSet.mU32[i]; + return result; +#endif +} + +UVec4 UVec4::sOr(UVec4Arg inV1, UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_or_si128(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vorrq_u32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mU32[0] | inV2.mU32[0], + inV1.mU32[1] | inV2.mU32[1], + inV1.mU32[2] | inV2.mU32[2], + inV1.mU32[3] | inV2.mU32[3]); +#endif +} + +UVec4 UVec4::sXor(UVec4Arg inV1, UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_xor_si128(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return veorq_u32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mU32[0] ^ inV2.mU32[0], + inV1.mU32[1] ^ inV2.mU32[1], + inV1.mU32[2] ^ inV2.mU32[2], + inV1.mU32[3] ^ inV2.mU32[3]); +#endif +} + +UVec4 UVec4::sAnd(UVec4Arg inV1, UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_and_si128(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vandq_u32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mU32[0] & inV2.mU32[0], + inV1.mU32[1] & inV2.mU32[1], + inV1.mU32[2] & inV2.mU32[2], + inV1.mU32[3] & inV2.mU32[3]); +#endif +} + + +UVec4 UVec4::sNot(UVec4Arg inV1) +{ +#if defined(JPH_USE_AVX512) + return _mm_ternarylogic_epi32(inV1.mValue, inV1.mValue, inV1.mValue, 0b01010101); +#elif defined(JPH_USE_SSE) + return sXor(inV1, sReplicate(0xffffffff)); +#elif defined(JPH_USE_NEON) + return vmvnq_u32(inV1.mValue); +#else + return UVec4(~inV1.mU32[0], ~inV1.mU32[1], ~inV1.mU32[2], ~inV1.mU32[3]); +#endif +} + +UVec4 UVec4::sSort4True(UVec4Arg inValue, UVec4Arg inIndex) +{ + // If inValue.z is false then shift W to Z + UVec4 v = UVec4::sSelect(inIndex.Swizzle(), inIndex, inValue.SplatZ()); + + // If inValue.y is false then shift Z and further to Y and further + v = UVec4::sSelect(v.Swizzle(), v, inValue.SplatY()); + + // If inValue.x is false then shift X and further to Y and further + v = UVec4::sSelect(v.Swizzle(), v, inValue.SplatX()); + + return v; +} + +UVec4 UVec4::operator * (UVec4Arg inV2) const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_mullo_epi32(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmulq_u32(mValue, inV2.mValue); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = mU32[i] * inV2.mU32[i]; + return result; +#endif +} + +UVec4 UVec4::operator + (UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_add_epi32(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vaddq_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_add_epi32(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + mValue = vaddq_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) + return _mm_shuffle_epi32(mValue, _MM_SHUFFLE(0, 0, 0, 0)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_u32(mValue, 0); +#else + return UVec4(mU32[0], mU32[0], mU32[0], mU32[0]); +#endif +} + +UVec4 UVec4::SplatY() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_epi32(mValue, _MM_SHUFFLE(1, 1, 1, 1)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_u32(mValue, 1); +#else + return UVec4(mU32[1], mU32[1], mU32[1], mU32[1]); +#endif +} + +UVec4 UVec4::SplatZ() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_epi32(mValue, _MM_SHUFFLE(2, 2, 2, 2)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_u32(mValue, 2); +#else + return UVec4(mU32[2], mU32[2], mU32[2], mU32[2]); +#endif +} + +UVec4 UVec4::SplatW() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_epi32(mValue, _MM_SHUFFLE(3, 3, 3, 3)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_u32(mValue, 3); +#else + return UVec4(mU32[3], mU32[3], mU32[3], mU32[3]); +#endif +} + +Vec4 UVec4::ToFloat() const +{ +#if defined(JPH_USE_SSE) + return _mm_cvtepi32_ps(mValue); +#elif defined(JPH_USE_NEON) + return vcvtq_f32_u32(mValue); +#else + return Vec4((float)mU32[0], (float)mU32[1], (float)mU32[2], (float)mU32[3]); +#endif +} + +Vec4 UVec4::ReinterpretAsFloat() const +{ +#if defined(JPH_USE_SSE) + return Vec4(_mm_castsi128_ps(mValue)); +#elif defined(JPH_USE_NEON) + return vreinterpretq_f32_u32(mValue); +#else + return *reinterpret_cast(this); +#endif +} + +void UVec4::StoreInt4(uint32 *outV) const +{ +#if defined(JPH_USE_SSE) + _mm_storeu_si128(reinterpret_cast<__m128i *>(outV), mValue); +#elif defined(JPH_USE_NEON) + vst1q_u32(outV, mValue); +#else + for (int i = 0; i < 4; ++i) + outV[i] = mU32[i]; +#endif +} + +void UVec4::StoreInt4Aligned(uint32 *outV) const +{ +#if defined(JPH_USE_SSE) + _mm_store_si128(reinterpret_cast<__m128i *>(outV), mValue); +#elif defined(JPH_USE_NEON) + vst1q_u32(outV, mValue); // ARM doesn't make distinction between aligned or not +#else + for (int i = 0; i < 4; ++i) + outV[i] = mU32[i]; +#endif +} + +int UVec4::CountTrues() const +{ +#if defined(JPH_USE_SSE) + return CountBits(_mm_movemask_ps(_mm_castsi128_ps(mValue))); +#elif defined(JPH_USE_NEON) + return vaddvq_u32(vshrq_n_u32(mValue, 31)); +#else + return (mU32[0] >> 31) + (mU32[1] >> 31) + (mU32[2] >> 31) + (mU32[3] >> 31); +#endif +} + +int UVec4::GetTrues() const +{ +#if defined(JPH_USE_SSE) + return _mm_movemask_ps(_mm_castsi128_ps(mValue)); +#elif defined(JPH_USE_NEON) + int32x4_t shift = JPH_NEON_INT32x4(0, 1, 2, 3); + return vaddvq_u32(vshlq_u32(vshrq_n_u32(mValue, 31), shift)); +#else + return (mU32[0] >> 31) | ((mU32[1] >> 31) << 1) | ((mU32[2] >> 31) << 2) | ((mU32[3] >> 31) << 3); +#endif +} + +bool UVec4::TestAnyTrue() const +{ + return GetTrues() != 0; +} + +bool UVec4::TestAnyXYZTrue() const +{ + return (GetTrues() & 0b111) != 0; +} + +bool UVec4::TestAllTrue() const +{ + return GetTrues() == 0b1111; +} + +bool UVec4::TestAllXYZTrue() const +{ + return (GetTrues() & 0b111) == 0b111; +} + +template +UVec4 UVec4::LogicalShiftLeft() const +{ + static_assert(Count <= 31, "Invalid shift"); + +#if defined(JPH_USE_SSE) + return _mm_slli_epi32(mValue, Count); +#elif defined(JPH_USE_NEON) + return vshlq_n_u32(mValue, Count); +#else + return UVec4(mU32[0] << Count, mU32[1] << Count, mU32[2] << Count, mU32[3] << Count); +#endif +} + +template +UVec4 UVec4::LogicalShiftRight() const +{ + static_assert(Count <= 31, "Invalid shift"); + +#if defined(JPH_USE_SSE) + return _mm_srli_epi32(mValue, Count); +#elif defined(JPH_USE_NEON) + return vshrq_n_u32(mValue, Count); +#else + return UVec4(mU32[0] >> Count, mU32[1] >> Count, mU32[2] >> Count, mU32[3] >> Count); +#endif +} + +template +UVec4 UVec4::ArithmeticShiftRight() const +{ + static_assert(Count <= 31, "Invalid shift"); + +#if defined(JPH_USE_SSE) + return _mm_srai_epi32(mValue, Count); +#elif defined(JPH_USE_NEON) + return vreinterpretq_u32_s32(vshrq_n_s32(vreinterpretq_s32_u32(mValue), Count)); +#else + return UVec4(uint32(int32_t(mU32[0]) >> Count), + uint32(int32_t(mU32[1]) >> Count), + uint32(int32_t(mU32[2]) >> Count), + uint32(int32_t(mU32[3]) >> Count)); +#endif +} + +UVec4 UVec4::Expand4Uint16Lo() const +{ +#if defined(JPH_USE_SSE) + return _mm_unpacklo_epi16(mValue, _mm_castps_si128(_mm_setzero_ps())); +#elif defined(JPH_USE_NEON) + uint16x4_t value = vget_low_u16(vreinterpretq_u16_u32(mValue)); + uint16x4_t zero = vdup_n_u16(0); + return vreinterpretq_u32_u16(vcombine_u16(vzip1_u16(value, zero), vzip2_u16(value, zero))); +#else + return UVec4(mU32[0] & 0xffff, + (mU32[0] >> 16) & 0xffff, + mU32[1] & 0xffff, + (mU32[1] >> 16) & 0xffff); +#endif +} + +UVec4 UVec4::Expand4Uint16Hi() const +{ +#if defined(JPH_USE_SSE) + return _mm_unpackhi_epi16(mValue, _mm_castps_si128(_mm_setzero_ps())); +#elif defined(JPH_USE_NEON) + uint16x4_t value = vget_high_u16(vreinterpretq_u16_u32(mValue)); + uint16x4_t zero = vdup_n_u16(0); + return vreinterpretq_u32_u16(vcombine_u16(vzip1_u16(value, zero), vzip2_u16(value, zero))); +#else + return UVec4(mU32[2] & 0xffff, + (mU32[2] >> 16) & 0xffff, + mU32[3] & 0xffff, + (mU32[3] >> 16) & 0xffff); +#endif +} + +UVec4 UVec4::Expand4Byte0() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_shuffle_epi8(mValue, _mm_set_epi32(int(0xffffff03), int(0xffffff02), int(0xffffff01), int(0xffffff00))); +#elif defined(JPH_USE_NEON) + uint8x16_t idx = JPH_NEON_UINT8x16(0x00, 0x7f, 0x7f, 0x7f, 0x01, 0x7f, 0x7f, 0x7f, 0x02, 0x7f, 0x7f, 0x7f, 0x03, 0x7f, 0x7f, 0x7f); + return vreinterpretq_u32_s8(vqtbl1q_s8(vreinterpretq_s8_u32(mValue), idx)); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = (mU32[0] >> (i * 8)) & 0xff; + return result; +#endif +} + +UVec4 UVec4::Expand4Byte4() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_shuffle_epi8(mValue, _mm_set_epi32(int(0xffffff07), int(0xffffff06), int(0xffffff05), int(0xffffff04))); +#elif defined(JPH_USE_NEON) + uint8x16_t idx = JPH_NEON_UINT8x16(0x04, 0x7f, 0x7f, 0x7f, 0x05, 0x7f, 0x7f, 0x7f, 0x06, 0x7f, 0x7f, 0x7f, 0x07, 0x7f, 0x7f, 0x7f); + return vreinterpretq_u32_s8(vqtbl1q_s8(vreinterpretq_s8_u32(mValue), idx)); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = (mU32[1] >> (i * 8)) & 0xff; + return result; +#endif +} + +UVec4 UVec4::Expand4Byte8() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_shuffle_epi8(mValue, _mm_set_epi32(int(0xffffff0b), int(0xffffff0a), int(0xffffff09), int(0xffffff08))); +#elif defined(JPH_USE_NEON) + uint8x16_t idx = JPH_NEON_UINT8x16(0x08, 0x7f, 0x7f, 0x7f, 0x09, 0x7f, 0x7f, 0x7f, 0x0a, 0x7f, 0x7f, 0x7f, 0x0b, 0x7f, 0x7f, 0x7f); + return vreinterpretq_u32_s8(vqtbl1q_s8(vreinterpretq_s8_u32(mValue), idx)); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = (mU32[2] >> (i * 8)) & 0xff; + return result; +#endif +} + +UVec4 UVec4::Expand4Byte12() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_shuffle_epi8(mValue, _mm_set_epi32(int(0xffffff0f), int(0xffffff0e), int(0xffffff0d), int(0xffffff0c))); +#elif defined(JPH_USE_NEON) + uint8x16_t idx = JPH_NEON_UINT8x16(0x0c, 0x7f, 0x7f, 0x7f, 0x0d, 0x7f, 0x7f, 0x7f, 0x0e, 0x7f, 0x7f, 0x7f, 0x0f, 0x7f, 0x7f, 0x7f); + return vreinterpretq_u32_s8(vqtbl1q_s8(vreinterpretq_s8_u32(mValue), idx)); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = (mU32[3] >> (i * 8)) & 0xff; + return result; +#endif +} + +UVec4 UVec4::ShiftComponents4Minus(int inCount) const +{ +#if defined(JPH_USE_SSE4_1) || defined(JPH_USE_NEON) + alignas(UVec4) static constexpr uint32 sFourMinusXShuffle[5][4] = + { + { 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff }, + { 0x0f0e0d0c, 0xffffffff, 0xffffffff, 0xffffffff }, + { 0x0b0a0908, 0x0f0e0d0c, 0xffffffff, 0xffffffff }, + { 0x07060504, 0x0b0a0908, 0x0f0e0d0c, 0xffffffff }, + { 0x03020100, 0x07060504, 0x0b0a0908, 0x0f0e0d0c } + }; +#endif + +#if defined(JPH_USE_SSE4_1) + return _mm_shuffle_epi8(mValue, *reinterpret_cast(sFourMinusXShuffle[inCount])); +#elif defined(JPH_USE_NEON) + uint8x16_t idx = vreinterpretq_u8_u32(*reinterpret_cast(sFourMinusXShuffle[inCount])); + return vreinterpretq_u32_s8(vqtbl1q_s8(vreinterpretq_s8_u32(mValue), idx)); +#else + UVec4 result = UVec4::sZero(); + for (int i = 0; i < inCount; i++) + result.mU32[i] = mU32[i + 4 - inCount]; + return result; +#endif +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Vec3.cpp b/thirdparty/jolt_physics/Jolt/Math/Vec3.cpp new file mode 100644 index 000000000000..c865387f79e6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Vec3.cpp @@ -0,0 +1,71 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +static void sAddVertex(StaticArray &ioVertices, Vec3Arg inVertex) +{ + bool found = false; + for (const Vec3 &v : ioVertices) + if (v == inVertex) + { + found = true; + break; + } + if (!found) + ioVertices.push_back(inVertex); +} + +static void sCreateVertices(StaticArray &ioVertices, Vec3Arg inDir1, Vec3Arg inDir2, Vec3Arg inDir3, int inLevel) +{ + Vec3 center1 = (inDir1 + inDir2).Normalized(); + Vec3 center2 = (inDir2 + inDir3).Normalized(); + Vec3 center3 = (inDir3 + inDir1).Normalized(); + + sAddVertex(ioVertices, center1); + sAddVertex(ioVertices, center2); + sAddVertex(ioVertices, center3); + + if (inLevel > 0) + { + int new_level = inLevel - 1; + sCreateVertices(ioVertices, inDir1, center1, center3, new_level); + sCreateVertices(ioVertices, center1, center2, center3, new_level); + sCreateVertices(ioVertices, center1, inDir2, center2, new_level); + sCreateVertices(ioVertices, center3, center2, inDir3, new_level); + } +} + +const StaticArray Vec3::sUnitSphere = []() { + + const int level = 3; + + StaticArray verts; + + // Add unit axis + verts.push_back(Vec3::sAxisX()); + verts.push_back(-Vec3::sAxisX()); + verts.push_back(Vec3::sAxisY()); + verts.push_back(-Vec3::sAxisY()); + verts.push_back(Vec3::sAxisZ()); + verts.push_back(-Vec3::sAxisZ()); + + // Subdivide + sCreateVertices(verts, Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), level); + sCreateVertices(verts, -Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), level); + sCreateVertices(verts, Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), level); + sCreateVertices(verts, -Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), level); + sCreateVertices(verts, Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), level); + sCreateVertices(verts, -Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), level); + sCreateVertices(verts, Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), level); + sCreateVertices(verts, -Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), level); + + return verts; +}(); + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Vec3.h b/thirdparty/jolt_physics/Jolt/Math/Vec3.h new file mode 100644 index 000000000000..18264db7e27f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Vec3.h @@ -0,0 +1,295 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// 3 component vector (stored as 4 vectors). +/// Note that we keep the 4th component the same as the 3rd component to avoid divisions by zero when JPH_FLOATING_POINT_EXCEPTIONS_ENABLED defined +class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Vec3 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // Underlying vector type +#if defined(JPH_USE_SSE) + using Type = __m128; +#elif defined(JPH_USE_NEON) + using Type = float32x4_t; +#else + using Type = Vec4::Type; +#endif + + // Argument type + using ArgType = Vec3Arg; + + /// Constructor + Vec3() = default; ///< Intentionally not initialized for performance reasons + Vec3(const Vec3 &inRHS) = default; + Vec3 & operator = (const Vec3 &inRHS) = default; + explicit JPH_INLINE Vec3(Vec4Arg inRHS); + JPH_INLINE Vec3(Type inRHS) : mValue(inRHS) { CheckW(); } + + /// Load 3 floats from memory + explicit JPH_INLINE Vec3(const Float3 &inV); + + /// Create a vector from 3 components + JPH_INLINE Vec3(float inX, float inY, float inZ); + + /// Vector with all zeros + static JPH_INLINE Vec3 sZero(); + + /// Vector with all NaN's + static JPH_INLINE Vec3 sNaN(); + + /// Vectors with the principal axis + static JPH_INLINE Vec3 sAxisX() { return Vec3(1, 0, 0); } + static JPH_INLINE Vec3 sAxisY() { return Vec3(0, 1, 0); } + static JPH_INLINE Vec3 sAxisZ() { return Vec3(0, 0, 1); } + + /// Replicate inV across all components + static JPH_INLINE Vec3 sReplicate(float inV); + + /// Load 3 floats from memory (reads 32 bits extra which it doesn't use) + static JPH_INLINE Vec3 sLoadFloat3Unsafe(const Float3 &inV); + + /// Return the minimum value of each of the components + static JPH_INLINE Vec3 sMin(Vec3Arg inV1, Vec3Arg inV2); + + /// Return the maximum of each of the components + static JPH_INLINE Vec3 sMax(Vec3Arg inV1, Vec3Arg inV2); + + /// Clamp a vector between min and max (component wise) + static JPH_INLINE Vec3 sClamp(Vec3Arg inV, Vec3Arg inMin, Vec3Arg inMax); + + /// Equals (component wise) + static JPH_INLINE UVec4 sEquals(Vec3Arg inV1, Vec3Arg inV2); + + /// Less than (component wise) + static JPH_INLINE UVec4 sLess(Vec3Arg inV1, Vec3Arg inV2); + + /// Less than or equal (component wise) + static JPH_INLINE UVec4 sLessOrEqual(Vec3Arg inV1, Vec3Arg inV2); + + /// Greater than (component wise) + static JPH_INLINE UVec4 sGreater(Vec3Arg inV1, Vec3Arg inV2); + + /// Greater than or equal (component wise) + static JPH_INLINE UVec4 sGreaterOrEqual(Vec3Arg inV1, Vec3Arg inV2); + + /// Calculates inMul1 * inMul2 + inAdd + static JPH_INLINE Vec3 sFusedMultiplyAdd(Vec3Arg inMul1, Vec3Arg inMul2, Vec3Arg inAdd); + + /// Component wise select, returns inNotSet when highest bit of inControl = 0 and inSet when highest bit of inControl = 1 + static JPH_INLINE Vec3 sSelect(Vec3Arg inNotSet, Vec3Arg inSet, UVec4Arg inControl); + + /// Logical or (component wise) + static JPH_INLINE Vec3 sOr(Vec3Arg inV1, Vec3Arg inV2); + + /// Logical xor (component wise) + static JPH_INLINE Vec3 sXor(Vec3Arg inV1, Vec3Arg inV2); + + /// Logical and (component wise) + static JPH_INLINE Vec3 sAnd(Vec3Arg inV1, Vec3Arg inV2); + + /// Get unit vector given spherical coordinates + /// inTheta \f$\in [0, \pi]\f$ is angle between vector and z-axis + /// inPhi \f$\in [0, 2 \pi]\f$ is the angle in the xy-plane starting from the x axis and rotating counter clockwise around the z-axis + static JPH_INLINE Vec3 sUnitSpherical(float inTheta, float inPhi); + + /// A set of vectors uniformly spanning the surface of a unit sphere, usable for debug purposes + JPH_EXPORT static const StaticArray sUnitSphere; + + /// Get random unit vector + template + static inline Vec3 sRandom(Random &inRandom); + + /// Get individual components +#if defined(JPH_USE_SSE) + JPH_INLINE float GetX() const { return _mm_cvtss_f32(mValue); } + JPH_INLINE float GetY() const { return mF32[1]; } + JPH_INLINE float GetZ() const { return mF32[2]; } +#elif defined(JPH_USE_NEON) + JPH_INLINE float GetX() const { return vgetq_lane_f32(mValue, 0); } + JPH_INLINE float GetY() const { return vgetq_lane_f32(mValue, 1); } + JPH_INLINE float GetZ() const { return vgetq_lane_f32(mValue, 2); } +#else + JPH_INLINE float GetX() const { return mF32[0]; } + JPH_INLINE float GetY() const { return mF32[1]; } + JPH_INLINE float GetZ() const { return mF32[2]; } +#endif + + /// Set individual components + JPH_INLINE void SetX(float inX) { mF32[0] = inX; } + JPH_INLINE void SetY(float inY) { mF32[1] = inY; } + JPH_INLINE void SetZ(float inZ) { mF32[2] = mF32[3] = inZ; } // Assure Z and W are the same + + /// Set all components + JPH_INLINE void Set(float inX, float inY, float inZ) { *this = Vec3(inX, inY, inZ); } + + /// Get float component by index + JPH_INLINE float operator [] (uint inCoordinate) const { JPH_ASSERT(inCoordinate < 3); return mF32[inCoordinate]; } + + /// Set float component by index + JPH_INLINE void SetComponent(uint inCoordinate, float inValue) { JPH_ASSERT(inCoordinate < 3); mF32[inCoordinate] = inValue; mValue = sFixW(mValue); } // Assure Z and W are the same + + /// Comparison + JPH_INLINE bool operator == (Vec3Arg inV2) const; + JPH_INLINE bool operator != (Vec3Arg inV2) const { return !(*this == inV2); } + + /// Test if two vectors are close + JPH_INLINE bool IsClose(Vec3Arg 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; + + /// Test if vector contains NaN elements + JPH_INLINE bool IsNaN() const; + + /// Multiply two float vectors (component wise) + JPH_INLINE Vec3 operator * (Vec3Arg inV2) const; + + /// Multiply vector with float + JPH_INLINE Vec3 operator * (float inV2) const; + + /// Multiply vector with float + friend JPH_INLINE Vec3 operator * (float inV1, Vec3Arg inV2); + + /// Divide vector by float + JPH_INLINE Vec3 operator / (float inV2) const; + + /// Multiply vector with float + JPH_INLINE Vec3 & operator *= (float inV2); + + /// Multiply vector with vector + JPH_INLINE Vec3 & operator *= (Vec3Arg inV2); + + /// Divide vector by float + JPH_INLINE Vec3 & operator /= (float inV2); + + /// Add two float vectors (component wise) + JPH_INLINE Vec3 operator + (Vec3Arg inV2) const; + + /// Add two float vectors (component wise) + JPH_INLINE Vec3 & operator += (Vec3Arg inV2); + + /// Negate + JPH_INLINE Vec3 operator - () const; + + /// Subtract two float vectors (component wise) + JPH_INLINE Vec3 operator - (Vec3Arg inV2) const; + + /// Subtract two float vectors (component wise) + JPH_INLINE Vec3 & operator -= (Vec3Arg inV2); + + /// Divide (component wise) + JPH_INLINE Vec3 operator / (Vec3Arg inV2) const; + + /// Swizzle the elements in inV + template + JPH_INLINE Vec3 Swizzle() const; + + /// Replicate the X component to all components + JPH_INLINE Vec4 SplatX() const; + + /// Replicate the Y component to all components + JPH_INLINE Vec4 SplatY() const; + + /// Replicate the Z component to all components + JPH_INLINE Vec4 SplatZ() 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 Vec3 Abs() const; + + /// Reciprocal vector (1 / value) for each of the components + JPH_INLINE Vec3 Reciprocal() const; + + /// Cross product + JPH_INLINE Vec3 Cross(Vec3Arg inV2) const; + + /// Dot product, returns the dot product in X, Y and Z components + JPH_INLINE Vec3 DotV(Vec3Arg inV2) const; + + /// Dot product, returns the dot product in X, Y, Z and W components + JPH_INLINE Vec4 DotV4(Vec3Arg inV2) const; + + /// Dot product + JPH_INLINE float Dot(Vec3Arg inV2) const; + + /// Squared length of vector + JPH_INLINE float LengthSq() const; + + /// Length of vector + JPH_INLINE float Length() const; + + /// Normalize vector + JPH_INLINE Vec3 Normalized() const; + + /// Normalize vector or return inZeroValue if the length of the vector is zero + JPH_INLINE Vec3 NormalizedOr(Vec3Arg inZeroValue) const; + + /// Store 3 floats to memory + JPH_INLINE void StoreFloat3(Float3 *outV) const; + + /// Convert each component from a float to an int + JPH_INLINE UVec4 ToInt() const; + + /// Reinterpret Vec3 as a UVec4 (doesn't change the bits) + JPH_INLINE UVec4 ReinterpretAsInt() const; + + /// Get the minimum of X, Y and Z + JPH_INLINE float ReduceMin() const; + + /// Get the maximum of X, Y and Z + JPH_INLINE float ReduceMax() const; + + /// Component wise square root + JPH_INLINE Vec3 Sqrt() const; + + /// Get normalized vector that is perpendicular to this vector + JPH_INLINE Vec3 GetNormalizedPerpendicular() const; + + /// Get vector that contains the sign of each element (returns 1.0f if positive, -1.0f if negative) + JPH_INLINE Vec3 GetSign() const; + + /// To String + friend ostream & operator << (ostream &inStream, Vec3Arg inV) + { + inStream << inV.mF32[0] << ", " << inV.mF32[1] << ", " << inV.mF32[2]; + return inStream; + } + + /// Internal helper function that checks that W is equal to Z, so e.g. dividing by it should not generate div by 0 + JPH_INLINE void CheckW() const; + + /// Internal helper function that ensures that the Z component is replicated to the W component to prevent divisions by zero + static JPH_INLINE Type sFixW(Type inValue); + + union + { + Type mValue; + float mF32[4]; + }; +}; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "Vec3.inl" diff --git a/thirdparty/jolt_physics/Jolt/Math/Vec3.inl b/thirdparty/jolt_physics/Jolt/Math/Vec3.inl new file mode 100644 index 000000000000..5a47f40ce6ba --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Vec3.inl @@ -0,0 +1,859 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +// Create a std::hash/JPH::Hash for Vec3 +JPH_MAKE_HASHABLE(JPH::Vec3, t.GetX(), t.GetY(), t.GetZ()) + +JPH_NAMESPACE_BEGIN + +void Vec3::CheckW() const +{ +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + // Avoid asserts when both components are NaN + JPH_ASSERT(reinterpret_cast(mF32)[2] == reinterpret_cast(mF32)[3]); +#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED +} + +JPH_INLINE Vec3::Type Vec3::sFixW(Type inValue) +{ +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + #if defined(JPH_USE_SSE) + return _mm_shuffle_ps(inValue, inValue, _MM_SHUFFLE(2, 2, 1, 0)); + #elif defined(JPH_USE_NEON) + return JPH_NEON_SHUFFLE_F32x4(inValue, inValue, 0, 1, 2, 2); + #else + Type value; + value.mData[0] = inValue.mData[0]; + value.mData[1] = inValue.mData[1]; + value.mData[2] = inValue.mData[2]; + value.mData[3] = inValue.mData[2]; + return value; + #endif +#else + return inValue; +#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED +} + +Vec3::Vec3(Vec4Arg inRHS) : + mValue(sFixW(inRHS.mValue)) +{ +} + +Vec3::Vec3(const Float3 &inV) +{ +#if defined(JPH_USE_SSE) + Type x = _mm_load_ss(&inV.x); + Type y = _mm_load_ss(&inV.y); + Type z = _mm_load_ss(&inV.z); + Type xy = _mm_unpacklo_ps(x, y); + mValue = _mm_shuffle_ps(xy, z, _MM_SHUFFLE(0, 0, 1, 0)); // Assure Z and W are the same +#elif defined(JPH_USE_NEON) + float32x2_t xy = vld1_f32(&inV.x); + float32x2_t zz = vdup_n_f32(inV.z); // Assure Z and W are the same + mValue = vcombine_f32(xy, zz); +#else + mF32[0] = inV[0]; + mF32[1] = inV[1]; + mF32[2] = inV[2]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF32[3] = inV[2]; + #endif +#endif +} + +Vec3::Vec3(float inX, float inY, float inZ) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_set_ps(inZ, inZ, inY, inX); +#elif defined(JPH_USE_NEON) + uint32x2_t xy = vcreate_u32(static_cast(BitCast(inX)) | (static_cast(BitCast(inY)) << 32)); + uint32x2_t zz = vreinterpret_u32_f32(vdup_n_f32(inZ)); + mValue = vreinterpretq_f32_u32(vcombine_u32(xy, zz)); +#else + mF32[0] = inX; + mF32[1] = inY; + mF32[2] = inZ; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF32[3] = inZ; + #endif +#endif +} + +template +Vec3 Vec3::Swizzle() const +{ + static_assert(SwizzleX <= 3, "SwizzleX template parameter out of range"); + static_assert(SwizzleY <= 3, "SwizzleY template parameter out of range"); + static_assert(SwizzleZ <= 3, "SwizzleZ template parameter out of range"); + +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(SwizzleZ, SwizzleZ, SwizzleY, SwizzleX)); // Assure Z and W are the same +#elif defined(JPH_USE_NEON) + return JPH_NEON_SHUFFLE_F32x4(mValue, mValue, SwizzleX, SwizzleY, SwizzleZ, SwizzleZ); +#else + return Vec3(mF32[SwizzleX], mF32[SwizzleY], mF32[SwizzleZ]); +#endif +} + +Vec3 Vec3::sZero() +{ +#if defined(JPH_USE_SSE) + return _mm_setzero_ps(); +#elif defined(JPH_USE_NEON) + return vdupq_n_f32(0); +#else + return Vec3(0, 0, 0); +#endif +} + +Vec3 Vec3::sReplicate(float inV) +{ +#if defined(JPH_USE_SSE) + return _mm_set1_ps(inV); +#elif defined(JPH_USE_NEON) + return vdupq_n_f32(inV); +#else + return Vec3(inV, inV, inV); +#endif +} + +Vec3 Vec3::sNaN() +{ + return sReplicate(numeric_limits::quiet_NaN()); +} + +Vec3 Vec3::sLoadFloat3Unsafe(const Float3 &inV) +{ +#if defined(JPH_USE_SSE) + Type v = _mm_loadu_ps(&inV.x); +#elif defined(JPH_USE_NEON) + Type v = vld1q_f32(&inV.x); +#else + Type v = { inV.x, inV.y, inV.z }; +#endif + return sFixW(v); +} + +Vec3 Vec3::sMin(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_min_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vminq_f32(inV1.mValue, inV2.mValue); +#else + return Vec3(min(inV1.mF32[0], inV2.mF32[0]), + min(inV1.mF32[1], inV2.mF32[1]), + min(inV1.mF32[2], inV2.mF32[2])); +#endif +} + +Vec3 Vec3::sMax(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_max_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmaxq_f32(inV1.mValue, inV2.mValue); +#else + return Vec3(max(inV1.mF32[0], inV2.mF32[0]), + max(inV1.mF32[1], inV2.mF32[1]), + max(inV1.mF32[2], inV2.mF32[2])); +#endif +} + +Vec3 Vec3::sClamp(Vec3Arg inV, Vec3Arg inMin, Vec3Arg inMax) +{ + return sMax(sMin(inV, inMax), inMin); +} + +UVec4 Vec3::sEquals(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmpeq_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vceqq_f32(inV1.mValue, inV2.mValue); +#else + uint32 z = inV1.mF32[2] == inV2.mF32[2]? 0xffffffffu : 0; + return UVec4(inV1.mF32[0] == inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] == inV2.mF32[1]? 0xffffffffu : 0, + z, + z); +#endif +} + +UVec4 Vec3::sLess(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmplt_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcltq_f32(inV1.mValue, inV2.mValue); +#else + uint32 z = inV1.mF32[2] < inV2.mF32[2]? 0xffffffffu : 0; + return UVec4(inV1.mF32[0] < inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] < inV2.mF32[1]? 0xffffffffu : 0, + z, + z); +#endif +} + +UVec4 Vec3::sLessOrEqual(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmple_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcleq_f32(inV1.mValue, inV2.mValue); +#else + uint32 z = inV1.mF32[2] <= inV2.mF32[2]? 0xffffffffu : 0; + return UVec4(inV1.mF32[0] <= inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] <= inV2.mF32[1]? 0xffffffffu : 0, + z, + z); +#endif +} + +UVec4 Vec3::sGreater(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmpgt_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcgtq_f32(inV1.mValue, inV2.mValue); +#else + uint32 z = inV1.mF32[2] > inV2.mF32[2]? 0xffffffffu : 0; + return UVec4(inV1.mF32[0] > inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] > inV2.mF32[1]? 0xffffffffu : 0, + z, + z); +#endif +} + +UVec4 Vec3::sGreaterOrEqual(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmpge_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcgeq_f32(inV1.mValue, inV2.mValue); +#else + uint32 z = inV1.mF32[2] >= inV2.mF32[2]? 0xffffffffu : 0; + return UVec4(inV1.mF32[0] >= inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] >= inV2.mF32[1]? 0xffffffffu : 0, + z, + z); +#endif +} + +Vec3 Vec3::sFusedMultiplyAdd(Vec3Arg inMul1, Vec3Arg inMul2, Vec3Arg inAdd) +{ +#if defined(JPH_USE_SSE) + #ifdef JPH_USE_FMADD + return _mm_fmadd_ps(inMul1.mValue, inMul2.mValue, inAdd.mValue); + #else + return _mm_add_ps(_mm_mul_ps(inMul1.mValue, inMul2.mValue), inAdd.mValue); + #endif +#elif defined(JPH_USE_NEON) + return vmlaq_f32(inAdd.mValue, inMul1.mValue, inMul2.mValue); +#else + return Vec3(inMul1.mF32[0] * inMul2.mF32[0] + inAdd.mF32[0], + inMul1.mF32[1] * inMul2.mF32[1] + inAdd.mF32[1], + inMul1.mF32[2] * inMul2.mF32[2] + inAdd.mF32[2]); +#endif +} + +Vec3 Vec3::sSelect(Vec3Arg inNotSet, Vec3Arg inSet, UVec4Arg inControl) +{ +#if defined(JPH_USE_SSE4_1) && !defined(JPH_PLATFORM_WASM) // _mm_blendv_ps has problems on FireFox + Type v = _mm_blendv_ps(inNotSet.mValue, inSet.mValue, _mm_castsi128_ps(inControl.mValue)); + return sFixW(v); +#elif defined(JPH_USE_SSE) + __m128 is_set = _mm_castsi128_ps(_mm_srai_epi32(inControl.mValue, 31)); + Type v = _mm_or_ps(_mm_and_ps(is_set, inSet.mValue), _mm_andnot_ps(is_set, inNotSet.mValue)); + return sFixW(v); +#elif defined(JPH_USE_NEON) + Type v = vbslq_f32(vreinterpretq_u32_s32(vshrq_n_s32(vreinterpretq_s32_u32(inControl.mValue), 31)), inSet.mValue, inNotSet.mValue); + return sFixW(v); +#else + Vec3 result; + for (int i = 0; i < 3; i++) + result.mF32[i] = (inControl.mU32[i] & 0x80000000u) ? inSet.mF32[i] : inNotSet.mF32[i]; +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + result.mF32[3] = result.mF32[2]; +#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + return result; +#endif +} + +Vec3 Vec3::sOr(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_or_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vreinterpretq_f32_u32(vorrq_u32(vreinterpretq_u32_f32(inV1.mValue), vreinterpretq_u32_f32(inV2.mValue))); +#else + return Vec3(UVec4::sOr(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat()); +#endif +} + +Vec3 Vec3::sXor(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_xor_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vreinterpretq_f32_u32(veorq_u32(vreinterpretq_u32_f32(inV1.mValue), vreinterpretq_u32_f32(inV2.mValue))); +#else + return Vec3(UVec4::sXor(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat()); +#endif +} + +Vec3 Vec3::sAnd(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_and_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vreinterpretq_f32_u32(vandq_u32(vreinterpretq_u32_f32(inV1.mValue), vreinterpretq_u32_f32(inV2.mValue))); +#else + return Vec3(UVec4::sAnd(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat()); +#endif +} + +Vec3 Vec3::sUnitSpherical(float inTheta, float inPhi) +{ + Vec4 s, c; + Vec4(inTheta, inPhi, 0, 0).SinCos(s, c); + return Vec3(s.GetX() * c.GetY(), s.GetX() * s.GetY(), c.GetX()); +} + +template +Vec3 Vec3::sRandom(Random &inRandom) +{ + std::uniform_real_distribution zero_to_one(0.0f, 1.0f); + float theta = JPH_PI * zero_to_one(inRandom); + float phi = 2.0f * JPH_PI * zero_to_one(inRandom); + return sUnitSpherical(theta, phi); +} + +bool Vec3::operator == (Vec3Arg inV2) const +{ + return sEquals(*this, inV2).TestAllXYZTrue(); +} + +bool Vec3::IsClose(Vec3Arg inV2, float inMaxDistSq) const +{ + return (inV2 - *this).LengthSq() <= inMaxDistSq; +} + +bool Vec3::IsNearZero(float inMaxDistSq) const +{ + return LengthSq() <= inMaxDistSq; +} + +Vec3 Vec3::operator * (Vec3Arg inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_mul_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmulq_f32(mValue, inV2.mValue); +#else + return Vec3(mF32[0] * inV2.mF32[0], mF32[1] * inV2.mF32[1], mF32[2] * inV2.mF32[2]); +#endif +} + +Vec3 Vec3::operator * (float inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_mul_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + return vmulq_n_f32(mValue, inV2); +#else + return Vec3(mF32[0] * inV2, mF32[1] * inV2, mF32[2] * inV2); +#endif +} + +Vec3 operator * (float inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_mul_ps(_mm_set1_ps(inV1), inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmulq_n_f32(inV2.mValue, inV1); +#else + return Vec3(inV1 * inV2.mF32[0], inV1 * inV2.mF32[1], inV1 * inV2.mF32[2]); +#endif +} + +Vec3 Vec3::operator / (float inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_div_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + return vdivq_f32(mValue, vdupq_n_f32(inV2)); +#else + return Vec3(mF32[0] / inV2, mF32[1] / inV2, mF32[2] / inV2); +#endif +} + +Vec3 &Vec3::operator *= (float inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_mul_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + mValue = vmulq_n_f32(mValue, inV2); +#else + for (int i = 0; i < 3; ++i) + mF32[i] *= inV2; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF32[3] = mF32[2]; + #endif +#endif + return *this; +} + +Vec3 &Vec3::operator *= (Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_mul_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + mValue = vmulq_f32(mValue, inV2.mValue); +#else + for (int i = 0; i < 3; ++i) + mF32[i] *= inV2.mF32[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF32[3] = mF32[2]; + #endif +#endif + return *this; +} + +Vec3 &Vec3::operator /= (float inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_div_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + mValue = vdivq_f32(mValue, vdupq_n_f32(inV2)); +#else + for (int i = 0; i < 3; ++i) + mF32[i] /= inV2; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF32[3] = mF32[2]; + #endif +#endif + return *this; +} + +Vec3 Vec3::operator + (Vec3Arg inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_add_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vaddq_f32(mValue, inV2.mValue); +#else + return Vec3(mF32[0] + inV2.mF32[0], mF32[1] + inV2.mF32[1], mF32[2] + inV2.mF32[2]); +#endif +} + +Vec3 &Vec3::operator += (Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_add_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + mValue = vaddq_f32(mValue, inV2.mValue); +#else + for (int i = 0; i < 3; ++i) + mF32[i] += inV2.mF32[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF32[3] = mF32[2]; + #endif +#endif + return *this; +} + +Vec3 Vec3::operator - () const +{ +#if defined(JPH_USE_SSE) + return _mm_sub_ps(_mm_setzero_ps(), mValue); +#elif defined(JPH_USE_NEON) + #ifdef JPH_CROSS_PLATFORM_DETERMINISTIC + return vsubq_f32(vdupq_n_f32(0), mValue); + #else + return vnegq_f32(mValue); + #endif +#else + #ifdef JPH_CROSS_PLATFORM_DETERMINISTIC + return Vec3(0.0f - mF32[0], 0.0f - mF32[1], 0.0f - mF32[2]); + #else + return Vec3(-mF32[0], -mF32[1], -mF32[2]); + #endif +#endif +} + +Vec3 Vec3::operator - (Vec3Arg inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_sub_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vsubq_f32(mValue, inV2.mValue); +#else + return Vec3(mF32[0] - inV2.mF32[0], mF32[1] - inV2.mF32[1], mF32[2] - inV2.mF32[2]); +#endif +} + +Vec3 &Vec3::operator -= (Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_sub_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + mValue = vsubq_f32(mValue, inV2.mValue); +#else + for (int i = 0; i < 3; ++i) + mF32[i] -= inV2.mF32[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF32[3] = mF32[2]; + #endif +#endif + return *this; +} + +Vec3 Vec3::operator / (Vec3Arg inV2) const +{ + inV2.CheckW(); // Check W equals Z to avoid div by zero +#if defined(JPH_USE_SSE) + return _mm_div_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vdivq_f32(mValue, inV2.mValue); +#else + return Vec3(mF32[0] / inV2.mF32[0], mF32[1] / inV2.mF32[1], mF32[2] / inV2.mF32[2]); +#endif +} + +Vec4 Vec3::SplatX() 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 Vec4(mF32[0], mF32[0], mF32[0], mF32[0]); +#endif +} + +Vec4 Vec3::SplatY() 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 Vec4(mF32[1], mF32[1], mF32[1], mF32[1]); +#endif +} + +Vec4 Vec3::SplatZ() 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 Vec4(mF32[2], mF32[2], mF32[2], mF32[2]); +#endif +} + +int Vec3::GetLowestComponentIndex() const +{ + return GetX() < GetY() ? (GetZ() < GetX() ? 2 : 0) : (GetZ() < GetY() ? 2 : 1); +} + +int Vec3::GetHighestComponentIndex() const +{ + return GetX() > GetY() ? (GetZ() > GetX() ? 2 : 0) : (GetZ() > GetY() ? 2 : 1); +} + +Vec3 Vec3::Abs() const +{ +#if defined(JPH_USE_AVX512) + return _mm_range_ps(mValue, mValue, 0b1000); +#elif defined(JPH_USE_SSE) + return _mm_max_ps(_mm_sub_ps(_mm_setzero_ps(), mValue), mValue); +#elif defined(JPH_USE_NEON) + return vabsq_f32(mValue); +#else + return Vec3(abs(mF32[0]), abs(mF32[1]), abs(mF32[2])); +#endif +} + +Vec3 Vec3::Reciprocal() const +{ + return sReplicate(1.0f) / mValue; +} + +Vec3 Vec3::Cross(Vec3Arg inV2) const +{ +#if defined(JPH_USE_SSE) + Type t1 = _mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same + t1 = _mm_mul_ps(t1, mValue); + Type t2 = _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same + t2 = _mm_mul_ps(t2, inV2.mValue); + Type t3 = _mm_sub_ps(t1, t2); + return _mm_shuffle_ps(t3, t3, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same +#elif defined(JPH_USE_NEON) + Type t1 = JPH_NEON_SHUFFLE_F32x4(inV2.mValue, inV2.mValue, 1, 2, 0, 0); // Assure Z and W are the same + t1 = vmulq_f32(t1, mValue); + Type t2 = JPH_NEON_SHUFFLE_F32x4(mValue, mValue, 1, 2, 0, 0); // Assure Z and W are the same + t2 = vmulq_f32(t2, inV2.mValue); + Type t3 = vsubq_f32(t1, t2); + return JPH_NEON_SHUFFLE_F32x4(t3, t3, 1, 2, 0, 0); // Assure Z and W are the same +#else + return Vec3(mF32[1] * inV2.mF32[2] - mF32[2] * inV2.mF32[1], + mF32[2] * inV2.mF32[0] - mF32[0] * inV2.mF32[2], + mF32[0] * inV2.mF32[1] - mF32[1] * inV2.mF32[0]); +#endif +} + +Vec3 Vec3::DotV(Vec3Arg inV2) const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_dp_ps(mValue, inV2.mValue, 0x7f); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, inV2.mValue); + mul = vsetq_lane_f32(0, mul, 3); + return vdupq_n_f32(vaddvq_f32(mul)); +#else + float dot = 0.0f; + for (int i = 0; i < 3; i++) + dot += mF32[i] * inV2.mF32[i]; + return Vec3::sReplicate(dot); +#endif +} + +Vec4 Vec3::DotV4(Vec3Arg inV2) const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_dp_ps(mValue, inV2.mValue, 0x7f); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, inV2.mValue); + mul = vsetq_lane_f32(0, mul, 3); + return vdupq_n_f32(vaddvq_f32(mul)); +#else + float dot = 0.0f; + for (int i = 0; i < 3; i++) + dot += mF32[i] * inV2.mF32[i]; + return Vec4::sReplicate(dot); +#endif +} + +float Vec3::Dot(Vec3Arg inV2) const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_cvtss_f32(_mm_dp_ps(mValue, inV2.mValue, 0x7f)); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, inV2.mValue); + mul = vsetq_lane_f32(0, mul, 3); + return vaddvq_f32(mul); +#else + float dot = 0.0f; + for (int i = 0; i < 3; i++) + dot += mF32[i] * inV2.mF32[i]; + return dot; +#endif +} + +float Vec3::LengthSq() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_cvtss_f32(_mm_dp_ps(mValue, mValue, 0x7f)); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, mValue); + mul = vsetq_lane_f32(0, mul, 3); + return vaddvq_f32(mul); +#else + float len_sq = 0.0f; + for (int i = 0; i < 3; i++) + len_sq += mF32[i] * mF32[i]; + return len_sq; +#endif +} + +float Vec3::Length() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_cvtss_f32(_mm_sqrt_ss(_mm_dp_ps(mValue, mValue, 0x7f))); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, mValue); + mul = vsetq_lane_f32(0, mul, 3); + float32x2_t sum = vdup_n_f32(vaddvq_f32(mul)); + return vget_lane_f32(vsqrt_f32(sum), 0); +#else + return sqrt(LengthSq()); +#endif +} + +Vec3 Vec3::Sqrt() const +{ +#if defined(JPH_USE_SSE) + return _mm_sqrt_ps(mValue); +#elif defined(JPH_USE_NEON) + return vsqrtq_f32(mValue); +#else + return Vec3(sqrt(mF32[0]), sqrt(mF32[1]), sqrt(mF32[2])); +#endif +} + +Vec3 Vec3::Normalized() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_div_ps(mValue, _mm_sqrt_ps(_mm_dp_ps(mValue, mValue, 0x7f))); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, mValue); + mul = vsetq_lane_f32(0, mul, 3); + float32x4_t sum = vdupq_n_f32(vaddvq_f32(mul)); + return vdivq_f32(mValue, vsqrtq_f32(sum)); +#else + return *this / Length(); +#endif +} + +Vec3 Vec3::NormalizedOr(Vec3Arg inZeroValue) const +{ +#if defined(JPH_USE_SSE4_1) && !defined(JPH_PLATFORM_WASM) // _mm_blendv_ps has problems on FireFox + Type len_sq = _mm_dp_ps(mValue, mValue, 0x7f); + // clang with '-ffast-math' (which you should not use!) can generate _mm_rsqrt_ps + // instructions which produce INFs/NaNs when they get a denormal float as input. + // We therefore treat denormals as zero here. + Type is_zero = _mm_cmple_ps(len_sq, _mm_set1_ps(FLT_MIN)); +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + if (_mm_movemask_ps(is_zero) == 0xf) + return inZeroValue; + else + return _mm_div_ps(mValue, _mm_sqrt_ps(len_sq)); +#else + return _mm_blendv_ps(_mm_div_ps(mValue, _mm_sqrt_ps(len_sq)), inZeroValue.mValue, is_zero); +#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, mValue); + mul = vsetq_lane_f32(0, mul, 3); + float32x4_t len_sq = vdupq_n_f32(vaddvq_f32(mul)); + uint32x4_t is_zero = vcleq_f32(len_sq, vdupq_n_f32(FLT_MIN)); + return vbslq_f32(is_zero, inZeroValue.mValue, vdivq_f32(mValue, vsqrtq_f32(len_sq))); +#else + float len_sq = LengthSq(); + if (len_sq <= FLT_MIN) + return inZeroValue; + else + return *this / sqrt(len_sq); +#endif +} + +bool Vec3::IsNormalized(float inTolerance) const +{ + return abs(LengthSq() - 1.0f) <= inTolerance; +} + +bool Vec3::IsNaN() const +{ +#if defined(JPH_USE_AVX512) + return (_mm_fpclass_ps_mask(mValue, 0b10000001) & 0x7) != 0; +#elif defined(JPH_USE_SSE) + return (_mm_movemask_ps(_mm_cmpunord_ps(mValue, mValue)) & 0x7) != 0; +#elif defined(JPH_USE_NEON) + uint32x4_t mask = JPH_NEON_UINT32x4(1, 1, 1, 0); + uint32x4_t is_equal = vceqq_f32(mValue, mValue); // If a number is not equal to itself it's a NaN + return vaddvq_u32(vandq_u32(is_equal, mask)) != 3; +#else + return isnan(mF32[0]) || isnan(mF32[1]) || isnan(mF32[2]); +#endif +} + +void Vec3::StoreFloat3(Float3 *outV) const +{ +#if defined(JPH_USE_SSE) + _mm_store_ss(&outV->x, mValue); + Vec3 t = Swizzle(); + _mm_store_ss(&outV->y, t.mValue); + t = t.Swizzle(); + _mm_store_ss(&outV->z, t.mValue); +#elif defined(JPH_USE_NEON) + float32x2_t xy = vget_low_f32(mValue); + vst1_f32(&outV->x, xy); + vst1q_lane_f32(&outV->z, mValue, 2); +#else + outV->x = mF32[0]; + outV->y = mF32[1]; + outV->z = mF32[2]; +#endif +} + +UVec4 Vec3::ToInt() const +{ +#if defined(JPH_USE_SSE) + return _mm_cvttps_epi32(mValue); +#elif defined(JPH_USE_NEON) + return vcvtq_u32_f32(mValue); +#else + return UVec4(uint32(mF32[0]), uint32(mF32[1]), uint32(mF32[2]), uint32(mF32[3])); +#endif +} + +UVec4 Vec3::ReinterpretAsInt() const +{ +#if defined(JPH_USE_SSE) + return UVec4(_mm_castps_si128(mValue)); +#elif defined(JPH_USE_NEON) + return vreinterpretq_u32_f32(mValue); +#else + return *reinterpret_cast(this); +#endif +} + +float Vec3::ReduceMin() const +{ + Vec3 v = sMin(mValue, Swizzle()); + v = sMin(v, v.Swizzle()); + return v.GetX(); +} + +float Vec3::ReduceMax() const +{ + Vec3 v = sMax(mValue, Swizzle()); + v = sMax(v, v.Swizzle()); + return v.GetX(); +} + +Vec3 Vec3::GetNormalizedPerpendicular() const +{ + if (abs(mF32[0]) > abs(mF32[1])) + { + float len = sqrt(mF32[0] * mF32[0] + mF32[2] * mF32[2]); + return Vec3(mF32[2], 0.0f, -mF32[0]) / len; + } + else + { + float len = sqrt(mF32[1] * mF32[1] + mF32[2] * mF32[2]); + return Vec3(0.0f, mF32[2], -mF32[1]) / len; + } +} + +Vec3 Vec3::GetSign() const +{ +#if defined(JPH_USE_AVX512) + return _mm_fixupimm_ps(mValue, mValue, _mm_set1_epi32(0xA9A90A00), 0); +#elif defined(JPH_USE_SSE) + Type minus_one = _mm_set1_ps(-1.0f); + Type one = _mm_set1_ps(1.0f); + return _mm_or_ps(_mm_and_ps(mValue, minus_one), one); +#elif defined(JPH_USE_NEON) + Type minus_one = vdupq_n_f32(-1.0f); + Type one = vdupq_n_f32(1.0f); + return vreinterpretq_f32_u32(vorrq_u32(vandq_u32(vreinterpretq_u32_f32(mValue), vreinterpretq_u32_f32(minus_one)), vreinterpretq_u32_f32(one))); +#else + return Vec3(std::signbit(mF32[0])? -1.0f : 1.0f, + std::signbit(mF32[1])? -1.0f : 1.0f, + std::signbit(mF32[2])? -1.0f : 1.0f); +#endif +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Vec4.h b/thirdparty/jolt_physics/Jolt/Math/Vec4.h new file mode 100644 index 000000000000..eb924a0f679c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Vec4.h @@ -0,0 +1,283 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Vec4 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // Underlying vector type +#if defined(JPH_USE_SSE) + using Type = __m128; +#elif defined(JPH_USE_NEON) + using Type = float32x4_t; +#else + using Type = struct { float mData[4]; }; +#endif + + /// Constructor + Vec4() = default; ///< Intentionally not initialized for performance reasons + Vec4(const Vec4 &inRHS) = default; + Vec4 & operator = (const Vec4 &inRHS) = default; + explicit JPH_INLINE Vec4(Vec3Arg inRHS); ///< WARNING: W component undefined! + JPH_INLINE Vec4(Vec3Arg inRHS, float inW); + JPH_INLINE Vec4(Type inRHS) : mValue(inRHS) { } + + /// Create a vector from 4 components + JPH_INLINE Vec4(float inX, float inY, float inZ, float inW); + + /// Vector with all zeros + static JPH_INLINE Vec4 sZero(); + + /// Vector with all NaN's + static JPH_INLINE Vec4 sNaN(); + + /// Replicate inV across all components + static JPH_INLINE Vec4 sReplicate(float inV); + + /// Load 4 floats from memory + static JPH_INLINE Vec4 sLoadFloat4(const Float4 *inV); + + /// Load 4 floats from memory, 16 bytes aligned + static JPH_INLINE Vec4 sLoadFloat4Aligned(const Float4 *inV); + + /// Gather 4 floats from memory at inBase + inOffsets[i] * Scale + template + static JPH_INLINE Vec4 sGatherFloat4(const float *inBase, UVec4Arg inOffsets); + + /// Return the minimum value of each of the components + static JPH_INLINE Vec4 sMin(Vec4Arg inV1, Vec4Arg inV2); + + /// Return the maximum of each of the components + static JPH_INLINE Vec4 sMax(Vec4Arg inV1, Vec4Arg inV2); + + /// Equals (component wise) + static JPH_INLINE UVec4 sEquals(Vec4Arg inV1, Vec4Arg inV2); + + /// Less than (component wise) + static JPH_INLINE UVec4 sLess(Vec4Arg inV1, Vec4Arg inV2); + + /// Less than or equal (component wise) + static JPH_INLINE UVec4 sLessOrEqual(Vec4Arg inV1, Vec4Arg inV2); + + /// Greater than (component wise) + static JPH_INLINE UVec4 sGreater(Vec4Arg inV1, Vec4Arg inV2); + + /// Greater than or equal (component wise) + static JPH_INLINE UVec4 sGreaterOrEqual(Vec4Arg inV1, Vec4Arg inV2); + + /// Calculates inMul1 * inMul2 + inAdd + static JPH_INLINE Vec4 sFusedMultiplyAdd(Vec4Arg inMul1, Vec4Arg inMul2, Vec4Arg inAdd); + + /// Component wise select, returns inNotSet when highest bit of inControl = 0 and inSet when highest bit of inControl = 1 + static JPH_INLINE Vec4 sSelect(Vec4Arg inNotSet, Vec4Arg inSet, UVec4Arg inControl); + + /// Logical or (component wise) + static JPH_INLINE Vec4 sOr(Vec4Arg inV1, Vec4Arg inV2); + + /// Logical xor (component wise) + static JPH_INLINE Vec4 sXor(Vec4Arg inV1, Vec4Arg inV2); + + /// Logical and (component wise) + static JPH_INLINE Vec4 sAnd(Vec4Arg inV1, Vec4Arg inV2); + + /// Sort the four elements of ioValue and sort ioIndex at the same time. + /// Based on a sorting network: http://en.wikipedia.org/wiki/Sorting_network + static JPH_INLINE void sSort4(Vec4 &ioValue, UVec4 &ioIndex); + + /// Reverse sort the four elements of ioValue (highest first) and sort ioIndex at the same time. + /// Based on a sorting network: http://en.wikipedia.org/wiki/Sorting_network + static JPH_INLINE void sSort4Reverse(Vec4 &ioValue, UVec4 &ioIndex); + + /// Get individual components +#if defined(JPH_USE_SSE) + JPH_INLINE float GetX() const { return _mm_cvtss_f32(mValue); } + JPH_INLINE float GetY() const { return mF32[1]; } + JPH_INLINE float GetZ() const { return mF32[2]; } + JPH_INLINE float GetW() const { return mF32[3]; } +#elif defined(JPH_USE_NEON) + JPH_INLINE float GetX() const { return vgetq_lane_f32(mValue, 0); } + JPH_INLINE float GetY() const { return vgetq_lane_f32(mValue, 1); } + JPH_INLINE float GetZ() const { return vgetq_lane_f32(mValue, 2); } + JPH_INLINE float GetW() const { return vgetq_lane_f32(mValue, 3); } +#else + JPH_INLINE float GetX() const { return mF32[0]; } + JPH_INLINE float GetY() const { return mF32[1]; } + JPH_INLINE float GetZ() const { return mF32[2]; } + JPH_INLINE float GetW() const { return mF32[3]; } +#endif + + /// Set individual components + JPH_INLINE void SetX(float inX) { mF32[0] = inX; } + JPH_INLINE void SetY(float inY) { mF32[1] = inY; } + JPH_INLINE void SetZ(float inZ) { mF32[2] = inZ; } + JPH_INLINE void SetW(float inW) { mF32[3] = inW; } + + /// Set all components + JPH_INLINE void Set(float inX, float inY, float inZ, float inW) { *this = Vec4(inX, inY, inZ, inW); } + + /// Get float component by index + JPH_INLINE float operator [] (uint inCoordinate) const { JPH_ASSERT(inCoordinate < 4); return mF32[inCoordinate]; } + JPH_INLINE float & operator [] (uint inCoordinate) { JPH_ASSERT(inCoordinate < 4); return mF32[inCoordinate]; } + + /// Comparison + JPH_INLINE bool operator == (Vec4Arg inV2) const; + JPH_INLINE bool operator != (Vec4Arg inV2) const { return !(*this == inV2); } + + /// Test if two vectors are close + JPH_INLINE bool IsClose(Vec4Arg inV2, float inMaxDistSq = 1.0e-12f) const; + + /// Test if vector is normalized + JPH_INLINE bool IsNormalized(float inTolerance = 1.0e-6f) const; + + /// Test if vector contains NaN elements + JPH_INLINE bool IsNaN() const; + + /// Multiply two float vectors (component wise) + JPH_INLINE Vec4 operator * (Vec4Arg inV2) const; + + /// Multiply vector with float + JPH_INLINE Vec4 operator * (float inV2) const; + + /// Multiply vector with float + friend JPH_INLINE Vec4 operator * (float inV1, Vec4Arg inV2); + + /// Divide vector by float + JPH_INLINE Vec4 operator / (float inV2) const; + + /// Multiply vector with float + JPH_INLINE Vec4 & operator *= (float inV2); + + /// Multiply vector with vector + JPH_INLINE Vec4 & operator *= (Vec4Arg inV2); + + /// Divide vector by float + JPH_INLINE Vec4 & operator /= (float inV2); + + /// Add two float vectors (component wise) + JPH_INLINE Vec4 operator + (Vec4Arg inV2) const; + + /// Add two float vectors (component wise) + JPH_INLINE Vec4 & operator += (Vec4Arg inV2); + + /// Negate + JPH_INLINE Vec4 operator - () const; + + /// Subtract two float vectors (component wise) + JPH_INLINE Vec4 operator - (Vec4Arg inV2) const; + + /// Subtract two float vectors (component wise) + JPH_INLINE Vec4 & operator -= (Vec4Arg inV2); + + /// Divide (component wise) + JPH_INLINE Vec4 operator / (Vec4Arg inV2) const; + + /// Swizzle the elements in inV + template + JPH_INLINE Vec4 Swizzle() const; + + /// Replicate the X component to all components + JPH_INLINE Vec4 SplatX() const; + + /// Replicate the Y component to all components + JPH_INLINE Vec4 SplatY() const; + + /// Replicate the Z component to all components + JPH_INLINE Vec4 SplatZ() const; + + /// Replicate the W component to all components + JPH_INLINE Vec4 SplatW() 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 + JPH_INLINE Vec4 DotV(Vec4Arg inV2) const; + + /// Dot product + JPH_INLINE float Dot(Vec4Arg inV2) const; + + /// Squared length of vector + JPH_INLINE float LengthSq() const; + + /// Length of vector + JPH_INLINE float Length() const; + + /// Normalize vector + JPH_INLINE Vec4 Normalized() const; + + /// Store 4 floats to memory + JPH_INLINE void StoreFloat4(Float4 *outV) const; + + /// Convert each component from a float to an int + JPH_INLINE UVec4 ToInt() const; + + /// Reinterpret Vec4 as a UVec4 (doesn't change the bits) + JPH_INLINE UVec4 ReinterpretAsInt() const; + + /// Store if X is negative in bit 0, Y in bit 1, Z in bit 2 and W in bit 3 + JPH_INLINE int GetSignBits() const; + + /// Get the minimum of X, Y, Z and W + JPH_INLINE float ReduceMin() const; + + /// Get the maximum of X, Y, Z and W + JPH_INLINE float ReduceMax() const; + + /// Component wise square root + JPH_INLINE Vec4 Sqrt() const; + + /// Get vector that contains the sign of each element (returns 1.0f if positive, -1.0f if negative) + JPH_INLINE Vec4 GetSign() const; + + /// Calculate the sine and cosine for each element of this vector (input in radians) + inline void SinCos(Vec4 &outSin, Vec4 &outCos) const; + + /// Calculate the tangent for each element of this vector (input in radians) + inline Vec4 Tan() const; + + /// Calculate the arc sine for each element of this vector (returns value in the range [-PI / 2, PI / 2]) + /// Note that all input values will be clamped to the range [-1, 1] and this function will not return NaNs like std::asin + inline Vec4 ASin() const; + + /// Calculate the arc cosine for each element of this vector (returns value in the range [0, PI]) + /// Note that all input values will be clamped to the range [-1, 1] and this function will not return NaNs like std::acos + inline Vec4 ACos() const; + + /// Calculate the arc tangent for each element of this vector (returns value in the range [-PI / 2, PI / 2]) + inline Vec4 ATan() const; + + /// 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); + + /// To String + friend ostream & operator << (ostream &inStream, Vec4Arg inV) + { + inStream << inV.mF32[0] << ", " << inV.mF32[1] << ", " << inV.mF32[2] << ", " << inV.mF32[3]; + return inStream; + } + + union + { + Type mValue; + float mF32[4]; + }; +}; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "Vec4.inl" diff --git a/thirdparty/jolt_physics/Jolt/Math/Vec4.inl b/thirdparty/jolt_physics/Jolt/Math/Vec4.inl new file mode 100644 index 000000000000..43676361c57e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Vec4.inl @@ -0,0 +1,981 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +// Constructor +Vec4::Vec4(Vec3Arg inRHS) : + mValue(inRHS.mValue) +{ +} + +Vec4::Vec4(Vec3Arg inRHS, float inW) +{ +#if defined(JPH_USE_SSE4_1) + mValue = _mm_blend_ps(inRHS.mValue, _mm_set1_ps(inW), 8); +#elif defined(JPH_USE_NEON) + mValue = vsetq_lane_f32(inW, inRHS.mValue, 3); +#else + for (int i = 0; i < 3; i++) + mF32[i] = inRHS.mF32[i]; + mF32[3] = inW; +#endif +} + +Vec4::Vec4(float inX, float inY, float inZ, float inW) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_set_ps(inW, inZ, inY, inX); +#elif defined(JPH_USE_NEON) + uint32x2_t xy = vcreate_u32(static_cast(BitCast(inX)) | (static_cast(BitCast(inY)) << 32)); + uint32x2_t zw = vcreate_u32(static_cast(BitCast(inZ)) | (static_cast(BitCast(inW)) << 32)); + mValue = vreinterpretq_f32_u32(vcombine_u32(xy, zw)); +#else + mF32[0] = inX; + mF32[1] = inY; + mF32[2] = inZ; + mF32[3] = inW; +#endif +} + +template +Vec4 Vec4::Swizzle() const +{ + static_assert(SwizzleX <= 3, "SwizzleX template parameter out of range"); + static_assert(SwizzleY <= 3, "SwizzleY template parameter out of range"); + static_assert(SwizzleZ <= 3, "SwizzleZ template parameter out of range"); + static_assert(SwizzleW <= 3, "SwizzleW template parameter out of range"); + +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(SwizzleW, SwizzleZ, SwizzleY, SwizzleX)); +#elif defined(JPH_USE_NEON) + return JPH_NEON_SHUFFLE_F32x4(mValue, mValue, SwizzleX, SwizzleY, SwizzleZ, SwizzleW); +#else + return Vec4(mF32[SwizzleX], mF32[SwizzleY], mF32[SwizzleZ], mF32[SwizzleW]); +#endif +} + +Vec4 Vec4::sZero() +{ +#if defined(JPH_USE_SSE) + return _mm_setzero_ps(); +#elif defined(JPH_USE_NEON) + return vdupq_n_f32(0); +#else + return Vec4(0, 0, 0, 0); +#endif +} + +Vec4 Vec4::sReplicate(float inV) +{ +#if defined(JPH_USE_SSE) + return _mm_set1_ps(inV); +#elif defined(JPH_USE_NEON) + return vdupq_n_f32(inV); +#else + return Vec4(inV, inV, inV, inV); +#endif +} + +Vec4 Vec4::sNaN() +{ + return sReplicate(numeric_limits::quiet_NaN()); +} + +Vec4 Vec4::sLoadFloat4(const Float4 *inV) +{ +#if defined(JPH_USE_SSE) + return _mm_loadu_ps(&inV->x); +#elif defined(JPH_USE_NEON) + return vld1q_f32(&inV->x); +#else + return Vec4(inV->x, inV->y, inV->z, inV->w); +#endif +} + +Vec4 Vec4::sLoadFloat4Aligned(const Float4 *inV) +{ +#if defined(JPH_USE_SSE) + return _mm_load_ps(&inV->x); +#elif defined(JPH_USE_NEON) + return vld1q_f32(&inV->x); +#else + return Vec4(inV->x, inV->y, inV->z, inV->w); +#endif +} + +template +Vec4 Vec4::sGatherFloat4(const float *inBase, UVec4Arg inOffsets) +{ +#if defined(JPH_USE_SSE) + #ifdef JPH_USE_AVX2 + return _mm_i32gather_ps(inBase, inOffsets.mValue, Scale); + #else + const uint8 *base = reinterpret_cast(inBase); + Type x = _mm_load_ss(reinterpret_cast(base + inOffsets.GetX() * Scale)); + Type y = _mm_load_ss(reinterpret_cast(base + inOffsets.GetY() * Scale)); + Type xy = _mm_unpacklo_ps(x, y); + Type z = _mm_load_ss(reinterpret_cast(base + inOffsets.GetZ() * Scale)); + Type w = _mm_load_ss(reinterpret_cast(base + inOffsets.GetW() * Scale)); + Type zw = _mm_unpacklo_ps(z, w); + return _mm_movelh_ps(xy, zw); + #endif +#else + const uint8 *base = reinterpret_cast(inBase); + float x = *reinterpret_cast(base + inOffsets.GetX() * Scale); + float y = *reinterpret_cast(base + inOffsets.GetY() * Scale); + float z = *reinterpret_cast(base + inOffsets.GetZ() * Scale); + float w = *reinterpret_cast(base + inOffsets.GetW() * Scale); + return Vec4(x, y, z, w); +#endif +} + +Vec4 Vec4::sMin(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_min_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vminq_f32(inV1.mValue, inV2.mValue); +#else + return Vec4(min(inV1.mF32[0], inV2.mF32[0]), + min(inV1.mF32[1], inV2.mF32[1]), + min(inV1.mF32[2], inV2.mF32[2]), + min(inV1.mF32[3], inV2.mF32[3])); +#endif +} + +Vec4 Vec4::sMax(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_max_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmaxq_f32(inV1.mValue, inV2.mValue); +#else + return Vec4(max(inV1.mF32[0], inV2.mF32[0]), + max(inV1.mF32[1], inV2.mF32[1]), + max(inV1.mF32[2], inV2.mF32[2]), + max(inV1.mF32[3], inV2.mF32[3])); +#endif +} + +UVec4 Vec4::sEquals(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmpeq_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vceqq_f32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mF32[0] == inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] == inV2.mF32[1]? 0xffffffffu : 0, + inV1.mF32[2] == inV2.mF32[2]? 0xffffffffu : 0, + inV1.mF32[3] == inV2.mF32[3]? 0xffffffffu : 0); +#endif +} + +UVec4 Vec4::sLess(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmplt_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcltq_f32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mF32[0] < inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] < inV2.mF32[1]? 0xffffffffu : 0, + inV1.mF32[2] < inV2.mF32[2]? 0xffffffffu : 0, + inV1.mF32[3] < inV2.mF32[3]? 0xffffffffu : 0); +#endif +} + +UVec4 Vec4::sLessOrEqual(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmple_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcleq_f32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mF32[0] <= inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] <= inV2.mF32[1]? 0xffffffffu : 0, + inV1.mF32[2] <= inV2.mF32[2]? 0xffffffffu : 0, + inV1.mF32[3] <= inV2.mF32[3]? 0xffffffffu : 0); +#endif +} + +UVec4 Vec4::sGreater(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmpgt_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcgtq_f32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mF32[0] > inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] > inV2.mF32[1]? 0xffffffffu : 0, + inV1.mF32[2] > inV2.mF32[2]? 0xffffffffu : 0, + inV1.mF32[3] > inV2.mF32[3]? 0xffffffffu : 0); +#endif +} + +UVec4 Vec4::sGreaterOrEqual(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmpge_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcgeq_f32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mF32[0] >= inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] >= inV2.mF32[1]? 0xffffffffu : 0, + inV1.mF32[2] >= inV2.mF32[2]? 0xffffffffu : 0, + inV1.mF32[3] >= inV2.mF32[3]? 0xffffffffu : 0); +#endif +} + +Vec4 Vec4::sFusedMultiplyAdd(Vec4Arg inMul1, Vec4Arg inMul2, Vec4Arg inAdd) +{ +#if defined(JPH_USE_SSE) + #ifdef JPH_USE_FMADD + return _mm_fmadd_ps(inMul1.mValue, inMul2.mValue, inAdd.mValue); + #else + return _mm_add_ps(_mm_mul_ps(inMul1.mValue, inMul2.mValue), inAdd.mValue); + #endif +#elif defined(JPH_USE_NEON) + return vmlaq_f32(inAdd.mValue, inMul1.mValue, inMul2.mValue); +#else + return Vec4(inMul1.mF32[0] * inMul2.mF32[0] + inAdd.mF32[0], + inMul1.mF32[1] * inMul2.mF32[1] + inAdd.mF32[1], + inMul1.mF32[2] * inMul2.mF32[2] + inAdd.mF32[2], + inMul1.mF32[3] * inMul2.mF32[3] + inAdd.mF32[3]); +#endif +} + +Vec4 Vec4::sSelect(Vec4Arg inNotSet, Vec4Arg inSet, UVec4Arg inControl) +{ +#if defined(JPH_USE_SSE4_1) && !defined(JPH_PLATFORM_WASM) // _mm_blendv_ps has problems on FireFox + return _mm_blendv_ps(inNotSet.mValue, inSet.mValue, _mm_castsi128_ps(inControl.mValue)); +#elif defined(JPH_USE_SSE) + __m128 is_set = _mm_castsi128_ps(_mm_srai_epi32(inControl.mValue, 31)); + return _mm_or_ps(_mm_and_ps(is_set, inSet.mValue), _mm_andnot_ps(is_set, inNotSet.mValue)); +#elif defined(JPH_USE_NEON) + return vbslq_f32(vreinterpretq_u32_s32(vshrq_n_s32(vreinterpretq_s32_u32(inControl.mValue), 31)), inSet.mValue, inNotSet.mValue); +#else + Vec4 result; + for (int i = 0; i < 4; i++) + result.mF32[i] = (inControl.mU32[i] & 0x80000000u) ? inSet.mF32[i] : inNotSet.mF32[i]; + return result; +#endif +} + +Vec4 Vec4::sOr(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_or_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vreinterpretq_f32_u32(vorrq_u32(vreinterpretq_u32_f32(inV1.mValue), vreinterpretq_u32_f32(inV2.mValue))); +#else + return UVec4::sOr(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat(); +#endif +} + +Vec4 Vec4::sXor(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_xor_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vreinterpretq_f32_u32(veorq_u32(vreinterpretq_u32_f32(inV1.mValue), vreinterpretq_u32_f32(inV2.mValue))); +#else + return UVec4::sXor(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat(); +#endif +} + +Vec4 Vec4::sAnd(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_and_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vreinterpretq_f32_u32(vandq_u32(vreinterpretq_u32_f32(inV1.mValue), vreinterpretq_u32_f32(inV2.mValue))); +#else + return UVec4::sAnd(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat(); +#endif +} + +void Vec4::sSort4(Vec4 &ioValue, UVec4 &ioIndex) +{ + // Pass 1, test 1st vs 3rd, 2nd vs 4th + Vec4 v1 = ioValue.Swizzle(); + UVec4 i1 = ioIndex.Swizzle(); + UVec4 c1 = sLess(ioValue, v1).Swizzle(); + ioValue = sSelect(ioValue, v1, c1); + ioIndex = UVec4::sSelect(ioIndex, i1, c1); + + // Pass 2, test 1st vs 2nd, 3rd vs 4th + Vec4 v2 = ioValue.Swizzle(); + UVec4 i2 = ioIndex.Swizzle(); + UVec4 c2 = sLess(ioValue, v2).Swizzle(); + ioValue = sSelect(ioValue, v2, c2); + ioIndex = UVec4::sSelect(ioIndex, i2, c2); + + // Pass 3, test 2nd vs 3rd component + Vec4 v3 = ioValue.Swizzle(); + UVec4 i3 = ioIndex.Swizzle(); + UVec4 c3 = sLess(ioValue, v3).Swizzle(); + ioValue = sSelect(ioValue, v3, c3); + ioIndex = UVec4::sSelect(ioIndex, i3, c3); +} + +void Vec4::sSort4Reverse(Vec4 &ioValue, UVec4 &ioIndex) +{ + // Pass 1, test 1st vs 3rd, 2nd vs 4th + Vec4 v1 = ioValue.Swizzle(); + UVec4 i1 = ioIndex.Swizzle(); + UVec4 c1 = sGreater(ioValue, v1).Swizzle(); + ioValue = sSelect(ioValue, v1, c1); + ioIndex = UVec4::sSelect(ioIndex, i1, c1); + + // Pass 2, test 1st vs 2nd, 3rd vs 4th + Vec4 v2 = ioValue.Swizzle(); + UVec4 i2 = ioIndex.Swizzle(); + UVec4 c2 = sGreater(ioValue, v2).Swizzle(); + ioValue = sSelect(ioValue, v2, c2); + ioIndex = UVec4::sSelect(ioIndex, i2, c2); + + // Pass 3, test 2nd vs 3rd component + Vec4 v3 = ioValue.Swizzle(); + UVec4 i3 = ioIndex.Swizzle(); + UVec4 c3 = sGreater(ioValue, v3).Swizzle(); + ioValue = sSelect(ioValue, v3, c3); + ioIndex = UVec4::sSelect(ioIndex, i3, c3); +} + +bool Vec4::operator == (Vec4Arg inV2) const +{ + return sEquals(*this, inV2).TestAllTrue(); +} + +bool Vec4::IsClose(Vec4Arg inV2, float inMaxDistSq) const +{ + return (inV2 - *this).LengthSq() <= inMaxDistSq; +} + +bool Vec4::IsNormalized(float inTolerance) const +{ + return abs(LengthSq() - 1.0f) <= inTolerance; +} + +bool Vec4::IsNaN() const +{ +#if defined(JPH_USE_AVX512) + return _mm_fpclass_ps_mask(mValue, 0b10000001) != 0; +#elif defined(JPH_USE_SSE) + return _mm_movemask_ps(_mm_cmpunord_ps(mValue, mValue)) != 0; +#elif defined(JPH_USE_NEON) + uint32x4_t is_equal = vceqq_f32(mValue, mValue); // If a number is not equal to itself it's a NaN + return vaddvq_u32(vshrq_n_u32(is_equal, 31)) != 4; +#else + return isnan(mF32[0]) || isnan(mF32[1]) || isnan(mF32[2]) || isnan(mF32[3]); +#endif +} + +Vec4 Vec4::operator * (Vec4Arg inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_mul_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmulq_f32(mValue, inV2.mValue); +#else + return Vec4(mF32[0] * inV2.mF32[0], + mF32[1] * inV2.mF32[1], + mF32[2] * inV2.mF32[2], + mF32[3] * inV2.mF32[3]); +#endif +} + +Vec4 Vec4::operator * (float inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_mul_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + return vmulq_n_f32(mValue, inV2); +#else + return Vec4(mF32[0] * inV2, mF32[1] * inV2, mF32[2] * inV2, mF32[3] * inV2); +#endif +} + +/// Multiply vector with float +Vec4 operator * (float inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_mul_ps(_mm_set1_ps(inV1), inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmulq_n_f32(inV2.mValue, inV1); +#else + return Vec4(inV1 * inV2.mF32[0], + inV1 * inV2.mF32[1], + inV1 * inV2.mF32[2], + inV1 * inV2.mF32[3]); +#endif +} + +Vec4 Vec4::operator / (float inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_div_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + return vdivq_f32(mValue, vdupq_n_f32(inV2)); +#else + return Vec4(mF32[0] / inV2, mF32[1] / inV2, mF32[2] / inV2, mF32[3] / inV2); +#endif +} + +Vec4 &Vec4::operator *= (float inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_mul_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + mValue = vmulq_n_f32(mValue, inV2); +#else + for (int i = 0; i < 4; ++i) + mF32[i] *= inV2; +#endif + return *this; +} + +Vec4 &Vec4::operator *= (Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_mul_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + mValue = vmulq_f32(mValue, inV2.mValue); +#else + for (int i = 0; i < 4; ++i) + mF32[i] *= inV2.mF32[i]; +#endif + return *this; +} + +Vec4 &Vec4::operator /= (float inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_div_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + mValue = vdivq_f32(mValue, vdupq_n_f32(inV2)); +#else + for (int i = 0; i < 4; ++i) + mF32[i] /= inV2; +#endif + return *this; +} + +Vec4 Vec4::operator + (Vec4Arg inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_add_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vaddq_f32(mValue, inV2.mValue); +#else + return Vec4(mF32[0] + inV2.mF32[0], + mF32[1] + inV2.mF32[1], + mF32[2] + inV2.mF32[2], + mF32[3] + inV2.mF32[3]); +#endif +} + +Vec4 &Vec4::operator += (Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_add_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + mValue = vaddq_f32(mValue, inV2.mValue); +#else + for (int i = 0; i < 4; ++i) + mF32[i] += inV2.mF32[i]; +#endif + return *this; +} + +Vec4 Vec4::operator - () const +{ +#if defined(JPH_USE_SSE) + return _mm_sub_ps(_mm_setzero_ps(), mValue); +#elif defined(JPH_USE_NEON) + #ifdef JPH_CROSS_PLATFORM_DETERMINISTIC + return vsubq_f32(vdupq_n_f32(0), mValue); + #else + return vnegq_f32(mValue); + #endif +#else + #ifdef JPH_CROSS_PLATFORM_DETERMINISTIC + return Vec4(0.0f - mF32[0], 0.0f - mF32[1], 0.0f - mF32[2], 0.0f - mF32[3]); + #else + return Vec4(-mF32[0], -mF32[1], -mF32[2], -mF32[3]); + #endif +#endif +} + +Vec4 Vec4::operator - (Vec4Arg inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_sub_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vsubq_f32(mValue, inV2.mValue); +#else + return Vec4(mF32[0] - inV2.mF32[0], + mF32[1] - inV2.mF32[1], + mF32[2] - inV2.mF32[2], + mF32[3] - inV2.mF32[3]); +#endif +} + +Vec4 &Vec4::operator -= (Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_sub_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + mValue = vsubq_f32(mValue, inV2.mValue); +#else + for (int i = 0; i < 4; ++i) + mF32[i] -= inV2.mF32[i]; +#endif + return *this; +} + +Vec4 Vec4::operator / (Vec4Arg inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_div_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vdivq_f32(mValue, inV2.mValue); +#else + return Vec4(mF32[0] / inV2.mF32[0], + mF32[1] / inV2.mF32[1], + mF32[2] / inV2.mF32[2], + mF32[3] / inV2.mF32[3]); +#endif +} + +Vec4 Vec4::SplatX() 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 Vec4(mF32[0], mF32[0], mF32[0], mF32[0]); +#endif +} + +Vec4 Vec4::SplatY() 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 Vec4(mF32[1], mF32[1], mF32[1], mF32[1]); +#endif +} + +Vec4 Vec4::SplatZ() 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 Vec4(mF32[2], mF32[2], mF32[2], mF32[2]); +#endif +} + +Vec4 Vec4::SplatW() 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 Vec4(mF32[3], mF32[3], mF32[3], mF32[3]); +#endif +} + +Vec4 Vec4::Abs() const +{ +#if defined(JPH_USE_AVX512) + return _mm_range_ps(mValue, mValue, 0b1000); +#elif defined(JPH_USE_SSE) + return _mm_max_ps(_mm_sub_ps(_mm_setzero_ps(), mValue), mValue); +#elif defined(JPH_USE_NEON) + return vabsq_f32(mValue); +#else + return Vec4(abs(mF32[0]), abs(mF32[1]), abs(mF32[2]), abs(mF32[3])); +#endif +} + +Vec4 Vec4::Reciprocal() const +{ + return sReplicate(1.0f) / mValue; +} + +Vec4 Vec4::DotV(Vec4Arg inV2) const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_dp_ps(mValue, inV2.mValue, 0xff); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, inV2.mValue); + return vdupq_n_f32(vaddvq_f32(mul)); +#else + // Brackets placed so that the order is consistent with the vectorized version + return Vec4::sReplicate((mF32[0] * inV2.mF32[0] + mF32[1] * inV2.mF32[1]) + (mF32[2] * inV2.mF32[2] + mF32[3] * inV2.mF32[3])); +#endif +} + +float Vec4::Dot(Vec4Arg inV2) const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_cvtss_f32(_mm_dp_ps(mValue, inV2.mValue, 0xff)); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, inV2.mValue); + return vaddvq_f32(mul); +#else + // Brackets placed so that the order is consistent with the vectorized version + return (mF32[0] * inV2.mF32[0] + mF32[1] * inV2.mF32[1]) + (mF32[2] * inV2.mF32[2] + mF32[3] * inV2.mF32[3]); +#endif +} + +float Vec4::LengthSq() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_cvtss_f32(_mm_dp_ps(mValue, mValue, 0xff)); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, mValue); + return vaddvq_f32(mul); +#else + // Brackets placed so that the order is consistent with the vectorized version + return (mF32[0] * mF32[0] + mF32[1] * mF32[1]) + (mF32[2] * mF32[2] + mF32[3] * mF32[3]); +#endif +} + +float Vec4::Length() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_cvtss_f32(_mm_sqrt_ss(_mm_dp_ps(mValue, mValue, 0xff))); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, mValue); + float32x2_t sum = vdup_n_f32(vaddvq_f32(mul)); + return vget_lane_f32(vsqrt_f32(sum), 0); +#else + // Brackets placed so that the order is consistent with the vectorized version + return sqrt((mF32[0] * mF32[0] + mF32[1] * mF32[1]) + (mF32[2] * mF32[2] + mF32[3] * mF32[3])); +#endif +} + +Vec4 Vec4::Sqrt() const +{ +#if defined(JPH_USE_SSE) + return _mm_sqrt_ps(mValue); +#elif defined(JPH_USE_NEON) + return vsqrtq_f32(mValue); +#else + return Vec4(sqrt(mF32[0]), sqrt(mF32[1]), sqrt(mF32[2]), sqrt(mF32[3])); +#endif +} + + +Vec4 Vec4::GetSign() const +{ +#if defined(JPH_USE_AVX512) + return _mm_fixupimm_ps(mValue, mValue, _mm_set1_epi32(0xA9A90A00), 0); +#elif defined(JPH_USE_SSE) + Type minus_one = _mm_set1_ps(-1.0f); + Type one = _mm_set1_ps(1.0f); + return _mm_or_ps(_mm_and_ps(mValue, minus_one), one); +#elif defined(JPH_USE_NEON) + Type minus_one = vdupq_n_f32(-1.0f); + Type one = vdupq_n_f32(1.0f); + return vreinterpretq_f32_u32(vorrq_u32(vandq_u32(vreinterpretq_u32_f32(mValue), vreinterpretq_u32_f32(minus_one)), vreinterpretq_u32_f32(one))); +#else + return Vec4(std::signbit(mF32[0])? -1.0f : 1.0f, + std::signbit(mF32[1])? -1.0f : 1.0f, + std::signbit(mF32[2])? -1.0f : 1.0f, + std::signbit(mF32[3])? -1.0f : 1.0f); +#endif +} + +Vec4 Vec4::Normalized() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_div_ps(mValue, _mm_sqrt_ps(_mm_dp_ps(mValue, mValue, 0xff))); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, mValue); + float32x4_t sum = vdupq_n_f32(vaddvq_f32(mul)); + return vdivq_f32(mValue, vsqrtq_f32(sum)); +#else + return *this / Length(); +#endif +} + +void Vec4::StoreFloat4(Float4 *outV) const +{ +#if defined(JPH_USE_SSE) + _mm_storeu_ps(&outV->x, mValue); +#elif defined(JPH_USE_NEON) + vst1q_f32(&outV->x, mValue); +#else + for (int i = 0; i < 4; ++i) + (&outV->x)[i] = mF32[i]; +#endif +} + +UVec4 Vec4::ToInt() const +{ +#if defined(JPH_USE_SSE) + return _mm_cvttps_epi32(mValue); +#elif defined(JPH_USE_NEON) + return vcvtq_u32_f32(mValue); +#else + return UVec4(uint32(mF32[0]), uint32(mF32[1]), uint32(mF32[2]), uint32(mF32[3])); +#endif +} + +UVec4 Vec4::ReinterpretAsInt() const +{ +#if defined(JPH_USE_SSE) + return UVec4(_mm_castps_si128(mValue)); +#elif defined(JPH_USE_NEON) + return vreinterpretq_u32_f32(mValue); +#else + return *reinterpret_cast(this); +#endif +} + +int Vec4::GetSignBits() const +{ +#if defined(JPH_USE_SSE) + return _mm_movemask_ps(mValue); +#elif defined(JPH_USE_NEON) + int32x4_t shift = JPH_NEON_INT32x4(0, 1, 2, 3); + return vaddvq_u32(vshlq_u32(vshrq_n_u32(vreinterpretq_u32_f32(mValue), 31), shift)); +#else + return (std::signbit(mF32[0])? 1 : 0) | (std::signbit(mF32[1])? 2 : 0) | (std::signbit(mF32[2])? 4 : 0) | (std::signbit(mF32[3])? 8 : 0); +#endif +} + +float Vec4::ReduceMin() const +{ + Vec4 v = sMin(mValue, Swizzle()); + v = sMin(v, v.Swizzle()); + return v.GetX(); +} + +float Vec4::ReduceMax() const +{ + Vec4 v = sMax(mValue, Swizzle()); + v = sMax(v, v.Swizzle()); + return v.GetX(); +} + +void Vec4::SinCos(Vec4 &outSin, Vec4 &outCos) const +{ + // Implementation based on sinf.c from the cephes library, combines sinf and cosf in a single function, changes octants to quadrants and vectorizes it + // Original implementation by Stephen L. Moshier (See: http://www.moshier.net/) + + // Make argument positive and remember sign for sin only since cos is symmetric around x (highest bit of a float is the sign bit) + UVec4 sin_sign = UVec4::sAnd(ReinterpretAsInt(), UVec4::sReplicate(0x80000000U)); + Vec4 x = Vec4::sXor(*this, sin_sign.ReinterpretAsFloat()); + + // x / (PI / 2) rounded to nearest int gives us the quadrant closest to x + UVec4 quadrant = (0.6366197723675814f * x + Vec4::sReplicate(0.5f)).ToInt(); + + // Make x relative to the closest quadrant. + // This does x = x - quadrant * PI / 2 using a two step Cody-Waite argument reduction. + // This improves the accuracy of the result by avoiding loss of significant bits in the subtraction. + // We start with x = x - quadrant * PI / 2, PI / 2 in hexadecimal notation is 0x3fc90fdb, we remove the lowest 16 bits to + // get 0x3fc90000 (= 1.5703125) this means we can now multiply with a number of up to 2^16 without losing any bits. + // This leaves us with: x = (x - quadrant * 1.5703125) - quadrant * (PI / 2 - 1.5703125). + // PI / 2 - 1.5703125 in hexadecimal is 0x39fdaa22, stripping the lowest 12 bits we get 0x39fda000 (= 0.0004837512969970703125) + // This leaves uw with: x = ((x - quadrant * 1.5703125) - quadrant * 0.0004837512969970703125) - quadrant * (PI / 2 - 1.5703125 - 0.0004837512969970703125) + // See: https://stackoverflow.com/questions/42455143/sine-cosine-modular-extended-precision-arithmetic + // After this we have x in the range [-PI / 4, PI / 4]. + Vec4 float_quadrant = quadrant.ToFloat(); + x = ((x - float_quadrant * 1.5703125f) - float_quadrant * 0.0004837512969970703125f) - float_quadrant * 7.549789948768648e-8f; + + // Calculate x2 = x^2 + Vec4 x2 = x * x; + + // Taylor expansion: + // Cos(x) = 1 - x^2/2! + x^4/4! - x^6/6! + x^8/8! + ... = (((x2/8!- 1/6!) * x2 + 1/4!) * x2 - 1/2!) * x2 + 1 + Vec4 taylor_cos = ((2.443315711809948e-5f * x2 - Vec4::sReplicate(1.388731625493765e-3f)) * x2 + Vec4::sReplicate(4.166664568298827e-2f)) * x2 * x2 - 0.5f * x2 + Vec4::sReplicate(1.0f); + // Sin(x) = x - x^3/3! + x^5/5! - x^7/7! + ... = ((-x2/7! + 1/5!) * x2 - 1/3!) * x2 * x + x + Vec4 taylor_sin = ((-1.9515295891e-4f * x2 + Vec4::sReplicate(8.3321608736e-3f)) * x2 - Vec4::sReplicate(1.6666654611e-1f)) * x2 * x + x; + + // The lowest 2 bits of quadrant indicate the quadrant that we are in. + // Let x be the original input value and x' our value that has been mapped to the range [-PI / 4, PI / 4]. + // since cos(x) = sin(x - PI / 2) and since we want to use the Taylor expansion as close as possible to 0, + // we can alternate between using the Taylor expansion for sin and cos according to the following table: + // + // quadrant sin(x) cos(x) + // XXX00b sin(x') cos(x') + // XXX01b cos(x') -sin(x') + // XXX10b -sin(x') -cos(x') + // XXX11b -cos(x') sin(x') + // + // So: sin_sign = bit2, cos_sign = bit1 ^ bit2, bit1 determines if we use sin or cos Taylor expansion + UVec4 bit1 = quadrant.LogicalShiftLeft<31>(); + UVec4 bit2 = UVec4::sAnd(quadrant.LogicalShiftLeft<30>(), UVec4::sReplicate(0x80000000U)); + + // Select which one of the results is sin and which one is cos + Vec4 s = Vec4::sSelect(taylor_sin, taylor_cos, bit1); + Vec4 c = Vec4::sSelect(taylor_cos, taylor_sin, bit1); + + // Update the signs + sin_sign = UVec4::sXor(sin_sign, bit2); + UVec4 cos_sign = UVec4::sXor(bit1, bit2); + + // Correct the signs + outSin = Vec4::sXor(s, sin_sign.ReinterpretAsFloat()); + outCos = Vec4::sXor(c, cos_sign.ReinterpretAsFloat()); +} + +Vec4 Vec4::Tan() const +{ + // Implementation based on tanf.c from the cephes library, see Vec4::SinCos for further details + // Original implementation by Stephen L. Moshier (See: http://www.moshier.net/) + + // Make argument positive + UVec4 tan_sign = UVec4::sAnd(ReinterpretAsInt(), UVec4::sReplicate(0x80000000U)); + Vec4 x = Vec4::sXor(*this, tan_sign.ReinterpretAsFloat()); + + // x / (PI / 2) rounded to nearest int gives us the quadrant closest to x + UVec4 quadrant = (0.6366197723675814f * x + Vec4::sReplicate(0.5f)).ToInt(); + + // Remap x to range [-PI / 4, PI / 4], see Vec4::SinCos + Vec4 float_quadrant = quadrant.ToFloat(); + x = ((x - float_quadrant * 1.5703125f) - float_quadrant * 0.0004837512969970703125f) - float_quadrant * 7.549789948768648e-8f; + + // Calculate x2 = x^2 + Vec4 x2 = x * x; + + // Roughly equivalent to the Taylor expansion: + // Tan(x) = x + x^3/3 + 2*x^5/15 + 17*x^7/315 + 62*x^9/2835 + ... + Vec4 tan = + (((((9.38540185543e-3f * x2 + Vec4::sReplicate(3.11992232697e-3f)) * x2 + Vec4::sReplicate(2.44301354525e-2f)) * x2 + + Vec4::sReplicate(5.34112807005e-2f)) * x2 + Vec4::sReplicate(1.33387994085e-1f)) * x2 + Vec4::sReplicate(3.33331568548e-1f)) * x2 * x + x; + + // For the 2nd and 4th quadrant we need to invert the value + UVec4 bit1 = quadrant.LogicalShiftLeft<31>(); + tan = Vec4::sSelect(tan, Vec4::sReplicate(-1.0f) / (tan JPH_IF_FLOATING_POINT_EXCEPTIONS_ENABLED(+ Vec4::sReplicate(FLT_MIN))), bit1); // Add small epsilon to prevent div by zero, works because tan is always positive + + // Put the sign back + return Vec4::sXor(tan, tan_sign.ReinterpretAsFloat()); +} + +Vec4 Vec4::ASin() const +{ + // Implementation based on asinf.c from the cephes library + // Original implementation by Stephen L. Moshier (See: http://www.moshier.net/) + + // Make argument positive + UVec4 asin_sign = UVec4::sAnd(ReinterpretAsInt(), UVec4::sReplicate(0x80000000U)); + Vec4 a = Vec4::sXor(*this, asin_sign.ReinterpretAsFloat()); + + // ASin is not defined outside the range [-1, 1] but it often happens that a value is slightly above 1 so we just clamp here + a = Vec4::sMin(a, Vec4::sReplicate(1.0f)); + + // When |x| <= 0.5 we use the asin approximation as is + Vec4 z1 = a * a; + Vec4 x1 = a; + + // When |x| > 0.5 we use the identity asin(x) = PI / 2 - 2 * asin(sqrt((1 - x) / 2)) + Vec4 z2 = 0.5f * (Vec4::sReplicate(1.0f) - a); + Vec4 x2 = z2.Sqrt(); + + // Select which of the two situations we have + UVec4 greater = Vec4::sGreater(a, Vec4::sReplicate(0.5f)); + Vec4 z = Vec4::sSelect(z1, z2, greater); + Vec4 x = Vec4::sSelect(x1, x2, greater); + + // Polynomial approximation of asin + z = ((((4.2163199048e-2f * z + Vec4::sReplicate(2.4181311049e-2f)) * z + Vec4::sReplicate(4.5470025998e-2f)) * z + Vec4::sReplicate(7.4953002686e-2f)) * z + Vec4::sReplicate(1.6666752422e-1f)) * z * x + x; + + // If |x| > 0.5 we need to apply the remainder of the identity above + z = Vec4::sSelect(z, Vec4::sReplicate(0.5f * JPH_PI) - (z + z), greater); + + // Put the sign back + return Vec4::sXor(z, asin_sign.ReinterpretAsFloat()); +} + +Vec4 Vec4::ACos() const +{ + // Not the most accurate, but simple + return Vec4::sReplicate(0.5f * JPH_PI) - ASin(); +} + +Vec4 Vec4::ATan() const +{ + // Implementation based on atanf.c from the cephes library + // Original implementation by Stephen L. Moshier (See: http://www.moshier.net/) + + // Make argument positive + UVec4 atan_sign = UVec4::sAnd(ReinterpretAsInt(), UVec4::sReplicate(0x80000000U)); + Vec4 x = Vec4::sXor(*this, atan_sign.ReinterpretAsFloat()); + Vec4 y = Vec4::sZero(); + + // If x > Tan(PI / 8) + UVec4 greater1 = Vec4::sGreater(x, Vec4::sReplicate(0.4142135623730950f)); + Vec4 x1 = (x - Vec4::sReplicate(1.0f)) / (x + Vec4::sReplicate(1.0f)); + + // If x > Tan(3 * PI / 8) + UVec4 greater2 = Vec4::sGreater(x, Vec4::sReplicate(2.414213562373095f)); + Vec4 x2 = Vec4::sReplicate(-1.0f) / (x JPH_IF_FLOATING_POINT_EXCEPTIONS_ENABLED(+ Vec4::sReplicate(FLT_MIN))); // Add small epsilon to prevent div by zero, works because x is always positive + + // Apply first if + x = Vec4::sSelect(x, x1, greater1); + y = Vec4::sSelect(y, Vec4::sReplicate(0.25f * JPH_PI), greater1); + + // Apply second if + x = Vec4::sSelect(x, x2, greater2); + y = Vec4::sSelect(y, Vec4::sReplicate(0.5f * JPH_PI), greater2); + + // Polynomial approximation + Vec4 z = x * x; + y += (((8.05374449538e-2f * z - Vec4::sReplicate(1.38776856032e-1f)) * z + Vec4::sReplicate(1.99777106478e-1f)) * z - Vec4::sReplicate(3.33329491539e-1f)) * z * x + x; + + // Put the sign back + return Vec4::sXor(y, atan_sign.ReinterpretAsFloat()); +} + +Vec4 Vec4::sATan2(Vec4Arg inY, Vec4Arg inX) +{ + UVec4 sign_mask = UVec4::sReplicate(0x80000000U); + + // Determine absolute value and sign of y + UVec4 y_sign = UVec4::sAnd(inY.ReinterpretAsInt(), sign_mask); + Vec4 y_abs = Vec4::sXor(inY, y_sign.ReinterpretAsFloat()); + + // Determine absolute value and sign of x + UVec4 x_sign = UVec4::sAnd(inX.ReinterpretAsInt(), sign_mask); + Vec4 x_abs = Vec4::sXor(inX, x_sign.ReinterpretAsFloat()); + + // Always divide smallest / largest to avoid dividing by zero + UVec4 x_is_numerator = Vec4::sLess(x_abs, y_abs); + Vec4 numerator = Vec4::sSelect(y_abs, x_abs, x_is_numerator); + Vec4 denominator = Vec4::sSelect(x_abs, y_abs, x_is_numerator); + Vec4 atan = (numerator / denominator).ATan(); + + // If we calculated x / y instead of y / x the result is PI / 2 - result (note that this is true because we know the result is positive because the input was positive) + atan = Vec4::sSelect(atan, Vec4::sReplicate(0.5f * JPH_PI) - atan, x_is_numerator); + + // Now we need to map to the correct quadrant + // x_sign y_sign result + // +1 +1 atan + // -1 +1 -atan + PI + // -1 -1 atan - PI + // +1 -1 -atan + // This can be written as: x_sign * y_sign * (atan - (x_sign < 0? PI : 0)) + atan -= Vec4::sAnd(x_sign.ArithmeticShiftRight<31>().ReinterpretAsFloat(), Vec4::sReplicate(JPH_PI)); + atan = Vec4::sXor(atan, UVec4::sXor(x_sign, y_sign).ReinterpretAsFloat()); + return atan; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Vector.h b/thirdparty/jolt_physics/Jolt/Math/Vector.h new file mode 100644 index 000000000000..b51a93c07b07 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Vector.h @@ -0,0 +1,211 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Templatized vector class +template +class [[nodiscard]] Vector +{ +public: + /// Constructor + inline Vector() = default; + inline Vector(const Vector &) = default; + + /// Dimensions + inline uint GetRows() const { return Rows; } + + /// Vector with all zeros + inline void SetZero() + { + for (uint r = 0; r < Rows; ++r) + mF32[r] = 0.0f; + } + + inline static Vector sZero() { Vector v; v.SetZero(); return v; } + + /// Copy a (part) of another vector into this vector + template + void CopyPart(const OtherVector &inV, uint inSourceRow, uint inNumRows, uint inDestRow) + { + for (uint r = 0; r < inNumRows; ++r) + mF32[inDestRow + r] = inV[inSourceRow + r]; + } + + /// Get float component by index + inline float operator [] (uint inCoordinate) const + { + JPH_ASSERT(inCoordinate < Rows); + return mF32[inCoordinate]; + } + + inline float & operator [] (uint inCoordinate) + { + JPH_ASSERT(inCoordinate < Rows); + return mF32[inCoordinate]; + } + + /// Comparison + inline bool operator == (const Vector &inV2) const + { + for (uint r = 0; r < Rows; ++r) + if (mF32[r] != inV2.mF32[r]) + return false; + return true; + } + + inline bool operator != (const Vector &inV2) const + { + for (uint r = 0; r < Rows; ++r) + if (mF32[r] != inV2.mF32[r]) + return true; + return false; + } + + /// Test if vector consists of all zeros + inline bool IsZero() const + { + for (uint r = 0; r < Rows; ++r) + if (mF32[r] != 0.0f) + return false; + return true; + } + + /// Test if two vectors are close to each other + inline bool IsClose(const Vector &inV2, float inMaxDistSq = 1.0e-12f) const + { + return (inV2 - *this).LengthSq() <= inMaxDistSq; + } + + /// Assignment + inline Vector & operator = (const Vector &) = default; + + /// Multiply vector with float + inline Vector operator * (const float inV2) const + { + Vector v; + for (uint r = 0; r < Rows; ++r) + v.mF32[r] = mF32[r] * inV2; + return v; + } + + inline Vector & operator *= (const float inV2) + { + for (uint r = 0; r < Rows; ++r) + mF32[r] *= inV2; + return *this; + } + + /// Multiply vector with float + inline friend Vector operator * (const float inV1, const Vector &inV2) + { + return inV2 * inV1; + } + + /// Divide vector by float + inline Vector operator / (float inV2) const + { + Vector v; + for (uint r = 0; r < Rows; ++r) + v.mF32[r] = mF32[r] / inV2; + return v; + } + + inline Vector & operator /= (float inV2) + { + for (uint r = 0; r < Rows; ++r) + mF32[r] /= inV2; + return *this; + } + + /// Add two float vectors (component wise) + inline Vector operator + (const Vector &inV2) const + { + Vector v; + for (uint r = 0; r < Rows; ++r) + v.mF32[r] = mF32[r] + inV2.mF32[r]; + return v; + } + + inline Vector & operator += (const Vector &inV2) + { + for (uint r = 0; r < Rows; ++r) + mF32[r] += inV2.mF32[r]; + return *this; + } + + /// Negate + inline Vector operator - () const + { + Vector v; + for (uint r = 0; r < Rows; ++r) + v.mF32[r] = -mF32[r]; + return v; + } + + /// Subtract two float vectors (component wise) + inline Vector operator - (const Vector &inV2) const + { + Vector v; + for (uint r = 0; r < Rows; ++r) + v.mF32[r] = mF32[r] - inV2.mF32[r]; + return v; + } + + inline Vector & operator -= (const Vector &inV2) + { + for (uint r = 0; r < Rows; ++r) + mF32[r] -= inV2.mF32[r]; + return *this; + } + + /// Dot product + inline float Dot(const Vector &inV2) const + { + float dot = 0.0f; + for (uint r = 0; r < Rows; ++r) + dot += mF32[r] * inV2.mF32[r]; + return dot; + } + + /// Squared length of vector + inline float LengthSq() const + { + return Dot(*this); + } + + /// Length of vector + inline float Length() const + { + return sqrt(LengthSq()); + } + + /// Check if vector is normalized + inline bool IsNormalized(float inToleranceSq = 1.0e-6f) + { + return abs(LengthSq() - 1.0f) <= inToleranceSq; + } + + /// Normalize vector + inline Vector Normalized() const + { + return *this / Length(); + } + + /// To String + friend ostream & operator << (ostream &inStream, const Vector &inV) + { + inStream << "["; + for (uint i = 0; i < Rows - 1; ++i) + inStream << inV.mF32[i] << ", "; + inStream << inV.mF32[Rows - 1] << "]"; + return inStream; + } + + float mF32[Rows]; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/GetPrimitiveTypeOfType.h b/thirdparty/jolt_physics/Jolt/ObjectStream/GetPrimitiveTypeOfType.h new file mode 100644 index 000000000000..cc8507636c54 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/GetPrimitiveTypeOfType.h @@ -0,0 +1,54 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Helper functions to get the underlying RTTI type of a type (so e.g. Array will return sometype) +template +const RTTI *GetPrimitiveTypeOfType(T *) +{ + return GetRTTIOfType((T *)nullptr); +} + +template +const RTTI *GetPrimitiveTypeOfType(T **) +{ + return GetRTTIOfType((T *)nullptr); +} + +template +const RTTI *GetPrimitiveTypeOfType(Ref *) +{ + return GetRTTIOfType((T *)nullptr); +} + +template +const RTTI *GetPrimitiveTypeOfType(RefConst *) +{ + return GetRTTIOfType((T *)nullptr); +} + +template +const RTTI *GetPrimitiveTypeOfType(Array *) +{ + return GetPrimitiveTypeOfType((T *)nullptr); +} + +template +const RTTI *GetPrimitiveTypeOfType(StaticArray *) +{ + return GetPrimitiveTypeOfType((T *)nullptr); +} + +template +const RTTI *GetPrimitiveTypeOfType(T (*)[N]) +{ + return GetPrimitiveTypeOfType((T *)nullptr); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStream.cpp b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStream.cpp new file mode 100644 index 000000000000..359a9e5548cd --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStream.cpp @@ -0,0 +1,38 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +// Define macro to declare functions for a specific primitive type +#define JPH_DECLARE_PRIMITIVE(name) \ + bool OSIsType(name *, int inArrayDepth, EOSDataType inDataType, const char *inClassName) \ + { \ + return inArrayDepth == 0 && inDataType == EOSDataType::T_##name; \ + } \ + bool OSReadData(IObjectStreamIn &ioStream, name &outPrimitive) \ + { \ + return ioStream.ReadPrimitiveData(outPrimitive); \ + } \ + void OSWriteDataType(IObjectStreamOut &ioStream, name *) \ + { \ + ioStream.WriteDataType(EOSDataType::T_##name); \ + } \ + void OSWriteData(IObjectStreamOut &ioStream, const name &inPrimitive) \ + { \ + ioStream.HintNextItem(); \ + ioStream.WritePrimitiveData(inPrimitive); \ + } + +// This file uses the JPH_DECLARE_PRIMITIVE macro to define all types +#include + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStream.h b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStream.h new file mode 100644 index 000000000000..adbfbb773c66 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStream.h @@ -0,0 +1,333 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +/// Base class for object stream input and output streams. +class JPH_EXPORT ObjectStream : public NonCopyable +{ +public: + /// Stream type + enum class EStreamType + { + Text, + Binary, + }; + +protected: + /// Destructor + virtual ~ObjectStream() = default; + + /// Identifier for objects + using Identifier = uint32; + + static constexpr int sVersion = 1; + static constexpr int sRevision = 0; + static constexpr Identifier sNullIdentifier = 0; +}; + +/// Interface class for reading from an object stream +class JPH_EXPORT IObjectStreamIn : public ObjectStream +{ +public: + ///@name Input type specific operations + virtual bool ReadDataType(EOSDataType &outType) = 0; + virtual bool ReadName(String &outName) = 0; + virtual bool ReadIdentifier(Identifier &outIdentifier) = 0; + virtual bool ReadCount(uint32 &outCount) = 0; + + ///@name Read primitives + virtual bool ReadPrimitiveData(uint8 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(uint16 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(int &outPrimitive) = 0; + virtual bool ReadPrimitiveData(uint32 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(uint64 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(float &outPrimitive) = 0; + virtual bool ReadPrimitiveData(double &outPrimitive) = 0; + virtual bool ReadPrimitiveData(bool &outPrimitive) = 0; + virtual bool ReadPrimitiveData(String &outPrimitive) = 0; + virtual bool ReadPrimitiveData(Float3 &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(Quat &outPrimitive) = 0; + virtual bool ReadPrimitiveData(Mat44 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(DMat44 &outPrimitive) = 0; + + ///@name Read compounds + virtual bool ReadClassData(const char *inClassName, void *inInstance) = 0; + virtual bool ReadPointerData(const RTTI *inRTTI, void **inPointer, int inRefCountOffset = -1) = 0; +}; + +/// Interface class for writing to an object stream +class JPH_EXPORT IObjectStreamOut : public ObjectStream +{ +public: + ///@name Output type specific operations + virtual void WriteDataType(EOSDataType inType) = 0; + virtual void WriteName(const char *inName) = 0; + virtual void WriteIdentifier(Identifier inIdentifier) = 0; + virtual void WriteCount(uint32 inCount) = 0; + + ///@name Write primitives + virtual void WritePrimitiveData(const uint8 &inPrimitive) = 0; + virtual void WritePrimitiveData(const uint16 &inPrimitive) = 0; + virtual void WritePrimitiveData(const int &inPrimitive) = 0; + virtual void WritePrimitiveData(const uint32 &inPrimitive) = 0; + virtual void WritePrimitiveData(const uint64 &inPrimitive) = 0; + virtual void WritePrimitiveData(const float &inPrimitive) = 0; + virtual void WritePrimitiveData(const double &inPrimitive) = 0; + 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 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 Quat &inPrimitive) = 0; + virtual void WritePrimitiveData(const Mat44 &inPrimitive) = 0; + virtual void WritePrimitiveData(const DMat44 &inPrimitive) = 0; + + ///@name Write compounds + virtual void WritePointerData(const RTTI *inRTTI, const void *inPointer) = 0; + virtual void WriteClassData(const RTTI *inRTTI, const void *inInstance) = 0; + + ///@name Layout hints (for text output) + virtual void HintNextItem() { /* Default is do nothing */ } + virtual void HintIndentUp() { /* Default is do nothing */ } + virtual void HintIndentDown() { /* Default is do nothing */ } +}; + +// Define macro to declare functions for a specific primitive type +#define JPH_DECLARE_PRIMITIVE(name) \ + JPH_EXPORT bool OSIsType(name *, int inArrayDepth, EOSDataType inDataType, const char *inClassName); \ + JPH_EXPORT bool OSReadData(IObjectStreamIn &ioStream, name &outPrimitive); \ + JPH_EXPORT void OSWriteDataType(IObjectStreamOut &ioStream, name *); \ + JPH_EXPORT void OSWriteData(IObjectStreamOut &ioStream, const name &inPrimitive); + +// This file uses the JPH_DECLARE_PRIMITIVE macro to define all types +#include + +// Define serialization templates +template +bool OSIsType(Array *, int inArrayDepth, EOSDataType inDataType, const char *inClassName) +{ + return (inArrayDepth > 0 && OSIsType(static_cast(nullptr), inArrayDepth - 1, inDataType, inClassName)); +} + +template +bool OSIsType(StaticArray *, int inArrayDepth, EOSDataType inDataType, const char *inClassName) +{ + return (inArrayDepth > 0 && OSIsType(static_cast(nullptr), inArrayDepth - 1, inDataType, inClassName)); +} + +template +bool OSIsType(T (*)[N], int inArrayDepth, EOSDataType inDataType, const char *inClassName) +{ + return (inArrayDepth > 0 && OSIsType(static_cast(nullptr), inArrayDepth - 1, inDataType, inClassName)); +} + +template +bool OSIsType(Ref *, int inArrayDepth, EOSDataType inDataType, const char *inClassName) +{ + return OSIsType(static_cast(nullptr), inArrayDepth, inDataType, inClassName); +} + +template +bool OSIsType(RefConst *, int inArrayDepth, EOSDataType inDataType, const char *inClassName) +{ + return OSIsType(static_cast(nullptr), inArrayDepth, inDataType, inClassName); +} + +/// Define serialization templates for dynamic arrays +template +bool OSReadData(IObjectStreamIn &ioStream, Array &inArray) +{ + bool continue_reading = true; + + // Read array length + uint32 array_length; + continue_reading = ioStream.ReadCount(array_length); + + // Read array items + if (continue_reading) + { + inArray.clear(); + inArray.resize(array_length); + for (uint32 el = 0; el < array_length && continue_reading; ++el) + continue_reading = OSReadData(ioStream, inArray[el]); + } + + return continue_reading; +} + +/// Define serialization templates for static arrays +template +bool OSReadData(IObjectStreamIn &ioStream, StaticArray &inArray) +{ + bool continue_reading = true; + + // Read array length + uint32 array_length; + continue_reading = ioStream.ReadCount(array_length); + + // Check if we can fit this many elements + if (array_length > N) + return false; + + // Read array items + if (continue_reading) + { + inArray.clear(); + inArray.resize(array_length); + for (uint32 el = 0; el < array_length && continue_reading; ++el) + continue_reading = OSReadData(ioStream, inArray[el]); + } + + return continue_reading; +} + +/// Define serialization templates for C style arrays +template +bool OSReadData(IObjectStreamIn &ioStream, T (&inArray)[N]) +{ + bool continue_reading = true; + + // Read array length + uint32 array_length; + continue_reading = ioStream.ReadCount(array_length); + if (array_length != N) + return false; + + // Read array items + for (uint32 el = 0; el < N && continue_reading; ++el) + continue_reading = OSReadData(ioStream, inArray[el]); + + return continue_reading; +} + +/// Define serialization templates for references +template +bool OSReadData(IObjectStreamIn &ioStream, Ref &inRef) +{ + return ioStream.ReadPointerData(JPH_RTTI(T), inRef.InternalGetPointer(), T::sInternalGetRefCountOffset()); +} + +template +bool OSReadData(IObjectStreamIn &ioStream, RefConst &inRef) +{ + return ioStream.ReadPointerData(JPH_RTTI(T), inRef.InternalGetPointer(), T::sInternalGetRefCountOffset()); +} + +// Define serialization templates for dynamic arrays +template +void OSWriteDataType(IObjectStreamOut &ioStream, Array *) +{ + ioStream.WriteDataType(EOSDataType::Array); + OSWriteDataType(ioStream, static_cast(nullptr)); +} + +template +void OSWriteData(IObjectStreamOut &ioStream, const Array &inArray) +{ + // Write size of array + ioStream.HintNextItem(); + ioStream.WriteCount(static_cast(inArray.size())); + + // Write data in array + ioStream.HintIndentUp(); + for (const T &v : inArray) + OSWriteData(ioStream, v); + ioStream.HintIndentDown(); +} + +/// Define serialization templates for static arrays +template +void OSWriteDataType(IObjectStreamOut &ioStream, StaticArray *) +{ + ioStream.WriteDataType(EOSDataType::Array); + OSWriteDataType(ioStream, static_cast(nullptr)); +} + +template +void OSWriteData(IObjectStreamOut &ioStream, const StaticArray &inArray) +{ + // Write size of array + ioStream.HintNextItem(); + ioStream.WriteCount(inArray.size()); + + // Write data in array + ioStream.HintIndentUp(); + for (const typename StaticArray::value_type &v : inArray) + OSWriteData(ioStream, v); + ioStream.HintIndentDown(); +} + +/// Define serialization templates for C style arrays +template +void OSWriteDataType(IObjectStreamOut &ioStream, T (*)[N]) +{ + ioStream.WriteDataType(EOSDataType::Array); + OSWriteDataType(ioStream, static_cast(nullptr)); +} + +template +void OSWriteData(IObjectStreamOut &ioStream, const T (&inArray)[N]) +{ + // Write size of array + ioStream.HintNextItem(); + ioStream.WriteCount(uint32(N)); + + // Write data in array + ioStream.HintIndentUp(); + for (const T &v : inArray) + OSWriteData(ioStream, v); + ioStream.HintIndentDown(); +} + +/// Define serialization templates for references +template +void OSWriteDataType(IObjectStreamOut &ioStream, Ref *) +{ + OSWriteDataType(ioStream, static_cast(nullptr)); +} + +template +void OSWriteData(IObjectStreamOut &ioStream, const Ref &inRef) +{ + if (inRef != nullptr) + ioStream.WritePointerData(GetRTTI(inRef.GetPtr()), inRef.GetPtr()); + else + ioStream.WritePointerData(nullptr, nullptr); +} + +template +void OSWriteDataType(IObjectStreamOut &ioStream, RefConst *) +{ + OSWriteDataType(ioStream, static_cast(nullptr)); +} + +template +void OSWriteData(IObjectStreamOut &ioStream, const RefConst &inRef) +{ + if (inRef != nullptr) + ioStream.WritePointerData(GetRTTI(inRef.GetPtr()), inRef.GetPtr()); + else + ioStream.WritePointerData(nullptr, nullptr); +} + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamBinaryIn.cpp b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamBinaryIn.cpp new file mode 100644 index 000000000000..e359f0482103 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamBinaryIn.cpp @@ -0,0 +1,234 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#ifdef JPH_OBJECT_STREAM + +#include + +JPH_NAMESPACE_BEGIN + +ObjectStreamBinaryIn::ObjectStreamBinaryIn(istream &inStream) : + ObjectStreamIn(inStream) +{ +} + +bool ObjectStreamBinaryIn::ReadDataType(EOSDataType &outType) +{ + uint32 type; + mStream.read((char *)&type, sizeof(type)); + if (mStream.fail()) return false; + outType = (EOSDataType)type; + return true; +} + +bool ObjectStreamBinaryIn::ReadName(String &outName) +{ + return ReadPrimitiveData(outName); +} + +bool ObjectStreamBinaryIn::ReadIdentifier(Identifier &outIdentifier) +{ + Identifier id; + mStream.read((char *)&id, sizeof(id)); + if (mStream.fail()) return false; + outIdentifier = id; + return true; +} + +bool ObjectStreamBinaryIn::ReadCount(uint32 &outCount) +{ + uint32 count; + mStream.read((char *)&count, sizeof(count)); + if (mStream.fail()) return false; + outCount = count; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(uint8 &outPrimitive) +{ + uint8 primitive; + mStream.read((char *)&primitive, sizeof(primitive)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(uint16 &outPrimitive) +{ + uint16 primitive; + mStream.read((char *)&primitive, sizeof(primitive)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(int &outPrimitive) +{ + int primitive; + mStream.read((char *)&primitive, sizeof(primitive)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(uint32 &outPrimitive) +{ + uint32 primitive; + mStream.read((char *)&primitive, sizeof(primitive)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(uint64 &outPrimitive) +{ + uint64 primitive; + mStream.read((char *)&primitive, sizeof(primitive)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(float &outPrimitive) +{ + float primitive; + mStream.read((char *)&primitive, sizeof(primitive)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(double &outPrimitive) +{ + double primitive; + mStream.read((char *)&primitive, sizeof(primitive)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(bool &outPrimitive) +{ + bool primitive; + mStream.read((char *)&primitive, sizeof(primitive)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(String &outPrimitive) +{ + // Read length or ID of string + uint32 len; + if (!ReadPrimitiveData(len)) + return false; + + // Check empty string + if (len == 0) + { + outPrimitive.clear(); + return true; + } + + // Check if it is an ID in the string table + if (len & 0x80000000) + { + StringTable::iterator i = mStringTable.find(len); + if (i == mStringTable.end()) + return false; + outPrimitive = i->second; + return true; + } + + // Read the string + char *data = (char *)JPH_STACK_ALLOC(len + 1); + mStream.read(data, len); + if (mStream.fail()) return false; + data[len] = 0; + outPrimitive = data; + + // Insert string in table + mStringTable.try_emplace(mNextStringID, outPrimitive); + mNextStringID++; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(Float3 &outPrimitive) +{ + Float3 primitive; + mStream.read((char *)&primitive, sizeof(Float3)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(Double3 &outPrimitive) +{ + Double3 primitive; + mStream.read((char *)&primitive, sizeof(Double3)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(Vec3 &outPrimitive) +{ + Float3 primitive; + mStream.read((char *)&primitive, sizeof(Float3)); + if (mStream.fail()) return false; + outPrimitive = Vec3(primitive); // Use Float3 constructor so that we initialize W too + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(DVec3 &outPrimitive) +{ + Double3 primitive; + mStream.read((char *)&primitive, sizeof(Double3)); + if (mStream.fail()) return false; + outPrimitive = DVec3(primitive); // Use Float3 constructor so that we initialize W too + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(Vec4 &outPrimitive) +{ + Vec4 primitive; + mStream.read((char *)&primitive, sizeof(primitive)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(Quat &outPrimitive) +{ + Quat primitive; + mStream.read((char *)&primitive, sizeof(primitive)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(Mat44 &outPrimitive) +{ + Mat44 primitive; + mStream.read((char *)&primitive, sizeof(primitive)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(DMat44 &outPrimitive) +{ + Vec4 c0, c1, c2; + DVec3 c3; + if (!ReadPrimitiveData(c0) || !ReadPrimitiveData(c1) || !ReadPrimitiveData(c2) || !ReadPrimitiveData(c3)) + return false; + outPrimitive = DMat44(c0, c1, c2, c3); + return true; +} + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamBinaryIn.h b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamBinaryIn.h new file mode 100644 index 000000000000..63774110d91d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamBinaryIn.h @@ -0,0 +1,55 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +/// Implementation of ObjectStream binary input stream. +class JPH_EXPORT ObjectStreamBinaryIn : public ObjectStreamIn +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit ObjectStreamBinaryIn(istream &inStream); + + ///@name Input type specific operations + virtual bool ReadDataType(EOSDataType &outType) override; + virtual bool ReadName(String &outName) override; + virtual bool ReadIdentifier(Identifier &outIdentifier) override; + virtual bool ReadCount(uint32 &outCount) override; + + virtual bool ReadPrimitiveData(uint8 &outPrimitive) override; + virtual bool ReadPrimitiveData(uint16 &outPrimitive) override; + virtual bool ReadPrimitiveData(int &outPrimitive) override; + virtual bool ReadPrimitiveData(uint32 &outPrimitive) override; + virtual bool ReadPrimitiveData(uint64 &outPrimitive) override; + virtual bool ReadPrimitiveData(float &outPrimitive) override; + virtual bool ReadPrimitiveData(double &outPrimitive) override; + virtual bool ReadPrimitiveData(bool &outPrimitive) override; + virtual bool ReadPrimitiveData(String &outPrimitive) override; + virtual bool ReadPrimitiveData(Float3 &outPrimitive) override; + virtual bool ReadPrimitiveData(Double3 &outPrimitive) override; + virtual bool ReadPrimitiveData(Vec3 &outPrimitive) override; + virtual bool ReadPrimitiveData(DVec3 &outPrimitive) override; + virtual bool ReadPrimitiveData(Vec4 &outPrimitive) override; + virtual bool ReadPrimitiveData(Quat &outPrimitive) override; + virtual bool ReadPrimitiveData(Mat44 &outPrimitive) override; + virtual bool ReadPrimitiveData(DMat44 &outPrimitive) override; + +private: + using StringTable = UnorderedMap; + + StringTable mStringTable; + uint32 mNextStringID = 0x80000000; +}; + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamBinaryOut.cpp b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamBinaryOut.cpp new file mode 100644 index 000000000000..042c8edb38df --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamBinaryOut.cpp @@ -0,0 +1,155 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#ifdef JPH_OBJECT_STREAM + +#include +#include + +JPH_NAMESPACE_BEGIN + +ObjectStreamBinaryOut::ObjectStreamBinaryOut(ostream &inStream) : + ObjectStreamOut(inStream) +{ + String header; + header = StringFormat("BOS%2d.%02d", ObjectStream::sVersion, ObjectStream::sRevision); + mStream.write(header.c_str(), header.size()); +} + +void ObjectStreamBinaryOut::WriteDataType(EOSDataType inType) +{ + mStream.write((const char *)&inType, sizeof(inType)); +} + +void ObjectStreamBinaryOut::WriteName(const char *inName) +{ + WritePrimitiveData(String(inName)); +} + +void ObjectStreamBinaryOut::WriteIdentifier(Identifier inIdentifier) +{ + mStream.write((const char *)&inIdentifier, sizeof(inIdentifier)); +} + +void ObjectStreamBinaryOut::WriteCount(uint32 inCount) +{ + mStream.write((const char *)&inCount, sizeof(inCount)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const uint8 &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(inPrimitive)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const uint16 &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(inPrimitive)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const int &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(inPrimitive)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const uint32 &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(inPrimitive)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const uint64 &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(inPrimitive)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const float &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(inPrimitive)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const double &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(inPrimitive)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const bool &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(inPrimitive)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const String &inPrimitive) +{ + // Empty strings are trivial + if (inPrimitive.empty()) + { + WritePrimitiveData((uint32)0); + return; + } + + // Check if we've already written this string + StringTable::iterator i = mStringTable.find(inPrimitive); + if (i != mStringTable.end()) + { + WritePrimitiveData(i->second); + return; + } + + // Insert string in table + mStringTable.try_emplace(inPrimitive, mNextStringID); + mNextStringID++; + + // Write string + uint32 len = min((uint32)inPrimitive.size(), (uint32)0x7fffffff); + WritePrimitiveData(len); + mStream.write(inPrimitive.c_str(), len); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const Float3 &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(Float3)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const Double3 &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(Double3)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const Vec3 &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, 3 * sizeof(float)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const DVec3 &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, 3 * sizeof(double)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const Vec4 &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(inPrimitive)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const Quat &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(inPrimitive)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const Mat44 &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(inPrimitive)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const DMat44 &inPrimitive) +{ + WritePrimitiveData(inPrimitive.GetColumn4(0)); + WritePrimitiveData(inPrimitive.GetColumn4(1)); + WritePrimitiveData(inPrimitive.GetColumn4(2)); + WritePrimitiveData(inPrimitive.GetTranslation()); +} + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM + diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamBinaryOut.h b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamBinaryOut.h new file mode 100644 index 000000000000..50464e28c7c3 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamBinaryOut.h @@ -0,0 +1,55 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +/// Implementation of ObjectStream binary output stream. +class JPH_EXPORT ObjectStreamBinaryOut : public ObjectStreamOut +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor and destructor + explicit ObjectStreamBinaryOut(ostream &inStream); + + ///@name Output type specific operations + virtual void WriteDataType(EOSDataType inType) override; + virtual void WriteName(const char *inName) override; + virtual void WriteIdentifier(Identifier inIdentifier) override; + virtual void WriteCount(uint32 inCount) override; + + virtual void WritePrimitiveData(const uint8 &inPrimitive) override; + virtual void WritePrimitiveData(const uint16 &inPrimitive) override; + virtual void WritePrimitiveData(const int &inPrimitive) override; + virtual void WritePrimitiveData(const uint32 &inPrimitive) override; + virtual void WritePrimitiveData(const uint64 &inPrimitive) override; + virtual void WritePrimitiveData(const float &inPrimitive) override; + virtual void WritePrimitiveData(const double &inPrimitive) override; + virtual void WritePrimitiveData(const bool &inPrimitive) override; + virtual void WritePrimitiveData(const String &inPrimitive) override; + virtual void WritePrimitiveData(const Float3 &inPrimitive) override; + virtual void WritePrimitiveData(const Double3 &inPrimitive) override; + virtual void WritePrimitiveData(const Vec3 &inPrimitive) override; + virtual void WritePrimitiveData(const DVec3 &inPrimitive) override; + virtual void WritePrimitiveData(const Vec4 &inPrimitive) override; + virtual void WritePrimitiveData(const Quat &inPrimitive) override; + virtual void WritePrimitiveData(const Mat44 &inPrimitive) override; + virtual void WritePrimitiveData(const DMat44 &inPrimitive) override; + +private: + using StringTable = UnorderedMap; + + StringTable mStringTable; + uint32 mNextStringID = 0x80000000; +}; + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamIn.cpp b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamIn.cpp new file mode 100644 index 000000000000..2f9d56cd894f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamIn.cpp @@ -0,0 +1,621 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#ifdef JPH_OBJECT_STREAM + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +ObjectStreamIn::ObjectStreamIn(istream &inStream) : + mStream(inStream) +{ +} + +bool ObjectStreamIn::GetInfo(istream &inStream, EStreamType &outType, int &outVersion, int &outRevision) +{ + // Read header and check if it is the correct format, e.g. "TOS 1.00" + char header[9]; + memset(header, 0, 9); + inStream.read(header, 8); + if ((header[0] == 'B' || header[0] == 'T') && header[1] == 'O' && header[2] == 'S' + && (header[3] == ' ' || isdigit(header[3])) && isdigit(header[4]) + && header[5] == '.' && isdigit(header[6]) && isdigit(header[7])) + { + // Check if this is a binary or text objectfile + switch (header[0]) + { + case 'T': outType = ObjectStream::EStreamType::Text; break; + case 'B': outType = ObjectStream::EStreamType::Binary; break; + default: JPH_ASSERT(false); break; + } + + // Extract version and revision + header[5] = '\0'; + outVersion = atoi(&header[3]); + outRevision = atoi(&header[6]); + + return true; + } + + Trace("ObjectStreamIn: Not a valid object stream."); + return false; +} + +ObjectStreamIn *ObjectStreamIn::Open(istream &inStream) +{ + // Check if file is an ObjectStream of the correct version and revision + EStreamType type; + int version; + int revision; + if (GetInfo(inStream, type, version, revision)) + { + if (version == sVersion && revision == sRevision) + { + // Create an input stream of the correct type + switch (type) + { + case EStreamType::Text: return new ObjectStreamTextIn(inStream); + case EStreamType::Binary: return new ObjectStreamBinaryIn(inStream); + default: JPH_ASSERT(false); + } + } + else + { + Trace("ObjectStreamIn: Different version stream (%d.%02d, expected %d.%02d).", version, revision, sVersion, sRevision); + } + } + + return nullptr; +} + +void *ObjectStreamIn::Read(const RTTI *inRTTI) +{ + using ObjectSet = UnorderedSet; + + // Read all information on the stream + void *main_object = nullptr; + bool continue_reading = true; + for (;;) + { + // Get type of next operation + EOSDataType data_type; + if (!ReadDataType(data_type)) + break; + + if (data_type == EOSDataType::Declare) + { + // Read type declaration + if (!ReadRTTI()) + { + Trace("ObjectStreamIn: Fatal error while reading class description for class %s.", inRTTI->GetName()); + continue_reading = false; + break; + } + } + else if (data_type == EOSDataType::Object) + { + const RTTI *rtti; + void *object = ReadObject(rtti); + if (!main_object && object) + { + // This is the first and thus main object of the file. + if (rtti->IsKindOf(inRTTI)) + { + // Object is of correct type + main_object = object; + } + else + { + Trace("ObjectStreamIn: Main object of different type. Expected %s, but found %s.", inRTTI->GetName(), rtti->GetName()); + continue_reading = false; + break; + } + } + } + else + { + // Invalid or out of place token found + Trace("ObjectStreamIn: Invalid or out of place token found."); + continue_reading = false; + break; + } + } + + // Resolve links (pointer, references) + if (continue_reading) + { + // Resolve links + ObjectSet referenced_objects; + for (Link &link : mUnresolvedLinks) + { + IdentifierMap::const_iterator j = mIdentifierMap.find(link.mIdentifier); + if (j != mIdentifierMap.end() && j->second.mRTTI->IsKindOf(link.mRTTI)) + { + const ObjectInfo &obj_info = j->second; + + // Set pointer + *link.mPointer = obj_info.mInstance; + + // Increment refcount if it was a referencing pointer + if (link.mRefCountOffset != -1) + ++(*(uint32 *)(((uint8 *)obj_info.mInstance) + link.mRefCountOffset)); + + // Add referenced object to the list + if (referenced_objects.find(obj_info.mInstance) == referenced_objects.end()) + referenced_objects.insert(obj_info.mInstance); + } + else + { + // Referenced object not found, set pointer to nullptr + Trace("ObjectStreamIn: Setting incorrect pointer to class of type %s to nullptr.", link.mRTTI->GetName()); + *link.mPointer = nullptr; + } + } + + // Release unreferenced objects except the main object + for (const IdentifierMap::value_type &j : mIdentifierMap) + { + const ObjectInfo &obj_info = j.second; + + if (obj_info.mInstance != main_object) + { + ObjectSet::const_iterator k = referenced_objects.find(obj_info.mInstance); + if (k == referenced_objects.end()) + { + Trace("ObjectStreamIn: Releasing unreferenced object of type %s.", obj_info.mRTTI->GetName()); + obj_info.mRTTI->DestructObject(obj_info.mInstance); + } + } + } + + return main_object; + } + else + { + // Release all objects if a fatal error occurred + for (const IdentifierMap::value_type &i : mIdentifierMap) + { + const ObjectInfo &obj_info = i.second; + obj_info.mRTTI->DestructObject(obj_info.mInstance); + } + + return nullptr; + } +} + +void *ObjectStreamIn::ReadObject(const RTTI *& outRTTI) +{ + // Read the object class + void *object = nullptr; + String class_name; + if (ReadName(class_name)) + { + // Get class description + ClassDescriptionMap::iterator i = mClassDescriptionMap.find(class_name); + if (i != mClassDescriptionMap.end()) + { + const ClassDescription &class_desc = i->second; + + // Read object identifier + Identifier identifier; + if (ReadIdentifier(identifier)) + { + // Check if this object can be read or must be skipped + if (identifier != sNullIdentifier + && class_desc.mRTTI + && !class_desc.mRTTI->IsAbstract()) + { + // Create object instance + outRTTI = class_desc.mRTTI; + object = outRTTI->CreateObject(); + + // Read object attributes + if (ReadClassData(class_desc, object)) + { + // Add object to identifier map + mIdentifierMap.try_emplace(identifier, object, outRTTI); + } + else + { + // Fatal error while reading attributes, release object + outRTTI->DestructObject(object); + object = nullptr; + } + } + else + { + // Skip this object + // TODO: This operation can fail, but there is no check yet + Trace("ObjectStreamIn: Found uncreatable object %s.", class_name.c_str()); + ReadClassData(class_desc, nullptr); + } + } + } + else + { + // TODO: This is a fatal error, but this function has no way of indicating this + Trace("ObjectStreamIn: Found object of unknown class %s.", class_name.c_str()); + } + } + + return object; +} + +bool ObjectStreamIn::ReadRTTI() +{ + // Read class name and find it's attribute info + String class_name; + if (!ReadName(class_name)) + return false; + + // Find class + const RTTI *rtti = Factory::sInstance->Find(class_name.c_str()); + if (rtti == nullptr) + Trace("ObjectStreamIn: Unknown class: \"%s\".", class_name.c_str()); + + // Insert class description + ClassDescription &class_desc = mClassDescriptionMap.try_emplace(class_name, rtti).first->second; + + // Read the number of entries in the description + uint32 count; + if (!ReadCount(count)) + return false; + + // Read the entries + for (uint32 i = 0; i < count; ++i) + { + AttributeDescription attribute; + + // Read name + String attribute_name; + if (!ReadName(attribute_name)) + return false; + + // Read type + if (!ReadDataType(attribute.mSourceType)) + return false; + + // Read array depth + while (attribute.mSourceType == EOSDataType::Array) + { + ++attribute.mArrayDepth; + if (!ReadDataType(attribute.mSourceType)) + return false; + } + + // Read instance/pointer class name + if ((attribute.mSourceType == EOSDataType::Instance || attribute.mSourceType == EOSDataType::Pointer) + && !ReadName(attribute.mClassName)) + return false; + + // Find attribute in rtti + if (rtti) + { + // Find attribute index + for (int idx = 0; idx < rtti->GetAttributeCount(); ++idx) + { + const SerializableAttribute &attr = rtti->GetAttribute(idx); + if (strcmp(attr.GetName(), attribute_name.c_str()) == 0) + { + attribute.mIndex = idx; + break; + } + } + + // Check if attribute is of expected type + if (attribute.mIndex >= 0) + { + const SerializableAttribute &attr = rtti->GetAttribute(attribute.mIndex); + if (attr.IsType(attribute.mArrayDepth, attribute.mSourceType, attribute.mClassName.c_str())) + { + // No conversion needed + attribute.mDestinationType = attribute.mSourceType; + } + else if (attribute.mArrayDepth == 0 && attribute.mClassName.empty()) + { + // Try to apply type conversions + if (attribute.mSourceType == EOSDataType::T_Vec3 && attr.IsType(0, EOSDataType::T_DVec3, "")) + attribute.mDestinationType = EOSDataType::T_DVec3; + else if (attribute.mSourceType == EOSDataType::T_DVec3 && attr.IsType(0, EOSDataType::T_Vec3, "")) + attribute.mDestinationType = EOSDataType::T_Vec3; + else + attribute.mIndex = -1; + } + else + { + // No conversion exists + attribute.mIndex = -1; + } + } + } + + // Add attribute to the class description + class_desc.mAttributes.push_back(attribute); + } + + return true; +} + +bool ObjectStreamIn::ReadClassData(const char *inClassName, void *inInstance) +{ + // Find the class description + ClassDescriptionMap::iterator i = mClassDescriptionMap.find(inClassName); + if (i != mClassDescriptionMap.end()) + return ReadClassData(i->second, inInstance); + + return false; +} + +bool ObjectStreamIn::ReadClassData(const ClassDescription &inClassDesc, void *inInstance) +{ + // Read data for this class + bool continue_reading = true; + + for (const AttributeDescription &attr_desc : inClassDesc.mAttributes) + { + // Read or skip the attribute data + if (attr_desc.mIndex >= 0 && inInstance) + { + const SerializableAttribute &attr = inClassDesc.mRTTI->GetAttribute(attr_desc.mIndex); + if (attr_desc.mSourceType == attr_desc.mDestinationType) + { + continue_reading = attr.ReadData(*this, inInstance); + } + else if (attr_desc.mSourceType == EOSDataType::T_Vec3 && attr_desc.mDestinationType == EOSDataType::T_DVec3) + { + // Vec3 to DVec3 + Vec3 tmp; + continue_reading = ReadPrimitiveData(tmp); + if (continue_reading) + *attr.GetMemberPointer(inInstance) = DVec3(tmp); + } + else if (attr_desc.mSourceType == EOSDataType::T_DVec3 && attr_desc.mDestinationType == EOSDataType::T_Vec3) + { + // DVec3 to Vec3 + DVec3 tmp; + continue_reading = ReadPrimitiveData(tmp); + if (continue_reading) + *attr.GetMemberPointer(inInstance) = Vec3(tmp); + } + else + { + JPH_ASSERT(false); // Unknown conversion + continue_reading = SkipAttributeData(attr_desc.mArrayDepth, attr_desc.mSourceType, attr_desc.mClassName.c_str()); + } + } + else + continue_reading = SkipAttributeData(attr_desc.mArrayDepth, attr_desc.mSourceType, attr_desc.mClassName.c_str()); + + if (!continue_reading) + break; + } + + return continue_reading; +} + +bool ObjectStreamIn::ReadPointerData(const RTTI *inRTTI, void **inPointer, int inRefCountOffset) +{ + Identifier identifier; + if (ReadIdentifier(identifier)) + { + if (identifier == sNullIdentifier) + { + // Set nullptr pointer + inPointer = nullptr; + } + else + { + // Put pointer on the list to be resolved later on + Link &link = mUnresolvedLinks.emplace_back(); + link.mPointer = inPointer; + link.mRefCountOffset = inRefCountOffset; + link.mIdentifier = identifier; + link.mRTTI = inRTTI; + } + + return true; + } + + return false; +} + +bool ObjectStreamIn::SkipAttributeData(int inArrayDepth, EOSDataType inDataType, const char *inClassName) +{ + bool continue_reading = true; + + // Get number of items to read + uint32 count = 1; + for (; inArrayDepth > 0; --inArrayDepth) + { + uint32 temporary; + if (ReadCount(temporary)) + { + // Multiply for multi dimensional arrays + count *= temporary; + } + else + { + // Fatal error while reading array size + continue_reading = false; + break; + } + } + + // Read data for all items + if (continue_reading) + { + if (inDataType == EOSDataType::Instance) + { + // Get the class description + ClassDescriptionMap::iterator i = mClassDescriptionMap.find(inClassName); + if (i != mClassDescriptionMap.end()) + { + for (; count > 0 && continue_reading; --count) + continue_reading = ReadClassData(i->second, nullptr); + } + else + { + continue_reading = false; + Trace("ObjectStreamIn: Found instance of unknown class %s.", inClassName); + } + } + else + { + for (; count > 0 && continue_reading; --count) + { + switch (inDataType) + { + case EOSDataType::Pointer: + { + Identifier temporary; + continue_reading = ReadIdentifier(temporary); + break; + } + + case EOSDataType::T_uint8: + { + uint8 temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_uint16: + { + uint16 temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_int: + { + int temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_uint32: + { + uint32 temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_uint64: + { + uint64 temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_float: + { + float temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_double: + { + double temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_bool: + { + bool temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_String: + { + String temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_Float3: + { + Float3 temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_Double3: + { + Double3 temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_Vec3: + { + Vec3 temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_DVec3: + { + DVec3 temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_Vec4: + { + Vec4 temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_Quat: + { + Quat temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_Mat44: + { + Mat44 temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_DMat44: + { + DMat44 temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::Array: + case EOSDataType::Object: + case EOSDataType::Declare: + case EOSDataType::Instance: + case EOSDataType::Invalid: + default: + continue_reading = false; + break; + } + } + } + } + + return continue_reading; +} + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamIn.h b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamIn.h new file mode 100644 index 000000000000..b59194ac01d6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamIn.h @@ -0,0 +1,148 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +/// ObjectStreamIn contains all logic for reading an object from disk. It is the base +/// class for the text and binary input streams (ObjectStreamTextIn and ObjectStreamBinaryIn). +class JPH_EXPORT ObjectStreamIn : public IObjectStreamIn +{ +private: + struct ClassDescription; + +public: + /// Main function to read an object from a stream + template + static bool sReadObject(istream &inStream, T *&outObject) + { + // Create the input stream + bool result = false; + ObjectStreamIn *stream = ObjectStreamIn::Open(inStream); + if (stream) + { + // Read the object + outObject = (T *)stream->Read(JPH_RTTI(T)); + result = (outObject != nullptr); + delete stream; + } + return result; + } + + /// Main function to read an object from a stream (reference counting pointer version) + template + static bool sReadObject(istream &inStream, Ref &outObject) + { + T *object = nullptr; + bool result = sReadObject(inStream, object); + outObject = object; + return result; + } + + /// Main function to read an object from a file + template + static bool sReadObject(const char *inFileName, T *&outObject) + { + std::ifstream stream; + stream.open(inFileName, std::ifstream::in | std::ifstream::binary); + if (!stream.is_open()) + return false; + return sReadObject(stream, outObject); + } + + /// Main function to read an object from a file (reference counting pointer version) + template + static bool sReadObject(const char *inFileName, Ref &outObject) + { + T *object = nullptr; + bool result = sReadObject(inFileName, object); + outObject = object; + return result; + } + + ////////////////////////////////////////////////////// + // EVERYTHING BELOW THIS SHOULD NOT DIRECTLY BE CALLED + ////////////////////////////////////////////////////// + + ///@name Serialization operations + void * Read(const RTTI *inRTTI); + void * ReadObject(const RTTI *& outRTTI); + bool ReadRTTI(); + virtual bool ReadClassData(const char *inClassName, void *inInstance) override; + bool ReadClassData(const ClassDescription &inClassDesc, void *inInstance); + virtual bool ReadPointerData(const RTTI *inRTTI, void **inPointer, int inRefCountOffset = -1) override; + bool SkipAttributeData(int inArrayDepth, EOSDataType inDataType, const char *inClassName); + +protected: + /// Constructor + explicit ObjectStreamIn(istream &inStream); + + /// Determine the type and version of an object stream + static bool GetInfo(istream &inStream, EStreamType &outType, int &outVersion, int &outRevision); + + /// Static constructor + static ObjectStreamIn * Open(istream &inStream); + + istream & mStream; + +private: + /// Class descriptions + struct AttributeDescription + { + int mArrayDepth = 0; + EOSDataType mSourceType = EOSDataType::Invalid; + EOSDataType mDestinationType = EOSDataType::Invalid; + String mClassName; + int mIndex = -1; + }; + + struct ClassDescription + { + ClassDescription() = default; + explicit ClassDescription(const RTTI *inRTTI) : mRTTI(inRTTI) { } + + const RTTI * mRTTI = nullptr; + Array mAttributes; + }; + + struct ObjectInfo + { + ObjectInfo() = default; + ObjectInfo(void *inInstance, const RTTI *inRTTI) : mInstance(inInstance), mRTTI(inRTTI) { } + + void * mInstance = nullptr; + const RTTI * mRTTI = nullptr; + }; + + struct Link + { + void ** mPointer; + int mRefCountOffset; + Identifier mIdentifier; + const RTTI * mRTTI; + }; + + using IdentifierMap = UnorderedMap; + using ClassDescriptionMap = UnorderedMap; + + ClassDescriptionMap mClassDescriptionMap; + IdentifierMap mIdentifierMap; ///< Links identifier to an object pointer + Array mUnresolvedLinks; ///< All pointers (links) are resolved after reading the entire file, e.g. when all object exist +}; + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamOut.cpp b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamOut.cpp new file mode 100644 index 000000000000..bfb29b837b5d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamOut.cpp @@ -0,0 +1,166 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +ObjectStreamOut::ObjectStreamOut(ostream &inStream) : + mStream(inStream) +{ +// Add all primitives to the class set +#define JPH_DECLARE_PRIMITIVE(name) mClassSet.insert(JPH_RTTI(name)); +#include +} + +ObjectStreamOut *ObjectStreamOut::Open(EStreamType inType, ostream &inStream) +{ + switch (inType) + { + case EStreamType::Text: return new ObjectStreamTextOut(inStream); + case EStreamType::Binary: return new ObjectStreamBinaryOut(inStream); + default: JPH_ASSERT(false); + } + return nullptr; +} + +bool ObjectStreamOut::Write(const void *inObject, const RTTI *inRTTI) +{ + // Assign a new identifier to the object and write it + mIdentifierMap.try_emplace(inObject, mNextIdentifier, inRTTI); + mNextIdentifier++; + WriteObject(inObject); + + // Write all linked objects + ObjectQueue::size_type cur = 0; + for (; cur < mObjectQueue.size() && !mStream.fail(); ++cur) + WriteObject(mObjectQueue[cur]); + mObjectQueue.erase(mObjectQueue.begin(), mObjectQueue.begin() + cur); + + return !mStream.fail(); +} + +void ObjectStreamOut::WriteObject(const void *inObject) +{ + // Find object identifier + IdentifierMap::iterator i = mIdentifierMap.find(inObject); + JPH_ASSERT(i != mIdentifierMap.end()); + + // Write class description and associated descriptions + QueueRTTI(i->second.mRTTI); + ClassQueue::size_type cur = 0; + for (; cur < mClassQueue.size() && !mStream.fail(); ++cur) + WriteRTTI(mClassQueue[cur]); + mClassQueue.erase(mClassQueue.begin(), mClassQueue.begin() + cur); + + HintNextItem(); + HintNextItem(); + + // Write object header. + WriteDataType(EOSDataType::Object); + WriteName(i->second.mRTTI->GetName()); + WriteIdentifier(i->second.mIdentifier); + + // Write attribute data + WriteClassData(i->second.mRTTI, inObject); +} + +void ObjectStreamOut::QueueRTTI(const RTTI *inRTTI) +{ + ClassSet::const_iterator i = mClassSet.find(inRTTI); + if (i == mClassSet.end()) + { + mClassSet.insert(inRTTI); + mClassQueue.push_back(inRTTI); + } +} + +void ObjectStreamOut::WriteRTTI(const RTTI *inRTTI) +{ + HintNextItem(); + HintNextItem(); + + // Write class header. E.g. in text mode: "class " + WriteDataType(EOSDataType::Declare); + WriteName(inRTTI->GetName()); + WriteCount(inRTTI->GetAttributeCount()); + + // Write class attribute info + HintIndentUp(); + for (int attr_index = 0; attr_index < inRTTI->GetAttributeCount(); ++attr_index) + { + // Get attribute + const SerializableAttribute &attr = inRTTI->GetAttribute(attr_index); + + // Write definition of attribute class if undefined + const RTTI *rtti = attr.GetMemberPrimitiveType(); + if (rtti != nullptr) + QueueRTTI(rtti); + + HintNextItem(); + + // Write attribute information. + WriteName(attr.GetName()); + attr.WriteDataType(*this); + } + HintIndentDown(); +} + +void ObjectStreamOut::WriteClassData(const RTTI *inRTTI, const void *inInstance) +{ + JPH_ASSERT(inInstance); + + // Write attributes + HintIndentUp(); + for (int attr_index = 0; attr_index < inRTTI->GetAttributeCount(); ++attr_index) + { + // Get attribute + const SerializableAttribute &attr = inRTTI->GetAttribute(attr_index); + attr.WriteData(*this, inInstance); + } + HintIndentDown(); +} + +void ObjectStreamOut::WritePointerData(const RTTI *inRTTI, const void *inPointer) +{ + Identifier identifier; + + if (inPointer) + { + // Check if this object has an identifier + IdentifierMap::iterator i = mIdentifierMap.find(inPointer); + if (i != mIdentifierMap.end()) + { + // Object already has an identifier + identifier = i->second.mIdentifier; + } + else + { + // Assign a new identifier to this object and queue it for serialization + identifier = mNextIdentifier++; + mIdentifierMap.try_emplace(inPointer, identifier, inRTTI); + mObjectQueue.push_back(inPointer); + } + } + else + { + // Write nullptr pointer + identifier = sNullIdentifier; + } + + // Write the identifier + HintNextItem(); + WriteIdentifier(identifier); +} + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamOut.h b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamOut.h new file mode 100644 index 000000000000..58bfd2f308f3 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamOut.h @@ -0,0 +1,101 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +/// ObjectStreamOut contains all logic for writing an object to disk. It is the base +/// class for the text and binary output streams (ObjectStreamTextOut and ObjectStreamBinaryOut). +class JPH_EXPORT ObjectStreamOut : public IObjectStreamOut +{ +private: + struct ObjectInfo; + +public: + /// Main function to write an object to a stream + template + static bool sWriteObject(ostream &inStream, ObjectStream::EStreamType inType, const T &inObject) + { + // Create the output stream + bool result = false; + ObjectStreamOut *stream = ObjectStreamOut::Open(inType, inStream); + if (stream) + { + // Write the object to the stream + result = stream->Write((void *)&inObject, GetRTTI(&inObject)); + delete stream; + } + + return result; + } + + /// Main function to write an object to a file + template + static bool sWriteObject(const char *inFileName, ObjectStream::EStreamType inType, const T &inObject) + { + std::ofstream stream; + stream.open(inFileName, std::ofstream::out | std::ofstream::trunc | std::ofstream::binary); + if (!stream.is_open()) + return false; + return sWriteObject(stream, inType, inObject); + } + + ////////////////////////////////////////////////////// + // EVERYTHING BELOW THIS SHOULD NOT DIRECTLY BE CALLED + ////////////////////////////////////////////////////// + + ///@name Serialization operations + bool Write(const void *inObject, const RTTI *inRTTI); + void WriteObject(const void *inObject); + void QueueRTTI(const RTTI *inRTTI); + void WriteRTTI(const RTTI *inRTTI); + virtual void WriteClassData(const RTTI *inRTTI, const void *inInstance) override; + virtual void WritePointerData(const RTTI *inRTTI, const void *inPointer) override; + +protected: + /// Static constructor + static ObjectStreamOut * Open(EStreamType inType, ostream &inStream); + + /// Constructor + explicit ObjectStreamOut(ostream &inStream); + + ostream & mStream; + +private: + struct ObjectInfo + { + ObjectInfo() : mIdentifier(0), mRTTI(nullptr) { } + ObjectInfo(Identifier inIdentifier, const RTTI *inRTTI) : mIdentifier(inIdentifier), mRTTI(inRTTI) { } + + Identifier mIdentifier; + const RTTI * mRTTI; + }; + + using IdentifierMap = UnorderedMap; + using ClassSet = UnorderedSet; + using ObjectQueue = Array; + using ClassQueue = Array; + + Identifier mNextIdentifier = sNullIdentifier + 1; ///< Next free identifier for this stream + IdentifierMap mIdentifierMap; ///< Links object pointer to an identifier + ObjectQueue mObjectQueue; ///< Queue of objects to be written + ClassSet mClassSet; ///< List of classes already written + ClassQueue mClassQueue; ///< List of classes waiting to be written +}; + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamTextIn.cpp b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamTextIn.cpp new file mode 100644 index 000000000000..3f345c40fdcf --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamTextIn.cpp @@ -0,0 +1,396 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#ifdef JPH_OBJECT_STREAM + +#include + +JPH_NAMESPACE_BEGIN + +ObjectStreamTextIn::ObjectStreamTextIn(istream &inStream) : + ObjectStreamIn(inStream) +{ +} + +bool ObjectStreamTextIn::ReadDataType(EOSDataType &outType) +{ + String token; + if (ReadWord(token)) + { + transform(token.begin(), token.end(), token.begin(), [](char inValue) { return (char)tolower(inValue); }); + if (token == "declare") + outType = EOSDataType::Declare; + else if (token == "object") + outType = EOSDataType::Object; + else if (token == "instance") + outType = EOSDataType::Instance; + else if (token == "pointer") + outType = EOSDataType::Pointer; + else if (token == "array") + outType = EOSDataType::Array; + else if (token == "uint8") + outType = EOSDataType::T_uint8; + else if (token == "uint16") + outType = EOSDataType::T_uint16; + else if (token == "int") + outType = EOSDataType::T_int; + else if (token == "uint32") + outType = EOSDataType::T_uint32; + else if (token == "uint64") + outType = EOSDataType::T_uint64; + else if (token == "float") + outType = EOSDataType::T_float; + else if (token == "double") + outType = EOSDataType::T_double; + else if (token == "bool") + outType = EOSDataType::T_bool; + else if (token == "string") + outType = EOSDataType::T_String; + else if (token == "float3") + outType = EOSDataType::T_Float3; + else if (token == "double3") + outType = EOSDataType::T_Double3; + else if (token == "vec3") + outType = EOSDataType::T_Vec3; + else if (token == "dvec3") + outType = EOSDataType::T_DVec3; + else if (token == "vec4") + outType = EOSDataType::T_Vec4; + else if (token == "quat") + outType = EOSDataType::T_Quat; + else if (token == "mat44") + outType = EOSDataType::T_Mat44; + else if (token == "dmat44") + outType = EOSDataType::T_DMat44; + else + { + Trace("ObjectStreamTextIn: Found unknown data type."); + return false; + } + return true; + } + return false; +} + +bool ObjectStreamTextIn::ReadName(String &outName) +{ + return ReadWord(outName); +} + +bool ObjectStreamTextIn::ReadIdentifier(Identifier &outIdentifier) +{ + String token; + if (!ReadWord(token)) + return false; + outIdentifier = (uint32)std::strtoul(token.c_str(), nullptr, 16); + if (errno == ERANGE) + { + outIdentifier = sNullIdentifier; + return false; + } + return true; +} + +bool ObjectStreamTextIn::ReadCount(uint32 &outCount) +{ + return ReadPrimitiveData(outCount); +} + +bool ObjectStreamTextIn::ReadPrimitiveData(uint8 &outPrimitive) +{ + String token; + if (!ReadWord(token)) + return false; + uint32 temporary; + IStringStream stream(token); + stream >> temporary; + if (!stream.fail()) + { + outPrimitive = (uint8)temporary; + return true; + } + return false; +} + +bool ObjectStreamTextIn::ReadPrimitiveData(uint16 &outPrimitive) +{ + String token; + if (!ReadWord(token)) + return false; + uint32 temporary; + IStringStream stream(token); + stream >> temporary; + if (!stream.fail()) + { + outPrimitive = (uint16)temporary; + return true; + } + return false; +} + +bool ObjectStreamTextIn::ReadPrimitiveData(int &outPrimitive) +{ + String token; + if (!ReadWord(token)) + return false; + IStringStream stream(token); + stream >> outPrimitive; + return !stream.fail(); +} + +bool ObjectStreamTextIn::ReadPrimitiveData(uint32 &outPrimitive) +{ + String token; + if (!ReadWord(token)) + return false; + IStringStream stream(token); + stream >> outPrimitive; + return !stream.fail(); +} + +bool ObjectStreamTextIn::ReadPrimitiveData(uint64 &outPrimitive) +{ + String token; + if (!ReadWord(token)) + return false; + IStringStream stream(token); + stream >> outPrimitive; + return !stream.fail(); +} + +bool ObjectStreamTextIn::ReadPrimitiveData(float &outPrimitive) +{ + String token; + if (!ReadWord(token)) + return false; + IStringStream stream(token); + stream >> outPrimitive; + return !stream.fail(); +} + +bool ObjectStreamTextIn::ReadPrimitiveData(double &outPrimitive) +{ + String token; + if (!ReadWord(token)) + return false; + IStringStream stream(token); + stream >> outPrimitive; + return !stream.fail(); +} + +bool ObjectStreamTextIn::ReadPrimitiveData(bool &outPrimitive) +{ + String token; + if (!ReadWord(token)) + return false; + transform(token.begin(), token.end(), token.begin(), [](char inValue) { return (char)tolower(inValue); }); + outPrimitive = token == "true"; + return outPrimitive || token == "false"; +} + +bool ObjectStreamTextIn::ReadPrimitiveData(String &outPrimitive) +{ + outPrimitive.clear(); + + char c; + + // Skip whitespace + for (;;) + { + if (!ReadChar(c)) + return false; + + if (!isspace(c)) + break; + } + + // Check if it is a opening quote + if (c != '\"') + return false; + + // Read string and interpret special characters + String result; + bool escaped = false; + for (;;) + { + if (!ReadChar(c)) + break; + + switch (c) + { + case '\n': + case '\t': + break; + + case '\\': + if (escaped) + { + result += '\\'; + escaped = false; + } + else + escaped = true; + break; + + case 'n': + if (escaped) + { + result += '\n'; + escaped = false; + } + else + result += 'n'; + break; + + case 't': + if (escaped) + { + result += '\t'; + escaped = false; + } + else + result += 't'; + break; + + case '\"': + if (escaped) + { + result += '\"'; + escaped = false; + } + else + { + // Found closing double quote + outPrimitive = result; + return true; + } + break; + + default: + if (escaped) + escaped = false; + else + result += c; + break; + } + } + + return false; +} + +bool ObjectStreamTextIn::ReadPrimitiveData(Float3 &outPrimitive) +{ + float x, y, z; + if (!ReadPrimitiveData(x) || !ReadPrimitiveData(y) || !ReadPrimitiveData(z)) + return false; + outPrimitive = Float3(x, y, z); + return true; +} + +bool ObjectStreamTextIn::ReadPrimitiveData(Double3 &outPrimitive) +{ + double x, y, z; + if (!ReadPrimitiveData(x) || !ReadPrimitiveData(y) || !ReadPrimitiveData(z)) + return false; + outPrimitive = Double3(x, y, z); + return true; +} + +bool ObjectStreamTextIn::ReadPrimitiveData(Vec3 &outPrimitive) +{ + float x, y, z; + if (!ReadPrimitiveData(x) || !ReadPrimitiveData(y) || !ReadPrimitiveData(z)) + return false; + outPrimitive = Vec3(x, y, z); + return true; +} + +bool ObjectStreamTextIn::ReadPrimitiveData(DVec3 &outPrimitive) +{ + double x, y, z; + if (!ReadPrimitiveData(x) || !ReadPrimitiveData(y) || !ReadPrimitiveData(z)) + return false; + outPrimitive = DVec3(x, y, z); + return true; +} + +bool ObjectStreamTextIn::ReadPrimitiveData(Vec4 &outPrimitive) +{ + float x, y, z, w; + if (!ReadPrimitiveData(x) || !ReadPrimitiveData(y) || !ReadPrimitiveData(z) || !ReadPrimitiveData(w)) + return false; + outPrimitive = Vec4(x, y, z, w); + return true; +} + +bool ObjectStreamTextIn::ReadPrimitiveData(Quat &outPrimitive) +{ + float x, y, z, w; + if (!ReadPrimitiveData(x) || !ReadPrimitiveData(y) || !ReadPrimitiveData(z) || !ReadPrimitiveData(w)) + return false; + outPrimitive = Quat(x, y, z, w); + return true; +} + +bool ObjectStreamTextIn::ReadPrimitiveData(Mat44 &outPrimitive) +{ + Vec4 c0, c1, c2, c3; + if (!ReadPrimitiveData(c0) || !ReadPrimitiveData(c1) || !ReadPrimitiveData(c2) || !ReadPrimitiveData(c3)) + return false; + outPrimitive = Mat44(c0, c1, c2, c3); + return true; +} + +bool ObjectStreamTextIn::ReadPrimitiveData(DMat44 &outPrimitive) +{ + Vec4 c0, c1, c2; + DVec3 c3; + if (!ReadPrimitiveData(c0) || !ReadPrimitiveData(c1) || !ReadPrimitiveData(c2) || !ReadPrimitiveData(c3)) + return false; + outPrimitive = DMat44(c0, c1, c2, c3); + return true; +} + +bool ObjectStreamTextIn::ReadChar(char &outChar) +{ + mStream.get(outChar); + return !mStream.eof(); +} + +bool ObjectStreamTextIn::ReadWord(String &outWord) +{ + outWord.clear(); + + char c; + + // Skip whitespace + for (;;) + { + if (!ReadChar(c)) + return false; + + if (!isspace(c)) + break; + } + + // Read word + for (;;) + { + outWord += c; + + if (!ReadChar(c)) + break; + + if (isspace(c)) + break; + } + + return !outWord.empty(); +} + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamTextIn.h b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamTextIn.h new file mode 100644 index 000000000000..c6a164e73ca5 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamTextIn.h @@ -0,0 +1,53 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +/// Implementation of ObjectStream text input stream. +class JPH_EXPORT ObjectStreamTextIn : public ObjectStreamIn +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit ObjectStreamTextIn(istream &inStream); + + ///@name Input type specific operations + virtual bool ReadDataType(EOSDataType &outType) override; + virtual bool ReadName(String &outName) override; + virtual bool ReadIdentifier(Identifier &outIdentifier) override; + virtual bool ReadCount(uint32 &outCount) override; + + virtual bool ReadPrimitiveData(uint8 &outPrimitive) override; + virtual bool ReadPrimitiveData(uint16 &outPrimitive) override; + virtual bool ReadPrimitiveData(int &outPrimitive) override; + virtual bool ReadPrimitiveData(uint32 &outPrimitive) override; + virtual bool ReadPrimitiveData(uint64 &outPrimitive) override; + virtual bool ReadPrimitiveData(float &outPrimitive) override; + virtual bool ReadPrimitiveData(double &outPrimitive) override; + virtual bool ReadPrimitiveData(bool &outPrimitive) override; + virtual bool ReadPrimitiveData(String &outPrimitive) override; + virtual bool ReadPrimitiveData(Float3 &outPrimitive) override; + virtual bool ReadPrimitiveData(Double3 &outPrimitive) override; + virtual bool ReadPrimitiveData(Vec3 &outPrimitive) override; + virtual bool ReadPrimitiveData(DVec3 &outPrimitive) override; + virtual bool ReadPrimitiveData(Vec4 &outPrimitive) override; + virtual bool ReadPrimitiveData(Quat &outPrimitive) override; + virtual bool ReadPrimitiveData(Mat44 &outPrimitive) override; + virtual bool ReadPrimitiveData(DMat44 &outPrimitive) override; + +private: + bool ReadChar(char &outChar); + bool ReadWord(String &outWord); +}; + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamTextOut.cpp b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamTextOut.cpp new file mode 100644 index 000000000000..7d5b13e33aca --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamTextOut.cpp @@ -0,0 +1,231 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#ifdef JPH_OBJECT_STREAM + +#include +#include + +JPH_NAMESPACE_BEGIN + +ObjectStreamTextOut::ObjectStreamTextOut(ostream &inStream) : + ObjectStreamOut(inStream) +{ + WriteWord(StringFormat("TOS%2d.%02d", ObjectStream::sVersion, ObjectStream::sRevision)); +} + +void ObjectStreamTextOut::WriteDataType(EOSDataType inType) +{ + switch (inType) + { + case EOSDataType::Declare: WriteWord("declare "); break; + case EOSDataType::Object: WriteWord("object "); break; + case EOSDataType::Instance: WriteWord("instance "); break; + case EOSDataType::Pointer: WriteWord("pointer "); break; + case EOSDataType::Array: WriteWord("array "); break; + case EOSDataType::T_uint8: WriteWord("uint8"); break; + case EOSDataType::T_uint16: WriteWord("uint16"); break; + case EOSDataType::T_int: WriteWord("int"); break; + case EOSDataType::T_uint32: WriteWord("uint32"); break; + case EOSDataType::T_uint64: WriteWord("uint64"); break; + case EOSDataType::T_float: WriteWord("float"); break; + case EOSDataType::T_double: WriteWord("double"); break; + case EOSDataType::T_bool: WriteWord("bool"); break; + case EOSDataType::T_String: WriteWord("string"); break; + case EOSDataType::T_Float3: WriteWord("float3"); break; + case EOSDataType::T_Double3: WriteWord("double3"); break; + case EOSDataType::T_Vec3: WriteWord("vec3"); break; + case EOSDataType::T_DVec3: WriteWord("dvec3"); break; + case EOSDataType::T_Vec4: WriteWord("vec4"); break; + case EOSDataType::T_Quat: WriteWord("quat"); break; + case EOSDataType::T_Mat44: WriteWord("mat44"); break; + case EOSDataType::T_DMat44: WriteWord("dmat44"); break; + case EOSDataType::Invalid: + default: JPH_ASSERT(false); break; + } +} + +void ObjectStreamTextOut::WriteName(const char *inName) +{ + WriteWord(String(inName) + " "); +} + +void ObjectStreamTextOut::WriteIdentifier(Identifier inIdentifier) +{ + WriteWord(StringFormat("%08X", inIdentifier)); +} + +void ObjectStreamTextOut::WriteCount(uint32 inCount) +{ + WriteWord(std::to_string(inCount)); +} + +void ObjectStreamTextOut::WritePrimitiveData(const uint8 &inPrimitive) +{ + WriteWord(std::to_string(inPrimitive)); +} + +void ObjectStreamTextOut::WritePrimitiveData(const uint16 &inPrimitive) +{ + WriteWord(std::to_string(inPrimitive)); +} + +void ObjectStreamTextOut::WritePrimitiveData(const int &inPrimitive) +{ + WriteWord(std::to_string(inPrimitive)); +} + +void ObjectStreamTextOut::WritePrimitiveData(const uint32 &inPrimitive) +{ + WriteWord(std::to_string(inPrimitive)); +} + +void ObjectStreamTextOut::WritePrimitiveData(const uint64 &inPrimitive) +{ + WriteWord(std::to_string(inPrimitive)); +} + +void ObjectStreamTextOut::WritePrimitiveData(const float &inPrimitive) +{ + std::ostringstream stream; + stream.precision(9); + stream << inPrimitive; + WriteWord(stream.str()); +} + +void ObjectStreamTextOut::WritePrimitiveData(const double &inPrimitive) +{ + std::ostringstream stream; + stream.precision(17); + stream << inPrimitive; + WriteWord(stream.str()); +} + +void ObjectStreamTextOut::WritePrimitiveData(const bool &inPrimitive) +{ + WriteWord(inPrimitive? "true" : "false"); +} + +void ObjectStreamTextOut::WritePrimitiveData(const Float3 &inPrimitive) +{ + WritePrimitiveData(inPrimitive.x); + WriteChar(' '); + WritePrimitiveData(inPrimitive.y); + WriteChar(' '); + WritePrimitiveData(inPrimitive.z); +} + +void ObjectStreamTextOut::WritePrimitiveData(const Double3 &inPrimitive) +{ + WritePrimitiveData(inPrimitive.x); + WriteChar(' '); + WritePrimitiveData(inPrimitive.y); + WriteChar(' '); + WritePrimitiveData(inPrimitive.z); +} + +void ObjectStreamTextOut::WritePrimitiveData(const Vec3 &inPrimitive) +{ + WritePrimitiveData(inPrimitive.GetX()); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetY()); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetZ()); +} + +void ObjectStreamTextOut::WritePrimitiveData(const DVec3 &inPrimitive) +{ + WritePrimitiveData(inPrimitive.GetX()); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetY()); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetZ()); +} + +void ObjectStreamTextOut::WritePrimitiveData(const Vec4 &inPrimitive) +{ + WritePrimitiveData(inPrimitive.GetX()); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetY()); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetZ()); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetW()); +} + +void ObjectStreamTextOut::WritePrimitiveData(const Quat &inPrimitive) +{ + WritePrimitiveData(inPrimitive.GetX()); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetY()); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetZ()); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetW()); +} + +void ObjectStreamTextOut::WritePrimitiveData(const Mat44 &inPrimitive) +{ + WritePrimitiveData(inPrimitive.GetColumn4(0)); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetColumn4(1)); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetColumn4(2)); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetColumn4(3)); +} + +void ObjectStreamTextOut::WritePrimitiveData(const DMat44 &inPrimitive) +{ + WritePrimitiveData(inPrimitive.GetColumn4(0)); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetColumn4(1)); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetColumn4(2)); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetTranslation()); +} + +void ObjectStreamTextOut::WritePrimitiveData(const String &inPrimitive) +{ + String temporary(inPrimitive); + StringReplace(temporary, "\\", "\\\\"); + StringReplace(temporary, "\n", "\\n"); + StringReplace(temporary, "\t", "\\t"); + StringReplace(temporary, "\"", "\\\""); + WriteWord(String("\"") + temporary + String("\"")); +} + +void ObjectStreamTextOut::HintNextItem() +{ + WriteWord("\r\n"); + for (int i = 0; i < mIndentation; ++i) + WriteWord(" "); +} + +void ObjectStreamTextOut::HintIndentUp() +{ + ++mIndentation; +} + +void ObjectStreamTextOut::HintIndentDown() +{ + --mIndentation; +} + +void ObjectStreamTextOut::WriteChar(char inChar) +{ + mStream.put(inChar); +} + +void ObjectStreamTextOut::WriteWord(const string_view &inWord) +{ + mStream << inWord; +} + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamTextOut.h b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamTextOut.h new file mode 100644 index 000000000000..5a71cfb47b49 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamTextOut.h @@ -0,0 +1,60 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +/// Implementation of ObjectStream text output stream. +class JPH_EXPORT ObjectStreamTextOut : public ObjectStreamOut +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor and destructor + explicit ObjectStreamTextOut(ostream &inStream); + + ///@name Output type specific operations + virtual void WriteDataType(EOSDataType inType) override; + virtual void WriteName(const char *inName) override; + virtual void WriteIdentifier(Identifier inIdentifier) override; + virtual void WriteCount(uint32 inCount) override; + + virtual void WritePrimitiveData(const uint8 &inPrimitive) override; + virtual void WritePrimitiveData(const uint16 &inPrimitive) override; + virtual void WritePrimitiveData(const int &inPrimitive) override; + virtual void WritePrimitiveData(const uint32 &inPrimitive) override; + virtual void WritePrimitiveData(const uint64 &inPrimitive) override; + virtual void WritePrimitiveData(const float &inPrimitive) override; + virtual void WritePrimitiveData(const double &inPrimitive) override; + virtual void WritePrimitiveData(const bool &inPrimitive) override; + virtual void WritePrimitiveData(const String &inPrimitive) override; + virtual void WritePrimitiveData(const Float3 &inPrimitive) override; + virtual void WritePrimitiveData(const Double3 &inPrimitive) override; + virtual void WritePrimitiveData(const Vec3 &inPrimitive) override; + virtual void WritePrimitiveData(const DVec3 &inPrimitive) override; + virtual void WritePrimitiveData(const Vec4 &inPrimitive) override; + virtual void WritePrimitiveData(const Quat &inPrimitive) override; + virtual void WritePrimitiveData(const Mat44 &inPrimitive) override; + virtual void WritePrimitiveData(const DMat44 &inPrimitive) override; + + ///@name Layout hints (for text output) + virtual void HintNextItem() override; + virtual void HintIndentUp() override; + virtual void HintIndentDown() override; + +private: + void WriteChar(char inChar); + void WriteWord(const string_view &inWord); + + int mIndentation = 0; +}; + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamTypes.h b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamTypes.h new file mode 100644 index 000000000000..6fb08af17107 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStreamTypes.h @@ -0,0 +1,24 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +// Note: Order is important, an enum is created and its value is stored in a binary stream! +JPH_DECLARE_PRIMITIVE(uint8) +JPH_DECLARE_PRIMITIVE(uint16) +JPH_DECLARE_PRIMITIVE(int) +JPH_DECLARE_PRIMITIVE(uint32) +JPH_DECLARE_PRIMITIVE(uint64) +JPH_DECLARE_PRIMITIVE(float) +JPH_DECLARE_PRIMITIVE(bool) +JPH_DECLARE_PRIMITIVE(String) +JPH_DECLARE_PRIMITIVE(Float3) +JPH_DECLARE_PRIMITIVE(Vec3) +JPH_DECLARE_PRIMITIVE(Vec4) +JPH_DECLARE_PRIMITIVE(Quat) +JPH_DECLARE_PRIMITIVE(Mat44) +JPH_DECLARE_PRIMITIVE(double) +JPH_DECLARE_PRIMITIVE(DVec3) +JPH_DECLARE_PRIMITIVE(DMat44) +JPH_DECLARE_PRIMITIVE(Double3) + +#undef JPH_DECLARE_PRIMITIVE diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttribute.h b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttribute.h new file mode 100644 index 000000000000..9a6d8802187e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttribute.h @@ -0,0 +1,111 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +class RTTI; +class IObjectStreamIn; +class IObjectStreamOut; + +/// Data type +enum class EOSDataType +{ + /// Control codes + Declare, ///< Used to declare the attributes of a new object type + Object, ///< Start of a new object + Instance, ///< Used in attribute declaration, indicates that an object is an instanced attribute (no pointer) + Pointer, ///< Used in attribute declaration, indicates that an object is a pointer attribute + Array, ///< Used in attribute declaration, indicates that this is an array of objects + + // Basic types (primitives) + #define JPH_DECLARE_PRIMITIVE(name) T_##name, + + // This file uses the JPH_DECLARE_PRIMITIVE macro to define all types + #include + + // Error values for read functions + Invalid, ///< Next token on the stream was not a valid data type +}; + +/// Attributes are members of classes that need to be serialized. +class SerializableAttribute +{ +public: + ///@ Serialization functions + using pGetMemberPrimitiveType = const RTTI * (*)(); + using pIsType = bool (*)(int inArrayDepth, EOSDataType inDataType, const char *inClassName); + using pReadData = bool (*)(IObjectStreamIn &ioStream, void *inObject); + using pWriteData = void (*)(IObjectStreamOut &ioStream, const void *inObject); + using pWriteDataType = void (*)(IObjectStreamOut &ioStream); + + /// Constructor + SerializableAttribute(const char *inName, uint inMemberOffset, pGetMemberPrimitiveType inGetMemberPrimitiveType, pIsType inIsType, pReadData inReadData, pWriteData inWriteData, pWriteDataType inWriteDataType) : mName(inName), mMemberOffset(inMemberOffset), mGetMemberPrimitiveType(inGetMemberPrimitiveType), mIsType(inIsType), mReadData(inReadData), mWriteData(inWriteData), mWriteDataType(inWriteDataType) { } + + /// Construct from other attribute with base class offset + SerializableAttribute(const SerializableAttribute &inOther, int inBaseOffset) : mName(inOther.mName), mMemberOffset(inOther.mMemberOffset + inBaseOffset), mGetMemberPrimitiveType(inOther.mGetMemberPrimitiveType), mIsType(inOther.mIsType), mReadData(inOther.mReadData), mWriteData(inOther.mWriteData), mWriteDataType(inOther.mWriteDataType) { } + + /// Name of the attribute + void SetName(const char *inName) { mName = inName; } + const char * GetName() const { return mName; } + + /// Access to the memory location that contains the member + template + inline T * GetMemberPointer(void *inObject) const { return reinterpret_cast(reinterpret_cast(inObject) + mMemberOffset); } + template + inline const T * GetMemberPointer(const void *inObject) const { return reinterpret_cast(reinterpret_cast(inObject) + mMemberOffset); } + + /// In case this attribute contains an RTTI type, return it (note that a Array will return the rtti of sometype) + const RTTI * GetMemberPrimitiveType() const + { + return mGetMemberPrimitiveType(); + } + + /// Check if this attribute is of a specific type + bool IsType(int inArrayDepth, EOSDataType inDataType, const char *inClassName) const + { + return mIsType(inArrayDepth, inDataType, inClassName); + } + + /// Read the data for this attribute into attribute containing class inObject + bool ReadData(IObjectStreamIn &ioStream, void *inObject) const + { + return mReadData(ioStream, GetMemberPointer(inObject)); + } + + /// Write the data for this attribute from attribute containing class inObject + void WriteData(IObjectStreamOut &ioStream, const void *inObject) const + { + mWriteData(ioStream, GetMemberPointer(inObject)); + } + + /// Write the data type of this attribute to a stream + void WriteDataType(IObjectStreamOut &ioStream) const + { + mWriteDataType(ioStream); + } + +private: + // Name of the attribute + const char * mName; + + // Offset of the member relative to the class + uint mMemberOffset; + + // In case this attribute contains an RTTI type, return it (note that a Array will return the rtti of sometype) + pGetMemberPrimitiveType mGetMemberPrimitiveType; + + // Serialization operations + pIsType mIsType; + pReadData mReadData; + pWriteData mWriteData; + pWriteDataType mWriteDataType; +}; + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttributeEnum.h b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttributeEnum.h new file mode 100644 index 000000000000..c6f241ea41c4 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttributeEnum.h @@ -0,0 +1,67 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifdef JPH_OBJECT_STREAM + +#include +#include + +JPH_NAMESPACE_BEGIN + +////////////////////////////////////////////////////////////////////////////////////////// +// Macros to add properties to be serialized +////////////////////////////////////////////////////////////////////////////////////////// + +template +inline void AddSerializableAttributeEnum(RTTI &inRTTI, uint inOffset, const char *inName) +{ + inRTTI.AddAttribute(SerializableAttribute(inName, inOffset, + []() -> const RTTI * + { + return nullptr; + }, + [](int inArrayDepth, EOSDataType inDataType, [[maybe_unused]] const char *inClassName) + { + return inArrayDepth == 0 && inDataType == EOSDataType::T_uint32; + }, + [](IObjectStreamIn &ioStream, void *inObject) + { + uint32 temporary; + if (OSReadData(ioStream, temporary)) + { + *reinterpret_cast(inObject) = static_cast(temporary); + return true; + } + return false; + }, + [](IObjectStreamOut &ioStream, const void *inObject) + { + static_assert(sizeof(MemberType) <= sizeof(uint32)); + uint32 temporary = uint32(*reinterpret_cast(inObject)); + OSWriteData(ioStream, temporary); + }, + [](IObjectStreamOut &ioStream) + { + ioStream.WriteDataType(EOSDataType::T_uint32); + })); +} + +// JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS +#define JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(class_name, member_name, alias_name) \ + AddSerializableAttributeEnum(inRTTI, offsetof(class_name, member_name), alias_name); + +// JPH_ADD_ENUM_ATTRIBUTE +#define JPH_ADD_ENUM_ATTRIBUTE(class_name, member_name) \ + JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(class_name, member_name, #member_name); + +JPH_NAMESPACE_END + +#else + +#define JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(...) +#define JPH_ADD_ENUM_ATTRIBUTE(...) + +#endif // JPH_OBJECT_STREAM diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttributeTyped.h b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttributeTyped.h new file mode 100644 index 000000000000..ce18325c46cb --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttributeTyped.h @@ -0,0 +1,60 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifdef JPH_OBJECT_STREAM + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +////////////////////////////////////////////////////////////////////////////////////////// +// Macros to add properties to be serialized +////////////////////////////////////////////////////////////////////////////////////////// + +template +inline void AddSerializableAttributeTyped(RTTI &inRTTI, uint inOffset, const char *inName) +{ + inRTTI.AddAttribute(SerializableAttribute(inName, inOffset, + []() + { + return GetPrimitiveTypeOfType((MemberType *)nullptr); + }, + [](int inArrayDepth, EOSDataType inDataType, const char *inClassName) + { + return OSIsType((MemberType *)nullptr, inArrayDepth, inDataType, inClassName); + }, + [](IObjectStreamIn &ioStream, void *inObject) + { + return OSReadData(ioStream, *reinterpret_cast(inObject)); + }, + [](IObjectStreamOut &ioStream, const void *inObject) + { + OSWriteData(ioStream, *reinterpret_cast(inObject)); + }, + [](IObjectStreamOut &ioStream) + { + OSWriteDataType(ioStream, (MemberType *)nullptr); + })); +} + +// JPH_ADD_ATTRIBUTE +#define JPH_ADD_ATTRIBUTE_WITH_ALIAS(class_name, member_name, alias_name) \ + AddSerializableAttributeTyped(inRTTI, offsetof(class_name, member_name), alias_name); + +// JPH_ADD_ATTRIBUTE +#define JPH_ADD_ATTRIBUTE(class_name, member_name) \ + JPH_ADD_ATTRIBUTE_WITH_ALIAS(class_name, member_name, #member_name) + +JPH_NAMESPACE_END + +#else + +#define JPH_ADD_ATTRIBUTE_WITH_ALIAS(...) +#define JPH_ADD_ATTRIBUTE(...) + +#endif // JPH_OBJECT_STREAM diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.cpp b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.cpp new file mode 100644 index 000000000000..98d3b3cf8d09 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.cpp @@ -0,0 +1,15 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(SerializableObject) +{ +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.h b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.h new file mode 100644 index 000000000000..86b8830c259a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.h @@ -0,0 +1,164 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +////////////////////////////////////////////////////////////////////////////////////////// +// Helper macros +////////////////////////////////////////////////////////////////////////////////////////// + +#ifdef JPH_OBJECT_STREAM + +// JPH_DECLARE_SERIALIZATION_FUNCTIONS +#define JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, prefix, class_name) \ + linkage prefix bool OSReadData(IObjectStreamIn &ioStream, class_name &inInstance); \ + linkage prefix bool OSReadData(IObjectStreamIn &ioStream, class_name *&inPointer); \ + linkage prefix bool OSIsType(class_name *, int inArrayDepth, EOSDataType inDataType, const char *inClassName); \ + linkage prefix bool OSIsType(class_name **, int inArrayDepth, EOSDataType inDataType, const char *inClassName); \ + linkage prefix void OSWriteData(IObjectStreamOut &ioStream, const class_name &inInstance); \ + linkage prefix void OSWriteData(IObjectStreamOut &ioStream, class_name *const &inPointer); \ + linkage prefix void OSWriteDataType(IObjectStreamOut &ioStream, class_name *); \ + linkage prefix void OSWriteDataType(IObjectStreamOut &ioStream, class_name **); + +// JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS +#define JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name) \ + bool OSReadData(IObjectStreamIn &ioStream, class_name &inInstance) \ + { \ + return ioStream.ReadClassData(#class_name, (void *)&inInstance); \ + } \ + bool OSReadData(IObjectStreamIn &ioStream, class_name *&inPointer) \ + { \ + return ioStream.ReadPointerData(JPH_RTTI(class_name), (void **)&inPointer); \ + } \ + bool OSIsType(class_name *, int inArrayDepth, EOSDataType inDataType, const char *inClassName) \ + { \ + return inArrayDepth == 0 && inDataType == EOSDataType::Instance && strcmp(inClassName, #class_name) == 0; \ + } \ + bool OSIsType(class_name **, int inArrayDepth, EOSDataType inDataType, const char *inClassName) \ + { \ + return inArrayDepth == 0 && inDataType == EOSDataType::Pointer && strcmp(inClassName, #class_name) == 0; \ + } \ + void OSWriteData(IObjectStreamOut &ioStream, const class_name &inInstance) \ + { \ + ioStream.WriteClassData(JPH_RTTI(class_name), (void *)&inInstance); \ + } \ + void OSWriteData(IObjectStreamOut &ioStream, class_name *const &inPointer) \ + { \ + if (inPointer) \ + ioStream.WritePointerData(GetRTTI(inPointer), (void *)inPointer); \ + else \ + ioStream.WritePointerData(nullptr, nullptr); \ + } \ + void OSWriteDataType(IObjectStreamOut &ioStream, class_name *) \ + { \ + ioStream.WriteDataType(EOSDataType::Instance); \ + ioStream.WriteName(#class_name); \ + } \ + void OSWriteDataType(IObjectStreamOut &ioStream, class_name **) \ + { \ + ioStream.WriteDataType(EOSDataType::Pointer); \ + ioStream.WriteName(#class_name); \ + } + +#else + +#define JPH_DECLARE_SERIALIZATION_FUNCTIONS(...) +#define JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(...) + +#endif // JPH_OBJECT_STREAM + +////////////////////////////////////////////////////////////////////////////////////////// +// Use these macros on non-virtual objects to make them serializable +////////////////////////////////////////////////////////////////////////////////////////// + +// JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL +#define JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(linkage, class_name) \ +public: \ + JPH_DECLARE_RTTI_NON_VIRTUAL(linkage, class_name) \ + JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, friend, class_name) \ + +// JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL +#define JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(class_name) \ + JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name) \ + JPH_IMPLEMENT_RTTI_NON_VIRTUAL(class_name) \ + +////////////////////////////////////////////////////////////////////////////////////////// +// Same as above, but when you cannot insert the declaration in the class itself +////////////////////////////////////////////////////////////////////////////////////////// + +// JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS +#define JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(linkage, class_name) \ + JPH_DECLARE_RTTI_OUTSIDE_CLASS(linkage, class_name) \ + JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, extern, class_name) \ + +// JPH_IMPLEMENT_SERIALIZABLE_OUTSIDE_CLASS +#define JPH_IMPLEMENT_SERIALIZABLE_OUTSIDE_CLASS(class_name) \ + JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name) \ + JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(class_name) \ + +////////////////////////////////////////////////////////////////////////////////////////// +// Same as above, but for classes that have virtual functions +////////////////////////////////////////////////////////////////////////////////////////// + +// JPH_DECLARE_SERIALIZABLE_VIRTUAL - Use for concrete, non-base classes +#define JPH_DECLARE_SERIALIZABLE_VIRTUAL(linkage, class_name) \ +public: \ + JPH_DECLARE_RTTI_VIRTUAL(linkage, class_name) \ + JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, friend, class_name) \ + +// JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL +#define JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(class_name) \ + JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name) \ + JPH_IMPLEMENT_RTTI_VIRTUAL(class_name) \ + +// JPH_DECLARE_SERIALIZABLE_ABSTRACT - Use for abstract, non-base classes +#define JPH_DECLARE_SERIALIZABLE_ABSTRACT(linkage, class_name) \ +public: \ + JPH_DECLARE_RTTI_ABSTRACT(linkage, class_name) \ + JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, friend, class_name) \ + +// JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT +#define JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(class_name) \ + JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name) \ + JPH_IMPLEMENT_RTTI_ABSTRACT(class_name) \ + +// JPH_DECLARE_SERIALIZABLE_VIRTUAL_BASE - Use for concrete base classes +#define JPH_DECLARE_SERIALIZABLE_VIRTUAL_BASE(linkage, class_name) \ +public: \ + JPH_DECLARE_RTTI_VIRTUAL_BASE(linkage, class_name) \ + JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, friend, class_name) \ + +// JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL_BASE +#define JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL_BASE(class_name) \ + JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name) \ + JPH_IMPLEMENT_RTTI_VIRTUAL_BASE(class_name) \ + +// JPH_DECLARE_SERIALIZABLE_ABSTRACT_BASE - Use for abstract base class +#define JPH_DECLARE_SERIALIZABLE_ABSTRACT_BASE(linkage, class_name) \ +public: \ + JPH_DECLARE_RTTI_ABSTRACT_BASE(linkage, class_name) \ + JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, friend, class_name) \ + +// JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT_BASE +#define JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT_BASE(class_name) \ + JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name) \ + JPH_IMPLEMENT_RTTI_ABSTRACT_BASE(class_name) + +/// 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 +{ + JPH_DECLARE_SERIALIZABLE_ABSTRACT_BASE(JPH_EXPORT, SerializableObject) + +public: + /// Constructor + virtual ~SerializableObject() = default; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/TypeDeclarations.cpp b/thirdparty/jolt_physics/Jolt/ObjectStream/TypeDeclarations.cpp new file mode 100644 index 000000000000..b081b22afe82 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/TypeDeclarations.cpp @@ -0,0 +1,62 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(uint8) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(uint16) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(int) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(uint32) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(uint64) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(float) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(double) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(bool) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(String) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(Float3) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(Double3) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(Vec3) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(DVec3) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(Vec4) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(Quat) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(Mat44) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(DMat44) { } + +JPH_IMPLEMENT_SERIALIZABLE_OUTSIDE_CLASS(Color) +{ + JPH_ADD_ATTRIBUTE(Color, r) + JPH_ADD_ATTRIBUTE(Color, g) + JPH_ADD_ATTRIBUTE(Color, b) + JPH_ADD_ATTRIBUTE(Color, a) +} + +JPH_IMPLEMENT_SERIALIZABLE_OUTSIDE_CLASS(AABox) +{ + JPH_ADD_ATTRIBUTE(AABox, mMin) + JPH_ADD_ATTRIBUTE(AABox, mMax) +} + +JPH_IMPLEMENT_SERIALIZABLE_OUTSIDE_CLASS(Triangle) +{ + JPH_ADD_ATTRIBUTE(Triangle, mV) + JPH_ADD_ATTRIBUTE(Triangle, mMaterialIndex) + JPH_ADD_ATTRIBUTE(Triangle, mUserData) +} + +JPH_IMPLEMENT_SERIALIZABLE_OUTSIDE_CLASS(IndexedTriangle) +{ + JPH_ADD_ATTRIBUTE(IndexedTriangle, mIdx) + JPH_ADD_ATTRIBUTE(IndexedTriangle, mMaterialIndex) + JPH_ADD_ATTRIBUTE(IndexedTriangle, mUserData) +} + +JPH_IMPLEMENT_SERIALIZABLE_OUTSIDE_CLASS(Plane) +{ + JPH_ADD_ATTRIBUTE(Plane, mNormalAndConstant) +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/TypeDeclarations.h b/thirdparty/jolt_physics/Jolt/ObjectStream/TypeDeclarations.h new file mode 100644 index 000000000000..015e1dbf1445 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/TypeDeclarations.h @@ -0,0 +1,42 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, uint8); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, uint16); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, int); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, uint32); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, uint64); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, float); +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, 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, 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, IndexedTriangle); +JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(JPH_EXPORT, Plane); + +JPH_NAMESPACE_END + +// These need to be added after all types have been registered or else clang under linux will not find GetRTTIOfType for the type +#include +#include diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/AllowedDOFs.h b/thirdparty/jolt_physics/Jolt/Physics/Body/AllowedDOFs.h new file mode 100644 index 000000000000..8445cb186335 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/AllowedDOFs.h @@ -0,0 +1,68 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Enum used in BodyCreationSettings and MotionProperties to indicate which degrees of freedom a body has +enum class EAllowedDOFs : uint8 +{ + None = 0b000000, ///< No degrees of freedom are allowed. Note that this is not valid and will crash. Use a static body instead. + All = 0b111111, ///< All degrees of freedom are allowed + TranslationX = 0b000001, ///< Body can move in world space X axis + TranslationY = 0b000010, ///< Body can move in world space Y axis + TranslationZ = 0b000100, ///< Body can move in world space Z axis + RotationX = 0b001000, ///< Body can rotate around world space X axis + RotationY = 0b010000, ///< Body can rotate around world space Y axis + RotationZ = 0b100000, ///< Body can rotate around world space Z axis + Plane2D = TranslationX | TranslationY | RotationZ, ///< Body can only move in X and Y axis and rotate around Z axis +}; + +/// Bitwise OR operator for EAllowedDOFs +constexpr EAllowedDOFs operator | (EAllowedDOFs inLHS, EAllowedDOFs inRHS) +{ + return EAllowedDOFs(uint8(inLHS) | uint8(inRHS)); +} + +/// Bitwise AND operator for EAllowedDOFs +constexpr EAllowedDOFs operator & (EAllowedDOFs inLHS, EAllowedDOFs inRHS) +{ + return EAllowedDOFs(uint8(inLHS) & uint8(inRHS)); +} + +/// Bitwise XOR operator for EAllowedDOFs +constexpr EAllowedDOFs operator ^ (EAllowedDOFs inLHS, EAllowedDOFs inRHS) +{ + return EAllowedDOFs(uint8(inLHS) ^ uint8(inRHS)); +} + +/// Bitwise NOT operator for EAllowedDOFs +constexpr EAllowedDOFs operator ~ (EAllowedDOFs inAllowedDOFs) +{ + return EAllowedDOFs(~uint8(inAllowedDOFs)); +} + +/// Bitwise OR assignment operator for EAllowedDOFs +constexpr EAllowedDOFs & operator |= (EAllowedDOFs &ioLHS, EAllowedDOFs inRHS) +{ + ioLHS = ioLHS | inRHS; + return ioLHS; +} + +/// Bitwise AND assignment operator for EAllowedDOFs +constexpr EAllowedDOFs & operator &= (EAllowedDOFs &ioLHS, EAllowedDOFs inRHS) +{ + ioLHS = ioLHS & inRHS; + return ioLHS; +} + +/// Bitwise XOR assignment operator for EAllowedDOFs +constexpr EAllowedDOFs & operator ^= (EAllowedDOFs &ioLHS, EAllowedDOFs inRHS) +{ + ioLHS = ioLHS ^ inRHS; + return ioLHS; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/Body.cpp b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.cpp new file mode 100644 index 000000000000..4386ec2db252 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.cpp @@ -0,0 +1,423 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +static const EmptyShape sFixedToWorldShape; +Body Body::sFixedToWorld(false); + +Body::Body(bool) : + mPosition(Vec3::sZero()), + mRotation(Quat::sIdentity()), + mShape(&sFixedToWorldShape), // Dummy shape + mFriction(0.0f), + mRestitution(0.0f), + mObjectLayer(cObjectLayerInvalid), + mMotionType(EMotionType::Static) +{ + sFixedToWorldShape.SetEmbedded(); +} + +void Body::SetMotionType(EMotionType inMotionType) +{ + if (mMotionType == inMotionType) + return; + + JPH_ASSERT(inMotionType == EMotionType::Static || mMotionProperties != nullptr, "Body needs to be created with mAllowDynamicOrKinematic set tot true"); + JPH_ASSERT(inMotionType != EMotionType::Static || !IsActive(), "Deactivate body first"); + JPH_ASSERT(inMotionType == EMotionType::Dynamic || !IsSoftBody(), "Soft bodies can only be dynamic, you can make individual vertices kinematic by setting their inverse mass to 0"); + + // Store new motion type + mMotionType = inMotionType; + + if (mMotionProperties != nullptr) + { + // Update cache + JPH_IF_ENABLE_ASSERTS(mMotionProperties->mCachedMotionType = inMotionType;) + + switch (inMotionType) + { + case EMotionType::Static: + // Stop the object + mMotionProperties->mLinearVelocity = Vec3::sZero(); + mMotionProperties->mAngularVelocity = Vec3::sZero(); + [[fallthrough]]; + + case EMotionType::Kinematic: + // Cancel forces + mMotionProperties->ResetForce(); + mMotionProperties->ResetTorque(); + break; + + case EMotionType::Dynamic: + break; + } + } +} + +void Body::SetAllowSleeping(bool inAllow) +{ + mMotionProperties->mAllowSleeping = inAllow; + if (inAllow) + ResetSleepTimer(); +} + +void Body::MoveKinematic(RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime) +{ + JPH_ASSERT(IsRigidBody()); // Only valid for rigid bodies + JPH_ASSERT(!IsStatic()); + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); + + // Calculate center of mass at end situation + RVec3 new_com = inTargetPosition + inTargetRotation * mShape->GetCenterOfMass(); + + // Calculate delta position and rotation + Vec3 delta_pos = Vec3(new_com - mPosition); + Quat delta_rotation = inTargetRotation * mRotation.Conjugated(); + + mMotionProperties->MoveKinematic(delta_pos, delta_rotation, inDeltaTime); +} + +void Body::CalculateWorldSpaceBoundsInternal() +{ + mBounds = mShape->GetWorldSpaceBounds(GetCenterOfMassTransform(), Vec3::sReplicate(1.0f)); +} + +void Body::SetPositionAndRotationInternal(RVec3Arg inPosition, QuatArg inRotation, bool inResetSleepTimer) +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite)); + + mPosition = inPosition + inRotation * mShape->GetCenterOfMass(); + mRotation = inRotation; + + // Initialize bounding box + CalculateWorldSpaceBoundsInternal(); + + // Reset sleeping test + if (inResetSleepTimer && mMotionProperties != nullptr) + ResetSleepTimer(); +} + +void Body::UpdateCenterOfMassInternal(Vec3Arg inPreviousCenterOfMass, bool inUpdateMassProperties) +{ + // Update center of mass position so the world position for this body stays the same + mPosition += mRotation * (mShape->GetCenterOfMass() - inPreviousCenterOfMass); + + // Recalculate mass and inertia if requested + if (inUpdateMassProperties && mMotionProperties != nullptr) + mMotionProperties->SetMassProperties(mMotionProperties->GetAllowedDOFs(), mShape->GetMassProperties()); +} + +void Body::SetShapeInternal(const Shape *inShape, bool inUpdateMassProperties) +{ + JPH_ASSERT(IsRigidBody()); // Only valid for rigid bodies + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite)); + + // Get the old center of mass + Vec3 old_com = mShape->GetCenterOfMass(); + + // Update the shape + mShape = inShape; + + // Update center of mass + UpdateCenterOfMassInternal(old_com, inUpdateMassProperties); + + // Recalculate bounding box + CalculateWorldSpaceBoundsInternal(); +} + +ECanSleep Body::UpdateSleepStateInternal(float inDeltaTime, float inMaxMovement, float inTimeBeforeSleep) +{ + // Check override & sensors will never go to sleep (they would stop detecting collisions with sleeping bodies) + if (!mMotionProperties->mAllowSleeping || IsSensor()) + return ECanSleep::CannotSleep; + + // Get the points to test + RVec3 points[3]; + GetSleepTestPoints(points); + +#ifdef JPH_DOUBLE_PRECISION + // Get base offset for spheres + DVec3 offset = mMotionProperties->GetSleepTestOffset(); +#endif // JPH_DOUBLE_PRECISION + + for (int i = 0; i < 3; ++i) + { + Sphere &sphere = mMotionProperties->mSleepTestSpheres[i]; + + // Make point relative to base offset +#ifdef JPH_DOUBLE_PRECISION + Vec3 p = Vec3(points[i] - offset); +#else + Vec3 p = points[i]; +#endif // JPH_DOUBLE_PRECISION + + // Encapsulate the point in a sphere + sphere.EncapsulatePoint(p); + + // Test if it exceeded the max movement + if (sphere.GetRadius() > inMaxMovement) + { + // Body is not sleeping, reset test + mMotionProperties->ResetSleepTestSpheres(points); + return ECanSleep::CannotSleep; + } + } + + return mMotionProperties->AccumulateSleepTime(inDeltaTime, inTimeBeforeSleep); +} + +void Body::GetSubmergedVolume(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outRelativeCenterOfBuoyancy) const +{ + // For GetSubmergedVolume we transform the surface relative to the body position for increased precision + Mat44 rotation = Mat44::sRotation(mRotation); + Plane surface_relative_to_body = Plane::sFromPointAndNormal(inSurfacePosition - mPosition, inSurfaceNormal); + + // Calculate amount of volume that is submerged and what the center of buoyancy is + mShape->GetSubmergedVolume(rotation, Vec3::sReplicate(1.0f), surface_relative_to_body, outTotalVolume, outSubmergedVolume, outRelativeCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, mPosition)); +} + +bool Body::ApplyBuoyancyImpulse(float inTotalVolume, float inSubmergedVolume, Vec3Arg inRelativeCenterOfBuoyancy, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime) +{ + JPH_ASSERT(IsRigidBody()); // Only implemented for rigid bodies currently + + // We follow the approach from 'Game Programming Gems 6' 2.5 Exact Buoyancy for Polyhedra + // All quantities below are in world space + + // If we're not submerged, there's no point in doing the rest of the calculations + if (inSubmergedVolume > 0.0f) + { + #ifdef JPH_DEBUG_RENDERER + // Draw submerged volume properties + if (Shape::sDrawSubmergedVolumes) + { + RVec3 center_of_buoyancy = mPosition + inRelativeCenterOfBuoyancy; + DebugRenderer::sInstance->DrawMarker(center_of_buoyancy, Color::sWhite, 2.0f); + DebugRenderer::sInstance->DrawText3D(center_of_buoyancy, StringFormat("%.3f / %.3f", (double)inSubmergedVolume, (double)inTotalVolume)); + } + #endif // JPH_DEBUG_RENDERER + + // When buoyancy is 1 we want neutral buoyancy, this means that the density of the liquid is the same as the density of the body at that point. + // Buoyancy > 1 should make the object float, < 1 should make it sink. + float inverse_mass = mMotionProperties->GetInverseMass(); + float fluid_density = inBuoyancy / (inTotalVolume * inverse_mass); + + // Buoyancy force = Density of Fluid * Submerged volume * Magnitude of gravity * Up direction (eq 2.5.1) + // Impulse = Force * Delta time + // We should apply this at the center of buoyancy (= center of mass of submerged volume) + Vec3 buoyancy_impulse = -fluid_density * inSubmergedVolume * mMotionProperties->GetGravityFactor() * inGravity * inDeltaTime; + + // Calculate the velocity of the center of buoyancy relative to the fluid + Vec3 linear_velocity = mMotionProperties->GetLinearVelocity(); + Vec3 angular_velocity = mMotionProperties->GetAngularVelocity(); + Vec3 center_of_buoyancy_velocity = linear_velocity + angular_velocity.Cross(inRelativeCenterOfBuoyancy); + Vec3 relative_center_of_buoyancy_velocity = inFluidVelocity - center_of_buoyancy_velocity; + + // Here we deviate from the article, instead of eq 2.5.14 we use a quadratic drag formula: https://en.wikipedia.org/wiki/Drag_%28physics%29 + // Drag force = 0.5 * Fluid Density * (Velocity of fluid - Velocity of center of buoyancy)^2 * Linear Drag * Area Facing the Relative Fluid Velocity + // Again Impulse = Force * Delta Time + // We should apply this at the center of buoyancy (= center of mass for submerged volume with no center of mass offset) + + // Get size of local bounding box + Vec3 size = mShape->GetLocalBounds().GetSize(); + + // Determine area of the local space bounding box in the direction of the relative velocity between the fluid and the center of buoyancy + float area = 0.0f; + 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; + area = local_relative_center_of_buoyancy_velocity.Abs().Dot(size.Swizzle() * size.Swizzle()) / sqrt(relative_center_of_buoyancy_velocity_len_sq); + } + + // Calculate the impulse + Vec3 drag_impulse = (0.5f * fluid_density * inLinearDrag * area * inDeltaTime) * relative_center_of_buoyancy_velocity * relative_center_of_buoyancy_velocity.Length(); + + // Clamp magnitude against current linear velocity to prevent overshoot + float linear_velocity_len_sq = linear_velocity.LengthSq(); + float drag_delta_linear_velocity_len_sq = (drag_impulse * inverse_mass).LengthSq(); + if (drag_delta_linear_velocity_len_sq > linear_velocity_len_sq) + drag_impulse *= sqrt(linear_velocity_len_sq / drag_delta_linear_velocity_len_sq); + + // Calculate the resulting delta linear velocity due to buoyancy and drag + Vec3 delta_linear_velocity = (drag_impulse + buoyancy_impulse) * inverse_mass; + mMotionProperties->AddLinearVelocityStep(delta_linear_velocity); + + // Determine average width of the body (across the three axis) + float l = (size.GetX() + size.GetY() + size.GetZ()) / 3.0f; + + // Drag torque = -Angular Drag * Mass * Submerged volume / Total volume * (Average width of body)^2 * Angular velocity (eq 2.5.15) + Vec3 drag_angular_impulse = (-inAngularDrag * inSubmergedVolume / inTotalVolume * inDeltaTime * Square(l) / inverse_mass) * angular_velocity; + Mat44 inv_inertia = GetInverseInertia(); + Vec3 drag_delta_angular_velocity = inv_inertia * drag_angular_impulse; + + // Clamp magnitude against the current angular velocity to prevent overshoot + float angular_velocity_len_sq = angular_velocity.LengthSq(); + float drag_delta_angular_velocity_len_sq = drag_delta_angular_velocity.LengthSq(); + if (drag_delta_angular_velocity_len_sq > angular_velocity_len_sq) + drag_delta_angular_velocity *= sqrt(angular_velocity_len_sq / drag_delta_angular_velocity_len_sq); + + // Calculate total delta angular velocity due to drag and buoyancy + Vec3 delta_angular_velocity = drag_delta_angular_velocity + inv_inertia * inRelativeCenterOfBuoyancy.Cross(buoyancy_impulse + drag_impulse); + mMotionProperties->AddAngularVelocityStep(delta_angular_velocity); + return true; + } + + return false; +} + +bool Body::ApplyBuoyancyImpulse(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime) +{ + JPH_PROFILE_FUNCTION(); + + float total_volume, submerged_volume; + Vec3 relative_center_of_buoyancy; + GetSubmergedVolume(inSurfacePosition, inSurfaceNormal, total_volume, submerged_volume, relative_center_of_buoyancy); + + return ApplyBuoyancyImpulse(total_volume, submerged_volume, relative_center_of_buoyancy, inBuoyancy, inLinearDrag, inAngularDrag, inFluidVelocity, inGravity, inDeltaTime); +} + +void Body::SaveState(StateRecorder &inStream) const +{ + // Only write properties that can change at runtime + inStream.Write(mPosition); + inStream.Write(mRotation); + + if (mMotionProperties != nullptr) + { + if (IsSoftBody()) + static_cast(mMotionProperties)->SaveState(inStream); + else + mMotionProperties->SaveState(inStream); + } +} + +void Body::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mPosition); + inStream.Read(mRotation); + + if (mMotionProperties != nullptr) + { + if (IsSoftBody()) + static_cast(mMotionProperties)->RestoreState(inStream); + else + mMotionProperties->RestoreState(inStream); + + JPH_IF_ENABLE_ASSERTS(mMotionProperties->mCachedMotionType = mMotionType); + } + + // Initialize bounding box + CalculateWorldSpaceBoundsInternal(); +} + +BodyCreationSettings Body::GetBodyCreationSettings() const +{ + JPH_ASSERT(IsRigidBody()); + + BodyCreationSettings result; + + result.mPosition = GetPosition(); + result.mRotation = GetRotation(); + result.mLinearVelocity = mMotionProperties != nullptr? mMotionProperties->GetLinearVelocity() : Vec3::sZero(); + result.mAngularVelocity = mMotionProperties != nullptr? mMotionProperties->GetAngularVelocity() : Vec3::sZero(); + result.mObjectLayer = GetObjectLayer(); + result.mUserData = mUserData; + result.mCollisionGroup = GetCollisionGroup(); + result.mMotionType = GetMotionType(); + result.mAllowedDOFs = mMotionProperties != nullptr? mMotionProperties->GetAllowedDOFs() : EAllowedDOFs::All; + result.mAllowDynamicOrKinematic = mMotionProperties != nullptr; + result.mIsSensor = IsSensor(); + result.mCollideKinematicVsNonDynamic = GetCollideKinematicVsNonDynamic(); + result.mUseManifoldReduction = GetUseManifoldReduction(); + result.mApplyGyroscopicForce = GetApplyGyroscopicForce(); + result.mMotionQuality = mMotionProperties != nullptr? mMotionProperties->GetMotionQuality() : EMotionQuality::Discrete; + result.mEnhancedInternalEdgeRemoval = GetEnhancedInternalEdgeRemoval(); + result.mAllowSleeping = mMotionProperties != nullptr? GetAllowSleeping() : true; + result.mFriction = GetFriction(); + result.mRestitution = GetRestitution(); + result.mLinearDamping = mMotionProperties != nullptr? mMotionProperties->GetLinearDamping() : 0.0f; + result.mAngularDamping = mMotionProperties != nullptr? mMotionProperties->GetAngularDamping() : 0.0f; + result.mMaxLinearVelocity = mMotionProperties != nullptr? mMotionProperties->GetMaxLinearVelocity() : 0.0f; + result.mMaxAngularVelocity = mMotionProperties != nullptr? mMotionProperties->GetMaxAngularVelocity() : 0.0f; + result.mGravityFactor = mMotionProperties != nullptr? mMotionProperties->GetGravityFactor() : 1.0f; + result.mNumVelocityStepsOverride = mMotionProperties != nullptr? mMotionProperties->GetNumVelocityStepsOverride() : 0; + result.mNumPositionStepsOverride = mMotionProperties != nullptr? mMotionProperties->GetNumPositionStepsOverride() : 0; + result.mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided; + + // Invert inertia and mass + if (mMotionProperties != nullptr) + { + float inv_mass = mMotionProperties->GetInverseMassUnchecked(); + Mat44 inv_inertia = mMotionProperties->GetLocalSpaceInverseInertiaUnchecked(); + + // Get mass + result.mMassPropertiesOverride.mMass = inv_mass != 0.0f? 1.0f / inv_mass : FLT_MAX; + + // Get inertia + Mat44 inertia; + if (inertia.SetInversed3x3(inv_inertia)) + { + // Inertia was invertible, we can use it + result.mMassPropertiesOverride.mInertia = inertia; + } + else + { + // Prevent division by zero + Vec3 diagonal = Vec3::sMax(inv_inertia.GetDiagonal3(), Vec3::sReplicate(FLT_MIN)); + result.mMassPropertiesOverride.mInertia = Mat44::sScale(diagonal.Reciprocal()); + } + } + else + { + result.mMassPropertiesOverride.mMass = FLT_MAX; + result.mMassPropertiesOverride.mInertia = Mat44::sScale(Vec3::sReplicate(FLT_MAX)); + } + + result.SetShape(GetShape()); + + return result; +} + +SoftBodyCreationSettings Body::GetSoftBodyCreationSettings() const +{ + JPH_ASSERT(IsSoftBody()); + + SoftBodyCreationSettings result; + + result.mPosition = GetPosition(); + result.mRotation = GetRotation(); + result.mUserData = mUserData; + result.mObjectLayer = GetObjectLayer(); + result.mCollisionGroup = GetCollisionGroup(); + result.mFriction = GetFriction(); + result.mRestitution = GetRestitution(); + const SoftBodyMotionProperties *mp = static_cast(mMotionProperties); + result.mNumIterations = mp->GetNumIterations(); + result.mLinearDamping = mp->GetLinearDamping(); + result.mMaxLinearVelocity = mp->GetMaxLinearVelocity(); + result.mGravityFactor = mp->GetGravityFactor(); + result.mPressure = mp->GetPressure(); + result.mUpdatePosition = mp->GetUpdatePosition(); + result.mSettings = mp->GetSettings(); + + return result; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/Body.h b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.h new file mode 100644 index 000000000000..63034f909fe4 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.h @@ -0,0 +1,429 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class StateRecorder; +class BodyCreationSettings; +class SoftBodyCreationSettings; + +/// A rigid body that can be simulated using the physics system +/// +/// Note that internally all properties (position, velocity etc.) are tracked relative to the center of mass of the object to simplify the simulation of the object. +/// +/// The offset between the position of the body and the center of mass position of the body is GetShape()->GetCenterOfMass(). +/// The functions that get/set the position of the body all indicate if they are relative to the center of mass or to the original position in which the shape was created. +/// +/// The linear velocity is also velocity of the center of mass, to correct for this: \f$VelocityCOM = Velocity - AngularVelocity \times ShapeCOM\f$. +class alignas(JPH_RVECTOR_ALIGNMENT) JPH_EXPORT_GCC_BUG_WORKAROUND Body : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Get the id of this body + inline const BodyID & GetID() const { return mID; } + + /// Get the type of body (rigid or soft) + inline EBodyType GetBodyType() const { return mBodyType; } + + /// Check if this body is a rigid body + inline bool IsRigidBody() const { return mBodyType == EBodyType::RigidBody; } + + /// Check if this body is a soft body + inline bool IsSoftBody() const { return mBodyType == EBodyType::SoftBody; } + + // See comment at GetIndexInActiveBodiesInternal for reasoning why TSAN is disabled here + JPH_TSAN_NO_SANITIZE + /// If this body is currently actively simulating (true) or sleeping (false) + inline bool IsActive() const { return mMotionProperties != nullptr && mMotionProperties->mIndexInActiveBodies != cInactiveIndex; } + + /// Check if this body is static (not movable) + inline bool IsStatic() const { return mMotionType == EMotionType::Static; } + + /// Check if this body is kinematic (keyframed), which means that it will move according to its current velocity, but forces don't affect it + inline bool IsKinematic() const { return mMotionType == EMotionType::Kinematic; } + + /// Check if this body is dynamic, which means that it moves and forces can act on it + inline bool IsDynamic() const { return mMotionType == EMotionType::Dynamic; } + + /// Check if a body could be made kinematic or dynamic (if it was created dynamic or with mAllowDynamicOrKinematic set to true) + inline bool CanBeKinematicOrDynamic() const { return mMotionProperties != nullptr; } + + /// Change the body to a sensor. A sensor will receive collision callbacks, but will not cause any collision responses and can be used as a trigger volume. + /// The cheapest sensor (in terms of CPU usage) is a sensor with motion type Static (they can be moved around using BodyInterface::SetPosition/SetPositionAndRotation). + /// These sensors will only detect collisions with active Dynamic or Kinematic bodies. As soon as a body go to sleep, the contact point with the sensor will be lost. + /// If you make a sensor Dynamic or Kinematic and activate them, the sensor will be able to detect collisions with sleeping bodies too. An active sensor will never go to sleep automatically. + /// When you make a Dynamic or Kinematic sensor, make sure it is in an ObjectLayer that does not collide with Static bodies or other sensors to avoid extra overhead in the broad phase. + inline void SetIsSensor(bool inIsSensor) { JPH_ASSERT(IsRigidBody()); if (inIsSensor) mFlags.fetch_or(uint8(EFlags::IsSensor), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::IsSensor)), memory_order_relaxed); } + + /// Check if this body is a sensor. + inline bool IsSensor() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::IsSensor)) != 0; } + + /// If kinematic objects can generate contact points against other kinematic or static objects. + /// Note that turning this on can be CPU intensive as much more collision detection work will be done without any effect on the simulation (kinematic objects are not affected by other kinematic/static objects). + /// This can be used to make sensors detect static objects. Note that the sensor must be kinematic and active for it to detect static objects. + inline void SetCollideKinematicVsNonDynamic(bool inCollide) { JPH_ASSERT(IsRigidBody()); if (inCollide) mFlags.fetch_or(uint8(EFlags::CollideKinematicVsNonDynamic), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::CollideKinematicVsNonDynamic)), memory_order_relaxed); } + + /// Check if kinematic objects can generate contact points against other kinematic or static objects. + inline bool GetCollideKinematicVsNonDynamic() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::CollideKinematicVsNonDynamic)) != 0; } + + /// If PhysicsSettings::mUseManifoldReduction is true, this allows turning off manifold reduction for this specific body. + /// Manifold reduction by default will combine contacts with similar normals that come from different SubShapeIDs (e.g. different triangles in a mesh shape or different compound shapes). + /// If the application requires tracking exactly which SubShapeIDs are in contact, you can turn off manifold reduction. Note that this comes at a performance cost. + /// Consider using BodyInterface::SetUseManifoldReduction if the body could already be in contact with other bodies to ensure that the contact cache is invalidated and you get the correct contact callbacks. + inline void SetUseManifoldReduction(bool inUseReduction) { JPH_ASSERT(IsRigidBody()); if (inUseReduction) mFlags.fetch_or(uint8(EFlags::UseManifoldReduction), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::UseManifoldReduction)), memory_order_relaxed); } + + /// Check if this body can use manifold reduction. + inline bool GetUseManifoldReduction() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::UseManifoldReduction)) != 0; } + + /// Checks if the combination of this body and inBody2 should use manifold reduction + inline bool GetUseManifoldReductionWithBody(const Body &inBody2) const { return ((mFlags.load(memory_order_relaxed) & inBody2.mFlags.load(memory_order_relaxed)) & uint8(EFlags::UseManifoldReduction)) != 0; } + + /// Set to indicate that the gyroscopic force should be applied to this body (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem) + inline void SetApplyGyroscopicForce(bool inApply) { JPH_ASSERT(IsRigidBody()); if (inApply) mFlags.fetch_or(uint8(EFlags::ApplyGyroscopicForce), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::ApplyGyroscopicForce)), memory_order_relaxed); } + + /// Check if the gyroscopic force is being applied for this body + inline bool GetApplyGyroscopicForce() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::ApplyGyroscopicForce)) != 0; } + + /// Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges. + inline void SetEnhancedInternalEdgeRemoval(bool inApply) { JPH_ASSERT(IsRigidBody()); if (inApply) mFlags.fetch_or(uint8(EFlags::EnhancedInternalEdgeRemoval), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::EnhancedInternalEdgeRemoval)), memory_order_relaxed); } + + /// Check if enhanced internal edge removal is turned on + inline bool GetEnhancedInternalEdgeRemoval() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::EnhancedInternalEdgeRemoval)) != 0; } + + /// Checks if the combination of this body and inBody2 should use enhanced internal edge removal + inline bool GetEnhancedInternalEdgeRemovalWithBody(const Body &inBody2) const { return ((mFlags.load(memory_order_relaxed) | inBody2.mFlags.load(memory_order_relaxed)) & uint8(EFlags::EnhancedInternalEdgeRemoval)) != 0; } + + /// Get the bodies motion type. + inline EMotionType GetMotionType() const { return mMotionType; } + + /// Set the motion type of this body. Consider using BodyInterface::SetMotionType instead of this function if the body may be active or if it needs to be activated. + void SetMotionType(EMotionType inMotionType); + + /// Get broadphase layer, this determines in which broad phase sub-tree the object is placed + inline BroadPhaseLayer GetBroadPhaseLayer() const { return mBroadPhaseLayer; } + + /// Get object layer, this determines which other objects it collides with + inline ObjectLayer GetObjectLayer() const { return mObjectLayer; } + + /// Collision group and sub-group ID, determines which other objects it collides with + const CollisionGroup & GetCollisionGroup() const { return mCollisionGroup; } + CollisionGroup & GetCollisionGroup() { return mCollisionGroup; } + void SetCollisionGroup(const CollisionGroup &inGroup) { mCollisionGroup = inGroup; } + + /// If this body can go to sleep. Note that disabling sleeping on a sleeping object will not wake it up. + bool GetAllowSleeping() const { return mMotionProperties->mAllowSleeping; } + void SetAllowSleeping(bool inAllow); + + /// Resets the sleep timer. This does not wake up the body if it is sleeping, but allows resetting the system that detects when a body is sleeping. + inline void ResetSleepTimer(); + + /// Friction (dimensionless number, usually between 0 and 1, 0 = no friction, 1 = friction force equals force that presses the two bodies together). Note that bodies can have negative friction but the combined friction (see PhysicsSystem::SetCombineFriction) should never go below zero. + inline float GetFriction() const { return mFriction; } + void SetFriction(float inFriction) { mFriction = inFriction; } + + /// Restitution (dimensionless number, usually between 0 and 1, 0 = completely inelastic collision response, 1 = completely elastic collision response). Note that bodies can have negative restitution but the combined restitution (see PhysicsSystem::SetCombineRestitution) should never go below zero. + inline float GetRestitution() const { return mRestitution; } + void SetRestitution(float inRestitution) { mRestitution = inRestitution; } + + /// Get world space linear velocity of the center of mass (unit: m/s) + inline Vec3 GetLinearVelocity() const { return !IsStatic()? mMotionProperties->GetLinearVelocity() : Vec3::sZero(); } + + /// Set world space linear velocity of the center of mass (unit: m/s). + /// If you want the body to wake up when it is sleeping, use BodyInterface::SetLinearVelocity instead. + void SetLinearVelocity(Vec3Arg inLinearVelocity) { JPH_ASSERT(!IsStatic()); mMotionProperties->SetLinearVelocity(inLinearVelocity); } + + /// Set world space linear velocity of the center of mass, will make sure the value is clamped against the maximum linear velocity. + /// If you want the body to wake up when it is sleeping, use BodyInterface::SetLinearVelocity instead. + void SetLinearVelocityClamped(Vec3Arg inLinearVelocity) { JPH_ASSERT(!IsStatic()); mMotionProperties->SetLinearVelocityClamped(inLinearVelocity); } + + /// Get world space angular velocity of the center of mass (unit: rad/s) + inline Vec3 GetAngularVelocity() const { return !IsStatic()? mMotionProperties->GetAngularVelocity() : Vec3::sZero(); } + + /// Set world space angular velocity of the center of mass (unit: rad/s). + /// If you want the body to wake up when it is sleeping, use BodyInterface::SetAngularVelocity instead. + void SetAngularVelocity(Vec3Arg inAngularVelocity) { JPH_ASSERT(!IsStatic()); mMotionProperties->SetAngularVelocity(inAngularVelocity); } + + /// Set world space angular velocity of the center of mass, will make sure the value is clamped against the maximum angular velocity. + /// If you want the body to wake up when it is sleeping, use BodyInterface::SetAngularVelocity instead. + void SetAngularVelocityClamped(Vec3Arg inAngularVelocity) { JPH_ASSERT(!IsStatic()); mMotionProperties->SetAngularVelocityClamped(inAngularVelocity); } + + /// Velocity of point inPoint (in center of mass space, e.g. on the surface of the body) of the body (unit: m/s) + inline Vec3 GetPointVelocityCOM(Vec3Arg inPointRelativeToCOM) const { return !IsStatic()? mMotionProperties->GetPointVelocityCOM(inPointRelativeToCOM) : Vec3::sZero(); } + + /// Velocity of point inPoint (in world space, e.g. on the surface of the body) of the body (unit: m/s) + inline Vec3 GetPointVelocity(RVec3Arg inPoint) const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return GetPointVelocityCOM(Vec3(inPoint - mPosition)); } + + /// Add force (unit: N) at center of mass for the next time step, will be reset after the next call to PhysicsSystem::Update. + /// If you want the body to wake up when it is sleeping, use BodyInterface::AddForce instead. + inline void AddForce(Vec3Arg inForce) { JPH_ASSERT(IsDynamic()); (Vec3::sLoadFloat3Unsafe(mMotionProperties->mForce) + inForce).StoreFloat3(&mMotionProperties->mForce); } + + /// Add force (unit: N) at inPosition for the next time step, will be reset after the next call to PhysicsSystem::Update. + /// If you want the body to wake up when it is sleeping, use BodyInterface::AddForce instead. + inline void AddForce(Vec3Arg inForce, RVec3Arg inPosition); + + /// Add torque (unit: N m) for the next time step, will be reset after the next call to PhysicsSystem::Update. + /// If you want the body to wake up when it is sleeping, use BodyInterface::AddTorque instead. + inline void AddTorque(Vec3Arg inTorque) { JPH_ASSERT(IsDynamic()); (Vec3::sLoadFloat3Unsafe(mMotionProperties->mTorque) + inTorque).StoreFloat3(&mMotionProperties->mTorque); } + + // Get the total amount of force applied to the center of mass this time step (through AddForce calls). Note that it will reset to zero after PhysicsSystem::Update. + inline Vec3 GetAccumulatedForce() const { JPH_ASSERT(IsDynamic()); return mMotionProperties->GetAccumulatedForce(); } + + // Get the total amount of torque applied to the center of mass this time step (through AddForce/AddTorque calls). Note that it will reset to zero after PhysicsSystem::Update. + inline Vec3 GetAccumulatedTorque() const { JPH_ASSERT(IsDynamic()); return mMotionProperties->GetAccumulatedTorque(); } + + // Reset the total accumulated force, not that this will be done automatically after every time step. + JPH_INLINE void ResetForce() { JPH_ASSERT(IsDynamic()); return mMotionProperties->ResetForce(); } + + // Reset the total accumulated torque, not that this will be done automatically after every time step. + JPH_INLINE void ResetTorque() { JPH_ASSERT(IsDynamic()); return mMotionProperties->ResetTorque(); } + + // Reset the current velocity and accumulated force and torque. + JPH_INLINE void ResetMotion() { JPH_ASSERT(!IsStatic()); return mMotionProperties->ResetMotion(); } + + /// Get inverse inertia tensor in world space + inline Mat44 GetInverseInertia() const; + + /// Add impulse to center of mass (unit: kg m/s). + /// If you want the body to wake up when it is sleeping, use BodyInterface::AddImpulse instead. + inline void AddImpulse(Vec3Arg inImpulse); + + /// Add impulse to point in world space (unit: kg m/s). + /// If you want the body to wake up when it is sleeping, use BodyInterface::AddImpulse instead. + inline void AddImpulse(Vec3Arg inImpulse, RVec3Arg inPosition); + + /// Add angular impulse in world space (unit: N m s). + /// If you want the body to wake up when it is sleeping, use BodyInterface::AddAngularImpulse instead. + inline void AddAngularImpulse(Vec3Arg inAngularImpulse); + + /// Set velocity of body such that it will be positioned at inTargetPosition/Rotation in inDeltaTime seconds. + /// If you want the body to wake up when it is sleeping, use BodyInterface::MoveKinematic instead. + void MoveKinematic(RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime); + + /// Gets the properties needed to do buoyancy calculations + /// @param inSurfacePosition Position of the fluid surface in world space + /// @param inSurfaceNormal Normal of the fluid surface (should point up) + /// @param outTotalVolume On return this contains the total volume of the shape + /// @param outSubmergedVolume On return this contains the submerged volume of the shape + /// @param outRelativeCenterOfBuoyancy On return this contains the center of mass of the submerged volume relative to the center of mass of the body + void GetSubmergedVolume(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outRelativeCenterOfBuoyancy) const; + + /// Applies an impulse to the body that simulates fluid buoyancy and drag. + /// If you want the body to wake up when it is sleeping, use BodyInterface::ApplyBuoyancyImpulse instead. + /// @param inSurfacePosition Position of the fluid surface in world space + /// @param inSurfaceNormal Normal of the fluid surface (should point up) + /// @param inBuoyancy The buoyancy factor for the body. 1 = neutral body, < 1 sinks, > 1 floats. Note that we don't use the fluid density since it is harder to configure than a simple number between [0, 2] + /// @param inLinearDrag Linear drag factor that slows down the body when in the fluid (approx. 0.5) + /// @param inAngularDrag Angular drag factor that slows down rotation when the body is in the fluid (approx. 0.01) + /// @param inFluidVelocity The average velocity of the fluid (in m/s) in which the body resides + /// @param inGravity The gravity vector (pointing down) + /// @param inDeltaTime Delta time of the next simulation step (in s) + /// @return true if an impulse was applied, false if the body was not in the fluid + bool ApplyBuoyancyImpulse(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime); + + /// Applies an impulse to the body that simulates fluid buoyancy and drag. + /// If you want the body to wake up when it is sleeping, use BodyInterface::ApplyBuoyancyImpulse instead. + /// @param inTotalVolume Total volume of the shape of this body (m^3) + /// @param inSubmergedVolume Submerged volume of the shape of this body (m^3) + /// @param inRelativeCenterOfBuoyancy The center of mass of the submerged volume relative to the center of mass of the body + /// @param inBuoyancy The buoyancy factor for the body. 1 = neutral body, < 1 sinks, > 1 floats. Note that we don't use the fluid density since it is harder to configure than a simple number between [0, 2] + /// @param inLinearDrag Linear drag factor that slows down the body when in the fluid (approx. 0.5) + /// @param inAngularDrag Angular drag factor that slows down rotation when the body is in the fluid (approx. 0.01) + /// @param inFluidVelocity The average velocity of the fluid (in m/s) in which the body resides + /// @param inGravity The gravity vector (pointing down) + /// @param inDeltaTime Delta time of the next simulation step (in s) + /// @return true if an impulse was applied, false if the body was not in the fluid + bool ApplyBuoyancyImpulse(float inTotalVolume, float inSubmergedVolume, Vec3Arg inRelativeCenterOfBuoyancy, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime); + + /// Check if this body has been added to the physics system + inline bool IsInBroadPhase() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::IsInBroadPhase)) != 0; } + + /// Check if this body has been changed in such a way that the collision cache should be considered invalid for any body interacting with this body + inline bool IsCollisionCacheInvalid() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::InvalidateContactCache)) != 0; } + + /// Get the shape of this body + inline const Shape * GetShape() const { return mShape; } + + /// World space position of the body + inline RVec3 GetPosition() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return mPosition - mRotation * mShape->GetCenterOfMass(); } + + /// World space rotation of the body + inline Quat GetRotation() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return mRotation; } + + /// Calculates the transform of this body + inline RMat44 GetWorldTransform() const; + + /// Gets the world space position of this body's center of mass + inline RVec3 GetCenterOfMassPosition() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return mPosition; } + + /// Calculates the transform for this body's center of mass + inline RMat44 GetCenterOfMassTransform() const; + + /// Calculates the inverse of the transform for this body's center of mass + inline RMat44 GetInverseCenterOfMassTransform() const; + + /// Get world space bounding box + inline const AABox & GetWorldSpaceBounds() const { return mBounds; } + + /// Access to the motion properties + const MotionProperties *GetMotionProperties() const { JPH_ASSERT(!IsStatic()); return mMotionProperties; } + MotionProperties * GetMotionProperties() { JPH_ASSERT(!IsStatic()); return mMotionProperties; } + + /// Access to the motion properties (version that does not check if the object is kinematic or dynamic) + const MotionProperties *GetMotionPropertiesUnchecked() const { return mMotionProperties; } + MotionProperties * GetMotionPropertiesUnchecked() { return mMotionProperties; } + + /// Access to the user data, can be used for anything by the application + uint64 GetUserData() const { return mUserData; } + void SetUserData(uint64 inUserData) { mUserData = inUserData; } + + /// Get surface normal of a particular sub shape and its world space surface position on this body + inline Vec3 GetWorldSpaceSurfaceNormal(const SubShapeID &inSubShapeID, RVec3Arg inPosition) const; + + /// Get the transformed shape of this body, which can be used to do collision detection outside of a body lock + inline TransformedShape GetTransformedShape() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return TransformedShape(mPosition, mRotation, mShape, mID); } + + /// Debug function to convert a body back to a body creation settings object to be able to save/recreate the body later + BodyCreationSettings GetBodyCreationSettings() const; + + /// Debug function to convert a soft body back to a soft body creation settings object to be able to save/recreate the body later + SoftBodyCreationSettings GetSoftBodyCreationSettings() const; + + /// A dummy body that can be used by constraints to attach a constraint to the world instead of another body + static Body sFixedToWorld; + + ///@name THESE FUNCTIONS ARE FOR INTERNAL USE ONLY AND SHOULD NOT BE CALLED BY THE APPLICATION + ///@{ + + /// Helper function for BroadPhase::FindCollidingPairs that returns true when two bodies can collide + /// It assumes that body 1 is dynamic and active and guarantees that it body 1 collides with body 2 that body 2 will not collide with body 1 in order to avoid finding duplicate collision pairs + static inline bool sFindCollidingPairsCanCollide(const Body &inBody1, const Body &inBody2); + + /// Update position using an Euler step (used during position integrate & constraint solving) + inline void AddPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime) { JPH_ASSERT(IsRigidBody()); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite)); mPosition += mMotionProperties->LockTranslation(inLinearVelocityTimesDeltaTime); JPH_ASSERT(!mPosition.IsNaN()); } + inline void SubPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime) { JPH_ASSERT(IsRigidBody()); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite)); mPosition -= mMotionProperties->LockTranslation(inLinearVelocityTimesDeltaTime); JPH_ASSERT(!mPosition.IsNaN()); } + + /// Update rotation using an Euler step (used during position integrate & constraint solving) + inline void AddRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime); + inline void SubRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime); + + /// Flag if body is in the broadphase (should only be called by the BroadPhase) + inline void SetInBroadPhaseInternal(bool inInBroadPhase) { if (inInBroadPhase) mFlags.fetch_or(uint8(EFlags::IsInBroadPhase), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::IsInBroadPhase)), memory_order_relaxed); } + + /// Invalidate the contact cache (should only be called by the BodyManager), will be reset the next simulation step. Returns true if the contact cache was still valid. + inline bool InvalidateContactCacheInternal() { return (mFlags.fetch_or(uint8(EFlags::InvalidateContactCache), memory_order_relaxed) & uint8(EFlags::InvalidateContactCache)) == 0; } + + /// Reset the collision cache invalid flag (should only be called by the BodyManager). + inline void ValidateContactCacheInternal() { JPH_IF_ENABLE_ASSERTS(uint8 old_val = ) mFlags.fetch_and(uint8(~uint8(EFlags::InvalidateContactCache)), memory_order_relaxed); JPH_ASSERT((old_val & uint8(EFlags::InvalidateContactCache)) != 0); } + + /// Updates world space bounding box (should only be called by the PhysicsSystem) + void CalculateWorldSpaceBoundsInternal(); + + /// Function to update body's position (should only be called by the BodyInterface since it also requires updating the broadphase) + void SetPositionAndRotationInternal(RVec3Arg inPosition, QuatArg inRotation, bool inResetSleepTimer = true); + + /// Updates the center of mass and optionally mass properties after shifting the center of mass or changes to the shape (should only be called by the BodyInterface since it also requires updating the broadphase) + /// @param inPreviousCenterOfMass Center of mass of the shape before the alterations + /// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated + void UpdateCenterOfMassInternal(Vec3Arg inPreviousCenterOfMass, bool inUpdateMassProperties); + + /// Function to update a body's shape (should only be called by the BodyInterface since it also requires updating the broadphase) + /// @param inShape The new shape for this body + /// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated + void SetShapeInternal(const Shape *inShape, bool inUpdateMassProperties); + + // TSAN detects a race between BodyManager::AddBodyToActiveBodies coming from PhysicsSystem::ProcessBodyPair and Body::GetIndexInActiveBodiesInternal coming from PhysicsSystem::ProcessBodyPair. + // When PhysicsSystem::ProcessBodyPair activates a body, it updates mIndexInActiveBodies and then updates BodyManager::mNumActiveBodies with release semantics. PhysicsSystem::ProcessBodyPair will + // then finish its loop of active bodies and at the end of the loop it will read BodyManager::mNumActiveBodies with acquire semantics to see if any bodies were activated during the loop. + // This means that changes to mIndexInActiveBodies must be visible to the thread, so TSANs report must be a false positive. We suppress the warning here. + JPH_TSAN_NO_SANITIZE + /// Access to the index in the BodyManager::mActiveBodies list + uint32 GetIndexInActiveBodiesInternal() const { return mMotionProperties != nullptr? mMotionProperties->mIndexInActiveBodies : cInactiveIndex; } + + /// Update eligibility for sleeping + ECanSleep UpdateSleepStateInternal(float inDeltaTime, float inMaxMovement, float inTimeBeforeSleep); + + /// Saving state for replay + void SaveState(StateRecorder &inStream) const; + + /// Restoring state for replay + void RestoreState(StateRecorder &inStream); + + ///@} + + static constexpr uint32 cInactiveIndex = MotionProperties::cInactiveIndex; ///< Constant indicating that body is not active + +private: + friend class BodyManager; + friend class BodyWithMotionProperties; + friend class SoftBodyWithMotionPropertiesAndShape; + + Body() = default; ///< Bodies must be created through BodyInterface::CreateBody + + explicit Body(bool); ///< Alternative constructor that initializes all members + + ~Body() { JPH_ASSERT(mMotionProperties == nullptr); } ///< Bodies must be destroyed through BodyInterface::DestroyBody + + inline void GetSleepTestPoints(RVec3 *outPoints) const; ///< Determine points to test for checking if body is sleeping: COM, COM + largest bounding box axis, COM + second largest bounding box axis + + enum class EFlags : uint8 + { + IsSensor = 1 << 0, ///< If this object is a sensor. A sensor will receive collision callbacks, but will not cause any collision responses and can be used as a trigger volume. + CollideKinematicVsNonDynamic = 1 << 1, ///< If kinematic objects can generate contact points against other kinematic or static objects. + IsInBroadPhase = 1 << 2, ///< Set this bit to indicate that the body is in the broadphase + InvalidateContactCache = 1 << 3, ///< Set this bit to indicate that all collision caches for this body are invalid, will be reset the next simulation step. + UseManifoldReduction = 1 << 4, ///< Set this bit to indicate that this body can use manifold reduction (if PhysicsSettings::mUseManifoldReduction is true) + ApplyGyroscopicForce = 1 << 5, ///< Set this bit to indicate that the gyroscopic force should be applied to this body (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem) + EnhancedInternalEdgeRemoval = 1 << 6, ///< Set this bit to indicate that enhanced internal edge removal should be used for this body (see BodyCreationSettings::mEnhancedInternalEdgeRemoval) + }; + + // 16 byte aligned + RVec3 mPosition; ///< World space position of center of mass + Quat mRotation; ///< World space rotation of center of mass + AABox mBounds; ///< World space bounding box of the body + + // 8 byte aligned + RefConst mShape; ///< Shape representing the volume of this body + MotionProperties * mMotionProperties = nullptr; ///< If this is a keyframed or dynamic object, this object holds all information about the movement + uint64 mUserData = 0; ///< User data, can be used for anything by the application + CollisionGroup mCollisionGroup; ///< The collision group this body belongs to (determines if two objects can collide) + + // 4 byte aligned + float mFriction; ///< Friction of the body (dimensionless number, usually between 0 and 1, 0 = no friction, 1 = friction force equals force that presses the two bodies together). Note that bodies can have negative friction but the combined friction (see PhysicsSystem::SetCombineFriction) should never go below zero. + float mRestitution; ///< Restitution of body (dimensionless number, usually between 0 and 1, 0 = completely inelastic collision response, 1 = completely elastic collision response). Note that bodies can have negative restitution but the combined restitution (see PhysicsSystem::SetCombineRestitution) should never go below zero. + BodyID mID; ///< ID of the body (index in the bodies array) + + // 2 or 4 bytes aligned + ObjectLayer mObjectLayer; ///< The collision layer this body belongs to (determines if two objects can collide) + + // 1 byte aligned + EBodyType mBodyType; ///< Type of body (rigid or soft) + BroadPhaseLayer mBroadPhaseLayer; ///< The broad phase layer this body belongs to + EMotionType mMotionType; ///< Type of motion (static, dynamic or kinematic) + atomic mFlags = 0; ///< See EFlags for possible flags + + // 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"); + +JPH_NAMESPACE_END + +#include "Body.inl" diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/Body.inl b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.inl new file mode 100644 index 000000000000..dbbd64dd7f94 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.inl @@ -0,0 +1,197 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +RMat44 Body::GetWorldTransform() const +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); + + return RMat44::sRotationTranslation(mRotation, mPosition).PreTranslated(-mShape->GetCenterOfMass()); +} + +RMat44 Body::GetCenterOfMassTransform() const +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); + + return RMat44::sRotationTranslation(mRotation, mPosition); +} + +RMat44 Body::GetInverseCenterOfMassTransform() const +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); + + return RMat44::sInverseRotationTranslation(mRotation, mPosition); +} + +inline bool Body::sFindCollidingPairsCanCollide(const Body &inBody1, const Body &inBody2) +{ + // First body should never be a soft body + JPH_ASSERT(!inBody1.IsSoftBody()); + + // One of these conditions must be true + // - We always allow detecting collisions between kinematic and non-dynamic bodies + // - One of the bodies must be dynamic to collide + // - A kinematic object can collide with a sensor + if (!inBody1.GetCollideKinematicVsNonDynamic() + && !inBody2.GetCollideKinematicVsNonDynamic() + && (!inBody1.IsDynamic() && !inBody2.IsDynamic()) + && !(inBody1.IsKinematic() && inBody2.IsSensor()) + && !(inBody2.IsKinematic() && inBody1.IsSensor())) + return false; + + // Check that body 1 is active + uint32 body1_index_in_active_bodies = inBody1.GetIndexInActiveBodiesInternal(); + JPH_ASSERT(!inBody1.IsStatic() && body1_index_in_active_bodies != Body::cInactiveIndex, "This function assumes that Body 1 is active"); + + // If the pair A, B collides we need to ensure that the pair B, A does not collide or else we will handle the collision twice. + // If A is the same body as B we don't want to collide (1) + // If A is dynamic / kinematic and B is static we should collide (2) + // If A is dynamic / kinematic and B is dynamic / kinematic we should only collide if + // - A is active and B is not active (3) + // - A is active and B will become active during this simulation step (4) + // - A is active and B is active, we require a condition that makes A, B collide and B, A not (5) + // + // In order to implement this we use the index in the active body list and make use of the fact that + // a body not in the active list has Body.Index = 0xffffffff which is the highest possible value for an uint32. + // + // Because we know that A is active we know that A.Index != 0xffffffff: + // (1) Because A.Index != 0xffffffff, if A.Index = B.Index then A = B, so to collide A.Index != B.Index + // (2) A.Index != 0xffffffff, B.Index = 0xffffffff (because it's static and cannot be in the active list), so to collide A.Index != B.Index + // (3) A.Index != 0xffffffff, B.Index = 0xffffffff (because it's not yet active), so to collide A.Index != B.Index + // (4) A.Index != 0xffffffff, B.Index = 0xffffffff currently. But it can activate during the Broad/NarrowPhase step at which point it + // will be added to the end of the active list which will make B.Index > A.Index (this holds only true when we don't deactivate + // bodies during the Broad/NarrowPhase step), so to collide A.Index < B.Index. + // (5) As tie breaker we can use the same condition A.Index < B.Index to collide, this means that if A, B collides then B, A won't + static_assert(Body::cInactiveIndex == 0xffffffff, "The algorithm below uses this value"); + if (!inBody2.IsSoftBody() && body1_index_in_active_bodies >= inBody2.GetIndexInActiveBodiesInternal()) + return false; + JPH_ASSERT(inBody1.GetID() != inBody2.GetID(), "Read the comment above, A and B are the same body which should not be possible!"); + + // Check collision group filter + if (!inBody1.GetCollisionGroup().CanCollide(inBody2.GetCollisionGroup())) + return false; + + return true; +} + +void Body::AddRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime) +{ + JPH_ASSERT(IsRigidBody()); + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite)); + + // This used to use the equation: d/dt R(t) = 1/2 * w(t) * R(t) so that R(t + dt) = R(t) + 1/2 * w(t) * R(t) * dt + // See: Appendix B of An Introduction to Physically Based Modeling: Rigid Body Simulation II-Nonpenetration Constraints + // URL: https://www.cs.cmu.edu/~baraff/sigcourse/notesd2.pdf + // But this is a first order approximation and does not work well for kinematic ragdolls that are driven to a new + // pose if the poses differ enough. So now we split w(t) * dt into an axis and angle part and create a quaternion with it. + // Note that the resulting quaternion is normalized since otherwise numerical drift will eventually make the rotation non-normalized. + float len = inAngularVelocityTimesDeltaTime.Length(); + if (len > 1.0e-6f) + { + mRotation = (Quat::sRotation(inAngularVelocityTimesDeltaTime / len, len) * mRotation).Normalized(); + JPH_ASSERT(!mRotation.IsNaN()); + } +} + +void Body::SubRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime) +{ + JPH_ASSERT(IsRigidBody()); + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite)); + + // See comment at Body::AddRotationStep + float len = inAngularVelocityTimesDeltaTime.Length(); + if (len > 1.0e-6f) + { + mRotation = (Quat::sRotation(inAngularVelocityTimesDeltaTime / len, -len) * mRotation).Normalized(); + JPH_ASSERT(!mRotation.IsNaN()); + } +} + +Vec3 Body::GetWorldSpaceSurfaceNormal(const SubShapeID &inSubShapeID, RVec3Arg inPosition) const +{ + RMat44 inv_com = GetInverseCenterOfMassTransform(); + return inv_com.Multiply3x3Transposed(mShape->GetSurfaceNormal(inSubShapeID, Vec3(inv_com * inPosition))).Normalized(); +} + +Mat44 Body::GetInverseInertia() const +{ + JPH_ASSERT(IsDynamic()); + + return GetMotionProperties()->GetInverseInertiaForRotation(Mat44::sRotation(mRotation)); +} + +void Body::AddForce(Vec3Arg inForce, RVec3Arg inPosition) +{ + AddForce(inForce); + AddTorque(Vec3(inPosition - mPosition).Cross(inForce)); +} + +void Body::AddImpulse(Vec3Arg inImpulse) +{ + JPH_ASSERT(IsDynamic()); + + SetLinearVelocityClamped(mMotionProperties->GetLinearVelocity() + inImpulse * mMotionProperties->GetInverseMass()); +} + +void Body::AddImpulse(Vec3Arg inImpulse, RVec3Arg inPosition) +{ + JPH_ASSERT(IsDynamic()); + + SetLinearVelocityClamped(mMotionProperties->GetLinearVelocity() + inImpulse * mMotionProperties->GetInverseMass()); + + SetAngularVelocityClamped(mMotionProperties->GetAngularVelocity() + mMotionProperties->MultiplyWorldSpaceInverseInertiaByVector(mRotation, Vec3(inPosition - mPosition).Cross(inImpulse))); +} + +void Body::AddAngularImpulse(Vec3Arg inAngularImpulse) +{ + JPH_ASSERT(IsDynamic()); + + SetAngularVelocityClamped(mMotionProperties->GetAngularVelocity() + mMotionProperties->MultiplyWorldSpaceInverseInertiaByVector(mRotation, inAngularImpulse)); +} + +void Body::GetSleepTestPoints(RVec3 *outPoints) const +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); + + // Center of mass is the first position + outPoints[0] = mPosition; + + // The second and third position are on the largest axis of the bounding box + Vec3 extent = mShape->GetLocalBounds().GetExtent(); + int lowest_component = extent.GetLowestComponentIndex(); + Mat44 rotation = Mat44::sRotation(mRotation); + switch (lowest_component) + { + case 0: + outPoints[1] = mPosition + extent.GetY() * rotation.GetColumn3(1); + outPoints[2] = mPosition + extent.GetZ() * rotation.GetColumn3(2); + break; + + case 1: + outPoints[1] = mPosition + extent.GetX() * rotation.GetColumn3(0); + outPoints[2] = mPosition + extent.GetZ() * rotation.GetColumn3(2); + break; + + case 2: + outPoints[1] = mPosition + extent.GetX() * rotation.GetColumn3(0); + outPoints[2] = mPosition + extent.GetY() * rotation.GetColumn3(1); + break; + + default: + JPH_ASSERT(false); + break; + } +} + +void Body::ResetSleepTimer() +{ + RVec3 points[3]; + GetSleepTestPoints(points); + mMotionProperties->ResetSleepTestSpheres(points); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyAccess.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyAccess.h new file mode 100644 index 000000000000..a7fd6a4ac3b9 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyAccess.h @@ -0,0 +1,68 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifdef JPH_ENABLE_ASSERTS + +JPH_NAMESPACE_BEGIN + +class JPH_EXPORT BodyAccess +{ +public: + /// Access rules, used to detect race conditions during simulation + enum class EAccess : uint8 + { + None = 0, + Read = 1, + ReadWrite = 3, + }; + + /// Grant a scope specific access rights on the current thread + class Grant + { + public: + inline Grant(EAccess inVelocity, EAccess inPosition) + { + EAccess &velocity = sVelocityAccess(); + EAccess &position = sPositionAccess(); + + JPH_ASSERT(velocity == EAccess::ReadWrite); + JPH_ASSERT(position == EAccess::ReadWrite); + + velocity = inVelocity; + position = inPosition; + } + + inline ~Grant() + { + sVelocityAccess() = EAccess::ReadWrite; + sPositionAccess() = EAccess::ReadWrite; + } + }; + + /// Check if we have permission + static inline bool sCheckRights(EAccess inRights, EAccess inDesiredRights) + { + return (uint8(inRights) & uint8(inDesiredRights)) == uint8(inDesiredRights); + } + + /// Access to read/write velocities + static inline EAccess & sVelocityAccess() + { + static thread_local EAccess sAccess = BodyAccess::EAccess::ReadWrite; + return sAccess; + } + + /// Access to read/write positions + static inline EAccess & sPositionAccess() + { + static thread_local EAccess sAccess = BodyAccess::EAccess::ReadWrite; + return sAccess; + } +}; + +JPH_NAMESPACE_END + +#endif // JPH_ENABLE_ASSERTS diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyActivationListener.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyActivationListener.h new file mode 100644 index 000000000000..2c8808a15058 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyActivationListener.h @@ -0,0 +1,28 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +class BodyID; + +/// A listener class that receives events when a body activates or deactivates. +/// It can be registered with the BodyManager (or PhysicsSystem). +class BodyActivationListener +{ +public: + /// Ensure virtual destructor + virtual ~BodyActivationListener() = default; + + /// Called whenever a body activates, note this can be called from any thread so make sure your code is thread safe. + /// At the time of the callback the body inBodyID will be locked and no bodies can be written/activated/deactivated from the callback. + virtual void OnBodyActivated(const BodyID &inBodyID, uint64 inBodyUserData) = 0; + + /// Called whenever a body deactivates, note this can be called from any thread so make sure your code is thread safe. + /// At the time of the callback the body inBodyID will be locked and no bodies can be written/activated/deactivated from the callback. + virtual void OnBodyDeactivated(const BodyID &inBodyID, uint64 inBodyUserData) = 0; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.cpp b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.cpp new file mode 100644 index 000000000000..9b6d3f92c41a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.cpp @@ -0,0 +1,234 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(BodyCreationSettings) +{ + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mPosition) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mRotation) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mLinearVelocity) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAngularVelocity) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mUserData) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mShape) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mCollisionGroup) + JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mObjectLayer) + JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mMotionType) + JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mAllowedDOFs) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAllowDynamicOrKinematic) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mIsSensor) + JPH_ADD_ATTRIBUTE_WITH_ALIAS(BodyCreationSettings, mCollideKinematicVsNonDynamic, "mSensorDetectsStatic") // This is the old name to keep backwards compatibility + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mUseManifoldReduction) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mApplyGyroscopicForce) + JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mMotionQuality) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mEnhancedInternalEdgeRemoval) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAllowSleeping) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mFriction) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mRestitution) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mLinearDamping) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAngularDamping) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mMaxLinearVelocity) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mMaxAngularVelocity) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mGravityFactor) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mNumVelocityStepsOverride) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mNumPositionStepsOverride) + JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mOverrideMassProperties) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mInertiaMultiplier) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mMassPropertiesOverride) +} + +void BodyCreationSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mPosition); + inStream.Write(mRotation); + inStream.Write(mLinearVelocity); + inStream.Write(mAngularVelocity); + mCollisionGroup.SaveBinaryState(inStream); + inStream.Write(mObjectLayer); + inStream.Write(mMotionType); + inStream.Write(mAllowedDOFs); + inStream.Write(mAllowDynamicOrKinematic); + inStream.Write(mIsSensor); + inStream.Write(mCollideKinematicVsNonDynamic); + inStream.Write(mUseManifoldReduction); + inStream.Write(mApplyGyroscopicForce); + inStream.Write(mMotionQuality); + inStream.Write(mEnhancedInternalEdgeRemoval); + inStream.Write(mAllowSleeping); + inStream.Write(mFriction); + inStream.Write(mRestitution); + inStream.Write(mLinearDamping); + inStream.Write(mAngularDamping); + inStream.Write(mMaxLinearVelocity); + inStream.Write(mMaxAngularVelocity); + inStream.Write(mGravityFactor); + inStream.Write(mNumVelocityStepsOverride); + inStream.Write(mNumPositionStepsOverride); + inStream.Write(mOverrideMassProperties); + inStream.Write(mInertiaMultiplier); + mMassPropertiesOverride.SaveBinaryState(inStream); +} + +void BodyCreationSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mPosition); + inStream.Read(mRotation); + inStream.Read(mLinearVelocity); + inStream.Read(mAngularVelocity); + mCollisionGroup.RestoreBinaryState(inStream); + inStream.Read(mObjectLayer); + inStream.Read(mMotionType); + inStream.Read(mAllowedDOFs); + inStream.Read(mAllowDynamicOrKinematic); + inStream.Read(mIsSensor); + inStream.Read(mCollideKinematicVsNonDynamic); + inStream.Read(mUseManifoldReduction); + inStream.Read(mApplyGyroscopicForce); + inStream.Read(mMotionQuality); + inStream.Read(mEnhancedInternalEdgeRemoval); + inStream.Read(mAllowSleeping); + inStream.Read(mFriction); + inStream.Read(mRestitution); + inStream.Read(mLinearDamping); + inStream.Read(mAngularDamping); + inStream.Read(mMaxLinearVelocity); + inStream.Read(mMaxAngularVelocity); + inStream.Read(mGravityFactor); + inStream.Read(mNumVelocityStepsOverride); + inStream.Read(mNumPositionStepsOverride); + inStream.Read(mOverrideMassProperties); + inStream.Read(mInertiaMultiplier); + mMassPropertiesOverride.RestoreBinaryState(inStream); +} + +Shape::ShapeResult BodyCreationSettings::ConvertShapeSettings() +{ + // If we already have a shape, return it + if (mShapePtr != nullptr) + { + mShape = nullptr; + + Shape::ShapeResult result; + result.Set(const_cast(mShapePtr.GetPtr())); + return result; + } + + // Check if we have shape settings + if (mShape == nullptr) + { + Shape::ShapeResult result; + result.SetError("No shape present!"); + return result; + } + + // Create the shape + Shape::ShapeResult result = mShape->Create(); + if (result.IsValid()) + mShapePtr = result.Get(); + mShape = nullptr; + return result; +} + +const Shape *BodyCreationSettings::GetShape() const +{ + // If we already have a shape, return it + if (mShapePtr != nullptr) + return mShapePtr; + + // Check if we have shape settings + if (mShape == nullptr) + return nullptr; + + // Create the shape + Shape::ShapeResult result = mShape->Create(); + if (result.IsValid()) + return result.Get(); + + Trace("Error: %s", result.GetError().c_str()); + JPH_ASSERT(false, "An error occurred during shape creation. Use ConvertShapeSettings() to convert the shape and get the error!"); + return nullptr; +} + +MassProperties BodyCreationSettings::GetMassProperties() const +{ + // Calculate mass properties + MassProperties mass_properties; + switch (mOverrideMassProperties) + { + case EOverrideMassProperties::CalculateMassAndInertia: + mass_properties = GetShape()->GetMassProperties(); + mass_properties.mInertia *= mInertiaMultiplier; + mass_properties.mInertia(3, 3) = 1.0f; + break; + case EOverrideMassProperties::CalculateInertia: + mass_properties = GetShape()->GetMassProperties(); + mass_properties.ScaleToMass(mMassPropertiesOverride.mMass); + mass_properties.mInertia *= mInertiaMultiplier; + mass_properties.mInertia(3, 3) = 1.0f; + break; + case EOverrideMassProperties::MassAndInertiaProvided: + mass_properties = mMassPropertiesOverride; + break; + } + return mass_properties; +} + +void BodyCreationSettings::SaveWithChildren(StreamOut &inStream, ShapeToIDMap *ioShapeMap, MaterialToIDMap *ioMaterialMap, GroupFilterToIDMap *ioGroupFilterMap) const +{ + // Save creation settings + SaveBinaryState(inStream); + + // Save shape + if (ioShapeMap != nullptr && ioMaterialMap != nullptr) + GetShape()->SaveWithChildren(inStream, *ioShapeMap, *ioMaterialMap); + else + inStream.Write(~uint32(0)); + + // Save group filter + StreamUtils::SaveObjectReference(inStream, mCollisionGroup.GetGroupFilter(), ioGroupFilterMap); +} + +BodyCreationSettings::BCSResult BodyCreationSettings::sRestoreWithChildren(StreamIn &inStream, IDToShapeMap &ioShapeMap, IDToMaterialMap &ioMaterialMap, IDToGroupFilterMap &ioGroupFilterMap) +{ + BCSResult result; + + // Read creation settings + BodyCreationSettings settings; + settings.RestoreBinaryState(inStream); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Error reading body creation settings"); + return result; + } + + // Read shape + Shape::ShapeResult shape_result = Shape::sRestoreWithChildren(inStream, ioShapeMap, ioMaterialMap); + if (shape_result.HasError()) + { + result.SetError(shape_result.GetError()); + return result; + } + settings.SetShape(shape_result.Get()); + + // Read group filter + Result gfresult = StreamUtils::RestoreObjectReference(inStream, ioGroupFilterMap); + if (gfresult.HasError()) + { + result.SetError(gfresult.GetError()); + return result; + } + settings.mCollisionGroup.SetGroupFilter(gfresult.Get()); + + result.Set(settings); + return result; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.h new file mode 100644 index 000000000000..8949b2ec3e0c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.h @@ -0,0 +1,124 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; + +/// Enum used in BodyCreationSettings to indicate how mass and inertia should be calculated +enum class EOverrideMassProperties : uint8 +{ + CalculateMassAndInertia, ///< Tells the system to calculate the mass and inertia based on density + CalculateInertia, ///< Tells the system to take the mass from mMassPropertiesOverride and to calculate the inertia based on density of the shapes and to scale it to the provided mass + MassAndInertiaProvided ///< Tells the system to take the mass and inertia from mMassPropertiesOverride +}; + +/// Settings for constructing a rigid body +class JPH_EXPORT BodyCreationSettings +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, BodyCreationSettings) + +public: + /// Constructor + BodyCreationSettings() = default; + BodyCreationSettings(const ShapeSettings *inShape, RVec3Arg inPosition, QuatArg inRotation, EMotionType inMotionType, ObjectLayer inObjectLayer) : mPosition(inPosition), mRotation(inRotation), mObjectLayer(inObjectLayer), mMotionType(inMotionType), mShape(inShape) { } + BodyCreationSettings(const Shape *inShape, RVec3Arg inPosition, QuatArg inRotation, EMotionType inMotionType, ObjectLayer inObjectLayer) : mPosition(inPosition), mRotation(inRotation), mObjectLayer(inObjectLayer), mMotionType(inMotionType), mShapePtr(inShape) { } + + /// Access to the shape settings object. This contains serializable (non-runtime optimized) information about the Shape. + const ShapeSettings * GetShapeSettings() const { return mShape; } + void SetShapeSettings(const ShapeSettings *inShape) { mShape = inShape; mShapePtr = nullptr; } + + /// Convert ShapeSettings object into a Shape object. This will free the ShapeSettings object and make the object ready for runtime. Serialization is no longer possible after this. + Shape::ShapeResult ConvertShapeSettings(); + + /// Access to the run-time shape object. Will convert from ShapeSettings object if needed. + const Shape * GetShape() const; + void SetShape(const Shape *inShape) { mShapePtr = inShape; mShape = nullptr; } + + /// Check if the mass properties of this body will be calculated (only relevant for kinematic or dynamic objects that need a MotionProperties object) + bool HasMassProperties() const { return mAllowDynamicOrKinematic || mMotionType != EMotionType::Static; } + + /// Calculate (or return when overridden) the mass and inertia for this body + MassProperties GetMassProperties() const; + + /// Saves the state of this object in binary form to inStream. Doesn't store the shape nor the group filter. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restore the state of this object from inStream. Doesn't restore the shape nor the group filter. + void RestoreBinaryState(StreamIn &inStream); + + using GroupFilterToIDMap = StreamUtils::ObjectToIDMap; + using IDToGroupFilterMap = StreamUtils::IDToObjectMap; + using ShapeToIDMap = Shape::ShapeToIDMap; + using IDToShapeMap = Shape::IDToShapeMap; + using MaterialToIDMap = StreamUtils::ObjectToIDMap; + using IDToMaterialMap = StreamUtils::IDToObjectMap; + + /// Save body creation settings, its shape, materials and group filter. Pass in an empty map in ioShapeMap / ioMaterialMap / ioGroupFilterMap or reuse the same map while saving multiple shapes to the same stream in order to avoid writing duplicates. + /// Pass nullptr to ioShapeMap and ioMaterial map to skip saving shapes + /// Pass nullptr to ioGroupFilterMap to skip saving group filters + void SaveWithChildren(StreamOut &inStream, ShapeToIDMap *ioShapeMap, MaterialToIDMap *ioMaterialMap, GroupFilterToIDMap *ioGroupFilterMap) const; + + using BCSResult = Result; + + /// Restore body creation settings, its shape, materials and group filter. Pass in an empty map in ioShapeMap / ioMaterialMap / ioGroupFilterMap or reuse the same map while reading multiple shapes from the same stream in order to restore duplicates. + static BCSResult sRestoreWithChildren(StreamIn &inStream, IDToShapeMap &ioShapeMap, IDToMaterialMap &ioMaterialMap, IDToGroupFilterMap &ioGroupFilterMap); + + RVec3 mPosition = RVec3::sZero(); ///< Position of the body (not of the center of mass) + Quat mRotation = Quat::sIdentity(); ///< Rotation of the body + Vec3 mLinearVelocity = Vec3::sZero(); ///< World space linear velocity of the center of mass (m/s) + Vec3 mAngularVelocity = Vec3::sZero(); ///< World space angular velocity (rad/s) + + /// User data value (can be used by application) + uint64 mUserData = 0; + + ///@name Collision settings + ObjectLayer mObjectLayer = 0; ///< The collision layer this body belongs to (determines if two objects can collide) + CollisionGroup mCollisionGroup; ///< The collision group this body belongs to (determines if two objects can collide) + + ///@name Simulation properties + EMotionType mMotionType = EMotionType::Dynamic; ///< Motion type, determines if the object is static, dynamic or kinematic + EAllowedDOFs mAllowedDOFs = EAllowedDOFs::All; ///< Which degrees of freedom this body has (can be used to limit simulation to 2D) + bool mAllowDynamicOrKinematic = false; ///< When this body is created as static, this setting tells the system to create a MotionProperties object so that the object can be switched to kinematic or dynamic + bool mIsSensor = false; ///< If this body is a sensor. A sensor will receive collision callbacks, but will not cause any collision responses and can be used as a trigger volume. See description at Body::SetIsSensor. + bool mCollideKinematicVsNonDynamic = false; ///< If kinematic objects can generate contact points against other kinematic or static objects. See description at Body::SetCollideKinematicVsNonDynamic. + bool mUseManifoldReduction = true; ///< If this body should use manifold reduction (see description at Body::SetUseManifoldReduction) + bool mApplyGyroscopicForce = false; ///< Set to indicate that the gyroscopic force should be applied to this body (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem) + EMotionQuality mMotionQuality = EMotionQuality::Discrete; ///< Motion quality, or how well it detects collisions when it has a high velocity + bool mEnhancedInternalEdgeRemoval = false; ///< Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges. + bool mAllowSleeping = true; ///< If this body can go to sleep or not + float mFriction = 0.2f; ///< Friction of the body (dimensionless number, usually between 0 and 1, 0 = no friction, 1 = friction force equals force that presses the two bodies together). Note that bodies can have negative friction but the combined friction (see PhysicsSystem::SetCombineFriction) should never go below zero. + float mRestitution = 0.0f; ///< Restitution of body (dimensionless number, usually between 0 and 1, 0 = completely inelastic collision response, 1 = completely elastic collision response). Note that bodies can have negative restitution but the combined restitution (see PhysicsSystem::SetCombineRestitution) should never go below zero. + float mLinearDamping = 0.05f; ///< Linear damping: dv/dt = -c * v. c must be between 0 and 1 but is usually close to 0. + float mAngularDamping = 0.05f; ///< Angular damping: dw/dt = -c * w. c must be between 0 and 1 but is usually close to 0. + float mMaxLinearVelocity = 500.0f; ///< Maximum linear velocity that this body can reach (m/s) + float mMaxAngularVelocity = 0.25f * JPH_PI * 60.0f; ///< Maximum angular velocity that this body can reach (rad/s) + float mGravityFactor = 1.0f; ///< Value to multiply gravity with for this body + uint mNumVelocityStepsOverride = 0; ///< Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island. + uint mNumPositionStepsOverride = 0; ///< Used only when this body is dynamic and colliding. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island. + + ///@name Mass properties of the body (by default calculated by the shape) + EOverrideMassProperties mOverrideMassProperties = EOverrideMassProperties::CalculateMassAndInertia; ///< Determines how mMassPropertiesOverride will be used + float mInertiaMultiplier = 1.0f; ///< When calculating the inertia (not when it is provided) the calculated inertia will be multiplied by this value + MassProperties mMassPropertiesOverride; ///< Contains replacement mass settings which override the automatically calculated values + +private: + /// Collision volume for the body + RefConst mShape; ///< Shape settings, can be serialized. Mutually exclusive with mShapePtr + RefConst mShapePtr; ///< Actual shape, cannot be serialized. Mutually exclusive with mShape +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyFilter.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyFilter.h new file mode 100644 index 000000000000..111d514018b6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyFilter.h @@ -0,0 +1,130 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class Body; + +/// Class function to filter out bodies, returns true if test should collide with body +class JPH_EXPORT BodyFilter : public NonCopyable +{ +public: + /// Destructor + virtual ~BodyFilter() = default; + + /// Filter function. Returns true if we should collide with inBodyID + virtual bool ShouldCollide([[maybe_unused]] const BodyID &inBodyID) const + { + return true; + } + + /// Filter function. Returns true if we should collide with inBody (this is called after the body is locked and makes it possible to filter based on body members) + virtual bool ShouldCollideLocked([[maybe_unused]] const Body &inBody) const + { + return true; + } +}; + +/// A simple body filter implementation that ignores a single, specified body +class JPH_EXPORT IgnoreSingleBodyFilter : public BodyFilter +{ +public: + /// Constructor, pass the body you want to ignore + explicit IgnoreSingleBodyFilter(const BodyID &inBodyID) : + mBodyID(inBodyID) + { + } + + /// Filter function. Returns true if we should collide with inBodyID + virtual bool ShouldCollide(const BodyID &inBodyID) const override + { + return mBodyID != inBodyID; + } + +private: + BodyID mBodyID; +}; + +/// A simple body filter implementation that ignores multiple, specified bodies +class JPH_EXPORT IgnoreMultipleBodiesFilter : public BodyFilter +{ +public: + /// Remove all bodies from the filter + void Clear() + { + mBodyIDs.clear(); + } + + /// Reserve space for inSize body ID's + void Reserve(uint inSize) + { + mBodyIDs.reserve(inSize); + } + + /// Add a body to be ignored + void IgnoreBody(const BodyID &inBodyID) + { + mBodyIDs.push_back(inBodyID); + } + + /// Filter function. Returns true if we should collide with inBodyID + virtual bool ShouldCollide(const BodyID &inBodyID) const override + { + return std::find(mBodyIDs.begin(), mBodyIDs.end(), inBodyID) == mBodyIDs.end(); + } + +private: + Array mBodyIDs; +}; + +/// Ignores a single body and chains the filter to another filter +class JPH_EXPORT IgnoreSingleBodyFilterChained : public BodyFilter +{ +public: + /// Constructor + explicit IgnoreSingleBodyFilterChained(const BodyID inBodyID, const BodyFilter &inFilter) : + mBodyID(inBodyID), + mFilter(inFilter) + { + } + + /// Filter function. Returns true if we should collide with inBodyID + virtual bool ShouldCollide(const BodyID &inBodyID) const override + { + return inBodyID != mBodyID && mFilter.ShouldCollide(inBodyID); + } + + /// Filter function. Returns true if we should collide with inBody (this is called after the body is locked and makes it possible to filter based on body members) + virtual bool ShouldCollideLocked(const Body &inBody) const override + { + return mFilter.ShouldCollideLocked(inBody); + } + +private: + BodyID mBodyID; + const BodyFilter & mFilter; +}; + +#ifdef JPH_DEBUG_RENDERER +/// Class function to filter out bodies for debug rendering, returns true if body should be rendered +class JPH_EXPORT BodyDrawFilter : public NonCopyable +{ +public: + /// Destructor + virtual ~BodyDrawFilter() = default; + + /// Filter function. Returns true if inBody should be rendered + virtual bool ShouldDraw([[maybe_unused]] const Body& inBody) const + { + return true; + } +}; +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyID.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyID.h new file mode 100644 index 000000000000..d2bbaccff7d2 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyID.h @@ -0,0 +1,100 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// ID of a body. This is a way of reasoning about bodies in a multithreaded simulation while avoiding race conditions. +class BodyID +{ +public: + JPH_OVERRIDE_NEW_DELETE + + static constexpr uint32 cInvalidBodyID = 0xffffffff; ///< The value for an invalid body ID + static constexpr uint32 cBroadPhaseBit = 0x00800000; ///< This bit is used by the broadphase + static constexpr uint32 cMaxBodyIndex = 0x7fffff; ///< Maximum value for body index (also the maximum amount of bodies supported - 1) + static constexpr uint8 cMaxSequenceNumber = 0xff; ///< Maximum value for the sequence number + + /// Construct invalid body ID + BodyID() : + mID(cInvalidBodyID) + { + } + + /// Construct from index and sequence number combined in a single uint32 (use with care!) + explicit BodyID(uint32 inID) : + mID(inID) + { + JPH_ASSERT((inID & cBroadPhaseBit) == 0 || inID == cInvalidBodyID); // Check bit used by broadphase + } + + /// Construct from index and sequence number + explicit BodyID(uint32 inID, uint8 inSequenceNumber) : + mID((uint32(inSequenceNumber) << 24) | inID) + { + JPH_ASSERT(inID < cMaxBodyIndex); // Should not use bit pattern for invalid ID and should not use the broadphase bit + } + + /// Get index in body array + inline uint32 GetIndex() const + { + return mID & cMaxBodyIndex; + } + + /// Get sequence number of body. + /// The sequence number can be used to check if a body ID with the same body index has been reused by another body. + /// It is mainly used in multi threaded situations where a body is removed and its body index is immediately reused by a body created from another thread. + /// Functions querying the broadphase can (after acquiring a body lock) detect that the body has been removed (we assume that this won't happen more than 128 times in a row). + inline uint8 GetSequenceNumber() const + { + return uint8(mID >> 24); + } + + /// Returns the index and sequence number combined in an uint32 + inline uint32 GetIndexAndSequenceNumber() const + { + return mID; + } + + /// Check if the ID is valid + inline bool IsInvalid() const + { + return mID == cInvalidBodyID; + } + + /// Equals check + inline bool operator == (const BodyID &inRHS) const + { + return mID == inRHS.mID; + } + + /// Not equals check + inline bool operator != (const BodyID &inRHS) const + { + return mID != inRHS.mID; + } + + /// Smaller than operator, can be used for sorting bodies + inline bool operator < (const BodyID &inRHS) const + { + return mID < inRHS.mID; + } + + /// Greater than operator, can be used for sorting bodies + inline bool operator > (const BodyID &inRHS) const + { + return mID > inRHS.mID; + } + +private: + uint32 mID; +}; + +JPH_NAMESPACE_END + +// Create a std::hash/JPH::Hash for BodyID +JPH_MAKE_HASHABLE(JPH::BodyID, t.GetIndexAndSequenceNumber()) diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.cpp b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.cpp new file mode 100644 index 000000000000..cc5f27de4191 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.cpp @@ -0,0 +1,1034 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +void BodyInterface::ActivateBodyInternal(Body &ioBody) const +{ + // Activate body or reset its sleep timer. + // Note that BodyManager::ActivateBodies also resets the sleep timer internally, but we avoid a mutex lock if the body is already active by calling ResetSleepTimer directly. + if (!ioBody.IsActive()) + mBodyManager->ActivateBodies(&ioBody.GetID(), 1); + else + ioBody.ResetSleepTimer(); +} + +Body *BodyInterface::CreateBody(const BodyCreationSettings &inSettings) +{ + Body *body = mBodyManager->AllocateBody(inSettings); + if (!mBodyManager->AddBody(body)) + { + mBodyManager->FreeBody(body); + return nullptr; + } + return body; +} + +Body *BodyInterface::CreateSoftBody(const SoftBodyCreationSettings &inSettings) +{ + Body *body = mBodyManager->AllocateSoftBody(inSettings); + if (!mBodyManager->AddBody(body)) + { + mBodyManager->FreeBody(body); + return nullptr; + } + return body; +} + +Body *BodyInterface::CreateBodyWithID(const BodyID &inBodyID, const BodyCreationSettings &inSettings) +{ + Body *body = mBodyManager->AllocateBody(inSettings); + if (!mBodyManager->AddBodyWithCustomID(body, inBodyID)) + { + mBodyManager->FreeBody(body); + return nullptr; + } + return body; +} + +Body *BodyInterface::CreateSoftBodyWithID(const BodyID &inBodyID, const SoftBodyCreationSettings &inSettings) +{ + Body *body = mBodyManager->AllocateSoftBody(inSettings); + if (!mBodyManager->AddBodyWithCustomID(body, inBodyID)) + { + mBodyManager->FreeBody(body); + return nullptr; + } + return body; +} + +Body *BodyInterface::CreateBodyWithoutID(const BodyCreationSettings &inSettings) const +{ + return mBodyManager->AllocateBody(inSettings); +} + +Body *BodyInterface::CreateSoftBodyWithoutID(const SoftBodyCreationSettings &inSettings) const +{ + return mBodyManager->AllocateSoftBody(inSettings); +} + +void BodyInterface::DestroyBodyWithoutID(Body *inBody) const +{ + mBodyManager->FreeBody(inBody); +} + +bool BodyInterface::AssignBodyID(Body *ioBody) +{ + return mBodyManager->AddBody(ioBody); +} + +bool BodyInterface::AssignBodyID(Body *ioBody, const BodyID &inBodyID) +{ + return mBodyManager->AddBodyWithCustomID(ioBody, inBodyID); +} + +Body *BodyInterface::UnassignBodyID(const BodyID &inBodyID) +{ + Body *body = nullptr; + mBodyManager->RemoveBodies(&inBodyID, 1, &body); + return body; +} + +void BodyInterface::UnassignBodyIDs(const BodyID *inBodyIDs, int inNumber, Body **outBodies) +{ + mBodyManager->RemoveBodies(inBodyIDs, inNumber, outBodies); +} + +void BodyInterface::DestroyBody(const BodyID &inBodyID) +{ + mBodyManager->DestroyBodies(&inBodyID, 1); +} + +void BodyInterface::DestroyBodies(const BodyID *inBodyIDs, int inNumber) +{ + mBodyManager->DestroyBodies(inBodyIDs, inNumber); +} + +void BodyInterface::AddBody(const BodyID &inBodyID, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + + // Add to broadphase + BodyID id = inBodyID; + BroadPhase::AddState add_state = mBroadPhase->AddBodiesPrepare(&id, 1); + mBroadPhase->AddBodiesFinalize(&id, 1, add_state); + + // Optionally activate body + if (inActivationMode == EActivation::Activate && !body.IsStatic()) + mBodyManager->ActivateBodies(&inBodyID, 1); + } +} + +void BodyInterface::RemoveBody(const BodyID &inBodyID) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + + // Deactivate body + if (body.IsActive()) + mBodyManager->DeactivateBodies(&inBodyID, 1); + + // Remove from broadphase + BodyID id = inBodyID; + mBroadPhase->RemoveBodies(&id, 1); + } +} + +bool BodyInterface::IsAdded(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + return lock.SucceededAndIsInBroadPhase(); +} + +BodyID BodyInterface::CreateAndAddBody(const BodyCreationSettings &inSettings, EActivation inActivationMode) +{ + const Body *b = CreateBody(inSettings); + if (b == nullptr) + return BodyID(); // Out of bodies + AddBody(b->GetID(), inActivationMode); + return b->GetID(); +} + +BodyID BodyInterface::CreateAndAddSoftBody(const SoftBodyCreationSettings &inSettings, EActivation inActivationMode) +{ + const Body *b = CreateSoftBody(inSettings); + if (b == nullptr) + return BodyID(); // Out of bodies + AddBody(b->GetID(), inActivationMode); + return b->GetID(); +} + +BodyInterface::AddState BodyInterface::AddBodiesPrepare(BodyID *ioBodies, int inNumber) +{ + return mBroadPhase->AddBodiesPrepare(ioBodies, inNumber); +} + +void BodyInterface::AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState, EActivation inActivationMode) +{ + BodyLockMultiWrite lock(*mBodyLockInterface, ioBodies, inNumber); + + // Add to broadphase + mBroadPhase->AddBodiesFinalize(ioBodies, inNumber, inAddState); + + // Optionally activate bodies + if (inActivationMode == EActivation::Activate) + mBodyManager->ActivateBodies(ioBodies, inNumber); +} + +void BodyInterface::AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState) +{ + mBroadPhase->AddBodiesAbort(ioBodies, inNumber, inAddState); +} + +void BodyInterface::RemoveBodies(BodyID *ioBodies, int inNumber) +{ + BodyLockMultiWrite lock(*mBodyLockInterface, ioBodies, inNumber); + + // Deactivate bodies + mBodyManager->DeactivateBodies(ioBodies, inNumber); + + // Remove from broadphase + mBroadPhase->RemoveBodies(ioBodies, inNumber); +} + +void BodyInterface::ActivateBody(const BodyID &inBodyID) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + ActivateBodyInternal(body); + } +} + +void BodyInterface::ActivateBodies(const BodyID *inBodyIDs, int inNumber) +{ + BodyLockMultiWrite lock(*mBodyLockInterface, inBodyIDs, inNumber); + + mBodyManager->ActivateBodies(inBodyIDs, inNumber); +} + +void BodyInterface::ActivateBodiesInAABox(const AABox &inBox, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) +{ + AllHitCollisionCollector collector; + mBroadPhase->CollideAABox(inBox, collector, inBroadPhaseLayerFilter, inObjectLayerFilter); + ActivateBodies(collector.mHits.data(), (int)collector.mHits.size()); +} + +void BodyInterface::DeactivateBody(const BodyID &inBodyID) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + + if (body.IsActive()) + mBodyManager->DeactivateBodies(&inBodyID, 1); + } +} + +void BodyInterface::DeactivateBodies(const BodyID *inBodyIDs, int inNumber) +{ + BodyLockMultiWrite lock(*mBodyLockInterface, inBodyIDs, inNumber); + + mBodyManager->DeactivateBodies(inBodyIDs, inNumber); +} + +bool BodyInterface::IsActive(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + return lock.Succeeded() && lock.GetBody().IsActive(); +} + +void BodyInterface::ResetSleepTimer(const BodyID &inBodyID) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + lock.GetBody().ResetSleepTimer(); +} + +TwoBodyConstraint *BodyInterface::CreateConstraint(const TwoBodyConstraintSettings *inSettings, const BodyID &inBodyID1, const BodyID &inBodyID2) +{ + BodyID constraint_bodies[] = { inBodyID1, inBodyID2 }; + BodyLockMultiWrite lock(*mBodyLockInterface, constraint_bodies, 2); + + Body *body1 = lock.GetBody(0); + Body *body2 = lock.GetBody(1); + + JPH_ASSERT(body1 != body2); + JPH_ASSERT(body1 != nullptr || body2 != nullptr); + + return inSettings->Create(body1 != nullptr? *body1 : Body::sFixedToWorld, body2 != nullptr? *body2 : Body::sFixedToWorld); +} + +void BodyInterface::ActivateConstraint(const TwoBodyConstraint *inConstraint) +{ + BodyID bodies[] = { inConstraint->GetBody1()->GetID(), inConstraint->GetBody2()->GetID() }; + ActivateBodies(bodies, 2); +} + +RefConst BodyInterface::GetShape(const BodyID &inBodyID) const +{ + RefConst shape; + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + shape = lock.GetBody().GetShape(); + return shape; +} + +void BodyInterface::SetShape(const BodyID &inBodyID, const Shape *inShape, bool inUpdateMassProperties, EActivation inActivationMode) const +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Check if shape actually changed + if (body.GetShape() != inShape) + { + // Update the shape + body.SetShapeInternal(inShape, inUpdateMassProperties); + + // Flag collision cache invalid for this body + mBodyManager->InvalidateContactCacheForBody(body); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); + } + + // Optionally activate body + if (inActivationMode == EActivation::Activate && !body.IsStatic()) + ActivateBodyInternal(body); + } + } +} + +void BodyInterface::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inPreviousCenterOfMass, bool inUpdateMassProperties, EActivation inActivationMode) const +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Update center of mass, mass and inertia + body.UpdateCenterOfMassInternal(inPreviousCenterOfMass, inUpdateMassProperties); + + // Recalculate bounding box + body.CalculateWorldSpaceBoundsInternal(); + + // Flag collision cache invalid for this body + mBodyManager->InvalidateContactCacheForBody(body); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); + } + + // Optionally activate body + if (inActivationMode == EActivation::Activate && !body.IsStatic()) + ActivateBodyInternal(body); + } +} + +void BodyInterface::SetObjectLayer(const BodyID &inBodyID, ObjectLayer inLayer) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Check if layer actually changed, updating the broadphase is rather expensive + if (body.GetObjectLayer() != inLayer) + { + // Update the layer on the body + mBodyManager->SetBodyObjectLayerInternal(body, inLayer); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesLayerChanged(&id, 1); + } + } + } +} + +ObjectLayer BodyInterface::GetObjectLayer(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetObjectLayer(); + else + return cObjectLayerInvalid; +} + +void BodyInterface::SetPositionAndRotation(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Update the position + body.SetPositionAndRotationInternal(inPosition, inRotation); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); + } + + // Optionally activate body + if (inActivationMode == EActivation::Activate && !body.IsStatic()) + ActivateBodyInternal(body); + } +} + +void BodyInterface::SetPositionAndRotationWhenChanged(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Check if there is enough change + if (!body.GetPosition().IsClose(inPosition) + || !body.GetRotation().IsClose(inRotation)) + { + // Update the position + body.SetPositionAndRotationInternal(inPosition, inRotation); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); + } + + // Optionally activate body + if (inActivationMode == EActivation::Activate && !body.IsStatic()) + ActivateBodyInternal(body); + } + } +} + +void BodyInterface::GetPositionAndRotation(const BodyID &inBodyID, RVec3 &outPosition, Quat &outRotation) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + outPosition = body.GetPosition(); + outRotation = body.GetRotation(); + } + else + { + outPosition = RVec3::sZero(); + outRotation = Quat::sIdentity(); + } +} + +void BodyInterface::SetPosition(const BodyID &inBodyID, RVec3Arg inPosition, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Update the position + body.SetPositionAndRotationInternal(inPosition, body.GetRotation()); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); + } + + // Optionally activate body + if (inActivationMode == EActivation::Activate && !body.IsStatic()) + ActivateBodyInternal(body); + } +} + +RVec3 BodyInterface::GetPosition(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetPosition(); + else + return RVec3::sZero(); +} + +RVec3 BodyInterface::GetCenterOfMassPosition(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetCenterOfMassPosition(); + else + return RVec3::sZero(); +} + +void BodyInterface::SetRotation(const BodyID &inBodyID, QuatArg inRotation, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Update the position + body.SetPositionAndRotationInternal(body.GetPosition(), inRotation); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); + } + + // Optionally activate body + if (inActivationMode == EActivation::Activate && !body.IsStatic()) + ActivateBodyInternal(body); + } +} + +Quat BodyInterface::GetRotation(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetRotation(); + else + return Quat::sIdentity(); +} + +RMat44 BodyInterface::GetWorldTransform(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetWorldTransform(); + else + return RMat44::sIdentity(); +} + +RMat44 BodyInterface::GetCenterOfMassTransform(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetCenterOfMassTransform(); + else + return RMat44::sIdentity(); +} + +void BodyInterface::MoveKinematic(const BodyID &inBodyID, RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + body.MoveKinematic(inTargetPosition, inTargetRotation, inDeltaTime); + + if (!body.IsActive() && (!body.GetLinearVelocity().IsNearZero() || !body.GetAngularVelocity().IsNearZero())) + mBodyManager->ActivateBodies(&inBodyID, 1); + } +} + +void BodyInterface::SetLinearAndAngularVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (!body.IsStatic()) + { + body.SetLinearVelocityClamped(inLinearVelocity); + body.SetAngularVelocityClamped(inAngularVelocity); + + if (!body.IsActive() && (!inLinearVelocity.IsNearZero() || !inAngularVelocity.IsNearZero())) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +void BodyInterface::GetLinearAndAngularVelocity(const BodyID &inBodyID, Vec3 &outLinearVelocity, Vec3 &outAngularVelocity) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + if (!body.IsStatic()) + { + outLinearVelocity = body.GetLinearVelocity(); + outAngularVelocity = body.GetAngularVelocity(); + return; + } + } + + outLinearVelocity = outAngularVelocity = Vec3::sZero(); +} + +void BodyInterface::SetLinearVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (!body.IsStatic()) + { + body.SetLinearVelocityClamped(inLinearVelocity); + + if (!body.IsActive() && !inLinearVelocity.IsNearZero()) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +Vec3 BodyInterface::GetLinearVelocity(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + if (!body.IsStatic()) + return body.GetLinearVelocity(); + } + + return Vec3::sZero(); +} + +void BodyInterface::AddLinearVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (!body.IsStatic()) + { + body.SetLinearVelocityClamped(body.GetLinearVelocity() + inLinearVelocity); + + if (!body.IsActive() && !body.GetLinearVelocity().IsNearZero()) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +void BodyInterface::AddLinearAndAngularVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (!body.IsStatic()) + { + body.SetLinearVelocityClamped(body.GetLinearVelocity() + inLinearVelocity); + body.SetAngularVelocityClamped(body.GetAngularVelocity() + inAngularVelocity); + + if (!body.IsActive() && (!body.GetLinearVelocity().IsNearZero() || !body.GetAngularVelocity().IsNearZero())) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +void BodyInterface::SetAngularVelocity(const BodyID &inBodyID, Vec3Arg inAngularVelocity) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (!body.IsStatic()) + { + body.SetAngularVelocityClamped(inAngularVelocity); + + if (!body.IsActive() && !inAngularVelocity.IsNearZero()) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +Vec3 BodyInterface::GetAngularVelocity(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + if (!body.IsStatic()) + return body.GetAngularVelocity(); + } + + return Vec3::sZero(); +} + +Vec3 BodyInterface::GetPointVelocity(const BodyID &inBodyID, RVec3Arg inPoint) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + if (!body.IsStatic()) + return body.GetPointVelocity(inPoint); + } + + return Vec3::sZero(); +} + +void BodyInterface::AddForce(const BodyID &inBodyID, Vec3Arg inForce, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic() && (inActivationMode == EActivation::Activate || body.IsActive())) + { + body.AddForce(inForce); + + if (inActivationMode == EActivation::Activate) + ActivateBodyInternal(body); + } + } +} + +void BodyInterface::AddForce(const BodyID &inBodyID, Vec3Arg inForce, RVec3Arg inPoint, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic() && (inActivationMode == EActivation::Activate || body.IsActive())) + { + body.AddForce(inForce, inPoint); + + if (inActivationMode == EActivation::Activate) + ActivateBodyInternal(body); + } + } +} + +void BodyInterface::AddTorque(const BodyID &inBodyID, Vec3Arg inTorque, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic() && (inActivationMode == EActivation::Activate || body.IsActive())) + { + body.AddTorque(inTorque); + + if (inActivationMode == EActivation::Activate) + ActivateBodyInternal(body); + } + } +} + +void BodyInterface::AddForceAndTorque(const BodyID &inBodyID, Vec3Arg inForce, Vec3Arg inTorque, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic() && (inActivationMode == EActivation::Activate || body.IsActive())) + { + body.AddForce(inForce); + body.AddTorque(inTorque); + + if (inActivationMode == EActivation::Activate) + ActivateBodyInternal(body); + } + } +} + +void BodyInterface::AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic()) + { + body.AddImpulse(inImpulse); + + if (!body.IsActive()) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +void BodyInterface::AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse, RVec3Arg inPoint) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic()) + { + body.AddImpulse(inImpulse, inPoint); + + if (!body.IsActive()) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +void BodyInterface::AddAngularImpulse(const BodyID &inBodyID, Vec3Arg inAngularImpulse) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic()) + { + body.AddAngularImpulse(inAngularImpulse); + + if (!body.IsActive()) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +bool BodyInterface::ApplyBuoyancyImpulse(const BodyID &inBodyID, RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic() + && body.ApplyBuoyancyImpulse(inSurfacePosition, inSurfaceNormal, inBuoyancy, inLinearDrag, inAngularDrag, inFluidVelocity, inGravity, inDeltaTime)) + { + ActivateBodyInternal(body); + return true; + } + } + + return false; +} + +void BodyInterface::SetPositionRotationAndVelocity(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Update the position + body.SetPositionAndRotationInternal(inPosition, inRotation); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); + } + + if (!body.IsStatic()) + { + body.SetLinearVelocityClamped(inLinearVelocity); + body.SetAngularVelocityClamped(inAngularVelocity); + + // Optionally activate body + if (!body.IsActive() && (!inLinearVelocity.IsNearZero() || !inAngularVelocity.IsNearZero())) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +void BodyInterface::SetMotionType(const BodyID &inBodyID, EMotionType inMotionType, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Deactivate if we're making the body static + if (body.IsActive() && inMotionType == EMotionType::Static) + mBodyManager->DeactivateBodies(&inBodyID, 1); + + body.SetMotionType(inMotionType); + + // Activate body if requested + if (inMotionType != EMotionType::Static && inActivationMode == EActivation::Activate) + ActivateBodyInternal(body); + } +} + +EBodyType BodyInterface::GetBodyType(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetBodyType(); + else + return EBodyType::RigidBody; +} + +EMotionType BodyInterface::GetMotionType(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetMotionType(); + else + return EMotionType::Static; +} + +void BodyInterface::SetMotionQuality(const BodyID &inBodyID, EMotionQuality inMotionQuality) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + mBodyManager->SetMotionQuality(lock.GetBody(), inMotionQuality); +} + +EMotionQuality BodyInterface::GetMotionQuality(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded() && !lock.GetBody().IsStatic()) + return lock.GetBody().GetMotionProperties()->GetMotionQuality(); + else + return EMotionQuality::Discrete; +} + +Mat44 BodyInterface::GetInverseInertia(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetInverseInertia(); + else + return Mat44::sIdentity(); +} + +void BodyInterface::SetRestitution(const BodyID &inBodyID, float inRestitution) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + lock.GetBody().SetRestitution(inRestitution); +} + +float BodyInterface::GetRestitution(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetRestitution(); + else + return 0.0f; +} + +void BodyInterface::SetFriction(const BodyID &inBodyID, float inFriction) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + lock.GetBody().SetFriction(inFriction); +} + +float BodyInterface::GetFriction(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetFriction(); + else + return 0.0f; +} + +void BodyInterface::SetGravityFactor(const BodyID &inBodyID, float inGravityFactor) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded() && lock.GetBody().GetMotionPropertiesUnchecked() != nullptr) + lock.GetBody().GetMotionPropertiesUnchecked()->SetGravityFactor(inGravityFactor); +} + +float BodyInterface::GetGravityFactor(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded() && lock.GetBody().GetMotionPropertiesUnchecked() != nullptr) + return lock.GetBody().GetMotionPropertiesUnchecked()->GetGravityFactor(); + else + return 1.0f; +} + +void BodyInterface::SetUseManifoldReduction(const BodyID &inBodyID, bool inUseReduction) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.GetUseManifoldReduction() != inUseReduction) + { + body.SetUseManifoldReduction(inUseReduction); + + // Flag collision cache invalid for this body + mBodyManager->InvalidateContactCacheForBody(body); + } + } +} + +bool BodyInterface::GetUseManifoldReduction(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetUseManifoldReduction(); + else + return true; +} + +TransformedShape BodyInterface::GetTransformedShape(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetTransformedShape(); + else + return TransformedShape(); +} + +uint64 BodyInterface::GetUserData(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetUserData(); + else + return 0; +} + +void BodyInterface::SetUserData(const BodyID &inBodyID, uint64 inUserData) const +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + lock.GetBody().SetUserData(inUserData); +} + +const PhysicsMaterial *BodyInterface::GetMaterial(const BodyID &inBodyID, const SubShapeID &inSubShapeID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetShape()->GetMaterial(inSubShapeID); + else + return PhysicsMaterial::sDefault; +} + +void BodyInterface::InvalidateContactCache(const BodyID &inBodyID) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + mBodyManager->InvalidateContactCacheForBody(lock.GetBody()); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.h new file mode 100644 index 000000000000..477b3564e404 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.h @@ -0,0 +1,298 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class Body; +class BodyCreationSettings; +class SoftBodyCreationSettings; +class BodyLockInterface; +class BroadPhase; +class BodyManager; +class TransformedShape; +class PhysicsMaterial; +class SubShapeID; +class Shape; +class TwoBodyConstraintSettings; +class TwoBodyConstraint; +class BroadPhaseLayerFilter; +class AABox; + +/// Class that provides operations on bodies using a body ID. Note that if you need to do multiple operations on a single body, it is more efficient to lock the body once and combine the operations. +/// All quantities are in world space unless otherwise specified. +class JPH_EXPORT BodyInterface : public NonCopyable +{ +public: + /// Initialize the interface (should only be called by PhysicsSystem) + void Init(BodyLockInterface &inBodyLockInterface, BodyManager &inBodyManager, BroadPhase &inBroadPhase) { mBodyLockInterface = &inBodyLockInterface; mBodyManager = &inBodyManager; mBroadPhase = &inBroadPhase; } + + /// Create a rigid body + /// @return Created body or null when out of bodies + Body * CreateBody(const BodyCreationSettings &inSettings); + + /// Create a soft body + /// @return Created body or null when out of bodies + Body * CreateSoftBody(const SoftBodyCreationSettings &inSettings); + + /// Create a rigid body with specified ID. This function can be used if a simulation is to run in sync between clients or if a simulation needs to be restored exactly. + /// The ID created on the server can be replicated to the client and used to create a deterministic simulation. + /// @return Created body or null when the body ID is invalid or a body of the same ID already exists. + Body * CreateBodyWithID(const BodyID &inBodyID, const BodyCreationSettings &inSettings); + + /// Create a soft body with specified ID. See comments at CreateBodyWithID. + Body * CreateSoftBodyWithID(const BodyID &inBodyID, const SoftBodyCreationSettings &inSettings); + + /// Advanced use only. Creates a rigid body without specifying an ID. This body cannot be added to the physics system until it has been assigned a body ID. + /// This can be used to decouple allocation from registering the body. A call to CreateBodyWithoutID followed by AssignBodyID is equivalent to calling CreateBodyWithID. + /// @return Created body + Body * CreateBodyWithoutID(const BodyCreationSettings &inSettings) const; + + /// Advanced use only. Creates a body without specifying an ID. See comments at CreateBodyWithoutID. + Body * CreateSoftBodyWithoutID(const SoftBodyCreationSettings &inSettings) const; + + /// Advanced use only. Destroy a body previously created with CreateBodyWithoutID that hasn't gotten an ID yet through the AssignBodyID function, + /// or a body that has had its body ID unassigned through UnassignBodyIDs. Bodies that have an ID should be destroyed through DestroyBody. + void DestroyBodyWithoutID(Body *inBody) const; + + /// Advanced use only. Assigns the next available body ID to a body that was created using CreateBodyWithoutID. After this call, the body can be added to the physics system. + /// @return false if the body already has an ID or out of body ids. + bool AssignBodyID(Body *ioBody); + + /// Advanced use only. Assigns a body ID to a body that was created using CreateBodyWithoutID. After this call, the body can be added to the physics system. + /// @return false if the body already has an ID or if the ID is not valid. + bool AssignBodyID(Body *ioBody, const BodyID &inBodyID); + + /// Advanced use only. See UnassignBodyIDs. Unassigns the ID of a single body. + Body * UnassignBodyID(const BodyID &inBodyID); + + /// Advanced use only. Removes a number of body IDs from their bodies and returns the body pointers. Before calling this, the body should have been removed from the physics system. + /// The body can be destroyed through DestroyBodyWithoutID. This can be used to decouple deallocation. A call to UnassignBodyIDs followed by calls to DestroyBodyWithoutID is equivalent to calling DestroyBodies. + /// @param inBodyIDs A list of body IDs + /// @param inNumber Number of bodies in the list + /// @param outBodies If not null on input, this will contain a list of body pointers corresponding to inBodyIDs that can be destroyed afterwards (caller assumes ownership over these). + void UnassignBodyIDs(const BodyID *inBodyIDs, int inNumber, Body **outBodies); + + /// Destroy a body. + /// Make sure that you remove the body from the physics system using BodyInterface::RemoveBody before calling this function. + void DestroyBody(const BodyID &inBodyID); + + /// Destroy multiple bodies + /// Make sure that you remove the bodies from the physics system using BodyInterface::RemoveBody before calling this function. + void DestroyBodies(const BodyID *inBodyIDs, int inNumber); + + /// Add body to the physics system. + /// Note that if you need to add multiple bodies, use the AddBodiesPrepare/AddBodiesFinalize function. + /// Adding many bodies, one at a time, results in a really inefficient broadphase until PhysicsSystem::OptimizeBroadPhase is called or when PhysicsSystem::Update rebuilds the tree! + /// 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. + void RemoveBody(const BodyID &inBodyID); + + /// Check if a body has been added to the physics system. + bool IsAdded(const BodyID &inBodyID) const; + + /// Combines CreateBody and AddBody + /// @return Created body ID or an invalid ID when out of bodies + BodyID CreateAndAddBody(const BodyCreationSettings &inSettings, EActivation inActivationMode); + + /// Combines CreateSoftBody and AddBody + /// @return Created body ID or an invalid ID when out of bodies + BodyID CreateAndAddSoftBody(const SoftBodyCreationSettings &inSettings, EActivation inActivationMode); + + /// Add state handle, used to keep track of a batch of bodies while adding them to the PhysicsSystem. + using AddState = void *; + + ///@name Batch adding interface + ///@{ + + /// Prepare adding inNumber bodies at ioBodies to the PhysicsSystem, returns a handle that should be used in AddBodiesFinalize/Abort. + /// This can be done on a background thread without influencing the PhysicsSystem. + /// ioBodies may be shuffled around by this function and should be kept that way until AddBodiesFinalize/Abort is called. + AddState AddBodiesPrepare(BodyID *ioBodies, int inNumber); + + /// Finalize adding bodies to the PhysicsSystem, supply the return value of AddBodiesPrepare in inAddState. + /// Please ensure that the ioBodies array passed to AddBodiesPrepare is unmodified and passed again to this function. + void AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState, EActivation inActivationMode); + + /// Abort adding bodies to the PhysicsSystem, supply the return value of AddBodiesPrepare in inAddState. + /// This can be done on a background thread without influencing the PhysicsSystem. + /// 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. + /// ioBodies may be shuffled around by this function. + void RemoveBodies(BodyID *ioBodies, int inNumber); + ///@} + + ///@name Activate / deactivate a body + ///@{ + void ActivateBody(const BodyID &inBodyID); + void ActivateBodies(const BodyID *inBodyIDs, int inNumber); + void ActivateBodiesInAABox(const AABox &inBox, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter); + void DeactivateBody(const BodyID &inBodyID); + void DeactivateBodies(const BodyID *inBodyIDs, int inNumber); + bool IsActive(const BodyID &inBodyID) const; + void ResetSleepTimer(const BodyID &inBodyID); + ///@} + + /// Create a two body constraint + TwoBodyConstraint * CreateConstraint(const TwoBodyConstraintSettings *inSettings, const BodyID &inBodyID1, const BodyID &inBodyID2); + + /// Activate non-static bodies attached to a constraint + void ActivateConstraint(const TwoBodyConstraint *inConstraint); + + ///@name Access to the shape of a body + ///@{ + + /// Get the current shape + RefConst GetShape(const BodyID &inBodyID) const; + + /// Set a new shape on the body + /// @param inBodyID Body ID of body that had its shape changed + /// @param inShape The new shape + /// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated + /// @param inActivationMode Whether or not to activate the body + void SetShape(const BodyID &inBodyID, const Shape *inShape, bool inUpdateMassProperties, EActivation inActivationMode) const; + + /// Notify all systems to indicate that a shape has changed (usable for MutableCompoundShapes) + /// @param inBodyID Body ID of body that had its shape changed + /// @param inPreviousCenterOfMass Center of mass of the shape before the alterations + /// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated + /// @param inActivationMode Whether or not to activate the body + void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inPreviousCenterOfMass, bool inUpdateMassProperties, EActivation inActivationMode) const; + ///@} + + ///@name Object layer of a body + ///@{ + void SetObjectLayer(const BodyID &inBodyID, ObjectLayer inLayer); + ObjectLayer GetObjectLayer(const BodyID &inBodyID) const; + ///@} + + ///@name Position and rotation of a body + ///@{ + void SetPositionAndRotation(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode); + void SetPositionAndRotationWhenChanged(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode); ///< Will only update the position/rotation and activate the body when the difference is larger than a very small number. This avoids updating the broadphase/waking up a body when the resulting position/orientation doesn't really change. + void GetPositionAndRotation(const BodyID &inBodyID, RVec3 &outPosition, Quat &outRotation) const; + void SetPosition(const BodyID &inBodyID, RVec3Arg inPosition, EActivation inActivationMode); + RVec3 GetPosition(const BodyID &inBodyID) const; + RVec3 GetCenterOfMassPosition(const BodyID &inBodyID) const; + void SetRotation(const BodyID &inBodyID, QuatArg inRotation, EActivation inActivationMode); + Quat GetRotation(const BodyID &inBodyID) const; + RMat44 GetWorldTransform(const BodyID &inBodyID) const; + RMat44 GetCenterOfMassTransform(const BodyID &inBodyID) const; + ///@} + + /// Set velocity of body such that it will be positioned at inTargetPosition/Rotation in inDeltaTime seconds (will activate body if needed) + void MoveKinematic(const BodyID &inBodyID, RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime); + + /// Linear or angular velocity (functions will activate body if needed). + /// 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 SetLinearAndAngularVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity); + void GetLinearAndAngularVelocity(const BodyID &inBodyID, Vec3 &outLinearVelocity, Vec3 &outAngularVelocity) const; + void SetLinearVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity); + Vec3 GetLinearVelocity(const BodyID &inBodyID) const; + void AddLinearVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity); ///< Add velocity to current velocity + void AddLinearAndAngularVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity); ///< Add linear and angular to current velocities + void SetAngularVelocity(const BodyID &inBodyID, Vec3Arg inAngularVelocity); + Vec3 GetAngularVelocity(const BodyID &inBodyID) const; + Vec3 GetPointVelocity(const BodyID &inBodyID, RVec3Arg inPoint) const; ///< Velocity of point inPoint (in world space, e.g. on the surface of the body) of the body + + /// Set the complete motion state of a body. + /// 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 + ///@{ + 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 + void AddTorque(const BodyID &inBodyID, Vec3Arg inTorque, EActivation inActivationMode = EActivation::Activate); ///< See Body::AddTorque + 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 + ///@{ + void AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse); ///< Applied at center of mass + void AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse, RVec3Arg inPoint); ///< Applied at inPoint + void AddAngularImpulse(const BodyID &inBodyID, Vec3Arg inAngularImpulse); + bool ApplyBuoyancyImpulse(const BodyID &inBodyID, RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime); + ///@} + + ///@name Body type + ///@{ + EBodyType GetBodyType(const BodyID &inBodyID) const; + ///@} + + ///@name Body motion type + ///@{ + void SetMotionType(const BodyID &inBodyID, EMotionType inMotionType, EActivation inActivationMode); + EMotionType GetMotionType(const BodyID &inBodyID) const; + ///@} + + ///@name Body motion quality + ///@{ + void SetMotionQuality(const BodyID &inBodyID, EMotionQuality inMotionQuality); + EMotionQuality GetMotionQuality(const BodyID &inBodyID) const; + ///@} + + /// Get inverse inertia tensor in world space + Mat44 GetInverseInertia(const BodyID &inBodyID) const; + + ///@name Restitution + ///@{ + void SetRestitution(const BodyID &inBodyID, float inRestitution); + float GetRestitution(const BodyID &inBodyID) const; + ///@} + + ///@name Friction + ///@{ + void SetFriction(const BodyID &inBodyID, float inFriction); + float GetFriction(const BodyID &inBodyID) const; + ///@} + + ///@name Gravity factor + ///@{ + void SetGravityFactor(const BodyID &inBodyID, float inGravityFactor); + float GetGravityFactor(const BodyID &inBodyID) const; + ///@} + + ///@name Manifold reduction + ///@{ + void SetUseManifoldReduction(const BodyID &inBodyID, bool inUseReduction); + bool GetUseManifoldReduction(const BodyID &inBodyID) const; + ///@} + + /// Get transform and shape for this body, used to perform collision detection + TransformedShape GetTransformedShape(const BodyID &inBodyID) const; + + /// Get the user data for a body + uint64 GetUserData(const BodyID &inBodyID) const; + void SetUserData(const BodyID &inBodyID, uint64 inUserData) const; + + /// Get the material for a particular sub shape + const PhysicsMaterial * GetMaterial(const BodyID &inBodyID, const SubShapeID &inSubShapeID) const; + + /// Set the Body::EFlags::InvalidateContactCache flag for the specified body. This means that the collision cache is invalid for any body pair involving that body until the next physics step. + void InvalidateContactCache(const BodyID &inBodyID); + +private: + /// Helper function to activate a single body + JPH_INLINE void ActivateBodyInternal(Body &ioBody) const; + + BodyLockInterface * mBodyLockInterface = nullptr; + BodyManager * mBodyManager = nullptr; + BroadPhase * mBroadPhase = nullptr; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLock.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLock.h new file mode 100644 index 000000000000..eccfec4d1e8e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLock.h @@ -0,0 +1,111 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Base class for locking bodies for the duration of the scope of this class (do not use directly) +template +class BodyLockBase : public NonCopyable +{ +public: + /// Constructor will lock the body + BodyLockBase(const BodyLockInterface &inBodyLockInterface, const BodyID &inBodyID) : + mBodyLockInterface(inBodyLockInterface) + { + if (inBodyID == BodyID()) + { + // Invalid body id + mBodyLockMutex = nullptr; + mBody = nullptr; + } + else + { + // Get mutex + mBodyLockMutex = Write? inBodyLockInterface.LockWrite(inBodyID) : inBodyLockInterface.LockRead(inBodyID); + + // Get a reference to the body or nullptr when it is no longer valid + mBody = inBodyLockInterface.TryGetBody(inBodyID); + } + } + + /// Explicitly release the lock (normally this is done in the destructor) + inline void ReleaseLock() + { + if (mBodyLockMutex != nullptr) + { + if (Write) + mBodyLockInterface.UnlockWrite(mBodyLockMutex); + else + mBodyLockInterface.UnlockRead(mBodyLockMutex); + + mBodyLockMutex = nullptr; + mBody = nullptr; + } + } + + /// Destructor will unlock the body + ~BodyLockBase() + { + ReleaseLock(); + } + + /// Test if the lock was successful (if the body ID was valid) + inline bool Succeeded() const + { + return mBody != nullptr; + } + + /// Test if the lock was successful (if the body ID was valid) and the body is still in the broad phase + inline bool SucceededAndIsInBroadPhase() const + { + return mBody != nullptr && mBody->IsInBroadPhase(); + } + + /// Access the body + inline BodyType & GetBody() const + { + JPH_ASSERT(mBody != nullptr, "Should check Succeeded() first"); + return *mBody; + } + +private: + const BodyLockInterface & mBodyLockInterface; + SharedMutex * mBodyLockMutex; + BodyType * mBody; +}; + +/// A body lock takes a body ID and locks the underlying body so that other threads cannot access its members +/// +/// The common usage pattern is: +/// +/// BodyLockInterface lock_interface = physics_system.GetBodyLockInterface(); // Or non-locking interface if the lock is already taken +/// BodyID body_id = ...; // Obtain ID to body +/// +/// // Scoped lock +/// { +/// BodyLockRead lock(lock_interface, body_id); +/// if (lock.Succeeded()) // body_id may no longer be valid +/// { +/// const Body &body = lock.GetBody(); +/// +/// // Do something with body +/// ... +/// } +/// } +class BodyLockRead : public BodyLockBase +{ + using BodyLockBase::BodyLockBase; +}; + +/// Specialization that locks a body for writing to. @see BodyLockRead for usage patterns. +class BodyLockWrite : public BodyLockBase +{ + using BodyLockBase::BodyLockBase; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockInterface.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockInterface.h new file mode 100644 index 000000000000..b65951fc8ccd --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockInterface.h @@ -0,0 +1,134 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Base class interface for locking a body. Usually you will use BodyLockRead / BodyLockWrite / BodyLockMultiRead / BodyLockMultiWrite instead. +class BodyLockInterface : public NonCopyable +{ +public: + /// Redefine MutexMask + using MutexMask = BodyManager::MutexMask; + + /// Constructor + explicit BodyLockInterface(BodyManager &inBodyManager) : mBodyManager(inBodyManager) { } + virtual ~BodyLockInterface() = default; + + ///@name Locking functions + ///@{ + virtual SharedMutex * LockRead(const BodyID &inBodyID) const = 0; + virtual void UnlockRead(SharedMutex *inMutex) const = 0; + virtual SharedMutex * LockWrite(const BodyID &inBodyID) const = 0; + virtual void UnlockWrite(SharedMutex *inMutex) const = 0; + ///@} + + /// Get the mask needed to lock all bodies + inline MutexMask GetAllBodiesMutexMask() const + { + return mBodyManager.GetAllBodiesMutexMask(); + } + + ///@name Batch locking functions + ///@{ + virtual MutexMask GetMutexMask(const BodyID *inBodies, int inNumber) const = 0; + virtual void LockRead(MutexMask inMutexMask) const = 0; + virtual void UnlockRead(MutexMask inMutexMask) const = 0; + virtual void LockWrite(MutexMask inMutexMask) const = 0; + virtual void UnlockWrite(MutexMask inMutexMask) const = 0; + ///@} + + /// Convert body ID to body + inline Body * TryGetBody(const BodyID &inBodyID) const { return mBodyManager.TryGetBody(inBodyID); } + +protected: + BodyManager & mBodyManager; +}; + +/// Implementation that performs no locking (assumes the lock has already been taken) +class BodyLockInterfaceNoLock final : public BodyLockInterface +{ +public: + using BodyLockInterface::BodyLockInterface; + + ///@name Locking functions + virtual SharedMutex * LockRead([[maybe_unused]] const BodyID &inBodyID) const override { return nullptr; } + virtual void UnlockRead([[maybe_unused]] SharedMutex *inMutex) const override { /* Nothing to do */ } + virtual SharedMutex * LockWrite([[maybe_unused]] const BodyID &inBodyID) const override { return nullptr; } + virtual void UnlockWrite([[maybe_unused]] SharedMutex *inMutex) const override { /* Nothing to do */ } + + ///@name Batch locking functions + virtual MutexMask GetMutexMask([[maybe_unused]] const BodyID *inBodies, [[maybe_unused]] int inNumber) const override { return 0; } + virtual void LockRead([[maybe_unused]] MutexMask inMutexMask) const override { /* Nothing to do */ } + virtual void UnlockRead([[maybe_unused]] MutexMask inMutexMask) const override { /* Nothing to do */ } + virtual void LockWrite([[maybe_unused]] MutexMask inMutexMask) const override { /* Nothing to do */ } + virtual void UnlockWrite([[maybe_unused]] MutexMask inMutexMask) const override { /* Nothing to do */ } +}; + +/// Implementation that uses the body manager to lock the correct mutex for a body +class BodyLockInterfaceLocking final : public BodyLockInterface +{ +public: + using BodyLockInterface::BodyLockInterface; + + ///@name Locking functions + virtual SharedMutex * LockRead(const BodyID &inBodyID) const override + { + SharedMutex &mutex = mBodyManager.GetMutexForBody(inBodyID); + PhysicsLock::sLockShared(mutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody)); + return &mutex; + } + + virtual void UnlockRead(SharedMutex *inMutex) const override + { + PhysicsLock::sUnlockShared(*inMutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody)); + } + + virtual SharedMutex * LockWrite(const BodyID &inBodyID) const override + { + SharedMutex &mutex = mBodyManager.GetMutexForBody(inBodyID); + PhysicsLock::sLock(mutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody)); + return &mutex; + } + + virtual void UnlockWrite(SharedMutex *inMutex) const override + { + PhysicsLock::sUnlock(*inMutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody)); + } + + ///@name Batch locking functions + virtual MutexMask GetMutexMask(const BodyID *inBodies, int inNumber) const override + { + return mBodyManager.GetMutexMask(inBodies, inNumber); + } + + virtual void LockRead(MutexMask inMutexMask) const override + { + mBodyManager.LockRead(inMutexMask); + } + + virtual void UnlockRead(MutexMask inMutexMask) const override + { + mBodyManager.UnlockRead(inMutexMask); + } + + virtual void LockWrite(MutexMask inMutexMask) const override + { + mBodyManager.LockWrite(inMutexMask); + } + + virtual void UnlockWrite(MutexMask inMutexMask) const override + { + mBodyManager.UnlockWrite(inMutexMask); + } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockMulti.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockMulti.h new file mode 100644 index 000000000000..5872729c029c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockMulti.h @@ -0,0 +1,104 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Base class for locking multiple bodies for the duration of the scope of this class (do not use directly) +template +class BodyLockMultiBase : public NonCopyable +{ +public: + /// Redefine MutexMask + using MutexMask = BodyLockInterface::MutexMask; + + /// Constructor will lock the bodies + BodyLockMultiBase(const BodyLockInterface &inBodyLockInterface, const BodyID *inBodyIDs, int inNumber) : + mBodyLockInterface(inBodyLockInterface), + mMutexMask(inBodyLockInterface.GetMutexMask(inBodyIDs, inNumber)), + mBodyIDs(inBodyIDs), + mNumBodyIDs(inNumber) + { + if (mMutexMask != 0) + { + // Get mutex + if (Write) + inBodyLockInterface.LockWrite(mMutexMask); + else + inBodyLockInterface.LockRead(mMutexMask); + } + } + + /// Destructor will unlock the bodies + ~BodyLockMultiBase() + { + if (mMutexMask != 0) + { + if (Write) + mBodyLockInterface.UnlockWrite(mMutexMask); + else + mBodyLockInterface.UnlockRead(mMutexMask); + } + } + + /// Access the body (returns null if body was not properly locked) + inline BodyType * GetBody(int inBodyIndex) const + { + // Range check + JPH_ASSERT(inBodyIndex >= 0 && inBodyIndex < mNumBodyIDs); + + // Get body ID + const BodyID &body_id = mBodyIDs[inBodyIndex]; + if (body_id.IsInvalid()) + return nullptr; + + // Get a reference to the body or nullptr when it is no longer valid + return mBodyLockInterface.TryGetBody(body_id); + } + +private: + const BodyLockInterface & mBodyLockInterface; + MutexMask mMutexMask; + const BodyID * mBodyIDs; + int mNumBodyIDs; +}; + +/// A multi body lock takes a number of body IDs and locks the underlying bodies so that other threads cannot access its members +/// +/// The common usage pattern is: +/// +/// BodyLockInterface lock_interface = physics_system.GetBodyLockInterface(); // Or non-locking interface if the lock is already taken +/// const BodyID *body_id = ...; // Obtain IDs to bodies +/// int num_body_ids = ...; +/// +/// // Scoped lock +/// { +/// BodyLockMultiRead lock(lock_interface, body_ids, num_body_ids); +/// for (int i = 0; i < num_body_ids; ++i) +/// { +/// const Body *body = lock.GetBody(i); +/// if (body != nullptr) +/// { +/// const Body &body = lock.Body(); +/// +/// // Do something with body +/// ... +/// } +/// } +/// } +class BodyLockMultiRead : public BodyLockMultiBase +{ + using BodyLockMultiBase::BodyLockMultiBase; +}; + +/// Specialization that locks multiple bodies for writing to. @see BodyLockMultiRead for usage patterns. +class BodyLockMultiWrite : public BodyLockMultiBase +{ + using BodyLockMultiBase::BodyLockMultiBase; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.cpp b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.cpp new file mode 100644 index 000000000000..a48e4b0f82fa --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.cpp @@ -0,0 +1,1156 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_ENABLE_ASSERTS + static thread_local bool sOverrideAllowActivation = false; + static thread_local bool sOverrideAllowDeactivation = false; + + bool BodyManager::sGetOverrideAllowActivation() + { + return sOverrideAllowActivation; + } + + void BodyManager::sSetOverrideAllowActivation(bool inValue) + { + sOverrideAllowActivation = inValue; + } + + bool BodyManager::sGetOverrideAllowDeactivation() + { + return sOverrideAllowDeactivation; + } + + void BodyManager::sSetOverrideAllowDeactivation(bool inValue) + { + sOverrideAllowDeactivation = inValue; + } +#endif + +// Helper class that combines a body and its motion properties +class BodyWithMotionProperties : public Body +{ +public: + JPH_OVERRIDE_NEW_DELETE + + MotionProperties mMotionProperties; +}; + +// Helper class that combines a soft body its motion properties and shape +class SoftBodyWithMotionPropertiesAndShape : public Body +{ +public: + SoftBodyWithMotionPropertiesAndShape() + { + mShape.SetEmbedded(); + } + + SoftBodyMotionProperties mMotionProperties; + SoftBodyShape mShape; +}; + +inline void BodyManager::sDeleteBody(Body *inBody) +{ + if (inBody->mMotionProperties != nullptr) + { + JPH_IF_ENABLE_ASSERTS(inBody->mMotionProperties = nullptr;) + if (inBody->IsSoftBody()) + { + inBody->mShape = nullptr; // Release the shape to avoid assertion on shape destruction because of embedded object with refcount > 0 + delete static_cast(inBody); + } + else + delete static_cast(inBody); + } + else + delete inBody; +} + +BodyManager::~BodyManager() +{ + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + // Destroy any bodies that are still alive + for (Body *b : mBodies) + if (sIsValidBodyPointer(b)) + sDeleteBody(b); + + for (BodyID *active_bodies : mActiveBodies) + delete [] active_bodies; +} + +void BodyManager::Init(uint inMaxBodies, uint inNumBodyMutexes, const BroadPhaseLayerInterface &inLayerInterface) +{ + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + // Num body mutexes must be a power of two and not bigger than our MutexMask + uint num_body_mutexes = Clamp(GetNextPowerOf2(inNumBodyMutexes == 0? 2 * thread::hardware_concurrency() : inNumBodyMutexes), 1, sizeof(MutexMask) * 8); +#ifdef JPH_TSAN_ENABLED + num_body_mutexes = min(num_body_mutexes, 32U); // TSAN errors out when locking too many mutexes on the same thread, see: https://github.com/google/sanitizers/issues/950 +#endif + + // Allocate the body mutexes + mBodyMutexes.Init(num_body_mutexes); + + // Allocate space for bodies + mBodies.reserve(inMaxBodies); + + // Allocate space for active bodies + for (BodyID *&active_bodies : mActiveBodies) + { + JPH_ASSERT(active_bodies == nullptr); + active_bodies = new BodyID [inMaxBodies]; + } + + // Allocate space for sequence numbers + mBodySequenceNumbers.resize(inMaxBodies, 0); + + // Keep layer interface + mBroadPhaseLayerInterface = &inLayerInterface; +} + +uint BodyManager::GetNumBodies() const +{ + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + return mNumBodies; +} + +BodyManager::BodyStats BodyManager::GetBodyStats() const +{ + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + BodyStats stats; + stats.mNumBodies = mNumBodies; + stats.mMaxBodies = uint(mBodies.capacity()); + + for (const Body *body : mBodies) + if (sIsValidBodyPointer(body)) + { + if (body->IsSoftBody()) + { + stats.mNumSoftBodies++; + if (body->IsActive()) + stats.mNumActiveSoftBodies++; + } + else + { + switch (body->GetMotionType()) + { + case EMotionType::Static: + stats.mNumBodiesStatic++; + break; + + case EMotionType::Dynamic: + stats.mNumBodiesDynamic++; + if (body->IsActive()) + stats.mNumActiveBodiesDynamic++; + break; + + case EMotionType::Kinematic: + stats.mNumBodiesKinematic++; + if (body->IsActive()) + stats.mNumActiveBodiesKinematic++; + break; + } + } + } + + return stats; +} + +Body *BodyManager::AllocateBody(const BodyCreationSettings &inBodyCreationSettings) const +{ + // Fill in basic properties + Body *body; + if (inBodyCreationSettings.HasMassProperties()) + { + BodyWithMotionProperties *bmp = new BodyWithMotionProperties; + body = bmp; + body->mMotionProperties = &bmp->mMotionProperties; + } + else + { + body = new Body; + } + body->mBodyType = EBodyType::RigidBody; + body->mShape = inBodyCreationSettings.GetShape(); + body->mUserData = inBodyCreationSettings.mUserData; + body->SetFriction(inBodyCreationSettings.mFriction); + body->SetRestitution(inBodyCreationSettings.mRestitution); + body->mMotionType = inBodyCreationSettings.mMotionType; + if (inBodyCreationSettings.mIsSensor) + body->SetIsSensor(true); + if (inBodyCreationSettings.mCollideKinematicVsNonDynamic) + body->SetCollideKinematicVsNonDynamic(true); + if (inBodyCreationSettings.mUseManifoldReduction) + body->SetUseManifoldReduction(true); + if (inBodyCreationSettings.mApplyGyroscopicForce) + body->SetApplyGyroscopicForce(true); + if (inBodyCreationSettings.mEnhancedInternalEdgeRemoval) + body->SetEnhancedInternalEdgeRemoval(true); + SetBodyObjectLayerInternal(*body, inBodyCreationSettings.mObjectLayer); + body->mObjectLayer = inBodyCreationSettings.mObjectLayer; + body->mCollisionGroup = inBodyCreationSettings.mCollisionGroup; + + if (inBodyCreationSettings.HasMassProperties()) + { + MotionProperties *mp = body->mMotionProperties; + mp->SetLinearDamping(inBodyCreationSettings.mLinearDamping); + mp->SetAngularDamping(inBodyCreationSettings.mAngularDamping); + mp->SetMaxLinearVelocity(inBodyCreationSettings.mMaxLinearVelocity); + mp->SetMaxAngularVelocity(inBodyCreationSettings.mMaxAngularVelocity); + mp->SetMassProperties(inBodyCreationSettings.mAllowedDOFs, inBodyCreationSettings.GetMassProperties()); + mp->SetLinearVelocity(inBodyCreationSettings.mLinearVelocity); // Needs to happen after setting the max linear/angular velocity and setting allowed DOFs + mp->SetAngularVelocity(inBodyCreationSettings.mAngularVelocity); + mp->SetGravityFactor(inBodyCreationSettings.mGravityFactor); + mp->SetNumVelocityStepsOverride(inBodyCreationSettings.mNumVelocityStepsOverride); + mp->SetNumPositionStepsOverride(inBodyCreationSettings.mNumPositionStepsOverride); + mp->mMotionQuality = inBodyCreationSettings.mMotionQuality; + mp->mAllowSleeping = inBodyCreationSettings.mAllowSleeping; + JPH_IF_ENABLE_ASSERTS(mp->mCachedBodyType = body->mBodyType;) + JPH_IF_ENABLE_ASSERTS(mp->mCachedMotionType = body->mMotionType;) + } + + // Position body + body->SetPositionAndRotationInternal(inBodyCreationSettings.mPosition, inBodyCreationSettings.mRotation); + + return body; +} + +/// Create a soft body using creation settings. The returned body will not be part of the body manager yet. +Body *BodyManager::AllocateSoftBody(const SoftBodyCreationSettings &inSoftBodyCreationSettings) const +{ + // Fill in basic properties + SoftBodyWithMotionPropertiesAndShape *bmp = new SoftBodyWithMotionPropertiesAndShape; + SoftBodyMotionProperties *mp = &bmp->mMotionProperties; + SoftBodyShape *shape = &bmp->mShape; + Body *body = bmp; + shape->mSoftBodyMotionProperties = mp; + body->mBodyType = EBodyType::SoftBody; + body->mMotionProperties = mp; + body->mShape = shape; + body->mUserData = inSoftBodyCreationSettings.mUserData; + body->SetFriction(inSoftBodyCreationSettings.mFriction); + body->SetRestitution(inSoftBodyCreationSettings.mRestitution); + body->mMotionType = EMotionType::Dynamic; + SetBodyObjectLayerInternal(*body, inSoftBodyCreationSettings.mObjectLayer); + body->mObjectLayer = inSoftBodyCreationSettings.mObjectLayer; + body->mCollisionGroup = inSoftBodyCreationSettings.mCollisionGroup; + mp->SetLinearDamping(inSoftBodyCreationSettings.mLinearDamping); + mp->SetAngularDamping(0); + mp->SetMaxLinearVelocity(inSoftBodyCreationSettings.mMaxLinearVelocity); + mp->SetMaxAngularVelocity(FLT_MAX); + mp->SetLinearVelocity(Vec3::sZero()); + mp->SetAngularVelocity(Vec3::sZero()); + mp->SetGravityFactor(inSoftBodyCreationSettings.mGravityFactor); + mp->mMotionQuality = EMotionQuality::Discrete; + mp->mAllowSleeping = inSoftBodyCreationSettings.mAllowSleeping; + JPH_IF_ENABLE_ASSERTS(mp->mCachedBodyType = body->mBodyType;) + JPH_IF_ENABLE_ASSERTS(mp->mCachedMotionType = body->mMotionType;) + mp->Initialize(inSoftBodyCreationSettings); + + body->SetPositionAndRotationInternal(inSoftBodyCreationSettings.mPosition, inSoftBodyCreationSettings.mMakeRotationIdentity? Quat::sIdentity() : inSoftBodyCreationSettings.mRotation); + + return body; +} + +void BodyManager::FreeBody(Body *inBody) const +{ + JPH_ASSERT(inBody->GetID().IsInvalid(), "This function should only be called on a body that doesn't have an ID yet, use DestroyBody otherwise"); + + sDeleteBody(inBody); +} + +bool BodyManager::AddBody(Body *ioBody) +{ + // Return error when body was already added + if (!ioBody->GetID().IsInvalid()) + return false; + + // Determine next free index + uint32 idx; + { + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + if (mBodyIDFreeListStart != cBodyIDFreeListEnd) + { + // Pop an item from the freelist + JPH_ASSERT(mBodyIDFreeListStart & cIsFreedBody); + idx = uint32(mBodyIDFreeListStart >> cFreedBodyIndexShift); + JPH_ASSERT(!sIsValidBodyPointer(mBodies[idx])); + mBodyIDFreeListStart = uintptr_t(mBodies[idx]); + mBodies[idx] = ioBody; + } + else + { + if (mBodies.size() < mBodies.capacity()) + { + // Allocate a new entry, note that the array should not actually resize since we've reserved it at init time + idx = uint32(mBodies.size()); + mBodies.push_back(ioBody); + } + else + { + // Out of bodies + return false; + } + } + + // Update cached number of bodies + mNumBodies++; + } + + // Get next sequence number and assign the ID + uint8 seq_no = GetNextSequenceNumber(idx); + ioBody->mID = BodyID(idx, seq_no); + return true; +} + +bool BodyManager::AddBodyWithCustomID(Body *ioBody, const BodyID &inBodyID) +{ + // Return error when body was already added + if (!ioBody->GetID().IsInvalid()) + return false; + + { + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + // Check if index is beyond the max body ID + uint32 idx = inBodyID.GetIndex(); + if (idx >= mBodies.capacity()) + return false; // Return error + + if (idx < mBodies.size()) + { + // Body array entry has already been allocated, check if there's a free body here + if (sIsValidBodyPointer(mBodies[idx])) + return false; // Return error + + // Remove the entry from the freelist + uintptr_t idx_start = mBodyIDFreeListStart >> cFreedBodyIndexShift; + if (idx == idx_start) + { + // First entry, easy to remove, the start of the list is our next + mBodyIDFreeListStart = uintptr_t(mBodies[idx]); + } + else + { + // Loop over the freelist and find the entry in the freelist pointing to our index + // TODO: This is O(N), see if this becomes a performance problem (don't want to put the freed bodies in a double linked list) + uintptr_t cur, next; + for (cur = idx_start; cur != cBodyIDFreeListEnd >> cFreedBodyIndexShift; cur = next) + { + next = uintptr_t(mBodies[cur]) >> cFreedBodyIndexShift; + if (next == idx) + { + mBodies[cur] = mBodies[idx]; + break; + } + } + JPH_ASSERT(cur != cBodyIDFreeListEnd >> cFreedBodyIndexShift); + } + + // Put the body in the slot + mBodies[idx] = ioBody; + } + else + { + // Ensure that all body IDs up to this body ID have been allocated and added to the free list + while (idx > mBodies.size()) + { + // Push the id onto the freelist + mBodies.push_back((Body *)mBodyIDFreeListStart); + mBodyIDFreeListStart = (uintptr_t(mBodies.size() - 1) << cFreedBodyIndexShift) | cIsFreedBody; + } + + // Add the element to the list + mBodies.push_back(ioBody); + } + + // Update cached number of bodies + mNumBodies++; + } + + // Assign the ID + ioBody->mID = inBodyID; + return true; +} + +Body *BodyManager::RemoveBodyInternal(const BodyID &inBodyID) +{ + // Get body + uint32 idx = inBodyID.GetIndex(); + Body *body = mBodies[idx]; + + // Validate that it can be removed + JPH_ASSERT(body->GetID() == inBodyID); + JPH_ASSERT(!body->IsActive()); + JPH_ASSERT(!body->IsInBroadPhase(), "Use BodyInterface::RemoveBody to remove this body first!"); + + // Push the id onto the freelist + mBodies[idx] = (Body *)mBodyIDFreeListStart; + mBodyIDFreeListStart = (uintptr_t(idx) << cFreedBodyIndexShift) | cIsFreedBody; + + return body; +} + +#if defined(JPH_DEBUG) && defined(JPH_ENABLE_ASSERTS) + +void BodyManager::ValidateFreeList() const +{ + // Check that the freelist is correct + size_t num_freed = 0; + for (uintptr_t start = mBodyIDFreeListStart; start != cBodyIDFreeListEnd; start = uintptr_t(mBodies[start >> cFreedBodyIndexShift])) + { + JPH_ASSERT(start & cIsFreedBody); + num_freed++; + } + JPH_ASSERT(mNumBodies == mBodies.size() - num_freed); +} + +#endif // defined(JPH_DEBUG) && _defined(JPH_ENABLE_ASSERTS) + +void BodyManager::RemoveBodies(const BodyID *inBodyIDs, int inNumber, Body **outBodies) +{ + // Don't take lock if no bodies are to be destroyed + if (inNumber <= 0) + return; + + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + // Update cached number of bodies + JPH_ASSERT(mNumBodies >= (uint)inNumber); + mNumBodies -= inNumber; + + for (const BodyID *b = inBodyIDs, *b_end = inBodyIDs + inNumber; b < b_end; b++) + { + // Remove body + Body *body = RemoveBodyInternal(*b); + + // Clear the ID + body->mID = BodyID(); + + // Return the body to the caller + if (outBodies != nullptr) + { + *outBodies = body; + ++outBodies; + } + } + +#if defined(JPH_DEBUG) && defined(JPH_ENABLE_ASSERTS) + ValidateFreeList(); +#endif // defined(JPH_DEBUG) && _defined(JPH_ENABLE_ASSERTS) +} + +void BodyManager::DestroyBodies(const BodyID *inBodyIDs, int inNumber) +{ + // Don't take lock if no bodies are to be destroyed + if (inNumber <= 0) + return; + + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + // Update cached number of bodies + JPH_ASSERT(mNumBodies >= (uint)inNumber); + mNumBodies -= inNumber; + + for (const BodyID *b = inBodyIDs, *b_end = inBodyIDs + inNumber; b < b_end; b++) + { + // Remove body + Body *body = RemoveBodyInternal(*b); + + // Free the body + sDeleteBody(body); + } + +#if defined(JPH_DEBUG) && defined(JPH_ENABLE_ASSERTS) + ValidateFreeList(); +#endif // defined(JPH_DEBUG) && _defined(JPH_ENABLE_ASSERTS) +} + +void BodyManager::AddBodyToActiveBodies(Body &ioBody) +{ + // Select the correct array to use + int type = (int)ioBody.GetBodyType(); + atomic &num_active_bodies = mNumActiveBodies[type]; + BodyID *active_bodies = mActiveBodies[type]; + + MotionProperties *mp = ioBody.mMotionProperties; + uint32 num_active_bodies_val = num_active_bodies.load(memory_order_relaxed); + mp->mIndexInActiveBodies = num_active_bodies_val; + JPH_ASSERT(num_active_bodies_val < GetMaxBodies()); + active_bodies[num_active_bodies_val] = ioBody.GetID(); + num_active_bodies.fetch_add(1, memory_order_release); // Increment atomic after setting the body ID so that PhysicsSystem::JobFindCollisions (which doesn't lock the mActiveBodiesMutex) will only read valid IDs + + // Count CCD bodies + if (mp->GetMotionQuality() == EMotionQuality::LinearCast) + mNumActiveCCDBodies++; +} + +void BodyManager::RemoveBodyFromActiveBodies(Body &ioBody) +{ + // Select the correct array to use + int type = (int)ioBody.GetBodyType(); + atomic &num_active_bodies = mNumActiveBodies[type]; + BodyID *active_bodies = mActiveBodies[type]; + + uint32 last_body_index = num_active_bodies.load(memory_order_relaxed) - 1; + MotionProperties *mp = ioBody.mMotionProperties; + if (mp->mIndexInActiveBodies != last_body_index) + { + // This is not the last body, use the last body to fill the hole + BodyID last_body_id = active_bodies[last_body_index]; + active_bodies[mp->mIndexInActiveBodies] = last_body_id; + + // Update that body's index in the active list + Body &last_body = *mBodies[last_body_id.GetIndex()]; + JPH_ASSERT(last_body.mMotionProperties->mIndexInActiveBodies == last_body_index); + last_body.mMotionProperties->mIndexInActiveBodies = mp->mIndexInActiveBodies; + } + + // Mark this body as no longer active + mp->mIndexInActiveBodies = Body::cInactiveIndex; + + // Remove unused element from active bodies list + num_active_bodies.fetch_sub(1, memory_order_release); + + // Count CCD bodies + if (mp->GetMotionQuality() == EMotionQuality::LinearCast) + mNumActiveCCDBodies--; +} + +void BodyManager::ActivateBodies(const BodyID *inBodyIDs, int inNumber) +{ + // Don't take lock if no bodies are to be activated + if (inNumber <= 0) + return; + + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + JPH_ASSERT(!mActiveBodiesLocked || sOverrideAllowActivation); + + for (const BodyID *b = inBodyIDs, *b_end = inBodyIDs + inNumber; b < b_end; b++) + if (!b->IsInvalid()) + { + BodyID body_id = *b; + Body &body = *mBodies[body_id.GetIndex()]; + + JPH_ASSERT(body.GetID() == body_id); + JPH_ASSERT(body.IsInBroadPhase(), "Use BodyInterface::AddBody to add the body first!"); + + if (!body.IsStatic()) + { + // Reset sleeping timer so that we don't immediately go to sleep again + body.ResetSleepTimer(); + + // Check if we're sleeping + if (body.mMotionProperties->mIndexInActiveBodies == Body::cInactiveIndex) + { + AddBodyToActiveBodies(body); + + // Call activation listener + if (mActivationListener != nullptr) + mActivationListener->OnBodyActivated(body_id, body.GetUserData()); + } + } + } +} + +void BodyManager::DeactivateBodies(const BodyID *inBodyIDs, int inNumber) +{ + // Don't take lock if no bodies are to be deactivated + if (inNumber <= 0) + return; + + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + JPH_ASSERT(!mActiveBodiesLocked || sOverrideAllowDeactivation); + + for (const BodyID *b = inBodyIDs, *b_end = inBodyIDs + inNumber; b < b_end; b++) + if (!b->IsInvalid()) + { + BodyID body_id = *b; + Body &body = *mBodies[body_id.GetIndex()]; + + JPH_ASSERT(body.GetID() == body_id); + JPH_ASSERT(body.IsInBroadPhase(), "Use BodyInterface::AddBody to add the body first!"); + + if (body.mMotionProperties != nullptr + && body.mMotionProperties->mIndexInActiveBodies != Body::cInactiveIndex) + { + // Remove the body from the active bodies list + RemoveBodyFromActiveBodies(body); + + // Mark this body as no longer active + body.mMotionProperties->mIslandIndex = Body::cInactiveIndex; + + // Reset velocity + body.mMotionProperties->mLinearVelocity = Vec3::sZero(); + body.mMotionProperties->mAngularVelocity = Vec3::sZero(); + + // Call activation listener + if (mActivationListener != nullptr) + mActivationListener->OnBodyDeactivated(body_id, body.GetUserData()); + } + } +} + +void BodyManager::SetMotionQuality(Body &ioBody, EMotionQuality inMotionQuality) +{ + MotionProperties *mp = ioBody.GetMotionPropertiesUnchecked(); + if (mp != nullptr && mp->GetMotionQuality() != inMotionQuality) + { + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + JPH_ASSERT(!mActiveBodiesLocked); + + bool is_active = ioBody.IsActive(); + if (is_active && mp->GetMotionQuality() == EMotionQuality::LinearCast) + --mNumActiveCCDBodies; + + mp->mMotionQuality = inMotionQuality; + + if (is_active && mp->GetMotionQuality() == EMotionQuality::LinearCast) + ++mNumActiveCCDBodies; + } +} + +void BodyManager::GetActiveBodies(EBodyType inType, BodyIDVector &outBodyIDs) const +{ + JPH_PROFILE_FUNCTION(); + + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + const BodyID *active_bodies = mActiveBodies[(int)inType]; + outBodyIDs.assign(active_bodies, active_bodies + mNumActiveBodies[(int)inType].load(memory_order_relaxed)); +} + +void BodyManager::GetBodyIDs(BodyIDVector &outBodies) const +{ + JPH_PROFILE_FUNCTION(); + + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + // Reserve space for all bodies + outBodies.clear(); + outBodies.reserve(mNumBodies); + + // Iterate the list and find the bodies that are not null + for (const Body *b : mBodies) + if (sIsValidBodyPointer(b)) + outBodies.push_back(b->GetID()); + + // Validate that our reservation was correct + JPH_ASSERT(outBodies.size() == mNumBodies); +} + +void BodyManager::SetBodyActivationListener(BodyActivationListener *inListener) +{ + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + mActivationListener = inListener; +} + +BodyManager::MutexMask BodyManager::GetMutexMask(const BodyID *inBodies, int inNumber) const +{ + JPH_ASSERT(sizeof(MutexMask) * 8 >= mBodyMutexes.GetNumMutexes(), "MutexMask must have enough bits"); + + if (inNumber >= (int)mBodyMutexes.GetNumMutexes()) + { + // Just lock everything if there are too many bodies + return GetAllBodiesMutexMask(); + } + else + { + MutexMask mask = 0; + for (const BodyID *b = inBodies, *b_end = inBodies + inNumber; b < b_end; ++b) + if (!b->IsInvalid()) + { + uint32 index = mBodyMutexes.GetMutexIndex(b->GetIndex()); + mask |= (MutexMask(1) << index); + } + return mask; + } +} + +void BodyManager::LockRead(MutexMask inMutexMask) const +{ + JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckLock(this, EPhysicsLockTypes::PerBody)); + + int index = 0; + for (MutexMask mask = inMutexMask; mask != 0; mask >>= 1, index++) + if (mask & 1) + mBodyMutexes.GetMutexByIndex(index).lock_shared(); +} + +void BodyManager::UnlockRead(MutexMask inMutexMask) const +{ + JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckUnlock(this, EPhysicsLockTypes::PerBody)); + + int index = 0; + for (MutexMask mask = inMutexMask; mask != 0; mask >>= 1, index++) + if (mask & 1) + mBodyMutexes.GetMutexByIndex(index).unlock_shared(); +} + +void BodyManager::LockWrite(MutexMask inMutexMask) const +{ + JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckLock(this, EPhysicsLockTypes::PerBody)); + + int index = 0; + for (MutexMask mask = inMutexMask; mask != 0; mask >>= 1, index++) + if (mask & 1) + mBodyMutexes.GetMutexByIndex(index).lock(); +} + +void BodyManager::UnlockWrite(MutexMask inMutexMask) const +{ + JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckUnlock(this, EPhysicsLockTypes::PerBody)); + + int index = 0; + for (MutexMask mask = inMutexMask; mask != 0; mask >>= 1, index++) + if (mask & 1) + mBodyMutexes.GetMutexByIndex(index).unlock(); +} + +void BodyManager::LockAllBodies() const +{ + JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckLock(this, EPhysicsLockTypes::PerBody)); + mBodyMutexes.LockAll(); + + PhysicsLock::sLock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); +} + +void BodyManager::UnlockAllBodies() const +{ + PhysicsLock::sUnlock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckUnlock(this, EPhysicsLockTypes::PerBody)); + mBodyMutexes.UnlockAll(); +} + +void BodyManager::SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const +{ + { + LockAllBodies(); + + // Determine which bodies to save + Array bodies; + bodies.reserve(mNumBodies); + for (const Body *b : mBodies) + if (sIsValidBodyPointer(b) && b->IsInBroadPhase() && (inFilter == nullptr || inFilter->ShouldSaveBody(*b))) + bodies.push_back(b); + + // Write state of bodies + uint32 num_bodies = (uint32)bodies.size(); + inStream.Write(num_bodies); + for (const Body *b : bodies) + { + inStream.Write(b->GetID()); + inStream.Write(b->IsActive()); + b->SaveState(inStream); + } + + UnlockAllBodies(); + } +} + +bool BodyManager::RestoreState(StateRecorder &inStream) +{ + BodyIDVector bodies_to_activate, bodies_to_deactivate; + + { + LockAllBodies(); + + if (inStream.IsValidating()) + { + // Read state of bodies, note this reads it in a way to be consistent with validation + uint32 old_num_bodies = 0; + for (const Body *b : mBodies) + if (sIsValidBodyPointer(b) && b->IsInBroadPhase()) + ++old_num_bodies; + uint32 num_bodies = old_num_bodies; // Initialize to current value for validation + inStream.Read(num_bodies); + if (num_bodies != old_num_bodies) + { + JPH_ASSERT(false, "Cannot handle adding/removing bodies"); + UnlockAllBodies(); + return false; + } + + for (Body *b : mBodies) + if (sIsValidBodyPointer(b) && b->IsInBroadPhase()) + { + BodyID body_id = b->GetID(); // Initialize to current value for validation + inStream.Read(body_id); + if (body_id != b->GetID()) + { + JPH_ASSERT(false, "Cannot handle adding/removing bodies"); + UnlockAllBodies(); + return false; + } + bool is_active = b->IsActive(); // Initialize to current value for validation + inStream.Read(is_active); + if (is_active != b->IsActive()) + { + if (is_active) + bodies_to_activate.push_back(body_id); + else + bodies_to_deactivate.push_back(body_id); + } + b->RestoreState(inStream); + } + } + else + { + // Not validating, we can be a bit more loose, read number of bodies + uint32 num_bodies = 0; + inStream.Read(num_bodies); + + // Iterate over the stored bodies and restore their state + for (uint32 idx = 0; idx < num_bodies; ++idx) + { + BodyID body_id; + inStream.Read(body_id); + Body *b = TryGetBody(body_id); + if (b == nullptr) + { + JPH_ASSERT(false, "Restoring state for non-existing body"); + UnlockAllBodies(); + return false; + } + bool is_active; + inStream.Read(is_active); + if (is_active != b->IsActive()) + { + if (is_active) + bodies_to_activate.push_back(body_id); + else + bodies_to_deactivate.push_back(body_id); + } + b->RestoreState(inStream); + } + } + + UnlockAllBodies(); + } + + { + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + for (BodyID body_id : bodies_to_activate) + { + Body *body = TryGetBody(body_id); + AddBodyToActiveBodies(*body); + } + + for (BodyID body_id : bodies_to_deactivate) + { + Body *body = TryGetBody(body_id); + RemoveBodyFromActiveBodies(*body); + } + } + + return true; +} + +void BodyManager::SaveBodyState(const Body &inBody, StateRecorder &inStream) const +{ + inStream.Write(inBody.IsActive()); + + inBody.SaveState(inStream); +} + +void BodyManager::RestoreBodyState(Body &ioBody, StateRecorder &inStream) +{ + bool is_active = ioBody.IsActive(); + inStream.Read(is_active); + + ioBody.RestoreState(inStream); + + if (is_active != ioBody.IsActive()) + { + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + JPH_ASSERT(!mActiveBodiesLocked || sOverrideAllowActivation); + + if (is_active) + AddBodyToActiveBodies(ioBody); + else + RemoveBodyFromActiveBodies(ioBody); + } +} + +#ifdef JPH_DEBUG_RENDERER +void BodyManager::Draw(const DrawSettings &inDrawSettings, const PhysicsSettings &inPhysicsSettings, DebugRenderer *inRenderer, const BodyDrawFilter *inBodyFilter) +{ + JPH_PROFILE_FUNCTION(); + + LockAllBodies(); + + for (const Body *body : mBodies) + if (sIsValidBodyPointer(body) && body->IsInBroadPhase() && (!inBodyFilter || inBodyFilter->ShouldDraw(*body))) + { + JPH_ASSERT(mBodies[body->GetID().GetIndex()] == body); + + bool is_sensor = body->IsSensor(); + + // Determine drawing mode + Color color; + if (is_sensor) + color = Color::sYellow; + else + switch (inDrawSettings.mDrawShapeColor) + { + case EShapeColor::InstanceColor: + // Each instance has own color + color = Color::sGetDistinctColor(body->mID.GetIndex()); + break; + + case EShapeColor::ShapeTypeColor: + color = ShapeFunctions::sGet(body->GetShape()->GetSubType()).mColor; + break; + + case EShapeColor::MotionTypeColor: + // Determine color based on motion type + switch (body->mMotionType) + { + case EMotionType::Static: + color = Color::sGrey; + break; + + case EMotionType::Kinematic: + color = Color::sGreen; + break; + + case EMotionType::Dynamic: + color = Color::sGetDistinctColor(body->mID.GetIndex()); + break; + + default: + JPH_ASSERT(false); + color = Color::sBlack; + break; + } + break; + + case EShapeColor::SleepColor: + // Determine color based on motion type + switch (body->mMotionType) + { + case EMotionType::Static: + color = Color::sGrey; + break; + + case EMotionType::Kinematic: + color = body->IsActive()? Color::sGreen : Color::sRed; + break; + + case EMotionType::Dynamic: + color = body->IsActive()? Color::sYellow : Color::sRed; + break; + + default: + JPH_ASSERT(false); + color = Color::sBlack; + break; + } + break; + + case EShapeColor::IslandColor: + // Determine color based on motion type + switch (body->mMotionType) + { + case EMotionType::Static: + color = Color::sGrey; + break; + + case EMotionType::Kinematic: + case EMotionType::Dynamic: + { + uint32 idx = body->GetMotionProperties()->GetIslandIndexInternal(); + color = idx != Body::cInactiveIndex? Color::sGetDistinctColor(idx) : Color::sLightGrey; + } + break; + + default: + JPH_ASSERT(false); + color = Color::sBlack; + break; + } + break; + + case EShapeColor::MaterialColor: + color = Color::sWhite; + break; + + default: + JPH_ASSERT(false); + color = Color::sBlack; + break; + } + + // Draw the results of GetSupportFunction + if (inDrawSettings.mDrawGetSupportFunction) + body->mShape->DrawGetSupportFunction(inRenderer, body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f), color, inDrawSettings.mDrawSupportDirection); + + // Draw the results of GetSupportingFace + if (inDrawSettings.mDrawGetSupportingFace) + body->mShape->DrawGetSupportingFace(inRenderer, body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f)); + + // Draw the shape + if (inDrawSettings.mDrawShape) + body->mShape->Draw(inRenderer, body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f), color, inDrawSettings.mDrawShapeColor == EShapeColor::MaterialColor, inDrawSettings.mDrawShapeWireframe || is_sensor); + + // Draw bounding box + if (inDrawSettings.mDrawBoundingBox) + inRenderer->DrawWireBox(body->mBounds, color); + + // Draw center of mass transform + if (inDrawSettings.mDrawCenterOfMassTransform) + inRenderer->DrawCoordinateSystem(body->GetCenterOfMassTransform(), 0.2f); + + // Draw world transform + if (inDrawSettings.mDrawWorldTransform) + inRenderer->DrawCoordinateSystem(body->GetWorldTransform(), 0.2f); + + // Draw world space linear and angular velocity + if (inDrawSettings.mDrawVelocity) + { + RVec3 pos = body->GetCenterOfMassPosition(); + inRenderer->DrawArrow(pos, pos + body->GetLinearVelocity(), Color::sGreen, 0.1f); + inRenderer->DrawArrow(pos, pos + body->GetAngularVelocity(), Color::sRed, 0.1f); + } + + if (inDrawSettings.mDrawMassAndInertia && body->IsDynamic()) + { + const MotionProperties *mp = body->GetMotionProperties(); + if (mp->GetInverseMass() > 0.0f + && !Vec3::sEquals(mp->GetInverseInertiaDiagonal(), Vec3::sZero()).TestAnyXYZTrue()) + { + // Invert mass again + float mass = 1.0f / mp->GetInverseMass(); + + // Invert diagonal again + Vec3 diagonal = mp->GetInverseInertiaDiagonal().Reciprocal(); + + // Determine how big of a box has the equivalent inertia + Vec3 box_size = MassProperties::sGetEquivalentSolidBoxSize(mass, diagonal); + + // Draw box with equivalent inertia + inRenderer->DrawWireBox(body->GetCenterOfMassTransform() * Mat44::sRotation(mp->GetInertiaRotation()), AABox(-0.5f * box_size, 0.5f * box_size), Color::sOrange); + + // Draw mass + inRenderer->DrawText3D(body->GetCenterOfMassPosition(), StringFormat("%.2f", (double)mass), Color::sOrange, 0.2f); + } + } + + if (inDrawSettings.mDrawSleepStats && body->IsDynamic() && body->IsActive()) + { + // Draw stats to know which bodies could go to sleep + String text = StringFormat("t: %.1f", (double)body->mMotionProperties->mSleepTestTimer); + uint8 g = uint8(Clamp(255.0f * body->mMotionProperties->mSleepTestTimer / inPhysicsSettings.mTimeBeforeSleep, 0.0f, 255.0f)); + Color sleep_color = Color(0, 255 - g, g); + inRenderer->DrawText3D(body->GetCenterOfMassPosition(), text, sleep_color, 0.2f); + for (int i = 0; i < 3; ++i) + inRenderer->DrawWireSphere(JPH_IF_DOUBLE_PRECISION(body->mMotionProperties->GetSleepTestOffset() +) body->mMotionProperties->mSleepTestSpheres[i].GetCenter(), body->mMotionProperties->mSleepTestSpheres[i].GetRadius(), sleep_color); + } + + if (body->IsSoftBody()) + { + const SoftBodyMotionProperties *mp = static_cast(body->GetMotionProperties()); + RMat44 com = body->GetCenterOfMassTransform(); + + if (inDrawSettings.mDrawSoftBodyVertices) + mp->DrawVertices(inRenderer, com); + + if (inDrawSettings.mDrawSoftBodyVertexVelocities) + mp->DrawVertexVelocities(inRenderer, com); + + if (inDrawSettings.mDrawSoftBodyEdgeConstraints) + mp->DrawEdgeConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor); + + if (inDrawSettings.mDrawSoftBodyBendConstraints) + mp->DrawBendConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor); + + if (inDrawSettings.mDrawSoftBodyVolumeConstraints) + mp->DrawVolumeConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor); + + if (inDrawSettings.mDrawSoftBodySkinConstraints) + mp->DrawSkinConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor); + + if (inDrawSettings.mDrawSoftBodyLRAConstraints) + mp->DrawLRAConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor); + + if (inDrawSettings.mDrawSoftBodyPredictedBounds) + mp->DrawPredictedBounds(inRenderer, com); + } + } + + UnlockAllBodies(); +} +#endif // JPH_DEBUG_RENDERER + +void BodyManager::InvalidateContactCacheForBody(Body &ioBody) +{ + // If this is the first time we flip the collision cache invalid flag, we need to add it to an internal list to ensure we reset the flag at the end of the physics update + if (ioBody.InvalidateContactCacheInternal()) + { + lock_guard lock(mBodiesCacheInvalidMutex); + mBodiesCacheInvalid.push_back(ioBody.GetID()); + } +} + +void BodyManager::ValidateContactCacheForAllBodies() +{ + lock_guard lock(mBodiesCacheInvalidMutex); + + for (const BodyID &b : mBodiesCacheInvalid) + { + // The body may have been removed between the call to InvalidateContactCacheForBody and this call, so check if it still exists + Body *body = TryGetBody(b); + if (body != nullptr) + body->ValidateContactCacheInternal(); + } + mBodiesCacheInvalid.clear(); +} + +#ifdef JPH_DEBUG +void BodyManager::ValidateActiveBodyBounds() +{ + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + for (uint type = 0; type < cBodyTypeCount; ++type) + for (BodyID *id = mActiveBodies[type], *id_end = mActiveBodies[type] + mNumActiveBodies[type].load(memory_order_relaxed); id < id_end; ++id) + { + const Body *body = mBodies[id->GetIndex()]; + AABox cached = body->GetWorldSpaceBounds(); + AABox calculated = body->GetShape()->GetWorldSpaceBounds(body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f)); + JPH_ASSERT(cached == calculated); + } +} +#endif // JPH_DEBUG + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.h new file mode 100644 index 000000000000..518574707a00 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.h @@ -0,0 +1,377 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +// Classes +class BodyCreationSettings; +class SoftBodyCreationSettings; +class BodyActivationListener; +class StateRecorderFilter; +struct PhysicsSettings; +#ifdef JPH_DEBUG_RENDERER +class DebugRenderer; +class BodyDrawFilter; +#endif // JPH_DEBUG_RENDERER + +#ifdef JPH_DEBUG_RENDERER + +/// Defines how to color soft body constraints +enum class ESoftBodyConstraintColor +{ + ConstraintType, /// Draw different types of constraints in different colors + ConstraintGroup, /// Draw constraints in the same group in the same color, non-parallel group will be red + ConstraintOrder, /// Draw constraints in the same group in the same color, non-parallel group will be red, and order within each group will be indicated with gradient +}; + +#endif // JPH_DEBUG_RENDERER + +/// Array of bodies +using BodyVector = Array; + +/// Array of body ID's +using BodyIDVector = Array; + +/// Class that contains all bodies +class JPH_EXPORT BodyManager : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Destructor + ~BodyManager(); + + /// Initialize the manager + void Init(uint inMaxBodies, uint inNumBodyMutexes, const BroadPhaseLayerInterface &inLayerInterface); + + /// Gets the current amount of bodies that are in the body manager + uint GetNumBodies() const; + + /// Gets the max bodies that we can support + uint GetMaxBodies() const { return uint(mBodies.capacity()); } + + /// Helper struct that counts the number of bodies of each type + struct BodyStats + { + uint mNumBodies = 0; ///< Total number of bodies in the body manager + uint mMaxBodies = 0; ///< Max allowed number of bodies in the body manager (as configured in Init(...)) + + uint mNumBodiesStatic = 0; ///< Number of static bodies + + uint mNumBodiesDynamic = 0; ///< Number of dynamic bodies + uint mNumActiveBodiesDynamic = 0; ///< Number of dynamic bodies that are currently active + + uint mNumBodiesKinematic = 0; ///< Number of kinematic bodies + uint mNumActiveBodiesKinematic = 0; ///< Number of kinematic bodies that are currently active + + uint mNumSoftBodies = 0; ///< Number of soft bodies + uint mNumActiveSoftBodies = 0; ///< Number of soft bodies that are currently active + }; + + /// Get stats about the bodies in the body manager (slow, iterates through all bodies) + BodyStats GetBodyStats() const; + + /// Create a body using creation settings. The returned body will not be part of the body manager yet. + Body * AllocateBody(const BodyCreationSettings &inBodyCreationSettings) const; + + /// Create a soft body using creation settings. The returned body will not be part of the body manager yet. + Body * AllocateSoftBody(const SoftBodyCreationSettings &inSoftBodyCreationSettings) const; + + /// Free a body that has not been added to the body manager yet (if it has, use DestroyBodies). + void FreeBody(Body *inBody) const; + + /// Add a body to the body manager, assigning it the next available ID. Returns false if no more IDs are available. + bool AddBody(Body *ioBody); + + /// Add a body to the body manager, assigning it a custom ID. Returns false if the ID is not valid. + bool AddBodyWithCustomID(Body *ioBody, const BodyID &inBodyID); + + /// Remove a list of bodies from the body manager + void RemoveBodies(const BodyID *inBodyIDs, int inNumber, Body **outBodies); + + /// Remove a set of bodies from the body manager and frees them. + void DestroyBodies(const BodyID *inBodyIDs, int inNumber); + + /// Activate a list of bodies. + /// This function should only be called when an exclusive lock for the bodies are held. + void ActivateBodies(const BodyID *inBodyIDs, int inNumber); + + /// Deactivate a list of bodies. + /// This function should only be called when an exclusive lock for the bodies are held. + void DeactivateBodies(const BodyID *inBodyIDs, int inNumber); + + /// Update the motion quality for a body + void SetMotionQuality(Body &ioBody, EMotionQuality inMotionQuality); + + /// Get copy of the list of active bodies under protection of a lock. + void GetActiveBodies(EBodyType inType, BodyIDVector &outBodyIDs) const; + + /// Get the list of active bodies. Note: Not thread safe. The active bodies list can change at any moment. + const BodyID * GetActiveBodiesUnsafe(EBodyType inType) const { return mActiveBodies[int(inType)]; } + + /// Get the number of active bodies. + uint32 GetNumActiveBodies(EBodyType inType) const { return mNumActiveBodies[int(inType)].load(memory_order_acquire); } + + /// Get the number of active bodies that are using continuous collision detection + uint32 GetNumActiveCCDBodies() const { return mNumActiveCCDBodies; } + + /// Listener that is notified whenever a body is activated/deactivated + void SetBodyActivationListener(BodyActivationListener *inListener); + BodyActivationListener * GetBodyActivationListener() const { return mActivationListener; } + + /// Check if this is a valid body pointer. When a body is freed the memory that the pointer occupies is reused to store a freelist. + static inline bool sIsValidBodyPointer(const Body *inBody) { return (uintptr_t(inBody) & cIsFreedBody) == 0; } + + /// Get all bodies. Note that this can contain invalid body pointers, call sIsValidBodyPointer to check. + const BodyVector & GetBodies() const { return mBodies; } + + /// Get all bodies. Note that this can contain invalid body pointers, call sIsValidBodyPointer to check. + BodyVector & GetBodies() { return mBodies; } + + /// Get all body IDs under the protection of a lock + void GetBodyIDs(BodyIDVector &outBodies) const; + + /// Access a body (not protected by lock) + const Body & GetBody(const BodyID &inID) const { return *mBodies[inID.GetIndex()]; } + + /// Access a body (not protected by lock) + Body & GetBody(const BodyID &inID) { return *mBodies[inID.GetIndex()]; } + + /// Access a body, will return a nullptr if the body ID is no longer valid (not protected by lock) + const Body * TryGetBody(const BodyID &inID) const + { + uint32 idx = inID.GetIndex(); + if (idx >= mBodies.size()) + return nullptr; + + const Body *body = mBodies[idx]; + if (sIsValidBodyPointer(body) && body->GetID() == inID) + return body; + + return nullptr; + } + + /// Access a body, will return a nullptr if the body ID is no longer valid (not protected by lock) + Body * TryGetBody(const BodyID &inID) + { + uint32 idx = inID.GetIndex(); + if (idx >= mBodies.size()) + return nullptr; + + Body *body = mBodies[idx]; + if (sIsValidBodyPointer(body) && body->GetID() == inID) + return body; + + return nullptr; + } + + /// Access the mutex for a single body + SharedMutex & GetMutexForBody(const BodyID &inID) const { return mBodyMutexes.GetMutexByObjectIndex(inID.GetIndex()); } + + /// Bodies are protected using an array of mutexes (so a fixed number, not 1 per body). Each bit in this mask indicates a locked mutex. + using MutexMask = uint64; + + ///@name Batch body mutex access (do not use directly) + ///@{ + MutexMask GetAllBodiesMutexMask() const { return mBodyMutexes.GetNumMutexes() == sizeof(MutexMask) * 8? ~MutexMask(0) : (MutexMask(1) << mBodyMutexes.GetNumMutexes()) - 1; } + MutexMask GetMutexMask(const BodyID *inBodies, int inNumber) const; + void LockRead(MutexMask inMutexMask) const; + void UnlockRead(MutexMask inMutexMask) const; + void LockWrite(MutexMask inMutexMask) const; + void UnlockWrite(MutexMask inMutexMask) const; + ///@} + + /// Lock all bodies. This should only be done during PhysicsSystem::Update(). + void LockAllBodies() const; + + /// Unlock all bodies. This should only be done during PhysicsSystem::Update(). + void UnlockAllBodies() const; + + /// Function to update body's layer (should only be called by the BodyInterface since it also requires updating the broadphase) + inline void SetBodyObjectLayerInternal(Body &ioBody, ObjectLayer inLayer) const { ioBody.mObjectLayer = inLayer; ioBody.mBroadPhaseLayer = mBroadPhaseLayerInterface->GetBroadPhaseLayer(inLayer); } + + /// Set the Body::EFlags::InvalidateContactCache flag for the specified body. This means that the collision cache is invalid for any body pair involving that body until the next physics step. + void InvalidateContactCacheForBody(Body &ioBody); + + /// Reset the Body::EFlags::InvalidateContactCache flag for all bodies. All contact pairs in the contact cache will now by valid again. + void ValidateContactCacheForAllBodies(); + + /// Saving state for replay + void SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const; + + /// Restoring state for replay. Returns false if failed. + bool RestoreState(StateRecorder &inStream); + + /// Save the state of a single body for replay + void SaveBodyState(const Body &inBody, StateRecorder &inStream) const; + + /// Save the state of a single body for replay + void RestoreBodyState(Body &inBody, StateRecorder &inStream); + +#ifdef JPH_DEBUG_RENDERER + enum class EShapeColor + { + InstanceColor, ///< Random color per instance + ShapeTypeColor, ///< Convex = green, scaled = yellow, compound = orange, mesh = red + MotionTypeColor, ///< Static = grey, keyframed = green, dynamic = random color per instance + SleepColor, ///< Static = grey, keyframed = green, dynamic = yellow, sleeping = red + IslandColor, ///< Static = grey, active = random color per island, sleeping = light grey + MaterialColor, ///< Color as defined by the PhysicsMaterial of the shape + }; + + /// Draw settings + struct DrawSettings + { + bool mDrawGetSupportFunction = false; ///< Draw the GetSupport() function, used for convex collision detection + bool mDrawSupportDirection = false; ///< When drawing the support function, also draw which direction mapped to a specific support point + bool mDrawGetSupportingFace = false; ///< Draw the faces that were found colliding during collision detection + bool mDrawShape = true; ///< Draw the shapes of all bodies + bool mDrawShapeWireframe = false; ///< When mDrawShape is true and this is true, the shapes will be drawn in wireframe instead of solid. + EShapeColor mDrawShapeColor = EShapeColor::MotionTypeColor; ///< Coloring scheme to use for shapes + bool mDrawBoundingBox = false; ///< Draw a bounding box per body + bool mDrawCenterOfMassTransform = false; ///< Draw the center of mass for each body + bool mDrawWorldTransform = false; ///< Draw the world transform (which can be different than the center of mass) for each body + bool mDrawVelocity = false; ///< Draw the velocity vector for each body + bool mDrawMassAndInertia = false; ///< Draw the mass and inertia (as the box equivalent) for each body + bool mDrawSleepStats = false; ///< Draw stats regarding the sleeping algorithm of each body + bool mDrawSoftBodyVertices = false; ///< Draw the vertices of soft bodies + bool mDrawSoftBodyVertexVelocities = false; ///< Draw the velocities of the vertices of soft bodies + bool mDrawSoftBodyEdgeConstraints = false; ///< Draw the edge constraints of soft bodies + bool mDrawSoftBodyBendConstraints = false; ///< Draw the bend constraints of soft bodies + 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 mDrawSoftBodyPredictedBounds = false; ///< Draw the predicted bounds of soft bodies + ESoftBodyConstraintColor mDrawSoftBodyConstraintColor = ESoftBodyConstraintColor::ConstraintType; ///< Coloring scheme to use for soft body constraints + }; + + /// Draw the state of the bodies (debugging purposes) + void Draw(const DrawSettings &inSettings, const PhysicsSettings &inPhysicsSettings, DebugRenderer *inRenderer, const BodyDrawFilter *inBodyFilter = nullptr); +#endif // JPH_DEBUG_RENDERER + +#ifdef JPH_ENABLE_ASSERTS + /// Lock the active body list, asserts when Activate/DeactivateBody is called. + void SetActiveBodiesLocked(bool inLocked) { mActiveBodiesLocked = inLocked; } + + /// Per thread override of the locked state, to be used by the PhysicsSystem only! + class GrantActiveBodiesAccess + { + public: + inline GrantActiveBodiesAccess(bool inAllowActivation, bool inAllowDeactivation) + { + JPH_ASSERT(!sGetOverrideAllowActivation()); + sSetOverrideAllowActivation(inAllowActivation); + + JPH_ASSERT(!sGetOverrideAllowDeactivation()); + sSetOverrideAllowDeactivation(inAllowDeactivation); + } + + inline ~GrantActiveBodiesAccess() + { + sSetOverrideAllowActivation(false); + sSetOverrideAllowDeactivation(false); + } + }; +#endif + +#ifdef JPH_DEBUG + /// Validate if the cached bounding boxes are correct for all active bodies + void ValidateActiveBodyBounds(); +#endif // JPH_DEBUG + +private: + /// Increment and get the sequence number of the body +#ifdef JPH_COMPILER_CLANG + __attribute__((no_sanitize("implicit-conversion"))) // We intentionally overflow the uint8 sequence number +#endif + inline uint8 GetNextSequenceNumber(int inBodyIndex) { return ++mBodySequenceNumbers[inBodyIndex]; } + + /// Add a single body to mActiveBodies, note doesn't lock the active body mutex! + inline void AddBodyToActiveBodies(Body &ioBody); + + /// Remove a single body from mActiveBodies, note doesn't lock the active body mutex! + inline void RemoveBodyFromActiveBodies(Body &ioBody); + + /// Helper function to remove a body from the manager + JPH_INLINE Body * RemoveBodyInternal(const BodyID &inBodyID); + + /// Helper function to delete a body (which could actually be a BodyWithMotionProperties) + inline static void sDeleteBody(Body *inBody); + +#if defined(JPH_DEBUG) && defined(JPH_ENABLE_ASSERTS) + /// Function to check that the free list is not corrupted + void ValidateFreeList() const; +#endif // defined(JPH_DEBUG) && _defined(JPH_ENABLE_ASSERTS) + + /// List of pointers to all bodies. Contains invalid pointers for deleted bodies, check with sIsValidBodyPointer. Note that this array is reserved to the max bodies that is passed in the Init function so that adding bodies will not reallocate the array. + BodyVector mBodies; + + /// Current number of allocated bodies + uint mNumBodies = 0; + + /// Indicates that there are no more freed body IDs + static constexpr uintptr_t cBodyIDFreeListEnd = ~uintptr_t(0); + + /// Bit that indicates a pointer in mBodies is actually the index of the next freed body. We use the lowest bit because we know that Bodies need to be 16 byte aligned so addresses can never end in a 1 bit. + static constexpr uintptr_t cIsFreedBody = uintptr_t(1); + + /// Amount of bits to shift to get an index to the next freed body + static constexpr uint cFreedBodyIndexShift = 1; + + /// Index of first entry in mBodies that is unused + uintptr_t mBodyIDFreeListStart = cBodyIDFreeListEnd; + + /// Protects mBodies array (but not the bodies it points to), mNumBodies and mBodyIDFreeListStart + mutable Mutex mBodiesMutex; + + /// An array of mutexes protecting the bodies in the mBodies array + using BodyMutexes = MutexArray; + mutable BodyMutexes mBodyMutexes; + + /// List of next sequence number for a body ID + Array mBodySequenceNumbers; + + /// Mutex that protects the mActiveBodies array + mutable Mutex mActiveBodiesMutex; + + /// List of all active dynamic bodies (size is equal to max amount of bodies) + BodyID * mActiveBodies[cBodyTypeCount] = { }; + + /// How many bodies there are in the list of active bodies + atomic mNumActiveBodies[cBodyTypeCount] = { }; + + /// How many of the active bodies have continuous collision detection enabled + uint32 mNumActiveCCDBodies = 0; + + /// Mutex that protects the mBodiesCacheInvalid array + mutable Mutex mBodiesCacheInvalidMutex; + + /// List of all bodies that should have their cache invalidated + BodyIDVector mBodiesCacheInvalid; + + /// Listener that is notified whenever a body is activated/deactivated + BodyActivationListener * mActivationListener = nullptr; + + /// Cached broadphase layer interface + const BroadPhaseLayerInterface *mBroadPhaseLayerInterface = nullptr; + +#ifdef JPH_ENABLE_ASSERTS + static bool sGetOverrideAllowActivation(); + static void sSetOverrideAllowActivation(bool inValue); + + static bool sGetOverrideAllowDeactivation(); + static void sSetOverrideAllowDeactivation(bool inValue); + + /// Debug system that tries to limit changes to active bodies during the PhysicsSystem::Update() + bool mActiveBodiesLocked = false; +#endif +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyPair.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyPair.h new file mode 100644 index 000000000000..8ac849a49a83 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyPair.h @@ -0,0 +1,36 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Structure that holds a body pair +struct alignas(uint64) BodyPair +{ + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + BodyPair() = default; + BodyPair(BodyID inA, BodyID inB) : mBodyA(inA), mBodyB(inB) { } + + /// Equals operator + bool operator == (const BodyPair &inRHS) const { return *reinterpret_cast(this) == *reinterpret_cast(&inRHS); } + + /// Smaller than operator, used for consistently ordering body pairs + bool operator < (const BodyPair &inRHS) const { return *reinterpret_cast(this) < *reinterpret_cast(&inRHS); } + + /// Get the hash value of this object + uint64 GetHash() const { return Hash64(*reinterpret_cast(this)); } + + BodyID mBodyA; + BodyID mBodyB; +}; + +static_assert(sizeof(BodyPair) == sizeof(uint64), "Mismatch in class size"); + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyType.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyType.h new file mode 100644 index 000000000000..984af06ad23a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyType.h @@ -0,0 +1,19 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Type of body +enum class EBodyType : uint8 +{ + RigidBody, ///< Rigid body consisting of a rigid shape + SoftBody, ///< Soft body consisting of a deformable shape +}; + +/// How many types of bodies there are +static constexpr uint cBodyTypeCount = 2; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MassProperties.cpp b/thirdparty/jolt_physics/Jolt/Physics/Body/MassProperties.cpp new file mode 100644 index 000000000000..91df6b0df3a2 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MassProperties.cpp @@ -0,0 +1,185 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(MassProperties) +{ + JPH_ADD_ATTRIBUTE(MassProperties, mMass) + JPH_ADD_ATTRIBUTE(MassProperties, mInertia) +} + +bool MassProperties::DecomposePrincipalMomentsOfInertia(Mat44 &outRotation, Vec3 &outDiagonal) const +{ + // Using eigendecomposition to get the principal components of the inertia tensor + // See: https://en.wikipedia.org/wiki/Eigendecomposition_of_a_matrix + Matrix<3, 3> inertia; + inertia.CopyPart(mInertia, 0, 0, 3, 3, 0, 0); + Matrix<3, 3> eigen_vec = Matrix<3, 3>::sIdentity(); + Vector<3> eigen_val; + if (!EigenValueSymmetric(inertia, eigen_vec, eigen_val)) + return false; + + // Sort so that the biggest value goes first + int indices[] = { 0, 1, 2 }; + InsertionSort(indices, indices + 3, [&eigen_val](int inLeft, int inRight) { return eigen_val[inLeft] > eigen_val[inRight]; }); + + // Convert to a regular Mat44 and Vec3 + outRotation = Mat44::sIdentity(); + for (int i = 0; i < 3; ++i) + { + outRotation.SetColumn3(i, Vec3(reinterpret_cast(eigen_vec.GetColumn(indices[i])))); + outDiagonal.SetComponent(i, eigen_val[indices[i]]); + } + + // Make sure that the rotation matrix is a right handed matrix + if (outRotation.GetAxisX().Cross(outRotation.GetAxisY()).Dot(outRotation.GetAxisZ()) < 0.0f) + outRotation.SetAxisZ(-outRotation.GetAxisZ()); + +#ifdef JPH_ENABLE_ASSERTS + // Validate that the solution is correct, for each axis we want to make sure that the difference in inertia is + // smaller than some fraction of the inertia itself in that axis + Mat44 new_inertia = outRotation * Mat44::sScale(outDiagonal) * outRotation.Inversed(); + for (int i = 0; i < 3; ++i) + JPH_ASSERT(new_inertia.GetColumn3(i).IsClose(mInertia.GetColumn3(i), mInertia.GetColumn3(i).LengthSq() * 1.0e-10f)); +#endif + + return true; +} + +void MassProperties::SetMassAndInertiaOfSolidBox(Vec3Arg inBoxSize, float inDensity) +{ + // Calculate mass + mMass = inBoxSize.GetX() * inBoxSize.GetY() * inBoxSize.GetZ() * inDensity; + + // Calculate inertia + Vec3 size_sq = inBoxSize * inBoxSize; + Vec3 scale = (size_sq.Swizzle() + size_sq.Swizzle()) * (mMass / 12.0f); + mInertia = Mat44::sScale(scale); +} + +void MassProperties::ScaleToMass(float inMass) +{ + if (mMass > 0.0f) + { + // Calculate how much we have to scale the inertia tensor + float mass_scale = inMass / mMass; + + // Update mass + mMass = inMass; + + // Update inertia tensor + for (int i = 0; i < 3; ++i) + mInertia.SetColumn4(i, mInertia.GetColumn4(i) * mass_scale); + } + else + { + // Just set the mass + mMass = inMass; + } +} + +Vec3 MassProperties::sGetEquivalentSolidBoxSize(float inMass, Vec3Arg inInertiaDiagonal) +{ + // Moment of inertia of a solid box has diagonal: + // mass / 12 * [size_y^2 + size_z^2, size_x^2 + size_z^2, size_x^2 + size_y^2] + // Solving for size_x, size_y and size_y (diagonal and mass are known): + Vec3 diagonal = inInertiaDiagonal * (12.0f / inMass); + return Vec3(sqrt(0.5f * (-diagonal[0] + diagonal[1] + diagonal[2])), sqrt(0.5f * (diagonal[0] - diagonal[1] + diagonal[2])), sqrt(0.5f * (diagonal[0] + diagonal[1] - diagonal[2]))); +} + +void MassProperties::Scale(Vec3Arg inScale) +{ + // See: https://en.wikipedia.org/wiki/Moment_of_inertia#Inertia_tensor + // The diagonal of the inertia tensor can be calculated like this: + // Ixx = sum_{k = 1 to n}(m_k * (y_k^2 + z_k^2)) + // Iyy = sum_{k = 1 to n}(m_k * (x_k^2 + z_k^2)) + // Izz = sum_{k = 1 to n}(m_k * (x_k^2 + y_k^2)) + // + // We want to isolate the terms x_k, y_k and z_k: + // d = [0.5, 0.5, 0.5].[Ixx, Iyy, Izz] + // [sum_{k = 1 to n}(m_k * x_k^2), sum_{k = 1 to n}(m_k * y_k^2), sum_{k = 1 to n}(m_k * z_k^2)] = [d, d, d] - [Ixx, Iyy, Izz] + Vec3 diagonal = mInertia.GetDiagonal3(); + Vec3 xyz_sq = Vec3::sReplicate(Vec3::sReplicate(0.5f).Dot(diagonal)) - diagonal; + + // When scaling a shape these terms change like this: + // sum_{k = 1 to n}(m_k * (scale_x * x_k)^2) = scale_x^2 * sum_{k = 1 to n}(m_k * x_k^2) + // Same for y_k and z_k + // Using these terms we can calculate the new diagonal of the inertia tensor: + Vec3 xyz_scaled_sq = inScale * inScale * xyz_sq; + float i_xx = xyz_scaled_sq.GetY() + xyz_scaled_sq.GetZ(); + float i_yy = xyz_scaled_sq.GetX() + xyz_scaled_sq.GetZ(); + float i_zz = xyz_scaled_sq.GetX() + xyz_scaled_sq.GetY(); + + // The off diagonal elements are calculated like: + // Ixy = -sum_{k = 1 to n}(x_k y_k) + // Ixz = -sum_{k = 1 to n}(x_k z_k) + // Iyz = -sum_{k = 1 to n}(y_k z_k) + // Scaling these is simple: + float i_xy = inScale.GetX() * inScale.GetY() * mInertia(0, 1); + float i_xz = inScale.GetX() * inScale.GetZ() * mInertia(0, 2); + float i_yz = inScale.GetY() * inScale.GetZ() * mInertia(1, 2); + + // Update inertia tensor + mInertia(0, 0) = i_xx; + mInertia(0, 1) = i_xy; + mInertia(1, 0) = i_xy; + mInertia(1, 1) = i_yy; + mInertia(0, 2) = i_xz; + mInertia(2, 0) = i_xz; + mInertia(1, 2) = i_yz; + mInertia(2, 1) = i_yz; + mInertia(2, 2) = i_zz; + + // Mass scales linear with volume (note that the scaling can be negative and we don't want the mass to become negative) + float mass_scale = abs(inScale.GetX() * inScale.GetY() * inScale.GetZ()); + mMass *= mass_scale; + + // Inertia scales linear with mass. This updates the m_k terms above. + mInertia *= mass_scale; + + // Ensure that the bottom right element is a 1 again + mInertia(3, 3) = 1.0f; +} + +void MassProperties::Rotate(Mat44Arg inRotation) +{ + mInertia = inRotation.Multiply3x3(mInertia).Multiply3x3RightTransposed(inRotation); +} + +void MassProperties::Translate(Vec3Arg inTranslation) +{ + // Transform the inertia using the parallel axis theorem: I' = I + m * (translation^2 E - translation translation^T) + // Where I is the original body's inertia and E the identity matrix + // See: https://en.wikipedia.org/wiki/Parallel_axis_theorem + mInertia += mMass * (Mat44::sScale(inTranslation.Dot(inTranslation)) - Mat44::sOuterProduct(inTranslation, inTranslation)); + + // Ensure that inertia is a 3x3 matrix, adding inertias causes the bottom right element to change + mInertia.SetColumn4(3, Vec4(0, 0, 0, 1)); +} + +void MassProperties::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mMass); + inStream.Write(mInertia); +} + +void MassProperties::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mMass); + inStream.Read(mInertia); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MassProperties.h b/thirdparty/jolt_physics/Jolt/Physics/Body/MassProperties.h new file mode 100644 index 000000000000..c2bfbffb2e10 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MassProperties.h @@ -0,0 +1,58 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; + +/// Describes the mass and inertia properties of a body. Used during body construction only. +class JPH_EXPORT MassProperties +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, MassProperties) + +public: + /// Using eigendecomposition, decompose the inertia tensor into a diagonal matrix D and a right-handed rotation matrix R so that the inertia tensor is \f$R \: D \: R^{-1}\f$. + /// @see https://en.wikipedia.org/wiki/Moment_of_inertia section 'Principal axes' + /// @param outRotation The rotation matrix R + /// @param outDiagonal The diagonal of the diagonal matrix D + /// @return True if successful, false if failed + bool DecomposePrincipalMomentsOfInertia(Mat44 &outRotation, Vec3 &outDiagonal) const; + + /// Set the mass and inertia of a box with edge size inBoxSize and density inDensity + void SetMassAndInertiaOfSolidBox(Vec3Arg inBoxSize, float inDensity); + + /// Set the mass and scale the inertia tensor to match the mass + void ScaleToMass(float inMass); + + /// Calculates the size of the solid box that has an inertia tensor diagonal inInertiaDiagonal + static Vec3 sGetEquivalentSolidBoxSize(float inMass, Vec3Arg inInertiaDiagonal); + + /// Rotate the inertia by 3x3 matrix inRotation + void Rotate(Mat44Arg inRotation); + + /// Translate the inertia by a vector inTranslation + void Translate(Vec3Arg inTranslation); + + /// Scale the mass and inertia by inScale, note that elements can be < 0 to flip the shape + void Scale(Vec3Arg inScale); + + /// Saves the state of this object in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restore the state of this object from inStream. + void RestoreBinaryState(StreamIn &inStream); + + /// Mass of the shape (kg) + float mMass = 0.0f; + + /// Inertia tensor of the shape (kg m^2) + Mat44 mInertia = Mat44::sZero(); +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.cpp b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.cpp new file mode 100644 index 000000000000..96aba179e119 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.cpp @@ -0,0 +1,92 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +void MotionProperties::SetMassProperties(EAllowedDOFs inAllowedDOFs, const MassProperties &inMassProperties) +{ + // Store allowed DOFs + mAllowedDOFs = inAllowedDOFs; + + // Decompose DOFs + uint allowed_translation_axis = uint(inAllowedDOFs) & 0b111; + uint allowed_rotation_axis = (uint(inAllowedDOFs) >> 3) & 0b111; + + // Set inverse mass + if (allowed_translation_axis == 0) + { + // No translation possible + mInvMass = 0.0f; + } + else + { + JPH_ASSERT(inMassProperties.mMass > 0.0f, "Invalid mass. " + "Some shapes like MeshShape or TriangleShape cannot calculate mass automatically, " + "in this case you need to provide it by setting BodyCreationSettings::mOverrideMassProperties and mMassPropertiesOverride."); + mInvMass = 1.0f / inMassProperties.mMass; + } + + if (allowed_rotation_axis == 0) + { + // No rotation possible + mInvInertiaDiagonal = Vec3::sZero(); + mInertiaRotation = Quat::sIdentity(); + } + else + { + // Set inverse inertia + Mat44 rotation; + Vec3 diagonal; + if (inMassProperties.DecomposePrincipalMomentsOfInertia(rotation, diagonal) + && !diagonal.IsNearZero()) + { + mInvInertiaDiagonal = diagonal.Reciprocal(); + mInertiaRotation = rotation.GetQuaternion(); + } + else + { + // Failed! Fall back to inertia tensor of sphere with radius 1. + mInvInertiaDiagonal = Vec3::sReplicate(2.5f * mInvMass); + mInertiaRotation = Quat::sIdentity(); + } + } + + JPH_ASSERT(mInvMass != 0.0f || mInvInertiaDiagonal != Vec3::sZero(), "Can't lock all axes, use a static body for this. This will crash with a division by zero later!"); +} + +void MotionProperties::SaveState(StateRecorder &inStream) const +{ + // Only write properties that can change at runtime + inStream.Write(mLinearVelocity); + inStream.Write(mAngularVelocity); + inStream.Write(mForce); + inStream.Write(mTorque); +#ifdef JPH_DOUBLE_PRECISION + inStream.Write(mSleepTestOffset); +#endif // JPH_DOUBLE_PRECISION + inStream.Write(mSleepTestSpheres); + inStream.Write(mSleepTestTimer); + inStream.Write(mAllowSleeping); +} + +void MotionProperties::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mLinearVelocity); + inStream.Read(mAngularVelocity); + inStream.Read(mForce); + inStream.Read(mTorque); +#ifdef JPH_DOUBLE_PRECISION + inStream.Read(mSleepTestOffset); +#endif // JPH_DOUBLE_PRECISION + inStream.Read(mSleepTestSpheres); + inStream.Read(mSleepTestTimer); + inStream.Read(mAllowSleeping); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.h b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.h new file mode 100644 index 000000000000..d8a485b61129 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.h @@ -0,0 +1,282 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class StateRecorder; + +/// Enum that determines if an object can go to sleep +enum class ECanSleep +{ + CannotSleep = 0, ///< Object cannot go to sleep + CanSleep = 1, ///< Object can go to sleep +}; + +/// The Body class only keeps track of state for static bodies, the MotionProperties class keeps the additional state needed for a moving Body. It has a 1-on-1 relationship with the body. +class JPH_EXPORT MotionProperties +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Motion quality, or how well it detects collisions when it has a high velocity + EMotionQuality GetMotionQuality() const { return mMotionQuality; } + + /// Get the allowed degrees of freedom that this body has (this can be changed by calling SetMassProperties) + inline EAllowedDOFs GetAllowedDOFs() const { return mAllowedDOFs; } + + /// If this body can go to sleep. + inline bool GetAllowSleeping() const { return mAllowSleeping; } + + /// Get world space linear velocity of the center of mass + inline Vec3 GetLinearVelocity() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::Read)); return mLinearVelocity; } + + /// Set world space linear velocity of the center of mass + void SetLinearVelocity(Vec3Arg inLinearVelocity) { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); JPH_ASSERT(inLinearVelocity.Length() <= mMaxLinearVelocity); mLinearVelocity = LockTranslation(inLinearVelocity); } + + /// Set world space linear velocity of the center of mass, will make sure the value is clamped against the maximum linear velocity + void SetLinearVelocityClamped(Vec3Arg inLinearVelocity) { mLinearVelocity = LockTranslation(inLinearVelocity); ClampLinearVelocity(); } + + /// Get world space angular velocity of the center of mass + inline Vec3 GetAngularVelocity() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::Read)); return mAngularVelocity; } + + /// Set world space angular velocity of the center of mass + void SetAngularVelocity(Vec3Arg inAngularVelocity) { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); JPH_ASSERT(inAngularVelocity.Length() <= mMaxAngularVelocity); mAngularVelocity = LockAngular(inAngularVelocity); } + + /// Set world space angular velocity of the center of mass, will make sure the value is clamped against the maximum angular velocity + void SetAngularVelocityClamped(Vec3Arg inAngularVelocity) { mAngularVelocity = LockAngular(inAngularVelocity); ClampAngularVelocity(); } + + /// Set velocity of body such that it will be rotate/translate by inDeltaPosition/Rotation in inDeltaTime seconds. + inline void MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRotation, float inDeltaTime); + + ///@name Velocity limits + ///@{ + + /// Maximum linear velocity that a body can achieve. Used to prevent the system from exploding. + inline float GetMaxLinearVelocity() const { return mMaxLinearVelocity; } + inline void SetMaxLinearVelocity(float inLinearVelocity) { JPH_ASSERT(inLinearVelocity >= 0.0f); mMaxLinearVelocity = inLinearVelocity; } + + /// Maximum angular velocity that a body can achieve. Used to prevent the system from exploding. + inline float GetMaxAngularVelocity() const { return mMaxAngularVelocity; } + inline void SetMaxAngularVelocity(float inAngularVelocity) { JPH_ASSERT(inAngularVelocity >= 0.0f); mMaxAngularVelocity = inAngularVelocity; } + ///@} + + /// Clamp velocity according to limit + inline void ClampLinearVelocity(); + inline void ClampAngularVelocity(); + + /// Get linear damping: dv/dt = -c * v. c must be between 0 and 1 but is usually close to 0. + inline float GetLinearDamping() const { return mLinearDamping; } + void SetLinearDamping(float inLinearDamping) { JPH_ASSERT(inLinearDamping >= 0.0f); mLinearDamping = inLinearDamping; } + + /// Get angular damping: dw/dt = -c * w. c must be between 0 and 1 but is usually close to 0. + inline float GetAngularDamping() const { return mAngularDamping; } + void SetAngularDamping(float inAngularDamping) { JPH_ASSERT(inAngularDamping >= 0.0f); mAngularDamping = inAngularDamping; } + + /// Get gravity factor (1 = normal gravity, 0 = no gravity) + inline float GetGravityFactor() const { return mGravityFactor; } + void SetGravityFactor(float inGravityFactor) { mGravityFactor = inGravityFactor; } + + /// Set the mass and inertia tensor + void SetMassProperties(EAllowedDOFs inAllowedDOFs, const MassProperties &inMassProperties); + + /// Get inverse mass (1 / mass). Should only be called on a dynamic object (static or kinematic bodies have infinite mass so should be treated as 1 / mass = 0) + inline float GetInverseMass() const { JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); return mInvMass; } + inline float GetInverseMassUnchecked() const { return mInvMass; } + + /// Set the inverse mass (1 / mass). + /// Note that mass and inertia are linearly related (e.g. inertia of a sphere with mass m and radius r is \f$2/5 \: m \: r^2\f$). + /// If you change mass, inertia should probably change as well. You can use ScaleToMass to update mass and inertia at the same time. + /// If all your translation degrees of freedom are restricted, make sure this is zero (see EAllowedDOFs). + void SetInverseMass(float inInverseMass) { mInvMass = inInverseMass; } + + /// Diagonal of inverse inertia matrix: D. Should only be called on a dynamic object (static or kinematic bodies have infinite mass so should be treated as D = 0) + inline Vec3 GetInverseInertiaDiagonal() const { JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); return mInvInertiaDiagonal; } + + /// Rotation (R) that takes inverse inertia diagonal to local space: \f$I_{body}^{-1} = R \: D \: R^{-1}\f$ + inline Quat GetInertiaRotation() const { return mInertiaRotation; } + + /// Set the inverse inertia tensor in local space by setting the diagonal and the rotation: \f$I_{body}^{-1} = R \: D \: R^{-1}\f$. + /// Note that mass and inertia are linearly related (e.g. inertia of a sphere with mass m and radius r is \f$2/5 \: m \: r^2\f$). + /// If you change inertia, mass should probably change as well. You can use ScaleToMass to update mass and inertia at the same time. + /// If all your rotation degrees of freedom are restricted, make sure this is zero (see EAllowedDOFs). + void SetInverseInertia(Vec3Arg inDiagonal, QuatArg inRot) { mInvInertiaDiagonal = inDiagonal; mInertiaRotation = inRot; } + + /// Sets the mass to inMass and scale the inertia tensor based on the ratio between the old and new mass. + /// Note that this only works when the current mass is finite (i.e. the body is dynamic and translational degrees of freedom are not restricted). + void ScaleToMass(float inMass); + + /// Get inverse inertia matrix (\f$I_{body}^{-1}\f$). Will be a matrix of zeros for a static or kinematic object. + inline Mat44 GetLocalSpaceInverseInertia() const; + + /// Same as GetLocalSpaceInverseInertia() but doesn't check if the body is dynamic + inline Mat44 GetLocalSpaceInverseInertiaUnchecked() const; + + /// Get inverse inertia matrix (\f$I^{-1}\f$) for a given object rotation (translation will be ignored). Zero if object is static or kinematic. + inline Mat44 GetInverseInertiaForRotation(Mat44Arg inRotation) const; + + /// Multiply a vector with the inverse world space inertia tensor (\f$I_{world}^{-1}\f$). Zero if object is static or kinematic. + JPH_INLINE Vec3 MultiplyWorldSpaceInverseInertiaByVector(QuatArg inBodyRotation, Vec3Arg inV) const; + + /// Velocity of point inPoint (in center of mass space, e.g. on the surface of the body) of the body (unit: m/s) + JPH_INLINE Vec3 GetPointVelocityCOM(Vec3Arg inPointRelativeToCOM) const { return mLinearVelocity + mAngularVelocity.Cross(inPointRelativeToCOM); } + + // Get the total amount of force applied to the center of mass this time step (through Body::AddForce calls). Note that it will reset to zero after PhysicsSystem::Update. + JPH_INLINE Vec3 GetAccumulatedForce() const { return Vec3::sLoadFloat3Unsafe(mForce); } + + // Get the total amount of torque applied to the center of mass this time step (through Body::AddForce/Body::AddTorque calls). Note that it will reset to zero after PhysicsSystem::Update. + JPH_INLINE Vec3 GetAccumulatedTorque() const { return Vec3::sLoadFloat3Unsafe(mTorque); } + + // Reset the total accumulated force, note that this will be done automatically after every time step. + JPH_INLINE void ResetForce() { mForce = Float3(0, 0, 0); } + + // Reset the total accumulated torque, note that this will be done automatically after every time step. + JPH_INLINE void ResetTorque() { mTorque = Float3(0, 0, 0); } + + // Reset the current velocity and accumulated force and torque. + JPH_INLINE void ResetMotion() + { + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); + mLinearVelocity = mAngularVelocity = Vec3::sZero(); + mForce = mTorque = Float3(0, 0, 0); + } + + /// Returns a vector where the linear components that are not allowed by mAllowedDOFs are set to 0 and the rest to 0xffffffff + JPH_INLINE UVec4 GetLinearDOFsMask() const + { + UVec4 mask(uint32(EAllowedDOFs::TranslationX), uint32(EAllowedDOFs::TranslationY), uint32(EAllowedDOFs::TranslationZ), 0); + return UVec4::sEquals(UVec4::sAnd(UVec4::sReplicate(uint32(mAllowedDOFs)), mask), mask); + } + + /// Takes a translation vector inV and returns a vector where the components that are not allowed by mAllowedDOFs are set to 0 + JPH_INLINE Vec3 LockTranslation(Vec3Arg inV) const + { + return Vec3::sAnd(inV, Vec3(GetLinearDOFsMask().ReinterpretAsFloat())); + } + + /// Returns a vector where the angular components that are not allowed by mAllowedDOFs are set to 0 and the rest to 0xffffffff + JPH_INLINE UVec4 GetAngularDOFsMask() const + { + UVec4 mask(uint32(EAllowedDOFs::RotationX), uint32(EAllowedDOFs::RotationY), uint32(EAllowedDOFs::RotationZ), 0); + return UVec4::sEquals(UVec4::sAnd(UVec4::sReplicate(uint32(mAllowedDOFs)), mask), mask); + } + + /// Takes an angular velocity / torque vector inV and returns a vector where the components that are not allowed by mAllowedDOFs are set to 0 + JPH_INLINE Vec3 LockAngular(Vec3Arg inV) const + { + return Vec3::sAnd(inV, Vec3(GetAngularDOFsMask().ReinterpretAsFloat())); + } + + /// Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island. + void SetNumVelocityStepsOverride(uint inN) { JPH_ASSERT(inN < 256); mNumVelocityStepsOverride = uint8(inN); } + uint GetNumVelocityStepsOverride() const { return mNumVelocityStepsOverride; } + + /// Used only when this body is dynamic and colliding. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island. + void SetNumPositionStepsOverride(uint inN) { JPH_ASSERT(inN < 256); mNumPositionStepsOverride = uint8(inN); } + uint GetNumPositionStepsOverride() const { return mNumPositionStepsOverride; } + + //////////////////////////////////////////////////////////// + // FUNCTIONS BELOW THIS LINE ARE FOR INTERNAL USE ONLY + //////////////////////////////////////////////////////////// + + ///@name Update linear and angular velocity (used during constraint solving) + ///@{ + inline void AddLinearVelocityStep(Vec3Arg inLinearVelocityChange) { JPH_DET_LOG("AddLinearVelocityStep: " << inLinearVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mLinearVelocity = LockTranslation(mLinearVelocity + inLinearVelocityChange); JPH_ASSERT(!mLinearVelocity.IsNaN()); } + inline void SubLinearVelocityStep(Vec3Arg inLinearVelocityChange) { JPH_DET_LOG("SubLinearVelocityStep: " << inLinearVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mLinearVelocity = LockTranslation(mLinearVelocity - inLinearVelocityChange); JPH_ASSERT(!mLinearVelocity.IsNaN()); } + inline void AddAngularVelocityStep(Vec3Arg inAngularVelocityChange) { JPH_DET_LOG("AddAngularVelocityStep: " << inAngularVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mAngularVelocity += inAngularVelocityChange; JPH_ASSERT(!mAngularVelocity.IsNaN()); } + inline void SubAngularVelocityStep(Vec3Arg inAngularVelocityChange) { JPH_DET_LOG("SubAngularVelocityStep: " << inAngularVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mAngularVelocity -= inAngularVelocityChange; JPH_ASSERT(!mAngularVelocity.IsNaN()); } + ///@} + + /// Apply the gyroscopic force (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem) + inline void ApplyGyroscopicForceInternal(QuatArg inBodyRotation, float inDeltaTime); + + /// Apply all accumulated forces, torques and drag (should only be called by the PhysicsSystem) + inline void ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, Vec3Arg inGravity, float inDeltaTime); + + /// Access to the island index + uint32 GetIslandIndexInternal() const { return mIslandIndex; } + void SetIslandIndexInternal(uint32 inIndex) { mIslandIndex = inIndex; } + + /// Access to the index in the active bodies array + uint32 GetIndexInActiveBodiesInternal() const { return mIndexInActiveBodies; } + +#ifdef JPH_DOUBLE_PRECISION + inline DVec3 GetSleepTestOffset() const { return DVec3::sLoadDouble3Unsafe(mSleepTestOffset); } +#endif // JPH_DOUBLE_PRECISION + + /// Reset spheres to center around inPoints with radius 0 + inline void ResetSleepTestSpheres(const RVec3 *inPoints); + + /// Reset the sleep test timer without resetting the sleep test spheres + inline void ResetSleepTestTimer() { mSleepTestTimer = 0.0f; } + + /// Accumulate sleep time and return if a body can go to sleep + inline ECanSleep AccumulateSleepTime(float inDeltaTime, float inTimeBeforeSleep); + + /// Saving state for replay + void SaveState(StateRecorder &inStream) const; + + /// Restoring state for replay + void RestoreState(StateRecorder &inStream); + + static constexpr uint32 cInactiveIndex = uint32(-1); ///< Constant indicating that body is not active + +private: + friend class BodyManager; + friend class Body; + + // 1st cache line + // 16 byte aligned + Vec3 mLinearVelocity { Vec3::sZero() }; ///< World space linear velocity of the center of mass (m/s) + Vec3 mAngularVelocity { Vec3::sZero() }; ///< World space angular velocity (rad/s) + Vec3 mInvInertiaDiagonal; ///< Diagonal of inverse inertia matrix: D + Quat mInertiaRotation; ///< Rotation (R) that takes inverse inertia diagonal to local space: Ibody^-1 = R * D * R^-1 + + // 2nd cache line + // 4 byte aligned + Float3 mForce { 0, 0, 0 }; ///< Accumulated world space force (N). Note loaded through intrinsics so ensure that the 4 bytes after this are readable! + Float3 mTorque { 0, 0, 0 }; ///< Accumulated world space torque (N m). Note loaded through intrinsics so ensure that the 4 bytes after this are readable! + float mInvMass; ///< Inverse mass of the object (1/kg) + float mLinearDamping; ///< Linear damping: dv/dt = -c * v. c must be between 0 and 1 but is usually close to 0. + float mAngularDamping; ///< Angular damping: dw/dt = -c * w. c must be between 0 and 1 but is usually close to 0. + float mMaxLinearVelocity; ///< Maximum linear velocity that this body can reach (m/s) + float mMaxAngularVelocity; ///< Maximum angular velocity that this body can reach (rad/s) + float mGravityFactor; ///< Factor to multiply gravity with + uint32 mIndexInActiveBodies = cInactiveIndex; ///< If the body is active, this is the index in the active body list or cInactiveIndex if it is not active (note that there are 2 lists, one for rigid and one for soft bodies) + uint32 mIslandIndex = cInactiveIndex; ///< Index of the island that this body is part of, when the body has not yet been updated or is not active this is cInactiveIndex + + // 1 byte aligned + EMotionQuality mMotionQuality; ///< Motion quality, or how well it detects collisions when it has a high velocity + bool mAllowSleeping; ///< If this body can go to sleep + EAllowedDOFs mAllowedDOFs = EAllowedDOFs::All; ///< Allowed degrees of freedom for this body + uint8 mNumVelocityStepsOverride = 0; ///< Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island. + uint8 mNumPositionStepsOverride = 0; ///< Used only when this body is dynamic and colliding. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island. + + // 3rd cache line (least frequently used) + // 4 byte aligned (or 8 byte if running in double precision) +#ifdef JPH_DOUBLE_PRECISION + Double3 mSleepTestOffset; ///< mSleepTestSpheres are relative to this offset to prevent floating point inaccuracies. Warning: Loaded using sLoadDouble3Unsafe which will read 8 extra bytes. +#endif // JPH_DOUBLE_PRECISION + Sphere mSleepTestSpheres[3]; ///< Measure motion for 3 points on the body to see if it is resting: COM, COM + largest bounding box axis, COM + second largest bounding box axis + float mSleepTestTimer; ///< How long this body has been within the movement tolerance + +#ifdef JPH_ENABLE_ASSERTS + EBodyType mCachedBodyType; ///< Copied from Body::mBodyType and cached for asserting purposes + EMotionType mCachedMotionType; ///< Copied from Body::mMotionType and cached for asserting purposes +#endif +}; + +JPH_NAMESPACE_END + +#include "MotionProperties.inl" diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl new file mode 100644 index 000000000000..e9521f7444de --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl @@ -0,0 +1,178 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +void MotionProperties::MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRotation, float inDeltaTime) +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); + JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody); + JPH_ASSERT(mCachedMotionType != EMotionType::Static); + + // Calculate required linear velocity + mLinearVelocity = LockTranslation(inDeltaPosition / inDeltaTime); + + // Calculate required angular velocity + Vec3 axis; + float angle; + inDeltaRotation.GetAxisAngle(axis, angle); + mAngularVelocity = LockAngular(axis * (angle / inDeltaTime)); +} + +void MotionProperties::ClampLinearVelocity() +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); + + float len_sq = mLinearVelocity.LengthSq(); + JPH_ASSERT(isfinite(len_sq)); + if (len_sq > Square(mMaxLinearVelocity)) + mLinearVelocity *= mMaxLinearVelocity / sqrt(len_sq); +} + +void MotionProperties::ClampAngularVelocity() +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); + + float len_sq = mAngularVelocity.LengthSq(); + JPH_ASSERT(isfinite(len_sq)); + if (len_sq > Square(mMaxAngularVelocity)) + mAngularVelocity *= mMaxAngularVelocity / sqrt(len_sq); +} + +inline Mat44 MotionProperties::GetLocalSpaceInverseInertiaUnchecked() const +{ + Mat44 rotation = Mat44::sRotation(mInertiaRotation); + Mat44 rotation_mul_scale_transposed(mInvInertiaDiagonal.SplatX() * rotation.GetColumn4(0), mInvInertiaDiagonal.SplatY() * rotation.GetColumn4(1), mInvInertiaDiagonal.SplatZ() * rotation.GetColumn4(2), Vec4(0, 0, 0, 1)); + return rotation.Multiply3x3RightTransposed(rotation_mul_scale_transposed); +} + +inline void MotionProperties::ScaleToMass(float inMass) +{ + JPH_ASSERT(mInvMass > 0.0f, "Body must have finite mass"); + JPH_ASSERT(inMass > 0.0f, "New mass cannot be zero"); + + float new_inv_mass = 1.0f / inMass; + mInvInertiaDiagonal *= new_inv_mass / mInvMass; + mInvMass = new_inv_mass; +} + +inline Mat44 MotionProperties::GetLocalSpaceInverseInertia() const +{ + JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); + return GetLocalSpaceInverseInertiaUnchecked(); +} + +Mat44 MotionProperties::GetInverseInertiaForRotation(Mat44Arg inRotation) const +{ + JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); + + Mat44 rotation = inRotation.Multiply3x3(Mat44::sRotation(mInertiaRotation)); + Mat44 rotation_mul_scale_transposed(mInvInertiaDiagonal.SplatX() * rotation.GetColumn4(0), mInvInertiaDiagonal.SplatY() * rotation.GetColumn4(1), mInvInertiaDiagonal.SplatZ() * rotation.GetColumn4(2), Vec4(0, 0, 0, 1)); + Mat44 inverse_inertia = rotation.Multiply3x3RightTransposed(rotation_mul_scale_transposed); + + // We need to mask out both the rows and columns of DOFs that are not allowed + Vec4 angular_dofs_mask = GetAngularDOFsMask().ReinterpretAsFloat(); + inverse_inertia.SetColumn4(0, Vec4::sAnd(inverse_inertia.GetColumn4(0), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatX()))); + inverse_inertia.SetColumn4(1, Vec4::sAnd(inverse_inertia.GetColumn4(1), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatY()))); + inverse_inertia.SetColumn4(2, Vec4::sAnd(inverse_inertia.GetColumn4(2), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatZ()))); + + return inverse_inertia; +} + +Vec3 MotionProperties::MultiplyWorldSpaceInverseInertiaByVector(QuatArg inBodyRotation, Vec3Arg inV) const +{ + JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); + + // Mask out columns of DOFs that are not allowed + Vec3 angular_dofs_mask = Vec3(GetAngularDOFsMask().ReinterpretAsFloat()); + Vec3 v = Vec3::sAnd(inV, angular_dofs_mask); + + // Multiply vector by inverse inertia + Mat44 rotation = Mat44::sRotation(inBodyRotation * mInertiaRotation); + Vec3 result = rotation.Multiply3x3(mInvInertiaDiagonal * rotation.Multiply3x3Transposed(v)); + + // Mask out rows of DOFs that are not allowed + return Vec3::sAnd(result, angular_dofs_mask); +} + +void MotionProperties::ApplyGyroscopicForceInternal(QuatArg inBodyRotation, float inDeltaTime) +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); + JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody); + JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); + + // Calculate local space inertia tensor (a diagonal in local space) + UVec4 is_zero = Vec3::sEquals(mInvInertiaDiagonal, Vec3::sZero()); + Vec3 denominator = Vec3::sSelect(mInvInertiaDiagonal, Vec3::sReplicate(1.0f), is_zero); + Vec3 nominator = Vec3::sSelect(Vec3::sReplicate(1.0f), Vec3::sZero(), is_zero); + Vec3 local_inertia = nominator / denominator; // Avoid dividing by zero, inertia in this axis will be zero + + // 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_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 + // Calculate the new angular momentum by applying the gyroscopic force and make sure the new magnitude is the same as the old one + // to avoid introducing energy into the system due to the Euler step + Vec3 new_local_momentum = local_momentum - inDeltaTime * local_angular_velocity.Cross(local_momentum); + float new_local_momentum_len_sq = new_local_momentum.LengthSq(); + new_local_momentum = new_local_momentum_len_sq > 0.0f? new_local_momentum * sqrt(local_momentum.LengthSq() / new_local_momentum_len_sq) : Vec3::sZero(); + + // Convert back to world space angular velocity + mAngularVelocity = inertia_space_to_world_space * (mInvInertiaDiagonal * new_local_momentum); +} + +void MotionProperties::ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, Vec3Arg inGravity, float inDeltaTime) +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); + JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody); + JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); + + // Update linear velocity + mLinearVelocity = LockTranslation(mLinearVelocity + inDeltaTime * (mGravityFactor * inGravity + mInvMass * GetAccumulatedForce())); + + // Update angular velocity + mAngularVelocity += inDeltaTime * MultiplyWorldSpaceInverseInertiaByVector(inBodyRotation, GetAccumulatedTorque()); + + // Linear damping: dv/dt = -c * v + // Solution: v(t) = v(0) * e^(-c * t) or v2 = v1 * e^(-c * dt) + // Taylor expansion of e^(-c * dt) = 1 - c * dt + ... + // Since dt is usually in the order of 1/60 and c is a low number too this approximation is good enough + mLinearVelocity *= max(0.0f, 1.0f - mLinearDamping * inDeltaTime); + mAngularVelocity *= max(0.0f, 1.0f - mAngularDamping * inDeltaTime); + + // Clamp velocities + ClampLinearVelocity(); + ClampAngularVelocity(); +} + +void MotionProperties::ResetSleepTestSpheres(const RVec3 *inPoints) +{ +#ifdef JPH_DOUBLE_PRECISION + // Make spheres relative to the first point and initialize them to zero radius + DVec3 offset = inPoints[0]; + offset.StoreDouble3(&mSleepTestOffset); + mSleepTestSpheres[0] = Sphere(Vec3::sZero(), 0.0f); + for (int i = 1; i < 3; ++i) + mSleepTestSpheres[i] = Sphere(Vec3(inPoints[i] - offset), 0.0f); +#else + // Initialize the spheres to zero radius around the supplied points + for (int i = 0; i < 3; ++i) + mSleepTestSpheres[i] = Sphere(inPoints[i], 0.0f); +#endif + + mSleepTestTimer = 0.0f; +} + +ECanSleep MotionProperties::AccumulateSleepTime(float inDeltaTime, float inTimeBeforeSleep) +{ + mSleepTestTimer += inDeltaTime; + return mSleepTestTimer >= inTimeBeforeSleep? ECanSleep::CanSleep : ECanSleep::CannotSleep; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionQuality.h b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionQuality.h new file mode 100644 index 000000000000..b1ba343c06ac --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionQuality.h @@ -0,0 +1,31 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Motion quality, or how well it detects collisions when it has a high velocity +enum class EMotionQuality : uint8 +{ + /// Update the body in discrete steps. Body will tunnel throuh thin objects if its velocity is high enough. + /// This is the cheapest way of simulating a body. + Discrete, + + /// Update the body using linear casting. When stepping the body, its collision shape is cast from + /// start to destination using the starting rotation. The body will not be able to tunnel through thin + /// objects at high velocity, but tunneling is still possible if the body is long and thin and has high + /// angular velocity. Time is stolen from the object (which means it will move up to the first collision + /// and will not bounce off the surface until the next integration step). This will make the body appear + /// to go slower when it collides with high velocity. In order to not get stuck, the body is always + /// allowed to move by a fraction of it's inner radius, which may eventually lead it to pass through geometry. + /// + /// Note that if you're using a collision listener, you can receive contact added/persisted notifications of contacts + /// that may in the end not happen. This happens between bodies that are using casting: If bodies A and B collide at t1 + /// and B and C collide at t2 where t2 < t1 and A and C don't collide. In this case you may receive an incorrect contact + /// point added callback between A and B (which will be removed the next frame). + LinearCast, +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionType.h b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionType.h new file mode 100644 index 000000000000..6de0d8c8ec73 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionType.h @@ -0,0 +1,17 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Motion type of a physics body +enum class EMotionType : uint8 +{ + Static, ///< Non movable + Kinematic, ///< Movable using velocities only, does not respond to forces + Dynamic, ///< Responds to forces as a normal physics object +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/Character.cpp b/thirdparty/jolt_physics/Jolt/Physics/Character/Character.cpp new file mode 100644 index 000000000000..14b2312870cd --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Character/Character.cpp @@ -0,0 +1,323 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +static inline const BodyLockInterface &sGetBodyLockInterface(const PhysicsSystem *inSystem, bool inLockBodies) +{ + return inLockBodies? static_cast(inSystem->GetBodyLockInterface()) : static_cast(inSystem->GetBodyLockInterfaceNoLock()); +} + +static inline BodyInterface &sGetBodyInterface(PhysicsSystem *inSystem, bool inLockBodies) +{ + return inLockBodies? inSystem->GetBodyInterface() : inSystem->GetBodyInterfaceNoLock(); +} + +static inline const NarrowPhaseQuery &sGetNarrowPhaseQuery(const PhysicsSystem *inSystem, bool inLockBodies) +{ + return inLockBodies? inSystem->GetNarrowPhaseQuery() : inSystem->GetNarrowPhaseQueryNoLock(); +} + +Character::Character(const CharacterSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem) : + CharacterBase(inSettings, inSystem), + mLayer(inSettings->mLayer) +{ + // Construct rigid body + BodyCreationSettings settings(mShape, inPosition, inRotation, EMotionType::Dynamic, mLayer); + settings.mAllowedDOFs = EAllowedDOFs::TranslationX | EAllowedDOFs::TranslationY | EAllowedDOFs::TranslationZ; + settings.mEnhancedInternalEdgeRemoval = inSettings->mEnhancedInternalEdgeRemoval; + settings.mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided; + settings.mMassPropertiesOverride.mMass = inSettings->mMass; + settings.mFriction = inSettings->mFriction; + settings.mGravityFactor = inSettings->mGravityFactor; + settings.mUserData = inUserData; + const Body *body = mSystem->GetBodyInterface().CreateBody(settings); + if (body != nullptr) + mBodyID = body->GetID(); +} + +Character::~Character() +{ + // Destroy the body + mSystem->GetBodyInterface().DestroyBody(mBodyID); +} + +void Character::AddToPhysicsSystem(EActivation inActivationMode, bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).AddBody(mBodyID, inActivationMode); +} + +void Character::RemoveFromPhysicsSystem(bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).RemoveBody(mBodyID); +} + +void Character::Activate(bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).ActivateBody(mBodyID); +} + +void Character::CheckCollision(RMat44Arg inCenterOfMassTransform, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const +{ + // Create query broadphase layer filter + DefaultBroadPhaseLayerFilter broadphase_layer_filter = mSystem->GetDefaultBroadPhaseLayerFilter(mLayer); + + // Create query object layer filter + DefaultObjectLayerFilter object_layer_filter = mSystem->GetDefaultLayerFilter(mLayer); + + // Ignore my own body + IgnoreSingleBodyFilter body_filter(mBodyID); + + // Settings for collide shape + CollideShapeSettings settings; + settings.mMaxSeparationDistance = inMaxSeparationDistance; + settings.mActiveEdgeMode = EActiveEdgeMode::CollideOnlyWithActive; + settings.mActiveEdgeMovementDirection = inMovementDirection; + settings.mBackFaceMode = EBackFaceMode::IgnoreBackFaces; + + sGetNarrowPhaseQuery(mSystem, inLockBodies).CollideShape(inShape, Vec3::sReplicate(1.0f), inCenterOfMassTransform, settings, inBaseOffset, ioCollector, broadphase_layer_filter, object_layer_filter, body_filter); +} + +void Character::CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const +{ + // Calculate center of mass transform + RMat44 center_of_mass = RMat44::sRotationTranslation(inRotation, inPosition).PreTranslated(inShape->GetCenterOfMass()); + + CheckCollision(center_of_mass, inMovementDirection, inMaxSeparationDistance, inShape, inBaseOffset, ioCollector, inLockBodies); +} + +void Character::CheckCollision(const Shape *inShape, float inMaxSeparationDistance, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const +{ + // Determine position and velocity of body + RMat44 query_transform; + Vec3 velocity; + { + BodyLockRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyID); + if (!lock.Succeeded()) + return; + + const Body &body = lock.GetBody(); + + // Correct the center of mass transform for the difference between the old and new center of mass shape + query_transform = body.GetCenterOfMassTransform().PreTranslated(inShape->GetCenterOfMass() - mShape->GetCenterOfMass()); + velocity = body.GetLinearVelocity(); + } + + CheckCollision(query_transform, velocity, inMaxSeparationDistance, inShape, inBaseOffset, ioCollector, inLockBodies); +} + +void Character::PostSimulation(float inMaxSeparationDistance, bool inLockBodies) +{ + // Get character position, rotation and velocity + RVec3 char_pos; + Quat char_rot; + Vec3 char_vel; + { + BodyLockRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyID); + if (!lock.Succeeded()) + return; + const Body &body = lock.GetBody(); + char_pos = body.GetPosition(); + char_rot = body.GetRotation(); + char_vel = body.GetLinearVelocity(); + } + + // Collector that finds the hit with the normal that is the most 'up' + class MyCollector : public CollideShapeCollector + { + public: + // Constructor + explicit MyCollector(Vec3Arg inUp, RVec3 inBaseOffset) : mBaseOffset(inBaseOffset), mUp(inUp) { } + + // See: CollectorType::AddHit + virtual void AddHit(const CollideShapeResult &inResult) override + { + Vec3 normal = -inResult.mPenetrationAxis.Normalized(); + float dot = normal.Dot(mUp); + if (dot > mBestDot) // Find the hit that is most aligned with the up vector + { + mGroundBodyID = inResult.mBodyID2; + mGroundBodySubShapeID = inResult.mSubShapeID2; + mGroundPosition = mBaseOffset + inResult.mContactPointOn2; + mGroundNormal = normal; + mBestDot = dot; + } + } + + BodyID mGroundBodyID; + SubShapeID mGroundBodySubShapeID; + RVec3 mGroundPosition = RVec3::sZero(); + Vec3 mGroundNormal = Vec3::sZero(); + + private: + RVec3 mBaseOffset; + Vec3 mUp; + float mBestDot = -FLT_MAX; + }; + + // Collide shape + MyCollector collector(mUp, char_pos); + CheckCollision(char_pos, char_rot, char_vel, inMaxSeparationDistance, mShape, char_pos, collector, inLockBodies); + + // Copy results + mGroundBodyID = collector.mGroundBodyID; + mGroundBodySubShapeID = collector.mGroundBodySubShapeID; + mGroundPosition = collector.mGroundPosition; + mGroundNormal = collector.mGroundNormal; + + // Get additional data from body + BodyLockRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mGroundBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + + // Update ground state + RMat44 inv_transform = RMat44::sInverseRotationTranslation(char_rot, char_pos); + if (mSupportingVolume.SignedDistance(Vec3(inv_transform * mGroundPosition)) > 0.0f) + mGroundState = EGroundState::NotSupported; + else if (IsSlopeTooSteep(mGroundNormal)) + mGroundState = EGroundState::OnSteepGround; + else + mGroundState = EGroundState::OnGround; + + // Copy other body properties + mGroundMaterial = body.GetShape()->GetMaterial(mGroundBodySubShapeID); + mGroundVelocity = body.GetPointVelocity(mGroundPosition); + mGroundUserData = body.GetUserData(); + } + else + { + mGroundState = EGroundState::InAir; + mGroundMaterial = PhysicsMaterial::sDefault; + mGroundVelocity = Vec3::sZero(); + mGroundUserData = 0; + } +} + +void Character::SetLinearAndAngularVelocity(Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).SetLinearAndAngularVelocity(mBodyID, inLinearVelocity, inAngularVelocity); +} + +Vec3 Character::GetLinearVelocity(bool inLockBodies) const +{ + return sGetBodyInterface(mSystem, inLockBodies).GetLinearVelocity(mBodyID); +} + +void Character::SetLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).SetLinearVelocity(mBodyID, inLinearVelocity); +} + +void Character::AddLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).AddLinearVelocity(mBodyID, inLinearVelocity); +} + +void Character::AddImpulse(Vec3Arg inImpulse, bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).AddImpulse(mBodyID, inImpulse); +} + +void Character::GetPositionAndRotation(RVec3 &outPosition, Quat &outRotation, bool inLockBodies) const +{ + sGetBodyInterface(mSystem, inLockBodies).GetPositionAndRotation(mBodyID, outPosition, outRotation); +} + +void Character::SetPositionAndRotation(RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode, bool inLockBodies) const +{ + sGetBodyInterface(mSystem, inLockBodies).SetPositionAndRotation(mBodyID, inPosition, inRotation, inActivationMode); +} + +RVec3 Character::GetPosition(bool inLockBodies) const +{ + return sGetBodyInterface(mSystem, inLockBodies).GetPosition(mBodyID); +} + +void Character::SetPosition(RVec3Arg inPosition, EActivation inActivationMode, bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).SetPosition(mBodyID, inPosition, inActivationMode); +} + +Quat Character::GetRotation(bool inLockBodies) const +{ + return sGetBodyInterface(mSystem, inLockBodies).GetRotation(mBodyID); +} + +void Character::SetRotation(QuatArg inRotation, EActivation inActivationMode, bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).SetRotation(mBodyID, inRotation, inActivationMode); +} + +RVec3 Character::GetCenterOfMassPosition(bool inLockBodies) const +{ + return sGetBodyInterface(mSystem, inLockBodies).GetCenterOfMassPosition(mBodyID); +} + +RMat44 Character::GetWorldTransform(bool inLockBodies) const +{ + return sGetBodyInterface(mSystem, inLockBodies).GetWorldTransform(mBodyID); +} + +void Character::SetLayer(ObjectLayer inLayer, bool inLockBodies) +{ + mLayer = inLayer; + + sGetBodyInterface(mSystem, inLockBodies).SetObjectLayer(mBodyID, inLayer); +} + +bool Character::SetShape(const Shape *inShape, float inMaxPenetrationDepth, bool inLockBodies) +{ + if (inMaxPenetrationDepth < FLT_MAX) + { + // Collector that checks if there is anything in the way while switching to inShape + class MyCollector : public CollideShapeCollector + { + public: + // Constructor + explicit MyCollector(float inMaxPenetrationDepth) : mMaxPenetrationDepth(inMaxPenetrationDepth) { } + + // See: CollectorType::AddHit + virtual void AddHit(const CollideShapeResult &inResult) override + { + if (inResult.mPenetrationDepth > mMaxPenetrationDepth) + { + mHadCollision = true; + ForceEarlyOut(); + } + } + + float mMaxPenetrationDepth; + bool mHadCollision = false; + }; + + // Test if anything is in the way of switching + RVec3 char_pos = GetPosition(inLockBodies); + MyCollector collector(inMaxPenetrationDepth); + CheckCollision(inShape, 0.0f, char_pos, collector, inLockBodies); + if (collector.mHadCollision) + return false; + } + + // Switch the shape + mShape = inShape; + sGetBodyInterface(mSystem, inLockBodies).SetShape(mBodyID, mShape, false, EActivation::Activate); + return true; +} + +TransformedShape Character::GetTransformedShape(bool inLockBodies) const +{ + return sGetBodyInterface(mSystem, inLockBodies).GetTransformedShape(mBodyID); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/Character.h b/thirdparty/jolt_physics/Jolt/Physics/Character/Character.h new file mode 100644 index 000000000000..db67f8b3b6b7 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Character/Character.h @@ -0,0 +1,147 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Contains the configuration of a character +class JPH_EXPORT CharacterSettings : public CharacterBaseSettings +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Layer that this character will be added to + ObjectLayer mLayer = 0; + + /// Mass of the character + float mMass = 80.0f; + + /// Friction for the character + float mFriction = 0.2f; + + /// Value to multiply gravity with for this character + float mGravityFactor = 1.0f; +}; + +/// Runtime character object. +/// This object usually represents the player or a humanoid AI. It uses a single rigid body, +/// usually with a capsule shape to simulate movement and collision for the character. +/// The character is a keyframed object, the application controls it by setting the velocity. +class JPH_EXPORT Character : public CharacterBase +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + /// @param inSettings The settings for the character + /// @param inPosition Initial position for the character + /// @param inRotation Initial rotation for the character (usually only around Y) + /// @param inUserData Application specific value + /// @param inSystem Physics system that this character will be added to later + Character(const CharacterSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem); + + /// Destructor + virtual ~Character() override; + + /// Add bodies and constraints to the system and optionally activate the bodies + void AddToPhysicsSystem(EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true); + + /// Remove bodies and constraints from the system + void RemoveFromPhysicsSystem(bool inLockBodies = true); + + /// Wake up the character + void Activate(bool inLockBodies = true); + + /// Needs to be called after every PhysicsSystem::Update + /// @param inMaxSeparationDistance Max distance between the floor and the character to still consider the character standing on the floor + /// @param inLockBodies If the collision query should use the locking body interface (true) or the non locking body interface (false) + void PostSimulation(float inMaxSeparationDistance, bool inLockBodies = true); + + /// Control the velocity of the character + void SetLinearAndAngularVelocity(Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, bool inLockBodies = true); + + /// Get the linear velocity of the character (m / s) + Vec3 GetLinearVelocity(bool inLockBodies = true) const; + + /// Set the linear velocity of the character (m / s) + void SetLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies = true); + + /// Add world space linear velocity to current velocity (m / s) + void AddLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies = true); + + /// Add impulse to the center of mass of the character + void AddImpulse(Vec3Arg inImpulse, bool inLockBodies = true); + + /// Get the body associated with this character + BodyID GetBodyID() const { return mBodyID; } + + /// Get position / rotation of the body + void GetPositionAndRotation(RVec3 &outPosition, Quat &outRotation, bool inLockBodies = true) const; + + /// Set the position / rotation of the body, optionally activating it. + void SetPositionAndRotation(RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true) const; + + /// Get the position of the character + RVec3 GetPosition(bool inLockBodies = true) const; + + /// Set the position of the character, optionally activating it. + void SetPosition(RVec3Arg inPostion, EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true); + + /// Get the rotation of the character + Quat GetRotation(bool inLockBodies = true) const; + + /// Set the rotation of the character, optionally activating it. + void SetRotation(QuatArg inRotation, EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true); + + /// Position of the center of mass of the underlying rigid body + RVec3 GetCenterOfMassPosition(bool inLockBodies = true) const; + + /// Calculate the world transform of the character + RMat44 GetWorldTransform(bool inLockBodies = true) const; + + /// Get the layer of the character + ObjectLayer GetLayer() const { return mLayer; } + + /// Update the layer of the character + void SetLayer(ObjectLayer inLayer, bool inLockBodies = true); + + /// Switch the shape of the character (e.g. for stance). When inMaxPenetrationDepth is not FLT_MAX, it checks + /// if the new shape collides before switching shape. Returns true if the switch succeeded. + bool SetShape(const Shape *inShape, float inMaxPenetrationDepth, bool inLockBodies = true); + + /// Get the transformed shape that represents the volume of the character, can be used for collision checks. + TransformedShape GetTransformedShape(bool inLockBodies = true) const; + + /// @brief Get all contacts for the character at a particular location + /// @param inPosition Position to test. + /// @param inRotation Rotation at which to test the shape. + /// @param inMovementDirection A hint in which direction the character is moving, will be used to calculate a proper normal. + /// @param inMaxSeparationDistance How much distance around the character you want to report contacts in (can be 0 to match the character exactly). + /// @param inShape Shape to test collision with. + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. GetPosition() since floats are most accurate near the origin + /// @param ioCollector Collision collector that receives the collision results. + /// @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; + +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; + + /// Check collisions between inShape and the world using the current position / rotation of the character + void CheckCollision(const Shape *inShape, float inMaxSeparationDistance, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const; + + /// The body of this character + BodyID mBodyID; + + /// The layer the body is in + ObjectLayer mLayer; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.cpp b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.cpp new file mode 100644 index 000000000000..d31425065097 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.cpp @@ -0,0 +1,59 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +CharacterBase::CharacterBase(const CharacterBaseSettings *inSettings, PhysicsSystem *inSystem) : + mSystem(inSystem), + mShape(inSettings->mShape), + mUp(inSettings->mUp), + mSupportingVolume(inSettings->mSupportingVolume) +{ + // Initialize max slope angle + SetMaxSlopeAngle(inSettings->mMaxSlopeAngle); +} + +const char *CharacterBase::sToString(EGroundState inState) +{ + switch (inState) + { + case EGroundState::OnGround: return "OnGround"; + case EGroundState::OnSteepGround: return "OnSteepGround"; + case EGroundState::NotSupported: return "NotSupported"; + case EGroundState::InAir: return "InAir"; + } + + JPH_ASSERT(false); + return "Unknown"; +} + +void CharacterBase::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mGroundState); + inStream.Write(mGroundBodyID); + inStream.Write(mGroundBodySubShapeID); + inStream.Write(mGroundPosition); + inStream.Write(mGroundNormal); + inStream.Write(mGroundVelocity); + // Can't save user data (may be a pointer) and material +} + +void CharacterBase::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mGroundState); + inStream.Read(mGroundBodyID); + inStream.Read(mGroundBodySubShapeID); + inStream.Read(mGroundPosition); + inStream.Read(mGroundNormal); + inStream.Read(mGroundVelocity); + mGroundUserData = 0; // Cannot restore user data + mGroundMaterial = PhysicsMaterial::sDefault; // Cannot restore material +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.h b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.h new file mode 100644 index 000000000000..fc19c3d9949b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.h @@ -0,0 +1,157 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; +class StateRecorder; + +/// Base class for configuration of a character +class JPH_EXPORT CharacterBaseSettings : public RefTarget +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + CharacterBaseSettings() = default; + CharacterBaseSettings(const CharacterBaseSettings &inSettings) = default; + CharacterBaseSettings & operator = (const CharacterBaseSettings &inSettings) = default; + + /// Virtual destructor + virtual ~CharacterBaseSettings() = default; + + /// Vector indicating the up direction of the character + Vec3 mUp = Vec3::sAxisY(); + + /// Plane, defined in local space relative to the character. Every contact behind this plane can support the + /// character, every contact in front of this plane is treated as only colliding with the player. + /// Default: Accept any contact. + Plane mSupportingVolume { Vec3::sAxisY(), -1.0e10f }; + + /// Maximum angle of slope that character can still walk on (radians). + float mMaxSlopeAngle = DegreesToRadians(50.0f); + + /// Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges. + bool mEnhancedInternalEdgeRemoval = false; + + /// Initial shape that represents the character's volume. + /// Usually this is a capsule, make sure the shape is made so that the bottom of the shape is at (0, 0, 0). + RefConst mShape; +}; + +/// Base class for character class +class JPH_EXPORT CharacterBase : public RefTarget, public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + CharacterBase(const CharacterBaseSettings *inSettings, PhysicsSystem *inSystem); + + /// Destructor + virtual ~CharacterBase() = default; + + /// Set the maximum angle of slope that character can still walk on (radians) + void SetMaxSlopeAngle(float inMaxSlopeAngle) { mCosMaxSlopeAngle = Cos(inMaxSlopeAngle); } + float GetCosMaxSlopeAngle() const { return mCosMaxSlopeAngle; } + + /// Set the up vector for the character + void SetUp(Vec3Arg inUp) { mUp = inUp; } + Vec3 GetUp() const { return mUp; } + + /// Check if the normal of the ground surface is too steep to walk on + bool IsSlopeTooSteep(Vec3Arg inNormal) const + { + // If cos max slope angle is close to one the system is turned off, + // otherwise check the angle between the up and normal vector + return mCosMaxSlopeAngle < cNoMaxSlopeAngle && inNormal.Dot(mUp) < mCosMaxSlopeAngle; + } + + /// Get the current shape that the character is using. + const Shape * GetShape() const { return mShape; } + + enum class EGroundState + { + OnGround, ///< Character is on the ground and can move freely. + OnSteepGround, ///< Character is on a slope that is too steep and can't climb up any further. The caller should start applying downward velocity if sliding from the slope is desired. + NotSupported, ///< Character is touching an object, but is not supported by it and should fall. The GetGroundXXX functions will return information about the touched object. + InAir, ///< Character is in the air and is not touching anything. + }; + + /// Debug function to convert enum values to string + static const char * sToString(EGroundState inState); + + ///@name Properties of the ground this character is standing on + + /// Current ground state + EGroundState GetGroundState() const { return mGroundState; } + + /// Returns true if the player is supported by normal or steep ground + bool IsSupported() const { return mGroundState == EGroundState::OnGround || mGroundState == EGroundState::OnSteepGround; } + + /// Get the contact point with the ground + RVec3 GetGroundPosition() const { return mGroundPosition; } + + /// Get the contact normal with the ground + Vec3 GetGroundNormal() const { return mGroundNormal; } + + /// Velocity in world space of ground + Vec3 GetGroundVelocity() const { return mGroundVelocity; } + + /// Material that the character is standing on + const PhysicsMaterial * GetGroundMaterial() const { return mGroundMaterial; } + + /// BodyID of the object the character is standing on. Note may have been removed! + BodyID GetGroundBodyID() const { return mGroundBodyID; } + + /// Sub part of the body that we're standing on. + SubShapeID GetGroundSubShapeID() const { return mGroundBodySubShapeID; } + + /// User data value of the body that we're standing on + uint64 GetGroundUserData() const { return mGroundUserData; } + + // Saving / restoring state for replay + virtual void SaveState(StateRecorder &inStream) const; + virtual void RestoreState(StateRecorder &inStream); + +protected: + // Cached physics system + PhysicsSystem * mSystem; + + // The shape that the body currently has + RefConst mShape; + + // The character's world space up axis + Vec3 mUp; + + // Every contact behind this plane can support the character + Plane mSupportingVolume; + + // Beyond this value there is no max slope + static constexpr float cNoMaxSlopeAngle = 0.9999f; + + // Cosine of the maximum angle of slope that character can still walk on + float mCosMaxSlopeAngle; + + // Ground properties + EGroundState mGroundState = EGroundState::InAir; + BodyID mGroundBodyID; + SubShapeID mGroundBodySubShapeID; + RVec3 mGroundPosition = RVec3::sZero(); + Vec3 mGroundNormal = Vec3::sZero(); + Vec3 mGroundVelocity = Vec3::sZero(); + RefConst mGroundMaterial = PhysicsMaterial::sDefault; + uint64 mGroundUserData = 0; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.cpp b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.cpp new file mode 100644 index 000000000000..6c16dc83be18 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.cpp @@ -0,0 +1,1734 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +void CharacterVsCharacterCollisionSimple::Remove(const CharacterVirtual *inCharacter) +{ + Array::iterator i = std::find(mCharacters.begin(), mCharacters.end(), inCharacter); + if (i != mCharacters.end()) + mCharacters.erase(i); +} + +void CharacterVsCharacterCollisionSimple::CollideCharacter(const CharacterVirtual *inCharacter, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector) const +{ + // Make shape 1 relative to inBaseOffset + Mat44 transform1 = inCenterOfMassTransform.PostTranslated(-inBaseOffset).ToMat44(); + + const Shape *shape = inCharacter->GetShape(); + CollideShapeSettings settings = inCollideShapeSettings; + + // Iterate over all characters + for (const CharacterVirtual *c : mCharacters) + if (c != inCharacter + && !ioCollector.ShouldEarlyOut()) + { + // Collector needs to know which character we're colliding with + ioCollector.SetUserData(reinterpret_cast(c)); + + // Make shape 2 relative to inBaseOffset + Mat44 transform2 = c->GetCenterOfMassTransform().PostTranslated(-inBaseOffset).ToMat44(); + + // We need to add the padding of character 2 so that we will detect collision with its outer shell + settings.mMaxSeparationDistance = inCollideShapeSettings.mMaxSeparationDistance + c->GetCharacterPadding(); + + // Note that this collides against the character's shape without padding, this will be corrected for in CharacterVirtual::GetContactsAtPosition + CollisionDispatch::sCollideShapeVsShape(shape, c->GetShape(), Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), transform1, transform2, SubShapeIDCreator(), SubShapeIDCreator(), settings, ioCollector); + } + + // Reset the user data + ioCollector.SetUserData(0); +} + +void CharacterVsCharacterCollisionSimple::CastCharacter(const CharacterVirtual *inCharacter, RMat44Arg inCenterOfMassTransform, Vec3Arg inDirection, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector) const +{ + // Convert shape cast relative to inBaseOffset + Mat44 transform1 = inCenterOfMassTransform.PostTranslated(-inBaseOffset).ToMat44(); + ShapeCast shape_cast(inCharacter->GetShape(), Vec3::sReplicate(1.0f), transform1, inDirection); + + // Iterate over all characters + for (const CharacterVirtual *c : mCharacters) + if (c != inCharacter + && !ioCollector.ShouldEarlyOut()) + { + // Collector needs to know which character we're colliding with + ioCollector.SetUserData(reinterpret_cast(c)); + + // Make shape 2 relative to inBaseOffset + Mat44 transform2 = c->GetCenterOfMassTransform().PostTranslated(-inBaseOffset).ToMat44(); + + // Note that this collides against the character's shape without padding, this will be corrected for in CharacterVirtual::GetFirstContactForSweep + CollisionDispatch::sCastShapeVsShapeWorldSpace(shape_cast, inShapeCastSettings, c->GetShape(), Vec3::sReplicate(1.0f), { }, transform2, SubShapeIDCreator(), SubShapeIDCreator(), ioCollector); + } + + // Reset the user data + ioCollector.SetUserData(0); +} + +CharacterVirtual::CharacterVirtual(const CharacterVirtualSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem) : + CharacterBase(inSettings, inSystem), + mBackFaceMode(inSettings->mBackFaceMode), + mPredictiveContactDistance(inSettings->mPredictiveContactDistance), + mMaxCollisionIterations(inSettings->mMaxCollisionIterations), + mMaxConstraintIterations(inSettings->mMaxConstraintIterations), + mMinTimeRemaining(inSettings->mMinTimeRemaining), + mCollisionTolerance(inSettings->mCollisionTolerance), + mCharacterPadding(inSettings->mCharacterPadding), + mMaxNumHits(inSettings->mMaxNumHits), + mHitReductionCosMaxAngle(inSettings->mHitReductionCosMaxAngle), + mPenetrationRecoverySpeed(inSettings->mPenetrationRecoverySpeed), + mEnhancedInternalEdgeRemoval(inSettings->mEnhancedInternalEdgeRemoval), + mShapeOffset(inSettings->mShapeOffset), + mPosition(inPosition), + mRotation(inRotation), + mUserData(inUserData) +{ + // Copy settings + SetMaxStrength(inSettings->mMaxStrength); + SetMass(inSettings->mMass); + + // Create an inner rigid body if requested + if (inSettings->mInnerBodyShape != nullptr) + { + BodyCreationSettings settings(inSettings->mInnerBodyShape, GetInnerBodyPosition(), mRotation, EMotionType::Kinematic, inSettings->mInnerBodyLayer); + settings.mAllowSleeping = false; // Disable sleeping so that we will receive sensor callbacks + settings.mUserData = inUserData; + mInnerBodyID = inSystem->GetBodyInterface().CreateAndAddBody(settings, EActivation::Activate); + } +} + +CharacterVirtual::~CharacterVirtual() +{ + if (!mInnerBodyID.IsInvalid()) + { + mSystem->GetBodyInterface().RemoveBody(mInnerBodyID); + mSystem->GetBodyInterface().DestroyBody(mInnerBodyID); + } +} + +void CharacterVirtual::UpdateInnerBodyTransform() +{ + if (!mInnerBodyID.IsInvalid()) + mSystem->GetBodyInterface().SetPositionAndRotation(mInnerBodyID, GetInnerBodyPosition(), mRotation, EActivation::DontActivate); +} + +void CharacterVirtual::GetAdjustedBodyVelocity(const Body& inBody, Vec3 &outLinearVelocity, Vec3 &outAngularVelocity) const +{ + // Get real velocity of body + if (!inBody.IsStatic()) + { + const MotionProperties *mp = inBody.GetMotionPropertiesUnchecked(); + outLinearVelocity = mp->GetLinearVelocity(); + outAngularVelocity = mp->GetAngularVelocity(); + } + else + { + outLinearVelocity = outAngularVelocity = Vec3::sZero(); + } + + // Allow application to override + if (mListener != nullptr) + mListener->OnAdjustBodyVelocity(this, inBody, outLinearVelocity, outAngularVelocity); +} + +Vec3 CharacterVirtual::CalculateCharacterGroundVelocity(RVec3Arg inCenterOfMass, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, float inDeltaTime) const +{ + // Get angular velocity + float angular_velocity_len_sq = inAngularVelocity.LengthSq(); + if (angular_velocity_len_sq < 1.0e-12f) + return inLinearVelocity; + float angular_velocity_len = sqrt(angular_velocity_len_sq); + + // Calculate the rotation that the object will make in the time step + Quat rotation = Quat::sRotation(inAngularVelocity / angular_velocity_len, angular_velocity_len * inDeltaTime); + + // Calculate where the new character position will be + RVec3 new_position = inCenterOfMass + rotation * Vec3(mPosition - inCenterOfMass); + + // Calculate the velocity + return inLinearVelocity + Vec3(new_position - mPosition) / inDeltaTime; +} + +template +void CharacterVirtual::sFillContactProperties(const CharacterVirtual *inCharacter, Contact &outContact, const Body &inBody, Vec3Arg inUp, RVec3Arg inBaseOffset, const taCollector &inCollector, const CollideShapeResult &inResult) +{ + // Get adjusted body velocity + Vec3 linear_velocity, angular_velocity; + inCharacter->GetAdjustedBodyVelocity(inBody, linear_velocity, angular_velocity); + + outContact.mPosition = inBaseOffset + inResult.mContactPointOn2; + outContact.mLinearVelocity = linear_velocity + angular_velocity.Cross(Vec3(outContact.mPosition - inBody.GetCenterOfMassPosition())); // Calculate point velocity + outContact.mContactNormal = -inResult.mPenetrationAxis.NormalizedOr(Vec3::sZero()); + outContact.mSurfaceNormal = inCollector.GetContext()->GetWorldSpaceSurfaceNormal(inResult.mSubShapeID2, outContact.mPosition); + if (outContact.mContactNormal.Dot(outContact.mSurfaceNormal) < 0.0f) + outContact.mSurfaceNormal = -outContact.mSurfaceNormal; // Flip surface normal if we're hitting a back face + if (outContact.mContactNormal.Dot(inUp) > outContact.mSurfaceNormal.Dot(inUp)) + outContact.mSurfaceNormal = outContact.mContactNormal; // Replace surface normal with contact normal if the contact normal is pointing more upwards + outContact.mDistance = -inResult.mPenetrationDepth; + outContact.mBodyB = inResult.mBodyID2; + outContact.mSubShapeIDB = inResult.mSubShapeID2; + outContact.mMotionTypeB = inBody.GetMotionType(); + outContact.mIsSensorB = inBody.IsSensor(); + outContact.mUserData = inBody.GetUserData(); + outContact.mMaterial = inCollector.GetContext()->GetMaterial(inResult.mSubShapeID2); +} + +void CharacterVirtual::sFillCharacterContactProperties(Contact &outContact, CharacterVirtual *inOtherCharacter, RVec3Arg inBaseOffset, const CollideShapeResult &inResult) +{ + outContact.mPosition = inBaseOffset + inResult.mContactPointOn2; + outContact.mLinearVelocity = inOtherCharacter->GetLinearVelocity(); + outContact.mSurfaceNormal = outContact.mContactNormal = -inResult.mPenetrationAxis.NormalizedOr(Vec3::sZero()); + outContact.mDistance = -inResult.mPenetrationDepth; + outContact.mCharacterB = inOtherCharacter; + outContact.mSubShapeIDB = inResult.mSubShapeID2; + outContact.mMotionTypeB = EMotionType::Kinematic; // Other character is kinematic, we can't directly move it + outContact.mIsSensorB = false; + outContact.mUserData = inOtherCharacter->GetUserData(); + outContact.mMaterial = PhysicsMaterial::sDefault; +} + +void CharacterVirtual::ContactCollector::AddHit(const CollideShapeResult &inResult) +{ + // If we exceed our contact limit, try to clean up near-duplicate contacts + if (mContacts.size() == mMaxHits) + { + // Flag that we hit this code path + mMaxHitsExceeded = true; + + // Check if we can do reduction + if (mHitReductionCosMaxAngle > -1.0f) + { + // Loop all contacts and find similar contacts + for (int i = (int)mContacts.size() - 1; i >= 0; --i) + { + Contact &contact_i = mContacts[i]; + for (int j = i - 1; j >= 0; --j) + { + Contact &contact_j = mContacts[j]; + if (contact_i.IsSameBody(contact_j) + && contact_i.mContactNormal.Dot(contact_j.mContactNormal) > mHitReductionCosMaxAngle) // Very similar contact normals + { + // Remove the contact with the biggest distance + bool i_is_last = i == (int)mContacts.size() - 1; + if (contact_i.mDistance > contact_j.mDistance) + { + // Remove i + if (!i_is_last) + contact_i = mContacts.back(); + mContacts.pop_back(); + + // Break out of the loop, i is now an element that we already processed + break; + } + else + { + // Remove j + contact_j = mContacts.back(); + mContacts.pop_back(); + + // If i was the last element, we just moved it into position j. Break out of the loop, we'll see it again later. + if (i_is_last) + break; + } + } + } + } + } + + if (mContacts.size() == mMaxHits) + { + // There are still too many hits, give up! + ForceEarlyOut(); + return; + } + } + + if (inResult.mBodyID2.IsInvalid()) + { + // Assuming this is a hit against another character + JPH_ASSERT(mOtherCharacter != nullptr); + + // Create contact with other character + mContacts.emplace_back(); + Contact &contact = mContacts.back(); + sFillCharacterContactProperties(contact, mOtherCharacter, mBaseOffset, inResult); + contact.mFraction = 0.0f; + } + else + { + // Create contact with other body + BodyLockRead lock(mSystem->GetBodyLockInterface(), inResult.mBodyID2); + if (lock.SucceededAndIsInBroadPhase()) + { + mContacts.emplace_back(); + Contact &contact = mContacts.back(); + sFillContactProperties(mCharacter, contact, lock.GetBody(), mUp, mBaseOffset, *this, inResult); + contact.mFraction = 0.0f; + } + } +} + +void CharacterVirtual::ContactCastCollector::AddHit(const ShapeCastResult &inResult) +{ + if (inResult.mFraction < mContact.mFraction // Since we're doing checks against the world and against characters, we may get a hit with a higher fraction than the previous hit + && inResult.mFraction > 0.0f // Ignore collisions at fraction = 0 + && inResult.mPenetrationAxis.Dot(mDisplacement) > 0.0f) // Ignore penetrations that we're moving away from + { + // Test if this contact should be ignored + for (const IgnoredContact &c : mIgnoredContacts) + if (c.mBodyID == inResult.mBodyID2 && c.mSubShapeID == inResult.mSubShapeID2) + return; + + Contact contact; + + if (inResult.mBodyID2.IsInvalid()) + { + // Assuming this is a hit against another character + JPH_ASSERT(mOtherCharacter != nullptr); + + // Create contact with other character + sFillCharacterContactProperties(contact, mOtherCharacter, mBaseOffset, inResult); + } + else + { + // Lock body only while we fetch contact properties + BodyLockRead lock(mSystem->GetBodyLockInterface(), inResult.mBodyID2); + if (!lock.SucceededAndIsInBroadPhase()) + return; + + // Sweeps don't result in OnContactAdded callbacks so we can ignore sensors here + const Body &body = lock.GetBody(); + if (body.IsSensor()) + return; + + // Convert the hit result into a contact + sFillContactProperties(mCharacter, contact, body, mUp, mBaseOffset, *this, inResult); + } + + contact.mFraction = inResult.mFraction; + + // Check if the contact that will make us penetrate more than the allowed tolerance + if (contact.mDistance + contact.mContactNormal.Dot(mDisplacement) < -mCharacter->mCollisionTolerance + && mCharacter->ValidateContact(contact)) + { + mContact = contact; + UpdateEarlyOutFraction(contact.mFraction); + } + } +} + +void CharacterVirtual::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 +{ + // Query shape transform + RMat44 transform = GetCenterOfMassTransform(inPosition, inRotation, inShape); + + // Settings for collide shape + CollideShapeSettings settings; + settings.mBackFaceMode = mBackFaceMode; + settings.mActiveEdgeMovementDirection = inMovementDirection; + settings.mMaxSeparationDistance = mCharacterPadding + inMaxSeparationDistance; + settings.mActiveEdgeMode = EActiveEdgeMode::CollideOnlyWithActive; + + // Body filter + IgnoreSingleBodyFilterChained body_filter(mInnerBodyID, inBodyFilter); + + // Select the right function + auto collide_shape_function = mEnhancedInternalEdgeRemoval? &NarrowPhaseQuery::CollideShapeWithInternalEdgeRemoval : &NarrowPhaseQuery::CollideShape; + + // Collide shape + (mSystem->GetNarrowPhaseQuery().*collide_shape_function)(inShape, Vec3::sReplicate(1.0f), transform, settings, inBaseOffset, ioCollector, inBroadPhaseLayerFilter, inObjectLayerFilter, body_filter, inShapeFilter); + + // Also collide with other characters + if (mCharacterVsCharacterCollision != nullptr) + { + ioCollector.SetContext(nullptr); // We're no longer colliding with a transformed shape, reset + mCharacterVsCharacterCollision->CollideCharacter(this, transform, settings, inBaseOffset, ioCollector); + } +} + +void CharacterVirtual::GetContactsAtPosition(RVec3Arg inPosition, Vec3Arg inMovementDirection, const Shape *inShape, TempContactList &outContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + // Remove previous results + outContacts.clear(); + + // Body filter + IgnoreSingleBodyFilterChained body_filter(mInnerBodyID, inBodyFilter); + + // Collide shape + ContactCollector collector(mSystem, this, mMaxNumHits, mHitReductionCosMaxAngle, mUp, mPosition, outContacts); + CheckCollision(inPosition, mRotation, inMovementDirection, mPredictiveContactDistance, inShape, mPosition, collector, inBroadPhaseLayerFilter, inObjectLayerFilter, body_filter, inShapeFilter); + + // The broadphase bounding boxes will not be deterministic, which means that the order in which the contacts are received by the collector is not deterministic. + // Therefore we need to sort the contacts to preserve determinism. Note that currently this will fail if we exceed mMaxNumHits hits. + QuickSort(outContacts.begin(), outContacts.end(), ContactOrderingPredicate()); + + // Flag if we exceeded the max number of hits + mMaxHitsExceeded = collector.mMaxHitsExceeded; + + // Reduce distance to contact by padding to ensure we stay away from the object by a little margin + // (this will make collision detection cheaper - especially for sweep tests as they won't hit the surface if we're properly sliding) + for (Contact &c : outContacts) + { + c.mDistance -= mCharacterPadding; + + if (c.mCharacterB != nullptr) + c.mDistance -= c.mCharacterB->mCharacterPadding; + } +} + +void CharacterVirtual::RemoveConflictingContacts(TempContactList &ioContacts, IgnoredContactList &outIgnoredContacts) const +{ + // Only use this algorithm if we're penetrating further than this (due to numerical precision issues we can always penetrate a little bit and we don't want to discard contacts if they just have a tiny penetration) + // We do need to account for padding (see GetContactsAtPosition) that is removed from the contact distances, to compensate we add it to the cMinRequiredPenetration + const float cMinRequiredPenetration = 1.25f * mCharacterPadding; + + // Discard conflicting penetrating contacts + for (size_t c1 = 0; c1 < ioContacts.size(); c1++) + { + Contact &contact1 = ioContacts[c1]; + if (contact1.mDistance <= -cMinRequiredPenetration) // Only for penetrations + for (size_t c2 = c1 + 1; c2 < ioContacts.size(); c2++) + { + Contact &contact2 = ioContacts[c2]; + if (contact1.IsSameBody(contact2) + && contact2.mDistance <= -cMinRequiredPenetration // Only for penetrations + && contact1.mContactNormal.Dot(contact2.mContactNormal) < 0.0f) // Only opposing normals + { + // Discard contacts with the least amount of penetration + if (contact1.mDistance < contact2.mDistance) + { + // Discard the 2nd contact + outIgnoredContacts.emplace_back(contact2.mBodyB, contact2.mSubShapeIDB); + ioContacts.erase(ioContacts.begin() + c2); + c2--; + } + else + { + // Discard the first contact + outIgnoredContacts.emplace_back(contact1.mBodyB, contact1.mSubShapeIDB); + ioContacts.erase(ioContacts.begin() + c1); + c1--; + break; + } + } + } + } +} + +bool CharacterVirtual::ValidateContact(const Contact &inContact) const +{ + if (mListener == nullptr) + return true; + + if (inContact.mCharacterB != nullptr) + return mListener->OnCharacterContactValidate(this, inContact.mCharacterB, inContact.mSubShapeIDB); + else + return mListener->OnContactValidate(this, inContact.mBodyB, inContact.mSubShapeIDB); +} + +void CharacterVirtual::ContactAdded(const Contact &inContact, CharacterContactSettings &ioSettings) const +{ + if (mListener != nullptr) + { + if (inContact.mCharacterB != nullptr) + mListener->OnCharacterContactAdded(this, inContact.mCharacterB, inContact.mSubShapeIDB, inContact.mPosition, -inContact.mContactNormal, ioSettings); + else + mListener->OnContactAdded(this, inContact.mBodyB, inContact.mSubShapeIDB, inContact.mPosition, -inContact.mContactNormal, ioSettings); + } +} + +template +inline static bool sCorrectFractionForCharacterPadding(const Shape *inShape, Mat44Arg inStart, Vec3Arg inDisplacement, Vec3Arg inScale, const T &inPolygon, float &ioFraction) +{ + if (inShape->GetType() == EShapeType::Convex) + { + // Get the support function for the shape we're casting + const ConvexShape *convex_shape = static_cast(inShape); + ConvexShape::SupportBuffer buffer; + const ConvexShape::Support *support = convex_shape->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, inScale); + + // Cast the shape against the polygon + GJKClosestPoint gjk; + return gjk.CastShape(inStart, inDisplacement, cDefaultCollisionTolerance, *support, inPolygon, ioFraction); + } + else if (inShape->GetSubType() == EShapeSubType::RotatedTranslated) + { + const RotatedTranslatedShape *rt_shape = static_cast(inShape); + return sCorrectFractionForCharacterPadding(rt_shape->GetInnerShape(), inStart * Mat44::sRotation(rt_shape->GetRotation()), inDisplacement, rt_shape->TransformScale(inScale), inPolygon, ioFraction); + } + else if (inShape->GetSubType() == EShapeSubType::Scaled) + { + const ScaledShape *scaled_shape = static_cast(inShape); + return sCorrectFractionForCharacterPadding(scaled_shape->GetInnerShape(), inStart, inDisplacement, inScale * scaled_shape->GetScale(), inPolygon, ioFraction); + } + else + { + JPH_ASSERT(false, "Not supported yet!"); + return false; + } +} + +bool CharacterVirtual::GetFirstContactForSweep(RVec3Arg inPosition, Vec3Arg inDisplacement, Contact &outContact, const IgnoredContactList &inIgnoredContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + // Too small distance -> skip checking + float displacement_len_sq = inDisplacement.LengthSq(); + if (displacement_len_sq < 1.0e-8f) + return false; + + // Calculate start transform + RMat44 start = GetCenterOfMassTransform(inPosition, mRotation, mShape); + + // Settings for the cast + ShapeCastSettings settings; + settings.mBackFaceModeTriangles = mBackFaceMode; + settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces; + settings.mActiveEdgeMode = EActiveEdgeMode::CollideOnlyWithActive; + settings.mUseShrunkenShapeAndConvexRadius = true; + settings.mReturnDeepestPoint = false; + + // Calculate how much extra fraction we need to add to the cast to account for the character padding + float character_padding_fraction = mCharacterPadding / sqrt(displacement_len_sq); + + // Body filter + IgnoreSingleBodyFilterChained body_filter(mInnerBodyID, inBodyFilter); + + // Cast shape + Contact contact; + contact.mFraction = 1.0f + character_padding_fraction; + RVec3 base_offset = start.GetTranslation(); + ContactCastCollector collector(mSystem, this, inDisplacement, mUp, inIgnoredContacts, base_offset, contact); + collector.ResetEarlyOutFraction(contact.mFraction); + RShapeCast shape_cast(mShape, Vec3::sReplicate(1.0f), start, inDisplacement); + mSystem->GetNarrowPhaseQuery().CastShape(shape_cast, settings, base_offset, collector, inBroadPhaseLayerFilter, inObjectLayerFilter, body_filter, inShapeFilter); + + // Also collide with other characters + if (mCharacterVsCharacterCollision != nullptr) + { + collector.SetContext(nullptr); // We're no longer colliding with a transformed shape, reset + mCharacterVsCharacterCollision->CastCharacter(this, start, inDisplacement, settings, base_offset, collector); + } + + if (contact.mBodyB.IsInvalid() && contact.mCharacterB == nullptr) + return false; + + // Store contact + outContact = contact; + + TransformedShape ts; + float character_padding = mCharacterPadding; + if (outContact.mCharacterB != nullptr) + { + // Create a transformed shape for the character + RMat44 com = outContact.mCharacterB->GetCenterOfMassTransform(); + ts = TransformedShape(com.GetTranslation(), com.GetQuaternion(), outContact.mCharacterB->GetShape(), BodyID(), SubShapeIDCreator()); + + // We need to take the other character's padding into account as well + character_padding += outContact.mCharacterB->mCharacterPadding; + } + else + { + // Create a transformed shape for the body + ts = mSystem->GetBodyInterface().GetTransformedShape(outContact.mBodyB); + } + + // Fetch the face we're colliding with + Shape::SupportingFace face; + ts.GetSupportingFace(outContact.mSubShapeIDB, -outContact.mContactNormal, base_offset, face); + + bool corrected = false; + if (face.size() >= 2) + { + // Inflate the colliding face by the character padding + PolygonConvexSupport polygon(face); + AddConvexRadius add_cvx(polygon, character_padding); + + // Correct fraction to hit this inflated face instead of the inner shape + corrected = sCorrectFractionForCharacterPadding(mShape, start.GetRotation(), inDisplacement, Vec3::sReplicate(1.0f), add_cvx, outContact.mFraction); + } + if (!corrected) + { + // When there's only a single contact point or when we were unable to correct the fraction, + // we can just move the fraction back so that the character and its padding don't hit the contact point anymore + outContact.mFraction = max(0.0f, outContact.mFraction - character_padding_fraction); + } + + // Ensure that we never return a fraction that's bigger than 1 (which could happen due to float precision issues). + outContact.mFraction = min(outContact.mFraction, 1.0f); + + return true; +} + +void CharacterVirtual::DetermineConstraints(TempContactList &inContacts, float inDeltaTime, ConstraintList &outConstraints) const +{ + for (Contact &c : inContacts) + { + Vec3 contact_velocity = c.mLinearVelocity; + + // Penetrating contact: Add a contact velocity that pushes the character out at the desired speed + if (c.mDistance < 0.0f) + contact_velocity -= c.mContactNormal * c.mDistance * mPenetrationRecoverySpeed / inDeltaTime; + + // Convert to a constraint + outConstraints.emplace_back(); + Constraint &constraint = outConstraints.back(); + constraint.mContact = &c; + constraint.mLinearVelocity = contact_velocity; + constraint.mPlane = Plane(c.mContactNormal, c.mDistance); + + // Next check if the angle is too steep and if it is add an additional constraint that holds the character back + if (IsSlopeTooSteep(c.mSurfaceNormal)) + { + // Only take planes that point up. + // Note that we use the contact normal to allow for better sliding as the surface normal may be in the opposite direction of movement. + float dot = c.mContactNormal.Dot(mUp); + if (dot > 1.0e-3f) // Add a little slack, if the normal is perfectly horizontal we already have our vertical plane. + { + // Mark the slope constraint as steep + constraint.mIsSteepSlope = true; + + // Make horizontal normal + Vec3 normal = (c.mContactNormal - dot * mUp).Normalized(); + + // Create a secondary constraint that blocks horizontal movement + outConstraints.emplace_back(); + Constraint &vertical_constraint = outConstraints.back(); + vertical_constraint.mContact = &c; + vertical_constraint.mLinearVelocity = contact_velocity.Dot(normal) * normal; // Project the contact velocity on the new normal so that both planes push at an equal rate + vertical_constraint.mPlane = Plane(normal, c.mDistance / normal.Dot(c.mContactNormal)); // Calculate the distance we have to travel horizontally to hit the contact plane + } + } + } +} + +bool CharacterVirtual::HandleContact(Vec3Arg inVelocity, Constraint &ioConstraint, float inDeltaTime) const +{ + Contact &contact = *ioConstraint.mContact; + + // Validate the contact point + if (!ValidateContact(contact)) + return false; + + // Send contact added event + CharacterContactSettings settings; + ContactAdded(contact, settings); + contact.mCanPushCharacter = settings.mCanPushCharacter; + + // We don't have any further interaction with sensors beyond an OnContactAdded notification + if (contact.mIsSensorB) + return false; + + // If body B cannot receive an impulse, we're done + if (!settings.mCanReceiveImpulses || contact.mMotionTypeB != EMotionType::Dynamic) + return true; + + // Lock the body we're colliding with + BodyLockWrite lock(mSystem->GetBodyLockInterface(), contact.mBodyB); + if (!lock.SucceededAndIsInBroadPhase()) + return false; // Body has been removed, we should not collide with it anymore + const Body &body = lock.GetBody(); + + // Calculate the velocity that we want to apply at B so that it will start moving at the character's speed at the contact point + constexpr float cDamping = 0.9f; + constexpr float cPenetrationResolution = 0.4f; + Vec3 relative_velocity = inVelocity - contact.mLinearVelocity; + float projected_velocity = relative_velocity.Dot(contact.mContactNormal); + float delta_velocity = -projected_velocity * cDamping - min(contact.mDistance, 0.0f) * cPenetrationResolution / inDeltaTime; + + // Don't apply impulses if we're separating + if (delta_velocity < 0.0f) + return true; + + // Determine mass properties of the body we're colliding with + const MotionProperties *motion_properties = body.GetMotionProperties(); + RVec3 center_of_mass = body.GetCenterOfMassPosition(); + Mat44 inverse_inertia = body.GetInverseInertia(); + float inverse_mass = motion_properties->GetInverseMass(); + + // Calculate the inverse of the mass of body B as seen at the contact point in the direction of the contact normal + Vec3 jacobian = Vec3(contact.mPosition - center_of_mass).Cross(contact.mContactNormal); + float inv_effective_mass = inverse_inertia.Multiply3x3(jacobian).Dot(jacobian) + inverse_mass; + + // Impulse P = M dv + float impulse = delta_velocity / inv_effective_mass; + + // Clamp the impulse according to the character strength, character strength is a force in newtons, P = F dt + float max_impulse = mMaxStrength * inDeltaTime; + impulse = min(impulse, max_impulse); + + // Calculate the world space impulse to apply + Vec3 world_impulse = -impulse * contact.mContactNormal; + + // Cancel impulse in down direction (we apply gravity later) + float impulse_dot_up = world_impulse.Dot(mUp); + if (impulse_dot_up < 0.0f) + world_impulse -= impulse_dot_up * mUp; + + // Now apply the impulse (body is already locked so we use the no-lock interface) + mSystem->GetBodyInterfaceNoLock().AddImpulse(contact.mBodyB, world_impulse, contact.mPosition); + return true; +} + +void CharacterVirtual::SolveConstraints(Vec3Arg inVelocity, float inDeltaTime, float inTimeRemaining, ConstraintList &ioConstraints, IgnoredContactList &ioIgnoredContacts, float &outTimeSimulated, Vec3 &outDisplacement, TempAllocator &inAllocator +#ifdef JPH_DEBUG_RENDERER + , bool inDrawConstraints +#endif // JPH_DEBUG_RENDERER + ) const +{ + // If there are no constraints we can immediately move to our target + if (ioConstraints.empty()) + { + outDisplacement = inVelocity * inTimeRemaining; + outTimeSimulated = inTimeRemaining; + return; + } + + // Create array that holds the constraints in order of time of impact (sort will happen later) + Array> sorted_constraints(inAllocator); + sorted_constraints.resize(ioConstraints.size()); + for (size_t index = 0; index < sorted_constraints.size(); index++) + sorted_constraints[index] = &ioConstraints[index]; + + // This is the velocity we use for the displacement, if we hit something it will be shortened + Vec3 velocity = inVelocity; + + // Keep track of the last velocity that was applied to the character so that we can detect when the velocity reverses + Vec3 last_velocity = inVelocity; + + // Start with no displacement + outDisplacement = Vec3::sZero(); + outTimeSimulated = 0.0f; + + // These are the contacts that we hit previously without moving a significant distance + Array> previous_contacts(inAllocator); + previous_contacts.resize(mMaxConstraintIterations); + int num_previous_contacts = 0; + + // Loop for a max amount of iterations + for (uint iteration = 0; iteration < mMaxConstraintIterations; iteration++) + { + // Calculate time of impact for all constraints + for (Constraint &c : ioConstraints) + { + // Project velocity on plane direction + c.mProjectedVelocity = c.mPlane.GetNormal().Dot(c.mLinearVelocity - velocity); + if (c.mProjectedVelocity < 1.0e-6f) + { + c.mTOI = FLT_MAX; + } + else + { + // Distance to plane + float dist = c.mPlane.SignedDistance(outDisplacement); + + if (dist - c.mProjectedVelocity * inTimeRemaining > -1.0e-4f) + { + // Too little penetration, accept the movement + c.mTOI = FLT_MAX; + } + else + { + // Calculate time of impact + c.mTOI = max(0.0f, dist / c.mProjectedVelocity); + } + } + } + + // Sort constraints on proximity + QuickSort(sorted_constraints.begin(), sorted_constraints.end(), [](const Constraint *inLHS, const Constraint *inRHS) { + // If both constraints hit at t = 0 then order the one that will push the character furthest first + // Note that because we add velocity to penetrating contacts, this will also resolve contacts that penetrate the most + if (inLHS->mTOI <= 0.0f && inRHS->mTOI <= 0.0f) + return inLHS->mProjectedVelocity > inRHS->mProjectedVelocity; + + // Then sort on time of impact + if (inLHS->mTOI != inRHS->mTOI) + return inLHS->mTOI < inRHS->mTOI; + + // As a tie breaker sort static first so it has the most influence + return inLHS->mContact->mMotionTypeB > inRHS->mContact->mMotionTypeB; + }); + + // Find the first valid constraint + Constraint *constraint = nullptr; + for (Constraint *c : sorted_constraints) + { + // Take the first contact and see if we can reach it + if (c->mTOI >= inTimeRemaining) + { + // We can reach our goal! + outDisplacement += velocity * inTimeRemaining; + outTimeSimulated += inTimeRemaining; + return; + } + + // Test if this contact was discarded by the contact callback before + if (c->mContact->mWasDiscarded) + continue; + + // Check if we made contact with this before + if (!c->mContact->mHadCollision) + { + // Handle the contact + if (!HandleContact(velocity, *c, inDeltaTime)) + { + // Constraint should be ignored, remove it from the list + c->mContact->mWasDiscarded = true; + + // Mark it as ignored for GetFirstContactForSweep + ioIgnoredContacts.emplace_back(c->mContact->mBodyB, c->mContact->mSubShapeIDB); + continue; + } + + c->mContact->mHadCollision = true; + } + + // Cancel velocity of constraint if it cannot push the character + if (!c->mContact->mCanPushCharacter) + c->mLinearVelocity = Vec3::sZero(); + + // We found the first constraint that we want to collide with + constraint = c; + break; + } + + if (constraint == nullptr) + { + // All constraints were discarded, we can reach our goal! + outDisplacement += velocity * inTimeRemaining; + outTimeSimulated += inTimeRemaining; + return; + } + + // Move to the contact + outDisplacement += velocity * constraint->mTOI; + inTimeRemaining -= constraint->mTOI; + outTimeSimulated += constraint->mTOI; + + // If there's not enough time left to be simulated, bail + if (inTimeRemaining < mMinTimeRemaining) + return; + + // If we've moved significantly, clear all previous contacts + if (constraint->mTOI > 1.0e-4f) + num_previous_contacts = 0; + + // Get the normal of the plane we're hitting + Vec3 plane_normal = constraint->mPlane.GetNormal(); + + // If we're hitting a steep slope we cancel the velocity towards the slope first so that we don't end up sliding up the slope + // (we may hit the slope before the vertical wall constraint we added which will result in a small movement up causing jitter in the character movement) + if (constraint->mIsSteepSlope) + { + // We're hitting a steep slope, create a vertical plane that blocks any further movement up the slope (note: not normalized) + Vec3 vertical_plane_normal = plane_normal - plane_normal.Dot(mUp) * mUp; + + // Get the relative velocity between the character and the constraint + Vec3 relative_velocity = velocity - constraint->mLinearVelocity; + + // Remove velocity towards the slope + velocity = velocity - min(0.0f, relative_velocity.Dot(vertical_plane_normal)) * vertical_plane_normal / vertical_plane_normal.LengthSq(); + } + + // Get the relative velocity between the character and the constraint + Vec3 relative_velocity = velocity - constraint->mLinearVelocity; + + // Calculate new velocity if we cancel the relative velocity in the normal direction + Vec3 new_velocity = velocity - relative_velocity.Dot(plane_normal) * plane_normal; + + // Find the normal of the previous contact that we will violate the most if we move in this new direction + float highest_penetration = 0.0f; + Constraint *other_constraint = nullptr; + for (Constraint **c = previous_contacts.data(); c < previous_contacts.data() + num_previous_contacts; ++c) + if (*c != constraint) + { + // Calculate how much we will penetrate if we move in this direction + Vec3 other_normal = (*c)->mPlane.GetNormal(); + float penetration = ((*c)->mLinearVelocity - new_velocity).Dot(other_normal); + if (penetration > highest_penetration) + { + // We don't want parallel or anti-parallel normals as that will cause our cross product below to become zero. Slack is approx 10 degrees. + float dot = other_normal.Dot(plane_normal); + if (dot < 0.984f && dot > -0.984f) + { + highest_penetration = penetration; + other_constraint = *c; + } + } + } + + // Check if we found a 2nd constraint + if (other_constraint != nullptr) + { + // Calculate the sliding direction and project the new velocity onto that sliding direction + Vec3 other_normal = other_constraint->mPlane.GetNormal(); + Vec3 slide_dir = plane_normal.Cross(other_normal).Normalized(); + Vec3 velocity_in_slide_dir = new_velocity.Dot(slide_dir) * slide_dir; + + // Cancel the constraint velocity in the other constraint plane's direction so that we won't try to apply it again and keep ping ponging between planes + constraint->mLinearVelocity -= min(0.0f, constraint->mLinearVelocity.Dot(other_normal)) * other_normal; + + // Cancel the other constraints velocity in this constraint plane's direction so that we won't try to apply it again and keep ping ponging between planes + other_constraint->mLinearVelocity -= min(0.0f, other_constraint->mLinearVelocity.Dot(plane_normal)) * plane_normal; + + // Calculate the velocity of this constraint perpendicular to the slide direction + Vec3 perpendicular_velocity = constraint->mLinearVelocity - constraint->mLinearVelocity.Dot(slide_dir) * slide_dir; + + // Calculate the velocity of the other constraint perpendicular to the slide direction + Vec3 other_perpendicular_velocity = other_constraint->mLinearVelocity - other_constraint->mLinearVelocity.Dot(slide_dir) * slide_dir; + + // Add all components together + new_velocity = velocity_in_slide_dir + perpendicular_velocity + other_perpendicular_velocity; + } + + // Allow application to modify calculated velocity + if (mListener != nullptr) + { + if (constraint->mContact->mCharacterB != nullptr) + mListener->OnCharacterContactSolve(this, constraint->mContact->mCharacterB, constraint->mContact->mSubShapeIDB, constraint->mContact->mPosition, constraint->mContact->mContactNormal, constraint->mContact->mLinearVelocity, constraint->mContact->mMaterial, velocity, new_velocity); + else + mListener->OnContactSolve(this, constraint->mContact->mBodyB, constraint->mContact->mSubShapeIDB, constraint->mContact->mPosition, constraint->mContact->mContactNormal, constraint->mContact->mLinearVelocity, constraint->mContact->mMaterial, velocity, new_velocity); + } + +#ifdef JPH_DEBUG_RENDERER + if (inDrawConstraints) + { + // Calculate where to draw + RVec3 offset = mPosition + Vec3(0, 0, 2.5f * (iteration + 1)); + + // Draw constraint plane + DebugRenderer::sInstance->DrawPlane(offset, constraint->mPlane.GetNormal(), Color::sCyan, 1.0f); + + // Draw 2nd constraint plane + if (other_constraint != nullptr) + DebugRenderer::sInstance->DrawPlane(offset, other_constraint->mPlane.GetNormal(), Color::sBlue, 1.0f); + + // Draw starting velocity + DebugRenderer::sInstance->DrawArrow(offset, offset + velocity, Color::sGreen, 0.05f); + + // Draw resulting velocity + DebugRenderer::sInstance->DrawArrow(offset, offset + new_velocity, Color::sRed, 0.05f); + } +#endif // JPH_DEBUG_RENDERER + + // Update the velocity + velocity = new_velocity; + + // Add the contact to the list so that next iteration we can avoid violating it again + previous_contacts[num_previous_contacts] = constraint; + num_previous_contacts++; + + // Check early out + if (constraint->mProjectedVelocity < 1.0e-8f // Constraint should not be pushing, otherwise there may be other constraints that are pushing us + && velocity.LengthSq() < 1.0e-8f) // There's not enough velocity left + return; + + // If the constraint has velocity we accept the new velocity, otherwise check that we didn't reverse velocity + if (!constraint->mLinearVelocity.IsNearZero(1.0e-8f)) + last_velocity = constraint->mLinearVelocity; + else if (velocity.Dot(last_velocity) < 0.0f) + return; + } +} + +void CharacterVirtual::UpdateSupportingContact(bool inSkipContactVelocityCheck, TempAllocator &inAllocator) +{ + // Flag contacts as having a collision if they're close enough but ignore contacts we're moving away from. + // Note that if we did MoveShape before we want to preserve any contacts that it marked as colliding + for (Contact &c : mActiveContacts) + if (!c.mWasDiscarded + && !c.mHadCollision + && c.mDistance < mCollisionTolerance + && (inSkipContactVelocityCheck || c.mSurfaceNormal.Dot(mLinearVelocity - c.mLinearVelocity) <= 1.0e-4f)) + { + if (ValidateContact(c) && !c.mIsSensorB) + c.mHadCollision = true; + else + c.mWasDiscarded = true; + } + + // Calculate transform that takes us to character local space + RMat44 inv_transform = RMat44::sInverseRotationTranslation(mRotation, mPosition); + + // Determine if we're supported or not + int num_supported = 0; + int num_sliding = 0; + int num_avg_normal = 0; + Vec3 avg_normal = Vec3::sZero(); + Vec3 avg_velocity = Vec3::sZero(); + const Contact *supporting_contact = nullptr; + float max_cos_angle = -FLT_MAX; + const Contact *deepest_contact = nullptr; + float smallest_distance = FLT_MAX; + for (const Contact &c : mActiveContacts) + if (c.mHadCollision) + { + // Calculate the angle between the plane normal and the up direction + float cos_angle = c.mSurfaceNormal.Dot(mUp); + + // Find the deepest contact + if (c.mDistance < smallest_distance) + { + deepest_contact = &c; + smallest_distance = c.mDistance; + } + + // If this contact is in front of our plane, we cannot be supported by it + if (mSupportingVolume.SignedDistance(Vec3(inv_transform * c.mPosition)) > 0.0f) + continue; + + // Find the contact with the normal that is pointing most upwards and store it + if (max_cos_angle < cos_angle) + { + supporting_contact = &c; + max_cos_angle = cos_angle; + } + + // Check if this is a sliding or supported contact + bool is_supported = mCosMaxSlopeAngle > cNoMaxSlopeAngle || cos_angle >= mCosMaxSlopeAngle; + if (is_supported) + num_supported++; + else + num_sliding++; + + // If the angle between the two is less than 85 degrees we also use it to calculate the average normal + if (cos_angle >= 0.08f) + { + avg_normal += c.mSurfaceNormal; + num_avg_normal++; + + // For static or dynamic objects or for contacts that don't support us just take the contact velocity + if (c.mMotionTypeB != EMotionType::Kinematic || !is_supported) + avg_velocity += c.mLinearVelocity; + else + { + // For keyframed objects that support us calculate the velocity at our position rather than at the contact position so that we properly follow the object + BodyLockRead lock(mSystem->GetBodyLockInterface(), c.mBodyB); + if (lock.SucceededAndIsInBroadPhase()) + { + const Body &body = lock.GetBody(); + + // Get adjusted body velocity + Vec3 linear_velocity, angular_velocity; + GetAdjustedBodyVelocity(body, linear_velocity, angular_velocity); + + // Calculate the ground velocity + avg_velocity += CalculateCharacterGroundVelocity(body.GetCenterOfMassPosition(), linear_velocity, angular_velocity, mLastDeltaTime); + } + else + { + // Fall back to contact velocity + avg_velocity += c.mLinearVelocity; + } + } + } + } + + // Take either the most supporting contact or the deepest contact + const Contact *best_contact = supporting_contact != nullptr? supporting_contact : deepest_contact; + + // Calculate average normal and velocity + if (num_avg_normal >= 1) + { + mGroundNormal = avg_normal.Normalized(); + mGroundVelocity = avg_velocity / float(num_avg_normal); + } + else if (best_contact != nullptr) + { + mGroundNormal = best_contact->mSurfaceNormal; + mGroundVelocity = best_contact->mLinearVelocity; + } + else + { + mGroundNormal = Vec3::sZero(); + mGroundVelocity = Vec3::sZero(); + } + + // Copy contact properties + if (best_contact != nullptr) + { + mGroundBodyID = best_contact->mBodyB; + mGroundBodySubShapeID = best_contact->mSubShapeIDB; + mGroundPosition = best_contact->mPosition; + mGroundMaterial = best_contact->mMaterial; + mGroundUserData = best_contact->mUserData; + } + else + { + mGroundBodyID = BodyID(); + mGroundBodySubShapeID = SubShapeID(); + mGroundPosition = RVec3::sZero(); + mGroundMaterial = PhysicsMaterial::sDefault; + mGroundUserData = 0; + } + + // Determine ground state + if (num_supported > 0) + { + // We made contact with something that supports us + mGroundState = EGroundState::OnGround; + } + else if (num_sliding > 0) + { + if ((mLinearVelocity - deepest_contact->mLinearVelocity).Dot(mUp) > 1.0e-4f) + { + // We cannot be on ground if we're moving upwards relative to the ground + mGroundState = EGroundState::OnSteepGround; + } + else + { + // If we're sliding down, we may actually be standing on multiple sliding contacts in such a way that we can't slide off, in this case we're also supported + + // Convert the contacts into constraints + TempContactList contacts(mActiveContacts.begin(), mActiveContacts.end(), inAllocator); + ConstraintList constraints(inAllocator); + constraints.reserve(contacts.size() * 2); + DetermineConstraints(contacts, mLastDeltaTime, constraints); + + // Solve the displacement using these constraints, this is used to check if we didn't move at all because we are supported + Vec3 displacement; + float time_simulated; + IgnoredContactList ignored_contacts(inAllocator); + ignored_contacts.reserve(contacts.size()); + SolveConstraints(-mUp, 1.0f, 1.0f, constraints, ignored_contacts, time_simulated, displacement, inAllocator); + + // If we're blocked then we're supported, otherwise we're sliding + float min_required_displacement_sq = Square(0.6f * mLastDeltaTime); + if (time_simulated < 0.001f || displacement.LengthSq() < min_required_displacement_sq) + mGroundState = EGroundState::OnGround; + else + mGroundState = EGroundState::OnSteepGround; + } + } + else + { + // Not supported by anything + mGroundState = best_contact != nullptr? EGroundState::NotSupported : EGroundState::InAir; + } +} + +void CharacterVirtual::StoreActiveContacts(const TempContactList &inContacts, TempAllocator &inAllocator) +{ + mActiveContacts.assign(inContacts.begin(), inContacts.end()); + + UpdateSupportingContact(true, inAllocator); +} + +void CharacterVirtual::MoveShape(RVec3 &ioPosition, Vec3Arg inVelocity, float inDeltaTime, ContactList *outActiveContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator +#ifdef JPH_DEBUG_RENDERER + , bool inDrawConstraints +#endif // JPH_DEBUG_RENDERER + ) const +{ + JPH_DET_LOG("CharacterVirtual::MoveShape: pos: " << ioPosition << " vel: " << inVelocity << " dt: " << inDeltaTime); + + Vec3 movement_direction = inVelocity.NormalizedOr(Vec3::sZero()); + + float time_remaining = inDeltaTime; + for (uint iteration = 0; iteration < mMaxCollisionIterations && time_remaining >= mMinTimeRemaining; iteration++) + { + JPH_DET_LOG("iter: " << iteration << " time: " << time_remaining); + + // Determine contacts in the neighborhood + TempContactList contacts(inAllocator); + contacts.reserve(mMaxNumHits); + GetContactsAtPosition(ioPosition, movement_direction, mShape, contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter); + +#ifdef JPH_ENABLE_DETERMINISM_LOG + for (const Contact &c : contacts) + JPH_DET_LOG("contact: " << c.mPosition << " vel: " << c.mLinearVelocity << " cnormal: " << c.mContactNormal << " snormal: " << c.mSurfaceNormal << " dist: " << c.mDistance << " fraction: " << c.mFraction << " body: " << c.mBodyB << " subshape: " << c.mSubShapeIDB); +#endif // JPH_ENABLE_DETERMINISM_LOG + + // Remove contacts with the same body that have conflicting normals + IgnoredContactList ignored_contacts(inAllocator); + ignored_contacts.reserve(contacts.size()); + RemoveConflictingContacts(contacts, ignored_contacts); + + // Convert contacts into constraints + ConstraintList constraints(inAllocator); + constraints.reserve(contacts.size() * 2); + DetermineConstraints(contacts, inDeltaTime, constraints); + +#ifdef JPH_DEBUG_RENDERER + bool draw_constraints = inDrawConstraints && iteration == 0; + if (draw_constraints) + { + for (const Constraint &c : constraints) + { + // Draw contact point + DebugRenderer::sInstance->DrawMarker(c.mContact->mPosition, Color::sYellow, 0.05f); + Vec3 dist_to_plane = -c.mPlane.GetConstant() * c.mPlane.GetNormal(); + + // Draw arrow towards surface that we're hitting + DebugRenderer::sInstance->DrawArrow(c.mContact->mPosition, c.mContact->mPosition - dist_to_plane, Color::sYellow, 0.05f); + + // Draw plane around the player position indicating the space that we can move + DebugRenderer::sInstance->DrawPlane(mPosition + dist_to_plane, c.mPlane.GetNormal(), Color::sCyan, 1.0f); + DebugRenderer::sInstance->DrawArrow(mPosition + dist_to_plane, mPosition + dist_to_plane + c.mContact->mSurfaceNormal, Color::sRed, 0.05f); + } + } +#endif // JPH_DEBUG_RENDERER + + // Solve the displacement using these constraints + Vec3 displacement; + float time_simulated; + SolveConstraints(inVelocity, inDeltaTime, time_remaining, constraints, ignored_contacts, time_simulated, displacement, inAllocator + #ifdef JPH_DEBUG_RENDERER + , draw_constraints + #endif // JPH_DEBUG_RENDERER + ); + + // Store the contacts now that the colliding ones have been marked + if (outActiveContacts != nullptr) + outActiveContacts->assign(contacts.begin(), contacts.end()); + + // Do a sweep to test if the path is really unobstructed + Contact cast_contact; + if (GetFirstContactForSweep(ioPosition, displacement, cast_contact, ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter)) + { + displacement *= cast_contact.mFraction; + time_simulated *= cast_contact.mFraction; + } + + // Update the position + ioPosition += displacement; + time_remaining -= time_simulated; + + // If the displacement during this iteration was too small we assume we cannot further progress this update + if (displacement.LengthSq() < 1.0e-8f) + break; + } +} + +void CharacterVirtual::SetUserData(uint64 inUserData) +{ + mUserData = inUserData; + + if (!mInnerBodyID.IsInvalid()) + mSystem->GetBodyInterface().SetUserData(mInnerBodyID, inUserData); +} + +Vec3 CharacterVirtual::CancelVelocityTowardsSteepSlopes(Vec3Arg inDesiredVelocity) const +{ + // If we're not pushing against a steep slope, return the desired velocity + // Note: This is important as WalkStairs overrides the ground state to OnGround when its first check fails but the second succeeds + if (mGroundState == CharacterVirtual::EGroundState::OnGround + || mGroundState == CharacterVirtual::EGroundState::InAir) + return inDesiredVelocity; + + Vec3 desired_velocity = inDesiredVelocity; + for (const Contact &c : mActiveContacts) + if (c.mHadCollision + && IsSlopeTooSteep(c.mSurfaceNormal)) + { + // Note that we use the contact normal to allow for better sliding as the surface normal may be in the opposite direction of movement. + Vec3 normal = c.mContactNormal; + + // Remove normal vertical component + normal -= normal.Dot(mUp) * mUp; + + // Cancel horizontal movement in opposite direction + float dot = normal.Dot(desired_velocity); + if (dot < 0.0f) + desired_velocity -= (dot * normal) / normal.LengthSq(); + } + return desired_velocity; +} + +void CharacterVirtual::Update(float inDeltaTime, Vec3Arg inGravity, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator) +{ + // If there's no delta time, we don't need to do anything + if (inDeltaTime <= 0.0f) + return; + + // Remember delta time for checking if we're supported by the ground + mLastDeltaTime = inDeltaTime; + + // Slide the shape through the world + MoveShape(mPosition, mLinearVelocity, inDeltaTime, &mActiveContacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator + #ifdef JPH_DEBUG_RENDERER + , sDrawConstraints + #endif // JPH_DEBUG_RENDERER + ); + + // Determine the object that we're standing on + UpdateSupportingContact(false, inAllocator); + + // Ensure that the rigid body ends up at the new position + UpdateInnerBodyTransform(); + + // If we're on the ground + if (!mGroundBodyID.IsInvalid() && mMass > 0.0f) + { + // Add the impulse to the ground due to gravity: P = F dt = M g dt + float normal_dot_gravity = mGroundNormal.Dot(inGravity); + if (normal_dot_gravity < 0.0f) + { + Vec3 world_impulse = -(mMass * normal_dot_gravity / inGravity.Length() * inDeltaTime) * inGravity; + mSystem->GetBodyInterface().AddImpulse(mGroundBodyID, world_impulse, mGroundPosition); + } + } +} + +void CharacterVirtual::RefreshContacts(const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator) +{ + // Determine the contacts + TempContactList contacts(inAllocator); + contacts.reserve(mMaxNumHits); + GetContactsAtPosition(mPosition, mLinearVelocity.NormalizedOr(Vec3::sZero()), mShape, contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter); + + StoreActiveContacts(contacts, inAllocator); +} + +void CharacterVirtual::UpdateGroundVelocity() +{ + BodyLockRead lock(mSystem->GetBodyLockInterface(), mGroundBodyID); + if (lock.SucceededAndIsInBroadPhase()) + { + const Body &body = lock.GetBody(); + + // Get adjusted body velocity + Vec3 linear_velocity, angular_velocity; + GetAdjustedBodyVelocity(body, linear_velocity, angular_velocity); + + // Calculate the ground velocity + mGroundVelocity = CalculateCharacterGroundVelocity(body.GetCenterOfMassPosition(), linear_velocity, angular_velocity, mLastDeltaTime); + } +} + +void CharacterVirtual::MoveToContact(RVec3Arg inPosition, const Contact &inContact, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator) +{ + // Set the new position + SetPosition(inPosition); + + // Trigger contact added callback + CharacterContactSettings dummy; + ContactAdded(inContact, dummy); + + // Determine the contacts + TempContactList contacts(inAllocator); + contacts.reserve(mMaxNumHits + 1); // +1 because we can add one extra below + GetContactsAtPosition(mPosition, mLinearVelocity.NormalizedOr(Vec3::sZero()), mShape, contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter); + + // Ensure that we mark inContact as colliding + bool found_contact = false; + for (Contact &c : contacts) + if (c.mBodyB == inContact.mBodyB + && c.mSubShapeIDB == inContact.mSubShapeIDB) + { + c.mHadCollision = true; + found_contact = true; + } + if (!found_contact) + { + contacts.push_back(inContact); + + Contact © = contacts.back(); + copy.mHadCollision = true; + } + + StoreActiveContacts(contacts, inAllocator); + JPH_ASSERT(mGroundState != EGroundState::InAir); + + // Ensure that the rigid body ends up at the new position + UpdateInnerBodyTransform(); +} + +bool CharacterVirtual::SetShape(const Shape *inShape, float inMaxPenetrationDepth, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator) +{ + if (mShape == nullptr || mSystem == nullptr) + { + // It hasn't been initialized yet + mShape = inShape; + return true; + } + + if (inShape != mShape && inShape != nullptr) + { + if (inMaxPenetrationDepth < FLT_MAX) + { + // Check collision around the new shape + TempContactList contacts(inAllocator); + contacts.reserve(mMaxNumHits); + GetContactsAtPosition(mPosition, mLinearVelocity.NormalizedOr(Vec3::sZero()), inShape, contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter); + + // Test if this results in penetration, if so cancel the transition + for (const Contact &c : contacts) + if (c.mDistance < -inMaxPenetrationDepth + && !c.mIsSensorB) + return false; + + StoreActiveContacts(contacts, inAllocator); + } + + // Set new shape + mShape = inShape; + } + + return mShape == inShape; +} + +void CharacterVirtual::SetInnerBodyShape(const Shape *inShape) +{ + mSystem->GetBodyInterface().SetShape(mInnerBodyID, inShape, false, EActivation::DontActivate); +} + +bool CharacterVirtual::CanWalkStairs(Vec3Arg inLinearVelocity) const +{ + // We can only walk stairs if we're supported + if (!IsSupported()) + return false; + + // Check if there's enough horizontal velocity to trigger a stair walk + Vec3 horizontal_velocity = inLinearVelocity - inLinearVelocity.Dot(mUp) * mUp; + if (horizontal_velocity.IsNearZero(1.0e-6f)) + return false; + + // Check contacts for steep slopes + for (const Contact &c : mActiveContacts) + if (c.mHadCollision + && c.mSurfaceNormal.Dot(horizontal_velocity - c.mLinearVelocity) < 0.0f // Pushing into the contact + && IsSlopeTooSteep(c.mSurfaceNormal)) // Slope too steep + return true; + + return false; +} + +bool CharacterVirtual::WalkStairs(float inDeltaTime, Vec3Arg inStepUp, Vec3Arg inStepForward, Vec3Arg inStepForwardTest, Vec3Arg inStepDownExtra, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator) +{ + // Move up + Vec3 up = inStepUp; + Contact contact; + IgnoredContactList dummy_ignored_contacts(inAllocator); + if (GetFirstContactForSweep(mPosition, up, contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter)) + { + if (contact.mFraction < 1.0e-6f) + return false; // No movement, cancel + + // Limit up movement to the first contact point + up *= contact.mFraction; + } + RVec3 up_position = mPosition + up; + +#ifdef JPH_DEBUG_RENDERER + // Draw sweep up + if (sDrawWalkStairs) + DebugRenderer::sInstance->DrawArrow(mPosition, up_position, Color::sWhite, 0.01f); +#endif // JPH_DEBUG_RENDERER + + // Collect normals of steep slopes that we would like to walk stairs on. + // We need to do this before calling MoveShape because it will update mActiveContacts. + Vec3 character_velocity = inStepForward / inDeltaTime; + Vec3 horizontal_velocity = character_velocity - character_velocity.Dot(mUp) * mUp; + Array> steep_slope_normals(inAllocator); + steep_slope_normals.reserve(mActiveContacts.size()); + for (const Contact &c : mActiveContacts) + if (c.mHadCollision + && c.mSurfaceNormal.Dot(horizontal_velocity - c.mLinearVelocity) < 0.0f // Pushing into the contact + && IsSlopeTooSteep(c.mSurfaceNormal)) // Slope too steep + steep_slope_normals.push_back(c.mSurfaceNormal); + if (steep_slope_normals.empty()) + return false; // No steep slopes, cancel + + // Horizontal movement + RVec3 new_position = up_position; + MoveShape(new_position, character_velocity, inDeltaTime, nullptr, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator); + Vec3 horizontal_movement = Vec3(new_position - up_position); + float horizontal_movement_sq = horizontal_movement.LengthSq(); + if (horizontal_movement_sq < 1.0e-8f) + return false; // No movement, cancel + + // Check if we made any progress towards any of the steep slopes, if not we just slid along the slope + // so we need to cancel the stair walk or else we will move faster than we should as we've done + // normal movement first and then stair walk. + bool made_progress = false; + float max_dot = -0.05f * inStepForward.Length(); + for (const Vec3 &normal : steep_slope_normals) + if (normal.Dot(horizontal_movement) < max_dot) + { + // We moved more than 5% of the forward step against a steep slope, accept this as progress + made_progress = true; + break; + } + if (!made_progress) + return false; + +#ifdef JPH_DEBUG_RENDERER + // Draw horizontal sweep + if (sDrawWalkStairs) + DebugRenderer::sInstance->DrawArrow(up_position, new_position, Color::sWhite, 0.01f); +#endif // JPH_DEBUG_RENDERER + + // Move down towards the floor. + // Note that we travel the same amount down as we traveled up with the specified extra + Vec3 down = -up + inStepDownExtra; + if (!GetFirstContactForSweep(new_position, down, contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter)) + return false; // No floor found, we're in mid air, cancel stair walk + +#ifdef JPH_DEBUG_RENDERER + // Draw sweep down + if (sDrawWalkStairs) + { + RVec3 debug_pos = new_position + contact.mFraction * down; + DebugRenderer::sInstance->DrawArrow(new_position, debug_pos, Color::sWhite, 0.01f); + DebugRenderer::sInstance->DrawArrow(contact.mPosition, contact.mPosition + contact.mSurfaceNormal, Color::sWhite, 0.01f); + mShape->Draw(DebugRenderer::sInstance, GetCenterOfMassTransform(debug_pos, mRotation, mShape), Vec3::sReplicate(1.0f), Color::sWhite, false, true); + } +#endif // JPH_DEBUG_RENDERER + + // Test for floor that will support the character + if (IsSlopeTooSteep(contact.mSurfaceNormal)) + { + // If no test position was provided, we cancel the stair walk + if (inStepForwardTest.IsNearZero()) + return false; + + // Delta time may be very small, so it may be that we hit the edge of a step and the normal is too horizontal. + // In order to judge if the floor is flat further along the sweep, we test again for a floor at inStepForwardTest + // and check if the normal is valid there. + RVec3 test_position = up_position; + MoveShape(test_position, inStepForwardTest / inDeltaTime, inDeltaTime, nullptr, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator); + float test_horizontal_movement_sq = Vec3(test_position - up_position).LengthSq(); + if (test_horizontal_movement_sq <= horizontal_movement_sq + 1.0e-8f) + return false; // We didn't move any further than in the previous test + + #ifdef JPH_DEBUG_RENDERER + // Draw 2nd sweep horizontal + if (sDrawWalkStairs) + DebugRenderer::sInstance->DrawArrow(up_position, test_position, Color::sCyan, 0.01f); + #endif // JPH_DEBUG_RENDERER + + // Then sweep down + Contact test_contact; + if (!GetFirstContactForSweep(test_position, down, test_contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter)) + return false; + + #ifdef JPH_DEBUG_RENDERER + // Draw 2nd sweep down + if (sDrawWalkStairs) + { + RVec3 debug_pos = test_position + test_contact.mFraction * down; + DebugRenderer::sInstance->DrawArrow(test_position, debug_pos, Color::sCyan, 0.01f); + DebugRenderer::sInstance->DrawArrow(test_contact.mPosition, test_contact.mPosition + test_contact.mSurfaceNormal, Color::sCyan, 0.01f); + mShape->Draw(DebugRenderer::sInstance, GetCenterOfMassTransform(debug_pos, mRotation, mShape), Vec3::sReplicate(1.0f), Color::sCyan, false, true); + } + #endif // JPH_DEBUG_RENDERER + + if (IsSlopeTooSteep(test_contact.mSurfaceNormal)) + return false; + } + + // Calculate new down position + down *= contact.mFraction; + new_position += down; + + // Move the character to the new location + MoveToContact(new_position, contact, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator); + + // Override ground state to 'on ground', it is possible that the contact normal is too steep, but in this case the inStepForwardTest has found a contact normal that is not too steep + mGroundState = EGroundState::OnGround; + + return true; +} + +bool CharacterVirtual::StickToFloor(Vec3Arg inStepDown, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator) +{ + // Try to find the floor + Contact contact; + IgnoredContactList dummy_ignored_contacts(inAllocator); + if (!GetFirstContactForSweep(mPosition, inStepDown, contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter)) + return false; // If no floor found, don't update our position + + // Calculate new position + RVec3 new_position = mPosition + contact.mFraction * inStepDown; + +#ifdef JPH_DEBUG_RENDERER + // Draw sweep down + if (sDrawStickToFloor) + { + DebugRenderer::sInstance->DrawArrow(mPosition, new_position, Color::sOrange, 0.01f); + mShape->Draw(DebugRenderer::sInstance, GetCenterOfMassTransform(new_position, mRotation, mShape), Vec3::sReplicate(1.0f), Color::sOrange, false, true); + } +#endif // JPH_DEBUG_RENDERER + + // Move the character to the new location + MoveToContact(new_position, contact, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator); + return true; +} + +void CharacterVirtual::ExtendedUpdate(float inDeltaTime, Vec3Arg inGravity, const ExtendedUpdateSettings &inSettings, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator) +{ + // Update the velocity + Vec3 desired_velocity = mLinearVelocity; + mLinearVelocity = CancelVelocityTowardsSteepSlopes(desired_velocity); + + // Remember old position + RVec3 old_position = mPosition; + + // Track if on ground before the update + bool ground_to_air = IsSupported(); + + // Update the character position (instant, do not have to wait for physics update) + Update(inDeltaTime, inGravity, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator); + + // ... and that we got into air after + if (IsSupported()) + ground_to_air = false; + + // If stick to floor enabled and we're going from supported to not supported + if (ground_to_air && !inSettings.mStickToFloorStepDown.IsNearZero()) + { + // If we're not moving up, stick to the floor + float velocity = Vec3(mPosition - old_position).Dot(mUp) / inDeltaTime; + if (velocity <= 1.0e-6f) + StickToFloor(inSettings.mStickToFloorStepDown, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator); + } + + // If walk stairs enabled + if (!inSettings.mWalkStairsStepUp.IsNearZero()) + { + // Calculate how much we wanted to move horizontally + Vec3 desired_horizontal_step = desired_velocity * inDeltaTime; + desired_horizontal_step -= desired_horizontal_step.Dot(mUp) * mUp; + float desired_horizontal_step_len = desired_horizontal_step.Length(); + if (desired_horizontal_step_len > 0.0f) + { + // Calculate how much we moved horizontally + Vec3 achieved_horizontal_step = Vec3(mPosition - old_position); + achieved_horizontal_step -= achieved_horizontal_step.Dot(mUp) * mUp; + + // Only count movement in the direction of the desired movement + // (otherwise we find it ok if we're sliding downhill while we're trying to climb uphill) + Vec3 step_forward_normalized = desired_horizontal_step / desired_horizontal_step_len; + achieved_horizontal_step = max(0.0f, achieved_horizontal_step.Dot(step_forward_normalized)) * step_forward_normalized; + float achieved_horizontal_step_len = achieved_horizontal_step.Length(); + + // If we didn't move as far as we wanted and we're against a slope that's too steep + if (achieved_horizontal_step_len + 1.0e-4f < desired_horizontal_step_len + && CanWalkStairs(desired_velocity)) + { + // Calculate how much we should step forward + // Note that we clamp the step forward to a minimum distance. This is done because at very high frame rates the delta time + // may be very small, causing a very small step forward. If the step becomes small enough, we may not move far enough + // horizontally to actually end up at the top of the step. + Vec3 step_forward = step_forward_normalized * max(inSettings.mWalkStairsMinStepForward, desired_horizontal_step_len - achieved_horizontal_step_len); + + // Calculate how far to scan ahead for a floor. This is only used in case the floor normal at step_forward is too steep. + // In that case an additional check will be performed at this distance to check if that normal is not too steep. + // Start with the ground normal in the horizontal plane and normalizing it + Vec3 step_forward_test = -mGroundNormal; + step_forward_test -= step_forward_test.Dot(mUp) * mUp; + step_forward_test = step_forward_test.NormalizedOr(step_forward_normalized); + + // If this normalized vector and the character forward vector is bigger than a preset angle, we use the character forward vector instead of the ground normal + // to do our forward test + if (step_forward_test.Dot(step_forward_normalized) < inSettings.mWalkStairsCosAngleForwardContact) + step_forward_test = step_forward_normalized; + + // Calculate the correct magnitude for the test vector + step_forward_test *= inSettings.mWalkStairsStepForwardTest; + + WalkStairs(inDeltaTime, inSettings.mWalkStairsStepUp, step_forward, step_forward_test, inSettings.mWalkStairsStepDownExtra, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator); + } + } + } +} + +void CharacterVirtual::Contact::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mPosition); + inStream.Write(mLinearVelocity); + inStream.Write(mContactNormal); + inStream.Write(mSurfaceNormal); + inStream.Write(mDistance); + inStream.Write(mFraction); + inStream.Write(mBodyB); + inStream.Write(mSubShapeIDB); + inStream.Write(mMotionTypeB); + inStream.Write(mHadCollision); + inStream.Write(mWasDiscarded); + inStream.Write(mCanPushCharacter); + // Cannot store user data (may be a pointer) and material +} + +void CharacterVirtual::Contact::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mPosition); + inStream.Read(mLinearVelocity); + inStream.Read(mContactNormal); + inStream.Read(mSurfaceNormal); + inStream.Read(mDistance); + inStream.Read(mFraction); + inStream.Read(mBodyB); + inStream.Read(mSubShapeIDB); + inStream.Read(mMotionTypeB); + inStream.Read(mHadCollision); + inStream.Read(mWasDiscarded); + inStream.Read(mCanPushCharacter); + mUserData = 0; // Cannot restore user data + mMaterial = PhysicsMaterial::sDefault; // Cannot restore material +} + +void CharacterVirtual::SaveState(StateRecorder &inStream) const +{ + CharacterBase::SaveState(inStream); + + inStream.Write(mPosition); + inStream.Write(mRotation); + inStream.Write(mLinearVelocity); + inStream.Write(mLastDeltaTime); + inStream.Write(mMaxHitsExceeded); + + // Store contacts that had collision, we're using it at the beginning of the step in CancelVelocityTowardsSteepSlopes + uint32 num_contacts = 0; + for (const Contact &c : mActiveContacts) + if (c.mHadCollision) + ++num_contacts; + inStream.Write(num_contacts); + for (const Contact &c : mActiveContacts) + if (c.mHadCollision) + c.SaveState(inStream); +} + +void CharacterVirtual::RestoreState(StateRecorder &inStream) +{ + CharacterBase::RestoreState(inStream); + + inStream.Read(mPosition); + inStream.Read(mRotation); + inStream.Read(mLinearVelocity); + inStream.Read(mLastDeltaTime); + inStream.Read(mMaxHitsExceeded); + + // When validating remove contacts that don't have collision since we didn't save them + if (inStream.IsValidating()) + for (int i = (int)mActiveContacts.size() - 1; i >= 0; --i) + if (!mActiveContacts[i].mHadCollision) + mActiveContacts.erase(mActiveContacts.begin() + i); + + uint32 num_contacts = (uint32)mActiveContacts.size(); + inStream.Read(num_contacts); + mActiveContacts.resize(num_contacts); + for (Contact &c : mActiveContacts) + c.RestoreState(inStream); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.h b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.h new file mode 100644 index 000000000000..65b8f89df560 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.h @@ -0,0 +1,642 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class CharacterVirtual; +class CollideShapeSettings; + +/// Contains the configuration of a character +class JPH_EXPORT CharacterVirtualSettings : public CharacterBaseSettings +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Character mass (kg). Used to push down objects with gravity when the character is standing on top. + float mMass = 70.0f; + + /// Maximum force with which the character can push other bodies (N). + float mMaxStrength = 100.0f; + + /// An extra offset applied to the shape in local space. This allows applying an extra offset to the shape in local space. + Vec3 mShapeOffset = Vec3::sZero(); + + ///@name Movement settings + EBackFaceMode mBackFaceMode = EBackFaceMode::CollideWithBackFaces; ///< When colliding with back faces, the character will not be able to move through back facing triangles. Use this if you have triangles that need to collide on both sides. + float mPredictiveContactDistance = 0.1f; ///< How far to scan outside of the shape for predictive contacts. A value of 0 will most likely cause the character to get stuck as it cannot properly calculate a sliding direction anymore. A value that's too high will cause ghost collisions. + uint mMaxCollisionIterations = 5; ///< Max amount of collision loops + uint mMaxConstraintIterations = 15; ///< How often to try stepping in the constraint solving + float mMinTimeRemaining = 1.0e-4f; ///< Early out condition: If this much time is left to simulate we are done + float mCollisionTolerance = 1.0e-3f; ///< How far we're willing to penetrate geometry + float mCharacterPadding = 0.02f; ///< How far we try to stay away from the geometry, this ensures that the sweep will hit as little as possible lowering the collision cost and reducing the risk of getting stuck + uint mMaxNumHits = 256; ///< Max num hits to collect in order to avoid excess of contact points collection + float mHitReductionCosMaxAngle = 0.999f; ///< Cos(angle) where angle is the maximum angle between two hits contact normals that are allowed to be merged during hit reduction. Default is around 2.5 degrees. Set to -1 to turn off. + float mPenetrationRecoverySpeed = 1.0f; ///< This value governs how fast a penetration will be resolved, 0 = nothing is resolved, 1 = everything in one update + + /// This character can optionally have an inner rigid body. This rigid body can be used to give the character presence in the world. When set it means that: + /// - Regular collision checks (e.g. NarrowPhaseQuery::CastRay) will collide with the rigid body (they cannot collide with CharacterVirtual since it is not added to the broad phase) + /// - Regular contact callbacks will be called through the ContactListener (next to the ones that will be passed to the CharacterContactListener) + /// - Fast moving objects of motion quality LinearCast will not be able to pass through the CharacterVirtual in 1 time step + RefConst mInnerBodyShape; + + /// Layer that the inner rigid body will be added to + ObjectLayer mInnerBodyLayer = 0; +}; + +/// This class contains settings that allow you to override the behavior of a character's collision response +class CharacterContactSettings +{ +public: + /// True when the object can push the virtual character. + bool mCanPushCharacter = true; + + /// True when the virtual character can apply impulses (push) the body. + /// Note that this only works against rigid bodies. Other CharacterVirtual objects can only be moved in their own update, + /// so you must ensure that in their OnCharacterContactAdded mCanPushCharacter is true. + bool mCanReceiveImpulses = true; +}; + +/// This class receives callbacks when a virtual character hits something. +class JPH_EXPORT CharacterContactListener +{ +public: + /// Destructor + virtual ~CharacterContactListener() = default; + + /// Callback to adjust the velocity of a body as seen by the character. Can be adjusted to e.g. implement a conveyor belt or an inertial dampener system of a sci-fi space ship. + /// Note that inBody2 is locked during the callback so you can read its properties freely. + virtual void OnAdjustBodyVelocity(const CharacterVirtual *inCharacter, const Body &inBody2, Vec3 &ioLinearVelocity, Vec3 &ioAngularVelocity) { /* Do nothing, the linear and angular velocity are already filled in */ } + + /// Checks if a character can collide with specified body. Return true if the contact is valid. + virtual bool OnContactValidate(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2) { return true; } + + /// Same as OnContactValidate but when colliding with a CharacterVirtual + virtual bool OnCharacterContactValidate(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2) { return true; } + + /// Called whenever the character collides with a body. + /// @param inCharacter Character that is being solved + /// @param inBodyID2 Body ID of body that is being hit + /// @param inSubShapeID2 Sub shape ID of shape that is being hit + /// @param inContactPosition World space contact position + /// @param inContactNormal World space contact normal + /// @param ioSettings Settings returned by the contact callback to indicate how the character should behave + virtual void OnContactAdded(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) { /* Default do nothing */ } + + /// Same as OnContactAdded but when colliding with a CharacterVirtual + virtual void OnCharacterContactAdded(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) { /* Default do nothing */ } + + /// Called whenever a contact is being used by the solver. Allows the listener to override the resulting character velocity (e.g. by preventing sliding along certain surfaces). + /// @param inCharacter Character that is being solved + /// @param inBodyID2 Body ID of body that is being hit + /// @param inSubShapeID2 Sub shape ID of shape that is being hit + /// @param inContactPosition World space contact position + /// @param inContactNormal World space contact normal + /// @param inContactVelocity World space velocity of contact point (e.g. for a moving platform) + /// @param inContactMaterial Material of contact point + /// @param inCharacterVelocity World space velocity of the character prior to hitting this contact + /// @param ioNewCharacterVelocity Contains the calculated world space velocity of the character after hitting this contact, this velocity slides along the surface of the contact. Can be modified by the listener to provide an alternative velocity. + virtual void OnContactSolve(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, Vec3Arg inContactVelocity, const PhysicsMaterial *inContactMaterial, Vec3Arg inCharacterVelocity, Vec3 &ioNewCharacterVelocity) { /* Default do nothing */ } + + /// Same as OnContactSolve but when colliding with a CharacterVirtual + virtual void OnCharacterContactSolve(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, Vec3Arg inContactVelocity, const PhysicsMaterial *inContactMaterial, Vec3Arg inCharacterVelocity, Vec3 &ioNewCharacterVelocity) { /* Default do nothing */ } +}; + +/// Interface class that allows a CharacterVirtual to check collision with other CharacterVirtual instances. +/// Since CharacterVirtual instances are not registered anywhere, it is up to the application to test collision against relevant characters. +/// The characters could be stored in a tree structure to make this more efficient. +class JPH_EXPORT CharacterVsCharacterCollision : public NonCopyable +{ +public: + virtual ~CharacterVsCharacterCollision() = default; + + /// Collide a character against other CharacterVirtuals. + /// @param inCharacter The character to collide. + /// @param inCenterOfMassTransform Center of mass transform for this character. + /// @param inCollideShapeSettings Settings for the collision check. + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. GetPosition() since floats are most accurate near the origin + /// @param ioCollector Collision collector that receives the collision results. + virtual void CollideCharacter(const CharacterVirtual *inCharacter, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector) const = 0; + + /// Cast a character against other CharacterVirtuals. + /// @param inCharacter The character to cast. + /// @param inCenterOfMassTransform Center of mass transform for this character. + /// @param inDirection Direction and length to cast in. + /// @param inShapeCastSettings Settings for the shape cast. + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. GetPosition() since floats are most accurate near the origin + /// @param ioCollector Collision collector that receives the collision results. + virtual void CastCharacter(const CharacterVirtual *inCharacter, RMat44Arg inCenterOfMassTransform, Vec3Arg inDirection, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector) const = 0; +}; + +/// Simple collision checker that loops over all registered characters. +/// Note that this is not thread safe, so make sure that only one CharacterVirtual is checking collision at a time. +class JPH_EXPORT CharacterVsCharacterCollisionSimple : public CharacterVsCharacterCollision +{ +public: + /// Add a character to the list of characters to check collision against. + void Add(CharacterVirtual *inCharacter) { mCharacters.push_back(inCharacter); } + + /// Remove a character from the list of characters to check collision against. + void Remove(const CharacterVirtual *inCharacter); + + // See: CharacterVsCharacterCollision + virtual void CollideCharacter(const CharacterVirtual *inCharacter, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector) const override; + virtual void CastCharacter(const CharacterVirtual *inCharacter, RMat44Arg inCenterOfMassTransform, Vec3Arg inDirection, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector) const override; + + Array mCharacters; ///< The list of characters to check collision against +}; + +/// Runtime character object. +/// This object usually represents the player. Contrary to the Character class it doesn't use a rigid body but moves doing collision checks only (hence the name virtual). +/// The advantage of this is that you can determine when the character moves in the frame (usually this has to happen at a very particular point in the frame) +/// but the downside is that other objects don't see this virtual character. In order to make this work it is recommended to pair a CharacterVirtual with a Character that +/// moves along. This Character should be keyframed (or at least have no gravity) and move along with the CharacterVirtual so that other rigid bodies can collide with it. +class JPH_EXPORT CharacterVirtual : public CharacterBase +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + /// @param inSettings The settings for the character + /// @param inPosition Initial position for the character + /// @param inRotation Initial rotation for the character (usually only around the up-axis) + /// @param inUserData Application specific value + /// @param inSystem Physics system that this character will be added to + CharacterVirtual(const CharacterVirtualSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem); + + /// Constructor without user data + CharacterVirtual(const CharacterVirtualSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, PhysicsSystem *inSystem) : CharacterVirtual(inSettings, inPosition, inRotation, 0, inSystem) { } + + /// Destructor + virtual ~CharacterVirtual() override; + + /// Set the contact listener + void SetListener(CharacterContactListener *inListener) { mListener = inListener; } + + /// Get the current contact listener + CharacterContactListener * GetListener() const { return mListener; } + + /// Set the character vs character collision interface + void SetCharacterVsCharacterCollision(CharacterVsCharacterCollision *inCharacterVsCharacterCollision) { mCharacterVsCharacterCollision = inCharacterVsCharacterCollision; } + + /// Get the linear velocity of the character (m / s) + Vec3 GetLinearVelocity() const { return mLinearVelocity; } + + /// Set the linear velocity of the character (m / s) + void SetLinearVelocity(Vec3Arg inLinearVelocity) { mLinearVelocity = inLinearVelocity; } + + /// Get the position of the character + RVec3 GetPosition() const { return mPosition; } + + /// Set the position of the character + void SetPosition(RVec3Arg inPosition) { mPosition = inPosition; UpdateInnerBodyTransform(); } + + /// Get the rotation of the character + Quat GetRotation() const { return mRotation; } + + /// Set the rotation of the character + void SetRotation(QuatArg inRotation) { mRotation = inRotation; UpdateInnerBodyTransform(); } + + // Get the center of mass position of the shape + inline RVec3 GetCenterOfMassPosition() const { return mPosition + (mRotation * (mShapeOffset + mShape->GetCenterOfMass()) + mCharacterPadding * mUp); } + + /// Calculate the world transform of the character + RMat44 GetWorldTransform() const { return RMat44::sRotationTranslation(mRotation, mPosition); } + + /// Calculates the transform for this character's center of mass + RMat44 GetCenterOfMassTransform() const { return GetCenterOfMassTransform(mPosition, mRotation, mShape); } + + /// Character mass (kg) + float GetMass() const { return mMass; } + void SetMass(float inMass) { mMass = inMass; } + + /// Maximum force with which the character can push other bodies (N) + float GetMaxStrength() const { return mMaxStrength; } + void SetMaxStrength(float inMaxStrength) { mMaxStrength = inMaxStrength; } + + /// This value governs how fast a penetration will be resolved, 0 = nothing is resolved, 1 = everything in one update + float GetPenetrationRecoverySpeed() const { return mPenetrationRecoverySpeed; } + void SetPenetrationRecoverySpeed(float inSpeed) { mPenetrationRecoverySpeed = inSpeed; } + + /// Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges. + bool GetEnhancedInternalEdgeRemoval() const { return mEnhancedInternalEdgeRemoval; } + void SetEnhancedInternalEdgeRemoval(bool inApply) { mEnhancedInternalEdgeRemoval = inApply; } + + /// Character padding + float GetCharacterPadding() const { return mCharacterPadding; } + + /// Max num hits to collect in order to avoid excess of contact points collection + uint GetMaxNumHits() const { return mMaxNumHits; } + void SetMaxNumHits(uint inMaxHits) { mMaxNumHits = inMaxHits; } + + /// Cos(angle) where angle is the maximum angle between two hits contact normals that are allowed to be merged during hit reduction. Default is around 2.5 degrees. Set to -1 to turn off. + float GetHitReductionCosMaxAngle() const { return mHitReductionCosMaxAngle; } + void SetHitReductionCosMaxAngle(float inCosMaxAngle) { mHitReductionCosMaxAngle = inCosMaxAngle; } + + /// Returns if we exceeded the maximum number of hits during the last collision check and had to discard hits based on distance. + /// This can be used to find areas that have too complex geometry for the character to navigate properly. + /// To solve you can either increase the max number of hits or simplify the geometry. Note that the character simulation will + /// try to do its best to select the most relevant contacts to avoid the character from getting stuck. + bool GetMaxHitsExceeded() const { return mMaxHitsExceeded; } + + /// An extra offset applied to the shape in local space. This allows applying an extra offset to the shape in local space. Note that setting it on the fly can cause the shape to teleport into collision. + Vec3 GetShapeOffset() const { return mShapeOffset; } + void SetShapeOffset(Vec3Arg inShapeOffset) { mShapeOffset = inShapeOffset; UpdateInnerBodyTransform(); } + + /// Access to the user data, can be used for anything by the application + uint64 GetUserData() const { return mUserData; } + void SetUserData(uint64 inUserData); + + /// Optional inner rigid body that proxies the character in the world. Can be used to update body properties. + BodyID GetInnerBodyID() const { return mInnerBodyID; } + + /// This function can be called prior to calling Update() to convert a desired velocity into a velocity that won't make the character move further onto steep slopes. + /// This velocity can then be set on the character using SetLinearVelocity() + /// @param inDesiredVelocity Velocity to clamp against steep walls + /// @return A new velocity vector that won't make the character move up steep slopes + Vec3 CancelVelocityTowardsSteepSlopes(Vec3Arg inDesiredVelocity) const; + + /// This is the main update function. It moves the character according to its current velocity (the character is similar to a kinematic body in the sense + /// that you set the velocity and the character will follow unless collision is blocking the way). Note it's your own responsibility to apply gravity to the character velocity! + /// Different surface materials (like ice) can be emulated by getting the ground material and adjusting the velocity and/or the max slope angle accordingly every frame. + /// @param inDeltaTime Time step to simulate. + /// @param inGravity Gravity vector (m/s^2). This gravity vector is only used when the character is standing on top of another object to apply downward force. + /// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase. + /// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer. + /// @param inBodyFilter Filter that is used to check if a character collides with a body. + /// @param inShapeFilter Filter that is used to check if a character collides with a subshape. + /// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns. + void Update(float inDeltaTime, Vec3Arg inGravity, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator); + + /// This function will return true if the character has moved into a slope that is too steep (e.g. a vertical wall). + /// You would call WalkStairs to attempt to step up stairs. + /// @param inLinearVelocity The linear velocity that the player desired. This is used to determine if we're pushing into a step. + bool CanWalkStairs(Vec3Arg inLinearVelocity) const; + + /// When stair walking is needed, you can call the WalkStairs function to cast up, forward and down again to try to find a valid position + /// @param inDeltaTime Time step to simulate. + /// @param inStepUp The direction and distance to step up (this corresponds to the max step height) + /// @param inStepForward The direction and distance to step forward after the step up + /// @param inStepForwardTest When running at a high frequency, inStepForward can be very small and it's likely that you hit the side of the stairs on the way down. This could produce a normal that violates the max slope angle. If this happens, we test again using this distance from the up position to see if we find a valid slope. + /// @param inStepDownExtra An additional translation that is added when stepping down at the end. Allows you to step further down than up. Set to zero if you don't want this. Should be in the opposite direction of up. + /// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase. + /// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer. + /// @param inBodyFilter Filter that is used to check if a character collides with a body. + /// @param inShapeFilter Filter that is used to check if a character collides with a subshape. + /// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns. + /// @return true if the stair walk was successful + bool WalkStairs(float inDeltaTime, Vec3Arg inStepUp, Vec3Arg inStepForward, Vec3Arg inStepForwardTest, Vec3Arg inStepDownExtra, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator); + + /// This function can be used to artificially keep the character to the floor. Normally when a character is on a small step and starts moving horizontally, the character will + /// lose contact with the floor because the initial vertical velocity is zero while the horizontal velocity is quite high. To prevent the character from losing contact with the floor, + /// we do an additional collision check downwards and if we find the floor within a certain distance, we project the character onto the floor. + /// @param inStepDown Max amount to project the character downwards (if no floor is found within this distance, the function will return false) + /// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase. + /// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer. + /// @param inBodyFilter Filter that is used to check if a character collides with a body. + /// @param inShapeFilter Filter that is used to check if a character collides with a subshape. + /// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns. + /// @return True if the character was successfully projected onto the floor. + bool StickToFloor(Vec3Arg inStepDown, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator); + + /// Settings struct with settings for ExtendedUpdate + struct ExtendedUpdateSettings + { + Vec3 mStickToFloorStepDown { 0, -0.5f, 0 }; ///< See StickToFloor inStepDown parameter. Can be zero to turn off. + Vec3 mWalkStairsStepUp { 0, 0.4f, 0 }; ///< See WalkStairs inStepUp parameter. Can be zero to turn off. + float mWalkStairsMinStepForward { 0.02f }; ///< See WalkStairs inStepForward parameter. Note that the parameter only indicates a magnitude, direction is taken from current velocity. + float mWalkStairsStepForwardTest { 0.15f }; ///< See WalkStairs inStepForwardTest parameter. Note that the parameter only indicates a magnitude, direction is taken from current velocity. + float mWalkStairsCosAngleForwardContact { Cos(DegreesToRadians(75.0f)) }; ///< Cos(angle) where angle is the maximum angle between the ground normal in the horizontal plane and the character forward vector where we're willing to adjust the step forward test towards the contact normal. + Vec3 mWalkStairsStepDownExtra { Vec3::sZero() }; ///< See WalkStairs inStepDownExtra + }; + + /// This function combines Update, StickToFloor and WalkStairs. This function serves as an example of how these functions could be combined. + /// Before calling, call SetLinearVelocity to update the horizontal/vertical speed of the character, typically this is: + /// - When on OnGround and not moving away from ground: velocity = GetGroundVelocity() + horizontal speed as input by player + optional vertical jump velocity + delta time * gravity + /// - Else: velocity = current vertical velocity + horizontal speed as input by player + delta time * gravity + /// @param inDeltaTime Time step to simulate. + /// @param inGravity Gravity vector (m/s^2). This gravity vector is only used when the character is standing on top of another object to apply downward force. + /// @param inSettings A structure containing settings for the algorithm. + /// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase. + /// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer. + /// @param inBodyFilter Filter that is used to check if a character collides with a body. + /// @param inShapeFilter Filter that is used to check if a character collides with a subshape. + /// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns. + void ExtendedUpdate(float inDeltaTime, Vec3Arg inGravity, const ExtendedUpdateSettings &inSettings, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator); + + /// This function can be used after a character has teleported to determine the new contacts with the world. + void RefreshContacts(const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator); + + /// Use the ground body ID to get an updated estimate of the ground velocity. This function can be used if the ground body has moved / changed velocity and you want a new estimate of the ground velocity. + /// It will not perform collision detection, so is less accurate than RefreshContacts but a lot faster. + void UpdateGroundVelocity(); + + /// Switch the shape of the character (e.g. for stance). + /// @param inShape The shape to switch to. + /// @param inMaxPenetrationDepth When inMaxPenetrationDepth is not FLT_MAX, it checks if the new shape collides before switching shape. This is the max penetration we're willing to accept after the switch. + /// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase. + /// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer. + /// @param inBodyFilter Filter that is used to check if a character collides with a body. + /// @param inShapeFilter Filter that is used to check if a character collides with a subshape. + /// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns. + /// @return Returns true if the switch succeeded. + bool SetShape(const Shape *inShape, float inMaxPenetrationDepth, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator); + + /// Updates the shape of the inner rigid body. Should be called after a successful call to SetShape. + void SetInnerBodyShape(const Shape *inShape); + + /// Get the transformed shape that represents the volume of the character, can be used for collision checks. + TransformedShape GetTransformedShape() const { return TransformedShape(GetCenterOfMassPosition(), mRotation, mShape, mInnerBodyID); } + + /// @brief Get all contacts for the character at a particular location. + /// When colliding with another character virtual, this pointer will be provided through CollideShapeCollector::SetUserContext before adding a hit. + /// @param inPosition Position to test, note that this position will be corrected for the character padding. + /// @param inRotation Rotation at which to test the shape. + /// @param inMovementDirection A hint in which direction the character is moving, will be used to calculate a proper normal. + /// @param inMaxSeparationDistance How much distance around the character you want to report contacts in (can be 0 to match the character exactly). + /// @param inShape Shape to test collision with. + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. GetPosition() since floats are most accurate near the origin + /// @param ioCollector Collision collector that receives the collision results. + /// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase. + /// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer. + /// @param inBodyFilter Filter that is used to check if a character collides with a body. + /// @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; + + // Saving / restoring state for replay + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + +#ifdef JPH_DEBUG_RENDERER + static inline bool sDrawConstraints = false; ///< Draw the current state of the constraints for iteration 0 when creating them + static inline bool sDrawWalkStairs = false; ///< Draw the state of the walk stairs algorithm + static inline bool sDrawStickToFloor = false; ///< Draw the state of the stick to floor algorithm +#endif + + // Encapsulates a collision contact + struct Contact + { + // Saving / restoring state for replay + void SaveState(StateRecorder &inStream) const; + void RestoreState(StateRecorder &inStream); + + // Checks if two contacts refer to the same body (or virtual character) + inline bool IsSameBody(const Contact &inOther) const { return mBodyB == inOther.mBodyB && mCharacterB == inOther.mCharacterB; } + + RVec3 mPosition; ///< Position where the character makes contact + Vec3 mLinearVelocity; ///< Velocity of the contact point + Vec3 mContactNormal; ///< Contact normal, pointing towards the character + Vec3 mSurfaceNormal; ///< Surface normal of the contact + float mDistance; ///< Distance to the contact <= 0 means that it is an actual contact, > 0 means predictive + float mFraction; ///< Fraction along the path where this contact takes place + BodyID mBodyB; ///< ID of body we're colliding with (if not invalid) + CharacterVirtual * mCharacterB = nullptr; ///< Character we're colliding with (if not null) + SubShapeID mSubShapeIDB; ///< Sub shape ID of body we're colliding with + EMotionType mMotionTypeB; ///< Motion type of B, used to determine the priority of the contact + bool mIsSensorB; ///< If B is a sensor + uint64 mUserData; ///< User data of B + const PhysicsMaterial * mMaterial; ///< Material of B + bool mHadCollision = false; ///< If the character actually collided with the contact (can be false if a predictive contact never becomes a real one) + bool mWasDiscarded = false; ///< If the contact validate callback chose to discard this contact + bool mCanPushCharacter = true; ///< When true, the velocity of the contact point can push the character + }; + + using TempContactList = Array>; + using ContactList = Array; + + /// Access to the internal list of contacts that the character has found. + const ContactList & GetActiveContacts() const { return mActiveContacts; } + + /// Check if the character is currently in contact with or has collided with another body in the last time step + bool HasCollidedWith(const BodyID &inBody) const + { + for (const CharacterVirtual::Contact &c : mActiveContacts) + if (c.mHadCollision && c.mBodyB == inBody) + return true; + return false; + } + + /// Check if the character is currently in contact with or has collided with another character in the last time step + bool HasCollidedWith(const CharacterVirtual *inCharacter) const + { + for (const CharacterVirtual::Contact &c : mActiveContacts) + if (c.mHadCollision && c.mCharacterB == inCharacter) + return true; + return false; + } + +private: + // Sorting predicate for making contact order deterministic + struct ContactOrderingPredicate + { + inline bool operator () (const Contact &inLHS, const Contact &inRHS) const + { + if (inLHS.mBodyB != inRHS.mBodyB) + return inLHS.mBodyB < inRHS.mBodyB; + + return inLHS.mSubShapeIDB.GetValue() < inRHS.mSubShapeIDB.GetValue(); + } + }; + + // A contact that needs to be ignored + struct IgnoredContact + { + IgnoredContact() = default; + IgnoredContact(const BodyID &inBodyID, const SubShapeID &inSubShapeID) : mBodyID(inBodyID), mSubShapeID(inSubShapeID) { } + + BodyID mBodyID; ///< ID of body we're colliding with + SubShapeID mSubShapeID; ///< Sub shape of body we're colliding with + }; + + using IgnoredContactList = Array>; + + // A constraint that limits the movement of the character + struct Constraint + { + Contact * mContact; ///< Contact that this constraint was generated from + float mTOI; ///< Calculated time of impact (can be negative if penetrating) + float mProjectedVelocity; ///< Velocity of the contact projected on the contact normal (negative if separating) + Vec3 mLinearVelocity; ///< Velocity of the contact (can contain a corrective velocity to resolve penetration) + Plane mPlane; ///< Plane around the origin that describes how far we can displace (from the origin) + bool mIsSteepSlope = false; ///< If this constraint belongs to a steep slope + }; + + using ConstraintList = Array>; + + // Collision collector that collects hits for CollideShape + class ContactCollector : public CollideShapeCollector + { + public: + ContactCollector(PhysicsSystem *inSystem, const CharacterVirtual *inCharacter, uint inMaxHits, float inHitReductionCosMaxAngle, Vec3Arg inUp, RVec3Arg inBaseOffset, TempContactList &outContacts) : mBaseOffset(inBaseOffset), mUp(inUp), mSystem(inSystem), mCharacter(inCharacter), mContacts(outContacts), mMaxHits(inMaxHits), mHitReductionCosMaxAngle(inHitReductionCosMaxAngle) { } + + virtual void SetUserData(uint64 inUserData) override { mOtherCharacter = reinterpret_cast(inUserData); } + + virtual void AddHit(const CollideShapeResult &inResult) override; + + RVec3 mBaseOffset; + Vec3 mUp; + PhysicsSystem * mSystem; + const CharacterVirtual * mCharacter; + CharacterVirtual * mOtherCharacter = nullptr; + TempContactList & mContacts; + uint mMaxHits; + float mHitReductionCosMaxAngle; + bool mMaxHitsExceeded = false; + }; + + // A collision collector that collects hits for CastShape + class ContactCastCollector : public CastShapeCollector + { + public: + ContactCastCollector(PhysicsSystem *inSystem, const CharacterVirtual *inCharacter, Vec3Arg inDisplacement, Vec3Arg inUp, const IgnoredContactList &inIgnoredContacts, RVec3Arg inBaseOffset, Contact &outContact) : mBaseOffset(inBaseOffset), mDisplacement(inDisplacement), mUp(inUp), mSystem(inSystem), mCharacter(inCharacter), mIgnoredContacts(inIgnoredContacts), mContact(outContact) { } + + virtual void SetUserData(uint64 inUserData) override { mOtherCharacter = reinterpret_cast(inUserData); } + + virtual void AddHit(const ShapeCastResult &inResult) override; + + RVec3 mBaseOffset; + Vec3 mDisplacement; + Vec3 mUp; + PhysicsSystem * mSystem; + const CharacterVirtual * mCharacter; + CharacterVirtual * mOtherCharacter = nullptr; + const IgnoredContactList & mIgnoredContacts; + Contact & mContact; + }; + + // Helper function to convert a Jolt collision result into a contact + template + inline static void sFillContactProperties(const CharacterVirtual *inCharacter, Contact &outContact, const Body &inBody, Vec3Arg inUp, RVec3Arg inBaseOffset, const taCollector &inCollector, const CollideShapeResult &inResult); + inline static void sFillCharacterContactProperties(Contact &outContact, CharacterVirtual *inOtherCharacter, RVec3Arg inBaseOffset, const CollideShapeResult &inResult); + + // Move the shape from ioPosition and try to displace it by inVelocity * inDeltaTime, this will try to slide the shape along the world geometry + void MoveShape(RVec3 &ioPosition, Vec3Arg inVelocity, float inDeltaTime, ContactList *outActiveContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator + #ifdef JPH_DEBUG_RENDERER + , bool inDrawConstraints = false + #endif // JPH_DEBUG_RENDERER + ) const; + + // Ask the callback if inContact is a valid contact point + bool ValidateContact(const Contact &inContact) const; + + // Trigger the contact callback for inContact and get the contact settings + void ContactAdded(const Contact &inContact, CharacterContactSettings &ioSettings) const; + + // Tests the shape for collision around inPosition + void GetContactsAtPosition(RVec3Arg inPosition, Vec3Arg inMovementDirection, const Shape *inShape, TempContactList &outContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const; + + // Remove penetrating contacts with the same body that have conflicting normals, leaving these will make the character mover get stuck + void RemoveConflictingContacts(TempContactList &ioContacts, IgnoredContactList &outIgnoredContacts) const; + + // Convert contacts into constraints. The character is assumed to start at the origin and the constraints are planes around the origin that confine the movement of the character. + void DetermineConstraints(TempContactList &inContacts, float inDeltaTime, ConstraintList &outConstraints) const; + + // Use the constraints to solve the displacement of the character. This will slide the character on the planes around the origin for as far as possible. + void SolveConstraints(Vec3Arg inVelocity, float inDeltaTime, float inTimeRemaining, ConstraintList &ioConstraints, IgnoredContactList &ioIgnoredContacts, float &outTimeSimulated, Vec3 &outDisplacement, TempAllocator &inAllocator + #ifdef JPH_DEBUG_RENDERER + , bool inDrawConstraints = false + #endif // JPH_DEBUG_RENDERER + ) const; + + // Get the velocity of a body adjusted by the contact listener + void GetAdjustedBodyVelocity(const Body& inBody, Vec3 &outLinearVelocity, Vec3 &outAngularVelocity) const; + + // Calculate the ground velocity of the character assuming it's standing on an object with specified linear and angular velocity and with specified center of mass. + // Note that we don't just take the point velocity because a point on an object with angular velocity traces an arc, + // so if you just take point velocity * delta time you get an error that accumulates over time + Vec3 CalculateCharacterGroundVelocity(RVec3Arg inCenterOfMass, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, float inDeltaTime) const; + + // Handle contact with physics object that we're colliding against + bool HandleContact(Vec3Arg inVelocity, Constraint &ioConstraint, float inDeltaTime) const; + + // Does a swept test of the shape from inPosition with displacement inDisplacement, returns true if there was a collision + bool GetFirstContactForSweep(RVec3Arg inPosition, Vec3Arg inDisplacement, Contact &outContact, const IgnoredContactList &inIgnoredContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const; + + // Store contacts so that we have proper ground information + void StoreActiveContacts(const TempContactList &inContacts, TempAllocator &inAllocator); + + // This function will determine which contacts are touching the character and will calculate the one that is supporting us + void UpdateSupportingContact(bool inSkipContactVelocityCheck, TempAllocator &inAllocator); + + /// This function can be called after moving the character to a new colliding position + void MoveToContact(RVec3Arg inPosition, const Contact &inContact, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator); + + // This function returns the actual center of mass of the shape, not corrected for the character padding + inline RMat44 GetCenterOfMassTransform(RVec3Arg inPosition, QuatArg inRotation, const Shape *inShape) const + { + return RMat44::sRotationTranslation(inRotation, inPosition).PreTranslated(mShapeOffset + inShape->GetCenterOfMass()).PostTranslated(mCharacterPadding * mUp); + } + + // This function returns the position of the inner rigid body + inline RVec3 GetInnerBodyPosition() const + { + return mPosition + (mRotation * mShapeOffset + mCharacterPadding * mUp); + } + + // Move the inner rigid body to the current position + void UpdateInnerBodyTransform(); + + // Our main listener for contacts + CharacterContactListener * mListener = nullptr; + + // Interface to detect collision between characters + CharacterVsCharacterCollision * mCharacterVsCharacterCollision = nullptr; + + // Movement settings + EBackFaceMode mBackFaceMode; // When colliding with back faces, the character will not be able to move through back facing triangles. Use this if you have triangles that need to collide on both sides. + float mPredictiveContactDistance; // How far to scan outside of the shape for predictive contacts. A value of 0 will most likely cause the character to get stuck as it cannot properly calculate a sliding direction anymore. A value that's too high will cause ghost collisions. + uint mMaxCollisionIterations; // Max amount of collision loops + uint mMaxConstraintIterations; // How often to try stepping in the constraint solving + float mMinTimeRemaining; // Early out condition: If this much time is left to simulate we are done + float mCollisionTolerance; // How far we're willing to penetrate geometry + float mCharacterPadding; // How far we try to stay away from the geometry, this ensures that the sweep will hit as little as possible lowering the collision cost and reducing the risk of getting stuck + uint mMaxNumHits; // Max num hits to collect in order to avoid excess of contact points collection + float mHitReductionCosMaxAngle; // Cos(angle) where angle is the maximum angle between two hits contact normals that are allowed to be merged during hit reduction. Default is around 2.5 degrees. Set to -1 to turn off. + float mPenetrationRecoverySpeed; // This value governs how fast a penetration will be resolved, 0 = nothing is resolved, 1 = everything in one update + bool mEnhancedInternalEdgeRemoval; // Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges. + + // Character mass (kg) + float mMass; + + // Maximum force with which the character can push other bodies (N) + float mMaxStrength; + + // An extra offset applied to the shape in local space. This allows applying an extra offset to the shape in local space. + Vec3 mShapeOffset = Vec3::sZero(); + + // Current position (of the base, not the center of mass) + RVec3 mPosition = RVec3::sZero(); + + // Current rotation (of the base, not of the center of mass) + Quat mRotation = Quat::sIdentity(); + + // Current linear velocity + Vec3 mLinearVelocity = Vec3::sZero(); + + // List of contacts that were active in the last frame + ContactList mActiveContacts; + + // Remembers the delta time of the last update + float mLastDeltaTime = 1.0f / 60.0f; + + // Remember if we exceeded the maximum number of hits and had to remove similar contacts + mutable bool mMaxHitsExceeded = false; + + // User data, can be used for anything by the application + uint64 mUserData = 0; + + // The inner rigid body that proxies the character in the world + BodyID mInnerBodyID; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/AABoxCast.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/AABoxCast.h new file mode 100644 index 000000000000..a1cedf1ad925 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/AABoxCast.h @@ -0,0 +1,20 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Structure that holds AABox moving linearly through 3d space +struct AABoxCast +{ + JPH_OVERRIDE_NEW_DELETE + + AABox mBox; ///< Axis aligned box at starting location + Vec3 mDirection; ///< Direction and length of the cast (anything beyond this length will not be reported as a hit) +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ActiveEdgeMode.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ActiveEdgeMode.h new file mode 100644 index 000000000000..30f96aeb123e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ActiveEdgeMode.h @@ -0,0 +1,17 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// How to treat active/inactive edges. +/// An active edge is an edge that either has no neighbouring edge or if the angle between the two connecting faces is too large, see: ActiveEdges +enum class EActiveEdgeMode : uint8 +{ + CollideOnlyWithActive, ///< Do not collide with inactive edges. For physics simulation, this gives less ghost collisions. + CollideWithAll, ///< Collide with all edges. Use this when you're interested in all collisions. +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ActiveEdges.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ActiveEdges.h new file mode 100644 index 000000000000..7e51d2a63b72 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ActiveEdges.h @@ -0,0 +1,114 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// An active edge is an edge that either has no neighbouring edge or if the angle between the two connecting faces is too large. +namespace ActiveEdges +{ + /// Helper function to check if an edge is active or not + /// @param inNormal1 Triangle normal of triangle on the left side of the edge (when looking along the edge from the top) + /// @param inNormal2 Triangle normal of triangle on the right side of the edge + /// @param inEdgeDirection Vector that points along the edge + /// @param inCosThresholdAngle Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive) + inline static bool IsEdgeActive(Vec3Arg inNormal1, Vec3Arg inNormal2, Vec3Arg inEdgeDirection, float inCosThresholdAngle) + { + // If normals are opposite the edges are active (the triangles are back to back) + float cos_angle_normals = inNormal1.Dot(inNormal2); + if (cos_angle_normals < -0.999848f) // cos(179 degrees) + return true; + + // Check if concave edge, if so we are not active + if (inNormal1.Cross(inNormal2).Dot(inEdgeDirection) < 0.0f) + return false; + + // Convex edge, active when angle bigger than threshold + return cos_angle_normals < inCosThresholdAngle; + } + + /// Replace normal by triangle normal if a hit is hitting an inactive edge + /// @param inV0 , inV1 , inV2 form the triangle + /// @param inTriangleNormal is the normal of the provided triangle (does not need to be normalized) + /// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active + /// @param inPoint Collision point on the triangle + /// @param inNormal Collision normal on the triangle (does not need to be normalized) + /// @param inMovementDirection Can be zero. This gives an indication of in which direction the motion is to determine if when we hit an inactive edge/triangle we should return the triangle normal. + /// @return Returns inNormal if an active edge was hit, otherwise returns inTriangleNormal + inline static Vec3 FixNormal(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inTriangleNormal, uint8 inActiveEdges, Vec3Arg inPoint, Vec3Arg inNormal, Vec3Arg inMovementDirection) + { + // Check: All of the edges are active, we have the correct normal already. No need to call this function! + JPH_ASSERT(inActiveEdges != 0b111); + + // If inNormal would affect movement less than inTriangleNormal use inNormal + // This is done since it is really hard to make a distinction between sliding over a horizontal triangulated grid and hitting an edge (in this case you want to use the triangle normal) + // and sliding over a triangulated grid and grazing a vertical triangle with an inactive edge (in this case using the triangle normal will cause the object to bounce back so we want to use the calculated normal). + // To solve this we take a movement hint to give an indication of what direction our object is moving. If the edge normal results in less motion difference than the triangle normal we use the edge normal. + float normal_length = inNormal.Length(); + float triangle_normal_length = inTriangleNormal.Length(); + if (inMovementDirection.Dot(inNormal) * triangle_normal_length < inMovementDirection.Dot(inTriangleNormal) * normal_length) + return inNormal; + + // Check: None of the edges are active, we need to use the triangle normal + if (inActiveEdges == 0) + return inTriangleNormal; + + // Some edges are active. + // If normal is parallel to the triangle normal we don't need to check the active edges. + if (inTriangleNormal.Dot(inNormal) > 0.999848f * normal_length * triangle_normal_length) // cos(1 degree) + return inNormal; + + const float cEpsilon = 1.0e-4f; + const float cOneMinusEpsilon = 1.0f - cEpsilon; + + uint colliding_edge; + + // Test where the contact point is in the triangle + float u, v, w; + ClosestPoint::GetBaryCentricCoordinates(inV0 - inPoint, inV1 - inPoint, inV2 - inPoint, u, v, w); + if (u > cOneMinusEpsilon) + { + // Colliding with v0, edge 0 or 2 needs to be active + colliding_edge = 0b101; + } + else if (v > cOneMinusEpsilon) + { + // Colliding with v1, edge 0 or 1 needs to be active + colliding_edge = 0b011; + } + else if (w > cOneMinusEpsilon) + { + // Colliding with v2, edge 1 or 2 needs to be active + colliding_edge = 0b110; + } + else if (u < cEpsilon) + { + // Colliding with edge v1, v2, edge 1 needs to be active + colliding_edge = 0b010; + } + else if (v < cEpsilon) + { + // Colliding with edge v0, v2, edge 2 needs to be active + colliding_edge = 0b100; + } + else if (w < cEpsilon) + { + // Colliding with edge v0, v1, edge 0 needs to be active + colliding_edge = 0b001; + } + else + { + // Interior hit + return inTriangleNormal; + } + + // If this edge is active, use the provided normal instead of the triangle normal + return (inActiveEdges & colliding_edge) != 0? inNormal : inTriangleNormal; + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BackFaceMode.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BackFaceMode.h new file mode 100644 index 000000000000..441dcd89a5fd --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BackFaceMode.h @@ -0,0 +1,16 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// How collision detection functions will treat back facing triangles +enum class EBackFaceMode : uint8 +{ + IgnoreBackFaces, ///< Ignore collision with back facing surfaces/triangles + CollideWithBackFaces, ///< Collide with back facing surfaces/triangles +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.cpp new file mode 100644 index 000000000000..1317d13393c7 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.cpp @@ -0,0 +1,16 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +void BroadPhase::Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface) +{ + mBodyManager = inBodyManager; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.h new file mode 100644 index 000000000000..8b6506e901d7 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.h @@ -0,0 +1,112 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +// Shorthand function to ifdef out code if broadphase stats tracking is off +#ifdef JPH_TRACK_BROADPHASE_STATS + #define JPH_IF_TRACK_BROADPHASE_STATS(...) __VA_ARGS__ +#else + #define JPH_IF_TRACK_BROADPHASE_STATS(...) +#endif // JPH_TRACK_BROADPHASE_STATS + +class BodyManager; +struct BodyPair; + +using BodyPairCollector = CollisionCollector; + +/// Used to do coarse collision detection operations to quickly prune out bodies that will not collide. +class JPH_EXPORT BroadPhase : public BroadPhaseQuery +{ +public: + /// Initialize the broadphase. + /// @param inBodyManager The body manager singleton + /// @param inLayerInterface Interface that maps object layers to broadphase layers. + /// Note that the broadphase takes a pointer to the data inside inObjectToBroadPhaseLayer so this object should remain static. + virtual void Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface); + + /// Should be called after many objects have been inserted to make the broadphase more efficient, usually done on startup only + virtual void Optimize() { /* Optionally overridden by implementation */ } + + /// Must be called just before updating the broadphase when none of the body mutexes are locked + virtual void FrameSync() { /* Optionally overridden by implementation */ } + + /// Must be called before UpdatePrepare to prevent modifications from being made to the tree + virtual void LockModifications() { /* Optionally overridden by implementation */ } + + /// Context used during broadphase update + struct UpdateState { void *mData[4]; }; + + /// Update the broadphase, needs to be called frequently to update the internal state when bodies have been modified. + /// The UpdatePrepare() function can run in a background thread without influencing the broadphase + virtual UpdateState UpdatePrepare() { return UpdateState(); } + + /// Finalizing the update will quickly apply the changes + virtual void UpdateFinalize([[maybe_unused]] const UpdateState &inUpdateState) { /* Optionally overridden by implementation */ } + + /// Must be called after UpdateFinalize to allow modifications to the broadphase + virtual void UnlockModifications() { /* Optionally overridden by implementation */ } + + /// Handle used during adding bodies to the broadphase + using AddState = void *; + + /// Prepare adding inNumber bodies at ioBodies to the broadphase, returns a handle that should be used in AddBodiesFinalize/Abort. + /// This can be done on a background thread without influencing the broadphase. + /// ioBodies may be shuffled around by this function and should be kept that way until AddBodiesFinalize/Abort is called. + virtual AddState AddBodiesPrepare([[maybe_unused]] BodyID *ioBodies, [[maybe_unused]] int inNumber) { return nullptr; } // By default the broadphase doesn't support this + + /// Finalize adding bodies to the broadphase, supply the return value of AddBodiesPrepare in inAddState. + /// Please ensure that the ioBodies array passed to AddBodiesPrepare is unmodified and passed again to this function. + virtual void AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) = 0; + + /// Abort adding bodies to the broadphase, supply the return value of AddBodiesPrepare in inAddState. + /// This can be done on a background thread without influencing the broadphase. + /// Please ensure that the ioBodies array passed to AddBodiesPrepare is unmodified and passed again to this function. + virtual void AddBodiesAbort([[maybe_unused]] BodyID *ioBodies, [[maybe_unused]] int inNumber, [[maybe_unused]] AddState inAddState) { /* By default nothing needs to be done */ } + + /// Remove inNumber bodies in ioBodies from the broadphase. + /// ioBodies may be shuffled around by this function. + virtual void RemoveBodies(BodyID *ioBodies, int inNumber) = 0; + + /// Call whenever the aabb of a body changes (can change order of ioBodies array) + /// inTakeLock should be false if we're between LockModifications/UnlockModificiations in which case care needs to be taken to not call this between UpdatePrepare/UpdateFinalize + virtual void NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock = true) = 0; + + /// Call whenever the layer (and optionally the aabb as well) of a body changes (can change order of ioBodies array) + virtual void NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber) = 0; + + /// Find all colliding pairs between dynamic bodies + /// Note that this function is very specifically tailored for the PhysicsSystem::Update function, hence it is not part of the BroadPhaseQuery interface. + /// One of the assumptions it can make is that no locking is needed during the query as it will only be called during a very particular part of the update. + /// @param ioActiveBodies is a list of bodies for which we need to find colliding pairs (this function can change the order of the ioActiveBodies array). This can be a subset of the set of active bodies in the system. + /// @param inNumActiveBodies is the size of the ioActiveBodies array. + /// @param inSpeculativeContactDistance Distance at which speculative contact points will be created. + /// @param inObjectVsBroadPhaseLayerFilter is the filter that determines if an object can collide with a broadphase layer. + /// @param inObjectLayerPairFilter is the filter that determines if two objects can collide. + /// @param ioPairCollector receives callbacks for every body pair found. + virtual void FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const = 0; + + /// Same as BroadPhaseQuery::CastAABox but can be implemented in a way to take no broad phase locks. + virtual void CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const = 0; + + /// Get the bounding box of all objects in the broadphase + virtual AABox GetBounds() const = 0; + +#ifdef JPH_TRACK_BROADPHASE_STATS + /// Trace the collected broadphase stats in CSV form. + /// This report can be used to judge and tweak the efficiency of the broadphase. + virtual void ReportStats() { /* Can be implemented by derived classes */ } +#endif // JPH_TRACK_BROADPHASE_STATS + +protected: + /// Link to the body manager that manages the bodies in this broadphase + BodyManager * mBodyManager = nullptr; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.cpp new file mode 100644 index 000000000000..fc2332182d15 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.cpp @@ -0,0 +1,313 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +void BroadPhaseBruteForce::AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) +{ + lock_guard lock(mMutex); + + BodyVector &bodies = mBodyManager->GetBodies(); + + // Allocate space + uint32 idx = (uint32)mBodyIDs.size(); + mBodyIDs.resize(idx + inNumber); + + // Add bodies + for (const BodyID *b = ioBodies, *b_end = ioBodies + inNumber; b < b_end; ++b) + { + Body &body = *bodies[b->GetIndex()]; + + // Validate that body ID is consistent with array index + JPH_ASSERT(body.GetID() == *b); + JPH_ASSERT(!body.IsInBroadPhase()); + + // Add it to the list + mBodyIDs[idx] = body.GetID(); + ++idx; + + // Indicate body is in the broadphase + body.SetInBroadPhaseInternal(true); + } + + // Resort + QuickSort(mBodyIDs.begin(), mBodyIDs.end()); +} + +void BroadPhaseBruteForce::RemoveBodies(BodyID *ioBodies, int inNumber) +{ + lock_guard lock(mMutex); + + BodyVector &bodies = mBodyManager->GetBodies(); + + JPH_ASSERT((int)mBodyIDs.size() >= inNumber); + + // Remove bodies + for (const BodyID *b = ioBodies, *b_end = ioBodies + inNumber; b < b_end; ++b) + { + Body &body = *bodies[b->GetIndex()]; + + // Validate that body ID is consistent with array index + JPH_ASSERT(body.GetID() == *b); + JPH_ASSERT(body.IsInBroadPhase()); + + // Find body id + Array::const_iterator it = std::lower_bound(mBodyIDs.begin(), mBodyIDs.end(), body.GetID()); + JPH_ASSERT(it != mBodyIDs.end()); + + // Remove element + mBodyIDs.erase(it); + + // Indicate body is no longer in the broadphase + body.SetInBroadPhaseInternal(false); + } +} + +void BroadPhaseBruteForce::NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock) +{ + // Do nothing, we directly reference the body +} + +void BroadPhaseBruteForce::NotifyBodiesLayerChanged(BodyID * ioBodies, int inNumber) +{ + // Do nothing, we directly reference the body +} + +void BroadPhaseBruteForce::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + shared_lock lock(mMutex); + + // Load ray + Vec3 origin(inRay.mOrigin); + RayInvDirection inv_direction(inRay.mDirection); + + // For all bodies + float early_out_fraction = ioCollector.GetEarlyOutFraction(); + for (BodyID b : mBodyIDs) + { + const Body &body = mBodyManager->GetBody(b); + + // Test layer + if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer())) + { + // Test intersection with ray + const AABox &bounds = body.GetWorldSpaceBounds(); + float fraction = RayAABox(origin, inv_direction, bounds.mMin, bounds.mMax); + if (fraction < early_out_fraction) + { + // Store hit + BroadPhaseCastResult result { b, fraction }; + ioCollector.AddHit(result); + if (ioCollector.ShouldEarlyOut()) + break; + early_out_fraction = ioCollector.GetEarlyOutFraction(); + } + } + } +} + +void BroadPhaseBruteForce::CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + shared_lock lock(mMutex); + + // For all bodies + for (BodyID b : mBodyIDs) + { + const Body &body = mBodyManager->GetBody(b); + + // Test layer + if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer())) + { + // Test intersection with box + const AABox &bounds = body.GetWorldSpaceBounds(); + if (bounds.Overlaps(inBox)) + { + // Store hit + ioCollector.AddHit(b); + if (ioCollector.ShouldEarlyOut()) + break; + } + } + } +} + +void BroadPhaseBruteForce::CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + shared_lock lock(mMutex); + + float radius_sq = Square(inRadius); + + // For all bodies + for (BodyID b : mBodyIDs) + { + const Body &body = mBodyManager->GetBody(b); + + // Test layer + if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer())) + { + // Test intersection with box + const AABox &bounds = body.GetWorldSpaceBounds(); + if (bounds.GetSqDistanceTo(inCenter) <= radius_sq) + { + // Store hit + ioCollector.AddHit(b); + if (ioCollector.ShouldEarlyOut()) + break; + } + } + } +} + +void BroadPhaseBruteForce::CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + shared_lock lock(mMutex); + + // For all bodies + for (BodyID b : mBodyIDs) + { + const Body &body = mBodyManager->GetBody(b); + + // Test layer + if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer())) + { + // Test intersection with box + const AABox &bounds = body.GetWorldSpaceBounds(); + if (bounds.Contains(inPoint)) + { + // Store hit + ioCollector.AddHit(b); + if (ioCollector.ShouldEarlyOut()) + break; + } + } + } +} + +void BroadPhaseBruteForce::CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + shared_lock lock(mMutex); + + // For all bodies + for (BodyID b : mBodyIDs) + { + const Body &body = mBodyManager->GetBody(b); + + // Test layer + if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer())) + { + // Test intersection with box + const AABox &bounds = body.GetWorldSpaceBounds(); + if (inBox.Overlaps(bounds)) + { + // Store hit + ioCollector.AddHit(b); + if (ioCollector.ShouldEarlyOut()) + break; + } + } + } +} + +void BroadPhaseBruteForce::CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + shared_lock lock(mMutex); + + // Load box + Vec3 origin(inBox.mBox.GetCenter()); + Vec3 extent(inBox.mBox.GetExtent()); + RayInvDirection inv_direction(inBox.mDirection); + + // For all bodies + float early_out_fraction = ioCollector.GetPositiveEarlyOutFraction(); + for (BodyID b : mBodyIDs) + { + const Body &body = mBodyManager->GetBody(b); + + // Test layer + if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer())) + { + // Test intersection with ray + const AABox &bounds = body.GetWorldSpaceBounds(); + float fraction = RayAABox(origin, inv_direction, bounds.mMin - extent, bounds.mMax + extent); + if (fraction < early_out_fraction) + { + // Store hit + BroadPhaseCastResult result { b, fraction }; + ioCollector.AddHit(result); + if (ioCollector.ShouldEarlyOut()) + break; + early_out_fraction = ioCollector.GetPositiveEarlyOutFraction(); + } + } + } +} + +void BroadPhaseBruteForce::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + CastAABoxNoLock(inBox, ioCollector, inBroadPhaseLayerFilter, inObjectLayerFilter); +} + +void BroadPhaseBruteForce::FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const +{ + shared_lock lock(mMutex); + + // Loop through all active bodies + size_t num_bodies = mBodyIDs.size(); + for (int b1 = 0; b1 < inNumActiveBodies; ++b1) + { + BodyID b1_id = ioActiveBodies[b1]; + const Body &body1 = mBodyManager->GetBody(b1_id); + const ObjectLayer layer1 = body1.GetObjectLayer(); + + // Expand the bounding box by the speculative contact distance + AABox bounds1 = body1.GetWorldSpaceBounds(); + bounds1.ExpandBy(Vec3::sReplicate(inSpeculativeContactDistance)); + + // For all other bodies + for (size_t b2 = 0; b2 < num_bodies; ++b2) + { + // Check if bodies can collide + BodyID b2_id = mBodyIDs[b2]; + const Body &body2 = mBodyManager->GetBody(b2_id); + if (!Body::sFindCollidingPairsCanCollide(body1, body2)) + continue; + + // Check if layers can collide + const ObjectLayer layer2 = body2.GetObjectLayer(); + if (!inObjectLayerPairFilter.ShouldCollide(layer1, layer2)) + continue; + + // Check if bounds overlap + const AABox &bounds2 = body2.GetWorldSpaceBounds(); + if (!bounds1.Overlaps(bounds2)) + continue; + + // Store overlapping pair + ioPairCollector.AddHit({ b1_id, b2_id }); + } + } +} + +AABox BroadPhaseBruteForce::GetBounds() const +{ + shared_lock lock(mMutex); + + AABox bounds; + for (BodyID b : mBodyIDs) + bounds.Encapsulate(mBodyManager->GetBody(b).GetWorldSpaceBounds()); + return bounds; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h new file mode 100644 index 000000000000..c3e20f5c8b84 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h @@ -0,0 +1,38 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Test BroadPhase implementation that does not do anything to speed up the operations. Can be used as a reference implementation. +class JPH_EXPORT BroadPhaseBruteForce final : public BroadPhase +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // Implementing interface of BroadPhase (see BroadPhase for documentation) + virtual void AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) override; + virtual void RemoveBodies(BodyID *ioBodies, int inNumber) override; + virtual void NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock) override; + virtual void NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber) override; + virtual void CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const override; + virtual AABox GetBounds() const override; + +private: + Array mBodyIDs; + mutable SharedMutex mMutex; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h new file mode 100644 index 000000000000..bc591e733b2f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h @@ -0,0 +1,148 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// An object layer can be mapped to a broadphase layer. Objects with the same broadphase layer will end up in the same sub structure (usually a tree) of the broadphase. +/// When there are many layers, this reduces the total amount of sub structures the broad phase needs to manage. Usually you want objects that don't collide with each other +/// in different broad phase layers, but there could be exceptions if objects layers only contain a minor amount of objects so it is not beneficial to give each layer its +/// own sub structure in the broadphase. +/// Note: This class requires explicit casting from and to Type to avoid confusion with ObjectLayer +class BroadPhaseLayer +{ +public: + using Type = uint8; + + JPH_INLINE BroadPhaseLayer() = default; + JPH_INLINE explicit constexpr BroadPhaseLayer(Type inValue) : mValue(inValue) { } + JPH_INLINE constexpr BroadPhaseLayer(const BroadPhaseLayer &) = default; + JPH_INLINE BroadPhaseLayer & operator = (const BroadPhaseLayer &) = default; + + JPH_INLINE constexpr bool operator == (const BroadPhaseLayer &inRHS) const + { + return mValue == inRHS.mValue; + } + + JPH_INLINE constexpr bool operator != (const BroadPhaseLayer &inRHS) const + { + return mValue != inRHS.mValue; + } + + JPH_INLINE constexpr bool operator < (const BroadPhaseLayer &inRHS) const + { + return mValue < inRHS.mValue; + } + + JPH_INLINE explicit constexpr operator Type() const + { + return mValue; + } + + JPH_INLINE Type GetValue() const + { + return mValue; + } + +private: + Type mValue; +}; + +/// Constant value used to indicate an invalid broad phase layer +static constexpr BroadPhaseLayer cBroadPhaseLayerInvalid(0xff); + +/// Interface that the application should implement to allow mapping object layers to broadphase layers +class JPH_EXPORT BroadPhaseLayerInterface : public NonCopyable +{ +public: + /// Destructor + virtual ~BroadPhaseLayerInterface() = default; + + /// Return the number of broadphase layers there are + virtual uint GetNumBroadPhaseLayers() const = 0; + + /// Convert an object layer to the corresponding broadphase layer + virtual BroadPhaseLayer GetBroadPhaseLayer(ObjectLayer inLayer) const = 0; + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + /// Get the user readable name of a broadphase layer (debugging purposes) + virtual const char * GetBroadPhaseLayerName(BroadPhaseLayer inLayer) const = 0; +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED +}; + +/// Class to test if an object can collide with a broadphase layer. Used while finding collision pairs. +class JPH_EXPORT ObjectVsBroadPhaseLayerFilter : public NonCopyable +{ +public: + /// Destructor + virtual ~ObjectVsBroadPhaseLayerFilter() = default; + + /// Returns true if an object layer should collide with a broadphase layer + virtual bool ShouldCollide([[maybe_unused]] ObjectLayer inLayer1, [[maybe_unused]] BroadPhaseLayer inLayer2) const + { + return true; + } +}; + +/// Filter class for broadphase layers +class JPH_EXPORT BroadPhaseLayerFilter : public NonCopyable +{ +public: + /// Destructor + virtual ~BroadPhaseLayerFilter() = default; + + /// Function to filter out broadphase layers when doing collision query test (return true to allow testing against objects with this layer) + virtual bool ShouldCollide([[maybe_unused]] BroadPhaseLayer inLayer) const + { + return true; + } +}; + +/// Default filter class that uses the pair filter in combination with a specified layer to filter layers +class JPH_EXPORT DefaultBroadPhaseLayerFilter : public BroadPhaseLayerFilter +{ +public: + /// Constructor + DefaultBroadPhaseLayerFilter(const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, ObjectLayer inLayer) : + mObjectVsBroadPhaseLayerFilter(inObjectVsBroadPhaseLayerFilter), + mLayer(inLayer) + { + } + + // See BroadPhaseLayerFilter::ShouldCollide + virtual bool ShouldCollide(BroadPhaseLayer inLayer) const override + { + return mObjectVsBroadPhaseLayerFilter.ShouldCollide(mLayer, inLayer); + } + +private: + const ObjectVsBroadPhaseLayerFilter &mObjectVsBroadPhaseLayerFilter; + ObjectLayer mLayer; +}; + +/// Allows objects from a specific broad phase layer only +class JPH_EXPORT SpecifiedBroadPhaseLayerFilter : public BroadPhaseLayerFilter +{ +public: + /// Constructor + explicit SpecifiedBroadPhaseLayerFilter(BroadPhaseLayer inLayer) : + mLayer(inLayer) + { + } + + // See BroadPhaseLayerFilter::ShouldCollide + virtual bool ShouldCollide(BroadPhaseLayer inLayer) const override + { + return mLayer == inLayer; + } + +private: + BroadPhaseLayer mLayer; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceMask.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceMask.h new file mode 100644 index 000000000000..15a894bb12c6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceMask.h @@ -0,0 +1,92 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// BroadPhaseLayerInterface implementation. +/// This defines a mapping between object and broadphase layers. +/// This implementation works together with ObjectLayerPairFilterMask and ObjectVsBroadPhaseLayerFilterMask. +/// A broadphase layer is suitable for an object if its group & inGroupsToInclude is not zero and its group & inGroupsToExclude is zero. +/// The broadphase layers are iterated from lowest to highest value and the first one that matches is taken. If none match then it takes the last layer. +class BroadPhaseLayerInterfaceMask : public BroadPhaseLayerInterface +{ +public: + JPH_OVERRIDE_NEW_DELETE + + explicit BroadPhaseLayerInterfaceMask(uint inNumBroadPhaseLayers) + { + JPH_ASSERT(inNumBroadPhaseLayers > 0); + mMapping.resize(inNumBroadPhaseLayers); + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + mBroadPhaseLayerNames.resize(inNumBroadPhaseLayers, "Undefined"); +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED + } + + // Configures a broadphase layer. + void ConfigureLayer(BroadPhaseLayer inBroadPhaseLayer, uint32 inGroupsToInclude, uint32 inGroupsToExclude) + { + JPH_ASSERT((BroadPhaseLayer::Type)inBroadPhaseLayer < (uint)mMapping.size()); + Mapping &m = mMapping[(BroadPhaseLayer::Type)inBroadPhaseLayer]; + m.mGroupsToInclude = inGroupsToInclude; + m.mGroupsToExclude = inGroupsToExclude; + } + + virtual uint GetNumBroadPhaseLayers() const override + { + return (uint)mMapping.size(); + } + + virtual BroadPhaseLayer GetBroadPhaseLayer(ObjectLayer inLayer) const override + { + // Try to find the first broadphase layer that matches + uint32 group = ObjectLayerPairFilterMask::sGetGroup(inLayer); + for (const Mapping &m : mMapping) + if ((group & m.mGroupsToInclude) != 0 && (group & m.mGroupsToExclude) == 0) + return BroadPhaseLayer(BroadPhaseLayer::Type(&m - mMapping.data())); + + // Fall back to the last broadphase layer + return BroadPhaseLayer(BroadPhaseLayer::Type(mMapping.size() - 1)); + } + + /// Returns true if an object layer should collide with a broadphase layer, this function is being called from ObjectVsBroadPhaseLayerFilterMask + inline bool ShouldCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const + { + uint32 mask = ObjectLayerPairFilterMask::sGetMask(inLayer1); + const Mapping &m = mMapping[(BroadPhaseLayer::Type)inLayer2]; + return &m == &mMapping.back() // Last layer may collide with anything + || (m.mGroupsToInclude & mask) != 0; // Mask allows it to collide with objects that could reside in this layer + } + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + void SetBroadPhaseLayerName(BroadPhaseLayer inLayer, const char *inName) + { + mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer] = inName; + } + + virtual const char * GetBroadPhaseLayerName(BroadPhaseLayer inLayer) const override + { + return mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer]; + } +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED + +private: + struct Mapping + { + uint32 mGroupsToInclude = 0; + uint32 mGroupsToExclude = ~uint32(0); + }; + Array mMapping; + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + Array mBroadPhaseLayerNames; +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceTable.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceTable.h new file mode 100644 index 000000000000..e777a08589bb --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceTable.h @@ -0,0 +1,64 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// BroadPhaseLayerInterface implementation. +/// This defines a mapping between object and broadphase layers. +/// This implementation uses a simple table +class BroadPhaseLayerInterfaceTable : public BroadPhaseLayerInterface +{ +public: + JPH_OVERRIDE_NEW_DELETE + + BroadPhaseLayerInterfaceTable(uint inNumObjectLayers, uint inNumBroadPhaseLayers) : + mNumBroadPhaseLayers(inNumBroadPhaseLayers) + { + mObjectToBroadPhase.resize(inNumObjectLayers, BroadPhaseLayer(0)); +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + mBroadPhaseLayerNames.resize(inNumBroadPhaseLayers, "Undefined"); +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED + } + + void MapObjectToBroadPhaseLayer(ObjectLayer inObjectLayer, BroadPhaseLayer inBroadPhaseLayer) + { + JPH_ASSERT((BroadPhaseLayer::Type)inBroadPhaseLayer < mNumBroadPhaseLayers); + mObjectToBroadPhase[inObjectLayer] = inBroadPhaseLayer; + } + + virtual uint GetNumBroadPhaseLayers() const override + { + return mNumBroadPhaseLayers; + } + + virtual BroadPhaseLayer GetBroadPhaseLayer(ObjectLayer inLayer) const override + { + return mObjectToBroadPhase[inLayer]; + } + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + void SetBroadPhaseLayerName(BroadPhaseLayer inLayer, const char *inName) + { + mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer] = inName; + } + + virtual const char * GetBroadPhaseLayerName(BroadPhaseLayer inLayer) const override + { + return mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer]; + } +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED + +private: + uint mNumBroadPhaseLayers; + Array mObjectToBroadPhase; +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + Array mBroadPhaseLayerNames; +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp new file mode 100644 index 000000000000..717244c02381 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp @@ -0,0 +1,609 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +BroadPhaseQuadTree::~BroadPhaseQuadTree() +{ + delete [] mLayers; +} + +void BroadPhaseQuadTree::Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface) +{ + BroadPhase::Init(inBodyManager, inLayerInterface); + + // Store input parameters + mBroadPhaseLayerInterface = &inLayerInterface; + mNumLayers = inLayerInterface.GetNumBroadPhaseLayers(); + JPH_ASSERT(mNumLayers < (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid); + +#ifdef JPH_ENABLE_ASSERTS + // Store lock context + mLockContext = inBodyManager; +#endif // JPH_ENABLE_ASSERTS + + // Store max bodies + mMaxBodies = inBodyManager->GetMaxBodies(); + + // Initialize tracking data + mTracking.resize(mMaxBodies); + + // Init allocator + // Estimate the amount of nodes we're going to need + uint32 num_leaves = (uint32)(mMaxBodies + 1) / 2; // Assume 50% fill + uint32 num_leaves_plus_internal_nodes = num_leaves + (num_leaves + 2) / 3; // = Sum(num_leaves * 4^-i) with i = [0, Inf]. + mAllocator.Init(2 * num_leaves_plus_internal_nodes, 256); // We use double the amount of nodes while rebuilding the tree during Update() + + // Init sub trees + mLayers = new QuadTree [mNumLayers]; + for (uint l = 0; l < mNumLayers; ++l) + { + mLayers[l].Init(mAllocator); + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + // Set the name of the layer + mLayers[l].SetName(inLayerInterface.GetBroadPhaseLayerName(BroadPhaseLayer(BroadPhaseLayer::Type(l)))); +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED + } +} + +void BroadPhaseQuadTree::FrameSync() +{ + JPH_PROFILE_FUNCTION(); + + // Take a unique lock on the old query lock so that we know no one is using the old nodes anymore. + // Note that nothing should be locked at this point to avoid risking a lock inversion deadlock. + // Note that in other places where we lock this mutex we don't use SharedLock to detect lock inversions. As long as + // nothing else is locked this is safe. This is why BroadPhaseQuery should be the highest priority lock. + UniqueLock root_lock(mQueryLocks[mQueryLockIdx ^ 1] JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseQuery)); + + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + mLayers[l].DiscardOldTree(); +} + +void BroadPhaseQuadTree::Optimize() +{ + JPH_PROFILE_FUNCTION(); + + FrameSync(); + + LockModifications(); + + for (uint l = 0; l < mNumLayers; ++l) + { + QuadTree &tree = mLayers[l]; + if (tree.HasBodies()) + { + QuadTree::UpdateState update_state; + tree.UpdatePrepare(mBodyManager->GetBodies(), mTracking, update_state, true); + tree.UpdateFinalize(mBodyManager->GetBodies(), mTracking, update_state); + } + } + + UnlockModifications(); + + mNextLayerToUpdate = 0; +} + +void BroadPhaseQuadTree::LockModifications() +{ + // From this point on we prevent modifications to the tree + PhysicsLock::sLock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate)); +} + +BroadPhase::UpdateState BroadPhaseQuadTree::UpdatePrepare() +{ + // LockModifications should have been called + JPH_ASSERT(mUpdateMutex.is_locked()); + + // Create update state + UpdateState update_state; + UpdateStateImpl *update_state_impl = reinterpret_cast(&update_state); + + // Loop until we've seen all layers + for (uint iteration = 0; iteration < mNumLayers; ++iteration) + { + // Get the layer + QuadTree &tree = mLayers[mNextLayerToUpdate]; + mNextLayerToUpdate = (mNextLayerToUpdate + 1) % mNumLayers; + + // If it is dirty we update this one + if (tree.HasBodies() && tree.IsDirty() && tree.CanBeUpdated()) + { + update_state_impl->mTree = &tree; + tree.UpdatePrepare(mBodyManager->GetBodies(), mTracking, update_state_impl->mUpdateState, false); + return update_state; + } + } + + // Nothing to update + update_state_impl->mTree = nullptr; + return update_state; +} + +void BroadPhaseQuadTree::UpdateFinalize(const UpdateState &inUpdateState) +{ + // LockModifications should have been called + JPH_ASSERT(mUpdateMutex.is_locked()); + + // Test if a tree was updated + const UpdateStateImpl *update_state_impl = reinterpret_cast(&inUpdateState); + if (update_state_impl->mTree == nullptr) + return; + + update_state_impl->mTree->UpdateFinalize(mBodyManager->GetBodies(), mTracking, update_state_impl->mUpdateState); + + // Make all queries from now on use the new lock + mQueryLockIdx = mQueryLockIdx ^ 1; +} + +void BroadPhaseQuadTree::UnlockModifications() +{ + // From this point on we allow modifications to the tree again + PhysicsLock::sUnlock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate)); +} + +BroadPhase::AddState BroadPhaseQuadTree::AddBodiesPrepare(BodyID *ioBodies, int inNumber) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inNumber > 0); + + const BodyVector &bodies = mBodyManager->GetBodies(); + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + LayerState *state = new LayerState [mNumLayers]; + + // Sort bodies on layer + Body * const * const bodies_ptr = bodies.data(); // C pointer or else sort is incredibly slow in debug mode + QuickSort(ioBodies, ioBodies + inNumber, [bodies_ptr](BodyID inLHS, BodyID inRHS) { return bodies_ptr[inLHS.GetIndex()]->GetBroadPhaseLayer() < bodies_ptr[inRHS.GetIndex()]->GetBroadPhaseLayer(); }); + + BodyID *b_start = ioBodies, *b_end = ioBodies + inNumber; + while (b_start < b_end) + { + // Get broadphase layer + BroadPhaseLayer::Type broadphase_layer = (BroadPhaseLayer::Type)bodies[b_start->GetIndex()]->GetBroadPhaseLayer(); + JPH_ASSERT(broadphase_layer < mNumLayers); + + // Find first body with different layer + BodyID *b_mid = std::upper_bound(b_start, b_end, broadphase_layer, [bodies_ptr](BroadPhaseLayer::Type inLayer, BodyID inBodyID) { return inLayer < (BroadPhaseLayer::Type)bodies_ptr[inBodyID.GetIndex()]->GetBroadPhaseLayer(); }); + + // Keep track of state for this layer + LayerState &layer_state = state[broadphase_layer]; + layer_state.mBodyStart = b_start; + layer_state.mBodyEnd = b_mid; + + // Insert all bodies of the same layer + mLayers[broadphase_layer].AddBodiesPrepare(bodies, mTracking, b_start, int(b_mid - b_start), layer_state.mAddState); + + // Keep track in which tree we placed the object + for (const BodyID *b = b_start; b < b_mid; ++b) + { + uint32 index = b->GetIndex(); + JPH_ASSERT(bodies[index]->GetID() == *b, "Provided BodyID doesn't match BodyID in body manager"); + JPH_ASSERT(!bodies[index]->IsInBroadPhase()); + Tracking &t = mTracking[index]; + JPH_ASSERT(t.mBroadPhaseLayer == (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid); + t.mBroadPhaseLayer = broadphase_layer; + JPH_ASSERT(t.mObjectLayer == cObjectLayerInvalid); + t.mObjectLayer = bodies[index]->GetObjectLayer(); + } + + // Repeat + b_start = b_mid; + } + + return state; +} + +void BroadPhaseQuadTree::AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) +{ + JPH_PROFILE_FUNCTION(); + + // This cannot run concurrently with UpdatePrepare()/UpdateFinalize() + SharedLock lock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate)); + + BodyVector &bodies = mBodyManager->GetBodies(); + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + LayerState *state = (LayerState *)inAddState; + + for (BroadPhaseLayer::Type broadphase_layer = 0; broadphase_layer < mNumLayers; broadphase_layer++) + { + const LayerState &l = state[broadphase_layer]; + if (l.mBodyStart != nullptr) + { + // Insert all bodies of the same layer + mLayers[broadphase_layer].AddBodiesFinalize(mTracking, int(l.mBodyEnd - l.mBodyStart), l.mAddState); + + // Mark added to broadphase + for (const BodyID *b = l.mBodyStart; b < l.mBodyEnd; ++b) + { + uint32 index = b->GetIndex(); + JPH_ASSERT(bodies[index]->GetID() == *b, "Provided BodyID doesn't match BodyID in body manager"); + JPH_ASSERT(mTracking[index].mBroadPhaseLayer == broadphase_layer); + JPH_ASSERT(mTracking[index].mObjectLayer == bodies[index]->GetObjectLayer()); + JPH_ASSERT(!bodies[index]->IsInBroadPhase()); + bodies[index]->SetInBroadPhaseInternal(true); + } + } + } + + delete [] state; +} + +void BroadPhaseQuadTree::AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState) +{ + JPH_PROFILE_FUNCTION(); + + JPH_IF_ENABLE_ASSERTS(const BodyVector &bodies = mBodyManager->GetBodies();) + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + LayerState *state = (LayerState *)inAddState; + + for (BroadPhaseLayer::Type broadphase_layer = 0; broadphase_layer < mNumLayers; broadphase_layer++) + { + const LayerState &l = state[broadphase_layer]; + if (l.mBodyStart != nullptr) + { + // Insert all bodies of the same layer + mLayers[broadphase_layer].AddBodiesAbort(mTracking, l.mAddState); + + // Reset bookkeeping + for (const BodyID *b = l.mBodyStart; b < l.mBodyEnd; ++b) + { + uint32 index = b->GetIndex(); + JPH_ASSERT(bodies[index]->GetID() == *b, "Provided BodyID doesn't match BodyID in body manager"); + JPH_ASSERT(!bodies[index]->IsInBroadPhase()); + Tracking &t = mTracking[index]; + JPH_ASSERT(t.mBroadPhaseLayer == broadphase_layer); + t.mBroadPhaseLayer = (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid; + t.mObjectLayer = cObjectLayerInvalid; + } + } + } + + delete [] state; +} + +void BroadPhaseQuadTree::RemoveBodies(BodyID *ioBodies, int inNumber) +{ + JPH_PROFILE_FUNCTION(); + + // This cannot run concurrently with UpdatePrepare()/UpdateFinalize() + SharedLock lock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate)); + + JPH_ASSERT(inNumber > 0); + + BodyVector &bodies = mBodyManager->GetBodies(); + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Sort bodies on layer + Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode + QuickSort(ioBodies, ioBodies + inNumber, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mBroadPhaseLayer < tracking[inRHS.GetIndex()].mBroadPhaseLayer; }); + + BodyID *b_start = ioBodies, *b_end = ioBodies + inNumber; + while (b_start < b_end) + { + // Get broad phase layer + BroadPhaseLayer::Type broadphase_layer = mTracking[b_start->GetIndex()].mBroadPhaseLayer; + JPH_ASSERT(broadphase_layer != (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid); + + // Find first body with different layer + BodyID *b_mid = std::upper_bound(b_start, b_end, broadphase_layer, [tracking](BroadPhaseLayer::Type inLayer, BodyID inBodyID) { return inLayer < tracking[inBodyID.GetIndex()].mBroadPhaseLayer; }); + + // Remove all bodies of the same layer + mLayers[broadphase_layer].RemoveBodies(bodies, mTracking, b_start, int(b_mid - b_start)); + + for (const BodyID *b = b_start; b < b_mid; ++b) + { + // Reset bookkeeping + uint32 index = b->GetIndex(); + Tracking &t = tracking[index]; + t.mBroadPhaseLayer = (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid; + t.mObjectLayer = cObjectLayerInvalid; + + // Mark removed from broadphase + JPH_ASSERT(bodies[index]->IsInBroadPhase()); + bodies[index]->SetInBroadPhaseInternal(false); + } + + // Repeat + b_start = b_mid; + } +} + +void BroadPhaseQuadTree::NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inNumber > 0); + + // This cannot run concurrently with UpdatePrepare()/UpdateFinalize() + if (inTakeLock) + PhysicsLock::sLockShared(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate)); + else + JPH_ASSERT(mUpdateMutex.is_locked()); + + const BodyVector &bodies = mBodyManager->GetBodies(); + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Sort bodies on layer + const Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode + QuickSort(ioBodies, ioBodies + inNumber, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mBroadPhaseLayer < tracking[inRHS.GetIndex()].mBroadPhaseLayer; }); + + BodyID *b_start = ioBodies, *b_end = ioBodies + inNumber; + while (b_start < b_end) + { + // Get broadphase layer + BroadPhaseLayer::Type broadphase_layer = tracking[b_start->GetIndex()].mBroadPhaseLayer; + JPH_ASSERT(broadphase_layer != (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid); + + // Find first body with different layer + BodyID *b_mid = std::upper_bound(b_start, b_end, broadphase_layer, [tracking](BroadPhaseLayer::Type inLayer, BodyID inBodyID) { return inLayer < tracking[inBodyID.GetIndex()].mBroadPhaseLayer; }); + + // Nodify all bodies of the same layer changed + mLayers[broadphase_layer].NotifyBodiesAABBChanged(bodies, mTracking, b_start, int(b_mid - b_start)); + + // Repeat + b_start = b_mid; + } + + if (inTakeLock) + PhysicsLock::sUnlockShared(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate)); +} + +void BroadPhaseQuadTree::NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inNumber > 0); + + // First sort the bodies that actually changed layer to beginning of the array + const BodyVector &bodies = mBodyManager->GetBodies(); + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + for (BodyID *body_id = ioBodies + inNumber - 1; body_id >= ioBodies; --body_id) + { + uint32 index = body_id->GetIndex(); + JPH_ASSERT(bodies[index]->GetID() == *body_id, "Provided BodyID doesn't match BodyID in body manager"); + const Body *body = bodies[index]; + BroadPhaseLayer::Type broadphase_layer = (BroadPhaseLayer::Type)body->GetBroadPhaseLayer(); + JPH_ASSERT(broadphase_layer < mNumLayers); + if (mTracking[index].mBroadPhaseLayer == broadphase_layer) + { + // Update tracking information + mTracking[index].mObjectLayer = body->GetObjectLayer(); + + // Move the body to the end, layer didn't change + std::swap(*body_id, ioBodies[inNumber - 1]); + --inNumber; + } + } + + if (inNumber > 0) + { + // Changing layer requires us to remove from one tree and add to another, so this is equivalent to removing all bodies first and then adding them again + RemoveBodies(ioBodies, inNumber); + AddState add_state = AddBodiesPrepare(ioBodies, inNumber); + AddBodiesFinalize(ioBodies, inNumber, add_state); + } +} + +void BroadPhaseQuadTree::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Prevent this from running in parallel with node deletion in FrameSync(), see notes there + shared_lock lock(mQueryLocks[mQueryLockIdx]); + + // Loop over all layers and test the ones that could hit + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + { + const QuadTree &tree = mLayers[l]; + if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l))) + { + JPH_PROFILE(tree.GetName()); + tree.CastRay(inRay, ioCollector, inObjectLayerFilter, mTracking); + if (ioCollector.ShouldEarlyOut()) + break; + } + } +} + +void BroadPhaseQuadTree::CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Prevent this from running in parallel with node deletion in FrameSync(), see notes there + shared_lock lock(mQueryLocks[mQueryLockIdx]); + + // Loop over all layers and test the ones that could hit + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + { + const QuadTree &tree = mLayers[l]; + if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l))) + { + JPH_PROFILE(tree.GetName()); + tree.CollideAABox(inBox, ioCollector, inObjectLayerFilter, mTracking); + if (ioCollector.ShouldEarlyOut()) + break; + } + } +} + +void BroadPhaseQuadTree::CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Prevent this from running in parallel with node deletion in FrameSync(), see notes there + shared_lock lock(mQueryLocks[mQueryLockIdx]); + + // Loop over all layers and test the ones that could hit + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + { + const QuadTree &tree = mLayers[l]; + if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l))) + { + JPH_PROFILE(tree.GetName()); + tree.CollideSphere(inCenter, inRadius, ioCollector, inObjectLayerFilter, mTracking); + if (ioCollector.ShouldEarlyOut()) + break; + } + } +} + +void BroadPhaseQuadTree::CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Prevent this from running in parallel with node deletion in FrameSync(), see notes there + shared_lock lock(mQueryLocks[mQueryLockIdx]); + + // Loop over all layers and test the ones that could hit + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + { + const QuadTree &tree = mLayers[l]; + if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l))) + { + JPH_PROFILE(tree.GetName()); + tree.CollidePoint(inPoint, ioCollector, inObjectLayerFilter, mTracking); + if (ioCollector.ShouldEarlyOut()) + break; + } + } +} + +void BroadPhaseQuadTree::CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Prevent this from running in parallel with node deletion in FrameSync(), see notes there + shared_lock lock(mQueryLocks[mQueryLockIdx]); + + // Loop over all layers and test the ones that could hit + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + { + const QuadTree &tree = mLayers[l]; + if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l))) + { + JPH_PROFILE(tree.GetName()); + tree.CollideOrientedBox(inBox, ioCollector, inObjectLayerFilter, mTracking); + if (ioCollector.ShouldEarlyOut()) + break; + } + } +} + +void BroadPhaseQuadTree::CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Loop over all layers and test the ones that could hit + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + { + const QuadTree &tree = mLayers[l]; + if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l))) + { + JPH_PROFILE(tree.GetName()); + tree.CastAABox(inBox, ioCollector, inObjectLayerFilter, mTracking); + if (ioCollector.ShouldEarlyOut()) + break; + } + } +} + +void BroadPhaseQuadTree::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + // Prevent this from running in parallel with node deletion in FrameSync(), see notes there + shared_lock lock(mQueryLocks[mQueryLockIdx]); + + CastAABoxNoLock(inBox, ioCollector, inBroadPhaseLayerFilter, inObjectLayerFilter); +} + +void BroadPhaseQuadTree::FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const +{ + JPH_PROFILE_FUNCTION(); + + const BodyVector &bodies = mBodyManager->GetBodies(); + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Note that we don't take any locks at this point. We know that the tree is not going to be swapped or deleted while finding collision pairs due to the way the jobs are scheduled in the PhysicsSystem::Update. + + // Sort bodies on layer + const Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode + QuickSort(ioActiveBodies, ioActiveBodies + inNumActiveBodies, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mObjectLayer < tracking[inRHS.GetIndex()].mObjectLayer; }); + + BodyID *b_start = ioActiveBodies, *b_end = ioActiveBodies + inNumActiveBodies; + while (b_start < b_end) + { + // Get broadphase layer + ObjectLayer object_layer = tracking[b_start->GetIndex()].mObjectLayer; + JPH_ASSERT(object_layer != cObjectLayerInvalid); + + // Find first body with different layer + BodyID *b_mid = std::upper_bound(b_start, b_end, object_layer, [tracking](ObjectLayer inLayer, BodyID inBodyID) { return inLayer < tracking[inBodyID.GetIndex()].mObjectLayer; }); + + // Loop over all layers and test the ones that could hit + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + { + const QuadTree &tree = mLayers[l]; + if (tree.HasBodies() && inObjectVsBroadPhaseLayerFilter.ShouldCollide(object_layer, BroadPhaseLayer(l))) + { + JPH_PROFILE(tree.GetName()); + tree.FindCollidingPairs(bodies, b_start, int(b_mid - b_start), inSpeculativeContactDistance, ioPairCollector, inObjectLayerPairFilter); + } + } + + // Repeat + b_start = b_mid; + } +} + +AABox BroadPhaseQuadTree::GetBounds() const +{ + // Prevent this from running in parallel with node deletion in FrameSync(), see notes there + shared_lock lock(mQueryLocks[mQueryLockIdx]); + + AABox bounds; + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + bounds.Encapsulate(mLayers[l].GetBounds()); + return bounds; +} + +#ifdef JPH_TRACK_BROADPHASE_STATS + +void BroadPhaseQuadTree::ReportStats() +{ + Trace("Query Type, Filter Description, Tree Name, Num Queries, Total Time (%%), Total Time Excl. Collector (%%), Nodes Visited, Bodies Visited, Hits Reported, Hits Reported vs Bodies Visited (%%), Hits Reported vs Nodes Visited"); + + uint64 total_ticks = 0; + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + total_ticks += mLayers[l].GetTicks100Pct(); + + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + mLayers[l].ReportStats(total_ticks); +} + +#endif // JPH_TRACK_BROADPHASE_STATS + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h new file mode 100644 index 000000000000..ae97c10f0e33 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h @@ -0,0 +1,108 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Fast SIMD based quad tree BroadPhase that is multithreading aware and tries to do a minimal amount of locking. +class JPH_EXPORT BroadPhaseQuadTree final : public BroadPhase +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Destructor + virtual ~BroadPhaseQuadTree() override; + + // Implementing interface of BroadPhase (see BroadPhase for documentation) + virtual void Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface) override; + virtual void Optimize() override; + virtual void FrameSync() override; + virtual void LockModifications() override; + virtual UpdateState UpdatePrepare() override; + virtual void UpdateFinalize(const UpdateState &inUpdateState) override; + virtual void UnlockModifications() override; + virtual AddState AddBodiesPrepare(BodyID *ioBodies, int inNumber) override; + virtual void AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) override; + virtual void AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState) override; + virtual void RemoveBodies(BodyID *ioBodies, int inNumber) override; + virtual void NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock) override; + virtual void NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber) override; + virtual void CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const override; + virtual AABox GetBounds() const override; +#ifdef JPH_TRACK_BROADPHASE_STATS + virtual void ReportStats() override; +#endif // JPH_TRACK_BROADPHASE_STATS + +private: + /// Helper struct for AddBodies handle + struct LayerState + { + JPH_OVERRIDE_NEW_DELETE + + BodyID * mBodyStart = nullptr; + BodyID * mBodyEnd; + QuadTree::AddState mAddState; + }; + + using Tracking = QuadTree::Tracking; + using TrackingVector = QuadTree::TrackingVector; + +#ifdef JPH_ENABLE_ASSERTS + /// Context used to lock a physics lock + PhysicsLockContext mLockContext = nullptr; +#endif // JPH_ENABLE_ASSERTS + + /// Max amount of bodies we support + size_t mMaxBodies = 0; + + /// Array that for each BodyID keeps track of where it is located in which tree + TrackingVector mTracking; + + /// Node allocator for all trees + QuadTree::Allocator mAllocator; + + /// Information about broad phase layers + const BroadPhaseLayerInterface *mBroadPhaseLayerInterface = nullptr; + + /// One tree per object layer + QuadTree * mLayers; + uint mNumLayers; + + /// UpdateState implementation for this tree used during UpdatePrepare/Finalize() + struct UpdateStateImpl + { + QuadTree * mTree; + QuadTree::UpdateState mUpdateState; + }; + + static_assert(sizeof(UpdateStateImpl) <= sizeof(UpdateState)); + static_assert(alignof(UpdateStateImpl) <= alignof(UpdateState)); + + /// Mutex that prevents object modification during UpdatePrepare/Finalize() + SharedMutex mUpdateMutex; + + /// We double buffer all trees so that we can query while building the next one and we destroy the old tree the next physics update. + /// This structure ensures that we wait for queries that are still using the old tree. + mutable SharedMutex mQueryLocks[2]; + + /// This index indicates which lock is currently active, it alternates between 0 and 1 + atomic mQueryLockIdx { 0 }; + + /// This is the next tree to update in UpdatePrepare() + uint32 mNextLayerToUpdate = 0; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h new file mode 100644 index 000000000000..10085e66fe8d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h @@ -0,0 +1,53 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +struct RayCast; +class BroadPhaseCastResult; +class AABox; +class OrientedBox; +struct AABoxCast; + +// Various collector configurations +using RayCastBodyCollector = CollisionCollector; +using CastShapeBodyCollector = CollisionCollector; +using CollideShapeBodyCollector = CollisionCollector; + +/// Interface to the broadphase that can perform collision queries. These queries will only test the bounding box of the body to quickly determine a potential set of colliding bodies. +/// The shapes of the bodies are not tested, if you want this then you should use the NarrowPhaseQuery interface. +class JPH_EXPORT BroadPhaseQuery : public NonCopyable +{ +public: + /// Virtual destructor + virtual ~BroadPhaseQuery() = default; + + /// Cast a ray and add any hits to ioCollector + virtual void CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0; + + /// Get bodies intersecting with inBox and any hits to ioCollector + virtual void CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0; + + /// Get bodies intersecting with a sphere and any hits to ioCollector + virtual void CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0; + + /// Get bodies intersecting with a point and any hits to ioCollector + virtual void CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0; + + /// Get bodies intersecting with an oriented box and any hits to ioCollector + virtual void CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0; + + /// Cast a box and add any hits to ioCollector + virtual void CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterMask.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterMask.h new file mode 100644 index 000000000000..20305a5f0ca8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterMask.h @@ -0,0 +1,35 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that determines if an object layer can collide with a broadphase layer. +/// This implementation works together with BroadPhaseLayerInterfaceMask and ObjectLayerPairFilterMask +class ObjectVsBroadPhaseLayerFilterMask : public ObjectVsBroadPhaseLayerFilter +{ +public: + JPH_OVERRIDE_NEW_DELETE + +/// Constructor + ObjectVsBroadPhaseLayerFilterMask(const BroadPhaseLayerInterfaceMask &inBroadPhaseLayerInterface) : + mBroadPhaseLayerInterface(inBroadPhaseLayerInterface) + { + } + + /// Returns true if an object layer should collide with a broadphase layer + virtual bool ShouldCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const override + { + // Just defer to BroadPhaseLayerInterface + return mBroadPhaseLayerInterface.ShouldCollide(inLayer1, inLayer2); + } + +private: + const BroadPhaseLayerInterfaceMask &mBroadPhaseLayerInterface; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterTable.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterTable.h new file mode 100644 index 000000000000..532ce6da0edb --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterTable.h @@ -0,0 +1,66 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that determines if an object layer can collide with a broadphase layer. +/// This implementation uses a table and constructs itself from an ObjectLayerPairFilter and a BroadPhaseLayerInterface. +class ObjectVsBroadPhaseLayerFilterTable : public ObjectVsBroadPhaseLayerFilter +{ +private: + /// Get which bit corresponds to the pair (inLayer1, inLayer2) + uint GetBit(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const + { + // Calculate at which bit the entry for this pair resides + return inLayer1 * mNumBroadPhaseLayers + (BroadPhaseLayer::Type)inLayer2; + } + +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct the table + /// @param inBroadPhaseLayerInterface The broad phase layer interface that maps object layers to broad phase layers + /// @param inNumBroadPhaseLayers Number of broad phase layers + /// @param inObjectLayerPairFilter The object layer pair filter that determines which object layers can collide + /// @param inNumObjectLayers Number of object layers + ObjectVsBroadPhaseLayerFilterTable(const BroadPhaseLayerInterface &inBroadPhaseLayerInterface, uint inNumBroadPhaseLayers, const ObjectLayerPairFilter &inObjectLayerPairFilter, uint inNumObjectLayers) : + mNumBroadPhaseLayers(inNumBroadPhaseLayers) + { + // Resize table and set all entries to false + mTable.resize((inNumBroadPhaseLayers * inNumObjectLayers + 7) / 8, 0); + + // Loop over all object layer pairs + for (ObjectLayer o1 = 0; o1 < inNumObjectLayers; ++o1) + for (ObjectLayer o2 = 0; o2 < inNumObjectLayers; ++o2) + { + // Get the broad phase layer for the second object layer + BroadPhaseLayer b2 = inBroadPhaseLayerInterface.GetBroadPhaseLayer(o2); + JPH_ASSERT((BroadPhaseLayer::Type)b2 < inNumBroadPhaseLayers); + + // If the object layers collide then so should the object and broadphase layer + if (inObjectLayerPairFilter.ShouldCollide(o1, o2)) + { + uint bit = GetBit(o1, b2); + mTable[bit >> 3] |= 1 << (bit & 0b111); + } + } + } + + /// Returns true if an object layer should collide with a broadphase layer + virtual bool ShouldCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const override + { + uint bit = GetBit(inLayer1, inLayer2); + return (mTable[bit >> 3] & (1 << (bit & 0b111))) != 0; + } + +private: + uint mNumBroadPhaseLayers; ///< The total number of broadphase layers + Array mTable; ///< The table of bits that indicates which layers collide +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp new file mode 100644 index 000000000000..cfcf8ba30091 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp @@ -0,0 +1,1692 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef JPH_DUMP_BROADPHASE_TREE +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END +#endif // JPH_DUMP_BROADPHASE_TREE + +JPH_NAMESPACE_BEGIN + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// QuadTree::Node +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +QuadTree::Node::Node(bool inIsChanged) : + mIsChanged(inIsChanged) +{ + // First reset bounds + Vec4 val = Vec4::sReplicate(cLargeFloat); + val.StoreFloat4((Float4 *)&mBoundsMinX); + val.StoreFloat4((Float4 *)&mBoundsMinY); + val.StoreFloat4((Float4 *)&mBoundsMinZ); + val = Vec4::sReplicate(-cLargeFloat); + val.StoreFloat4((Float4 *)&mBoundsMaxX); + val.StoreFloat4((Float4 *)&mBoundsMaxY); + val.StoreFloat4((Float4 *)&mBoundsMaxZ); + + // Reset child node ids + mChildNodeID[0] = NodeID::sInvalid(); + mChildNodeID[1] = NodeID::sInvalid(); + mChildNodeID[2] = NodeID::sInvalid(); + mChildNodeID[3] = NodeID::sInvalid(); +} + +void QuadTree::Node::GetChildBounds(int inChildIndex, AABox &outBounds) const +{ + // Read bounding box in order min -> max + outBounds.mMin = Vec3(mBoundsMinX[inChildIndex], mBoundsMinY[inChildIndex], mBoundsMinZ[inChildIndex]); + outBounds.mMax = Vec3(mBoundsMaxX[inChildIndex], mBoundsMaxY[inChildIndex], mBoundsMaxZ[inChildIndex]); +} + +void QuadTree::Node::SetChildBounds(int inChildIndex, const AABox &inBounds) +{ + // Set max first (this keeps the bounding box invalid for reading threads) + mBoundsMaxZ[inChildIndex] = inBounds.mMax.GetZ(); + mBoundsMaxY[inChildIndex] = inBounds.mMax.GetY(); + mBoundsMaxX[inChildIndex] = inBounds.mMax.GetX(); + + // Then set min (and make box valid) + mBoundsMinZ[inChildIndex] = inBounds.mMin.GetZ(); + mBoundsMinY[inChildIndex] = inBounds.mMin.GetY(); + mBoundsMinX[inChildIndex] = inBounds.mMin.GetX(); // Min X becomes valid last +} + +void QuadTree::Node::InvalidateChildBounds(int inChildIndex) +{ + // First we make the box invalid by setting the min to cLargeFloat + mBoundsMinX[inChildIndex] = cLargeFloat; // Min X becomes invalid first + mBoundsMinY[inChildIndex] = cLargeFloat; + mBoundsMinZ[inChildIndex] = cLargeFloat; + + // Then we reset the max values too + mBoundsMaxX[inChildIndex] = -cLargeFloat; + mBoundsMaxY[inChildIndex] = -cLargeFloat; + mBoundsMaxZ[inChildIndex] = -cLargeFloat; +} + +void QuadTree::Node::GetNodeBounds(AABox &outBounds) const +{ + // Get first child bounds + GetChildBounds(0, outBounds); + + // Encapsulate other child bounds + for (int child_idx = 1; child_idx < 4; ++child_idx) + { + AABox tmp; + GetChildBounds(child_idx, tmp); + outBounds.Encapsulate(tmp); + } +} + +bool QuadTree::Node::EncapsulateChildBounds(int inChildIndex, const AABox &inBounds) +{ + bool changed = AtomicMin(mBoundsMinX[inChildIndex], inBounds.mMin.GetX()); + changed |= AtomicMin(mBoundsMinY[inChildIndex], inBounds.mMin.GetY()); + changed |= AtomicMin(mBoundsMinZ[inChildIndex], inBounds.mMin.GetZ()); + changed |= AtomicMax(mBoundsMaxX[inChildIndex], inBounds.mMax.GetX()); + changed |= AtomicMax(mBoundsMaxY[inChildIndex], inBounds.mMax.GetY()); + changed |= AtomicMax(mBoundsMaxZ[inChildIndex], inBounds.mMax.GetZ()); + return changed; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// QuadTree +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +const float QuadTree::cLargeFloat = 1.0e30f; +const AABox QuadTree::cInvalidBounds(Vec3::sReplicate(cLargeFloat), Vec3::sReplicate(-cLargeFloat)); + +void QuadTree::GetBodyLocation(const TrackingVector &inTracking, BodyID inBodyID, uint32 &outNodeIdx, uint32 &outChildIdx) const +{ + uint32 body_location = inTracking[inBodyID.GetIndex()].mBodyLocation; + JPH_ASSERT(body_location != Tracking::cInvalidBodyLocation); + outNodeIdx = body_location & 0x3fffffff; + outChildIdx = body_location >> 30; + JPH_ASSERT(mAllocator->Get(outNodeIdx).mChildNodeID[outChildIdx] == inBodyID, "Make sure that the body is in the node where it should be"); +} + +void QuadTree::SetBodyLocation(TrackingVector &ioTracking, BodyID inBodyID, uint32 inNodeIdx, uint32 inChildIdx) const +{ + JPH_ASSERT(inNodeIdx <= 0x3fffffff); + JPH_ASSERT(inChildIdx < 4); + JPH_ASSERT(mAllocator->Get(inNodeIdx).mChildNodeID[inChildIdx] == inBodyID, "Make sure that the body is in the node where it should be"); + ioTracking[inBodyID.GetIndex()].mBodyLocation = inNodeIdx + (inChildIdx << 30); + +#ifdef JPH_ENABLE_ASSERTS + uint32 v1, v2; + GetBodyLocation(ioTracking, inBodyID, v1, v2); + JPH_ASSERT(v1 == inNodeIdx); + JPH_ASSERT(v2 == inChildIdx); +#endif +} + +void QuadTree::sInvalidateBodyLocation(TrackingVector &ioTracking, BodyID inBodyID) +{ + ioTracking[inBodyID.GetIndex()].mBodyLocation = Tracking::cInvalidBodyLocation; +} + +QuadTree::~QuadTree() +{ + // Get rid of any nodes that are still to be freed + DiscardOldTree(); + + // Get the current root node + const RootNode &root_node = GetCurrentRoot(); + + // Collect all bodies + Allocator::Batch free_batch; + NodeID node_stack[cStackSize]; + node_stack[0] = root_node.GetNodeID(); + JPH_ASSERT(node_stack[0].IsValid()); + if (node_stack[0].IsNode()) + { + int top = 0; + do + { + // Process node + NodeID node_id = node_stack[top]; + JPH_ASSERT(!node_id.IsBody()); + uint32 node_idx = node_id.GetNodeIndex(); + const Node &node = mAllocator->Get(node_idx); + + // Recurse and get all child nodes + for (NodeID child_node_id : node.mChildNodeID) + if (child_node_id.IsValid() && child_node_id.IsNode()) + { + JPH_ASSERT(top < cStackSize); + node_stack[top] = child_node_id; + top++; + } + + // Mark node to be freed + mAllocator->AddObjectToBatch(free_batch, node_idx); + --top; + } + while (top >= 0); + } + + // Now free all nodes + mAllocator->DestructObjectBatch(free_batch); +} + +uint32 QuadTree::AllocateNode(bool inIsChanged) +{ + uint32 index = mAllocator->ConstructObject(inIsChanged); + if (index == Allocator::cInvalidObjectIndex) + { + Trace("QuadTree: Out of nodes!"); + std::abort(); + } + return index; +} + +void QuadTree::Init(Allocator &inAllocator) +{ + // Store allocator + mAllocator = &inAllocator; + + // Allocate root node + mRootNode[mRootNodeIndex].mIndex = AllocateNode(false); +} + +void QuadTree::DiscardOldTree() +{ + // Check if there is an old tree + RootNode &old_root_node = mRootNode[mRootNodeIndex ^ 1]; + if (old_root_node.mIndex != cInvalidNodeIndex) + { + // Clear the root + old_root_node.mIndex = cInvalidNodeIndex; + + // Now free all old nodes + mAllocator->DestructObjectBatch(mFreeNodeBatch); + + // Clear the batch + mFreeNodeBatch = Allocator::Batch(); + } +} + +AABox QuadTree::GetBounds() const +{ + uint32 node_idx = GetCurrentRoot().mIndex; + JPH_ASSERT(node_idx != cInvalidNodeIndex); + const Node &node = mAllocator->Get(node_idx); + + AABox bounds; + node.GetNodeBounds(bounds); + return bounds; +} + +void QuadTree::UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTracking, UpdateState &outUpdateState, bool inFullRebuild) +{ +#ifdef JPH_ENABLE_ASSERTS + // We only read positions + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::Read); +#endif + + // Assert we have no nodes pending deletion, this means DiscardOldTree wasn't called yet + JPH_ASSERT(mFreeNodeBatch.mNumObjects == 0); + + // Mark tree non-dirty + mIsDirty = false; + + // Get the current root node + const RootNode &root_node = GetCurrentRoot(); + +#ifdef JPH_DUMP_BROADPHASE_TREE + DumpTree(root_node.GetNodeID(), StringFormat("%s_PRE", mName).c_str()); +#endif + + // Assert sane data +#ifdef JPH_DEBUG + ValidateTree(inBodies, ioTracking, root_node.mIndex, mNumBodies); +#endif + + // Create space for all body ID's + NodeID *node_ids = new NodeID [mNumBodies]; + NodeID *cur_node_id = node_ids; + + // Collect all bodies + NodeID node_stack[cStackSize]; + node_stack[0] = root_node.GetNodeID(); + JPH_ASSERT(node_stack[0].IsValid()); + int top = 0; + do + { + // Check if node is a body + NodeID node_id = node_stack[top]; + if (node_id.IsBody()) + { + // Validate that we're still in the right layer + #ifdef JPH_ENABLE_ASSERTS + uint32 body_index = node_id.GetBodyID().GetIndex(); + JPH_ASSERT(ioTracking[body_index].mObjectLayer == inBodies[body_index]->GetObjectLayer()); + #endif + + // Store body + *cur_node_id = node_id; + ++cur_node_id; + } + else + { + // Process normal node + uint32 node_idx = node_id.GetNodeIndex(); + const Node &node = mAllocator->Get(node_idx); + + if (!node.mIsChanged && !inFullRebuild) + { + // Node is unchanged, treat it as a whole + *cur_node_id = node_id; + ++cur_node_id; + } + else + { + // Node is changed, recurse and get all children + for (NodeID child_node_id : node.mChildNodeID) + if (child_node_id.IsValid()) + { + if (top < cStackSize) + { + node_stack[top] = child_node_id; + top++; + } + else + { + JPH_ASSERT(false, "Stack full!\n" + "This must be a very deep tree. Are you batch adding bodies through BodyInterface::AddBodiesPrepare/AddBodiesFinalize?\n" + "If you add lots of bodies through BodyInterface::AddBody you may need to call PhysicsSystem::OptimizeBroadPhase to rebuild the tree."); + + // Falling back to adding the node as a whole + *cur_node_id = child_node_id; + ++cur_node_id; + } + } + + // Mark node to be freed + mAllocator->AddObjectToBatch(mFreeNodeBatch, node_idx); + } + } + --top; + } + while (top >= 0); + + // Check that our book keeping matches + uint32 num_node_ids = uint32(cur_node_id - node_ids); + JPH_ASSERT(inFullRebuild? num_node_ids == mNumBodies : num_node_ids <= mNumBodies); + + // This will be the new root node id + NodeID root_node_id; + + if (num_node_ids > 0) + { + // We mark the first 5 levels (max 1024 nodes) of the newly built tree as 'changed' so that + // those nodes get recreated every time when we rebuild the tree. This balances the amount of + // time we spend on rebuilding the tree ('unchanged' nodes will be put in the new tree as a whole) + // vs the quality of the built tree. + constexpr uint cMaxDepthMarkChanged = 5; + + // Build new tree + AABox root_bounds; + root_node_id = BuildTree(inBodies, ioTracking, node_ids, num_node_ids, cMaxDepthMarkChanged, root_bounds); + + if (root_node_id.IsBody()) + { + // For a single body we need to allocate a new root node + uint32 root_idx = AllocateNode(false); + Node &root = mAllocator->Get(root_idx); + root.SetChildBounds(0, root_bounds); + root.mChildNodeID[0] = root_node_id; + SetBodyLocation(ioTracking, root_node_id.GetBodyID(), root_idx, 0); + root_node_id = NodeID::sFromNodeIndex(root_idx); + } + } + else + { + // Empty tree, create root node + uint32 root_idx = AllocateNode(false); + root_node_id = NodeID::sFromNodeIndex(root_idx); + } + + // Delete temporary data + delete [] node_ids; + + outUpdateState.mRootNodeID = root_node_id; +} + +void QuadTree::UpdateFinalize([[maybe_unused]] const BodyVector &inBodies, [[maybe_unused]] const TrackingVector &inTracking, const UpdateState &inUpdateState) +{ + // Tree building is complete, now we switch the old with the new tree + uint32 new_root_idx = mRootNodeIndex ^ 1; + RootNode &new_root_node = mRootNode[new_root_idx]; + { + // Note: We don't need to lock here as the old tree stays available so any queries + // that use it can continue using it until DiscardOldTree is called. This slot + // should be empty and unused at this moment. + JPH_ASSERT(new_root_node.mIndex == cInvalidNodeIndex); + new_root_node.mIndex = inUpdateState.mRootNodeID.GetNodeIndex(); + } + + // All queries that start from now on will use this new tree + mRootNodeIndex = new_root_idx; + +#ifdef JPH_DUMP_BROADPHASE_TREE + DumpTree(new_root_node.GetNodeID(), StringFormat("%s_POST", mName).c_str()); +#endif + +#ifdef JPH_DEBUG + ValidateTree(inBodies, inTracking, new_root_node.mIndex, mNumBodies); +#endif +} + +void QuadTree::sPartition(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inNumber, int &outMidPoint) +{ + // Handle trivial case + if (inNumber <= 4) + { + outMidPoint = inNumber / 2; + return; + } + + // Calculate bounding box of box centers + Vec3 center_min = Vec3::sReplicate(cLargeFloat); + Vec3 center_max = Vec3::sReplicate(-cLargeFloat); + for (const Vec3 *c = ioNodeCenters, *c_end = ioNodeCenters + inNumber; c < c_end; ++c) + { + Vec3 center = *c; + center_min = Vec3::sMin(center_min, center); + center_max = Vec3::sMax(center_max, center); + } + + // Calculate split plane + int dimension = (center_max - center_min).GetHighestComponentIndex(); + float split = 0.5f * (center_min + center_max)[dimension]; + + // Divide bodies + int start = 0, end = inNumber; + while (start < end) + { + // Search for first element that is on the right hand side of the split plane + while (start < end && ioNodeCenters[start][dimension] < split) + ++start; + + // Search for the first element that is on the left hand side of the split plane + while (start < end && ioNodeCenters[end - 1][dimension] >= split) + --end; + + if (start < end) + { + // Swap the two elements + std::swap(ioNodeIDs[start], ioNodeIDs[end - 1]); + std::swap(ioNodeCenters[start], ioNodeCenters[end - 1]); + ++start; + --end; + } + } + JPH_ASSERT(start == end); + + if (start > 0 && start < inNumber) + { + // Success! + outMidPoint = start; + } + else + { + // Failed to divide bodies + outMidPoint = inNumber / 2; + } +} + +void QuadTree::sPartition4(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inBegin, int inEnd, int *outSplit) +{ + NodeID *node_ids = ioNodeIDs + inBegin; + Vec3 *node_centers = ioNodeCenters + inBegin; + int number = inEnd - inBegin; + + // Partition entire range + sPartition(node_ids, node_centers, number, outSplit[2]); + + // Partition lower half + sPartition(node_ids, node_centers, outSplit[2], outSplit[1]); + + // Partition upper half + sPartition(node_ids + outSplit[2], node_centers + outSplit[2], number - outSplit[2], outSplit[3]); + + // Convert to proper range + outSplit[0] = inBegin; + outSplit[1] += inBegin; + outSplit[2] += inBegin; + outSplit[3] += outSplit[2]; + outSplit[4] = inEnd; +} + +AABox QuadTree::GetNodeOrBodyBounds(const BodyVector &inBodies, NodeID inNodeID) const +{ + if (inNodeID.IsNode()) + { + // It is a node + uint32 node_idx = inNodeID.GetNodeIndex(); + const Node &node = mAllocator->Get(node_idx); + + AABox bounds; + node.GetNodeBounds(bounds); + return bounds; + } + else + { + // It is a body + return inBodies[inNodeID.GetBodyID().GetIndex()]->GetWorldSpaceBounds(); + } +} + +QuadTree::NodeID QuadTree::BuildTree(const BodyVector &inBodies, TrackingVector &ioTracking, NodeID *ioNodeIDs, int inNumber, uint inMaxDepthMarkChanged, AABox &outBounds) +{ + // Trivial case: No bodies in tree + if (inNumber == 0) + { + outBounds = cInvalidBounds; + return NodeID::sInvalid(); + } + + // Trivial case: When we have 1 body or node, return it + if (inNumber == 1) + { + if (ioNodeIDs->IsNode()) + { + // When returning an existing node as root, ensure that no parent has been set + Node &node = mAllocator->Get(ioNodeIDs->GetNodeIndex()); + node.mParentNodeIndex = cInvalidNodeIndex; + } + outBounds = GetNodeOrBodyBounds(inBodies, *ioNodeIDs); + return *ioNodeIDs; + } + + // Calculate centers of all bodies that are to be inserted + Vec3 *centers = new Vec3 [inNumber]; + JPH_ASSERT(IsAligned(centers, JPH_VECTOR_ALIGNMENT)); + Vec3 *c = centers; + for (const NodeID *n = ioNodeIDs, *n_end = ioNodeIDs + inNumber; n < n_end; ++n, ++c) + *c = GetNodeOrBodyBounds(inBodies, *n).GetCenter(); + + // The algorithm is a recursive tree build, but to avoid the call overhead we keep track of a stack here + struct StackEntry + { + uint32 mNodeIdx; // Node index of node that is generated + int mChildIdx; // Index of child that we're currently processing + int mSplit[5]; // Indices where the node ID's have been split to form 4 partitions + uint32 mDepth; // Depth of this node in the tree + Vec3 mNodeBoundsMin; // Bounding box of this node, accumulated while iterating over children + Vec3 mNodeBoundsMax; + }; + static_assert(sizeof(StackEntry) == 64); + StackEntry stack[cStackSize / 4]; // We don't process 4 at a time in this loop but 1, so the stack can be 4x as small + int top = 0; + + // Create root node + stack[0].mNodeIdx = AllocateNode(inMaxDepthMarkChanged > 0); + stack[0].mChildIdx = -1; + stack[0].mDepth = 0; + stack[0].mNodeBoundsMin = Vec3::sReplicate(cLargeFloat); + stack[0].mNodeBoundsMax = Vec3::sReplicate(-cLargeFloat); + sPartition4(ioNodeIDs, centers, 0, inNumber, stack[0].mSplit); + + for (;;) + { + StackEntry &cur_stack = stack[top]; + + // Next child + cur_stack.mChildIdx++; + + // Check if all children processed + if (cur_stack.mChildIdx >= 4) + { + // Terminate if there's nothing left to pop + if (top <= 0) + break; + + // Add our bounds to our parents bounds + StackEntry &prev_stack = stack[top - 1]; + prev_stack.mNodeBoundsMin = Vec3::sMin(prev_stack.mNodeBoundsMin, cur_stack.mNodeBoundsMin); + prev_stack.mNodeBoundsMax = Vec3::sMax(prev_stack.mNodeBoundsMax, cur_stack.mNodeBoundsMax); + + // Store parent node + Node &node = mAllocator->Get(cur_stack.mNodeIdx); + node.mParentNodeIndex = prev_stack.mNodeIdx; + + // Store this node's properties in the parent node + Node &parent_node = mAllocator->Get(prev_stack.mNodeIdx); + parent_node.mChildNodeID[prev_stack.mChildIdx] = NodeID::sFromNodeIndex(cur_stack.mNodeIdx); + parent_node.SetChildBounds(prev_stack.mChildIdx, AABox(cur_stack.mNodeBoundsMin, cur_stack.mNodeBoundsMax)); + + // Pop entry from stack + --top; + } + else + { + // Get low and high index to bodies to process + int low = cur_stack.mSplit[cur_stack.mChildIdx]; + int high = cur_stack.mSplit[cur_stack.mChildIdx + 1]; + int num_bodies = high - low; + + if (num_bodies == 1) + { + // Get body info + NodeID child_node_id = ioNodeIDs[low]; + AABox bounds = GetNodeOrBodyBounds(inBodies, child_node_id); + + // Update node + Node &node = mAllocator->Get(cur_stack.mNodeIdx); + node.mChildNodeID[cur_stack.mChildIdx] = child_node_id; + node.SetChildBounds(cur_stack.mChildIdx, bounds); + + if (child_node_id.IsNode()) + { + // Update parent for this node + Node &child_node = mAllocator->Get(child_node_id.GetNodeIndex()); + child_node.mParentNodeIndex = cur_stack.mNodeIdx; + } + else + { + // Set location in tracking + SetBodyLocation(ioTracking, child_node_id.GetBodyID(), cur_stack.mNodeIdx, cur_stack.mChildIdx); + } + + // Encapsulate bounding box in parent + cur_stack.mNodeBoundsMin = Vec3::sMin(cur_stack.mNodeBoundsMin, bounds.mMin); + cur_stack.mNodeBoundsMax = Vec3::sMax(cur_stack.mNodeBoundsMax, bounds.mMax); + } + else if (num_bodies > 1) + { + // Allocate new node + StackEntry &new_stack = stack[++top]; + JPH_ASSERT(top < cStackSize / 4); + uint32 next_depth = cur_stack.mDepth + 1; + new_stack.mNodeIdx = AllocateNode(inMaxDepthMarkChanged > next_depth); + new_stack.mChildIdx = -1; + new_stack.mDepth = next_depth; + new_stack.mNodeBoundsMin = Vec3::sReplicate(cLargeFloat); + new_stack.mNodeBoundsMax = Vec3::sReplicate(-cLargeFloat); + sPartition4(ioNodeIDs, centers, low, high, new_stack.mSplit); + } + } + } + + // Delete temporary data + delete [] centers; + + // Store bounding box of root + outBounds.mMin = stack[0].mNodeBoundsMin; + outBounds.mMax = stack[0].mNodeBoundsMax; + + // Return root + return NodeID::sFromNodeIndex(stack[0].mNodeIdx); +} + +void QuadTree::MarkNodeAndParentsChanged(uint32 inNodeIndex) +{ + uint32 node_idx = inNodeIndex; + + do + { + // If node has changed, parent will be too + Node &node = mAllocator->Get(node_idx); + if (node.mIsChanged) + break; + + // Mark node as changed + node.mIsChanged = true; + + // Get our parent + node_idx = node.mParentNodeIndex; + } + while (node_idx != cInvalidNodeIndex); +} + +void QuadTree::WidenAndMarkNodeAndParentsChanged(uint32 inNodeIndex, const AABox &inNewBounds) +{ + uint32 node_idx = inNodeIndex; + + for (;;) + { + // Mark node as changed + Node &node = mAllocator->Get(node_idx); + node.mIsChanged = true; + + // Get our parent + uint32 parent_idx = node.mParentNodeIndex; + if (parent_idx == cInvalidNodeIndex) + break; + + // Find which child of the parent we're in + Node &parent_node = mAllocator->Get(parent_idx); + NodeID node_id = NodeID::sFromNodeIndex(node_idx); + int child_idx = -1; + for (int i = 0; i < 4; ++i) + if (parent_node.mChildNodeID[i] == node_id) + { + // Found one, set the node index and child index and update the bounding box too + child_idx = i; + break; + } + JPH_ASSERT(child_idx != -1, "Nodes don't get removed from the tree, we must have found it"); + + // To avoid any race conditions with other threads we only enlarge bounding boxes + if (!parent_node.EncapsulateChildBounds(child_idx, inNewBounds)) + { + // No changes to bounding box, only marking as changed remains to be done + if (!parent_node.mIsChanged) + MarkNodeAndParentsChanged(parent_idx); + break; + } + + // Update node index + node_idx = parent_idx; + } +} + +bool QuadTree::TryInsertLeaf(TrackingVector &ioTracking, int inNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies) +{ + // Tentively assign the node as parent + bool leaf_is_node = inLeafID.IsNode(); + if (leaf_is_node) + { + uint32 leaf_idx = inLeafID.GetNodeIndex(); + mAllocator->Get(leaf_idx).mParentNodeIndex = inNodeIndex; + } + + // Fetch node that we're adding to + Node &node = mAllocator->Get(inNodeIndex); + + // Find an empty child + for (uint32 child_idx = 0; child_idx < 4; ++child_idx) + if (node.mChildNodeID[child_idx].CompareExchange(NodeID::sInvalid(), inLeafID)) // Check if we can claim it + { + // We managed to add it to the node + + // If leaf was a body, we need to update its bookkeeping + if (!leaf_is_node) + SetBodyLocation(ioTracking, inLeafID.GetBodyID(), inNodeIndex, child_idx); + + // Now set the bounding box making the child valid for queries + node.SetChildBounds(child_idx, inLeafBounds); + + // Widen the bounds for our parents too + WidenAndMarkNodeAndParentsChanged(inNodeIndex, inLeafBounds); + + // Update body counter + mNumBodies += inLeafNumBodies; + + // And we're done + return true; + } + + return false; +} + +bool QuadTree::TryCreateNewRoot(TrackingVector &ioTracking, atomic &ioRootNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies) +{ + // Fetch old root + uint32 root_idx = ioRootNodeIndex; + Node &root = mAllocator->Get(root_idx); + + // Create new root, mark this new root as changed as we're not creating a very efficient tree at this point + uint32 new_root_idx = AllocateNode(true); + Node &new_root = mAllocator->Get(new_root_idx); + + // First child is current root, note that since the tree may be modified concurrently we cannot assume that the bounds of our child will be correct so we set a very large bounding box + new_root.mChildNodeID[0] = NodeID::sFromNodeIndex(root_idx); + new_root.SetChildBounds(0, AABox(Vec3::sReplicate(-cLargeFloat), Vec3::sReplicate(cLargeFloat))); + + // Second child is new leaf + new_root.mChildNodeID[1] = inLeafID; + new_root.SetChildBounds(1, inLeafBounds); + + // Tentatively assign new root as parent + bool leaf_is_node = inLeafID.IsNode(); + if (leaf_is_node) + { + uint32 leaf_idx = inLeafID.GetNodeIndex(); + mAllocator->Get(leaf_idx).mParentNodeIndex = new_root_idx; + } + + // Try to swap it + if (ioRootNodeIndex.compare_exchange_strong(root_idx, new_root_idx)) + { + // We managed to set the new root + + // If leaf was a body, we need to update its bookkeeping + if (!leaf_is_node) + SetBodyLocation(ioTracking, inLeafID.GetBodyID(), new_root_idx, 1); + + // Store parent node for old root + root.mParentNodeIndex = new_root_idx; + + // Update body counter + mNumBodies += inLeafNumBodies; + + // And we're done + return true; + } + + // Failed to swap, someone else must have created a new root, try again + mAllocator->DestructObject(new_root_idx); + return false; +} + +void QuadTree::AddBodiesPrepare(const BodyVector &inBodies, TrackingVector &ioTracking, BodyID *ioBodyIDs, int inNumber, AddState &outState) +{ + // Assert sane input + JPH_ASSERT(ioBodyIDs != nullptr); + JPH_ASSERT(inNumber > 0); + +#ifdef JPH_ENABLE_ASSERTS + // Below we just cast the body ID's to node ID's, check here that that is valid + for (const BodyID *b = ioBodyIDs, *b_end = ioBodyIDs + inNumber; b < b_end; ++b) + NodeID::sFromBodyID(*b); +#endif + + // Build subtree for the new bodies, note that we mark all nodes as 'not changed' + // so they will stay together as a batch and will make the tree rebuild cheaper + outState.mLeafID = BuildTree(inBodies, ioTracking, (NodeID *)ioBodyIDs, inNumber, 0, outState.mLeafBounds); + +#ifdef JPH_DEBUG + if (outState.mLeafID.IsNode()) + ValidateTree(inBodies, ioTracking, outState.mLeafID.GetNodeIndex(), inNumber); +#endif +} + +void QuadTree::AddBodiesFinalize(TrackingVector &ioTracking, int inNumberBodies, const AddState &inState) +{ + // Assert sane input + JPH_ASSERT(inNumberBodies > 0); + + // Mark tree dirty + mIsDirty = true; + + // Get the current root node + RootNode &root_node = GetCurrentRoot(); + + for (;;) + { + // Check if we can insert the body in the root + if (TryInsertLeaf(ioTracking, root_node.mIndex, inState.mLeafID, inState.mLeafBounds, inNumberBodies)) + return; + + // Check if we can create a new root + if (TryCreateNewRoot(ioTracking, root_node.mIndex, inState.mLeafID, inState.mLeafBounds, inNumberBodies)) + return; + } +} + +void QuadTree::AddBodiesAbort(TrackingVector &ioTracking, const AddState &inState) +{ + // Collect all bodies + Allocator::Batch free_batch; + NodeID node_stack[cStackSize]; + node_stack[0] = inState.mLeafID; + JPH_ASSERT(node_stack[0].IsValid()); + int top = 0; + do + { + // Check if node is a body + NodeID child_node_id = node_stack[top]; + if (child_node_id.IsBody()) + { + // Reset location of body + sInvalidateBodyLocation(ioTracking, child_node_id.GetBodyID()); + } + else + { + // Process normal node + uint32 node_idx = child_node_id.GetNodeIndex(); + const Node &node = mAllocator->Get(node_idx); + for (NodeID sub_child_node_id : node.mChildNodeID) + if (sub_child_node_id.IsValid()) + { + JPH_ASSERT(top < cStackSize); + node_stack[top] = sub_child_node_id; + top++; + } + + // Mark it to be freed + mAllocator->AddObjectToBatch(free_batch, node_idx); + } + --top; + } + while (top >= 0); + + // Now free all nodes as a single batch + mAllocator->DestructObjectBatch(free_batch); +} + +void QuadTree::RemoveBodies([[maybe_unused]] const BodyVector &inBodies, TrackingVector &ioTracking, const BodyID *ioBodyIDs, int inNumber) +{ + // Assert sane input + JPH_ASSERT(ioBodyIDs != nullptr); + JPH_ASSERT(inNumber > 0); + + // Mark tree dirty + mIsDirty = true; + + for (const BodyID *cur = ioBodyIDs, *end = ioBodyIDs + inNumber; cur < end; ++cur) + { + // Check if BodyID is correct + JPH_ASSERT(inBodies[cur->GetIndex()]->GetID() == *cur, "Provided BodyID doesn't match BodyID in body manager"); + + // Get location of body + uint32 node_idx, child_idx; + GetBodyLocation(ioTracking, *cur, node_idx, child_idx); + + // First we reset our internal bookkeeping + sInvalidateBodyLocation(ioTracking, *cur); + + // Then we make the bounding box invalid, no queries can find this node anymore + Node &node = mAllocator->Get(node_idx); + node.InvalidateChildBounds(child_idx); + + // Finally we reset the child id, this makes the node available for adds again + node.mChildNodeID[child_idx] = NodeID::sInvalid(); + + // We don't need to bubble up our bounding box changes to our parents since we never make volumes smaller, only bigger + // But we do need to mark the nodes as changed so that the tree can be rebuilt + MarkNodeAndParentsChanged(node_idx); + } + + mNumBodies -= inNumber; +} + +void QuadTree::NotifyBodiesAABBChanged(const BodyVector &inBodies, const TrackingVector &inTracking, const BodyID *ioBodyIDs, int inNumber) +{ + // Assert sane input + JPH_ASSERT(ioBodyIDs != nullptr); + JPH_ASSERT(inNumber > 0); + + for (const BodyID *cur = ioBodyIDs, *end = ioBodyIDs + inNumber; cur < end; ++cur) + { + // Check if BodyID is correct + const Body *body = inBodies[cur->GetIndex()]; + JPH_ASSERT(body->GetID() == *cur, "Provided BodyID doesn't match BodyID in body manager"); + + // Get the new bounding box + const AABox &new_bounds = body->GetWorldSpaceBounds(); + + // Get location of body + uint32 node_idx, child_idx; + GetBodyLocation(inTracking, *cur, node_idx, child_idx); + + // Widen bounds for node + Node &node = mAllocator->Get(node_idx); + if (node.EncapsulateChildBounds(child_idx, new_bounds)) + { + // Mark tree dirty + mIsDirty = true; + + // If bounds changed, widen the bounds for our parents too + WidenAndMarkNodeAndParentsChanged(node_idx, new_bounds); + } + } +} + +template +JPH_INLINE void QuadTree::WalkTree(const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking, Visitor &ioVisitor JPH_IF_TRACK_BROADPHASE_STATS(, LayerToStats &ioStats)) const +{ + // Get the root + const RootNode &root_node = GetCurrentRoot(); + +#ifdef JPH_TRACK_BROADPHASE_STATS + // Start tracking stats + int bodies_visited = 0; + int hits_collected = 0; + int nodes_visited = 0; + uint64 collector_ticks = 0; + + uint64 start = GetProcessorTickCount(); +#endif // JPH_TRACK_BROADPHASE_STATS + + NodeID node_stack[cStackSize]; + node_stack[0] = root_node.GetNodeID(); + int top = 0; + do + { + // Check if node is a body + NodeID child_node_id = node_stack[top]; + if (child_node_id.IsBody()) + { + // Track amount of bodies visited + JPH_IF_TRACK_BROADPHASE_STATS(++bodies_visited;) + + BodyID body_id = child_node_id.GetBodyID(); + ObjectLayer object_layer = inTracking[body_id.GetIndex()].mObjectLayer; // We're not taking a lock on the body, so it may be in the process of being removed so check if the object layer is invalid + if (object_layer != cObjectLayerInvalid && inObjectLayerFilter.ShouldCollide(object_layer)) + { + JPH_PROFILE("VisitBody"); + + // Track amount of hits + JPH_IF_TRACK_BROADPHASE_STATS(++hits_collected;) + + // Start track time the collector takes + JPH_IF_TRACK_BROADPHASE_STATS(uint64 collector_start = GetProcessorTickCount();) + + // We found a body we collide with, call our visitor + ioVisitor.VisitBody(body_id, top); + + // End track time the collector takes + JPH_IF_TRACK_BROADPHASE_STATS(collector_ticks += GetProcessorTickCount() - collector_start;) + + // Check if we're done + if (ioVisitor.ShouldAbort()) + break; + } + } + else if (child_node_id.IsValid()) + { + JPH_IF_TRACK_BROADPHASE_STATS(++nodes_visited;) + + // Check if stack can hold more nodes + if (top + 4 < cStackSize) + { + // Process normal node + const Node &node = mAllocator->Get(child_node_id.GetNodeIndex()); + JPH_ASSERT(IsAligned(&node, JPH_CACHE_LINE_SIZE)); + + // Load bounds of 4 children + Vec4 bounds_minx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinX); + Vec4 bounds_miny = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinY); + Vec4 bounds_minz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinZ); + Vec4 bounds_maxx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxX); + Vec4 bounds_maxy = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxY); + Vec4 bounds_maxz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxZ); + + // Load ids for 4 children + UVec4 child_ids = UVec4::sLoadInt4Aligned((const uint32 *)&node.mChildNodeID[0]); + + // Check which sub nodes to visit + int num_results = ioVisitor.VisitNodes(bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz, child_ids, top); + child_ids.StoreInt4((uint32 *)&node_stack[top]); + top += num_results; + } + else + JPH_ASSERT(false, "Stack full!\n" + "This must be a very deep tree. Are you batch adding bodies through BodyInterface::AddBodiesPrepare/AddBodiesFinalize?\n" + "If you add lots of bodies through BodyInterface::AddBody you may need to call PhysicsSystem::OptimizeBroadPhase to rebuild the tree."); + } + + // Fetch next node until we find one that the visitor wants to see + do + --top; + while (top >= 0 && !ioVisitor.ShouldVisitNode(top)); + } + while (top >= 0); + +#ifdef JPH_TRACK_BROADPHASE_STATS + // Calculate total time the broadphase walk took + uint64 total_ticks = GetProcessorTickCount() - start; + + // Update stats under lock protection (slow!) + { + unique_lock lock(mStatsMutex); + Stat &s = ioStats[inObjectLayerFilter.GetDescription()]; + s.mNumQueries++; + s.mNodesVisited += nodes_visited; + s.mBodiesVisited += bodies_visited; + s.mHitsReported += hits_collected; + s.mTotalTicks += total_ticks; + s.mCollectorTicks += collector_ticks; + } +#endif // JPH_TRACK_BROADPHASE_STATS +} + +void QuadTree::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const +{ + class Visitor + { + public: + /// Constructor + JPH_INLINE Visitor(const RayCast &inRay, RayCastBodyCollector &ioCollector) : + mOrigin(inRay.mOrigin), + mInvDirection(inRay.mDirection), + mCollector(ioCollector) + { + mFractionStack[0] = -1; + } + + /// Returns true if further processing of the tree should be aborted + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Returns true if this node / body should be visited, false if no hit can be generated + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mFractionStack[inStackTop] < mCollector.GetEarlyOutFraction(); + } + + /// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector. + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) + { + // Test the ray against 4 bounding boxes + Vec4 fraction = RayAABox4(mOrigin, mInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(fraction, mCollector.GetEarlyOutFraction(), ioChildNodeIDs, &mFractionStack[inStackTop]); + } + + /// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore + JPH_INLINE void VisitBody(const BodyID &inBodyID, int inStackTop) + { + // Store potential hit with body + BroadPhaseCastResult result { inBodyID, mFractionStack[inStackTop] }; + mCollector.AddHit(result); + } + + private: + Vec3 mOrigin; + RayInvDirection mInvDirection; + RayCastBodyCollector & mCollector; + float mFractionStack[cStackSize]; + }; + + Visitor visitor(inRay, ioCollector); + WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCastRayStats)); +} + +void QuadTree::CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const +{ + class Visitor + { + public: + /// Constructor + JPH_INLINE Visitor(const AABox &inBox, CollideShapeBodyCollector &ioCollector) : + mBox(inBox), + mCollector(ioCollector) + { + } + + /// Returns true if further processing of the tree should be aborted + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Returns true if this node / body should be visited, false if no hit can be generated + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return true; + } + + /// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector. + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) const + { + // Test the box vs 4 boxes + UVec4 hitting = AABox4VsBox(mBox, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(hitting, ioChildNodeIDs); + } + + /// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore + JPH_INLINE void VisitBody(const BodyID &inBodyID, int inStackTop) + { + // Store potential hit with body + mCollector.AddHit(inBodyID); + } + + private: + const AABox & mBox; + CollideShapeBodyCollector & mCollector; + }; + + Visitor visitor(inBox, ioCollector); + WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCollideAABoxStats)); +} + +void QuadTree::CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const +{ + class Visitor + { + public: + /// Constructor + JPH_INLINE Visitor(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector) : + mCenterX(inCenter.SplatX()), + mCenterY(inCenter.SplatY()), + mCenterZ(inCenter.SplatZ()), + mRadiusSq(Vec4::sReplicate(Square(inRadius))), + mCollector(ioCollector) + { + } + + /// Returns true if further processing of the tree should be aborted + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Returns true if this node / body should be visited, false if no hit can be generated + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return true; + } + + /// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector. + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) const + { + // Test 4 boxes vs sphere + UVec4 hitting = AABox4VsSphere(mCenterX, mCenterY, mCenterZ, mRadiusSq, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(hitting, ioChildNodeIDs); + } + + /// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore + JPH_INLINE void VisitBody(const BodyID &inBodyID, int inStackTop) + { + // Store potential hit with body + mCollector.AddHit(inBodyID); + } + + private: + Vec4 mCenterX; + Vec4 mCenterY; + Vec4 mCenterZ; + Vec4 mRadiusSq; + CollideShapeBodyCollector & mCollector; + }; + + Visitor visitor(inCenter, inRadius, ioCollector); + WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCollideSphereStats)); +} + +void QuadTree::CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const +{ + class Visitor + { + public: + /// Constructor + JPH_INLINE Visitor(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector) : + mPoint(inPoint), + mCollector(ioCollector) + { + } + + /// Returns true if further processing of the tree should be aborted + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Returns true if this node / body should be visited, false if no hit can be generated + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return true; + } + + /// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector. + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) const + { + // Test if point overlaps with box + UVec4 hitting = AABox4VsPoint(mPoint, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(hitting, ioChildNodeIDs); + } + + /// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore + JPH_INLINE void VisitBody(const BodyID &inBodyID, int inStackTop) + { + // Store potential hit with body + mCollector.AddHit(inBodyID); + } + + private: + Vec3 mPoint; + CollideShapeBodyCollector & mCollector; + }; + + Visitor visitor(inPoint, ioCollector); + WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCollidePointStats)); +} + +void QuadTree::CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const +{ + class Visitor + { + public: + /// Constructor + JPH_INLINE Visitor(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector) : + mBox(inBox), + mCollector(ioCollector) + { + } + + /// Returns true if further processing of the tree should be aborted + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Returns true if this node / body should be visited, false if no hit can be generated + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return true; + } + + /// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector. + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) const + { + // Test if point overlaps with box + UVec4 hitting = AABox4VsBox(mBox, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(hitting, ioChildNodeIDs); + } + + /// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore + JPH_INLINE void VisitBody(const BodyID &inBodyID, int inStackTop) + { + // Store potential hit with body + mCollector.AddHit(inBodyID); + } + + private: + OrientedBox mBox; + CollideShapeBodyCollector & mCollector; + }; + + Visitor visitor(inBox, ioCollector); + WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCollideOrientedBoxStats)); +} + +void QuadTree::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const +{ + class Visitor + { + public: + /// Constructor + JPH_INLINE Visitor(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector) : + mOrigin(inBox.mBox.GetCenter()), + mExtent(inBox.mBox.GetExtent()), + mInvDirection(inBox.mDirection), + mCollector(ioCollector) + { + mFractionStack[0] = -1; + } + + /// Returns true if further processing of the tree should be aborted + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Returns true if this node / body should be visited, false if no hit can be generated + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mFractionStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction(); + } + + /// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector. + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) + { + // Enlarge them by the casted aabox extents + Vec4 bounds_min_x = inBoundsMinX, bounds_min_y = inBoundsMinY, bounds_min_z = inBoundsMinZ, bounds_max_x = inBoundsMaxX, bounds_max_y = inBoundsMaxY, bounds_max_z = inBoundsMaxZ; + AABox4EnlargeWithExtent(mExtent, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test 4 children + Vec4 fraction = RayAABox4(mOrigin, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(fraction, mCollector.GetPositiveEarlyOutFraction(), ioChildNodeIDs, &mFractionStack[inStackTop]); + } + + /// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore + JPH_INLINE void VisitBody(const BodyID &inBodyID, int inStackTop) + { + // Store potential hit with body + BroadPhaseCastResult result { inBodyID, mFractionStack[inStackTop] }; + mCollector.AddHit(result); + } + + private: + Vec3 mOrigin; + Vec3 mExtent; + RayInvDirection mInvDirection; + CastShapeBodyCollector & mCollector; + float mFractionStack[cStackSize]; + }; + + Visitor visitor(inBox, ioCollector); + WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCastAABoxStats)); +} + +void QuadTree::FindCollidingPairs(const BodyVector &inBodies, const BodyID *inActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, BodyPairCollector &ioPairCollector, const ObjectLayerPairFilter &inObjectLayerPairFilter) const +{ + // Note that we don't lock the tree at this point. We know that the tree is not going to be swapped or deleted while finding collision pairs due to the way the jobs are scheduled in the PhysicsSystem::Update. + // We double check this at the end of the function. + const RootNode &root_node = GetCurrentRoot(); + JPH_ASSERT(root_node.mIndex != cInvalidNodeIndex); + + // Assert sane input + JPH_ASSERT(inActiveBodies != nullptr); + JPH_ASSERT(inNumActiveBodies > 0); + + NodeID node_stack[cStackSize]; + + // Loop over all active bodies + for (int b1 = 0; b1 < inNumActiveBodies; ++b1) + { + BodyID b1_id = inActiveBodies[b1]; + const Body &body1 = *inBodies[b1_id.GetIndex()]; + JPH_ASSERT(!body1.IsStatic()); + + // Expand the bounding box by the speculative contact distance + AABox bounds1 = body1.GetWorldSpaceBounds(); + bounds1.ExpandBy(Vec3::sReplicate(inSpeculativeContactDistance)); + + // Test each body with the tree + node_stack[0] = root_node.GetNodeID(); + int top = 0; + do + { + // Check if node is a body + NodeID child_node_id = node_stack[top]; + if (child_node_id.IsBody()) + { + // Don't collide with self + BodyID b2_id = child_node_id.GetBodyID(); + if (b1_id != b2_id) + { + // Collision between dynamic pairs need to be picked up only once + const Body &body2 = *inBodies[b2_id.GetIndex()]; + if (inObjectLayerPairFilter.ShouldCollide(body1.GetObjectLayer(), body2.GetObjectLayer()) + && Body::sFindCollidingPairsCanCollide(body1, body2) + && bounds1.Overlaps(body2.GetWorldSpaceBounds())) // In the broadphase we widen the bounding box when a body moves, do a final check to see if the bounding boxes actually overlap + { + // Store potential hit between bodies + ioPairCollector.AddHit({ b1_id, b2_id }); + } + } + } + else if (child_node_id.IsValid()) + { + // Process normal node + const Node &node = mAllocator->Get(child_node_id.GetNodeIndex()); + JPH_ASSERT(IsAligned(&node, JPH_CACHE_LINE_SIZE)); + + // Get bounds of 4 children + Vec4 bounds_minx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinX); + Vec4 bounds_miny = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinY); + Vec4 bounds_minz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinZ); + Vec4 bounds_maxx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxX); + Vec4 bounds_maxy = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxY); + Vec4 bounds_maxz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxZ); + + // Test overlap + UVec4 overlap = AABox4VsBox(bounds1, bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz); + int num_results = overlap.CountTrues(); + if (num_results > 0) + { + // Load ids for 4 children + UVec4 child_ids = UVec4::sLoadInt4Aligned((const uint32 *)&node.mChildNodeID[0]); + + // Sort so that overlaps are first + child_ids = UVec4::sSort4True(overlap, child_ids); + + // Push them onto the stack + if (top + 4 < cStackSize) + { + child_ids.StoreInt4((uint32 *)&node_stack[top]); + top += num_results; + } + else + JPH_ASSERT(false, "Stack full!\n" + "This must be a very deep tree. Are you batch adding bodies through BodyInterface::AddBodiesPrepare/AddBodiesFinalize?\n" + "If you add lots of bodies through BodyInterface::AddBody you may need to call PhysicsSystem::OptimizeBroadPhase to rebuild the tree."); + } + } + --top; + } + while (top >= 0); + } + + // Test that the root node was not swapped while finding collision pairs. + // This would mean that UpdateFinalize/DiscardOldTree ran during collision detection which should not be possible due to the way the jobs are scheduled. + JPH_ASSERT(root_node.mIndex != cInvalidNodeIndex); + JPH_ASSERT(&root_node == &GetCurrentRoot()); +} + +#ifdef JPH_DEBUG + +void QuadTree::ValidateTree(const BodyVector &inBodies, const TrackingVector &inTracking, uint32 inNodeIndex, uint32 inNumExpectedBodies) const +{ + JPH_PROFILE_FUNCTION(); + + // Root should be valid + JPH_ASSERT(inNodeIndex != cInvalidNodeIndex); + + // To avoid call overhead, create a stack in place + struct StackEntry + { + uint32 mNodeIndex; + uint32 mParentNodeIndex; + }; + StackEntry stack[cStackSize]; + stack[0].mNodeIndex = inNodeIndex; + stack[0].mParentNodeIndex = cInvalidNodeIndex; + int top = 0; + + uint32 num_bodies = 0; + + do + { + // Copy entry from the stack + StackEntry cur_stack = stack[top]; + + // Validate parent + const Node &node = mAllocator->Get(cur_stack.mNodeIndex); + JPH_ASSERT(node.mParentNodeIndex == cur_stack.mParentNodeIndex); + + // Validate that when a parent is not-changed that all of its children are also + JPH_ASSERT(cur_stack.mParentNodeIndex == cInvalidNodeIndex || mAllocator->Get(cur_stack.mParentNodeIndex).mIsChanged || !node.mIsChanged); + + // Loop children + for (uint32 i = 0; i < 4; ++i) + { + NodeID child_node_id = node.mChildNodeID[i]; + if (child_node_id.IsValid()) + { + if (child_node_id.IsNode()) + { + // Child is a node, recurse + uint32 child_idx = child_node_id.GetNodeIndex(); + JPH_ASSERT(top < cStackSize); + StackEntry &new_entry = stack[top++]; + new_entry.mNodeIndex = child_idx; + new_entry.mParentNodeIndex = cur_stack.mNodeIndex; + + // Validate that the bounding box is bigger or equal to the bounds in the tree + // Bounding box could also be invalid if all children of our child were removed + AABox child_bounds; + node.GetChildBounds(i, child_bounds); + AABox real_child_bounds; + mAllocator->Get(child_idx).GetNodeBounds(real_child_bounds); + JPH_ASSERT(child_bounds.Contains(real_child_bounds) || !real_child_bounds.IsValid()); + } + else + { + // Increment number of bodies found + ++num_bodies; + + // Check if tracker matches position of body + uint32 node_idx, child_idx; + GetBodyLocation(inTracking, child_node_id.GetBodyID(), node_idx, child_idx); + JPH_ASSERT(node_idx == cur_stack.mNodeIndex); + JPH_ASSERT(child_idx == i); + + // Validate that the body bounds are bigger or equal to the bounds in the tree + AABox body_bounds; + node.GetChildBounds(i, body_bounds); + const Body *body = inBodies[child_node_id.GetBodyID().GetIndex()]; + AABox cached_body_bounds = body->GetWorldSpaceBounds(); + AABox real_body_bounds = body->GetShape()->GetWorldSpaceBounds(body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f)); + JPH_ASSERT(cached_body_bounds == real_body_bounds); // Check that cached body bounds are up to date + JPH_ASSERT(body_bounds.Contains(real_body_bounds)); + } + } + } + --top; + } + while (top >= 0); + + // Check that the amount of bodies in the tree matches our counter + JPH_ASSERT(num_bodies == inNumExpectedBodies); +} + +#endif + +#ifdef JPH_DUMP_BROADPHASE_TREE + +void QuadTree::DumpTree(const NodeID &inRoot, const char *inFileNamePrefix) const +{ + // Open DOT file + std::ofstream f; + f.open(StringFormat("%s.dot", inFileNamePrefix).c_str(), std::ofstream::out | std::ofstream::trunc); + if (!f.is_open()) + return; + + // Write header + f << "digraph {\n"; + + // Iterate the entire tree + NodeID node_stack[cStackSize]; + node_stack[0] = inRoot; + JPH_ASSERT(node_stack[0].IsValid()); + int top = 0; + do + { + // Check if node is a body + NodeID node_id = node_stack[top]; + if (node_id.IsBody()) + { + // Output body + String body_id = ConvertToString(node_id.GetBodyID().GetIndex()); + f << "body" << body_id << "[label = \"Body " << body_id << "\"]\n"; + } + else + { + // Process normal node + uint32 node_idx = node_id.GetNodeIndex(); + const Node &node = mAllocator->Get(node_idx); + + // Get bounding box + AABox bounds; + node.GetNodeBounds(bounds); + + // Output node + String node_str = ConvertToString(node_idx); + f << "node" << node_str << "[label = \"Node " << node_str << "\nVolume: " << ConvertToString(bounds.GetVolume()) << "\" color=" << (node.mIsChanged? "red" : "black") << "]\n"; + + // Recurse and get all children + for (NodeID child_node_id : node.mChildNodeID) + if (child_node_id.IsValid()) + { + JPH_ASSERT(top < cStackSize); + node_stack[top] = child_node_id; + top++; + + // Output link + f << "node" << node_str << " -> "; + if (child_node_id.IsBody()) + f << "body" << ConvertToString(child_node_id.GetBodyID().GetIndex()); + else + f << "node" << ConvertToString(child_node_id.GetNodeIndex()); + f << "\n"; + } + } + --top; + } + while (top >= 0); + + // Finish DOT file + f << "}\n"; + f.close(); + + // Convert to svg file + String cmd = StringFormat("dot %s.dot -Tsvg -o %s.svg", inFileNamePrefix, inFileNamePrefix); + system(cmd.c_str()); +} + +#endif // JPH_DUMP_BROADPHASE_TREE + +#ifdef JPH_TRACK_BROADPHASE_STATS + +uint64 QuadTree::GetTicks100Pct(const LayerToStats &inLayer) const +{ + uint64 total_ticks = 0; + for (const LayerToStats::value_type &kv : inLayer) + total_ticks += kv.second.mTotalTicks; + return total_ticks; +} + +void QuadTree::ReportStats(const char *inName, const LayerToStats &inLayer, uint64 inTicks100Pct) const +{ + for (const LayerToStats::value_type &kv : inLayer) + { + double total_pct = 100.0 * double(kv.second.mTotalTicks) / double(inTicks100Pct); + double total_pct_excl_collector = 100.0 * double(kv.second.mTotalTicks - kv.second.mCollectorTicks) / double(inTicks100Pct); + double hits_reported_vs_bodies_visited = kv.second.mBodiesVisited > 0? 100.0 * double(kv.second.mHitsReported) / double(kv.second.mBodiesVisited) : 100.0; + double hits_reported_vs_nodes_visited = kv.second.mNodesVisited > 0? double(kv.second.mHitsReported) / double(kv.second.mNodesVisited) : -1.0; + + std::stringstream str; + str << inName << ", " << kv.first << ", " << mName << ", " << kv.second.mNumQueries << ", " << total_pct << ", " << total_pct_excl_collector << ", " << kv.second.mNodesVisited << ", " << kv.second.mBodiesVisited << ", " << kv.second.mHitsReported << ", " << hits_reported_vs_bodies_visited << ", " << hits_reported_vs_nodes_visited; + Trace(str.str().c_str()); + } +} + +uint64 QuadTree::GetTicks100Pct() const +{ + uint64 total_ticks = 0; + total_ticks += GetTicks100Pct(mCastRayStats); + total_ticks += GetTicks100Pct(mCollideAABoxStats); + total_ticks += GetTicks100Pct(mCollideSphereStats); + total_ticks += GetTicks100Pct(mCollidePointStats); + total_ticks += GetTicks100Pct(mCollideOrientedBoxStats); + total_ticks += GetTicks100Pct(mCastAABoxStats); + return total_ticks; +} + +void QuadTree::ReportStats(uint64 inTicks100Pct) const +{ + unique_lock lock(mStatsMutex); + ReportStats("RayCast", mCastRayStats, inTicks100Pct); + ReportStats("CollideAABox", mCollideAABoxStats, inTicks100Pct); + ReportStats("CollideSphere", mCollideSphereStats, inTicks100Pct); + ReportStats("CollidePoint", mCollidePointStats, inTicks100Pct); + ReportStats("CollideOrientedBox", mCollideOrientedBoxStats, inTicks100Pct); + ReportStats("CastAABox", mCastAABoxStats, inTicks100Pct); +} + +#endif // JPH_TRACK_BROADPHASE_STATS + +uint QuadTree::GetMaxTreeDepth(const NodeID &inNodeID) const +{ + // Reached a leaf? + if (!inNodeID.IsValid() || inNodeID.IsBody()) + return 0; + + // Recurse to children + uint max_depth = 0; + const Node &node = mAllocator->Get(inNodeID.GetNodeIndex()); + for (NodeID child_node_id : node.mChildNodeID) + max_depth = max(max_depth, GetMaxTreeDepth(child_node_id)); + return max_depth + 1; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.h new file mode 100644 index 000000000000..d698beb5f372 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.h @@ -0,0 +1,390 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +//#define JPH_DUMP_BROADPHASE_TREE + +JPH_NAMESPACE_BEGIN + +/// Internal tree structure in broadphase, is essentially a quad AABB tree. +/// Tree is lockless (except for UpdatePrepare/Finalize() function), modifying objects in the tree will widen the aabbs of parent nodes to make the node fit. +/// During the UpdatePrepare/Finalize() call the tree is rebuilt to achieve a tight fit again. +class JPH_EXPORT QuadTree : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + +private: + // Forward declare + class AtomicNodeID; + + /// Class that points to either a body or a node in the tree + class NodeID + { + public: + JPH_OVERRIDE_NEW_DELETE + + /// Default constructor does not initialize + inline NodeID() = default; + + /// Construct a node ID + static inline NodeID sInvalid() { return NodeID(cInvalidNodeIndex); } + static inline NodeID sFromBodyID(BodyID inID) { NodeID node_id(inID.GetIndexAndSequenceNumber()); JPH_ASSERT(node_id.IsBody()); return node_id; } + static inline NodeID sFromNodeIndex(uint32 inIdx) { NodeID node_id(inIdx | cIsNode); JPH_ASSERT(node_id.IsNode()); return node_id; } + + /// Check what type of ID it is + inline bool IsValid() const { return mID != cInvalidNodeIndex; } + inline bool IsBody() const { return (mID & cIsNode) == 0; } + inline bool IsNode() const { return (mID & cIsNode) != 0; } + + /// Get body or node index + inline BodyID GetBodyID() const { JPH_ASSERT(IsBody()); return BodyID(mID); } + inline uint32 GetNodeIndex() const { JPH_ASSERT(IsNode()); return mID & ~cIsNode; } + + /// Comparison + inline bool operator == (const BodyID &inRHS) const { return mID == inRHS.GetIndexAndSequenceNumber(); } + inline bool operator == (const NodeID &inRHS) const { return mID == inRHS.mID; } + + private: + friend class AtomicNodeID; + + inline explicit NodeID(uint32 inID) : mID(inID) { } + + static const uint32 cIsNode = BodyID::cBroadPhaseBit; ///< If this bit is set it means that the ID refers to a node, otherwise it refers to a body + + uint32 mID; + }; + + static_assert(sizeof(NodeID) == sizeof(BodyID), "Body id's should have the same size as NodeIDs"); + + /// A NodeID that uses atomics to store the value + class AtomicNodeID + { + public: + /// Constructor + AtomicNodeID() = default; + explicit AtomicNodeID(const NodeID &inRHS) : mID(inRHS.mID) { } + + /// Assignment + inline void operator = (const NodeID &inRHS) { mID = inRHS.mID; } + + /// Getting the value + inline operator NodeID () const { return NodeID(mID); } + + /// Check if the ID is valid + inline bool IsValid() const { return mID != cInvalidNodeIndex; } + + /// Comparison + inline bool operator == (const BodyID &inRHS) const { return mID == inRHS.GetIndexAndSequenceNumber(); } + inline bool operator == (const NodeID &inRHS) const { return mID == inRHS.mID; } + + /// Atomically compare and swap value. Expects inOld value, replaces with inNew value or returns false + inline bool CompareExchange(NodeID inOld, NodeID inNew) { return mID.compare_exchange_strong(inOld.mID, inNew.mID); } + + private: + atomic mID; + }; + + /// Class that represents a node in the tree + class Node + { + public: + /// Construct node + explicit Node(bool inIsChanged); + + /// Get bounding box encapsulating all children + void GetNodeBounds(AABox &outBounds) const; + + /// Get bounding box in a consistent way with the functions below (check outBounds.IsValid() before using the box) + void GetChildBounds(int inChildIndex, AABox &outBounds) const; + + /// Set the bounds in such a way that other threads will either see a fully correct bounding box or a bounding box with no volume + void SetChildBounds(int inChildIndex, const AABox &inBounds); + + /// Invalidate bounding box in such a way that other threads will not temporarily see a very large bounding box + void InvalidateChildBounds(int inChildIndex); + + /// Encapsulate inBounds in node bounds, returns true if there were changes + bool EncapsulateChildBounds(int inChildIndex, const AABox &inBounds); + + /// Bounding box for child nodes or bodies (all initially set to invalid so no collision test will ever traverse to the leaf) + atomic mBoundsMinX[4]; + atomic mBoundsMinY[4]; + atomic mBoundsMinZ[4]; + atomic mBoundsMaxX[4]; + atomic mBoundsMaxY[4]; + atomic mBoundsMaxZ[4]; + + /// Index of child node or body ID. + AtomicNodeID mChildNodeID[4]; + + /// Index of the parent node. + /// Note: This value is unreliable during the UpdatePrepare/Finalize() function as a node may be relinked to the newly built tree. + atomic mParentNodeIndex = cInvalidNodeIndex; + + /// If this part of the tree has changed, if not, we will treat this sub tree as a single body during the UpdatePrepare/Finalize(). + /// If any changes are made to an object inside this sub tree then the direct path from the body to the top of the tree will become changed. + atomic mIsChanged; + + // Padding to align to 124 bytes + uint32 mPadding = 0; + }; + + // Maximum size of the stack during tree walk + static constexpr int cStackSize = 128; + + static_assert(sizeof(atomic) == 4, "Assuming that an atomic doesn't add any additional storage"); + static_assert(sizeof(atomic) == 4, "Assuming that an atomic doesn't add any additional storage"); + static_assert(std::is_trivially_destructible(), "Assuming that we don't have a destructor"); + +public: + /// Class that allocates tree nodes, can be shared between multiple trees + using Allocator = FixedSizeFreeList; + + static_assert(Allocator::ObjectStorageSize == 128, "Node should be 128 bytes"); + + /// Data to track location of a Body in the tree + struct Tracking + { + /// Constructor to satisfy the vector class + Tracking() = default; + Tracking(const Tracking &inRHS) : mBroadPhaseLayer(inRHS.mBroadPhaseLayer.load()), mObjectLayer(inRHS.mObjectLayer.load()), mBodyLocation(inRHS.mBodyLocation.load()) { } + + /// Invalid body location identifier + static const uint32 cInvalidBodyLocation = 0xffffffff; + + atomic mBroadPhaseLayer = (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid; + atomic mObjectLayer = cObjectLayerInvalid; + atomic mBodyLocation { cInvalidBodyLocation }; + }; + + using TrackingVector = Array; + + /// Destructor + ~QuadTree(); + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + /// Name of the tree for debugging purposes + void SetName(const char *inName) { mName = inName; } + inline const char * GetName() const { return mName; } +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED + + /// Check if there is anything in the tree + inline bool HasBodies() const { return mNumBodies != 0; } + + /// Check if the tree needs an UpdatePrepare/Finalize() + inline bool IsDirty() const { return mIsDirty; } + + /// Check if this tree can get an UpdatePrepare/Finalize() or if it needs a DiscardOldTree() first + inline bool CanBeUpdated() const { return mFreeNodeBatch.mNumObjects == 0; } + + /// Initialization + void Init(Allocator &inAllocator); + + struct UpdateState + { + NodeID mRootNodeID; ///< This will be the new root node id + }; + + /// Will throw away the previous frame's nodes so that we can start building a new tree in the background + void DiscardOldTree(); + + /// Get the bounding box for this tree + AABox GetBounds() const; + + /// Update the broadphase, needs to be called regularly to achieve a tight fit of the tree when bodies have been modified. + /// UpdatePrepare() will build the tree, UpdateFinalize() will lock the root of the tree shortly and swap the trees and afterwards clean up temporary data structures. + void UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTracking, UpdateState &outUpdateState, bool inFullRebuild); + void UpdateFinalize(const BodyVector &inBodies, const TrackingVector &inTracking, const UpdateState &inUpdateState); + + /// Temporary data structure to pass information between AddBodiesPrepare and AddBodiesFinalize/Abort + struct AddState + { + NodeID mLeafID = NodeID::sInvalid(); + AABox mLeafBounds; + }; + + /// Prepare adding inNumber bodies at ioBodyIDs to the quad tree, returns the state in outState that should be used in AddBodiesFinalize. + /// This can be done on a background thread without influencing the broadphase. + /// ioBodyIDs may be shuffled around by this function. + void AddBodiesPrepare(const BodyVector &inBodies, TrackingVector &ioTracking, BodyID *ioBodyIDs, int inNumber, AddState &outState); + + /// Finalize adding bodies to the quadtree, supply the same number of bodies as in AddBodiesPrepare. + void AddBodiesFinalize(TrackingVector &ioTracking, int inNumberBodies, const AddState &inState); + + /// Abort adding bodies to the quadtree, supply the same bodies and state as in AddBodiesPrepare. + /// This can be done on a background thread without influencing the broadphase. + void AddBodiesAbort(TrackingVector &ioTracking, const AddState &inState); + + /// Remove inNumber bodies in ioBodyIDs from the quadtree. + void RemoveBodies(const BodyVector &inBodies, TrackingVector &ioTracking, const BodyID *ioBodyIDs, int inNumber); + + /// Call whenever the aabb of a body changes. + void NotifyBodiesAABBChanged(const BodyVector &inBodies, const TrackingVector &inTracking, const BodyID *ioBodyIDs, int inNumber); + + /// Cast a ray and get the intersecting bodies in ioCollector. + void CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const; + + /// Get bodies intersecting with inBox in ioCollector + void CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const; + + /// Get bodies intersecting with a sphere in ioCollector + void CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const; + + /// Get bodies intersecting with a point and any hits to ioCollector + void CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const; + + /// Get bodies intersecting with an oriented box and any hits to ioCollector + void CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const; + + /// Cast a box and get intersecting bodies in ioCollector + void CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const; + + /// Find all colliding pairs between dynamic bodies, calls ioPairCollector for every pair found + void FindCollidingPairs(const BodyVector &inBodies, const BodyID *inActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, BodyPairCollector &ioPairCollector, const ObjectLayerPairFilter &inObjectLayerPairFilter) const; + +#ifdef JPH_TRACK_BROADPHASE_STATS + /// Sum up all the ticks spent in the various layers + uint64 GetTicks100Pct() const; + + /// Trace the stats of this tree to the TTY + void ReportStats(uint64 inTicks100Pct) const; +#endif // JPH_TRACK_BROADPHASE_STATS + +private: + /// Constants + static const uint32 cInvalidNodeIndex = 0xffffffff; ///< Value used to indicate node index is invalid + static const float cLargeFloat; ///< A large floating point number that is small enough to not cause any overflows + static const AABox cInvalidBounds; ///< Invalid bounding box using cLargeFloat + + /// We alternate between two trees in order to let collision queries complete in parallel to adding/removing objects to the tree + struct RootNode + { + /// Get the ID of the root node + inline NodeID GetNodeID() const { return NodeID::sFromNodeIndex(mIndex); } + + /// Index of the root node of the tree (this is always a node, never a body id) + atomic mIndex { cInvalidNodeIndex }; + }; + + /// Caches location of body inBodyID in the tracker, body can be found in mNodes[inNodeIdx].mChildNodeID[inChildIdx] + void GetBodyLocation(const TrackingVector &inTracking, BodyID inBodyID, uint32 &outNodeIdx, uint32 &outChildIdx) const; + void SetBodyLocation(TrackingVector &ioTracking, BodyID inBodyID, uint32 inNodeIdx, uint32 inChildIdx) const; + static void sInvalidateBodyLocation(TrackingVector &ioTracking, BodyID inBodyID); + + /// Get the current root of the tree + JPH_INLINE const RootNode & GetCurrentRoot() const { return mRootNode[mRootNodeIndex]; } + JPH_INLINE RootNode & GetCurrentRoot() { return mRootNode[mRootNodeIndex]; } + + /// Depending on if inNodeID is a body or tree node return the bounding box + inline AABox GetNodeOrBodyBounds(const BodyVector &inBodies, NodeID inNodeID) const; + + /// Mark node and all of its parents as changed + inline void MarkNodeAndParentsChanged(uint32 inNodeIndex); + + /// Widen parent bounds of node inNodeIndex to encapsulate inNewBounds, also mark node and all of its parents as changed + inline void WidenAndMarkNodeAndParentsChanged(uint32 inNodeIndex, const AABox &inNewBounds); + + /// Allocate a new node + inline uint32 AllocateNode(bool inIsChanged); + + /// Try to insert a new leaf to the tree at inNodeIndex + inline bool TryInsertLeaf(TrackingVector &ioTracking, int inNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies); + + /// Try to replace the existing root with a new root that contains both the existing root and the new leaf + inline bool TryCreateNewRoot(TrackingVector &ioTracking, atomic &ioRootNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies); + + /// Build a tree for ioBodyIDs, returns the NodeID of the root (which will be the ID of a single body if inNumber = 1). All tree levels up to inMaxDepthMarkChanged will be marked as 'changed'. + NodeID BuildTree(const BodyVector &inBodies, TrackingVector &ioTracking, NodeID *ioNodeIDs, int inNumber, uint inMaxDepthMarkChanged, AABox &outBounds); + + /// Sorts ioNodeIDs spatially into 2 groups. Second groups starts at ioNodeIDs + outMidPoint. + /// After the function returns ioNodeIDs and ioNodeCenters will be shuffled + static void sPartition(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inNumber, int &outMidPoint); + + /// Sorts ioNodeIDs from inBegin to (but excluding) inEnd spatially into 4 groups. + /// outSplit needs to be 5 ints long, when the function returns each group runs from outSplit[i] to (but excluding) outSplit[i + 1] + /// After the function returns ioNodeIDs and ioNodeCenters will be shuffled + static void sPartition4(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inBegin, int inEnd, int *outSplit); + +#ifdef JPH_DEBUG + /// Validate that the tree is consistent. + /// Note: This function only works if the tree is not modified while we're traversing it. + void ValidateTree(const BodyVector &inBodies, const TrackingVector &inTracking, uint32 inNodeIndex, uint32 inNumExpectedBodies) const; +#endif + +#ifdef JPH_DUMP_BROADPHASE_TREE + /// Dump the tree in DOT format (see: https://graphviz.org/) + void DumpTree(const NodeID &inRoot, const char *inFileNamePrefix) const; +#endif + + /// Allocator that controls adding / freeing nodes + Allocator * mAllocator = nullptr; + + /// This is a list of nodes that must be deleted after the trees are swapped and the old tree is no longer in use + Allocator::Batch mFreeNodeBatch; + + /// Number of bodies currently in the tree + /// This is aligned to be in a different cache line from the `Allocator` pointer to prevent cross-thread syncs + /// when reading nodes. + alignas(JPH_CACHE_LINE_SIZE) atomic mNumBodies { 0 }; + + /// We alternate between two tree root nodes. When updating, we activate the new tree and we keep the old tree alive. + /// for queries that are in progress until the next time DiscardOldTree() is called. + RootNode mRootNode[2]; + atomic mRootNodeIndex { 0 }; + + /// Flag to keep track of changes to the broadphase, if false, we don't need to UpdatePrepare/Finalize() + atomic mIsDirty = false; + +#ifdef JPH_TRACK_BROADPHASE_STATS + /// Mutex protecting the various LayerToStats members + mutable Mutex mStatsMutex; + + struct Stat + { + uint64 mNumQueries = 0; + uint64 mNodesVisited = 0; + uint64 mBodiesVisited = 0; + uint64 mHitsReported = 0; + uint64 mTotalTicks = 0; + uint64 mCollectorTicks = 0; + }; + + using LayerToStats = UnorderedMap; + + /// Sum up all the ticks in a layer + uint64 GetTicks100Pct(const LayerToStats &inLayer) const; + + /// Trace the stats of a single query type to the TTY + void ReportStats(const char *inName, const LayerToStats &inLayer, uint64 inTicks100Pct) const; + + mutable LayerToStats mCastRayStats; + mutable LayerToStats mCollideAABoxStats; + mutable LayerToStats mCollideSphereStats; + mutable LayerToStats mCollidePointStats; + mutable LayerToStats mCollideOrientedBoxStats; + mutable LayerToStats mCastAABoxStats; +#endif // JPH_TRACK_BROADPHASE_STATS + + /// Debug function to get the depth of the tree from node inNodeID + uint GetMaxTreeDepth(const NodeID &inNodeID) const; + + /// Walk the node tree calling the Visitor::VisitNodes for each node encountered and Visitor::VisitBody for each body encountered + template + JPH_INLINE void WalkTree(const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking, Visitor &ioVisitor JPH_IF_TRACK_BROADPHASE_STATS(, LayerToStats &ioStats)) const; + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + /// Name of this tree for debugging purposes + const char * mName = "Layer"; +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.cpp new file mode 100644 index 000000000000..a69208a04dd7 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.cpp @@ -0,0 +1,109 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +CastConvexVsTriangles::CastConvexVsTriangles(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, CastShapeCollector &ioCollector) : + mShapeCast(inShapeCast), + mShapeCastSettings(inShapeCastSettings), + mCenterOfMassTransform2(inCenterOfMassTransform2), + mScale(inScale), + mSubShapeIDCreator1(inSubShapeIDCreator1), + mCollector(ioCollector) +{ + JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Convex); + + // Determine if shape is inside out or not + mScaleSign = ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f; +} + +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; + Vec3 v2 = mScale * inV2; + + // Calculate triangle normal + Vec3 triangle_normal = mScaleSign * (v1 - v0).Cross(v2 - v0); + + // Backface check + bool back_facing = triangle_normal.Dot(mShapeCast.mDirection) > 0.0f; + if (mShapeCastSettings.mBackFaceModeTriangles == EBackFaceMode::IgnoreBackFaces && back_facing) + return; + + // Create triangle support function + TriangleConvexSupport triangle { v0, v1, v2 }; + + // Check if we already created the cast shape support function + if (mSupport == nullptr) + { + // Determine if we want to use the actual shape or a shrunken shape with convex radius + ConvexShape::ESupportMode support_mode = mShapeCastSettings.mUseShrunkenShapeAndConvexRadius? ConvexShape::ESupportMode::ExcludeConvexRadius : ConvexShape::ESupportMode::Default; + + // Create support function + mSupport = static_cast(mShapeCast.mShape)->GetSupportFunction(support_mode, mSupportBuffer, mShapeCast.mScale); + } + + EPAPenetrationDepth epa; + float fraction = mCollector.GetEarlyOutFraction(); + Vec3 contact_point_a, contact_point_b, contact_normal; + if (epa.CastShape(mShapeCast.mCenterOfMassStart, mShapeCast.mDirection, mShapeCastSettings.mCollisionTolerance, mShapeCastSettings.mPenetrationTolerance, *mSupport, triangle, mSupport->GetConvexRadius(), 0.0f, mShapeCastSettings.mReturnDeepestPoint, fraction, contact_point_a, contact_point_b, contact_normal)) + { + // Check if we have enabled active edge detection + if (mShapeCastSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive && inActiveEdges != 0b111) + { + // Convert the active edge velocity hint to local space + Vec3 active_edge_movement_direction = mCenterOfMassTransform2.Multiply3x3Transposed(mShapeCastSettings.mActiveEdgeMovementDirection); + + // Update the contact normal to account for active edges + // Note that we flip the triangle normal as the penetration axis is pointing towards the triangle instead of away + contact_normal = ActiveEdges::FixNormal(v0, v1, v2, back_facing? triangle_normal : -triangle_normal, inActiveEdges, contact_point_b, contact_normal, active_edge_movement_direction); + } + + // Convert to world space + contact_point_a = mCenterOfMassTransform2 * contact_point_a; + contact_point_b = mCenterOfMassTransform2 * contact_point_b; + Vec3 contact_normal_world = mCenterOfMassTransform2.Multiply3x3(contact_normal); + + // Its a hit, store the sub shape id's + ShapeCastResult result(fraction, contact_point_a, contact_point_b, contact_normal_world, back_facing, mSubShapeIDCreator1.GetID(), inSubShapeID2, TransformedShape::sGetBodyID(mCollector.GetContext())); + + // Early out if this hit is deeper than the collector's early out value + if (fraction == 0.0f && -result.mPenetrationDepth >= mCollector.GetEarlyOutFraction()) + return; + + // Gather faces + if (mShapeCastSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces) + { + // Get supporting face of shape 1 + Mat44 transform_1_to_2 = mShapeCast.mCenterOfMassStart; + transform_1_to_2.SetTranslation(transform_1_to_2.GetTranslation() + fraction * mShapeCast.mDirection); + static_cast(mShapeCast.mShape)->GetSupportingFace(SubShapeID(), transform_1_to_2.Multiply3x3Transposed(-contact_normal), mShapeCast.mScale, mCenterOfMassTransform2 * transform_1_to_2, result.mShape1Face); + + // Get face of the triangle + triangle.GetSupportingFace(contact_normal, result.mShape2Face); + + // Convert to world space + for (Vec3 &p : result.mShape2Face) + p = mCenterOfMassTransform2 * p; + } + + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) + mCollector.AddHit(result); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.h new file mode 100644 index 000000000000..6a6c161cd243 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.h @@ -0,0 +1,46 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Collision detection helper that casts a convex object vs one or more triangles +class JPH_EXPORT CastConvexVsTriangles +{ +public: + /// Constructor + /// @param inShapeCast The shape to cast against the triangles and its start and direction + /// @param inShapeCastSettings Settings for performing the cast + /// @param inScale Local space scale for the shape to cast against (scales relative to its center of mass). + /// @param inCenterOfMassTransform2 Is the center of mass transform of shape 2 (excluding scale), this is used to provide a transform to the shape cast result so that local quantities can be transformed into world space. + /// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for the casting shape + /// @param ioCollector The collector that receives the results. + CastConvexVsTriangles(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, CastShapeCollector &ioCollector); + + /// Cast convex object with a single triangle + /// @param inV0 , inV1 , inV2: CCW triangle vertices + /// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active + /// An active edge is an edge that is not connected to another triangle in such a way that it is impossible to collide with the edge + /// @param inSubShapeID2 The sub shape ID for the triangle + void Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2); + +protected: + const ShapeCast & mShapeCast; + const ShapeCastSettings & mShapeCastSettings; + const Mat44 & mCenterOfMassTransform2; + Vec3 mScale; + SubShapeIDCreator mSubShapeIDCreator1; + CastShapeCollector & mCollector; + +private: + ConvexShape::SupportBuffer mSupportBuffer; ///< Buffer that holds the support function of the cast shape + const ConvexShape::Support * mSupport = nullptr; ///< Support function of the cast shape + float mScaleSign; ///< Sign of the scale, -1 if object is inside out, 1 if not +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastResult.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastResult.h new file mode 100644 index 000000000000..6bb49feb7c8b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastResult.h @@ -0,0 +1,37 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Structure that holds a ray cast or other object cast hit +class BroadPhaseCastResult +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Function required by the CollisionCollector. A smaller fraction is considered to be a 'better hit'. For rays/cast shapes we can just use the collision fraction. + inline float GetEarlyOutFraction() const { return mFraction; } + + /// Reset this result so it can be reused for a new cast. + inline void Reset() { mBodyID = BodyID(); mFraction = 1.0f + FLT_EPSILON; } + + BodyID mBodyID; ///< Body that was hit + float mFraction = 1.0f + FLT_EPSILON; ///< Hit fraction of the ray/object [0, 1], HitPoint = Start + mFraction * (End - Start) +}; + +/// Specialization of cast result against a shape +class RayCastResult : public BroadPhaseCastResult +{ +public: + JPH_OVERRIDE_NEW_DELETE + + SubShapeID mSubShapeID2; ///< Sub shape ID of shape that we collided against +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.cpp new file mode 100644 index 000000000000..166883be8da3 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.cpp @@ -0,0 +1,223 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +CastSphereVsTriangles::CastSphereVsTriangles(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, CastShapeCollector &ioCollector) : + mStart(inShapeCast.mCenterOfMassStart.GetTranslation()), + mDirection(inShapeCast.mDirection), + mShapeCastSettings(inShapeCastSettings), + mCenterOfMassTransform2(inCenterOfMassTransform2), + mScale(inScale), + mSubShapeIDCreator1(inSubShapeIDCreator1), + mCollector(ioCollector) +{ + // Cast to sphere shape + JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::Sphere); + const SphereShape *sphere = static_cast(inShapeCast.mShape); + + // Scale the radius + mRadius = sphere->GetRadius() * abs(inShapeCast.mScale.GetX()); + + // Determine if shape is inside out or not + mScaleSign = ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f; +} + +void CastSphereVsTriangles::AddHit(bool inBackFacing, const SubShapeID &inSubShapeID2, float inFraction, Vec3Arg inContactPointA, Vec3Arg inContactPointB, Vec3Arg inContactNormal) +{ + // Convert to world space + Vec3 contact_point_a = mCenterOfMassTransform2 * (mStart + inContactPointA); + Vec3 contact_point_b = mCenterOfMassTransform2 * (mStart + inContactPointB); + Vec3 contact_normal_world = mCenterOfMassTransform2.Multiply3x3(inContactNormal); + + // Its a hit, store the sub shape id's + ShapeCastResult result(inFraction, contact_point_a, contact_point_b, contact_normal_world, inBackFacing, mSubShapeIDCreator1.GetID(), inSubShapeID2, TransformedShape::sGetBodyID(mCollector.GetContext())); + + // Note: We don't gather faces here because that's only useful if both shapes have a face. Since the sphere always has only 1 contact point, the manifold is always a point. + + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) + mCollector.AddHit(result); +} + +void CastSphereVsTriangles::AddHitWithActiveEdgeDetection(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, bool inBackFacing, Vec3Arg inTriangleNormal, uint8 inActiveEdges, const SubShapeID &inSubShapeID2, float inFraction, Vec3Arg inContactPointA, Vec3Arg inContactPointB, Vec3Arg inContactNormal) +{ + // Check if we have enabled active edge detection + Vec3 contact_normal = inContactNormal; + if (mShapeCastSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive && inActiveEdges != 0b111) + { + // Convert the active edge velocity hint to local space + Vec3 active_edge_movement_direction = mCenterOfMassTransform2.Multiply3x3Transposed(mShapeCastSettings.mActiveEdgeMovementDirection); + + // Update the contact normal to account for active edges + // Note that we flip the triangle normal as the penetration axis is pointing towards the triangle instead of away + contact_normal = ActiveEdges::FixNormal(inV0, inV1, inV2, inBackFacing? inTriangleNormal : -inTriangleNormal, inActiveEdges, inContactPointB, inContactNormal, active_edge_movement_direction); + } + + AddHit(inBackFacing, inSubShapeID2, inFraction, inContactPointA, inContactPointB, contact_normal); +} + +// This is a simplified version of the ray cylinder test from: Real Time Collision Detection - Christer Ericson +// Chapter 5.3.7, page 194-197. Some conditions have been removed as we're not interested in hitting the caps of the cylinder. +// Note that the ray origin is assumed to be the origin here. +float CastSphereVsTriangles::RayCylinder(Vec3Arg inRayDirection, Vec3Arg inCylinderA, Vec3Arg inCylinderB, float inRadius) const +{ + // Calculate cylinder axis + Vec3 axis = inCylinderB - inCylinderA; + + // Make ray start relative to cylinder side A (moving cylinder A to the origin) + Vec3 start = -inCylinderA; + + // Test if segment is fully on the A side of the cylinder + float start_dot_axis = start.Dot(axis); + float direction_dot_axis = inRayDirection.Dot(axis); + float end_dot_axis = start_dot_axis + direction_dot_axis; + if (start_dot_axis < 0.0f && end_dot_axis < 0.0f) + return FLT_MAX; + + // Test if segment is fully on the B side of the cylinder + float axis_len_sq = axis.LengthSq(); + if (start_dot_axis > axis_len_sq && end_dot_axis > axis_len_sq) + return FLT_MAX; + + // Calculate a, b and c, the factors for quadratic equation + // We're basically solving the ray: x = start + direction * t + // The closest point to x on the segment A B is: w = (x . axis) * axis / (axis . axis) + // The distance between x and w should be radius: (x - w) . (x - w) = radius^2 + // Solving this gives the following: + float a = axis_len_sq * inRayDirection.LengthSq() - Square(direction_dot_axis); + if (abs(a) < 1.0e-6f) + return FLT_MAX; // Segment runs parallel to cylinder axis, stop processing, we will either hit at fraction = 0 or we'll hit a vertex + float b = axis_len_sq * start.Dot(inRayDirection) - direction_dot_axis * start_dot_axis; // should be multiplied by 2, instead we'll divide a and c by 2 when we solve the quadratic equation + float c = axis_len_sq * (start.LengthSq() - Square(inRadius)) - Square(start_dot_axis); + float det = Square(b) - a * c; // normally 4 * a * c but since both a and c need to be divided by 2 we lose the 4 + if (det < 0.0f) + return FLT_MAX; // No solution to quadractic equation + + // Solve fraction t where the ray hits the cylinder + float t = -(b + sqrt(det)) / a; // normally divided by 2 * a but since a should be divided by 2 we lose the 2 + if (t < 0.0f || t > 1.0f) + return FLT_MAX; // Intersection lies outside segment + if (start_dot_axis + t * direction_dot_axis < 0.0f || start_dot_axis + t * direction_dot_axis > axis_len_sq) + return FLT_MAX; // Intersection outside the end point of the cylinder, stop processing, we will possibly hit a vertex + return t; +} + +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; + Vec3 v2 = mScale * inV2 - mStart; + + // Calculate triangle normal + Vec3 triangle_normal = mScaleSign * (v1 - v0).Cross(v2 - v0); + float triangle_normal_len = triangle_normal.Length(); + if (triangle_normal_len == 0.0f) + return; // Degenerate triangle + triangle_normal /= triangle_normal_len; + + // Backface check + float normal_dot_direction = triangle_normal.Dot(mDirection); + bool back_facing = normal_dot_direction > 0.0f; + if (mShapeCastSettings.mBackFaceModeTriangles == EBackFaceMode::IgnoreBackFaces && back_facing) + return; + + // Test if distance between the sphere and plane of triangle is smaller or equal than the radius + if (abs(v0.Dot(triangle_normal)) <= mRadius) + { + // Check if the sphere intersects at the start of the cast + uint32 closest_feature; + Vec3 q = ClosestPoint::GetClosestPointOnTriangle(v0, v1, v2, closest_feature); + float q_len_sq = q.LengthSq(); + if (q_len_sq <= Square(mRadius)) + { + // Early out if this hit is deeper than the collector's early out value + float q_len = sqrt(q_len_sq); + float penetration_depth = mRadius - q_len; + if (-penetration_depth >= mCollector.GetEarlyOutFraction()) + return; + + // Generate contact point + Vec3 contact_normal = q_len > 0.0f? q / q_len : Vec3::sAxisY(); + Vec3 contact_point_a = q + contact_normal * penetration_depth; + Vec3 contact_point_b = q; + AddHitWithActiveEdgeDetection(v0, v1, v2, back_facing, triangle_normal, inActiveEdges, inSubShapeID2, 0.0f, contact_point_a, contact_point_b, contact_normal); + return; + } + } + else + { + // Check if cast is not parallel to the plane of the triangle + float abs_normal_dot_direction = abs(normal_dot_direction); + if (abs_normal_dot_direction > 1.0e-6f) + { + // Calculate the point on the sphere that will hit the triangle's plane first and calculate a fraction where it will do so + Vec3 d = Sign(normal_dot_direction) * mRadius * triangle_normal; + float plane_intersection = (v0 - d).Dot(triangle_normal) / normal_dot_direction; + + // Check if sphere will hit in the interval that we're interested in + if (plane_intersection * abs_normal_dot_direction < -mRadius // Sphere hits the plane before the sweep, cannot intersect + || plane_intersection >= mCollector.GetEarlyOutFraction()) // Sphere hits the plane after the sweep / early out fraction, cannot intersect + return; + + // We can only report an interior hit if we're hitting the plane during our sweep and not before + if (plane_intersection >= 0.0f) + { + // Calculate the point of contact on the plane + Vec3 p = d + plane_intersection * mDirection; + + // Check if this is an interior point + float u, v, w; + if (ClosestPoint::GetBaryCentricCoordinates(v0 - p, v1 - p, v2 - p, u, v, w) + && u >= 0.0f && v >= 0.0f && w >= 0.0f) + { + // Interior point, we found the collision point. We don't need to check active edges. + AddHit(back_facing, inSubShapeID2, plane_intersection, p, p, back_facing? triangle_normal : -triangle_normal); + return; + } + } + } + } + + // Test 3 edges + float fraction = RayCylinder(mDirection, v0, v1, mRadius); + fraction = min(fraction, RayCylinder(mDirection, v1, v2, mRadius)); + fraction = min(fraction, RayCylinder(mDirection, v2, v0, mRadius)); + + // Test 3 vertices + fraction = min(fraction, RaySphere(Vec3::sZero(), mDirection, v0, mRadius)); + fraction = min(fraction, RaySphere(Vec3::sZero(), mDirection, v1, mRadius)); + fraction = min(fraction, RaySphere(Vec3::sZero(), mDirection, v2, mRadius)); + + // Check if we have a collision + JPH_ASSERT(fraction >= 0.0f); + if (fraction < mCollector.GetEarlyOutFraction()) + { + // Calculate the center of the sphere at the point of contact + Vec3 p = fraction * mDirection; + + // Get contact point and normal + uint32 closest_feature; + Vec3 q = ClosestPoint::GetClosestPointOnTriangle(v0 - p, v1 - p, v2 - p, closest_feature); + Vec3 contact_normal = q.Normalized(); + Vec3 contact_point_ab = p + q; + AddHitWithActiveEdgeDetection(v0, v1, v2, back_facing, triangle_normal, inActiveEdges, inSubShapeID2, fraction, contact_point_ab, contact_point_ab, contact_normal); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.h new file mode 100644 index 000000000000..e37bf6829317 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.h @@ -0,0 +1,49 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Collision detection helper that casts a sphere vs one or more triangles +class JPH_EXPORT CastSphereVsTriangles +{ +public: + /// Constructor + /// @param inShapeCast The sphere to cast against the triangles and its start and direction + /// @param inShapeCastSettings Settings for performing the cast + /// @param inScale Local space scale for the shape to cast against (scales relative to its center of mass). + /// @param inCenterOfMassTransform2 Is the center of mass transform of shape 2 (excluding scale), this is used to provide a transform to the shape cast result so that local quantities can be transformed into world space. + /// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for the casting shape + /// @param ioCollector The collector that receives the results. + CastSphereVsTriangles(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, CastShapeCollector &ioCollector); + + /// Cast sphere with a single triangle + /// @param inV0 , inV1 , inV2: CCW triangle vertices + /// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active + /// An active edge is an edge that is not connected to another triangle in such a way that it is impossible to collide with the edge + /// @param inSubShapeID2 The sub shape ID for the triangle + void Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2); + +protected: + Vec3 mStart; ///< Starting location of the sphere + Vec3 mDirection; ///< Direction and length of movement of sphere + float mRadius; ///< Scaled radius of sphere + const ShapeCastSettings & mShapeCastSettings; + const Mat44 & mCenterOfMassTransform2; + Vec3 mScale; + SubShapeIDCreator mSubShapeIDCreator1; + CastShapeCollector & mCollector; + +private: + void AddHit(bool inBackFacing, const SubShapeID &inSubShapeID2, float inFraction, Vec3Arg inContactPointA, Vec3Arg inContactPointB, Vec3Arg inContactNormal); + void AddHitWithActiveEdgeDetection(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, bool inBackFacing, Vec3Arg inTriangleNormal, uint8 inActiveEdges, const SubShapeID &inSubShapeID2, float inFraction, Vec3Arg inContactPointA, Vec3Arg inContactPointB, Vec3Arg inContactNormal); + float RayCylinder(Vec3Arg inRayDirection, Vec3Arg inCylinderA, Vec3Arg inCylinderB, float inRadius) const; + + float mScaleSign; ///< Sign of the scale, -1 if object is inside out, 1 if not +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollectFacesMode.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollectFacesMode.h new file mode 100644 index 000000000000..4cac6256db1d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollectFacesMode.h @@ -0,0 +1,16 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Whether or not to collect faces, used by CastShape and CollideShape +enum class ECollectFacesMode : uint8 +{ + CollectFaces, ///< mShape1/2Face is desired + NoFaces ///< mShape1/2Face is not desired +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp new file mode 100644 index 000000000000..f00b0023aa94 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp @@ -0,0 +1,150 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +CollideConvexVsTriangles::CollideConvexVsTriangles(const ConvexShape *inShape1, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeID &inSubShapeID1, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector) : + mCollideShapeSettings(inCollideShapeSettings), + mCollector(ioCollector), + mShape1(inShape1), + mScale1(inScale1), + mScale2(inScale2), + mTransform1(inCenterOfMassTransform1), + mSubShapeID1(inSubShapeID1) +{ + // Get transforms + Mat44 inverse_transform2 = inCenterOfMassTransform2.InversedRotationTranslation(); + Mat44 transform1_to_2 = inverse_transform2 * inCenterOfMassTransform1; + mTransform2To1 = transform1_to_2.InversedRotationTranslation(); + + // Calculate bounds + mBoundsOf1 = inShape1->GetLocalBounds().Scaled(inScale1); + mBoundsOf1.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance)); + mBoundsOf1InSpaceOf2 = mBoundsOf1.Transformed(transform1_to_2); // Convert bounding box of 1 into space of 2 + + // Determine if shape 2 is inside out or not + mScaleSign2 = ScaleHelpers::IsInsideOut(inScale2)? -1.0f : 1.0f; +} + +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); + Vec3 v2 = mTransform2To1 * (mScale2 * inV2); + + // Calculate triangle normal + Vec3 triangle_normal = mScaleSign2 * (v1 - v0).Cross(v2 - v0); + + // Backface check + bool back_facing = triangle_normal.Dot(v0) > 0.0f; + if (mCollideShapeSettings.mBackFaceMode == EBackFaceMode::IgnoreBackFaces && back_facing) + return; + + // Get bounding box for triangle + AABox triangle_bbox = AABox::sFromTwoPoints(v0, v1); + triangle_bbox.Encapsulate(v2); + + // Get intersection between triangle and shape box, if there is none, we're done + if (!triangle_bbox.Overlaps(mBoundsOf1)) + return; + + // Create triangle support function + TriangleConvexSupport triangle(v0, v1, v2); + + // Perform collision detection + // Note: As we don't remember the penetration axis from the last iteration, and it is likely that the shape (A) we're colliding the triangle (B) against is in front of the triangle, + // and the penetration axis is the shortest distance along to push B out of collision, we use the inverse of the triangle normal as an initial penetration axis. This has been seen + // to improve performance by approx. 5% over using a fixed axis like (1, 0, 0). + Vec3 penetration_axis = -triangle_normal, point1, point2; + EPAPenetrationDepth pen_depth; + EPAPenetrationDepth::EStatus status; + + // Get the support function + if (mShape1ExCvxRadius == nullptr) + mShape1ExCvxRadius = mShape1->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, mBufferExCvxRadius, mScale1); + + // Perform GJK step + status = pen_depth.GetPenetrationDepthStepGJK(*mShape1ExCvxRadius, mShape1ExCvxRadius->GetConvexRadius() + mCollideShapeSettings.mMaxSeparationDistance, triangle, 0.0f, mCollideShapeSettings.mCollisionTolerance, penetration_axis, point1, point2); + + // Check result of collision detection + if (status == EPAPenetrationDepth::EStatus::NotColliding) + return; + else if (status == EPAPenetrationDepth::EStatus::Indeterminate) + { + // Need to run expensive EPA algorithm + + // Get the support function + if (mShape1IncCvxRadius == nullptr) + mShape1IncCvxRadius = mShape1->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, mBufferIncCvxRadius, mScale1); + + // Add convex radius + AddConvexRadius shape1_add_max_separation_distance(*mShape1IncCvxRadius, mCollideShapeSettings.mMaxSeparationDistance); + + // Perform EPA step + if (!pen_depth.GetPenetrationDepthStepEPA(shape1_add_max_separation_distance, triangle, mCollideShapeSettings.mPenetrationTolerance, penetration_axis, point1, point2)) + return; + } + + // Check if the penetration is bigger than the early out fraction + float penetration_depth = (point2 - point1).Length() - mCollideShapeSettings.mMaxSeparationDistance; + if (-penetration_depth >= mCollector.GetEarlyOutFraction()) + return; + + // Correct point1 for the added separation distance + float penetration_axis_len = penetration_axis.Length(); + if (penetration_axis_len > 0.0f) + point1 -= penetration_axis * (mCollideShapeSettings.mMaxSeparationDistance / penetration_axis_len); + + // Check if we have enabled active edge detection + if (mCollideShapeSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive && inActiveEdges != 0b111) + { + // Convert the active edge velocity hint to local space + Vec3 active_edge_movement_direction = mTransform1.Multiply3x3Transposed(mCollideShapeSettings.mActiveEdgeMovementDirection); + + // Update the penetration axis to account for active edges + // Note that we flip the triangle normal as the penetration axis is pointing towards the triangle instead of away + penetration_axis = ActiveEdges::FixNormal(v0, v1, v2, back_facing? triangle_normal : -triangle_normal, inActiveEdges, point2, penetration_axis, active_edge_movement_direction); + } + + // Convert to world space + point1 = mTransform1 * point1; + point2 = mTransform1 * point2; + Vec3 penetration_axis_world = mTransform1.Multiply3x3(penetration_axis); + + // Create collision result + CollideShapeResult result(point1, point2, penetration_axis_world, penetration_depth, mSubShapeID1, inSubShapeID2, TransformedShape::sGetBodyID(mCollector.GetContext())); + + // Gather faces + if (mCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces) + { + // Get supporting face of shape 1 + mShape1->GetSupportingFace(SubShapeID(), -penetration_axis, mScale1, mTransform1, result.mShape1Face); + + // Get face of the triangle + result.mShape2Face.resize(3); + result.mShape2Face[0] = mTransform1 * v0; + result.mShape2Face[1] = mTransform1 * v1; + result.mShape2Face[2] = mTransform1 * v2; + } + + // Notify the collector + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) + mCollector.AddHit(result); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.h new file mode 100644 index 000000000000..8adfa3b41080 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.h @@ -0,0 +1,56 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; + +/// Collision detection helper that collides a convex object vs one or more triangles +class JPH_EXPORT CollideConvexVsTriangles +{ +public: + /// Constructor + /// @param inShape1 The convex shape to collide against triangles + /// @param inScale1 Local space scale for the convex object (scales relative to its center of mass) + /// @param inScale2 Local space scale for the triangles + /// @param inCenterOfMassTransform1 Transform that takes the center of mass of 1 into world space + /// @param inCenterOfMassTransform2 Transform that takes the center of mass of 2 into world space + /// @param inSubShapeID1 Sub shape ID of the convex object + /// @param inCollideShapeSettings Settings for the collide shape query + /// @param ioCollector The collector that will receive the results + CollideConvexVsTriangles(const ConvexShape *inShape1, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeID &inSubShapeID1, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector); + + /// Collide convex object with a single triangle + /// @param inV0 , inV1 , inV2: CCW triangle vertices + /// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active + /// An active edge is an edge that is not connected to another triangle in such a way that it is impossible to collide with the edge + /// @param inSubShapeID2 The sub shape ID for the triangle + void Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2); + +protected: + const CollideShapeSettings & mCollideShapeSettings; ///< Settings for this collision operation + CollideShapeCollector & mCollector; ///< The collector that will receive the results + const ConvexShape * mShape1; ///< The shape that we're colliding with + Vec3 mScale1; ///< The scale of the shape (in shape local space) of the shape we're colliding with + Vec3 mScale2; ///< The scale of the shape (in shape local space) of the shape we're colliding against + Mat44 mTransform1; ///< Transform of the shape we're colliding with + Mat44 mTransform2To1; ///< Transform that takes a point in space of the colliding shape to the shape we're colliding with + AABox mBoundsOf1; ///< Bounds of the colliding shape in local space + AABox mBoundsOf1InSpaceOf2; ///< Bounds of the colliding shape in space of shape we're colliding with + SubShapeID mSubShapeID1; ///< Sub shape ID of colliding shape + float mScaleSign2; ///< Sign of the scale of object 2, -1 if object is inside out, 1 if not + ConvexShape::SupportBuffer mBufferExCvxRadius; ///< Buffer that holds the support function data excluding convex radius + ConvexShape::SupportBuffer mBufferIncCvxRadius; ///< Buffer that holds the support function data including convex radius + const ConvexShape::Support * mShape1ExCvxRadius = nullptr; ///< Actual support function object excluding convex radius + const ConvexShape::Support * mShape1IncCvxRadius = nullptr; ///< Actual support function object including convex radius +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollidePointResult.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollidePointResult.h new file mode 100644 index 000000000000..8601b3c40a03 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollidePointResult.h @@ -0,0 +1,25 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Structure that holds the result of colliding a point against a shape +class CollidePointResult +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Function required by the CollisionCollector. A smaller fraction is considered to be a 'better hit'. For point queries there is no sensible return value. + inline float GetEarlyOutFraction() const { return 0.0f; } + + BodyID mBodyID; ///< Body that was hit + SubShapeID mSubShapeID2; ///< Sub shape ID of shape that we collided against +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideShape.h new file mode 100644 index 000000000000..2c5f8f217ffa --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideShape.h @@ -0,0 +1,105 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that contains all information of two colliding shapes +class CollideShapeResult +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Default constructor + CollideShapeResult() = default; + + /// Constructor + CollideShapeResult(Vec3Arg inContactPointOn1, Vec3Arg inContactPointOn2, Vec3Arg inPenetrationAxis, float inPenetrationDepth, const SubShapeID &inSubShapeID1, const SubShapeID &inSubShapeID2, const BodyID &inBodyID2) : + mContactPointOn1(inContactPointOn1), + mContactPointOn2(inContactPointOn2), + mPenetrationAxis(inPenetrationAxis), + mPenetrationDepth(inPenetrationDepth), + mSubShapeID1(inSubShapeID1), + mSubShapeID2(inSubShapeID2), + mBodyID2(inBodyID2) + { + } + + /// Function required by the CollisionCollector. A smaller fraction is considered to be a 'better hit'. We use -penetration depth to get the hit with the biggest penetration depth + inline float GetEarlyOutFraction() const { return -mPenetrationDepth; } + + /// Reverses the hit result, swapping contact point 1 with contact point 2 etc. + inline CollideShapeResult Reversed() const + { + CollideShapeResult result; + result.mContactPointOn2 = mContactPointOn1; + result.mContactPointOn1 = mContactPointOn2; + result.mPenetrationAxis = -mPenetrationAxis; + result.mPenetrationDepth = mPenetrationDepth; + result.mSubShapeID2 = mSubShapeID1; + result.mSubShapeID1 = mSubShapeID2; + result.mBodyID2 = mBodyID2; + result.mShape2Face = mShape1Face; + result.mShape1Face = mShape2Face; + return result; + } + + using Face = StaticArray; + + Vec3 mContactPointOn1; ///< Contact point on the surface of shape 1 (in world space or relative to base offset) + Vec3 mContactPointOn2; ///< Contact point on the surface of shape 2 (in world space or relative to base offset). If the penetration depth is 0, this will be the same as mContactPointOn1. + Vec3 mPenetrationAxis; ///< Direction to move shape 2 out of collision along the shortest path (magnitude is meaningless, in world space). You can use -mPenetrationAxis.Normalized() as contact normal. + float mPenetrationDepth; ///< Penetration depth (move shape 2 by this distance to resolve the collision) + SubShapeID mSubShapeID1; ///< Sub shape ID that identifies the face on shape 1 + SubShapeID mSubShapeID2; ///< Sub shape ID that identifies the face on shape 2 + BodyID mBodyID2; ///< BodyID to which shape 2 belongs to + Face mShape1Face; ///< Colliding face on shape 1 (optional result, in world space or relative to base offset) + Face mShape2Face; ///< Colliding face on shape 2 (optional result, in world space or relative to base offset) +}; + +/// Settings to be passed with a collision query +class CollideSettingsBase +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// How active edges (edges that a moving object should bump into) are handled + EActiveEdgeMode mActiveEdgeMode = EActiveEdgeMode::CollideOnlyWithActive; + + /// If colliding faces should be collected or only the collision point + ECollectFacesMode mCollectFacesMode = ECollectFacesMode::NoFaces; + + /// If objects are closer than this distance, they are considered to be colliding (used for GJK) (unit: meter) + float mCollisionTolerance = cDefaultCollisionTolerance; + + /// A factor that determines the accuracy of the penetration depth calculation. If the change of the squared distance is less than tolerance * current_penetration_depth^2 the algorithm will terminate. (unit: dimensionless) + float mPenetrationTolerance = cDefaultPenetrationTolerance; + + /// When mActiveEdgeMode is CollideOnlyWithActive a movement direction can be provided. When hitting an inactive edge, the system will select the triangle normal as penetration depth only if it impedes the movement less than with the calculated penetration depth. + Vec3 mActiveEdgeMovementDirection = Vec3::sZero(); +}; + +/// Settings to be passed with a collision query +class CollideShapeSettings : public CollideSettingsBase +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// When > 0 contacts in the vicinity of the query shape can be found. All nearest contacts that are not further away than this distance will be found (unit: meter) + float mMaxSeparationDistance = 0.0f; + + /// How backfacing triangles should be treated + EBackFaceMode mBackFaceMode = EBackFaceMode::IgnoreBackFaces; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h new file mode 100644 index 000000000000..e976aa9de225 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h @@ -0,0 +1,110 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that allows iterating over the vertices of a soft body. +/// It tracks the largest penetration and allows storing the resulting collision in a different structure than the soft body vertex itself. +class CollideSoftBodyVertexIterator +{ +public: + /// Default constructor + CollideSoftBodyVertexIterator() = default; + CollideSoftBodyVertexIterator(const CollideSoftBodyVertexIterator &) = default; + + /// Construct using (strided) pointers + CollideSoftBodyVertexIterator(const StridedPtr &inPosition, const StridedPtr &inInvMass, const StridedPtr &inCollisionPlane, const StridedPtr &inLargestPenetration, const StridedPtr &inCollidingShapeIndex) : + mPosition(inPosition), + mInvMass(inInvMass), + mCollisionPlane(inCollisionPlane), + mLargestPenetration(inLargestPenetration), + mCollidingShapeIndex(inCollidingShapeIndex) + { + } + + /// Construct using a soft body vertex + explicit CollideSoftBodyVertexIterator(SoftBodyVertex *inVertices) : + mPosition(&inVertices->mPosition, sizeof(SoftBodyVertex)), + mInvMass(&inVertices->mInvMass, sizeof(SoftBodyVertex)), + mCollisionPlane(&inVertices->mCollisionPlane, sizeof(SoftBodyVertex)), + mLargestPenetration(&inVertices->mLargestPenetration, sizeof(SoftBodyVertex)), + mCollidingShapeIndex(&inVertices->mCollidingShapeIndex, sizeof(SoftBodyVertex)) + { + } + + /// Default assignment + CollideSoftBodyVertexIterator & operator = (const CollideSoftBodyVertexIterator &) = default; + + /// Equality operator. + /// Note: Only used to determine end iterator, so we only compare position. + bool operator != (const CollideSoftBodyVertexIterator &inRHS) const + { + return mPosition != inRHS.mPosition; + } + + /// Next vertex + CollideSoftBodyVertexIterator & operator ++ () + { + ++mPosition; + ++mInvMass; + ++mCollisionPlane; + ++mLargestPenetration; + ++mCollidingShapeIndex; + return *this; + } + + /// Add an offset + /// Note: Only used to determine end iterator, so we only set position. + CollideSoftBodyVertexIterator operator + (int inOffset) const + { + return CollideSoftBodyVertexIterator(mPosition + inOffset, StridedPtr(), StridedPtr(), StridedPtr(), StridedPtr()); + } + + /// Get the position of the current vertex + Vec3 GetPosition() const + { + return *mPosition; + } + + /// Get the inverse mass of the current vertex + float GetInvMass() const + { + return *mInvMass; + } + + /// Update penetration of the current vertex + /// @return Returns true if the vertex has the largest penetration so far, this means you need to follow up by calling SetCollision + bool UpdatePenetration(float inLargestPenetration) const + { + float &penetration = *mLargestPenetration; + if (penetration >= inLargestPenetration) + return false; + penetration = inLargestPenetration; + return true; + } + + /// Update the collision of the current vertex + void SetCollision(const Plane &inCollisionPlane, int inCollidingShapeIndex) const + { + *mCollisionPlane = inCollisionPlane; + *mCollidingShapeIndex = inCollidingShapeIndex; + } + +private: + /// Input data + StridedPtr mPosition; + StridedPtr mInvMass; + + /// Output data + StridedPtr mCollisionPlane; + StridedPtr mLargestPenetration; + StridedPtr mCollidingShapeIndex; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h new file mode 100644 index 000000000000..c91becb64259 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h @@ -0,0 +1,90 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Collision detection helper that collides soft body vertices vs triangles +class JPH_EXPORT CollideSoftBodyVerticesVsTriangles +{ +public: + CollideSoftBodyVerticesVsTriangles(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) : + mTransform(inCenterOfMassTransform * Mat44::sScale(inScale)), + mInvTransform(mTransform.Inversed()), + mNormalSign(ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f) + { + } + + JPH_INLINE void StartVertex(const CollideSoftBodyVertexIterator &inVertex) + { + mLocalPosition = mInvTransform * inVertex.GetPosition(); + mClosestDistanceSq = FLT_MAX; + } + + JPH_INLINE void ProcessTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + // Get the closest point from the vertex to the triangle + uint32 set; + Vec3 closest_point = ClosestPoint::GetClosestPointOnTriangle(inV0 - mLocalPosition, inV1 - mLocalPosition, inV2 - mLocalPosition, set); + float dist_sq = closest_point.LengthSq(); + if (dist_sq < mClosestDistanceSq) + { + mV0 = inV0; + mV1 = inV1; + mV2 = inV2; + mClosestPoint = closest_point; + mClosestDistanceSq = dist_sq; + mSet = set; + } + } + + JPH_INLINE void FinishVertex(const CollideSoftBodyVertexIterator &ioVertex, int inCollidingShapeIndex) const + { + if (mClosestDistanceSq < FLT_MAX) + { + // Convert triangle to world space + Vec3 v0 = mTransform * mV0; + Vec3 v1 = mTransform * mV1; + Vec3 v2 = mTransform * mV2; + Vec3 triangle_normal = mNormalSign * (v1 - v0).Cross(v2 - v0).NormalizedOr(Vec3::sAxisY()); + + if (mSet == 0b111) + { + // Closest is interior to the triangle, use plane as collision plane but don't allow more than 0.1 m penetration + // because otherwise a triangle half a level a way will have a huge penetration if it is back facing + float penetration = min(triangle_normal.Dot(v0 - ioVertex.GetPosition()), 0.1f); + if (ioVertex.UpdatePenetration(penetration)) + ioVertex.SetCollision(Plane::sFromPointAndNormal(v0, triangle_normal), inCollidingShapeIndex); + } + else + { + // Closest point is on an edge or vertex, use closest point as collision plane + Vec3 closest_point = mTransform * (mLocalPosition + mClosestPoint); + Vec3 normal = ioVertex.GetPosition() - closest_point; + if (normal.Dot(triangle_normal) > 0.0f) // Ignore back facing edges + { + float normal_length = normal.Length(); + float penetration = -normal_length; + if (ioVertex.UpdatePenetration(penetration)) + ioVertex.SetCollision(Plane::sFromPointAndNormal(closest_point, normal_length > 0.0f? normal / normal_length : triangle_normal), inCollidingShapeIndex); + } + } + } + } + + Mat44 mTransform; + Mat44 mInvTransform; + Vec3 mLocalPosition; + Vec3 mV0, mV1, mV2; + Vec3 mClosestPoint; + float mNormalSign; + float mClosestDistanceSq; + uint32 mSet; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp new file mode 100644 index 000000000000..92ed28a7b495 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp @@ -0,0 +1,123 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +static constexpr uint8 sClosestFeatureToActiveEdgesMask[] = { + 0b000, // 0b000: Invalid, guarded by an assert + 0b101, // 0b001: Vertex 1 -> edge 1 or 3 + 0b011, // 0b010: Vertex 2 -> edge 1 or 2 + 0b001, // 0b011: Vertex 1 & 2 -> edge 1 + 0b110, // 0b100: Vertex 3 -> edge 2 or 3 + 0b100, // 0b101: Vertex 1 & 3 -> edge 3 + 0b010, // 0b110: Vertex 2 & 3 -> edge 2 + // 0b111: Vertex 1, 2 & 3 -> interior, guarded by an if +}; + +CollideSphereVsTriangles::CollideSphereVsTriangles(const SphereShape *inShape1, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeID &inSubShapeID1, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector) : + mCollideShapeSettings(inCollideShapeSettings), + mCollector(ioCollector), + mShape1(inShape1), + mScale2(inScale2), + mTransform2(inCenterOfMassTransform2), + mSubShapeID1(inSubShapeID1) +{ + // Calculate the center of the sphere in the space of 2 + mSphereCenterIn2 = inCenterOfMassTransform2.Multiply3x3Transposed(inCenterOfMassTransform1.GetTranslation() - inCenterOfMassTransform2.GetTranslation()); + + // Determine if shape 2 is inside out or not + mScaleSign2 = ScaleHelpers::IsInsideOut(inScale2)? -1.0f : 1.0f; + + // Check that the sphere is uniformly scaled + JPH_ASSERT(ScaleHelpers::IsUniformScale(inScale1.Abs())); + mRadius = abs(inScale1.GetX()) * inShape1->GetRadius(); + mRadiusPlusMaxSeparationSq = Square(mRadius + inCollideShapeSettings.mMaxSeparationDistance); +} + +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; + Vec3 v2 = mScale2 * inV2 - mSphereCenterIn2; + + // Calculate triangle normal + Vec3 triangle_normal = mScaleSign2 * (v1 - v0).Cross(v2 - v0); + + // Backface check + bool back_facing = triangle_normal.Dot(v0) > 0.0f; + if (mCollideShapeSettings.mBackFaceMode == EBackFaceMode::IgnoreBackFaces && back_facing) + return; + + // Check if we collide with the sphere + uint32 closest_feature; + Vec3 point2 = ClosestPoint::GetClosestPointOnTriangle(v0, v1, v2, closest_feature); + float point2_len_sq = point2.LengthSq(); + if (point2_len_sq > mRadiusPlusMaxSeparationSq) + return; + + // Calculate penetration depth + float penetration_depth = mRadius - sqrt(point2_len_sq); + if (-penetration_depth >= mCollector.GetEarlyOutFraction()) + return; + + // Calculate penetration axis, direction along which to push 2 to move it out of collision (this is always away from the sphere center) + Vec3 penetration_axis = point2.NormalizedOr(Vec3::sAxisY()); + + // Calculate the point on the sphere + Vec3 point1 = mRadius * penetration_axis; + + // Check if we have enabled active edge detection + JPH_ASSERT(closest_feature != 0); + if (mCollideShapeSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive + && closest_feature != 0b111 // For an interior hit we should already have the right normal + && (inActiveEdges & sClosestFeatureToActiveEdgesMask[closest_feature]) == 0) // If we didn't hit an active edge we should take the triangle normal + { + // Convert the active edge velocity hint to local space + Vec3 active_edge_movement_direction = mTransform2.Multiply3x3Transposed(mCollideShapeSettings.mActiveEdgeMovementDirection); + + // See ActiveEdges::FixNormal. If penetration_axis affects the movement less than the triangle normal we keep penetration_axis. + Vec3 new_penetration_axis = back_facing? triangle_normal : -triangle_normal; + if (active_edge_movement_direction.Dot(penetration_axis) * new_penetration_axis.Length() >= active_edge_movement_direction.Dot(new_penetration_axis)) + penetration_axis = new_penetration_axis; + } + + // Convert to world space + point1 = mTransform2 * (mSphereCenterIn2 + point1); + point2 = mTransform2 * (mSphereCenterIn2 + point2); + Vec3 penetration_axis_world = mTransform2.Multiply3x3(penetration_axis); + + // Create collision result + CollideShapeResult result(point1, point2, penetration_axis_world, penetration_depth, mSubShapeID1, inSubShapeID2, TransformedShape::sGetBodyID(mCollector.GetContext())); + + // Gather faces + if (mCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces) + { + // The sphere doesn't have a supporting face + + // Get face of triangle 2 + result.mShape2Face.resize(3); + result.mShape2Face[0] = mTransform2 * (mSphereCenterIn2 + v0); + result.mShape2Face[1] = mTransform2 * (mSphereCenterIn2 + v1); + result.mShape2Face[2] = mTransform2 * (mSphereCenterIn2 + v2); + } + + // Notify the collector + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) + mCollector.AddHit(result); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.h new file mode 100644 index 000000000000..5d16d9a884fc --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.h @@ -0,0 +1,50 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; + +/// Collision detection helper that collides a sphere vs one or more triangles +class JPH_EXPORT CollideSphereVsTriangles +{ +public: + /// Constructor + /// @param inShape1 The sphere to collide against triangles + /// @param inScale1 Local space scale for the sphere (scales relative to its center of mass) + /// @param inScale2 Local space scale for the triangles + /// @param inCenterOfMassTransform1 Transform that takes the center of mass of 1 into world space + /// @param inCenterOfMassTransform2 Transform that takes the center of mass of 2 into world space + /// @param inSubShapeID1 Sub shape ID of the convex object + /// @param inCollideShapeSettings Settings for the collide shape query + /// @param ioCollector The collector that will receive the results + CollideSphereVsTriangles(const SphereShape *inShape1, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeID &inSubShapeID1, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector); + + /// Collide sphere with a single triangle + /// @param inV0 , inV1 , inV2: CCW triangle vertices + /// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active + /// An active edge is an edge that is not connected to another triangle in such a way that it is impossible to collide with the edge + /// @param inSubShapeID2 The sub shape ID for the triangle + void Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2); + +protected: + const CollideShapeSettings & mCollideShapeSettings; ///< Settings for this collision operation + CollideShapeCollector & mCollector; ///< The collector that will receive the results + const SphereShape * mShape1; ///< The shape that we're colliding with + Vec3 mScale2; ///< The scale of the shape (in shape local space) of the shape we're colliding against + Mat44 mTransform2; ///< Transform of the shape we're colliding against + Vec3 mSphereCenterIn2; ///< The center of the sphere in the space of 2 + SubShapeID mSubShapeID1; ///< Sub shape ID of colliding shape + float mScaleSign2; ///< Sign of the scale of object 2, -1 if object is inside out, 1 if not + float mRadius; ///< Radius of the sphere + float mRadiusPlusMaxSeparationSq; ///< (Radius + Max SeparationDistance)^2 +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionCollector.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionCollector.h new file mode 100644 index 000000000000..ec608502719d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionCollector.h @@ -0,0 +1,105 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +class Body; +class TransformedShape; + +/// Traits to use for CastRay +class CollisionCollectorTraitsCastRay +{ +public: + /// For rays the early out fraction is the fraction along the line to order hits. + static constexpr float InitialEarlyOutFraction = 1.0f + FLT_EPSILON; ///< Furthest hit: Fraction is 1 + epsilon + static constexpr float ShouldEarlyOutFraction = 0.0f; ///< Closest hit: Fraction is 0 +}; + +/// Traits to use for CastShape +class CollisionCollectorTraitsCastShape +{ +public: + /// For rays the early out fraction is the fraction along the line to order hits. + static constexpr float InitialEarlyOutFraction = 1.0f + FLT_EPSILON; ///< Furthest hit: Fraction is 1 + epsilon + static constexpr float ShouldEarlyOutFraction = -FLT_MAX; ///< Deepest hit: Penetration is infinite +}; + +/// Traits to use for CollideShape +class CollisionCollectorTraitsCollideShape +{ +public: + /// For shape collisions we use -penetration depth to order hits. + static constexpr float InitialEarlyOutFraction = FLT_MAX; ///< Most shallow hit: Separation is infinite + static constexpr float ShouldEarlyOutFraction = -FLT_MAX; ///< Deepest hit: Penetration is infinite +}; + +/// Traits to use for CollidePoint +using CollisionCollectorTraitsCollidePoint = CollisionCollectorTraitsCollideShape; + +/// Virtual interface that allows collecting multiple collision results +template +class CollisionCollector +{ +public: + /// Declare ResultType so that derived classes can use it + using ResultType = ResultTypeArg; + + /// Default constructor + CollisionCollector() = default; + + /// Constructor to initialize from another collector + template + explicit CollisionCollector(const CollisionCollector &inRHS) : mEarlyOutFraction(inRHS.GetEarlyOutFraction()), mContext(inRHS.GetContext()) { } + CollisionCollector(const CollisionCollector &inRHS) = default; + + /// Destructor + virtual ~CollisionCollector() = default; + + /// If you want to reuse this collector, call Reset() + virtual void Reset() { mEarlyOutFraction = TraitsType::InitialEarlyOutFraction; } + + /// When running a query through the NarrowPhaseQuery class, this will be called for every body that is potentially colliding. + /// It allows collecting additional information needed by the collision collector implementation from the body under lock protection + /// before AddHit is called (e.g. the user data pointer or the velocity of the body). + virtual void OnBody([[maybe_unused]] const Body &inBody) { /* Collects nothing by default */ } + + /// Set by the collision detection functions to the current TransformedShape that we're colliding against before calling the AddHit function + void SetContext(const TransformedShape *inContext) { mContext = inContext; } + const TransformedShape *GetContext() const { return mContext; } + + /// This function can be used to set some user data on the collision collector + virtual void SetUserData(uint64 inUserData) { /* Does nothing by default */ } + + /// This function will be called for every hit found, it's up to the application to decide how to store the hit + virtual void AddHit(const ResultType &inResult) = 0; + + /// Update the early out fraction (should be lower than before) + inline void UpdateEarlyOutFraction(float inFraction) { JPH_ASSERT(inFraction <= mEarlyOutFraction); mEarlyOutFraction = inFraction; } + + /// Reset the early out fraction to a specific value + inline void ResetEarlyOutFraction(float inFraction = TraitsType::InitialEarlyOutFraction) { mEarlyOutFraction = inFraction; } + + /// Force the collision detection algorithm to terminate as soon as possible. Call this from the AddHit function when a satisfying hit is found. + inline void ForceEarlyOut() { mEarlyOutFraction = TraitsType::ShouldEarlyOutFraction; } + + /// When true, the collector will no longer accept any additional hits and the collision detection routine should early out as soon as possible + inline bool ShouldEarlyOut() const { return mEarlyOutFraction <= TraitsType::ShouldEarlyOutFraction; } + + /// Get the current early out value + inline float GetEarlyOutFraction() const { return mEarlyOutFraction; } + + /// Get the current early out value but make sure it's bigger than zero, this is used for shape casting as negative values are used for penetration + inline float GetPositiveEarlyOutFraction() const { return max(FLT_MIN, mEarlyOutFraction); } + +private: + /// The early out fraction determines the fraction below which the collector is still accepting a hit (can be used to reduce the amount of work) + float mEarlyOutFraction = TraitsType::InitialEarlyOutFraction; + + /// Set by the collision detection functions to the current TransformedShape of the body that we're colliding against before calling the AddHit function + const TransformedShape *mContext = nullptr; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionCollectorImpl.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionCollectorImpl.h new file mode 100644 index 000000000000..7bdd5da27972 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionCollectorImpl.h @@ -0,0 +1,134 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Simple implementation that collects all hits and optionally sorts them on distance +template +class AllHitCollisionCollector : public CollectorType +{ +public: + /// Redeclare ResultType + using ResultType = typename CollectorType::ResultType; + + // See: CollectorType::Reset + virtual void Reset() override + { + CollectorType::Reset(); + + mHits.clear(); + } + + // See: CollectorType::AddHit + virtual void AddHit(const ResultType &inResult) override + { + mHits.push_back(inResult); + } + + /// Order hits on closest first + void Sort() + { + QuickSort(mHits.begin(), mHits.end(), [](const ResultType &inLHS, const ResultType &inRHS) { return inLHS.GetEarlyOutFraction() < inRHS.GetEarlyOutFraction(); }); + } + + /// Check if any hits were collected + inline bool HadHit() const + { + return !mHits.empty(); + } + + Array mHits; +}; + +/// Simple implementation that collects the closest / deepest hit +template +class ClosestHitCollisionCollector : public CollectorType +{ +public: + /// Redeclare ResultType + using ResultType = typename CollectorType::ResultType; + + // See: CollectorType::Reset + virtual void Reset() override + { + CollectorType::Reset(); + + mHadHit = false; + } + + // See: CollectorType::AddHit + virtual void AddHit(const ResultType &inResult) override + { + float early_out = inResult.GetEarlyOutFraction(); + if (!mHadHit || early_out < mHit.GetEarlyOutFraction()) + { + // Update early out fraction + CollectorType::UpdateEarlyOutFraction(early_out); + + // Store hit + mHit = inResult; + mHadHit = true; + } + } + + /// Check if this collector has had a hit + inline bool HadHit() const + { + return mHadHit; + } + + ResultType mHit; + +private: + bool mHadHit = false; +}; + +/// Simple implementation that collects any hit +template +class AnyHitCollisionCollector : public CollectorType +{ +public: + /// Redeclare ResultType + using ResultType = typename CollectorType::ResultType; + + // See: CollectorType::Reset + virtual void Reset() override + { + CollectorType::Reset(); + + mHadHit = false; + } + + // See: CollectorType::AddHit + virtual void AddHit(const ResultType &inResult) override + { + // Test that the collector is not collecting more hits after forcing an early out + JPH_ASSERT(!mHadHit); + + // Abort any further testing + CollectorType::ForceEarlyOut(); + + // Store hit + mHit = inResult; + mHadHit = true; + } + + /// Check if this collector has had a hit + inline bool HadHit() const + { + return mHadHit; + } + + ResultType mHit; + +private: + bool mHadHit = false; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionDispatch.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionDispatch.cpp new file mode 100644 index 000000000000..259a9526e8bc --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionDispatch.cpp @@ -0,0 +1,107 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +CollisionDispatch::CollideShape CollisionDispatch::sCollideShape[NumSubShapeTypes][NumSubShapeTypes]; +CollisionDispatch::CastShape CollisionDispatch::sCastShape[NumSubShapeTypes][NumSubShapeTypes]; + +void CollisionDispatch::sInit() +{ + for (uint i = 0; i < NumSubShapeTypes; ++i) + for (uint j = 0; j < NumSubShapeTypes; ++j) + { + if (sCollideShape[i][j] == nullptr) + sCollideShape[i][j] = [](const Shape *, const Shape *, Vec3Arg, Vec3Arg, Mat44Arg, Mat44Arg, const SubShapeIDCreator &, const SubShapeIDCreator &, const CollideShapeSettings &, CollideShapeCollector &, const ShapeFilter &) + { + JPH_ASSERT(false, "Unsupported shape pair"); + }; + + if (sCastShape[i][j] == nullptr) + sCastShape[i][j] = [](const ShapeCast &, const ShapeCastSettings &, const Shape *, Vec3Arg, const ShapeFilter &, Mat44Arg, const SubShapeIDCreator &, const SubShapeIDCreator &, CastShapeCollector &) + { + JPH_ASSERT(false, "Unsupported shape pair"); + }; + } +} + +void CollisionDispatch::sReversedCollideShape(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) +{ + // A collision collector that flips the collision results + class ReversedCollector : public CollideShapeCollector + { + public: + explicit ReversedCollector(CollideShapeCollector &ioCollector) : + CollideShapeCollector(ioCollector), + mCollector(ioCollector) + { + } + + virtual void AddHit(const CollideShapeResult &inResult) override + { + // Add the reversed hit + mCollector.AddHit(inResult.Reversed()); + + // If our chained collector updated its early out fraction, we need to follow + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + + private: + CollideShapeCollector & mCollector; + }; + + ReversedShapeFilter shape_filter(inShapeFilter); + ReversedCollector collector(ioCollector); + sCollideShapeVsShape(inShape2, inShape1, inScale2, inScale1, inCenterOfMassTransform2, inCenterOfMassTransform1, inSubShapeIDCreator2, inSubShapeIDCreator1, inCollideShapeSettings, collector, shape_filter); +} + +void CollisionDispatch::sReversedCastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + // A collision collector that flips the collision results + class ReversedCollector : public CastShapeCollector + { + public: + explicit ReversedCollector(CastShapeCollector &ioCollector, Vec3Arg inWorldDirection) : + CastShapeCollector(ioCollector), + mCollector(ioCollector), + mWorldDirection(inWorldDirection) + { + } + + virtual void AddHit(const ShapeCastResult &inResult) override + { + // Add the reversed hit + mCollector.AddHit(inResult.Reversed(mWorldDirection)); + + // If our chained collector updated its early out fraction, we need to follow + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + + private: + CastShapeCollector & mCollector; + Vec3 mWorldDirection; + }; + + // Reverse the shape cast (shape cast is in local space to shape 2) + Mat44 com_start_inv = inShapeCast.mCenterOfMassStart.InversedRotationTranslation(); + ShapeCast local_shape_cast(inShape, inScale, com_start_inv, -com_start_inv.Multiply3x3(inShapeCast.mDirection)); + + // Calculate the center of mass of shape 1 at start of sweep + Mat44 shape1_com = inCenterOfMassTransform2 * inShapeCast.mCenterOfMassStart; + + // Calculate the world space direction vector of the shape cast + Vec3 world_direction = -inCenterOfMassTransform2.Multiply3x3(inShapeCast.mDirection); + + // Forward the cast + ReversedShapeFilter shape_filter(inShapeFilter); + ReversedCollector collector(ioCollector, world_direction); + sCastShapeVsShapeLocalSpace(local_shape_cast, inShapeCastSettings, inShapeCast.mShape, inShapeCast.mScale, shape_filter, shape1_com, inSubShapeIDCreator2, inSubShapeIDCreator1, collector); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionDispatch.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionDispatch.h new file mode 100644 index 000000000000..6842c8154a5c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionDispatch.h @@ -0,0 +1,97 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; + +/// Dispatch function, main function to handle collisions between shapes +class JPH_EXPORT CollisionDispatch +{ +public: + /// Collide 2 shapes and pass any collision on to ioCollector + /// @param inShape1 The first shape + /// @param inShape2 The second shape + /// @param inScale1 Local space scale of shape 1 (scales relative to its center of mass) + /// @param inScale2 Local space scale of shape 2 (scales relative to its center of mass) + /// @param inCenterOfMassTransform1 Transform to transform center of mass of shape 1 into world space + /// @param inCenterOfMassTransform2 Transform to transform center of mass of shape 2 into world space + /// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for shape 1 + /// @param inSubShapeIDCreator2 Class that tracks the current sub shape ID for shape 2 + /// @param inCollideShapeSettings Options for the CollideShape test + /// @param ioCollector The collector that receives the results. + /// @param inShapeFilter allows selectively disabling collisions between pairs of (sub) shapes. + static inline 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 = { }) + { + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseStat track(NarrowPhaseStat::sCollideShape[(int)inShape1->GetSubType()][(int)inShape2->GetSubType()]);) + + // Only test shape if it passes the shape filter + if (inShapeFilter.ShouldCollide(inShape1, inSubShapeIDCreator1.GetID(), inShape2, inSubShapeIDCreator2.GetID())) + sCollideShape[(int)inShape1->GetSubType()][(int)inShape2->GetSubType()](inShape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); + } + + /// Cast a shape against this shape, passes any hits found to ioCollector. + /// Note: This version takes the shape cast in local space relative to the center of mass of inShape, take a look at sCastShapeVsShapeWorldSpace if you have a shape cast in world space. + /// @param inShapeCastLocal The shape to cast against the other shape and its start and direction. + /// @param inShapeCastSettings Settings for performing the cast + /// @param inShape The shape to cast against. + /// @param inScale Local space scale for the shape to cast against (scales relative to its center of mass). + /// @param inShapeFilter allows selectively disabling collisions between pairs of (sub) shapes. + /// @param inCenterOfMassTransform2 Is the center of mass transform of shape 2 (excluding scale), this is used to provide a transform to the shape cast result so that local hit result quantities can be transformed into world space. + /// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for the casting shape + /// @param inSubShapeIDCreator2 Class that tracks the current sub shape ID for the shape we're casting against + /// @param ioCollector The collector that receives the results. + static inline void sCastShapeVsShapeLocalSpace(const ShapeCast &inShapeCastLocal, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) + { + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseStat track(NarrowPhaseStat::sCastShape[(int)inShapeCastLocal.mShape->GetSubType()][(int)inShape->GetSubType()]);) + + // Only test shape if it passes the shape filter + if (inShapeFilter.ShouldCollide(inShapeCastLocal.mShape, inSubShapeIDCreator1.GetID(), inShape, inSubShapeIDCreator2.GetID())) + sCastShape[(int)inShapeCastLocal.mShape->GetSubType()][(int)inShape->GetSubType()](inShapeCastLocal, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); + } + + /// See: sCastShapeVsShapeLocalSpace. + /// The only difference is that the shape cast (inShapeCastWorld) is provided in world space. + /// Note: A shape cast contains the center of mass start of the shape, if you have the world transform of the shape you probably want to construct it using ShapeCast::sFromWorldTransform. + static inline void sCastShapeVsShapeWorldSpace(const ShapeCast &inShapeCastWorld, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) + { + ShapeCast local_shape_cast = inShapeCastWorld.PostTransformed(inCenterOfMassTransform2.InversedRotationTranslation()); + sCastShapeVsShapeLocalSpace(local_shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); + } + + /// Function that collides 2 shapes (see sCollideShapeVsShape) + using CollideShape = void (*)(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); + + /// Function that casts a shape vs another shape (see sCastShapeVsShapeLocalSpace) + using CastShape = void (*)(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + /// Initialize all collision functions with a function that asserts and returns no collision + static void sInit(); + + /// Register a collide shape function in the collision table + static void sRegisterCollideShape(EShapeSubType inType1, EShapeSubType inType2, CollideShape inFunction) { sCollideShape[(int)inType1][(int)inType2] = inFunction; } + + /// Register a cast shape function in the collision table + static void sRegisterCastShape(EShapeSubType inType1, EShapeSubType inType2, CastShape inFunction) { sCastShape[(int)inType1][(int)inType2] = inFunction; } + + /// An implementation of CollideShape that swaps inShape1 and inShape2 and swaps the result back, can be registered if the collision function only exists the other way around + static void sReversedCollideShape(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); + + /// An implementation of CastShape that swaps inShape1 and inShape2 and swaps the result back, can be registered if the collision function only exists the other way around + static void sReversedCastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + +private: + static CollideShape sCollideShape[NumSubShapeTypes][NumSubShapeTypes]; + static CastShape sCastShape[NumSubShapeTypes][NumSubShapeTypes]; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionGroup.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionGroup.cpp new file mode 100644 index 000000000000..a8c98b6dc4ca --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionGroup.cpp @@ -0,0 +1,33 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(CollisionGroup) +{ + JPH_ADD_ATTRIBUTE(CollisionGroup, mGroupFilter) + JPH_ADD_ATTRIBUTE(CollisionGroup, mGroupID) + JPH_ADD_ATTRIBUTE(CollisionGroup, mSubGroupID) +} + +void CollisionGroup::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mGroupID); + inStream.Write(mSubGroupID); +} + +void CollisionGroup::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mGroupID); + inStream.Read(mSubGroupID); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionGroup.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionGroup.h new file mode 100644 index 000000000000..5e8c6cf1f5ee --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionGroup.h @@ -0,0 +1,94 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; + +/// Two objects collide with each other if: +/// - Both don't have a group filter +/// - The first group filter says that the objects can collide +/// - Or if there's no filter for the first object, the second group filter says the objects can collide +class JPH_EXPORT CollisionGroup +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, CollisionGroup) + +public: + using GroupID = uint32; + using SubGroupID = uint32; + + static const GroupID cInvalidGroup = ~GroupID(0); + static const SubGroupID cInvalidSubGroup = ~SubGroupID(0); + + /// Default constructor + CollisionGroup() = default; + + /// Construct with all properties + CollisionGroup(const GroupFilter *inFilter, GroupID inGroupID, SubGroupID inSubGroupID) : mGroupFilter(inFilter), mGroupID(inGroupID), mSubGroupID(inSubGroupID) { } + + /// Set the collision group filter + inline void SetGroupFilter(const GroupFilter *inFilter) + { + mGroupFilter = inFilter; + } + + /// Get the collision group filter + inline const GroupFilter *GetGroupFilter() const + { + return mGroupFilter; + } + + /// Set the main group id for this object + inline void SetGroupID(GroupID inID) + { + mGroupID = inID; + } + + inline GroupID GetGroupID() const + { + return mGroupID; + } + + /// Add this object to a sub group + inline void SetSubGroupID(SubGroupID inID) + { + mSubGroupID = inID; + } + + inline SubGroupID GetSubGroupID() const + { + return mSubGroupID; + } + + /// Check if this object collides with another object + bool CanCollide(const CollisionGroup &inOther) const + { + // Call the CanCollide function of the first group filter that's not null + if (mGroupFilter != nullptr) + return mGroupFilter->CanCollide(*this, inOther); + else if (inOther.mGroupFilter != nullptr) + return inOther.mGroupFilter->CanCollide(inOther, *this); + else + return true; + } + + /// Saves the state of this object in binary form to inStream. Does not save group filter. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restore the state of this object from inStream. Does not save group filter. + void RestoreBinaryState(StreamIn &inStream); + +private: + RefConst mGroupFilter; + GroupID mGroupID = cInvalidGroup; + SubGroupID mSubGroupID = cInvalidSubGroup; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ContactListener.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ContactListener.h new file mode 100644 index 000000000000..1a24b4fb88a5 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ContactListener.h @@ -0,0 +1,119 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class Body; +class CollideShapeResult; + +/// Array of contact points +using ContactPoints = StaticArray; + +/// Manifold class, describes the contact surface between two bodies +class ContactManifold +{ +public: + /// Swaps shape 1 and 2 + ContactManifold SwapShapes() const { return { mBaseOffset, -mWorldSpaceNormal, mPenetrationDepth, mSubShapeID2, mSubShapeID1, mRelativeContactPointsOn2, mRelativeContactPointsOn1 }; } + + /// Access to the world space contact positions + inline RVec3 GetWorldSpaceContactPointOn1(uint inIndex) const { return mBaseOffset + mRelativeContactPointsOn1[inIndex]; } + inline RVec3 GetWorldSpaceContactPointOn2(uint inIndex) const { return mBaseOffset + mRelativeContactPointsOn2[inIndex]; } + + RVec3 mBaseOffset; ///< Offset to which all the contact points are relative + Vec3 mWorldSpaceNormal; ///< Normal for this manifold, direction along which to move body 2 out of collision along the shortest path + float mPenetrationDepth; ///< Penetration depth (move shape 2 by this distance to resolve the collision). If this value is negative, this is a speculative contact point and may not actually result in a velocity change as during solving the bodies may not actually collide. + SubShapeID mSubShapeID1; ///< Sub shapes that formed this manifold (note that when multiple manifolds are combined because they're coplanar, we lose some information here because we only keep track of one sub shape pair that we encounter, see description at Body::SetUseManifoldReduction) + SubShapeID mSubShapeID2; + ContactPoints mRelativeContactPointsOn1; ///< Contact points on the surface of shape 1 relative to mBaseOffset. + ContactPoints mRelativeContactPointsOn2; ///< Contact points on the surface of shape 2 relative to mBaseOffset. If there's no penetration, this will be the same as mRelativeContactPointsOn1. If there is penetration they will be different. +}; + +/// When a contact point is added or persisted, the callback gets a chance to override certain properties of the contact constraint. +/// The values are filled in with their defaults by the system so the callback doesn't need to modify anything, but it can if it wants to. +class ContactSettings +{ +public: + float mCombinedFriction; ///< Combined friction for the body pair (see: PhysicsSystem::SetCombineFriction) + float mCombinedRestitution; ///< Combined restitution for the body pair (see: PhysicsSystem::SetCombineRestitution) + float mInvMassScale1 = 1.0f; ///< Scale factor for the inverse mass of body 1 (0 = infinite mass, 1 = use original mass, 2 = body has half the mass). For the same contact pair, you should strive to keep the value the same over time. + float mInvInertiaScale1 = 1.0f; ///< Scale factor for the inverse inertia of body 1 (usually same as mInvMassScale1) + float mInvMassScale2 = 1.0f; ///< Scale factor for the inverse mass of body 2 (0 = infinite mass, 1 = use original mass, 2 = body has half the mass). For the same contact pair, you should strive to keep the value the same over time. + float mInvInertiaScale2 = 1.0f; ///< Scale factor for the inverse inertia of body 2 (usually same as mInvMassScale2) + bool mIsSensor; ///< If the contact should be treated as a sensor vs body contact (no collision response) + Vec3 mRelativeLinearSurfaceVelocity = Vec3::sZero(); ///< Relative linear surface velocity between the bodies (world space surface velocity of body 2 - world space surface velocity of body 1), can be used to create a conveyor belt effect + Vec3 mRelativeAngularSurfaceVelocity = Vec3::sZero(); ///< Relative angular surface velocity between the bodies (world space angular surface velocity of body 2 - world space angular surface velocity of body 1). Note that this angular velocity is relative to the center of mass of body 1, so if you want it relative to body 2's center of mass you need to add body 2 angular velocity x (body 1 world space center of mass - body 2 world space center of mass) to mRelativeLinearSurfaceVelocity. +}; + +/// Return value for the OnContactValidate callback. Determines if the contact is being processed or not. +/// Results are ordered so that the strongest accept has the lowest number and the strongest reject the highest number (which allows for easy combining of results) +enum class ValidateResult +{ + AcceptAllContactsForThisBodyPair, ///< Accept this and any further contact points for this body pair + AcceptContact, ///< Accept this contact only (and continue calling this callback for every contact manifold for the same body pair) + RejectContact, ///< Reject this contact only (but process any other contact manifolds for the same body pair) + 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. +/// During OnContactRemoved you cannot access the bodies at all, see the comments at that function. +class ContactListener +{ +public: + /// Ensure virtual destructor + 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. + /// 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! + /// 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! + /// 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! + /// 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 + /// detect this by keeping the old shape (before adding/removing a part) around until the next PhysicsSystem::Update (when the OnContactPersisted + /// callbacks are triggered) and resolving the sub shape ID against both the old and new shape to see if they still refer to the same child shape. + 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. + /// 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 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 */ } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/EstimateCollisionResponse.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/EstimateCollisionResponse.cpp new file mode 100644 index 000000000000..53cf12b5d41d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/EstimateCollisionResponse.cpp @@ -0,0 +1,213 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +void EstimateCollisionResponse(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, CollisionEstimationResult &outResult, float inCombinedFriction, float inCombinedRestitution, float inMinVelocityForRestitution, uint inNumIterations) +{ + // Note this code is based on AxisConstraintPart, see that class for more comments on the math + + ContactPoints::size_type num_points = inManifold.mRelativeContactPointsOn1.size(); + JPH_ASSERT(num_points == inManifold.mRelativeContactPointsOn2.size()); + + // Start with zero impulses + outResult.mImpulses.resize(num_points); + memset(outResult.mImpulses.data(), 0, num_points * sizeof(CollisionEstimationResult::Impulse)); + + // Calculate friction directions + outResult.mTangent1 = inManifold.mWorldSpaceNormal.GetNormalizedPerpendicular(); + outResult.mTangent2 = inManifold.mWorldSpaceNormal.Cross(outResult.mTangent1); + + // Get body velocities + EMotionType motion_type1 = inBody1.GetMotionType(); + const MotionProperties *motion_properties1 = inBody1.GetMotionPropertiesUnchecked(); + if (motion_type1 != EMotionType::Static) + { + outResult.mLinearVelocity1 = motion_properties1->GetLinearVelocity(); + outResult.mAngularVelocity1 = motion_properties1->GetAngularVelocity(); + } + else + outResult.mLinearVelocity1 = outResult.mAngularVelocity1 = Vec3::sZero(); + + EMotionType motion_type2 = inBody2.GetMotionType(); + const MotionProperties *motion_properties2 = inBody2.GetMotionPropertiesUnchecked(); + if (motion_type2 != EMotionType::Static) + { + outResult.mLinearVelocity2 = motion_properties2->GetLinearVelocity(); + outResult.mAngularVelocity2 = motion_properties2->GetAngularVelocity(); + } + else + outResult.mLinearVelocity2 = outResult.mAngularVelocity2 = Vec3::sZero(); + + // Get inverse mass and inertia + float inv_m1, inv_m2; + Mat44 inv_i1, inv_i2; + if (motion_type1 == EMotionType::Dynamic) + { + inv_m1 = motion_properties1->GetInverseMass(); + inv_i1 = inBody1.GetInverseInertia(); + } + else + { + inv_m1 = 0.0f; + inv_i1 = Mat44::sZero(); + } + + if (motion_type2 == EMotionType::Dynamic) + { + inv_m2 = motion_properties2->GetInverseMass(); + inv_i2 = inBody2.GetInverseInertia(); + } + else + { + inv_m2 = 0.0f; + inv_i2 = Mat44::sZero(); + } + + // Get center of masses relative to the base offset + Vec3 com1 = Vec3(inBody1.GetCenterOfMassPosition() - inManifold.mBaseOffset); + Vec3 com2 = Vec3(inBody2.GetCenterOfMassPosition() - inManifold.mBaseOffset); + + struct AxisConstraint + { + inline void Initialize(Vec3Arg inR1, Vec3Arg inR2, Vec3Arg inWorldSpaceNormal, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2) + { + // Calculate effective mass: K^-1 = (J M^-1 J^T)^-1 + mR1PlusUxAxis = inR1.Cross(inWorldSpaceNormal); + mR2xAxis = inR2.Cross(inWorldSpaceNormal); + mInvI1_R1PlusUxAxis = inInvI1.Multiply3x3(mR1PlusUxAxis); + mInvI2_R2xAxis = inInvI2.Multiply3x3(mR2xAxis); + mEffectiveMass = 1.0f / (inInvM1 + mInvI1_R1PlusUxAxis.Dot(mR1PlusUxAxis) + inInvM2 + mInvI2_R2xAxis.Dot(mR2xAxis)); + mBias = 0.0f; + } + + inline float SolveGetLambda(Vec3Arg inWorldSpaceNormal, const CollisionEstimationResult &inResult) const + { + // Calculate jacobian multiplied by linear/angular velocity + float jv = inWorldSpaceNormal.Dot(inResult.mLinearVelocity1 - inResult.mLinearVelocity2) + mR1PlusUxAxis.Dot(inResult.mAngularVelocity1) - mR2xAxis.Dot(inResult.mAngularVelocity2); + + // Lagrange multiplier is: + // + // lambda = -K^-1 (J v + b) + return mEffectiveMass * (jv - mBias); + } + + inline void SolveApplyLambda(Vec3Arg inWorldSpaceNormal, float inInvM1, float inInvM2, float inLambda, CollisionEstimationResult &ioResult) const + { + // Apply impulse to body velocities + ioResult.mLinearVelocity1 -= (inLambda * inInvM1) * inWorldSpaceNormal; + ioResult.mAngularVelocity1 -= inLambda * mInvI1_R1PlusUxAxis; + ioResult.mLinearVelocity2 += (inLambda * inInvM2) * inWorldSpaceNormal; + ioResult.mAngularVelocity2 += inLambda * mInvI2_R2xAxis; + } + + inline void Solve(Vec3Arg inWorldSpaceNormal, float inInvM1, float inInvM2, float inMinLambda, float inMaxLambda, float &ioTotalLambda, CollisionEstimationResult &ioResult) const + { + // Calculate new total lambda + float total_lambda = ioTotalLambda + SolveGetLambda(inWorldSpaceNormal, ioResult); + + // Clamp impulse + total_lambda = Clamp(total_lambda, inMinLambda, inMaxLambda); + + SolveApplyLambda(inWorldSpaceNormal, inInvM1, inInvM2, total_lambda - ioTotalLambda, ioResult); + + ioTotalLambda = total_lambda; + } + + Vec3 mR1PlusUxAxis; + Vec3 mR2xAxis; + Vec3 mInvI1_R1PlusUxAxis; + Vec3 mInvI2_R2xAxis; + float mEffectiveMass; + float mBias; + }; + + struct Constraint + { + AxisConstraint mContact; + AxisConstraint mFriction1; + AxisConstraint mFriction2; + }; + + // Initialize the constraint properties + Constraint constraints[ContactPoints::Capacity]; + for (uint c = 0; c < num_points; ++c) + { + Constraint &constraint = constraints[c]; + + // Calculate contact points relative to body 1 and 2 + Vec3 p = 0.5f * (inManifold.mRelativeContactPointsOn1[c] + inManifold.mRelativeContactPointsOn2[c]); + Vec3 r1 = p - com1; + Vec3 r2 = p - com2; + + // Initialize contact constraint + constraint.mContact.Initialize(r1, r2, inManifold.mWorldSpaceNormal, inv_m1, inv_m2, inv_i1, inv_i2); + + // Handle elastic collisions + if (inCombinedRestitution > 0.0f) + { + // Calculate velocity of contact point + Vec3 relative_velocity = outResult.mLinearVelocity2 + outResult.mAngularVelocity2.Cross(r2) - outResult.mLinearVelocity1 - outResult.mAngularVelocity1.Cross(r1); + float normal_velocity = relative_velocity.Dot(inManifold.mWorldSpaceNormal); + + // If it is big enough, apply restitution + if (normal_velocity < -inMinVelocityForRestitution) + constraint.mContact.mBias = inCombinedRestitution * normal_velocity; + } + + if (inCombinedFriction > 0.0f) + { + // Initialize friction constraints + constraint.mFriction1.Initialize(r1, r2, outResult.mTangent1, inv_m1, inv_m2, inv_i1, inv_i2); + constraint.mFriction2.Initialize(r1, r2, outResult.mTangent2, inv_m1, inv_m2, inv_i1, inv_i2); + } + } + + // If there's only 1 contact point, we only need 1 iteration + int num_iterations = inCombinedFriction <= 0.0f && num_points == 1? 1 : inNumIterations; + + // Solve iteratively + for (int iteration = 0; iteration < num_iterations; ++iteration) + { + // Solve friction constraints first + if (inCombinedFriction > 0.0f && iteration > 0) // For first iteration the contact impulse is zero so there's no point in applying friction + for (uint c = 0; c < num_points; ++c) + { + const Constraint &constraint = constraints[c]; + CollisionEstimationResult::Impulse &impulse = outResult.mImpulses[c]; + + float lambda1 = impulse.mFrictionImpulse1 + constraint.mFriction1.SolveGetLambda(outResult.mTangent1, outResult); + float lambda2 = impulse.mFrictionImpulse2 + constraint.mFriction2.SolveGetLambda(outResult.mTangent2, outResult); + + // Calculate max impulse based on contact impulse + float max_impulse = inCombinedFriction * impulse.mContactImpulse; + + // If the total lambda that we will apply is too large, scale it back + float total_lambda_sq = Square(lambda1) + Square(lambda2); + if (total_lambda_sq > Square(max_impulse)) + { + float scale = max_impulse / sqrt(total_lambda_sq); + lambda1 *= scale; + lambda2 *= scale; + } + + constraint.mFriction1.SolveApplyLambda(outResult.mTangent1, inv_m1, inv_m2, lambda1 - impulse.mFrictionImpulse1, outResult); + constraint.mFriction2.SolveApplyLambda(outResult.mTangent2, inv_m1, inv_m2, lambda2 - impulse.mFrictionImpulse2, outResult); + + impulse.mFrictionImpulse1 = lambda1; + impulse.mFrictionImpulse2 = lambda2; + } + + // Solve contact constraints last + for (uint c = 0; c < num_points; ++c) + constraints[c].mContact.Solve(inManifold.mWorldSpaceNormal, inv_m1, inv_m2, 0.0f, FLT_MAX, outResult.mImpulses[c].mContactImpulse, outResult); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/EstimateCollisionResponse.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/EstimateCollisionResponse.h new file mode 100644 index 000000000000..45098d1477e6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/EstimateCollisionResponse.h @@ -0,0 +1,48 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// A structure that contains the estimated contact and friction impulses and the resulting body velocities +struct CollisionEstimationResult +{ + Vec3 mLinearVelocity1; ///< The estimated linear velocity of body 1 after collision + Vec3 mAngularVelocity1; ///< The estimated angular velocity of body 1 after collision + Vec3 mLinearVelocity2; ///< The estimated linear velocity of body 2 after collision + Vec3 mAngularVelocity2; ///< The estimated angular velocity of body 2 after collision + + Vec3 mTangent1; ///< Normalized tangent of contact normal + Vec3 mTangent2; ///< Second normalized tangent of contact normal (forms a basis with mTangent1 and mWorldSpaceNormal) + + struct Impulse + { + float mContactImpulse; ///< Estimated contact impulses (kg m / s) + float mFrictionImpulse1; ///< Estimated friction impulses in the direction of tangent 1 (kg m / s) + float mFrictionImpulse2; ///< Estimated friction impulses in the direction of tangent 2 (kg m / s) + }; + + using Impulses = StaticArray; + + Impulses mImpulses; +}; + +/// This function estimates the contact impulses and body velocity changes as a result of a collision. +/// It can be used in the ContactListener::OnContactAdded to determine the strength of the collision to e.g. play a sound or trigger a particle system. +/// This function is accurate when two bodies collide but will not be accurate when more than 2 bodies collide at the same time as it does not know about these other collisions. +/// +/// @param inBody1 Colliding body 1 +/// @param inBody2 Colliding body 2 +/// @param inManifold The collision manifold +/// @param outResult A structure that contains the estimated contact and friction impulses and the resulting body velocities +/// @param inCombinedFriction The combined friction of body 1 and body 2 (see ContactSettings::mCombinedFriction) +/// @param inCombinedRestitution The combined restitution of body 1 and body 2 (see ContactSettings::mCombinedRestitution) +/// @param inMinVelocityForRestitution Minimal velocity required for restitution to be applied (see PhysicsSettings::mMinVelocityForRestitution) +/// @param inNumIterations Number of iterations to use for the impulse estimation (see PhysicsSettings::mNumVelocitySteps, note you can probably use a lower number for a decent estimate). If you set the number of iterations to 1 then no friction will be calculated. +JPH_EXPORT void EstimateCollisionResponse(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, CollisionEstimationResult &outResult, float inCombinedFriction, float inCombinedRestitution, float inMinVelocityForRestitution = 1.0f, uint inNumIterations = 10); + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.cpp new file mode 100644 index 000000000000..80c7620fdad1 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.cpp @@ -0,0 +1,32 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT_BASE(GroupFilter) +{ + JPH_ADD_BASE_CLASS(GroupFilter, SerializableObject) +} + +void GroupFilter::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(GetRTTI()->GetHash()); +} + +void GroupFilter::RestoreBinaryState(StreamIn &inStream) +{ + // RTTI hash is read in sRestoreFromBinaryState +} + +GroupFilter::GroupFilterResult GroupFilter::sRestoreFromBinaryState(StreamIn &inStream) +{ + return StreamUtils::RestoreObject(inStream, &GroupFilter::RestoreBinaryState); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.h new file mode 100644 index 000000000000..5e01043a8f37 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.h @@ -0,0 +1,41 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollisionGroup; +class StreamIn; +class StreamOut; + +/// Abstract class that checks if two CollisionGroups collide +class JPH_EXPORT GroupFilter : public SerializableObject, public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, GroupFilter) + +public: + /// Virtual destructor + virtual ~GroupFilter() override = default; + + /// Check if two groups collide + virtual bool CanCollide(const CollisionGroup &inGroup1, const CollisionGroup &inGroup2) const = 0; + + /// Saves the contents of the group filter in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const; + + using GroupFilterResult = Result>; + + /// Creates a GroupFilter of the correct type and restores its contents from the binary stream inStream. + static GroupFilterResult sRestoreFromBinaryState(StreamIn &inStream); + +protected: + /// This function should not be called directly, it is used by sRestoreFromBinaryState. + virtual void RestoreBinaryState(StreamIn &inStream); +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilterTable.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilterTable.cpp new file mode 100644 index 000000000000..dd69d27c8522 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilterTable.cpp @@ -0,0 +1,38 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(GroupFilterTable) +{ + JPH_ADD_BASE_CLASS(GroupFilterTable, GroupFilter) + + JPH_ADD_ATTRIBUTE(GroupFilterTable, mNumSubGroups) + JPH_ADD_ATTRIBUTE(GroupFilterTable, mTable) +} + +void GroupFilterTable::SaveBinaryState(StreamOut &inStream) const +{ + GroupFilter::SaveBinaryState(inStream); + + inStream.Write(mNumSubGroups); + inStream.Write(mTable); +} + +void GroupFilterTable::RestoreBinaryState(StreamIn &inStream) +{ + GroupFilter::RestoreBinaryState(inStream); + + inStream.Read(mNumSubGroups); + inStream.Read(mTable); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilterTable.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilterTable.h new file mode 100644 index 000000000000..76cae277da6d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilterTable.h @@ -0,0 +1,130 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Implementation of GroupFilter that stores a bit table with one bit per sub shape ID pair to determine if they collide or not +/// +/// The collision rules: +/// - If one of the objects is in the cInvalidGroup the objects will collide. +/// - If the objects are in different groups they will collide. +/// - If they're in the same group but their collision filter is different they will not collide. +/// - If they're in the same group and their collision filters match, we'll use the SubGroupID and the table below. +/// +/// For N = 6 sub groups the table will look like: +/// +/// sub group 1 ---> +/// sub group 2 x..... +/// | ox.... +/// | oox... +/// V ooox.. +/// oooox. +/// ooooox +/// +/// * 'x' means sub group 1 == sub group 2 and we define this to never collide. +/// * 'o' is a bit that we have to store that defines if the sub groups collide or not. +/// * '.' is a bit we don't need to store because the table is symmetric, we take care that group 2 > group 1 by swapping sub group 1 and sub group 2 if needed. +/// +/// The total number of bits we need to store is (N * (N - 1)) / 2 +class JPH_EXPORT GroupFilterTable final : public GroupFilter +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, GroupFilterTable) + +private: + using GroupID = CollisionGroup::GroupID; + using SubGroupID = CollisionGroup::SubGroupID; + + /// Get which bit corresponds to the pair (inSubGroup1, inSubGroup2) + int GetBit(SubGroupID inSubGroup1, SubGroupID inSubGroup2) const + { + JPH_ASSERT(inSubGroup1 != inSubGroup2); + + // We store the lower left half only, so swap the inputs when trying to access the top right half + if (inSubGroup1 > inSubGroup2) + std::swap(inSubGroup1, inSubGroup2); + + JPH_ASSERT(inSubGroup2 < mNumSubGroups); + + // Calculate at which bit the entry for this pair resides + // We use the fact that a row always starts at inSubGroup2 * (inSubGroup2 - 1) / 2 + // (this is the amount of bits needed to store a table of inSubGroup2 entries) + return (inSubGroup2 * (inSubGroup2 - 1)) / 2 + inSubGroup1; + } + +public: + /// Constructs the table with inNumSubGroups subgroups, initially all collision pairs are enabled except when the sub group ID is the same + explicit GroupFilterTable(uint inNumSubGroups = 0) : + mNumSubGroups(inNumSubGroups) + { + // By default everything collides + int table_size = ((inNumSubGroups * (inNumSubGroups - 1)) / 2 + 7) / 8; + mTable.resize(table_size, 0xff); + } + + /// Copy constructor + GroupFilterTable(const GroupFilterTable &inRHS) : mNumSubGroups(inRHS.mNumSubGroups), mTable(inRHS.mTable) { } + + /// Disable collision between two sub groups + void DisableCollision(SubGroupID inSubGroup1, SubGroupID inSubGroup2) + { + int bit = GetBit(inSubGroup1, inSubGroup2); + mTable[bit >> 3] &= (0xff ^ (1 << (bit & 0b111))); + } + + /// Enable collision between two sub groups + void EnableCollision(SubGroupID inSubGroup1, SubGroupID inSubGroup2) + { + int bit = GetBit(inSubGroup1, inSubGroup2); + mTable[bit >> 3] |= 1 << (bit & 0b111); + } + + /// Check if the collision between two subgroups is enabled + inline bool IsCollisionEnabled(SubGroupID inSubGroup1, SubGroupID inSubGroup2) const + { + // Test if the bit is set for this group pair + int bit = GetBit(inSubGroup1, inSubGroup2); + return (mTable[bit >> 3] & (1 << (bit & 0b111))) != 0; + } + + /// Checks if two CollisionGroups collide + virtual bool CanCollide(const CollisionGroup &inGroup1, const CollisionGroup &inGroup2) const override + { + // If one of the groups is cInvalidGroup the objects will collide (note that the if following this if will ensure that group2 is not cInvalidGroup) + if (inGroup1.GetGroupID() == CollisionGroup::cInvalidGroup) + return true; + + // If the objects are in different groups, they collide + if (inGroup1.GetGroupID() != inGroup2.GetGroupID()) + return true; + + // If the collision filters do not match, but they're in the same group we ignore the collision + if (inGroup1.GetGroupFilter() != inGroup2.GetGroupFilter()) + return false; + + // If they are in the same sub group, they don't collide + if (inGroup1.GetSubGroupID() == inGroup2.GetSubGroupID()) + return false; + + // Check the bit table + return IsCollisionEnabled(inGroup1.GetSubGroupID(), inGroup2.GetSubGroupID()); + } + + // See: GroupFilter::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + +protected: + // See: GroupFilter::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + uint mNumSubGroups; ///< The number of subgroups that this group filter supports + Array mTable; ///< The table of bits that indicates which pairs collide +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/InternalEdgeRemovingCollector.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/InternalEdgeRemovingCollector.h new file mode 100644 index 000000000000..0d67729125e4 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/InternalEdgeRemovingCollector.h @@ -0,0 +1,250 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +//#define JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + +#ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG +#include +#endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + +JPH_NAMESPACE_BEGIN + +/// Removes internal edges from collision results. Can be used to filter out 'ghost collisions'. +/// Based on: Contact generation for meshes - Pierre Terdiman (https://www.codercorner.com/MeshContacts.pdf) +class InternalEdgeRemovingCollector : public CollideShapeCollector +{ + static constexpr uint cMaxDelayedResults = 16; + static constexpr uint cMaxVoidedFeatures = 128; + + /// Check if a vertex is voided + inline bool IsVoided(const SubShapeID &inSubShapeID, Vec3 inV) const + { + for (const Voided &vf : mVoidedFeatures) + if (vf.mSubShapeID == inSubShapeID + && inV.IsClose(Vec3::sLoadFloat3Unsafe(vf.mFeature), 1.0e-8f)) + return true; + return false; + } + + /// Add all vertices of a face to the voided features + inline void VoidFeatures(const CollideShapeResult &inResult) + { + if (mVoidedFeatures.size() < cMaxVoidedFeatures) + for (const Vec3 &v : inResult.mShape2Face) + if (!IsVoided(inResult.mSubShapeID1, v)) + { + Voided vf; + v.StoreFloat3(&vf.mFeature); + vf.mSubShapeID = inResult.mSubShapeID1; + mVoidedFeatures.push_back(vf); + if (mVoidedFeatures.size() == cMaxVoidedFeatures) + break; + } + } + + /// Call the chained collector + inline void Chain(const CollideShapeResult &inResult) + { + // Make sure the chained collector has the same context as we do + mChainedCollector.SetContext(GetContext()); + + // Forward the hit + mChainedCollector.AddHit(inResult); + + // If our chained collector updated its early out fraction, we need to follow + UpdateEarlyOutFraction(mChainedCollector.GetEarlyOutFraction()); + } + + /// Call the chained collector and void all features of inResult + inline void ChainAndVoid(const CollideShapeResult &inResult) + { + Chain(inResult); + VoidFeatures(inResult); + + #ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + DebugRenderer::sInstance->DrawWirePolygon(RMat44::sIdentity(), inResult.mShape2Face, Color::sGreen); + DebugRenderer::sInstance->DrawArrow(RVec3(inResult.mContactPointOn2), RVec3(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) : + mChainedCollector(inChainedCollector) + { + } + + // See: CollideShapeCollector::Reset + virtual void Reset() override + { + CollideShapeCollector::Reset(); + + mChainedCollector.Reset(); + + mVoidedFeatures.clear(); + mDelayedResults.clear(); + } + + // See: CollideShapeCollector::OnBody + virtual void OnBody(const Body &inBody) override + { + // Just forward the call to our chained collector + mChainedCollector.OnBody(inBody); + } + + // See: CollideShapeCollector::AddHit + virtual void AddHit(const CollideShapeResult &inResult) override + { + // We only support welding when the shape is a triangle or has more vertices so that we can calculate a normal + if (inResult.mShape2Face.size() < 3) + return ChainAndVoid(inResult); + + // Get the triangle normal of shape 2 face + Vec3 triangle_normal = (inResult.mShape2Face[1] - inResult.mShape2Face[0]).Cross(inResult.mShape2Face[2] - inResult.mShape2Face[0]); + float triangle_normal_len = triangle_normal.Length(); + if (triangle_normal_len < 1e-6f) + return ChainAndVoid(inResult); + + // If the triangle normal matches the contact normal within 1 degree, we can process the contact immediately + // We make the assumption here that if the contact normal and the triangle normal align that the we're dealing with a 'face contact' + Vec3 contact_normal = -inResult.mPenetrationAxis; + float contact_normal_len = inResult.mPenetrationAxis.Length(); + if (triangle_normal.Dot(contact_normal) > 0.999848f * contact_normal_len * triangle_normal_len) // cos(1 degree) + return ChainAndVoid(inResult); + + // Delayed processing + if (mDelayedResults.size() == cMaxDelayedResults) + return ChainAndVoid(inResult); + mDelayedResults.push_back(inResult); + } + + /// After all hits have been added, call this function to process the delayed results + void Flush() + { + // Sort on biggest penetration depth first + uint sorted_indices[cMaxDelayedResults]; + for (uint i = 0; i < uint(mDelayedResults.size()); ++i) + sorted_indices[i] = i; + QuickSort(sorted_indices, sorted_indices + mDelayedResults.size(), [this](uint inLHS, uint inRHS) { return mDelayedResults[inLHS].mPenetrationDepth > mDelayedResults[inRHS].mPenetrationDepth; }); + + // Loop over all results + for (uint i = 0; i < uint(mDelayedResults.size()); ++i) + { + const CollideShapeResult &r = mDelayedResults[sorted_indices[i]]; + + // Determine which vertex or which edge is the closest to the contact point + float best_dist_sq = FLT_MAX; + uint best_v1_idx = 0; + uint best_v2_idx = 0; + uint num_v = uint(r.mShape2Face.size()); + uint v1_idx = num_v - 1; + Vec3 v1 = r.mShape2Face[v1_idx] - r.mContactPointOn2; + for (uint v2_idx = 0; v2_idx < num_v; ++v2_idx) + { + Vec3 v2 = r.mShape2Face[v2_idx] - r.mContactPointOn2; + Vec3 v1_v2 = v2 - v1; + float denominator = v1_v2.LengthSq(); + if (denominator < Square(FLT_EPSILON)) + { + // Degenerate, assume v1 is closest, v2 will be tested in a later iteration + float v1_len_sq = v1.LengthSq(); + if (v1_len_sq < best_dist_sq) + { + best_dist_sq = v1_len_sq; + best_v1_idx = v1_idx; + best_v2_idx = v1_idx; + } + } + else + { + // Taken from ClosestPoint::GetBaryCentricCoordinates + float fraction = -v1.Dot(v1_v2) / denominator; + if (fraction < 1.0e-6f) + { + // Closest lies on v1 + float v1_len_sq = v1.LengthSq(); + if (v1_len_sq < best_dist_sq) + { + best_dist_sq = v1_len_sq; + best_v1_idx = v1_idx; + best_v2_idx = v1_idx; + } + } + else if (fraction < 1.0f - 1.0e-6f) + { + // Closest lies on the line segment v1, v2 + Vec3 closest = v1 + fraction * v1_v2; + float closest_len_sq = closest.LengthSq(); + if (closest_len_sq < best_dist_sq) + { + best_dist_sq = closest_len_sq; + best_v1_idx = v1_idx; + best_v2_idx = v2_idx; + } + } + // else closest is v2, but v2 will be tested in a later iteration + } + + v1_idx = v2_idx; + v1 = v2; + } + + // Check if this vertex/edge is voided + bool voided = IsVoided(r.mSubShapeID1, r.mShape2Face[best_v1_idx]) + && (best_v1_idx == best_v2_idx || IsVoided(r.mSubShapeID1, r.mShape2Face[best_v2_idx])); + + #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); + #endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + + // No voided features, accept the contact + if (!voided) + Chain(r); + + // Void the features of this face + VoidFeatures(r); + } + + // All delayed results have been processed + mVoidedFeatures.clear(); + mDelayedResults.clear(); + } + + /// 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 = { }) + { + 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); + CollisionDispatch::sCollideShapeVsShape(inShape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, wrapper, inShapeFilter); + wrapper.Flush(); + } + +private: + // This algorithm tests a convex shape (shape 1) against a set of polygons (shape 2). + // This assumption doesn't hold if the shape we're testing is a compound shape, so we must also + // store the sub shape ID and ignore voided features that belong to another sub shape ID. + struct Voided + { + Float3 mFeature; // Feature that is voided (of shape 2). Read with Vec3::sLoadFloat3Unsafe so must not be the last member. + SubShapeID mSubShapeID; // Sub shape ID of the shape that is colliding against the feature (of shape 1). + }; + + CollideShapeCollector & mChainedCollector; + StaticArray mVoidedFeatures; + StaticArray mDelayedResults; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.cpp new file mode 100644 index 000000000000..3331f5d7a03d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.cpp @@ -0,0 +1,237 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +void PruneContactPoints(Vec3Arg inPenetrationAxis, ContactPoints &ioContactPointsOn1, ContactPoints &ioContactPointsOn2 JPH_IF_DEBUG_RENDERER(, RVec3Arg inCenterOfMass)) +{ + // Makes no sense to call this with 4 or less points + JPH_ASSERT(ioContactPointsOn1.size() > 4); + + // Both arrays should have the same size + JPH_ASSERT(ioContactPointsOn1.size() == ioContactPointsOn2.size()); + + // Penetration axis must be normalized + JPH_ASSERT(inPenetrationAxis.IsNormalized()); + + // We use a heuristic of (distance to center of mass) * (penetration depth) to find the contact point that we should keep + // Neither of those two terms should ever become zero, so we clamp against this minimum value + constexpr float cMinDistanceSq = 1.0e-6f; // 1 mm + + ContactPoints projected; + StaticArray penetration_depth_sq; + for (ContactPoints::size_type i = 0; i < ioContactPointsOn1.size(); ++i) + { + // Project contact points on the plane through inCenterOfMass with normal inPenetrationAxis and center around the center of mass of body 1 + // (note that since all points are relative to inCenterOfMass we can project onto the plane through the origin) + Vec3 v1 = ioContactPointsOn1[i]; + projected.push_back(v1 - v1.Dot(inPenetrationAxis) * inPenetrationAxis); + + // Calculate penetration depth^2 of each point and clamp against the minimal distance + Vec3 v2 = ioContactPointsOn2[i]; + penetration_depth_sq.push_back(max(cMinDistanceSq, (v2 - v1).LengthSq())); + } + + // Find the point that is furthest away from the center of mass (its torque will have the biggest influence) + // and the point that has the deepest penetration depth. Use the heuristic (distance to center of mass) * (penetration depth) for this. + uint point1 = 0; + float val = max(cMinDistanceSq, projected[0].LengthSq()) * penetration_depth_sq[0]; + for (uint i = 0; i < projected.size(); ++i) + { + float v = max(cMinDistanceSq, projected[i].LengthSq()) * penetration_depth_sq[i]; + if (v > val) + { + val = v; + point1 = i; + } + } + Vec3 point1v = projected[point1]; + + // Find point furthest from the first point forming a line segment with point1. Again combine this with the heuristic + // for deepest point as per above. + uint point2 = uint(-1); + val = -FLT_MAX; + for (uint i = 0; i < projected.size(); ++i) + if (i != point1) + { + float v = max(cMinDistanceSq, (projected[i] - point1v).LengthSq()) * penetration_depth_sq[i]; + if (v > val) + { + val = v; + point2 = i; + } + } + JPH_ASSERT(point2 != uint(-1)); + Vec3 point2v = projected[point2]; + + // Find furthest points on both sides of the line segment in order to maximize the area + uint point3 = uint(-1); + uint point4 = uint(-1); + float min_val = 0.0f; + float max_val = 0.0f; + Vec3 perp = (point2v - point1v).Cross(inPenetrationAxis); + for (uint i = 0; i < projected.size(); ++i) + if (i != point1 && i != point2) + { + float v = perp.Dot(projected[i] - point1v); + if (v < min_val) + { + min_val = v; + point3 = i; + } + else if (v > max_val) + { + max_val = v; + point4 = i; + } + } + + // Add points to array (in order so they form a polygon) + StaticArray points_to_keep_on_1, points_to_keep_on_2; + points_to_keep_on_1.push_back(ioContactPointsOn1[point1]); + points_to_keep_on_2.push_back(ioContactPointsOn2[point1]); + if (point3 != uint(-1)) + { + points_to_keep_on_1.push_back(ioContactPointsOn1[point3]); + points_to_keep_on_2.push_back(ioContactPointsOn2[point3]); + } + points_to_keep_on_1.push_back(ioContactPointsOn1[point2]); + points_to_keep_on_2.push_back(ioContactPointsOn2[point2]); + if (point4 != uint(-1)) + { + JPH_ASSERT(point3 != point4); + points_to_keep_on_1.push_back(ioContactPointsOn1[point4]); + points_to_keep_on_2.push_back(ioContactPointsOn2[point4]); + } + +#ifdef JPH_DEBUG_RENDERER + if (ContactConstraintManager::sDrawContactPointReduction) + { + // Draw input polygon + DebugRenderer::sInstance->DrawWirePolygon(RMat44::sTranslation(inCenterOfMass), ioContactPointsOn1, Color::sOrange, 0.05f); + + // Draw primary axis + DebugRenderer::sInstance->DrawArrow(inCenterOfMass + ioContactPointsOn1[point1], inCenterOfMass + ioContactPointsOn1[point2], Color::sRed, 0.05f); + + // Draw contact points we kept + for (Vec3 p : points_to_keep_on_1) + DebugRenderer::sInstance->DrawMarker(inCenterOfMass + p, Color::sGreen, 0.1f); + } +#endif // JPH_DEBUG_RENDERER + + // Copy the points back to the input buffer + ioContactPointsOn1 = points_to_keep_on_1; + ioContactPointsOn2 = points_to_keep_on_2; +} + +void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inPenetrationAxis, float inMaxContactDistanceSq , const ConvexShape::SupportingFace &inShape1Face, const ConvexShape::SupportingFace &inShape2Face, ContactPoints &outContactPoints1, ContactPoints &outContactPoints2 JPH_IF_DEBUG_RENDERER(, RVec3Arg inCenterOfMass)) +{ +#ifdef JPH_DEBUG_RENDERER + if (ContactConstraintManager::sDrawContactPoint) + { + RVec3 cp1 = inCenterOfMass + inContactPoint1; + RVec3 cp2 = inCenterOfMass + inContactPoint2; + + // Draw contact points + DebugRenderer::sInstance->DrawMarker(cp1, Color::sRed, 0.1f); + DebugRenderer::sInstance->DrawMarker(cp2, Color::sGreen, 0.1f); + + // Draw contact normal + DebugRenderer::sInstance->DrawArrow(cp1, cp1 + inPenetrationAxis.Normalized(), Color::sRed, 0.05f); + } +#endif // JPH_DEBUG_RENDERER + + // Remember size before adding new points, to check at the end if we added some + ContactPoints::size_type old_size = outContactPoints1.size(); + + // Check if both shapes have polygon faces + if (inShape1Face.size() >= 2 // The dynamic shape needs to have at least 2 points or else there can never be more than 1 contact point + && inShape2Face.size() >= 3) // The dynamic/static shape needs to have at least 3 points (in the case that it has 2 points only if the edges match exactly you can have 2 contact points, but this situation is unstable anyhow) + { + // Clip the polygon of face 2 against that of 1 + ConvexShape::SupportingFace clipped_face; + if (inShape1Face.size() >= 3) + ClipPolyVsPoly(inShape2Face, inShape1Face, inPenetrationAxis, clipped_face); + else if (inShape1Face.size() == 2) + ClipPolyVsEdge(inShape2Face, inShape1Face[0], inShape1Face[1], inPenetrationAxis, clipped_face); + + // Project the points back onto the plane of shape 1 face and only keep those that are behind the plane + Vec3 plane_origin = inShape1Face[0]; + Vec3 plane_normal; + Vec3 first_edge = inShape1Face[1] - plane_origin; + if (inShape1Face.size() >= 3) + { + // Three vertices, can just calculate the normal + plane_normal = first_edge.Cross(inShape1Face[2] - plane_origin); + } + else + { + // Two vertices, first find a perpendicular to the edge and penetration axis and then use the perpendicular together with the edge to form a normal + plane_normal = first_edge.Cross(inPenetrationAxis).Cross(first_edge); + } + + // Check if the plane normal has any length, if not the clipped shape is so small that we'll just use the contact points + float plane_normal_len_sq = plane_normal.LengthSq(); + if (plane_normal_len_sq > 0.0f) + { + // Discard points of faces that are too far away to collide + for (Vec3 p2 : clipped_face) + { + float distance = (p2 - plane_origin).Dot(plane_normal); // Note should divide by length of plane_normal (unnormalized here) + if (distance <= 0.0f || Square(distance) < inMaxContactDistanceSq * plane_normal_len_sq) // Must be close enough to plane, note we correct for not dividing by plane normal length here + { + // Project point back on shape 1 using the normal, note we correct for not dividing by plane normal length here: + // p1 = p2 - (distance / sqrt(plane_normal_len_sq)) * (plane_normal / sqrt(plane_normal_len_sq)); + Vec3 p1 = p2 - (distance / plane_normal_len_sq) * plane_normal; + + outContactPoints1.push_back(p1); + outContactPoints2.push_back(p2); + } + } + } + + #ifdef JPH_DEBUG_RENDERER + if (ContactConstraintManager::sDrawSupportingFaces) + { + RMat44 com = RMat44::sTranslation(inCenterOfMass); + + // Draw clipped poly + DebugRenderer::sInstance->DrawWirePolygon(com, clipped_face, Color::sOrange); + + // Draw supporting faces + DebugRenderer::sInstance->DrawWirePolygon(com, inShape1Face, Color::sRed, 0.05f); + DebugRenderer::sInstance->DrawWirePolygon(com, inShape2Face, Color::sGreen, 0.05f); + + // Draw normal + if (plane_normal_len_sq > 0.0f) + { + RVec3 plane_origin_ws = inCenterOfMass + plane_origin; + DebugRenderer::sInstance->DrawArrow(plane_origin_ws, plane_origin_ws + plane_normal / sqrt(plane_normal_len_sq), Color::sYellow, 0.05f); + } + + // Draw contact points that remain after distance check + for (ContactPoints::size_type p = old_size; p < outContactPoints1.size(); ++p) + DebugRenderer::sInstance->DrawMarker(inCenterOfMass + outContactPoints1[p], Color::sYellow, 0.1f); + } + #endif // JPH_DEBUG_RENDERER + } + + // If the clipping result is empty, use the contact point itself + if (outContactPoints1.size() == old_size) + { + outContactPoints1.push_back(inContactPoint1); + outContactPoints2.push_back(inContactPoint2); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h new file mode 100644 index 000000000000..72a5b8f0c14a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h @@ -0,0 +1,44 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Remove contact points if there are > 4 (no more than 4 are needed for a stable solution) +/// @param inPenetrationAxis is the world space penetration axis (must be normalized) +/// @param ioContactPointsOn1 The contact points on shape 1 relative to inCenterOfMass +/// @param ioContactPointsOn2 The contact points on shape 2 relative to inCenterOfMass +/// On output ioContactPointsOn1/2 are reduced to 4 or less points +#ifdef JPH_DEBUG_RENDERER +/// @param inCenterOfMass Center of mass position of body 1 +#endif +JPH_EXPORT void PruneContactPoints(Vec3Arg inPenetrationAxis, ContactPoints &ioContactPointsOn1, ContactPoints &ioContactPointsOn2 +#ifdef JPH_DEBUG_RENDERER + , RVec3Arg inCenterOfMass +#endif + ); + +/// Determine contact points between 2 faces of 2 shapes and return them in outContactPoints 1 & 2 +/// @param inContactPoint1 The contact point on shape 1 relative to inCenterOfMass +/// @param inContactPoint2 The contact point on shape 2 relative to inCenterOfMass +/// @param inPenetrationAxis The local space penetration axis in world space +/// @param inMaxContactDistanceSq After face 2 is clipped against face 1, each remaining point on face 2 is tested against the plane of face 1. If the distance^2 on the positive side of the plane is larger than this distance, the point will be discarded as a contact point. +/// @param inShape1Face The supporting faces on shape 1 relative to inCenterOfMass +/// @param inShape2Face The supporting faces on shape 2 relative to inCenterOfMass +/// @param outContactPoints1 Returns the contact points between the two shapes for shape 1 relative to inCenterOfMass (any existing points in the output array are left as is) +/// @param outContactPoints2 Returns the contact points between the two shapes for shape 2 relative to inCenterOfMass (any existing points in the output array are left as is) +#ifdef JPH_DEBUG_RENDERER +/// @param inCenterOfMass Center of mass position of body 1 +#endif +JPH_EXPORT void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inPenetrationAxis, float inMaxContactDistanceSq, const ConvexShape::SupportingFace &inShape1Face, const ConvexShape::SupportingFace &inShape2Face, ContactPoints &outContactPoints1, ContactPoints &outContactPoints2 +#ifdef JPH_DEBUG_RENDERER + , RVec3Arg inCenterOfMass +#endif + ); + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.cpp new file mode 100644 index 000000000000..4431a1047e0c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.cpp @@ -0,0 +1,493 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +bool NarrowPhaseQuery::CastRay(const RRayCast &inRay, RayCastResult &ioHit, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter) const +{ + JPH_PROFILE_FUNCTION(); + + class MyCollector : public RayCastBodyCollector + { + public: + MyCollector(const RRayCast &inRay, RayCastResult &ioHit, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter) : + mRay(inRay), + mHit(ioHit), + mBodyLockInterface(inBodyLockInterface), + mBodyFilter(inBodyFilter) + { + UpdateEarlyOutFraction(ioHit.mFraction); + } + + virtual void AddHit(const ResultType &inResult) override + { + JPH_ASSERT(inResult.mFraction < mHit.mFraction, "This hit should not have been passed on to the collector"); + + // Only test shape if it passes the body filter + if (mBodyFilter.ShouldCollide(inResult.mBodyID)) + { + // Lock the body + BodyLockRead lock(mBodyLockInterface, inResult.mBodyID); + if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks + { + const Body &body = lock.GetBody(); + + // Check body filter again now that we've locked the body + if (mBodyFilter.ShouldCollideLocked(body)) + { + // Collect the transformed shape + TransformedShape ts = body.GetTransformedShape(); + + // Release the lock now, we have all the info we need in the transformed shape + lock.ReleaseLock(); + + // Do narrow phase collision check + if (ts.CastRay(mRay, mHit)) + { + // Test that we didn't find a further hit by accident + JPH_ASSERT(mHit.mFraction >= 0.0f && mHit.mFraction < GetEarlyOutFraction()); + + // Update early out fraction based on narrow phase collector + UpdateEarlyOutFraction(mHit.mFraction); + } + } + } + } + } + + RRayCast mRay; + RayCastResult & mHit; + const BodyLockInterface & mBodyLockInterface; + const BodyFilter & mBodyFilter; + }; + + // Do broadphase test, note that the broadphase uses floats so we drop precision here + MyCollector collector(inRay, ioHit, *mBodyLockInterface, inBodyFilter); + mBroadPhaseQuery->CastRay(RayCast(inRay), collector, inBroadPhaseLayerFilter, inObjectLayerFilter); + return ioHit.mFraction <= 1.0f; +} + +void NarrowPhaseQuery::CastRay(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + class MyCollector : public RayCastBodyCollector + { + public: + MyCollector(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) : + RayCastBodyCollector(ioCollector), + mRay(inRay), + mRayCastSettings(inRayCastSettings), + mCollector(ioCollector), + mBodyLockInterface(inBodyLockInterface), + mBodyFilter(inBodyFilter), + mShapeFilter(inShapeFilter) + { + } + + virtual void AddHit(const ResultType &inResult) override + { + JPH_ASSERT(inResult.mFraction < mCollector.GetEarlyOutFraction(), "This hit should not have been passed on to the collector"); + + // Only test shape if it passes the body filter + if (mBodyFilter.ShouldCollide(inResult.mBodyID)) + { + // Lock the body + BodyLockRead lock(mBodyLockInterface, inResult.mBodyID); + if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks + { + const Body &body = lock.GetBody(); + + // Check body filter again now that we've locked the body + if (mBodyFilter.ShouldCollideLocked(body)) + { + // Collect the transformed shape + TransformedShape ts = body.GetTransformedShape(); + + // Notify collector of new body + mCollector.OnBody(body); + + // Release the lock now, we have all the info we need in the transformed shape + lock.ReleaseLock(); + + // Do narrow phase collision check + ts.CastRay(mRay, mRayCastSettings, mCollector, mShapeFilter); + + // Update early out fraction based on narrow phase collector + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + } + } + } + + RRayCast mRay; + RayCastSettings mRayCastSettings; + CastRayCollector & mCollector; + const BodyLockInterface & mBodyLockInterface; + const BodyFilter & mBodyFilter; + const ShapeFilter & mShapeFilter; + }; + + // Do broadphase test, note that the broadphase uses floats so we drop precision here + MyCollector collector(inRay, inRayCastSettings, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter); + mBroadPhaseQuery->CastRay(RayCast(inRay), collector, inBroadPhaseLayerFilter, inObjectLayerFilter); +} + +void NarrowPhaseQuery::CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + class MyCollector : public CollideShapeBodyCollector + { + public: + MyCollector(RVec3Arg inPoint, CollidePointCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) : + CollideShapeBodyCollector(ioCollector), + mPoint(inPoint), + mCollector(ioCollector), + mBodyLockInterface(inBodyLockInterface), + mBodyFilter(inBodyFilter), + mShapeFilter(inShapeFilter) + { + } + + virtual void AddHit(const ResultType &inResult) override + { + // Only test shape if it passes the body filter + if (mBodyFilter.ShouldCollide(inResult)) + { + // Lock the body + BodyLockRead lock(mBodyLockInterface, inResult); + if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks + { + const Body &body = lock.GetBody(); + + // Check body filter again now that we've locked the body + if (mBodyFilter.ShouldCollideLocked(body)) + { + // Collect the transformed shape + TransformedShape ts = body.GetTransformedShape(); + + // Notify collector of new body + mCollector.OnBody(body); + + // Release the lock now, we have all the info we need in the transformed shape + lock.ReleaseLock(); + + // Do narrow phase collision check + ts.CollidePoint(mPoint, mCollector, mShapeFilter); + + // Update early out fraction based on narrow phase collector + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + } + } + } + + RVec3 mPoint; + CollidePointCollector & mCollector; + const BodyLockInterface & mBodyLockInterface; + const BodyFilter & mBodyFilter; + const ShapeFilter & mShapeFilter; + }; + + // Do broadphase test (note: truncates double to single precision since the broadphase uses single precision) + MyCollector collector(inPoint, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter); + mBroadPhaseQuery->CollidePoint(Vec3(inPoint), collector, inBroadPhaseLayerFilter, inObjectLayerFilter); +} + +void NarrowPhaseQuery::CollideShape(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + class MyCollector : public CollideShapeBodyCollector + { + public: + MyCollector(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) : + CollideShapeBodyCollector(ioCollector), + mShape(inShape), + mShapeScale(inShapeScale), + mCenterOfMassTransform(inCenterOfMassTransform), + mCollideShapeSettings(inCollideShapeSettings), + mBaseOffset(inBaseOffset), + mCollector(ioCollector), + mBodyLockInterface(inBodyLockInterface), + mBodyFilter(inBodyFilter), + mShapeFilter(inShapeFilter) + { + } + + virtual void AddHit(const ResultType &inResult) override + { + // Only test shape if it passes the body filter + if (mBodyFilter.ShouldCollide(inResult)) + { + // Lock the body + BodyLockRead lock(mBodyLockInterface, inResult); + if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks + { + const Body &body = lock.GetBody(); + + // Check body filter again now that we've locked the body + if (mBodyFilter.ShouldCollideLocked(body)) + { + // Collect the transformed shape + TransformedShape ts = body.GetTransformedShape(); + + // Notify collector of new body + mCollector.OnBody(body); + + // Release the lock now, we have all the info we need in the transformed shape + lock.ReleaseLock(); + + // Do narrow phase collision check + ts.CollideShape(mShape, mShapeScale, mCenterOfMassTransform, mCollideShapeSettings, mBaseOffset, mCollector, mShapeFilter); + + // Update early out fraction based on narrow phase collector + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + } + } + } + + const Shape * mShape; + Vec3 mShapeScale; + RMat44 mCenterOfMassTransform; + const CollideShapeSettings & mCollideShapeSettings; + RVec3 mBaseOffset; + CollideShapeCollector & mCollector; + const BodyLockInterface & mBodyLockInterface; + const BodyFilter & mBodyFilter; + const ShapeFilter & mShapeFilter; + }; + + // Calculate bounds for shape and expand by max separation distance + AABox bounds = inShape->GetWorldSpaceBounds(inCenterOfMassTransform, inShapeScale); + bounds.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance)); + + // Do broadphase test + MyCollector collector(inShape, inShapeScale, inCenterOfMassTransform, inCollideShapeSettings, inBaseOffset, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter); + mBroadPhaseQuery->CollideAABox(bounds, collector, inBroadPhaseLayerFilter, inObjectLayerFilter); +} + +void NarrowPhaseQuery::CollideShapeWithInternalEdgeRemoval(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + class MyCollector : public CollideShapeBodyCollector + { + public: + MyCollector(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) : + CollideShapeBodyCollector(ioCollector), + mShape(inShape), + mShapeScale(inShapeScale), + mCenterOfMassTransform(inCenterOfMassTransform), + mBaseOffset(inBaseOffset), + mBodyLockInterface(inBodyLockInterface), + mBodyFilter(inBodyFilter), + mShapeFilter(inShapeFilter), + mCollideShapeSettings(inCollideShapeSettings), + mCollector(ioCollector) + { + // We require these settings for internal edge removal to work + mCollideShapeSettings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll; + mCollideShapeSettings.mCollectFacesMode = ECollectFacesMode::CollectFaces; + } + + virtual void AddHit(const ResultType &inResult) override + { + // Only test shape if it passes the body filter + if (mBodyFilter.ShouldCollide(inResult)) + { + // Lock the body + BodyLockRead lock(mBodyLockInterface, inResult); + if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks + { + const Body &body = lock.GetBody(); + + // Check body filter again now that we've locked the body + if (mBodyFilter.ShouldCollideLocked(body)) + { + // Collect the transformed shape + TransformedShape ts = body.GetTransformedShape(); + + // Notify collector of new body + mCollector.OnBody(body); + + // Release the lock now, we have all the info we need in the transformed shape + lock.ReleaseLock(); + + // Do narrow phase collision check + ts.CollideShape(mShape, mShapeScale, mCenterOfMassTransform, mCollideShapeSettings, mBaseOffset, mCollector, mShapeFilter); + + // After each body, we need to flush the InternalEdgeRemovingCollector because it uses 'ts' as context and it will go out of scope at the end of this block + mCollector.Flush(); + + // Update early out fraction based on narrow phase collector + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + } + } + } + + const Shape * mShape; + Vec3 mShapeScale; + RMat44 mCenterOfMassTransform; + RVec3 mBaseOffset; + const BodyLockInterface & mBodyLockInterface; + const BodyFilter & mBodyFilter; + const ShapeFilter & mShapeFilter; + CollideShapeSettings mCollideShapeSettings; + InternalEdgeRemovingCollector mCollector; + }; + + // Calculate bounds for shape and expand by max separation distance + AABox bounds = inShape->GetWorldSpaceBounds(inCenterOfMassTransform, inShapeScale); + bounds.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance)); + + // Do broadphase test + MyCollector collector(inShape, inShapeScale, inCenterOfMassTransform, inCollideShapeSettings, inBaseOffset, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter); + mBroadPhaseQuery->CollideAABox(bounds, collector, inBroadPhaseLayerFilter, inObjectLayerFilter); +} + +void NarrowPhaseQuery::CastShape(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + class MyCollector : public CastShapeBodyCollector + { + public: + MyCollector(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) : + CastShapeBodyCollector(ioCollector), + mShapeCast(inShapeCast), + mShapeCastSettings(inShapeCastSettings), + mBaseOffset(inBaseOffset), + mCollector(ioCollector), + mBodyLockInterface(inBodyLockInterface), + mBodyFilter(inBodyFilter), + mShapeFilter(inShapeFilter) + { + } + + virtual void AddHit(const ResultType &inResult) override + { + JPH_ASSERT(inResult.mFraction <= max(0.0f, mCollector.GetEarlyOutFraction()), "This hit should not have been passed on to the collector"); + + // Only test shape if it passes the body filter + if (mBodyFilter.ShouldCollide(inResult.mBodyID)) + { + // Lock the body + BodyLockRead lock(mBodyLockInterface, inResult.mBodyID); + if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks + { + const Body &body = lock.GetBody(); + + // Check body filter again now that we've locked the body + if (mBodyFilter.ShouldCollideLocked(body)) + { + // Collect the transformed shape + TransformedShape ts = body.GetTransformedShape(); + + // Notify collector of new body + mCollector.OnBody(body); + + // Release the lock now, we have all the info we need in the transformed shape + lock.ReleaseLock(); + + // Do narrow phase collision check + ts.CastShape(mShapeCast, mShapeCastSettings, mBaseOffset, mCollector, mShapeFilter); + + // Update early out fraction based on narrow phase collector + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + } + } + } + + RShapeCast mShapeCast; + const ShapeCastSettings & mShapeCastSettings; + RVec3 mBaseOffset; + CastShapeCollector & mCollector; + const BodyLockInterface & mBodyLockInterface; + const BodyFilter & mBodyFilter; + const ShapeFilter & mShapeFilter; + }; + + // Do broadphase test + MyCollector collector(inShapeCast, inShapeCastSettings, inBaseOffset, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter); + mBroadPhaseQuery->CastAABox({ inShapeCast.mShapeWorldBounds, inShapeCast.mDirection }, collector, inBroadPhaseLayerFilter, inObjectLayerFilter); +} + +void NarrowPhaseQuery::CollectTransformedShapes(const AABox &inBox, TransformedShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + class MyCollector : public CollideShapeBodyCollector + { + public: + MyCollector(const AABox &inBox, TransformedShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) : + CollideShapeBodyCollector(ioCollector), + mBox(inBox), + mCollector(ioCollector), + mBodyLockInterface(inBodyLockInterface), + mBodyFilter(inBodyFilter), + mShapeFilter(inShapeFilter) + { + } + + virtual void AddHit(const ResultType &inResult) override + { + // Only test shape if it passes the body filter + if (mBodyFilter.ShouldCollide(inResult)) + { + // Lock the body + BodyLockRead lock(mBodyLockInterface, inResult); + if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks + { + const Body &body = lock.GetBody(); + + // Check body filter again now that we've locked the body + if (mBodyFilter.ShouldCollideLocked(body)) + { + // Collect the transformed shape + TransformedShape ts = body.GetTransformedShape(); + + // Notify collector of new body + mCollector.OnBody(body); + + // Release the lock now, we have all the info we need in the transformed shape + lock.ReleaseLock(); + + // Do narrow phase collision check + ts.CollectTransformedShapes(mBox, mCollector, mShapeFilter); + + // Update early out fraction based on narrow phase collector + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + } + } + } + + const AABox & mBox; + TransformedShapeCollector & mCollector; + const BodyLockInterface & mBodyLockInterface; + const BodyFilter & mBodyFilter; + const ShapeFilter & mShapeFilter; + }; + + // Do broadphase test + MyCollector collector(inBox, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter); + mBroadPhaseQuery->CollideAABox(inBox, collector, inBroadPhaseLayerFilter, inObjectLayerFilter); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.h new file mode 100644 index 000000000000..087cc6ca3f95 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.h @@ -0,0 +1,77 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class Shape; +class CollideShapeSettings; +class RayCastResult; + +/// Class that provides an interface for doing precise collision detection against the broad and then the narrow phase. +/// Unlike a BroadPhaseQuery, the NarrowPhaseQuery will test against shapes and will return collision information against triangles, spheres etc. +class JPH_EXPORT NarrowPhaseQuery : public NonCopyable +{ +public: + /// Initialize the interface (should only be called by PhysicsSystem) + void Init(BodyLockInterface &inBodyLockInterface, BroadPhaseQuery &inBroadPhaseQuery) { mBodyLockInterface = &inBodyLockInterface; mBroadPhaseQuery = &inBroadPhaseQuery; } + + /// Cast a ray and find the closest hit. Returns true if it finds a hit. Hits further than ioHit.mFraction will not be considered and in this case ioHit will remain unmodified (and the function will return false). + /// Convex objects will be treated as solid (meaning if the ray starts inside, you'll get a hit fraction of 0) and back face hits against triangles are returned. + /// If you want the surface normal of the hit use Body::GetWorldSpaceSurfaceNormal(ioHit.mSubShapeID2, inRay.GetPointOnRay(ioHit.mFraction)) on body with ID ioHit.mBodyID. + bool CastRay(const RRayCast &inRay, RayCastResult &ioHit, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }) const; + + /// Cast a ray, allows collecting multiple hits. Note that this version is more flexible but also slightly slower than the CastRay function that returns only a single hit. + /// If you want the surface normal of the hit use Body::GetWorldSpaceSurfaceNormal(collected sub shape ID, inRay.GetPointOnRay(collected fraction)) on body with collected body ID. + void CastRay(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const; + + /// Check if inPoint is inside any shapes. For this tests all shapes are treated as if they were solid. + /// For a mesh shape, this test will only provide sensible information if the mesh is a closed manifold. + /// For each shape that collides, ioCollector will receive a hit + void CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const; + + /// Collide a shape with the system + /// @param inShape Shape to test + /// @param inShapeScale Scale in local space of shape + /// @param inCenterOfMassTransform Center of mass transform for the shape + /// @param inCollideShapeSettings Settings + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. inCenterOfMassTransform.GetTranslation() since floats are most accurate near the origin + /// @param ioCollector Collector that receives the hits + /// @param inBroadPhaseLayerFilter Filter that filters at broadphase level + /// @param inObjectLayerFilter Filter that filters at layer level + /// @param inBodyFilter Filter that filters at body level + /// @param inShapeFilter Filter that filters at shape level + void CollideShape(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const; + + /// Same as CollideShape, but uses InternalEdgeRemovingCollector to remove internal edges from the collision results (a.k.a. ghost collisions) + void CollideShapeWithInternalEdgeRemoval(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const; + + /// Cast a shape and report any hits to ioCollector + /// @param inShapeCast The shape cast and its position and direction + /// @param inShapeCastSettings Settings for the shape cast + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. inShapeCast.mCenterOfMassStart.GetTranslation() since floats are most accurate near the origin + /// @param ioCollector Collector that receives the hits + /// @param inBroadPhaseLayerFilter Filter that filters at broadphase level + /// @param inObjectLayerFilter Filter that filters at layer level + /// @param inBodyFilter Filter that filters at body level + /// @param inShapeFilter Filter that filters at shape level + void CastShape(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const; + + /// Collect all leaf transformed shapes that fall inside world space box inBox + void CollectTransformedShapes(const AABox &inBox, TransformedShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const; + +private: + BodyLockInterface * mBodyLockInterface = nullptr; + BroadPhaseQuery * mBroadPhaseQuery = nullptr; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseStats.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseStats.cpp new file mode 100644 index 000000000000..69c611372537 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseStats.cpp @@ -0,0 +1,62 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +#ifdef JPH_TRACK_NARROWPHASE_STATS + +JPH_NAMESPACE_BEGIN + +NarrowPhaseStat NarrowPhaseStat::sCollideShape[NumSubShapeTypes][NumSubShapeTypes]; +NarrowPhaseStat NarrowPhaseStat::sCastShape[NumSubShapeTypes][NumSubShapeTypes]; + +thread_local TrackNarrowPhaseStat *TrackNarrowPhaseStat::sRoot = nullptr; + +void NarrowPhaseStat::ReportStats(const char *inName, EShapeSubType inType1, EShapeSubType inType2, uint64 inTicks100Pct) const +{ + double total_pct = 100.0 * double(mTotalTicks) / double(inTicks100Pct); + double total_pct_excl_children = 100.0 * double(mTotalTicks - mChildTicks) / double(inTicks100Pct); + + std::stringstream str; + str << inName << ", " << sSubShapeTypeNames[(int)inType1] << ", " << sSubShapeTypeNames[(int)inType2] << ", " << mNumQueries << ", " << total_pct << ", " << total_pct_excl_children << ", " << total_pct_excl_children / mNumQueries << ", " << mHitsReported; + Trace(str.str().c_str()); +} + +void NarrowPhaseStat::sReportStats() +{ + Trace("Query Type, Shape Type 1, Shape Type 2, Num Queries, Total Time (%%), Total Time Excl Children (%%), Total Time Excl. Children / Query (%%), Hits Reported"); + + uint64 total_ticks = 0; + for (EShapeSubType t1 : sAllSubShapeTypes) + for (EShapeSubType t2 : sAllSubShapeTypes) + { + const NarrowPhaseStat &collide_stat = sCollideShape[(int)t1][(int)t2]; + total_ticks += collide_stat.mTotalTicks - collide_stat.mChildTicks; + + const NarrowPhaseStat &cast_stat = sCastShape[(int)t1][(int)t2]; + total_ticks += cast_stat.mTotalTicks - cast_stat.mChildTicks; + } + + for (EShapeSubType t1 : sAllSubShapeTypes) + for (EShapeSubType t2 : sAllSubShapeTypes) + { + const NarrowPhaseStat &stat = sCollideShape[(int)t1][(int)t2]; + if (stat.mNumQueries > 0) + stat.ReportStats("CollideShape", t1, t2, total_ticks); + } + + for (EShapeSubType t1 : sAllSubShapeTypes) + for (EShapeSubType t2 : sAllSubShapeTypes) + { + const NarrowPhaseStat &stat = sCastShape[(int)t1][(int)t2]; + if (stat.mNumQueries > 0) + stat.ReportStats("CastShape", t1, t2, total_ticks); + } +} + +JPH_NAMESPACE_END + +#endif // JPH_TRACK_NARROWPHASE_STATS diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseStats.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseStats.h new file mode 100644 index 000000000000..813933bb754e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseStats.h @@ -0,0 +1,110 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic") + +// Shorthand function to ifdef out code if narrow phase stats tracking is off +#ifdef JPH_TRACK_NARROWPHASE_STATS + #define JPH_IF_TRACK_NARROWPHASE_STATS(...) __VA_ARGS__ +#else + #define JPH_IF_TRACK_NARROWPHASE_STATS(...) +#endif // JPH_TRACK_NARROWPHASE_STATS + +JPH_SUPPRESS_WARNING_POP + +#ifdef JPH_TRACK_NARROWPHASE_STATS + +JPH_NAMESPACE_BEGIN + +/// Structure that tracks narrow phase timing information for a particular combination of shapes +class NarrowPhaseStat +{ +public: + /// Trace an individual stat in CSV form. + void ReportStats(const char *inName, EShapeSubType inType1, EShapeSubType inType2, uint64 inTicks100Pct) const; + + /// Trace the collected broadphase stats in CSV form. + /// This report can be used to judge and tweak the efficiency of the broadphase. + static void sReportStats(); + + atomic mNumQueries = 0; + atomic mHitsReported = 0; + atomic mTotalTicks = 0; + atomic mChildTicks = 0; + + static NarrowPhaseStat sCollideShape[NumSubShapeTypes][NumSubShapeTypes]; + static NarrowPhaseStat sCastShape[NumSubShapeTypes][NumSubShapeTypes]; +}; + +/// Object that tracks the start and end of a narrow phase operation +class TrackNarrowPhaseStat +{ +public: + TrackNarrowPhaseStat(NarrowPhaseStat &inStat) : + mStat(inStat), + mParent(sRoot), + mStart(GetProcessorTickCount()) + { + // Make this the new root of the chain + sRoot = this; + } + + ~TrackNarrowPhaseStat() + { + uint64 delta_ticks = GetProcessorTickCount() - mStart; + + // Notify parent of time spent in child + if (mParent != nullptr) + mParent->mStat.mChildTicks += delta_ticks; + + // Increment stats at this level + mStat.mNumQueries++; + mStat.mTotalTicks += delta_ticks; + + // Restore root pointer + JPH_ASSERT(sRoot == this); + sRoot = mParent; + } + + NarrowPhaseStat & mStat; + TrackNarrowPhaseStat * mParent; + uint64 mStart; + + static thread_local TrackNarrowPhaseStat *sRoot; +}; + +/// Object that tracks the start and end of a hit being processed by a collision collector +class TrackNarrowPhaseCollector +{ +public: + TrackNarrowPhaseCollector() : + mStart(GetProcessorTickCount()) + { + } + + ~TrackNarrowPhaseCollector() + { + // Mark time spent in collector as 'child' time for the parent + uint64 delta_ticks = GetProcessorTickCount() - mStart; + if (TrackNarrowPhaseStat::sRoot != nullptr) + TrackNarrowPhaseStat::sRoot->mStat.mChildTicks += delta_ticks; + + // Notify all parents of a hit + for (TrackNarrowPhaseStat *track = TrackNarrowPhaseStat::sRoot; track != nullptr; track = track->mParent) + track->mStat.mHitsReported++; + } + +private: + uint64 mStart; +}; + +JPH_NAMESPACE_END + +#endif // JPH_TRACK_NARROWPHASE_STATS diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayer.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayer.h new file mode 100644 index 000000000000..bfb9e6c9a88d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayer.h @@ -0,0 +1,111 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Layer that objects can be in, determines which other objects it can collide with +#ifndef JPH_OBJECT_LAYER_BITS + #define JPH_OBJECT_LAYER_BITS 16 +#endif // JPH_OBJECT_LAYER_BITS +#if JPH_OBJECT_LAYER_BITS == 16 + using ObjectLayer = uint16; +#elif JPH_OBJECT_LAYER_BITS == 32 + using ObjectLayer = uint32; +#else + #error "JPH_OBJECT_LAYER_BITS must be 16 or 32" +#endif + +/// Constant value used to indicate an invalid object layer +static constexpr ObjectLayer cObjectLayerInvalid = ObjectLayer(~ObjectLayer(0U)); + +/// Filter class for object layers +class JPH_EXPORT ObjectLayerFilter : public NonCopyable +{ +public: + /// Destructor + virtual ~ObjectLayerFilter() = default; + + /// Function to filter out object layers when doing collision query test (return true to allow testing against objects with this layer) + virtual bool ShouldCollide([[maybe_unused]] ObjectLayer inLayer) const + { + return true; + } + +#ifdef JPH_TRACK_BROADPHASE_STATS + /// Get a string that describes this filter for stat tracking purposes + virtual String GetDescription() const + { + return "No Description"; + } +#endif // JPH_TRACK_BROADPHASE_STATS +}; + +/// Filter class to test if two objects can collide based on their object layer. Used while finding collision pairs. +class JPH_EXPORT ObjectLayerPairFilter : public NonCopyable +{ +public: + /// Destructor + virtual ~ObjectLayerPairFilter() = default; + + /// Returns true if two layers can collide + virtual bool ShouldCollide([[maybe_unused]] ObjectLayer inLayer1, [[maybe_unused]] ObjectLayer inLayer2) const + { + return true; + } +}; + +/// Default filter class that uses the pair filter in combination with a specified layer to filter layers +class JPH_EXPORT DefaultObjectLayerFilter : public ObjectLayerFilter +{ +public: + /// Constructor + DefaultObjectLayerFilter(const ObjectLayerPairFilter &inObjectLayerPairFilter, ObjectLayer inLayer) : + mObjectLayerPairFilter(inObjectLayerPairFilter), + mLayer(inLayer) + { + } + + /// Copy constructor + DefaultObjectLayerFilter(const DefaultObjectLayerFilter &inRHS) : + mObjectLayerPairFilter(inRHS.mObjectLayerPairFilter), + mLayer(inRHS.mLayer) + { + } + + // See ObjectLayerFilter::ShouldCollide + virtual bool ShouldCollide(ObjectLayer inLayer) const override + { + return mObjectLayerPairFilter.ShouldCollide(mLayer, inLayer); + } + +private: + const ObjectLayerPairFilter & mObjectLayerPairFilter; + ObjectLayer mLayer; +}; + +/// Allows objects from a specific layer only +class JPH_EXPORT SpecifiedObjectLayerFilter : public ObjectLayerFilter +{ +public: + /// Constructor + explicit SpecifiedObjectLayerFilter(ObjectLayer inLayer) : + mLayer(inLayer) + { + } + + // See ObjectLayerFilter::ShouldCollide + virtual bool ShouldCollide(ObjectLayer inLayer) const override + { + return mLayer == inLayer; + } + +private: + ObjectLayer mLayer; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayerPairFilterMask.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayerPairFilterMask.h new file mode 100644 index 000000000000..dc3494c2ee3e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayerPairFilterMask.h @@ -0,0 +1,52 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Filter class to test if two objects can collide based on their object layer. Used while finding collision pairs. +/// Uses group bits and mask bits. Two layers can collide if Object1.Group & Object2.Mask is non-zero and Object2.Group & Object1.Mask is non-zero. +/// The behavior is similar to that in e.g. Bullet. +/// This implementation works together with BroadPhaseLayerInterfaceMask and ObjectVsBroadPhaseLayerFilterMask +class ObjectLayerPairFilterMask : public ObjectLayerPairFilter +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Number of bits for the group and mask bits + static constexpr uint32 cNumBits = JPH_OBJECT_LAYER_BITS / 2; + static constexpr uint32 cMask = (1 << cNumBits) - 1; + + /// Construct an ObjectLayer from a group and mask bits + static ObjectLayer sGetObjectLayer(uint32 inGroup, uint32 inMask = cMask) + { + JPH_ASSERT((inGroup & ~cMask) == 0); + JPH_ASSERT((inMask & ~cMask) == 0); + return ObjectLayer((inGroup & cMask) | (inMask << cNumBits)); + } + + /// Get the group bits from an ObjectLayer + static inline uint32 sGetGroup(ObjectLayer inObjectLayer) + { + return uint32(inObjectLayer) & cMask; + } + + /// Get the mask bits from an ObjectLayer + static inline uint32 sGetMask(ObjectLayer inObjectLayer) + { + return uint32(inObjectLayer) >> cNumBits; + } + + /// Returns true if two layers can collide + virtual bool ShouldCollide(ObjectLayer inObject1, ObjectLayer inObject2) const override + { + return (sGetGroup(inObject1) & sGetMask(inObject2)) != 0 + && (sGetGroup(inObject2) & sGetMask(inObject1)) != 0; + } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayerPairFilterTable.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayerPairFilterTable.h new file mode 100644 index 000000000000..cb17f3c5dae0 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayerPairFilterTable.h @@ -0,0 +1,78 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Filter class to test if two objects can collide based on their object layer. Used while finding collision pairs. +/// This implementation uses a table to determine if two layers can collide. +class ObjectLayerPairFilterTable : public ObjectLayerPairFilter +{ +private: + /// Get which bit corresponds to the pair (inLayer1, inLayer2) + uint GetBit(ObjectLayer inLayer1, ObjectLayer inLayer2) const + { + // We store the lower left half only, so swap the inputs when trying to access the top right half + if (inLayer1 > inLayer2) + std::swap(inLayer1, inLayer2); + + JPH_ASSERT(inLayer2 < mNumObjectLayers); + + // Calculate at which bit the entry for this pair resides + // We use the fact that a row always starts at inLayer2 * (inLayer2 + 1) / 2 + // (this is the amount of bits needed to store a table of inLayer2 entries) + return (inLayer2 * (inLayer2 + 1)) / 2 + inLayer1; + } + +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructs the table with inNumObjectLayers Layers, initially all layer pairs are disabled + explicit ObjectLayerPairFilterTable(uint inNumObjectLayers) : + mNumObjectLayers(inNumObjectLayers) + { + // By default nothing collides + // For the first layer we only need to store 1 bit, for the second 2 bits, for the third 3 bits, etc. + // We use the formula Sum_i=1^N i = N * (N + 1) / 2 to calculate the size of the table + int table_size = (inNumObjectLayers * (inNumObjectLayers + 1) / 2 + 7) / 8; + mTable.resize(table_size, 0); + } + + /// Get the number of object layers + uint GetNumObjectLayers() const + { + return mNumObjectLayers; + } + + /// Disable collision between two object layers + void DisableCollision(ObjectLayer inLayer1, ObjectLayer inLayer2) + { + uint bit = GetBit(inLayer1, inLayer2); + mTable[bit >> 3] &= (0xff ^ (1 << (bit & 0b111))); + } + + /// Enable collision between two object layers + void EnableCollision(ObjectLayer inLayer1, ObjectLayer inLayer2) + { + uint bit = GetBit(inLayer1, inLayer2); + mTable[bit >> 3] |= 1 << (bit & 0b111); + } + + /// Returns true if two layers can collide + virtual bool ShouldCollide(ObjectLayer inObject1, ObjectLayer inObject2) const override + { + // Test if the bit is set for this group pair + uint bit = GetBit(inObject1, inObject2); + return (mTable[bit >> 3] & (1 << (bit & 0b111))) != 0; + } + +private: + uint mNumObjectLayers; ///< The number of layers that this table supports + Array mTable; ///< The table of bits that indicates which layers collide +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.cpp new file mode 100644 index 000000000000..17a982e74992 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.cpp @@ -0,0 +1,35 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +RefConst PhysicsMaterial::sDefault; + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PhysicsMaterial) +{ + JPH_ADD_BASE_CLASS(PhysicsMaterial, SerializableObject) +} + +void PhysicsMaterial::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(GetRTTI()->GetHash()); +} + +void PhysicsMaterial::RestoreBinaryState(StreamIn &inStream) +{ + // RTTI hash is read in sRestoreFromBinaryState +} + +PhysicsMaterial::PhysicsMaterialResult PhysicsMaterial::sRestoreFromBinaryState(StreamIn &inStream) +{ + return StreamUtils::RestoreObject(inStream, &PhysicsMaterial::RestoreBinaryState); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.h new file mode 100644 index 000000000000..d13c9644b6f8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.h @@ -0,0 +1,52 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; + +/// This structure describes the surface of (part of) a shape. You should inherit from it to define additional +/// information that is interesting for the simulation. The 2 materials involved in a contact could be used +/// to decide which sound or particle effects to play. +/// +/// If you inherit from this material, don't forget to create a suitable default material in sDefault +class JPH_EXPORT PhysicsMaterial : public SerializableObject, public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PhysicsMaterial) + +public: + /// Virtual destructor + virtual ~PhysicsMaterial() override = default; + + /// Default material that is used when a shape has no materials defined + static RefConst sDefault; + + // Properties + virtual const char * GetDebugName() const { return "Unknown"; } + virtual Color GetDebugColor() const { return Color::sGrey; } + + /// Saves the contents of the material in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const; + + using PhysicsMaterialResult = Result>; + + /// Creates a PhysicsMaterial of the correct type and restores its contents from the binary stream inStream. + static PhysicsMaterialResult sRestoreFromBinaryState(StreamIn &inStream); + +protected: + /// This function should not be called directly, it is used by sRestoreFromBinaryState. + virtual void RestoreBinaryState(StreamIn &inStream); +}; + +using PhysicsMaterialList = Array>; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterialSimple.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterialSimple.cpp new file mode 100644 index 000000000000..02a569822c63 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterialSimple.cpp @@ -0,0 +1,38 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PhysicsMaterialSimple) +{ + JPH_ADD_BASE_CLASS(PhysicsMaterialSimple, PhysicsMaterial) + + JPH_ADD_ATTRIBUTE(PhysicsMaterialSimple, mDebugName) + JPH_ADD_ATTRIBUTE(PhysicsMaterialSimple, mDebugColor) +} + +void PhysicsMaterialSimple::SaveBinaryState(StreamOut &inStream) const +{ + PhysicsMaterial::SaveBinaryState(inStream); + + inStream.Write(mDebugName); + inStream.Write(mDebugColor); +} + +void PhysicsMaterialSimple::RestoreBinaryState(StreamIn &inStream) +{ + PhysicsMaterial::RestoreBinaryState(inStream); + + inStream.Read(mDebugName); + inStream.Read(mDebugColor); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterialSimple.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterialSimple.h new file mode 100644 index 000000000000..63ffff72d6ef --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterialSimple.h @@ -0,0 +1,37 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Sample implementation of PhysicsMaterial that just holds the needed properties directly +class JPH_EXPORT PhysicsMaterialSimple : public PhysicsMaterial +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PhysicsMaterialSimple) + +public: + /// Constructor + PhysicsMaterialSimple() = default; + PhysicsMaterialSimple(const string_view &inName, ColorArg inColor) : mDebugName(inName), mDebugColor(inColor) { } + + // Properties + virtual const char * GetDebugName() const override { return mDebugName.c_str(); } + virtual Color GetDebugColor() const override { return mDebugColor; } + + // See: PhysicsMaterial::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + +protected: + // See: PhysicsMaterial::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + String mDebugName; ///< Name of the material, used for debugging purposes + Color mDebugColor = Color::sGrey; ///< Color of the material, used to render the shapes +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/RayCast.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/RayCast.h new file mode 100644 index 000000000000..c0a7ea66a618 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/RayCast.h @@ -0,0 +1,87 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Structure that holds a single ray cast +template +struct RayCastT +{ + JPH_OVERRIDE_NEW_DELETE + + /// Constructors + RayCastT() = default; // Allow raycast to be created uninitialized + RayCastT(typename Vec::ArgType inOrigin, Vec3Arg inDirection) : mOrigin(inOrigin), mDirection(inDirection) { } + RayCastT(const RayCastT &) = default; + + /// Transform this ray using inTransform + RayCastType Transformed(typename Mat::ArgType inTransform) const + { + Vec ray_origin = inTransform * mOrigin; + Vec3 ray_direction(inTransform * (mOrigin + mDirection) - ray_origin); + return { ray_origin, ray_direction }; + } + + /// Translate ray using inTranslation + RayCastType Translated(typename Vec::ArgType inTranslation) const + { + return { inTranslation + mOrigin, mDirection }; + } + + /// Get point with fraction inFraction on ray (0 = start of ray, 1 = end of ray) + inline Vec GetPointOnRay(float inFraction) const + { + return mOrigin + inFraction * mDirection; + } + + Vec mOrigin; ///< Origin of the ray + Vec3 mDirection; ///< Direction and length of the ray (anything beyond this length will not be reported as a hit) +}; + +struct RayCast : public RayCastT +{ + using RayCastT::RayCastT; +}; + +struct RRayCast : public RayCastT +{ + using RayCastT::RayCastT; + + /// Convert from RayCast, converts single to double precision + explicit RRayCast(const RayCast &inRay) : + RRayCast(RVec3(inRay.mOrigin), inRay.mDirection) + { + } + + /// Convert to RayCast, which implies casting from double precision to single precision + explicit operator RayCast() const + { + return RayCast(Vec3(mOrigin), mDirection); + } +}; + +/// Settings to be passed with a ray cast +class RayCastSettings +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Set the backfacing mode for all shapes + void SetBackFaceMode(EBackFaceMode inMode) { mBackFaceModeTriangles = mBackFaceModeConvex = inMode; } + + /// How backfacing triangles should be treated (should we report back facing hits for triangle based shapes, e.g. MeshShape/HeightFieldShape?) + EBackFaceMode mBackFaceModeTriangles = EBackFaceMode::IgnoreBackFaces; + + /// How backfacing convex objects should be treated (should we report back facing hits for convex shapes?) + EBackFaceMode mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces; + + /// If convex shapes should be treated as solid. When true, a ray starting inside a convex shape will generate a hit at fraction 0. + bool mTreatConvexAsSolid = true; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.cpp new file mode 100644 index 000000000000..dc7ea4828575 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.cpp @@ -0,0 +1,312 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(BoxShapeSettings) +{ + JPH_ADD_BASE_CLASS(BoxShapeSettings, ConvexShapeSettings) + + JPH_ADD_ATTRIBUTE(BoxShapeSettings, mHalfExtent) + JPH_ADD_ATTRIBUTE(BoxShapeSettings, mConvexRadius) +} + +static const Vec3 sUnitBoxTriangles[] = { + Vec3(-1, 1, -1), Vec3(-1, 1, 1), Vec3(1, 1, 1), + Vec3(-1, 1, -1), Vec3(1, 1, 1), Vec3(1, 1, -1), + Vec3(-1, -1, -1), Vec3(1, -1, -1), Vec3(1, -1, 1), + Vec3(-1, -1, -1), Vec3(1, -1, 1), Vec3(-1, -1, 1), + Vec3(-1, 1, -1), Vec3(-1, -1, -1), Vec3(-1, -1, 1), + Vec3(-1, 1, -1), Vec3(-1, -1, 1), Vec3(-1, 1, 1), + Vec3(1, 1, 1), Vec3(1, -1, 1), Vec3(1, -1, -1), + Vec3(1, 1, 1), Vec3(1, -1, -1), Vec3(1, 1, -1), + Vec3(-1, 1, 1), Vec3(-1, -1, 1), Vec3(1, -1, 1), + Vec3(-1, 1, 1), Vec3(1, -1, 1), Vec3(1, 1, 1), + Vec3(-1, 1, -1), Vec3(1, 1, -1), Vec3(1, -1, -1), + Vec3(-1, 1, -1), Vec3(1, -1, -1), Vec3(-1, -1, -1) +}; + +ShapeSettings::ShapeResult BoxShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new BoxShape(*this, mCachedResult); + return mCachedResult; +} + +BoxShape::BoxShape(const BoxShapeSettings &inSettings, ShapeResult &outResult) : + ConvexShape(EShapeSubType::Box, inSettings, outResult), + mHalfExtent(inSettings.mHalfExtent), + mConvexRadius(inSettings.mConvexRadius) +{ + // Check convex radius + if (inSettings.mConvexRadius < 0.0f + || inSettings.mHalfExtent.ReduceMin() <= inSettings.mConvexRadius) + { + outResult.SetError("Invalid convex radius"); + return; + } + + // Result is valid + outResult.Set(this); +} + +class BoxShape::Box final : public Support +{ +public: + Box(const AABox &inBox, float inConvexRadius) : + mBox(inBox), + mConvexRadius(inConvexRadius) + { + static_assert(sizeof(Box) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(Box))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + return mBox.GetSupport(inDirection); + } + + virtual float GetConvexRadius() const override + { + return mConvexRadius; + } + +private: + AABox mBox; + float mConvexRadius; +}; + +const ConvexShape::Support *BoxShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const +{ + // Scale our half extents + Vec3 scaled_half_extent = inScale.Abs() * mHalfExtent; + + switch (inMode) + { + case ESupportMode::IncludeConvexRadius: + case ESupportMode::Default: + { + // Make box out of our half extents + AABox box = AABox(-scaled_half_extent, scaled_half_extent); + JPH_ASSERT(box.IsValid()); + return new (&inBuffer) Box(box, 0.0f); + } + + case ESupportMode::ExcludeConvexRadius: + { + // Reduce the box by our convex radius + float convex_radius = ScaleHelpers::ScaleConvexRadius(mConvexRadius, inScale); + Vec3 convex_radius3 = Vec3::sReplicate(convex_radius); + Vec3 reduced_half_extent = scaled_half_extent - convex_radius3; + AABox box = AABox(-reduced_half_extent, reduced_half_extent); + JPH_ASSERT(box.IsValid()); + return new (&inBuffer) Box(box, convex_radius); + } + } + + JPH_ASSERT(false); + return nullptr; +} + +void BoxShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + Vec3 scaled_half_extent = inScale.Abs() * mHalfExtent; + AABox box(-scaled_half_extent, scaled_half_extent); + box.GetSupportingFace(inDirection, outVertices); + + // Transform to world space + for (Vec3 &v : outVertices) + v = inCenterOfMassTransform * v; +} + +MassProperties BoxShape::GetMassProperties() const +{ + MassProperties p; + p.SetMassAndInertiaOfSolidBox(2.0f * mHalfExtent, GetDensity()); + return p; +} + +Vec3 BoxShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + // Get component that is closest to the surface of the box + int index = (inLocalSurfacePosition.Abs() - mHalfExtent).Abs().GetLowestComponentIndex(); + + // Calculate normal + Vec3 normal = Vec3::sZero(); + normal.SetComponent(index, inLocalSurfacePosition[index] > 0.0f? 1.0f : -1.0f); + return normal; +} + +#ifdef JPH_DEBUG_RENDERER +void BoxShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + inRenderer->DrawBox(inCenterOfMassTransform * Mat44::sScale(inScale.Abs()), GetLocalBounds(), inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode); +} +#endif // JPH_DEBUG_RENDERER + +bool BoxShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + // Test hit against box + float fraction = max(RayAABox(inRay.mOrigin, RayInvDirection(inRay.mDirection), -mHalfExtent, mHalfExtent), 0.0f); + if (fraction < ioHit.mFraction) + { + ioHit.mFraction = fraction; + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + return false; +} + +void BoxShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + float min_fraction, max_fraction; + RayAABox(inRay.mOrigin, RayInvDirection(inRay.mDirection), -mHalfExtent, mHalfExtent, min_fraction, max_fraction); + if (min_fraction <= max_fraction // Ray should intersect + && max_fraction >= 0.0f // End of ray should be inside box + && min_fraction < ioCollector.GetEarlyOutFraction()) // Start of ray should be before early out fraction + { + // Better hit than the current hit + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + hit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + + // Check front side + if (inRayCastSettings.mTreatConvexAsSolid || min_fraction > 0.0f) + { + hit.mFraction = max(0.0f, min_fraction); + ioCollector.AddHit(hit); + } + + // Check back side hit + if (inRayCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces + && max_fraction < ioCollector.GetEarlyOutFraction()) + { + hit.mFraction = max_fraction; + ioCollector.AddHit(hit); + } + } +} + +void BoxShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + if (Vec3::sLessOrEqual(inPoint.Abs(), mHalfExtent).TestAllXYZTrue()) + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); +} + +void BoxShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation(); + Vec3 half_extent = inScale.Abs() * mHalfExtent; + + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) + { + // Convert to local space + Vec3 local_pos = inverse_transform * v.GetPosition(); + + // Clamp point to inside box + Vec3 clamped_point = Vec3::sMax(Vec3::sMin(local_pos, half_extent), -half_extent); + + // Test if point was inside + if (clamped_point == local_pos) + { + // Calculate closest distance to surface + Vec3 delta = half_extent - local_pos.Abs(); + int index = delta.GetLowestComponentIndex(); + float penetration = delta[index]; + if (v.UpdatePenetration(penetration)) + { + // Calculate contact point and normal + Vec3 possible_normals[] = { Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ() }; + Vec3 normal = local_pos.GetSign() * possible_normals[index]; + Vec3 point = normal * half_extent; + + // Store collision + v.SetCollision(Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); + } + } + else + { + // Calculate normal + Vec3 normal = local_pos - clamped_point; + float normal_length = normal.Length(); + + // Penetration will be negative since we're not penetrating + float penetration = -normal_length; + if (v.UpdatePenetration(penetration)) + { + normal /= normal_length; + + // Store collision + v.SetCollision(Plane::sFromPointAndNormal(clamped_point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); + } + } + } +} + +void BoxShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + new (&ioContext) GetTrianglesContextVertexList(inPositionCOM, inRotation, inScale, Mat44::sScale(mHalfExtent), sUnitBoxTriangles, sizeof(sUnitBoxTriangles) / sizeof(Vec3), GetMaterial()); +} + +int BoxShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + return ((GetTrianglesContextVertexList &)ioContext).GetTrianglesNext(inMaxTrianglesRequested, outTriangleVertices, outMaterials); +} + +void BoxShape::SaveBinaryState(StreamOut &inStream) const +{ + ConvexShape::SaveBinaryState(inStream); + + inStream.Write(mHalfExtent); + inStream.Write(mConvexRadius); +} + +void BoxShape::RestoreBinaryState(StreamIn &inStream) +{ + ConvexShape::RestoreBinaryState(inStream); + + inStream.Read(mHalfExtent); + inStream.Read(mConvexRadius); +} + +void BoxShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Box); + f.mConstruct = []() -> Shape * { return new BoxShape; }; + f.mColor = Color::sGreen; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.h new file mode 100644 index 000000000000..030dd2db803e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.h @@ -0,0 +1,115 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a BoxShape +class JPH_EXPORT BoxShapeSettings final : public ConvexShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, BoxShapeSettings) + +public: + /// Default constructor for deserialization + BoxShapeSettings() = default; + + /// Create a box with half edge length inHalfExtent and convex radius inConvexRadius. + /// (internally the convex radius will be subtracted from the half extent so the total box will not grow with the convex radius). + BoxShapeSettings(Vec3Arg inHalfExtent, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mHalfExtent(inHalfExtent), mConvexRadius(inConvexRadius) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + Vec3 mHalfExtent = Vec3::sZero(); ///< Half the size of the box (including convex radius) + float mConvexRadius = 0.0f; +}; + +/// A box, centered around the origin +class JPH_EXPORT BoxShape final : public ConvexShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + BoxShape() : ConvexShape(EShapeSubType::Box) { } + BoxShape(const BoxShapeSettings &inSettings, ShapeResult &outResult); + + /// Create a box with half edge length inHalfExtent and convex radius inConvexRadius. + /// (internally the convex radius will be subtracted from the half extent so the total box will not grow with the convex radius). + BoxShape(Vec3Arg inHalfExtent, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShape(EShapeSubType::Box, inMaterial), mHalfExtent(inHalfExtent), mConvexRadius(inConvexRadius) { JPH_ASSERT(inConvexRadius >= 0.0f); JPH_ASSERT(inHalfExtent.ReduceMin() >= inConvexRadius); } + + /// Get half extent of box + Vec3 GetHalfExtent() const { return mHalfExtent; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override { return AABox(-mHalfExtent, mHalfExtent); } + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mHalfExtent.ReduceMin(); } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See ConvexShape::GetSupportFunction + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 12); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return GetLocalBounds().GetVolume(); } + + /// Get the convex radius of this box + float GetConvexRadius() const { return mConvexRadius; } + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Class for GetSupportFunction + class Box; + + Vec3 mHalfExtent = Vec3::sZero(); ///< Half the size of the box (including convex radius) + float mConvexRadius = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CapsuleShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CapsuleShape.cpp new file mode 100644 index 000000000000..48a6cba3e623 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CapsuleShape.cpp @@ -0,0 +1,438 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(CapsuleShapeSettings) +{ + JPH_ADD_BASE_CLASS(CapsuleShapeSettings, ConvexShapeSettings) + + JPH_ADD_ATTRIBUTE(CapsuleShapeSettings, mRadius) + JPH_ADD_ATTRIBUTE(CapsuleShapeSettings, mHalfHeightOfCylinder) +} + +static const int cCapsuleDetailLevel = 2; + +static const StaticArray sCapsuleTopTriangles = []() { + StaticArray verts; + GetTrianglesContextVertexList::sCreateHalfUnitSphereTop(verts, cCapsuleDetailLevel); + return verts; +}(); + +static const StaticArray sCapsuleMiddleTriangles = []() { + StaticArray verts; + GetTrianglesContextVertexList::sCreateUnitOpenCylinder(verts, cCapsuleDetailLevel); + return verts; +}(); + +static const StaticArray sCapsuleBottomTriangles = []() { + StaticArray verts; + GetTrianglesContextVertexList::sCreateHalfUnitSphereBottom(verts, cCapsuleDetailLevel); + return verts; +}(); + +ShapeSettings::ShapeResult CapsuleShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + { + Ref shape; + if (IsValid() && IsSphere()) + { + // If the capsule has no height, use a sphere instead + shape = new SphereShape(mRadius, mMaterial); + mCachedResult.Set(shape); + } + else + shape = new CapsuleShape(*this, mCachedResult); + } + return mCachedResult; +} + +CapsuleShape::CapsuleShape(const CapsuleShapeSettings &inSettings, ShapeResult &outResult) : + ConvexShape(EShapeSubType::Capsule, inSettings, outResult), + mRadius(inSettings.mRadius), + mHalfHeightOfCylinder(inSettings.mHalfHeightOfCylinder) +{ + if (inSettings.mHalfHeightOfCylinder <= 0.0f) + { + outResult.SetError("Invalid height"); + return; + } + + if (inSettings.mRadius <= 0.0f) + { + outResult.SetError("Invalid radius"); + return; + } + + outResult.Set(this); +} + +class CapsuleShape::CapsuleNoConvex final : public Support +{ +public: + CapsuleNoConvex(Vec3Arg inHalfHeightOfCylinder, float inConvexRadius) : + mHalfHeightOfCylinder(inHalfHeightOfCylinder), + mConvexRadius(inConvexRadius) + { + static_assert(sizeof(CapsuleNoConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(CapsuleNoConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + if (inDirection.GetY() > 0) + return mHalfHeightOfCylinder; + else + return -mHalfHeightOfCylinder; + } + + virtual float GetConvexRadius() const override + { + return mConvexRadius; + } + +private: + Vec3 mHalfHeightOfCylinder; + float mConvexRadius; +}; + +class CapsuleShape::CapsuleWithConvex final : public Support +{ +public: + CapsuleWithConvex(Vec3Arg inHalfHeightOfCylinder, float inRadius) : + mHalfHeightOfCylinder(inHalfHeightOfCylinder), + mRadius(inRadius) + { + static_assert(sizeof(CapsuleWithConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(CapsuleWithConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + float len = inDirection.Length(); + Vec3 radius = len > 0.0f? inDirection * (mRadius / len) : Vec3::sZero(); + + if (inDirection.GetY() > 0) + return radius + mHalfHeightOfCylinder; + else + return radius - mHalfHeightOfCylinder; + } + + virtual float GetConvexRadius() const override + { + return 0.0f; + } + +private: + Vec3 mHalfHeightOfCylinder; + float mRadius; +}; + +const ConvexShape::Support *CapsuleShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + // Get scaled capsule + Vec3 abs_scale = inScale.Abs(); + float scale = abs_scale.GetX(); + Vec3 scaled_half_height_of_cylinder = Vec3(0, scale * mHalfHeightOfCylinder, 0); + float scaled_radius = scale * mRadius; + + switch (inMode) + { + case ESupportMode::IncludeConvexRadius: + return new (&inBuffer) CapsuleWithConvex(scaled_half_height_of_cylinder, scaled_radius); + + case ESupportMode::ExcludeConvexRadius: + case ESupportMode::Default: + return new (&inBuffer) CapsuleNoConvex(scaled_half_height_of_cylinder, scaled_radius); + } + + JPH_ASSERT(false); + return nullptr; +} + +void CapsuleShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + JPH_ASSERT(IsValidScale(inScale)); + + // Get direction in horizontal plane + Vec3 direction = inDirection; + direction.SetComponent(1, 0.0f); + + // Check zero vector, in this case we're hitting from top/bottom so there's no supporting face + float len = direction.Length(); + if (len == 0.0f) + return; + + // Get scaled capsule + Vec3 abs_scale = inScale.Abs(); + float scale = abs_scale.GetX(); + Vec3 scaled_half_height_of_cylinder = Vec3(0, scale * mHalfHeightOfCylinder, 0); + float scaled_radius = scale * mRadius; + + // Get support point for top and bottom sphere in the opposite of 'direction' (including convex radius) + Vec3 support = (scaled_radius / len) * direction; + Vec3 support_top = scaled_half_height_of_cylinder - support; + Vec3 support_bottom = -scaled_half_height_of_cylinder - support; + + // Get projection on inDirection + // Note that inDirection is not normalized, so we need to divide by inDirection.Length() to get the actual projection + // We've multiplied both sides of the if below with inDirection.Length() + float proj_top = support_top.Dot(inDirection); + float proj_bottom = support_bottom.Dot(inDirection); + + // If projection is roughly equal then return line, otherwise we return nothing as there's only 1 point + if (abs(proj_top - proj_bottom) < cCapsuleProjectionSlop * inDirection.Length()) + { + outVertices.push_back(inCenterOfMassTransform * support_top); + outVertices.push_back(inCenterOfMassTransform * support_bottom); + } +} + +MassProperties CapsuleShape::GetMassProperties() const +{ + MassProperties p; + + float density = GetDensity(); + + // Calculate inertia and mass according to: + // https://www.gamedev.net/resources/_/technical/math-and-physics/capsule-inertia-tensor-r3856 + // Note that there is an error in eq 14, H^2/2 should be H^2/4 in Ixx and Izz, eq 12 does contain the correct value + float radius_sq = Square(mRadius); + float height = 2.0f * mHalfHeightOfCylinder; + float cylinder_mass = JPH_PI * height * radius_sq * density; + float hemisphere_mass = (2.0f * JPH_PI / 3.0f) * radius_sq * mRadius * density; + + // From cylinder + float height_sq = Square(height); + float inertia_y = radius_sq * cylinder_mass * 0.5f; + float inertia_xz = inertia_y * 0.5f + cylinder_mass * height_sq / 12.0f; + + // From hemispheres + float temp = hemisphere_mass * 4.0f * radius_sq / 5.0f; + inertia_y += temp; + inertia_xz += temp + hemisphere_mass * (0.5f * height_sq + (3.0f / 4.0f) * height * mRadius); + + // Mass is cylinder + hemispheres + p.mMass = cylinder_mass + hemisphere_mass * 2.0f; + + // Set inertia + p.mInertia = Mat44::sScale(Vec3(inertia_xz, inertia_y, inertia_xz)); + + return p; +} + +Vec3 CapsuleShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + if (inLocalSurfacePosition.GetY() > mHalfHeightOfCylinder) + return (inLocalSurfacePosition - Vec3(0, mHalfHeightOfCylinder, 0)).Normalized(); + else if (inLocalSurfacePosition.GetY() < -mHalfHeightOfCylinder) + return (inLocalSurfacePosition - Vec3(0, -mHalfHeightOfCylinder, 0)).Normalized(); + else + return Vec3(inLocalSurfacePosition.GetX(), 0, inLocalSurfacePosition.GetZ()).NormalizedOr(Vec3::sAxisX()); +} + +AABox CapsuleShape::GetLocalBounds() const +{ + Vec3 extent = Vec3::sReplicate(mRadius) + Vec3(0, mHalfHeightOfCylinder, 0); + return AABox(-extent, extent); +} + +AABox CapsuleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Vec3 abs_scale = inScale.Abs(); + float scale = abs_scale.GetX(); + Vec3 extent = Vec3::sReplicate(scale * mRadius); + Vec3 height = Vec3(0, scale * mHalfHeightOfCylinder, 0); + Vec3 p1 = inCenterOfMassTransform * -height; + Vec3 p2 = inCenterOfMassTransform * height; + return AABox(Vec3::sMin(p1, p2) - extent, Vec3::sMax(p1, p2) + extent); +} + +#ifdef JPH_DEBUG_RENDERER +void CapsuleShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + inRenderer->DrawCapsule(inCenterOfMassTransform * Mat44::sScale(inScale.Abs().GetX()), mHalfHeightOfCylinder, mRadius, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode); +} +#endif // JPH_DEBUG_RENDERER + +bool CapsuleShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + // Test ray against capsule + float fraction = RayCapsule(inRay.mOrigin, inRay.mDirection, mHalfHeightOfCylinder, mRadius); + if (fraction < ioHit.mFraction) + { + ioHit.mFraction = fraction; + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + return false; +} + +void CapsuleShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + float radius_sq = Square(mRadius); + + // Get vertical distance to the top/bottom sphere centers + float delta_y = abs(inPoint.GetY()) - mHalfHeightOfCylinder; + + // Get distance in horizontal plane + float xz_sq = Square(inPoint.GetX()) + Square(inPoint.GetZ()); + + // Check if the point is in one of the two spheres + bool in_sphere = xz_sq + Square(delta_y) <= radius_sq; + + // Check if the point is in the cylinder in the middle + bool in_cylinder = delta_y <= 0.0f && xz_sq <= radius_sq; + + if (in_sphere || in_cylinder) + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); +} + +void CapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation(); + + // Get scaled capsule + float scale = abs(inScale.GetX()); + float half_height_of_cylinder = scale * mHalfHeightOfCylinder; + float radius = scale * mRadius; + + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) + { + // Calculate penetration + Vec3 local_pos = inverse_transform * v.GetPosition(); + if (abs(local_pos.GetY()) <= half_height_of_cylinder) + { + // Near cylinder + Vec3 normal = local_pos; + normal.SetY(0.0f); + float normal_length = normal.Length(); + float penetration = radius - normal_length; + if (v.UpdatePenetration(penetration)) + { + // Calculate contact point and normal + normal = normal_length > 0.0f? normal / normal_length : Vec3::sAxisX(); + Vec3 point = radius * normal; + + // Store collision + v.SetCollision(Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); + } + } + else + { + // Near cap + Vec3 center = Vec3(0, Sign(local_pos.GetY()) * half_height_of_cylinder, 0); + Vec3 delta = local_pos - center; + float distance = delta.Length(); + float penetration = radius - distance; + if (v.UpdatePenetration(penetration)) + { + // Calculate contact point and normal + Vec3 normal = delta / distance; + Vec3 point = center + radius * normal; + + // Store collision + v.SetCollision(Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); + } + } + } +} + +void CapsuleShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Vec3 abs_scale = inScale.Abs(); + float scale = abs_scale.GetX(); + + GetTrianglesContextMultiVertexList *context = new (&ioContext) GetTrianglesContextMultiVertexList(false, GetMaterial()); + + Mat44 world_matrix = Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(scale); + + Mat44 top_matrix = world_matrix * Mat44(Vec4(mRadius, 0, 0, 0), Vec4(0, mRadius, 0, 0), Vec4(0, 0, mRadius, 0), Vec4(0, mHalfHeightOfCylinder, 0, 1)); + context->AddPart(top_matrix, sCapsuleTopTriangles.data(), sCapsuleTopTriangles.size()); + + Mat44 middle_matrix = world_matrix * Mat44::sScale(Vec3(mRadius, mHalfHeightOfCylinder, mRadius)); + context->AddPart(middle_matrix, sCapsuleMiddleTriangles.data(), sCapsuleMiddleTriangles.size()); + + Mat44 bottom_matrix = world_matrix * Mat44(Vec4(mRadius, 0, 0, 0), Vec4(0, mRadius, 0, 0), Vec4(0, 0, mRadius, 0), Vec4(0, -mHalfHeightOfCylinder, 0, 1)); + context->AddPart(bottom_matrix, sCapsuleBottomTriangles.data(), sCapsuleBottomTriangles.size()); +} + +int CapsuleShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + return ((GetTrianglesContextMultiVertexList &)ioContext).GetTrianglesNext(inMaxTrianglesRequested, outTriangleVertices, outMaterials); +} + +void CapsuleShape::SaveBinaryState(StreamOut &inStream) const +{ + ConvexShape::SaveBinaryState(inStream); + + inStream.Write(mRadius); + inStream.Write(mHalfHeightOfCylinder); +} + +void CapsuleShape::RestoreBinaryState(StreamIn &inStream) +{ + ConvexShape::RestoreBinaryState(inStream); + + inStream.Read(mRadius); + inStream.Read(mHalfHeightOfCylinder); +} + +bool CapsuleShape::IsValidScale(Vec3Arg inScale) const +{ + return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScale(inScale.Abs()); +} + +Vec3 CapsuleShape::MakeScaleValid(Vec3Arg inScale) const +{ + Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale); + + return scale.GetSign() * ScaleHelpers::MakeUniformScale(scale.Abs()); +} + +void CapsuleShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Capsule); + f.mConstruct = []() -> Shape * { return new CapsuleShape; }; + f.mColor = Color::sGreen; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CapsuleShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CapsuleShape.h new file mode 100644 index 000000000000..77af7eceb612 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CapsuleShape.h @@ -0,0 +1,129 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a CapsuleShape +class JPH_EXPORT CapsuleShapeSettings final : public ConvexShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, CapsuleShapeSettings) + +public: + /// Default constructor for deserialization + CapsuleShapeSettings() = default; + + /// Create a capsule centered around the origin with one sphere cap at (0, -inHalfHeightOfCylinder, 0) and the other at (0, inHalfHeightOfCylinder, 0) + CapsuleShapeSettings(float inHalfHeightOfCylinder, float inRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mRadius(inRadius), mHalfHeightOfCylinder(inHalfHeightOfCylinder) { } + + /// Check if this is a valid capsule shape + bool IsValid() const { return mRadius > 0.0f && mHalfHeightOfCylinder >= 0.0f; } + + /// Checks if the settings of this capsule make this shape a sphere + bool IsSphere() const { return mHalfHeightOfCylinder == 0.0f; } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + float mRadius = 0.0f; + float mHalfHeightOfCylinder = 0.0f; +}; + +/// A capsule, implemented as a line segment with convex radius +class JPH_EXPORT CapsuleShape final : public ConvexShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + CapsuleShape() : ConvexShape(EShapeSubType::Capsule) { } + CapsuleShape(const CapsuleShapeSettings &inSettings, ShapeResult &outResult); + + /// Create a capsule centered around the origin with one sphere cap at (0, -inHalfHeightOfCylinder, 0) and the other at (0, inHalfHeightOfCylinder, 0) + CapsuleShape(float inHalfHeightOfCylinder, float inRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShape(EShapeSubType::Capsule, inMaterial), mRadius(inRadius), mHalfHeightOfCylinder(inHalfHeightOfCylinder) { JPH_ASSERT(inHalfHeightOfCylinder > 0.0f); JPH_ASSERT(inRadius > 0.0f); } + + /// Radius of the cylinder + float GetRadius() const { return mRadius; } + + /// Get half of the height of the cylinder + float GetHalfHeightOfCylinder() const { return mHalfHeightOfCylinder; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mRadius; } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See ConvexShape::GetSupportFunction + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + using ConvexShape::CastRay; + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return 4.0f / 3.0f * JPH_PI * Cubed(mRadius) + 2.0f * JPH_PI * mHalfHeightOfCylinder * Square(mRadius); } + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // See Shape::MakeScaleValid + virtual Vec3 MakeScaleValid(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Classes for GetSupportFunction + class CapsuleNoConvex; + class CapsuleWithConvex; + + float mRadius = 0.0f; + float mHalfHeightOfCylinder = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.cpp new file mode 100644 index 000000000000..f5fe04ecf213 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.cpp @@ -0,0 +1,428 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(CompoundShapeSettings) +{ + JPH_ADD_BASE_CLASS(CompoundShapeSettings, ShapeSettings) + + JPH_ADD_ATTRIBUTE(CompoundShapeSettings, mSubShapes) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(CompoundShapeSettings::SubShapeSettings) +{ + JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mShape) + JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mPosition) + JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mRotation) + JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mUserData) +} + +void CompoundShapeSettings::AddShape(Vec3Arg inPosition, QuatArg inRotation, const ShapeSettings *inShape, uint32 inUserData) +{ + // Add shape + SubShapeSettings shape; + shape.mPosition = inPosition; + shape.mRotation = inRotation; + shape.mShape = inShape; + shape.mUserData = inUserData; + mSubShapes.push_back(shape); +} + +void CompoundShapeSettings::AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData) +{ + // Add shape + SubShapeSettings shape; + shape.mPosition = inPosition; + shape.mRotation = inRotation; + shape.mShapePtr = inShape; + shape.mUserData = inUserData; + mSubShapes.push_back(shape); +} + +bool CompoundShape::MustBeStatic() const +{ + for (const SubShape &shape : mSubShapes) + if (shape.mShape->MustBeStatic()) + return true; + + return false; +} + +MassProperties CompoundShape::GetMassProperties() const +{ + MassProperties p; + + // Calculate mass and inertia + p.mMass = 0.0f; + p.mInertia = Mat44::sZero(); + for (const SubShape &shape : mSubShapes) + { + // Rotate and translate inertia of child into place + MassProperties child = shape.mShape->GetMassProperties(); + child.Rotate(Mat44::sRotation(shape.GetRotation())); + child.Translate(shape.GetPositionCOM()); + + // Accumulate mass and inertia + p.mMass += child.mMass; + p.mInertia += child.mInertia; + } + + // Ensure that inertia is a 3x3 matrix, adding inertias causes the bottom right element to change + p.mInertia.SetColumn4(3, Vec4(0, 0, 0, 1)); + + return p; +} + +AABox CompoundShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + if (mSubShapes.size() <= 10) + { + AABox bounds; + for (const SubShape &shape : mSubShapes) + { + Mat44 transform = inCenterOfMassTransform * shape.GetLocalTransformNoScale(inScale); + bounds.Encapsulate(shape.mShape->GetWorldSpaceBounds(transform, shape.TransformScale(inScale))); + } + return bounds; + } + else + { + // If there are too many shapes, use the base class function (this will result in a slightly wider bounding box) + return Shape::GetWorldSpaceBounds(inCenterOfMassTransform, inScale); + } +} + +uint CompoundShape::GetSubShapeIDBitsRecursive() const +{ + // Add max of child bits to our bits + uint child_bits = 0; + for (const SubShape &shape : mSubShapes) + child_bits = max(child_bits, shape.mShape->GetSubShapeIDBitsRecursive()); + return child_bits + GetSubShapeIDBits(); +} + +const PhysicsMaterial *CompoundShape::GetMaterial(const SubShapeID &inSubShapeID) const +{ + // Decode sub shape index + SubShapeID remainder; + uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder); + + // Pass call on + return mSubShapes[index].mShape->GetMaterial(remainder); +} + +const Shape *CompoundShape::GetLeafShape(const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const +{ + // Decode sub shape index + SubShapeID remainder; + uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder); + if (index >= mSubShapes.size()) + { + // No longer valid index + outRemainder = SubShapeID(); + return nullptr; + } + + // Pass call on + return mSubShapes[index].mShape->GetLeafShape(remainder, outRemainder); +} + +uint64 CompoundShape::GetSubShapeUserData(const SubShapeID &inSubShapeID) const +{ + // Decode sub shape index + SubShapeID remainder; + uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder); + if (index >= mSubShapes.size()) + return 0; // No longer valid index + + // Pass call on + return mSubShapes[index].mShape->GetSubShapeUserData(remainder); +} + +TransformedShape CompoundShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const +{ + // Get the sub shape + const SubShape &sub_shape = mSubShapes[GetSubShapeIndexFromID(inSubShapeID, outRemainder)]; + + // Calculate transform for sub shape + Vec3 position = inPositionCOM + inRotation * (inScale * sub_shape.GetPositionCOM()); + Quat rotation = inRotation * sub_shape.GetRotation(); + Vec3 scale = sub_shape.TransformScale(inScale); + + // Return transformed shape + TransformedShape ts(RVec3(position), rotation, sub_shape.mShape, BodyID()); + ts.SetShapeScale(scale); + return ts; +} + +Vec3 CompoundShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + // Decode sub shape index + SubShapeID remainder; + uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder); + + // Transform surface position to local space and pass call on + const SubShape &shape = mSubShapes[index]; + Mat44 transform = Mat44::sInverseRotationTranslation(shape.GetRotation(), shape.GetPositionCOM()); + Vec3 normal = shape.mShape->GetSurfaceNormal(remainder, transform * inLocalSurfacePosition); + + // Transform normal to this shape's space + return transform.Multiply3x3Transposed(normal); +} + +void CompoundShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + // Decode sub shape index + SubShapeID remainder; + uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder); + + // Apply transform and pass on to sub shape + const SubShape &shape = mSubShapes[index]; + Mat44 transform = shape.GetLocalTransformNoScale(inScale); + shape.mShape->GetSupportingFace(remainder, transform.Multiply3x3Transposed(inDirection), shape.TransformScale(inScale), inCenterOfMassTransform * transform, outVertices); +} + +void CompoundShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + outTotalVolume = 0.0f; + outSubmergedVolume = 0.0f; + outCenterOfBuoyancy = Vec3::sZero(); + + for (const SubShape &shape : mSubShapes) + { + // Get center of mass transform of child + Mat44 transform = inCenterOfMassTransform * shape.GetLocalTransformNoScale(inScale); + + // Recurse to child + float total_volume, submerged_volume; + Vec3 center_of_buoyancy; + shape.mShape->GetSubmergedVolume(transform, shape.TransformScale(inScale), inSurface, total_volume, submerged_volume, center_of_buoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset)); + + // Accumulate volumes + outTotalVolume += total_volume; + outSubmergedVolume += submerged_volume; + + // The center of buoyancy is the weighted average of the center of buoyancy of our child shapes + outCenterOfBuoyancy += submerged_volume * center_of_buoyancy; + } + + if (outSubmergedVolume > 0.0f) + outCenterOfBuoyancy /= outSubmergedVolume; + +#ifdef JPH_DEBUG_RENDERER + // Draw center of buoyancy + if (sDrawSubmergedVolumes) + DebugRenderer::sInstance->DrawWireSphere(inBaseOffset + outCenterOfBuoyancy, 0.05f, Color::sRed, 1); +#endif // JPH_DEBUG_RENDERER +} + +#ifdef JPH_DEBUG_RENDERER +void CompoundShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + for (const SubShape &shape : mSubShapes) + { + Mat44 transform = shape.GetLocalTransformNoScale(inScale); + shape.mShape->Draw(inRenderer, inCenterOfMassTransform * transform, shape.TransformScale(inScale), inColor, inUseMaterialColors, inDrawWireframe); + } +} + +void CompoundShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const +{ + for (const SubShape &shape : mSubShapes) + { + Mat44 transform = shape.GetLocalTransformNoScale(inScale); + shape.mShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform * transform, shape.TransformScale(inScale), inColor, inDrawSupportDirection); + } +} + +void CompoundShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + for (const SubShape &shape : mSubShapes) + { + Mat44 transform = shape.GetLocalTransformNoScale(inScale); + shape.mShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform * transform, shape.TransformScale(inScale)); + } +} +#endif // JPH_DEBUG_RENDERER + +void CompoundShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + for (const SubShape &shape : mSubShapes) + { + Mat44 transform = shape.GetLocalTransformNoScale(inScale); + shape.mShape->CollideSoftBodyVertices(inCenterOfMassTransform * transform, shape.TransformScale(inScale), inVertices, inNumVertices, inCollidingShapeIndex); + } +} + +void CompoundShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const +{ + for (const SubShape &shape : mSubShapes) + shape.mShape->TransformShape(inCenterOfMassTransform * Mat44::sRotationTranslation(shape.GetRotation(), shape.GetPositionCOM()), ioCollector); +} + +void CompoundShape::sCastCompoundVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + // Fetch compound shape from cast shape + JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Compound); + const CompoundShape *compound = static_cast(inShapeCast.mShape); + + // Number of sub shapes + int n = (int)compound->mSubShapes.size(); + + // Determine amount of bits for sub shape + uint sub_shape_bits = compound->GetSubShapeIDBits(); + + // Recurse to sub shapes + for (int i = 0; i < n; ++i) + { + const SubShape &shape = compound->mSubShapes[i]; + + // Create ID for sub shape + SubShapeIDCreator shape1_sub_shape_id = inSubShapeIDCreator1.PushID(i, sub_shape_bits); + + // Transform the shape cast and update the shape + Mat44 transform = inShapeCast.mCenterOfMassStart * shape.GetLocalTransformNoScale(inShapeCast.mScale); + Vec3 scale = shape.TransformScale(inShapeCast.mScale); + ShapeCast shape_cast(shape.mShape, scale, transform, inShapeCast.mDirection); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, shape1_sub_shape_id, inSubShapeIDCreator2, ioCollector); + + if (ioCollector.ShouldEarlyOut()) + break; + } +} + +void CompoundShape::SaveBinaryState(StreamOut &inStream) const +{ + Shape::SaveBinaryState(inStream); + + inStream.Write(mCenterOfMass); + inStream.Write(mLocalBounds.mMin); + inStream.Write(mLocalBounds.mMax); + inStream.Write(mInnerRadius); + + // Write sub shapes + inStream.Write(mSubShapes, [](const SubShape &inElement, StreamOut &inS) { + inS.Write(inElement.mUserData); + inS.Write(inElement.mPositionCOM); + inS.Write(inElement.mRotation); + }); +} + +void CompoundShape::RestoreBinaryState(StreamIn &inStream) +{ + Shape::RestoreBinaryState(inStream); + + inStream.Read(mCenterOfMass); + inStream.Read(mLocalBounds.mMin); + inStream.Read(mLocalBounds.mMax); + inStream.Read(mInnerRadius); + + // Read sub shapes + inStream.Read(mSubShapes, [](StreamIn &inS, SubShape &outElement) { + inS.Read(outElement.mUserData); + inS.Read(outElement.mPositionCOM); + inS.Read(outElement.mRotation); + outElement.mIsRotationIdentity = outElement.mRotation == Float3(0, 0, 0); + }); +} + +void CompoundShape::SaveSubShapeState(ShapeList &outSubShapes) const +{ + outSubShapes.clear(); + outSubShapes.reserve(mSubShapes.size()); + for (const SubShape &shape : mSubShapes) + outSubShapes.push_back(shape.mShape); +} + +void CompoundShape::RestoreSubShapeState(const ShapeRefC *inSubShapes, uint inNumShapes) +{ + JPH_ASSERT(mSubShapes.size() == inNumShapes); + for (uint i = 0; i < inNumShapes; ++i) + mSubShapes[i].mShape = inSubShapes[i]; +} + +Shape::Stats CompoundShape::GetStatsRecursive(VisitedShapes &ioVisitedShapes) const +{ + // Get own stats + Stats stats = Shape::GetStatsRecursive(ioVisitedShapes); + + // Add child stats + for (const SubShape &shape : mSubShapes) + { + Stats child_stats = shape.mShape->GetStatsRecursive(ioVisitedShapes); + stats.mSizeBytes += child_stats.mSizeBytes; + stats.mNumTriangles += child_stats.mNumTriangles; + } + + return stats; +} + +float CompoundShape::GetVolume() const +{ + float volume = 0.0f; + for (const SubShape &shape : mSubShapes) + volume += shape.mShape->GetVolume(); + return volume; +} + +bool CompoundShape::IsValidScale(Vec3Arg inScale) const +{ + if (!Shape::IsValidScale(inScale)) + return false; + + for (const SubShape &shape : mSubShapes) + { + // Test if the scale is non-uniform and the shape is rotated + if (!shape.IsValidScale(inScale)) + return false; + + // Test the child shape + if (!shape.mShape->IsValidScale(shape.TransformScale(inScale))) + return false; + } + + return true; +} + +Vec3 CompoundShape::MakeScaleValid(Vec3Arg inScale) const +{ + Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale); + if (CompoundShape::IsValidScale(scale)) + return scale; + + Vec3 abs_uniform_scale = ScaleHelpers::MakeUniformScale(scale.Abs()); + Vec3 uniform_scale = scale.GetSign() * abs_uniform_scale; + if (CompoundShape::IsValidScale(uniform_scale)) + return uniform_scale; + + return Sign(scale.GetX()) * abs_uniform_scale; +} + +void CompoundShape::sRegister() +{ + for (EShapeSubType s1 : sCompoundSubShapeTypes) + for (EShapeSubType s2 : sAllSubShapeTypes) + CollisionDispatch::sRegisterCastShape(s1, s2, sCastCompoundVsShape); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.h new file mode 100644 index 000000000000..0bbd7c4d7a07 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.h @@ -0,0 +1,350 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; +class OrientedBox; + +/// Base class settings to construct a compound shape +class JPH_EXPORT CompoundShapeSettings : public ShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, CompoundShapeSettings) + +public: + /// Constructor. Use AddShape to add the parts. + CompoundShapeSettings() = default; + + /// Add a shape to the compound. + void AddShape(Vec3Arg inPosition, QuatArg inRotation, const ShapeSettings *inShape, uint32 inUserData = 0); + + /// Add a shape to the compound. Variant that uses a concrete shape, which means this object cannot be serialized. + void AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData = 0); + + struct SubShapeSettings + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SubShapeSettings) + + RefConst mShape; ///< Sub shape (either this or mShapePtr needs to be filled up) + RefConst mShapePtr; ///< Sub shape (either this or mShape needs to be filled up) + Vec3 mPosition; ///< Position of the sub shape + Quat mRotation; ///< Rotation of the sub shape + uint32 mUserData = 0; ///< User data value (can be used by the application for any purpose) + }; + + using SubShapes = Array; + + SubShapes mSubShapes; +}; + +/// Base class for a compound shape +class JPH_EXPORT CompoundShape : public Shape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit CompoundShape(EShapeSubType inSubType) : Shape(EShapeType::Compound, inSubType) { } + CompoundShape(EShapeSubType inSubType, const ShapeSettings &inSettings, ShapeResult &outResult) : Shape(EShapeType::Compound, inSubType, inSettings, outResult) { } + + // See Shape::GetCenterOfMass + virtual Vec3 GetCenterOfMass() const override { return mCenterOfMass; } + + // See Shape::MustBeStatic + virtual bool MustBeStatic() const override; + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override { return mLocalBounds; } + + // See Shape::GetSubShapeIDBitsRecursive + virtual uint GetSubShapeIDBitsRecursive() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mInnerRadius; } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetMaterial + virtual const PhysicsMaterial * GetMaterial(const SubShapeID &inSubShapeID) const override; + + // See Shape::GetLeafShape + virtual const Shape * GetLeafShape(const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const override; + + // See Shape::GetSubShapeUserData + virtual uint64 GetSubShapeUserData(const SubShapeID &inSubShapeID) const override; + + // See Shape::GetSubShapeTransformedShape + virtual TransformedShape GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; + + // See Shape::DrawGetSupportFunction + virtual void DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override; + + // See Shape::DrawGetSupportingFace + virtual void DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; +#endif // JPH_DEBUG_RENDERER + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::TransformShape + virtual void TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); } + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); return 0; } + + /// Get which sub shape's bounding boxes overlap with an axis aligned box + /// @param inBox The axis aligned box to test against (relative to the center of mass of this shape) + /// @param outSubShapeIndices Buffer where to place the indices of the sub shapes that intersect + /// @param inMaxSubShapeIndices How many indices will fit in the buffer (normally you'd provide a buffer of GetNumSubShapes() indices) + /// @return How many indices were placed in outSubShapeIndices + virtual int GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const = 0; + + /// Get which sub shape's bounding boxes overlap with an axis aligned box + /// @param inBox The axis aligned box to test against (relative to the center of mass of this shape) + /// @param outSubShapeIndices Buffer where to place the indices of the sub shapes that intersect + /// @param inMaxSubShapeIndices How many indices will fit in the buffer (normally you'd provide a buffer of GetNumSubShapes() indices) + /// @return How many indices were placed in outSubShapeIndices + virtual int GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const = 0; + + struct SubShape + { + /// Initialize sub shape from sub shape settings + /// @param inSettings Settings object + /// @param outResult Result object, only used in case of error + /// @return True on success, false on failure + bool FromSettings(const CompoundShapeSettings::SubShapeSettings &inSettings, ShapeResult &outResult) + { + if (inSettings.mShapePtr != nullptr) + { + // Use provided shape + mShape = inSettings.mShapePtr; + } + else + { + // Create child shape + ShapeResult child_result = inSettings.mShape->Create(); + if (!child_result.IsValid()) + { + outResult = child_result; + return false; + } + mShape = child_result.Get(); + } + + // Copy user data + mUserData = inSettings.mUserData; + + SetTransform(inSettings.mPosition, inSettings.mRotation, Vec3::sZero() /* Center of mass not yet calculated */); + return true; + } + + /// Update the transform of this sub shape + /// @param inPosition New position + /// @param inRotation New orientation + /// @param inCenterOfMass The center of mass of the compound shape + JPH_INLINE void SetTransform(Vec3Arg inPosition, QuatArg inRotation, Vec3Arg inCenterOfMass) + { + SetPositionCOM(inPosition - inCenterOfMass + inRotation * mShape->GetCenterOfMass()); + + mIsRotationIdentity = inRotation.IsClose(Quat::sIdentity()) || inRotation.IsClose(-Quat::sIdentity()); + SetRotation(mIsRotationIdentity? Quat::sIdentity() : inRotation); + } + + /// Get the local transform for this shape given the scale of the child shape + /// The total transform of the child shape will be GetLocalTransformNoScale(inScale) * Mat44::sScaling(TransformScale(inScale)) + /// @param inScale The scale of the child shape (in local space of this shape) + JPH_INLINE Mat44 GetLocalTransformNoScale(Vec3Arg inScale) const + { + JPH_ASSERT(IsValidScale(inScale)); + return Mat44::sRotationTranslation(GetRotation(), inScale * GetPositionCOM()); + } + + /// Test if inScale is valid for this sub shape + inline bool IsValidScale(Vec3Arg inScale) const + { + // We can always handle uniform scale or identity rotations + if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(inScale)) + return true; + + return ScaleHelpers::CanScaleBeRotated(GetRotation(), inScale); + } + + /// Transform the scale to the local space of the child shape + inline Vec3 TransformScale(Vec3Arg inScale) const + { + // We don't need to transform uniform scale or if the rotation is identity + if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(inScale)) + return inScale; + + return ScaleHelpers::RotateScale(GetRotation(), inScale); + } + + /// Compress the center of mass position + JPH_INLINE void SetPositionCOM(Vec3Arg inPositionCOM) + { + inPositionCOM.StoreFloat3(&mPositionCOM); + } + + /// Uncompress the center of mass position + JPH_INLINE Vec3 GetPositionCOM() const + { + return Vec3::sLoadFloat3Unsafe(mPositionCOM); + } + + /// Compress the rotation + JPH_INLINE void SetRotation(QuatArg inRotation) + { + inRotation.StoreFloat3(&mRotation); + } + + /// Uncompress the rotation + JPH_INLINE Quat GetRotation() const + { + return mIsRotationIdentity? Quat::sIdentity() : Quat::sLoadFloat3Unsafe(mRotation); + } + + RefConst mShape; + Float3 mPositionCOM; ///< Note: Position of center of mass of sub shape! + Float3 mRotation; ///< Note: X, Y, Z of rotation quaternion - note we read 4 bytes beyond this so make sure there's something there + uint32 mUserData; ///< User data value (put here because it falls in padding bytes) + bool mIsRotationIdentity; ///< If mRotation is close to identity (put here because it falls in padding bytes) + // 3 padding bytes left + }; + + static_assert(sizeof(SubShape) == (JPH_CPU_ADDRESS_BITS == 64? 40 : 36), "Compiler added unexpected padding"); + + using SubShapes = Array; + + /// Access to the sub shapes of this compound + const SubShapes & GetSubShapes() const { return mSubShapes; } + + /// Get the total number of sub shapes + uint GetNumSubShapes() const { return uint(mSubShapes.size()); } + + /// Access to a particular sub shape + const SubShape & GetSubShape(uint inIdx) const { return mSubShapes[inIdx]; } + + /// Get the user data associated with a shape in this compound + uint32 GetCompoundUserData(uint inIdx) const { return mSubShapes[inIdx].mUserData; } + + /// Set the user data associated with a shape in this compound + void SetCompoundUserData(uint inIdx, uint32 inUserData) { mSubShapes[inIdx].mUserData = inUserData; } + + /// Check if a sub shape ID is still valid for this shape + /// @param inSubShapeID Sub shape id that indicates the leaf shape relative to this shape + /// @return True if the ID is valid, false if not + inline bool IsSubShapeIDValid(SubShapeID inSubShapeID) const + { + SubShapeID remainder; + return inSubShapeID.PopID(GetSubShapeIDBits(), remainder) < mSubShapes.size(); + } + + /// Convert SubShapeID to sub shape index + /// @param inSubShapeID Sub shape id that indicates the leaf shape relative to this shape + /// @param outRemainder This is the sub shape ID for the sub shape of the compound after popping off the index + /// @return The index of the sub shape of this compound + inline uint32 GetSubShapeIndexFromID(SubShapeID inSubShapeID, SubShapeID &outRemainder) const + { + uint32 idx = inSubShapeID.PopID(GetSubShapeIDBits(), outRemainder); + JPH_ASSERT(idx < mSubShapes.size(), "Invalid SubShapeID"); + return idx; + } + + /// @brief Convert a sub shape index to a sub shape ID + /// @param inIdx Index of the sub shape of this compound + /// @param inParentSubShapeID Parent SubShapeID (describing the path to the compound shape) + /// @return A sub shape ID creator that contains the full path to the sub shape with index inIdx + inline SubShapeIDCreator GetSubShapeIDFromIndex(int inIdx, const SubShapeIDCreator &inParentSubShapeID) const + { + return inParentSubShapeID.PushID(inIdx, GetSubShapeIDBits()); + } + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void SaveSubShapeState(ShapeList &outSubShapes) const override; + virtual void RestoreSubShapeState(const ShapeRefC *inSubShapes, uint inNumShapes) override; + + // See Shape::GetStatsRecursive + virtual Stats GetStatsRecursive(VisitedShapes &ioVisitedShapes) const override; + + // See Shape::GetVolume + virtual float GetVolume() const override; + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // See Shape::MakeScaleValid + virtual Vec3 MakeScaleValid(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + + // Visitors for collision detection + struct CastRayVisitor; + struct CastRayVisitorCollector; + struct CollidePointVisitor; + struct CastShapeVisitor; + struct CollectTransformedShapesVisitor; + struct CollideCompoundVsShapeVisitor; + struct CollideShapeVsCompoundVisitor; + template struct GetIntersectingSubShapesVisitor; + + /// Determine amount of bits needed to encode sub shape id + inline uint GetSubShapeIDBits() const + { + // Ensure we have enough bits to encode our shape [0, n - 1] + uint32 n = uint32(mSubShapes.size()) - 1; + return 32 - CountLeadingZeros(n); + } + + /// Determine the inner radius of this shape + inline void CalculateInnerRadius() + { + mInnerRadius = FLT_MAX; + for (const SubShape &s : mSubShapes) + mInnerRadius = min(mInnerRadius, s.mShape->GetInnerRadius()); + } + + Vec3 mCenterOfMass { Vec3::sZero() }; ///< Center of mass of the compound + AABox mLocalBounds; + SubShapes mSubShapes; + float mInnerRadius = FLT_MAX; ///< Smallest radius of GetInnerRadius() of child shapes + +private: + // Helper functions called by CollisionDispatch + static void sCastCompoundVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShapeVisitors.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShapeVisitors.h new file mode 100644 index 000000000000..1b1e3867b07b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShapeVisitors.h @@ -0,0 +1,460 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +struct CompoundShape::CastRayVisitor +{ + JPH_INLINE CastRayVisitor(const RayCast &inRay, const CompoundShape *inShape, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) : + mRay(inRay), + mHit(ioHit), + mSubShapeIDCreator(inSubShapeIDCreator), + mSubShapeBits(inShape->GetSubShapeIDBits()) + { + // Determine ray properties of cast + mInvDirection.Set(inRay.mDirection); + } + + /// Returns true when collision detection should abort because it's not possible to find a better hit + JPH_INLINE bool ShouldAbort() const + { + return mHit.mFraction <= 0.0f; + } + + /// Test ray against 4 bounding boxes and returns the distance where the ray enters the bounding box + JPH_INLINE Vec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return RayAABox4(mRay.mOrigin, mInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + /// Test the ray against a single subshape + JPH_INLINE void VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex) + { + // Create ID for sub shape + SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator.PushID(inSubShapeIndex, mSubShapeBits); + + // Transform the ray + Mat44 transform = Mat44::sInverseRotationTranslation(inSubShape.GetRotation(), inSubShape.GetPositionCOM()); + RayCast ray = mRay.Transformed(transform); + if (inSubShape.mShape->CastRay(ray, shape2_sub_shape_id, mHit)) + mReturnValue = true; + } + + RayInvDirection mInvDirection; + const RayCast & mRay; + RayCastResult & mHit; + SubShapeIDCreator mSubShapeIDCreator; + uint mSubShapeBits; + bool mReturnValue = false; +}; + +struct CompoundShape::CastRayVisitorCollector +{ + JPH_INLINE CastRayVisitorCollector(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const CompoundShape *inShape, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) : + mRay(inRay), + mCollector(ioCollector), + mSubShapeIDCreator(inSubShapeIDCreator), + mSubShapeBits(inShape->GetSubShapeIDBits()), + mRayCastSettings(inRayCastSettings), + mShapeFilter(inShapeFilter) + { + // Determine ray properties of cast + mInvDirection.Set(inRay.mDirection); + } + + /// Returns true when collision detection should abort because it's not possible to find a better hit + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Test ray against 4 bounding boxes and returns the distance where the ray enters the bounding box + JPH_INLINE Vec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return RayAABox4(mRay.mOrigin, mInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + /// Test the ray against a single subshape + JPH_INLINE void VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex) + { + // Create ID for sub shape + SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator.PushID(inSubShapeIndex, mSubShapeBits); + + // Transform the ray + Mat44 transform = Mat44::sInverseRotationTranslation(inSubShape.GetRotation(), inSubShape.GetPositionCOM()); + RayCast ray = mRay.Transformed(transform); + inSubShape.mShape->CastRay(ray, mRayCastSettings, shape2_sub_shape_id, mCollector, mShapeFilter); + } + + RayInvDirection mInvDirection; + const RayCast & mRay; + CastRayCollector & mCollector; + SubShapeIDCreator mSubShapeIDCreator; + uint mSubShapeBits; + RayCastSettings mRayCastSettings; + const ShapeFilter & mShapeFilter; +}; + +struct CompoundShape::CollidePointVisitor +{ + JPH_INLINE CollidePointVisitor(Vec3Arg inPoint, const CompoundShape *inShape, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) : + mPoint(inPoint), + mSubShapeIDCreator(inSubShapeIDCreator), + mCollector(ioCollector), + mSubShapeBits(inShape->GetSubShapeIDBits()), + mShapeFilter(inShapeFilter) + { + } + + /// Returns true when collision detection should abort because it's not possible to find a better hit + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Test if point overlaps with 4 boxes, returns true for the ones that do + JPH_INLINE UVec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return AABox4VsPoint(mPoint, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + /// Test the point against a single subshape + JPH_INLINE void VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex) + { + // Create ID for sub shape + SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator.PushID(inSubShapeIndex, mSubShapeBits); + + // Transform the point + Mat44 transform = Mat44::sInverseRotationTranslation(inSubShape.GetRotation(), inSubShape.GetPositionCOM()); + inSubShape.mShape->CollidePoint(transform * mPoint, shape2_sub_shape_id, mCollector, mShapeFilter); + } + + Vec3 mPoint; + SubShapeIDCreator mSubShapeIDCreator; + CollidePointCollector & mCollector; + uint mSubShapeBits; + const ShapeFilter & mShapeFilter; +}; + +struct CompoundShape::CastShapeVisitor +{ + JPH_INLINE CastShapeVisitor(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const CompoundShape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) : + mBoxCenter(inShapeCast.mShapeWorldBounds.GetCenter()), + mBoxExtent(inShapeCast.mShapeWorldBounds.GetExtent()), + mScale(inScale), + mShapeCast(inShapeCast), + mShapeCastSettings(inShapeCastSettings), + mShapeFilter(inShapeFilter), + mCollector(ioCollector), + mCenterOfMassTransform2(inCenterOfMassTransform2), + mSubShapeIDCreator1(inSubShapeIDCreator1), + mSubShapeIDCreator2(inSubShapeIDCreator2), + mSubShapeBits(inShape->GetSubShapeIDBits()) + { + // Determine ray properties of cast + mInvDirection.Set(inShapeCast.mDirection); + } + + /// Returns true when collision detection should abort because it's not possible to find a better hit + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Tests the shape cast against 4 bounding boxes, returns the distance along the shape cast where the shape first enters the bounding box + JPH_INLINE Vec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + // Scale the bounding boxes + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Enlarge them by the casted shape's box extents + AABox4EnlargeWithExtent(mBoxExtent, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test ray against the bounding boxes + return RayAABox4(mBoxCenter, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + } + + /// Test the cast shape against a single subshape + JPH_INLINE void VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex) + { + JPH_ASSERT(inSubShape.IsValidScale(mScale)); + + // Create ID for sub shape + SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator2.PushID(inSubShapeIndex, mSubShapeBits); + + // Calculate the local transform for this sub shape + Mat44 local_transform = Mat44::sRotationTranslation(inSubShape.GetRotation(), mScale * inSubShape.GetPositionCOM()); + + // Transform the center of mass of 2 + Mat44 center_of_mass_transform2 = mCenterOfMassTransform2 * local_transform; + + // Transform the shape cast + ShapeCast shape_cast = mShapeCast.PostTransformed(local_transform.InversedRotationTranslation()); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, mShapeCastSettings, inSubShape.mShape, inSubShape.TransformScale(mScale), mShapeFilter, center_of_mass_transform2, mSubShapeIDCreator1, shape2_sub_shape_id, mCollector); + } + + RayInvDirection mInvDirection; + Vec3 mBoxCenter; + Vec3 mBoxExtent; + Vec3 mScale; + const ShapeCast & mShapeCast; + const ShapeCastSettings & mShapeCastSettings; + const ShapeFilter & mShapeFilter; + CastShapeCollector & mCollector; + Mat44 mCenterOfMassTransform2; + SubShapeIDCreator mSubShapeIDCreator1; + SubShapeIDCreator mSubShapeIDCreator2; + uint mSubShapeBits; +}; + +struct CompoundShape::CollectTransformedShapesVisitor +{ + JPH_INLINE CollectTransformedShapesVisitor(const AABox &inBox, const CompoundShape *inShape, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) : + mBox(inBox), + mLocalBox(Mat44::sInverseRotationTranslation(inRotation, inPositionCOM), inBox), + mPositionCOM(inPositionCOM), + mRotation(inRotation), + mScale(inScale), + mSubShapeIDCreator(inSubShapeIDCreator), + mCollector(ioCollector), + mSubShapeBits(inShape->GetSubShapeIDBits()), + mShapeFilter(inShapeFilter) + { + } + + /// Returns true when collision detection should abort because it's not possible to find a better hit + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Tests 4 bounding boxes against the query box, returns true for the ones that collide + JPH_INLINE UVec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which nodes collide + return AABox4VsBox(mLocalBox, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + } + + /// Collect the transformed sub shapes for a single subshape + JPH_INLINE void VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex) + { + JPH_ASSERT(inSubShape.IsValidScale(mScale)); + + // Create ID for sub shape + SubShapeIDCreator sub_shape_id = mSubShapeIDCreator.PushID(inSubShapeIndex, mSubShapeBits); + + // Calculate world transform for sub shape + Vec3 position = mPositionCOM + mRotation * (mScale * inSubShape.GetPositionCOM()); + Quat rotation = mRotation * inSubShape.GetRotation(); + + // Recurse to sub shape + inSubShape.mShape->CollectTransformedShapes(mBox, position, rotation, inSubShape.TransformScale(mScale), sub_shape_id, mCollector, mShapeFilter); + } + + AABox mBox; + OrientedBox mLocalBox; + Vec3 mPositionCOM; + Quat mRotation; + Vec3 mScale; + SubShapeIDCreator mSubShapeIDCreator; + TransformedShapeCollector & mCollector; + uint mSubShapeBits; + const ShapeFilter & mShapeFilter; +}; + +struct CompoundShape::CollideCompoundVsShapeVisitor +{ + JPH_INLINE CollideCompoundVsShapeVisitor(const CompoundShape *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) : + mCollideShapeSettings(inCollideShapeSettings), + mCollector(ioCollector), + mShape2(inShape2), + mScale1(inScale1), + mScale2(inScale2), + mTransform1(inCenterOfMassTransform1), + mTransform2(inCenterOfMassTransform2), + mSubShapeIDCreator1(inSubShapeIDCreator1), + mSubShapeIDCreator2(inSubShapeIDCreator2), + mSubShapeBits(inShape1->GetSubShapeIDBits()), + mShapeFilter(inShapeFilter) + { + // Get transform from shape 2 to shape 1 + Mat44 transform2_to_1 = inCenterOfMassTransform1.InversedRotationTranslation() * inCenterOfMassTransform2; + + // Convert bounding box of 2 into space of 1 + mBoundsOf2InSpaceOf1 = inShape2->GetLocalBounds().Scaled(inScale2).Transformed(transform2_to_1); + } + + /// Returns true when collision detection should abort because it's not possible to find a better hit + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Tests the bounds of shape 2 vs 4 bounding boxes, returns true for the ones that intersect + JPH_INLINE UVec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + // Scale the bounding boxes + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale1, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which boxes collide + return AABox4VsBox(mBoundsOf2InSpaceOf1, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + } + + /// Test the shape against a single subshape + JPH_INLINE void VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex) + { + // Get world transform of 1 + Mat44 transform1 = mTransform1 * inSubShape.GetLocalTransformNoScale(mScale1); + + // Create ID for sub shape + SubShapeIDCreator shape1_sub_shape_id = mSubShapeIDCreator1.PushID(inSubShapeIndex, mSubShapeBits); + + CollisionDispatch::sCollideShapeVsShape(inSubShape.mShape, mShape2, inSubShape.TransformScale(mScale1), mScale2, transform1, mTransform2, shape1_sub_shape_id, mSubShapeIDCreator2, mCollideShapeSettings, mCollector, mShapeFilter); + } + + const CollideShapeSettings & mCollideShapeSettings; + CollideShapeCollector & mCollector; + const Shape * mShape2; + Vec3 mScale1; + Vec3 mScale2; + Mat44 mTransform1; + Mat44 mTransform2; + AABox mBoundsOf2InSpaceOf1; + SubShapeIDCreator mSubShapeIDCreator1; + SubShapeIDCreator mSubShapeIDCreator2; + uint mSubShapeBits; + const ShapeFilter & mShapeFilter; +}; + +struct CompoundShape::CollideShapeVsCompoundVisitor +{ + JPH_INLINE CollideShapeVsCompoundVisitor(const Shape *inShape1, const CompoundShape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) : + mCollideShapeSettings(inCollideShapeSettings), + mCollector(ioCollector), + mShape1(inShape1), + mScale1(inScale1), + mScale2(inScale2), + mTransform1(inCenterOfMassTransform1), + mTransform2(inCenterOfMassTransform2), + mSubShapeIDCreator1(inSubShapeIDCreator1), + mSubShapeIDCreator2(inSubShapeIDCreator2), + mSubShapeBits(inShape2->GetSubShapeIDBits()), + mShapeFilter(inShapeFilter) + { + // Get transform from shape 1 to shape 2 + Mat44 transform1_to_2 = inCenterOfMassTransform2.InversedRotationTranslation() * inCenterOfMassTransform1; + + // Convert bounding box of 1 into space of 2 + mBoundsOf1InSpaceOf2 = inShape1->GetLocalBounds().Scaled(inScale1).Transformed(transform1_to_2); + mBoundsOf1InSpaceOf2.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance)); + } + + /// Returns true when collision detection should abort because it's not possible to find a better hit + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Tests the bounds of shape 1 vs 4 bounding boxes, returns true for the ones that intersect + JPH_INLINE UVec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + // Scale the bounding boxes + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which bounding boxes collide + return AABox4VsBox(mBoundsOf1InSpaceOf2, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + } + + /// Test the shape against a single subshape + JPH_INLINE void VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex) + { + // Create ID for sub shape + SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator2.PushID(inSubShapeIndex, mSubShapeBits); + + // Get world transform of 2 + Mat44 transform2 = mTransform2 * inSubShape.GetLocalTransformNoScale(mScale2); + + CollisionDispatch::sCollideShapeVsShape(mShape1, inSubShape.mShape, mScale1, inSubShape.TransformScale(mScale2), mTransform1, transform2, mSubShapeIDCreator1, shape2_sub_shape_id, mCollideShapeSettings, mCollector, mShapeFilter); + } + + const CollideShapeSettings & mCollideShapeSettings; + CollideShapeCollector & mCollector; + const Shape * mShape1; + Vec3 mScale1; + Vec3 mScale2; + Mat44 mTransform1; + Mat44 mTransform2; + AABox mBoundsOf1InSpaceOf2; + SubShapeIDCreator mSubShapeIDCreator1; + SubShapeIDCreator mSubShapeIDCreator2; + uint mSubShapeBits; + const ShapeFilter & mShapeFilter; +}; + +template +struct CompoundShape::GetIntersectingSubShapesVisitor +{ + JPH_INLINE GetIntersectingSubShapesVisitor(const BoxType &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) : + mBox(inBox), + mSubShapeIndices(outSubShapeIndices), + mMaxSubShapeIndices(inMaxSubShapeIndices) + { + } + + /// Returns true when collision detection should abort because the buffer is full + JPH_INLINE bool ShouldAbort() const + { + return mNumResults >= mMaxSubShapeIndices; + } + + /// Tests the box vs 4 bounding boxes, returns true for the ones that intersect + JPH_INLINE UVec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + // Test which bounding boxes collide + return AABox4VsBox(mBox, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + /// Records a hit + JPH_INLINE void VisitShape([[maybe_unused]] const SubShape &inSubShape, uint32 inSubShapeIndex) + { + JPH_ASSERT(mNumResults < mMaxSubShapeIndices); + *mSubShapeIndices++ = inSubShapeIndex; + mNumResults++; + } + + /// Get the number of indices that were found + JPH_INLINE int GetNumResults() const + { + return mNumResults; + } + +private: + BoxType mBox; + uint * mSubShapeIndices; + int mMaxSubShapeIndices; + int mNumResults = 0; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.cpp new file mode 100644 index 000000000000..1a1126e19056 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.cpp @@ -0,0 +1,1308 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ConvexHullShapeSettings) +{ + JPH_ADD_BASE_CLASS(ConvexHullShapeSettings, ConvexShapeSettings) + + JPH_ADD_ATTRIBUTE(ConvexHullShapeSettings, mPoints) + JPH_ADD_ATTRIBUTE(ConvexHullShapeSettings, mMaxConvexRadius) + JPH_ADD_ATTRIBUTE(ConvexHullShapeSettings, mMaxErrorConvexRadius) + JPH_ADD_ATTRIBUTE(ConvexHullShapeSettings, mHullTolerance) +} + +ShapeSettings::ShapeResult ConvexHullShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new ConvexHullShape(*this, mCachedResult); + return mCachedResult; +} + +ConvexHullShape::ConvexHullShape(const ConvexHullShapeSettings &inSettings, ShapeResult &outResult) : + ConvexShape(EShapeSubType::ConvexHull, inSettings, outResult), + mConvexRadius(inSettings.mMaxConvexRadius) +{ + using BuilderFace = ConvexHullBuilder::Face; + using Edge = ConvexHullBuilder::Edge; + using Faces = Array; + + // Check convex radius + if (mConvexRadius < 0.0f) + { + outResult.SetError("Invalid convex radius"); + return; + } + + // Build convex hull + const char *error = nullptr; + ConvexHullBuilder builder(inSettings.mPoints); + ConvexHullBuilder::EResult result = builder.Initialize(cMaxPointsInHull, inSettings.mHullTolerance, error); + if (result != ConvexHullBuilder::EResult::Success && result != ConvexHullBuilder::EResult::MaxVerticesReached) + { + outResult.SetError(error); + return; + } + const Faces &builder_faces = builder.GetFaces(); + + // Check the consistency of the resulting hull if we fully built it + if (result == ConvexHullBuilder::EResult::Success) + { + ConvexHullBuilder::Face *max_error_face; + float max_error_distance, coplanar_distance; + int max_error_idx; + builder.DetermineMaxError(max_error_face, max_error_distance, max_error_idx, coplanar_distance); + if (max_error_distance > 4.0f * max(coplanar_distance, inSettings.mHullTolerance)) // Coplanar distance could be bigger than the allowed tolerance if the points are far apart + { + outResult.SetError(StringFormat("Hull building failed, point %d had an error of %g (relative to tolerance: %g)", max_error_idx, (double)max_error_distance, double(max_error_distance / inSettings.mHullTolerance))); + return; + } + } + + // Calculate center of mass and volume + builder.GetCenterOfMassAndVolume(mCenterOfMass, mVolume); + + // Calculate covariance matrix + // See: + // - Why the inertia tensor is the inertia tensor - Jonathan Blow (http://number-none.com/blow/inertia/deriving_i.html) + // - How to find the inertia tensor (or other mass properties) of a 3D solid body represented by a triangle mesh (Draft) - Jonathan Blow, Atman J Binstock (http://number-none.com/blow/inertia/bb_inertia.doc) + Mat44 covariance_canonical(Vec4(1.0f / 60.0f, 1.0f / 120.0f, 1.0f / 120.0f, 0), Vec4(1.0f / 120.0f, 1.0f / 60.0f, 1.0f / 120.0f, 0), Vec4(1.0f / 120.0f, 1.0f / 120.0f, 1.0f / 60.0f, 0), Vec4(0, 0, 0, 1)); + Mat44 covariance_matrix = Mat44::sZero(); + for (BuilderFace *f : builder_faces) + { + // Fourth point of the tetrahedron is at the center of mass, we subtract it from the other points so we get a tetrahedron with one vertex at zero + // The first point on the face will be used to form a triangle fan + Edge *e = f->mFirstEdge; + Vec3 v1 = inSettings.mPoints[e->mStartIdx] - mCenterOfMass; + + // Get the 2nd point + e = e->mNextEdge; + Vec3 v2 = inSettings.mPoints[e->mStartIdx] - mCenterOfMass; + + // Loop over the triangle fan + for (e = e->mNextEdge; e != f->mFirstEdge; e = e->mNextEdge) + { + Vec3 v3 = inSettings.mPoints[e->mStartIdx] - mCenterOfMass; + + // Affine transform that transforms a unit tetrahedon (with vertices (0, 0, 0), (1, 0, 0), (0, 1, 0) and (0, 0, 1) to this tetrahedron + Mat44 a(Vec4(v1, 0), Vec4(v2, 0), Vec4(v3, 0), Vec4(0, 0, 0, 1)); + + // Calculate covariance matrix for this tetrahedron + float det_a = a.GetDeterminant3x3(); + Mat44 c = det_a * (a * covariance_canonical * a.Transposed()); + + // Add it + covariance_matrix += c; + + // Prepare for next triangle + v2 = v3; + } + } + + // Calculate inertia matrix assuming density is 1, note that element (3, 3) is garbage + mInertia = Mat44::sIdentity() * (covariance_matrix(0, 0) + covariance_matrix(1, 1) + covariance_matrix(2, 2)) - covariance_matrix; + + // Convert polygons from the builder to our internal representation + using VtxMap = UnorderedMap; + VtxMap vertex_map; + vertex_map.reserve(VtxMap::size_type(inSettings.mPoints.size())); + for (BuilderFace *builder_face : builder_faces) + { + // Determine where the vertices go + JPH_ASSERT(mVertexIdx.size() <= 0xFFFF); + uint16 first_vertex = (uint16)mVertexIdx.size(); + uint16 num_vertices = 0; + + // Loop over vertices in face + Edge *edge = builder_face->mFirstEdge; + do + { + // Remap to new index, not all points in the original input set are required to form the hull + uint8 new_idx; + int original_idx = edge->mStartIdx; + VtxMap::iterator m = vertex_map.find(original_idx); + if (m != vertex_map.end()) + { + // Found, reuse + new_idx = m->second; + } + else + { + // This is a new point + // Make relative to center of mass + Vec3 p = inSettings.mPoints[original_idx] - mCenterOfMass; + + // Update local bounds + mLocalBounds.Encapsulate(p); + + // Add to point list + JPH_ASSERT(mPoints.size() <= 0xff); + new_idx = (uint8)mPoints.size(); + mPoints.push_back({ p }); + vertex_map[original_idx] = new_idx; + } + + // Append to vertex list + JPH_ASSERT(mVertexIdx.size() < 0xffff); + mVertexIdx.push_back(new_idx); + num_vertices++; + + edge = edge->mNextEdge; + } while (edge != builder_face->mFirstEdge); + + // Add face + mFaces.push_back({ first_vertex, num_vertices }); + + // Add plane + Plane plane = Plane::sFromPointAndNormal(builder_face->mCentroid - mCenterOfMass, builder_face->mNormal.Normalized()); + mPlanes.push_back(plane); + } + + // Test if GetSupportFunction can support this many points + if (mPoints.size() > cMaxPointsInHull) + { + outResult.SetError(StringFormat("Internal error: Too many points in hull (%u), max allowed %d", (uint)mPoints.size(), cMaxPointsInHull)); + return; + } + + for (int p = 0; p < (int)mPoints.size(); ++p) + { + // For each point, find faces that use the point + Array faces; + for (int f = 0; f < (int)mFaces.size(); ++f) + { + const Face &face = mFaces[f]; + for (int v = 0; v < face.mNumVertices; ++v) + if (mVertexIdx[face.mFirstVertex + v] == p) + { + faces.push_back(f); + break; + } + } + + if (faces.size() < 2) + { + outResult.SetError("A point must be connected to 2 or more faces!"); + return; + } + + // Find the 3 normals that form the largest tetrahedron + // The largest tetrahedron we can get is ((1, 0, 0) x (0, 1, 0)) . (0, 0, 1) = 1, if the volume is only 5% of that, + // the three vectors are too coplanar and we fall back to using only 2 plane normals + float biggest_volume = 0.05f; + int best3[3] = { -1, -1, -1 }; + + // When using 2 normals, we get the two with the biggest angle between them with a minimal difference of 1 degree + // otherwise we fall back to just using 1 plane normal + float smallest_dot = Cos(DegreesToRadians(1.0f)); + int best2[2] = { -1, -1 }; + + for (int face1 = 0; face1 < (int)faces.size(); ++face1) + { + Vec3 normal1 = mPlanes[faces[face1]].GetNormal(); + for (int face2 = face1 + 1; face2 < (int)faces.size(); ++face2) + { + Vec3 normal2 = mPlanes[faces[face2]].GetNormal(); + Vec3 cross = normal1.Cross(normal2); + + // Determine the 2 face normals that are most apart + float dot = normal1.Dot(normal2); + if (dot < smallest_dot) + { + smallest_dot = dot; + best2[0] = faces[face1]; + best2[1] = faces[face2]; + } + + // Determine the 3 face normals that form the largest tetrahedron + for (int face3 = face2 + 1; face3 < (int)faces.size(); ++face3) + { + Vec3 normal3 = mPlanes[faces[face3]].GetNormal(); + float volume = abs(cross.Dot(normal3)); + if (volume > biggest_volume) + { + biggest_volume = volume; + best3[0] = faces[face1]; + best3[1] = faces[face2]; + best3[2] = faces[face3]; + } + } + } + } + + // If we didn't find 3 planes, use 2, if we didn't find 2 use 1 + if (best3[0] != -1) + faces = { best3[0], best3[1], best3[2] }; + else if (best2[0] != -1) + faces = { best2[0], best2[1] }; + else + faces = { faces[0] }; + + // Copy the faces to the points buffer + Point &point = mPoints[p]; + point.mNumFaces = (int)faces.size(); + for (int i = 0; i < (int)faces.size(); ++i) + point.mFaces[i] = faces[i]; + } + + // If the convex radius is already zero, there's no point in further reducing it + if (mConvexRadius > 0.0f) + { + // Find out how thin the hull is by walking over all planes and checking the thickness of the hull in that direction + float min_size = FLT_MAX; + for (const Plane &plane : mPlanes) + { + // Take the point that is furthest away from the plane as thickness of this hull + float max_dist = 0.0f; + for (const Point &point : mPoints) + { + float dist = -plane.SignedDistance(point.mPosition); // Point is always behind plane, so we need to negate + if (dist > max_dist) + max_dist = dist; + } + min_size = min(min_size, max_dist); + } + + // We need to fit in 2x the convex radius in min_size, so reduce the convex radius if it's bigger than that + mConvexRadius = min(mConvexRadius, 0.5f * min_size); + } + + // Now walk over all points and see if we have to further reduce the convex radius because of sharp edges + if (mConvexRadius > 0.0f) + { + for (const Point &point : mPoints) + if (point.mNumFaces != 1) // If we have a single face, shifting back is easy and we don't need to reduce the convex radius + { + // Get first two planes + Plane p1 = mPlanes[point.mFaces[0]]; + Plane p2 = mPlanes[point.mFaces[1]]; + Plane p3; + Vec3 offset_mask; + + if (point.mNumFaces == 3) + { + // Get third plane + p3 = mPlanes[point.mFaces[2]]; + + // All 3 planes will be offset by the convex radius + offset_mask = Vec3::sReplicate(1); + } + else + { + // Third plane has normal perpendicular to the other two planes and goes through the vertex position + JPH_ASSERT(point.mNumFaces == 2); + p3 = Plane::sFromPointAndNormal(point.mPosition, p1.GetNormal().Cross(p2.GetNormal())); + + // Only the first and 2nd plane will be offset, the 3rd plane is only there to guide the intersection point + offset_mask = Vec3(1, 1, 0); + } + + // Plane equation: point . normal + constant = 0 + // Offsetting the plane backwards with convex radius r: point . normal + constant + r = 0 + // To find the intersection 'point' of 3 planes we solve: + // |n1x n1y n1z| |x| | r + c1 | + // |n2x n2y n2z| |y| = - | r + c2 | <=> n point = -r (1, 1, 1) - (c1, c2, c3) + // |n3x n3y n3z| |z| | r + c3 | + // Where point = (x, y, z), n1x is the x component of the first plane, c1 = plane constant of plane 1, etc. + // The relation between how much the intersection point shifts as a function of r is: -r * n^-1 (1, 1, 1) = r * offset + // Where offset = -n^-1 (1, 1, 1) or -n^-1 (1, 1, 0) in case only the first 2 planes are offset + // The error that is introduced by a convex radius r is: error = r * |offset| - r + // So the max convex radius given error is: r = error / (|offset| - 1) + Mat44 n = Mat44(Vec4(p1.GetNormal(), 0), Vec4(p2.GetNormal(), 0), Vec4(p3.GetNormal(), 0), Vec4(0, 0, 0, 1)).Transposed(); + float det_n = n.GetDeterminant3x3(); + if (det_n == 0.0f) + { + // If the determinant is zero, the matrix is not invertible so no solution exists to move the point backwards and we have to choose a convex radius of zero + mConvexRadius = 0.0f; + break; + } + Mat44 adj_n = n.Adjointed3x3(); + float offset = ((adj_n * offset_mask) / det_n).Length(); + JPH_ASSERT(offset > 1.0f); + float max_convex_radius = inSettings.mMaxErrorConvexRadius / (offset - 1.0f); + mConvexRadius = min(mConvexRadius, max_convex_radius); + } + } + + // Calculate the inner radius by getting the minimum distance from the origin to the planes of the hull + mInnerRadius = FLT_MAX; + for (const Plane &p : mPlanes) + mInnerRadius = min(mInnerRadius, -p.GetConstant()); + mInnerRadius = max(0.0f, mInnerRadius); // Clamp against zero, this should do nothing as the shape is centered around the center of mass but for flat convex hulls there may be numerical round off issues + + outResult.Set(this); +} + +MassProperties ConvexHullShape::GetMassProperties() const +{ + MassProperties p; + + float density = GetDensity(); + + // Calculate mass + p.mMass = density * mVolume; + + // Calculate inertia matrix + p.mInertia = density * mInertia; + p.mInertia(3, 3) = 1.0f; + + return p; +} + +Vec3 ConvexHullShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + const Plane &first_plane = mPlanes[0]; + Vec3 best_normal = first_plane.GetNormal(); + float best_dist = abs(first_plane.SignedDistance(inLocalSurfacePosition)); + + // Find the face that has the shortest distance to the surface point + for (Array::size_type i = 1; i < mFaces.size(); ++i) + { + const Plane &plane = mPlanes[i]; + Vec3 plane_normal = plane.GetNormal(); + float dist = abs(plane.SignedDistance(inLocalSurfacePosition)); + if (dist < best_dist) + { + best_dist = dist; + best_normal = plane_normal; + } + } + + return best_normal; +} + +class ConvexHullShape::HullNoConvex final : public Support +{ +public: + explicit HullNoConvex(float inConvexRadius) : + mConvexRadius(inConvexRadius) + { + static_assert(sizeof(HullNoConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(HullNoConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + // Find the point with the highest projection on inDirection + float best_dot = -FLT_MAX; + Vec3 best_point = Vec3::sZero(); + + for (Vec3 point : mPoints) + { + // Check if its support is bigger than the current max + float dot = point.Dot(inDirection); + if (dot > best_dot) + { + best_dot = dot; + best_point = point; + } + } + + return best_point; + } + + virtual float GetConvexRadius() const override + { + return mConvexRadius; + } + + using PointsArray = StaticArray; + + inline PointsArray & GetPoints() + { + return mPoints; + } + + const PointsArray & GetPoints() const + { + return mPoints; + } + +private: + float mConvexRadius; + PointsArray mPoints; +}; + +class ConvexHullShape::HullWithConvex final : public Support +{ +public: + explicit HullWithConvex(const ConvexHullShape *inShape) : + mShape(inShape) + { + static_assert(sizeof(HullWithConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(HullWithConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + // Find the point with the highest projection on inDirection + float best_dot = -FLT_MAX; + Vec3 best_point = Vec3::sZero(); + + for (const Point &point : mShape->mPoints) + { + // Check if its support is bigger than the current max + float dot = point.mPosition.Dot(inDirection); + if (dot > best_dot) + { + best_dot = dot; + best_point = point.mPosition; + } + } + + return best_point; + } + + virtual float GetConvexRadius() const override + { + return 0.0f; + } + +private: + const ConvexHullShape * mShape; +}; + +class ConvexHullShape::HullWithConvexScaled final : public Support +{ +public: + HullWithConvexScaled(const ConvexHullShape *inShape, Vec3Arg inScale) : + mShape(inShape), + mScale(inScale) + { + static_assert(sizeof(HullWithConvexScaled) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(HullWithConvexScaled))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + // Find the point with the highest projection on inDirection + float best_dot = -FLT_MAX; + Vec3 best_point = Vec3::sZero(); + + for (const Point &point : mShape->mPoints) + { + // Calculate scaled position + Vec3 pos = mScale * point.mPosition; + + // Check if its support is bigger than the current max + float dot = pos.Dot(inDirection); + if (dot > best_dot) + { + best_dot = dot; + best_point = pos; + } + } + + return best_point; + } + + virtual float GetConvexRadius() const override + { + return 0.0f; + } + +private: + const ConvexHullShape * mShape; + Vec3 mScale; +}; + +const ConvexShape::Support *ConvexHullShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const +{ + // If there's no convex radius, we don't need to shrink the hull + if (mConvexRadius == 0.0f) + { + if (ScaleHelpers::IsNotScaled(inScale)) + return new (&inBuffer) HullWithConvex(this); + else + return new (&inBuffer) HullWithConvexScaled(this, inScale); + } + + switch (inMode) + { + case ESupportMode::IncludeConvexRadius: + case ESupportMode::Default: + if (ScaleHelpers::IsNotScaled(inScale)) + return new (&inBuffer) HullWithConvex(this); + else + return new (&inBuffer) HullWithConvexScaled(this, inScale); + + case ESupportMode::ExcludeConvexRadius: + if (ScaleHelpers::IsNotScaled(inScale)) + { + // Create support function + HullNoConvex *hull = new (&inBuffer) HullNoConvex(mConvexRadius); + HullNoConvex::PointsArray &transformed_points = hull->GetPoints(); + JPH_ASSERT(mPoints.size() <= cMaxPointsInHull, "Not enough space, this should have been caught during shape creation!"); + + for (const Point &point : mPoints) + { + Vec3 new_point; + + if (point.mNumFaces == 1) + { + // Simply shift back by the convex radius using our 1 plane + new_point = point.mPosition - mPlanes[point.mFaces[0]].GetNormal() * mConvexRadius; + } + else + { + // Get first two planes and offset inwards by convex radius + Plane p1 = mPlanes[point.mFaces[0]].Offset(-mConvexRadius); + Plane p2 = mPlanes[point.mFaces[1]].Offset(-mConvexRadius); + Plane p3; + + if (point.mNumFaces == 3) + { + // Get third plane and offset inwards by convex radius + p3 = mPlanes[point.mFaces[2]].Offset(-mConvexRadius); + } + else + { + // Third plane has normal perpendicular to the other two planes and goes through the vertex position + JPH_ASSERT(point.mNumFaces == 2); + p3 = Plane::sFromPointAndNormal(point.mPosition, p1.GetNormal().Cross(p2.GetNormal())); + } + + // Find intersection point between the three planes + if (!Plane::sIntersectPlanes(p1, p2, p3, new_point)) + { + // Fallback: Just push point back using the first plane + new_point = point.mPosition - p1.GetNormal() * mConvexRadius; + } + } + + // Add point + transformed_points.push_back(new_point); + } + + return hull; + } + else + { + // Calculate scaled convex radius + float convex_radius = ScaleHelpers::ScaleConvexRadius(mConvexRadius, inScale); + + // Create new support function + HullNoConvex *hull = new (&inBuffer) HullNoConvex(convex_radius); + HullNoConvex::PointsArray &transformed_points = hull->GetPoints(); + JPH_ASSERT(mPoints.size() <= cMaxPointsInHull, "Not enough space, this should have been caught during shape creation!"); + + // Precalculate inverse scale + Vec3 inv_scale = inScale.Reciprocal(); + + for (const Point &point : mPoints) + { + // Calculate scaled position + Vec3 pos = inScale * point.mPosition; + + // Transform normals for plane 1 with scale + Vec3 n1 = (inv_scale * mPlanes[point.mFaces[0]].GetNormal()).Normalized(); + + Vec3 new_point; + + if (point.mNumFaces == 1) + { + // Simply shift back by the convex radius using our 1 plane + new_point = pos - n1 * convex_radius; + } + else + { + // Transform normals for plane 2 with scale + Vec3 n2 = (inv_scale * mPlanes[point.mFaces[1]].GetNormal()).Normalized(); + + // Get first two planes and offset inwards by convex radius + Plane p1 = Plane::sFromPointAndNormal(pos, n1).Offset(-convex_radius); + Plane p2 = Plane::sFromPointAndNormal(pos, n2).Offset(-convex_radius); + Plane p3; + + if (point.mNumFaces == 3) + { + // Transform last normal with scale + Vec3 n3 = (inv_scale * mPlanes[point.mFaces[2]].GetNormal()).Normalized(); + + // Get third plane and offset inwards by convex radius + p3 = Plane::sFromPointAndNormal(pos, n3).Offset(-convex_radius); + } + else + { + // Third plane has normal perpendicular to the other two planes and goes through the vertex position + JPH_ASSERT(point.mNumFaces == 2); + p3 = Plane::sFromPointAndNormal(pos, n1.Cross(n2)); + } + + // Find intersection point between the three planes + if (!Plane::sIntersectPlanes(p1, p2, p3, new_point)) + { + // Fallback: Just push point back using the first plane + new_point = pos - n1 * convex_radius; + } + } + + // Add point + transformed_points.push_back(new_point); + } + + return hull; + } + } + + JPH_ASSERT(false); + return nullptr; +} + +void ConvexHullShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + Vec3 inv_scale = inScale.Reciprocal(); + + // Need to transform the plane normals using inScale + // Transforming a direction with matrix M is done through multiplying by (M^-1)^T + // In this case M is a diagonal matrix with the scale vector, so we need to multiply our normal by 1 / scale and renormalize afterwards + Vec3 plane0_normal = inv_scale * mPlanes[0].GetNormal(); + float best_dot = plane0_normal.Dot(inDirection) / plane0_normal.Length(); + int best_face_idx = 0; + + for (Array::size_type i = 1; i < mPlanes.size(); ++i) + { + Vec3 plane_normal = inv_scale * mPlanes[i].GetNormal(); + float dot = plane_normal.Dot(inDirection) / plane_normal.Length(); + if (dot < best_dot) + { + best_dot = dot; + best_face_idx = (int)i; + } + } + + // Get vertices + const Face &best_face = mFaces[best_face_idx]; + const uint8 *first_vtx = mVertexIdx.data() + best_face.mFirstVertex; + const uint8 *end_vtx = first_vtx + best_face.mNumVertices; + + // If we have more than 1/2 the capacity of outVertices worth of vertices, we start skipping vertices (note we can't fill the buffer completely since extra edges will be generated by clipping). + // TODO: This really needs a better algorithm to determine which vertices are important! + int max_vertices_to_return = outVertices.capacity() / 2; + int delta_vtx = (int(best_face.mNumVertices) + max_vertices_to_return) / max_vertices_to_return; + + // Calculate transform with scale + Mat44 transform = inCenterOfMassTransform.PreScaled(inScale); + + if (ScaleHelpers::IsInsideOut(inScale)) + { + // Flip winding of supporting face + for (const uint8 *v = end_vtx - 1; v >= first_vtx; v -= delta_vtx) + outVertices.push_back(transform * mPoints[*v].mPosition); + } + else + { + // Normal winding of supporting face + for (const uint8 *v = first_vtx; v < end_vtx; v += delta_vtx) + outVertices.push_back(transform * mPoints[*v].mPosition); + } +} + +void ConvexHullShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + // Trivially calculate total volume + Vec3 abs_scale = inScale.Abs(); + outTotalVolume = mVolume * abs_scale.GetX() * abs_scale.GetY() * abs_scale.GetZ(); + + // Check if shape has been scaled inside out + bool is_inside_out = ScaleHelpers::IsInsideOut(inScale); + + // Convert the points to world space and determine the distance to the surface + int num_points = int(mPoints.size()); + PolyhedronSubmergedVolumeCalculator::Point *buffer = (PolyhedronSubmergedVolumeCalculator::Point *)JPH_STACK_ALLOC(num_points * sizeof(PolyhedronSubmergedVolumeCalculator::Point)); + PolyhedronSubmergedVolumeCalculator submerged_vol_calc(inCenterOfMassTransform * Mat44::sScale(inScale), &mPoints[0].mPosition, sizeof(Point), num_points, inSurface, buffer JPH_IF_DEBUG_RENDERER(, inBaseOffset)); + + if (submerged_vol_calc.AreAllAbove()) + { + // We're above the water + outSubmergedVolume = 0.0f; + outCenterOfBuoyancy = Vec3::sZero(); + } + else if (submerged_vol_calc.AreAllBelow()) + { + // We're fully submerged + outSubmergedVolume = outTotalVolume; + outCenterOfBuoyancy = inCenterOfMassTransform.GetTranslation(); + } + else + { + // Calculate submerged volume + int reference_point_idx = submerged_vol_calc.GetReferencePointIdx(); + for (const Face &f : mFaces) + { + const uint8 *first_vtx = mVertexIdx.data() + f.mFirstVertex; + const uint8 *end_vtx = first_vtx + f.mNumVertices; + + // If any of the vertices of this face are the reference point, the volume will be zero so we can skip this face + bool degenerate = false; + for (const uint8 *v = first_vtx; v < end_vtx; ++v) + if (*v == reference_point_idx) + { + degenerate = true; + break; + } + if (degenerate) + continue; + + // Triangulate the face + int i1 = *first_vtx; + if (is_inside_out) + { + // Reverse winding + for (const uint8 *v = first_vtx + 2; v < end_vtx; ++v) + { + int i2 = *(v - 1); + int i3 = *v; + submerged_vol_calc.AddFace(i1, i3, i2); + } + } + else + { + // Normal winding + for (const uint8 *v = first_vtx + 2; v < end_vtx; ++v) + { + int i2 = *(v - 1); + int i3 = *v; + submerged_vol_calc.AddFace(i1, i2, i3); + } + } + } + + // Get the results + submerged_vol_calc.GetResult(outSubmergedVolume, outCenterOfBuoyancy); + } + +#ifdef JPH_DEBUG_RENDERER + // Draw center of buoyancy + if (sDrawSubmergedVolumes) + DebugRenderer::sInstance->DrawWireSphere(inBaseOffset + outCenterOfBuoyancy, 0.05f, Color::sRed, 1); +#endif // JPH_DEBUG_RENDERER +} + +#ifdef JPH_DEBUG_RENDERER +void ConvexHullShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + if (mGeometry == nullptr) + { + Array triangles; + for (const Face &f : mFaces) + { + const uint8 *first_vtx = mVertexIdx.data() + f.mFirstVertex; + const uint8 *end_vtx = first_vtx + f.mNumVertices; + + // Draw first triangle of polygon + Vec3 v0 = mPoints[first_vtx[0]].mPosition; + Vec3 v1 = mPoints[first_vtx[1]].mPosition; + Vec3 v2 = mPoints[first_vtx[2]].mPosition; + Vec3 uv_direction = (v1 - v0).Normalized(); + triangles.push_back({ v0, v1, v2, Color::sWhite, v0, uv_direction }); + + // Draw any other triangles in this polygon + for (const uint8 *v = first_vtx + 3; v < end_vtx; ++v) + triangles.push_back({ v0, mPoints[*(v - 1)].mPosition, mPoints[*v].mPosition, Color::sWhite, v0, uv_direction }); + } + mGeometry = new DebugRenderer::Geometry(inRenderer->CreateTriangleBatch(triangles), GetLocalBounds()); + } + + // Test if the shape is scaled inside out + DebugRenderer::ECullMode cull_mode = ScaleHelpers::IsInsideOut(inScale)? DebugRenderer::ECullMode::CullFrontFace : DebugRenderer::ECullMode::CullBackFace; + + // Determine the draw mode + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + + // Draw the geometry + Color color = inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor; + RMat44 transform = inCenterOfMassTransform.PreScaled(inScale); + inRenderer->DrawGeometry(transform, color, mGeometry, cull_mode, DebugRenderer::ECastShadow::On, draw_mode); + + // Draw the outline if requested + if (sDrawFaceOutlines) + for (const Face &f : mFaces) + { + const uint8 *first_vtx = mVertexIdx.data() + f.mFirstVertex; + const uint8 *end_vtx = first_vtx + f.mNumVertices; + + // Draw edges of face + inRenderer->DrawLine(transform * mPoints[*(end_vtx - 1)].mPosition, transform * mPoints[*first_vtx].mPosition, Color::sGrey); + for (const uint8 *v = first_vtx + 1; v < end_vtx; ++v) + inRenderer->DrawLine(transform * mPoints[*(v - 1)].mPosition, transform * mPoints[*v].mPosition, Color::sGrey); + } +} + +void ConvexHullShape::DrawShrunkShape(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + // Get the shrunk points + SupportBuffer buffer; + const HullNoConvex *support = mConvexRadius > 0.0f? static_cast(GetSupportFunction(ESupportMode::ExcludeConvexRadius, buffer, inScale)) : nullptr; + + RMat44 transform = inCenterOfMassTransform * Mat44::sScale(inScale); + + for (int p = 0; p < (int)mPoints.size(); ++p) + { + const Point &point = mPoints[p]; + RVec3 position = transform * point.mPosition; + RVec3 shrunk_point = support != nullptr? transform * support->GetPoints()[p] : position; + + // Draw difference between shrunk position and position + inRenderer->DrawLine(position, shrunk_point, Color::sGreen); + + // Draw face normals that are contributing + for (int i = 0; i < point.mNumFaces; ++i) + inRenderer->DrawLine(position, position + 0.1f * mPlanes[point.mFaces[i]].GetNormal(), Color::sYellow); + + // Draw point index + inRenderer->DrawText3D(position, ConvertToString(p), Color::sWhite, 0.1f); + } +} +#endif // JPH_DEBUG_RENDERER + +bool ConvexHullShape::CastRayHelper(const RayCast &inRay, float &outMinFraction, float &outMaxFraction) const +{ + if (mFaces.size() == 2) + { + // If we have only 2 faces, we're a flat convex hull and we need to test edges instead of planes + + // Check if plane is parallel to ray + const Plane &p = mPlanes.front(); + Vec3 plane_normal = p.GetNormal(); + float direction_projection = inRay.mDirection.Dot(plane_normal); + if (abs(direction_projection) >= 1.0e-12f) + { + // Calculate intersection point + float distance_to_plane = inRay.mOrigin.Dot(plane_normal) + p.GetConstant(); + float fraction = -distance_to_plane / direction_projection; + if (fraction < 0.0f || fraction > 1.0f) + { + // Does not hit plane, no hit + outMinFraction = 0.0f; + outMaxFraction = 1.0f + FLT_EPSILON; + return false; + } + Vec3 intersection_point = inRay.mOrigin + fraction * inRay.mDirection; + + // Test all edges to see if point is inside polygon + const Face &f = mFaces.front(); + const uint8 *first_vtx = mVertexIdx.data() + f.mFirstVertex; + const uint8 *end_vtx = first_vtx + f.mNumVertices; + Vec3 p1 = mPoints[*end_vtx].mPosition; + for (const uint8 *v = first_vtx; v < end_vtx; ++v) + { + Vec3 p2 = mPoints[*v].mPosition; + if ((p2 - p1).Cross(intersection_point - p1).Dot(plane_normal) < 0.0f) + { + // Outside polygon, no hit + outMinFraction = 0.0f; + outMaxFraction = 1.0f + FLT_EPSILON; + return false; + } + p1 = p2; + } + + // Inside polygon, a hit + outMinFraction = fraction; + outMaxFraction = fraction; + return true; + } + else + { + // Parallel ray doesn't hit + outMinFraction = 0.0f; + outMaxFraction = 1.0f + FLT_EPSILON; + return false; + } + } + else + { + // Clip ray against all planes + int fractions_set = 0; + bool all_inside = true; + float min_fraction = 0.0f, max_fraction = 1.0f + FLT_EPSILON; + for (const Plane &p : mPlanes) + { + // Check if the ray origin is behind this plane + Vec3 plane_normal = p.GetNormal(); + float distance_to_plane = inRay.mOrigin.Dot(plane_normal) + p.GetConstant(); + bool is_outside = distance_to_plane > 0.0f; + all_inside &= !is_outside; + + // Check if plane is parallel to ray + float direction_projection = inRay.mDirection.Dot(plane_normal); + if (abs(direction_projection) >= 1.0e-12f) + { + // Get intersection fraction between ray and plane + float fraction = -distance_to_plane / direction_projection; + + // Update interval of ray that is inside the hull + if (direction_projection < 0.0f) + { + min_fraction = max(fraction, min_fraction); + fractions_set |= 1; + } + else + { + max_fraction = min(fraction, max_fraction); + fractions_set |= 2; + } + } + else if (is_outside) + return false; // Outside the plane and parallel, no hit! + } + + // Test if both min and max have been set + if (fractions_set == 3) + { + // Output fractions + outMinFraction = min_fraction; + outMaxFraction = max_fraction; + + // Test if the infinite ray intersects with the hull (the length will be checked later) + return min_fraction <= max_fraction && max_fraction >= 0.0f; + } + else + { + // Degenerate case, either the ray is parallel to all planes or the ray has zero length + outMinFraction = 0.0f; + outMaxFraction = 1.0f + FLT_EPSILON; + + // Return if the origin is inside the hull + return all_inside; + } + } +} + +bool ConvexHullShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + // Determine if ray hits the shape + float min_fraction, max_fraction; + if (CastRayHelper(inRay, min_fraction, max_fraction) + && min_fraction < ioHit.mFraction) // Check if this is a closer hit + { + // Better hit than the current hit + ioHit.mFraction = min_fraction; + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + return false; +} + +void ConvexHullShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Determine if ray hits the shape + float min_fraction, max_fraction; + if (CastRayHelper(inRay, min_fraction, max_fraction) + && min_fraction < ioCollector.GetEarlyOutFraction()) // Check if this is closer than the early out fraction + { + // Better hit than the current hit + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + hit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + + // Check front side hit + if (inRayCastSettings.mTreatConvexAsSolid || min_fraction > 0.0f) + { + hit.mFraction = min_fraction; + ioCollector.AddHit(hit); + } + + // Check back side hit + if (inRayCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces + && max_fraction < ioCollector.GetEarlyOutFraction()) + { + hit.mFraction = max_fraction; + ioCollector.AddHit(hit); + } + } +} + +void ConvexHullShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Check if point is behind all planes + for (const Plane &p : mPlanes) + if (p.SignedDistance(inPoint) > 0.0f) + return; + + // Point is inside + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); +} + +void ConvexHullShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation(); + + Vec3 inv_scale = inScale.Reciprocal(); + bool is_not_scaled = ScaleHelpers::IsNotScaled(inScale); + float scale_flip = ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f; + + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) + { + Vec3 local_pos = inverse_transform * v.GetPosition(); + + // Find most facing plane + float max_distance = -FLT_MAX; + Vec3 max_plane_normal = Vec3::sZero(); + uint max_plane_idx = 0; + if (is_not_scaled) + { + // Without scale, it is trivial to calculate the distance to the hull + for (const Plane &p : mPlanes) + { + float distance = p.SignedDistance(local_pos); + if (distance > max_distance) + { + max_distance = distance; + max_plane_normal = p.GetNormal(); + max_plane_idx = uint(&p - mPlanes.data()); + } + } + } + else + { + // When there's scale we need to calculate the planes first + for (uint i = 0; i < (uint)mPlanes.size(); ++i) + { + // Calculate plane normal and point by scaling the original plane + Vec3 plane_normal = (inv_scale * mPlanes[i].GetNormal()).Normalized(); + Vec3 plane_point = inScale * mPoints[mVertexIdx[mFaces[i].mFirstVertex]].mPosition; + + float distance = plane_normal.Dot(local_pos - plane_point); + if (distance > max_distance) + { + max_distance = distance; + max_plane_normal = plane_normal; + max_plane_idx = i; + } + } + } + bool is_outside = max_distance > 0.0f; + + // Project point onto that plane + Vec3 closest_point = local_pos - 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) + if (is_outside) + { + // Loop over edges + float closest_point_dist_sq = FLT_MAX; + const Face &face = mFaces[max_plane_idx]; + for (const uint8 *v_start = &mVertexIdx[face.mFirstVertex], *v1 = v_start, *v_end = v_start + face.mNumVertices; v1 < v_end; ++v1) + { + // Find second point + const uint8 *v2 = v1 + 1; + if (v2 == v_end) + v2 = v_start; + + // Get edge points + Vec3 p1 = inScale * mPoints[*v1].mPosition; + Vec3 p2 = inScale * mPoints[*v2].mPosition; + + // Check if the position is outside the edge (if not, the face will be closer) + Vec3 edge_normal = (p2 - p1).Cross(max_plane_normal); + if (scale_flip * edge_normal.Dot(local_pos - p1) > 0.0f) + { + // Get closest point on edge + uint32 set; + 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; + } + } + } + + // Check if this is the largest penetration + Vec3 normal = local_pos - closest_point; + float normal_length = normal.Length(); + float penetration = normal_length; + if (is_outside) + penetration = -penetration; + else + normal = -normal; + 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); + + // Store collision + v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); + } + } +} + +class ConvexHullShape::CHSGetTrianglesContext +{ +public: + CHSGetTrianglesContext(Mat44Arg inTransform, bool inIsInsideOut) : mTransform(inTransform), mIsInsideOut(inIsInsideOut) { } + + Mat44 mTransform; + bool mIsInsideOut; + size_t mCurrentFace = 0; +}; + +void ConvexHullShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + static_assert(sizeof(CHSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(&ioContext, alignof(CHSGetTrianglesContext))); + + new (&ioContext) CHSGetTrianglesContext(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale), ScaleHelpers::IsInsideOut(inScale)); +} + +int ConvexHullShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + static_assert(cGetTrianglesMinTrianglesRequested >= 12, "cGetTrianglesMinTrianglesRequested is too small"); + JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested); + + CHSGetTrianglesContext &context = (CHSGetTrianglesContext &)ioContext; + + int total_num_triangles = 0; + for (; context.mCurrentFace < mFaces.size(); ++context.mCurrentFace) + { + const Face &f = mFaces[context.mCurrentFace]; + + const uint8 *first_vtx = mVertexIdx.data() + f.mFirstVertex; + const uint8 *end_vtx = first_vtx + f.mNumVertices; + + // Check if there is still room in the output buffer for this face + int num_triangles = f.mNumVertices - 2; + inMaxTrianglesRequested -= num_triangles; + if (inMaxTrianglesRequested < 0) + break; + total_num_triangles += num_triangles; + + // Get first triangle of polygon + Vec3 v0 = context.mTransform * mPoints[first_vtx[0]].mPosition; + Vec3 v1 = context.mTransform * mPoints[first_vtx[1]].mPosition; + Vec3 v2 = context.mTransform * mPoints[first_vtx[2]].mPosition; + v0.StoreFloat3(outTriangleVertices++); + if (context.mIsInsideOut) + { + // Store first triangle in this polygon flipped + v2.StoreFloat3(outTriangleVertices++); + v1.StoreFloat3(outTriangleVertices++); + + // Store other triangles in this polygon flipped + for (const uint8 *v = first_vtx + 3; v < end_vtx; ++v) + { + v0.StoreFloat3(outTriangleVertices++); + (context.mTransform * mPoints[*v].mPosition).StoreFloat3(outTriangleVertices++); + (context.mTransform * mPoints[*(v - 1)].mPosition).StoreFloat3(outTriangleVertices++); + } + } + else + { + // Store first triangle in this polygon + v1.StoreFloat3(outTriangleVertices++); + v2.StoreFloat3(outTriangleVertices++); + + // Store other triangles in this polygon + for (const uint8 *v = first_vtx + 3; v < end_vtx; ++v) + { + v0.StoreFloat3(outTriangleVertices++); + (context.mTransform * mPoints[*(v - 1)].mPosition).StoreFloat3(outTriangleVertices++); + (context.mTransform * mPoints[*v].mPosition).StoreFloat3(outTriangleVertices++); + } + } + } + + // Store materials + if (outMaterials != nullptr) + { + const PhysicsMaterial *material = GetMaterial(); + for (const PhysicsMaterial **m = outMaterials, **m_end = outMaterials + total_num_triangles; m < m_end; ++m) + *m = material; + } + + return total_num_triangles; +} + +void ConvexHullShape::SaveBinaryState(StreamOut &inStream) const +{ + ConvexShape::SaveBinaryState(inStream); + + inStream.Write(mCenterOfMass); + inStream.Write(mInertia); + inStream.Write(mLocalBounds.mMin); + inStream.Write(mLocalBounds.mMax); + inStream.Write(mPoints); + inStream.Write(mFaces); + inStream.Write(mPlanes); + inStream.Write(mVertexIdx); + inStream.Write(mConvexRadius); + inStream.Write(mVolume); + inStream.Write(mInnerRadius); +} + +void ConvexHullShape::RestoreBinaryState(StreamIn &inStream) +{ + ConvexShape::RestoreBinaryState(inStream); + + inStream.Read(mCenterOfMass); + inStream.Read(mInertia); + inStream.Read(mLocalBounds.mMin); + inStream.Read(mLocalBounds.mMax); + inStream.Read(mPoints); + inStream.Read(mFaces); + inStream.Read(mPlanes); + inStream.Read(mVertexIdx); + inStream.Read(mConvexRadius); + inStream.Read(mVolume); + inStream.Read(mInnerRadius); +} + +Shape::Stats ConvexHullShape::GetStats() const +{ + // Count number of triangles + uint triangle_count = 0; + for (const Face &f : mFaces) + triangle_count += f.mNumVertices - 2; + + return Stats( + sizeof(*this) + + mPoints.size() * sizeof(Point) + + mFaces.size() * sizeof(Face) + + mPlanes.size() * sizeof(Plane) + + mVertexIdx.size() * sizeof(uint8), + triangle_count); +} + +void ConvexHullShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::ConvexHull); + f.mConstruct = []() -> Shape * { return new ConvexHullShape; }; + f.mColor = Color::sGreen; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.h new file mode 100644 index 000000000000..4068791f431a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.h @@ -0,0 +1,202 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a ConvexHullShape +class JPH_EXPORT ConvexHullShapeSettings final : public ConvexShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, ConvexHullShapeSettings) + +public: + /// Default constructor for deserialization + ConvexHullShapeSettings() = default; + + /// Create a convex hull from inPoints and maximum convex radius inMaxConvexRadius, the radius is automatically lowered if the hull requires it. + /// (internally this will be subtracted so the total size will not grow with the convex radius). + ConvexHullShapeSettings(const Vec3 *inPoints, int inNumPoints, float inMaxConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mPoints(inPoints, inPoints + inNumPoints), mMaxConvexRadius(inMaxConvexRadius) { } + ConvexHullShapeSettings(const Array &inPoints, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mPoints(inPoints), mMaxConvexRadius(inConvexRadius) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + Array mPoints; ///< Points to create the hull from + float mMaxConvexRadius = 0.0f; ///< Convex radius as supplied by the constructor. Note that during hull creation the convex radius can be made smaller if the value is too big for the hull. + float mMaxErrorConvexRadius = 0.05f; ///< Maximum distance between the shrunk hull + convex radius and the actual hull. + float mHullTolerance = 1.0e-3f; ///< Points are allowed this far outside of the hull (increasing this yields a hull with less vertices). Note that the actual used value can be larger if the points of the hull are far apart. +}; + +/// A convex hull +class JPH_EXPORT ConvexHullShape final : public ConvexShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Maximum amount of points supported in a convex hull. Note that while constructing a hull, interior points are discarded so you can provide more points. + /// The ConvexHullShapeSettings::Create function will return an error when too many points are provided. + static constexpr int cMaxPointsInHull = 256; + + /// Constructor + ConvexHullShape() : ConvexShape(EShapeSubType::ConvexHull) { } + ConvexHullShape(const ConvexHullShapeSettings &inSettings, ShapeResult &outResult); + + // See Shape::GetCenterOfMass + virtual Vec3 GetCenterOfMass() const override { return mCenterOfMass; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override { return mLocalBounds; } + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mInnerRadius; } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See ConvexShape::GetSupportFunction + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; + + /// Debugging helper draw function that draws how all points are moved when a shape is shrunk by the convex radius + void DrawShrunkShape(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override; + + // See Shape::GetVolume + virtual float GetVolume() const override { return mVolume; } + + /// Get the convex radius of this convex hull + float GetConvexRadius() const { return mConvexRadius; } + + /// Get the planes of this convex hull + const Array & GetPlanes() const { return mPlanes; } + + /// Get the number of vertices in this convex hull + inline uint GetNumPoints() const { return uint(mPoints.size()); } + + /// Get a vertex of this convex hull relative to the center of mass + inline Vec3 GetPoint(uint inIndex) const { return mPoints[inIndex].mPosition; } + + /// Get the number of faces in this convex hull + inline uint GetNumFaces() const { return uint(mFaces.size()); } + + /// Get the number of vertices in a face + inline uint GetNumVerticesInFace(uint inFaceIndex) const { return mFaces[inFaceIndex].mNumVertices; } + + /// Get the vertices indices of a face + /// @param inFaceIndex Index of the face. + /// @param inMaxVertices Maximum number of vertices to return. + /// @param outVertices Array of vertices indices, must be at least inMaxVertices in size, the vertices are returned in counter clockwise order and the positions can be obtained using GetPoint(index). + /// @return Number of vertices in face, if this is bigger than inMaxVertices, not all vertices were retrieved. + inline uint GetFaceVertices(uint inFaceIndex, uint inMaxVertices, uint *outVertices) const + { + const Face &face = mFaces[inFaceIndex]; + const uint8 *first_vertex = mVertexIdx.data() + face.mFirstVertex; + uint num_vertices = min(face.mNumVertices, inMaxVertices); + for (uint i = 0; i < num_vertices; ++i) + outVertices[i] = first_vertex[i]; + return face.mNumVertices; + } + + // Register shape functions with the registry + static void sRegister(); + +#ifdef JPH_DEBUG_RENDERER + /// Draw the outlines of the faces of the convex hull when drawing the shape + inline static bool sDrawFaceOutlines = false; +#endif // JPH_DEBUG_RENDERER + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + /// Helper function that returns the min and max fraction along the ray that hits the convex hull. Returns false if there is no hit. + bool CastRayHelper(const RayCast &inRay, float &outMinFraction, float &outMaxFraction) const; + + /// Class for GetTrianglesStart/Next + class CHSGetTrianglesContext; + + /// Classes for GetSupportFunction + class HullNoConvex; + class HullWithConvex; + class HullWithConvexScaled; + + struct Face + { + uint16 mFirstVertex; ///< First index in mVertexIdx to use + uint16 mNumVertices = 0; ///< Number of vertices in the mVertexIdx to use + }; + + static_assert(sizeof(Face) == 4, "Unexpected size"); + static_assert(alignof(Face) == 2, "Unexpected alignment"); + + struct Point + { + Vec3 mPosition; ///< Position of vertex + int mNumFaces = 0; ///< Number of faces in the face array below + int mFaces[3] = { -1, -1, -1 }; ///< Indices of 3 neighboring faces with the biggest difference in normal (used to shift vertices for convex radius) + }; + + static_assert(sizeof(Point) == 32, "Unexpected size"); + static_assert(alignof(Point) == JPH_VECTOR_ALIGNMENT, "Unexpected alignment"); + + Vec3 mCenterOfMass; ///< Center of mass of this convex hull + Mat44 mInertia; ///< Inertia matrix assuming density is 1 (needs to be multiplied by density) + AABox mLocalBounds; ///< Local bounding box for the convex hull + Array mPoints; ///< Points on the convex hull surface + Array mFaces; ///< Faces of the convex hull surface + Array mPlanes; ///< Planes for the faces (1-on-1 with mFaces array, separate because they need to be 16 byte aligned) + Array mVertexIdx; ///< A list of vertex indices (indexing in mPoints) for each of the faces + float mConvexRadius = 0.0f; ///< Convex radius + float mVolume; ///< Total volume of the convex hull + float mInnerRadius = FLT_MAX; ///< Radius of the biggest sphere that fits entirely in the convex hull + +#ifdef JPH_DEBUG_RENDERER + mutable DebugRenderer::GeometryRef mGeometry; +#endif // JPH_DEBUG_RENDERER +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexShape.cpp new file mode 100644 index 000000000000..35e23ee01cb6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexShape.cpp @@ -0,0 +1,559 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(ConvexShapeSettings) +{ + JPH_ADD_BASE_CLASS(ConvexShapeSettings, ShapeSettings) + + JPH_ADD_ATTRIBUTE(ConvexShapeSettings, mDensity) + JPH_ADD_ATTRIBUTE(ConvexShapeSettings, mMaterial) +} + +const StaticArray ConvexShape::sUnitSphereTriangles = []() { + const int level = 2; + + StaticArray verts; + GetTrianglesContextVertexList::sCreateHalfUnitSphereTop(verts, level); + GetTrianglesContextVertexList::sCreateHalfUnitSphereBottom(verts, level); + return verts; +}(); + +void ConvexShape::sCollideConvexVsConvex(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, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + // Get the shapes + JPH_ASSERT(inShape1->GetType() == EShapeType::Convex); + JPH_ASSERT(inShape2->GetType() == EShapeType::Convex); + const ConvexShape *shape1 = static_cast(inShape1); + const ConvexShape *shape2 = static_cast(inShape2); + + // Get transforms + Mat44 inverse_transform1 = inCenterOfMassTransform1.InversedRotationTranslation(); + Mat44 transform_2_to_1 = inverse_transform1 * inCenterOfMassTransform2; + + // Get bounding boxes + AABox shape1_bbox = shape1->GetLocalBounds().Scaled(inScale1); + shape1_bbox.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance)); + AABox shape2_bbox = shape2->GetLocalBounds().Scaled(inScale2); + + // Check if they overlap + if (!OrientedBox(transform_2_to_1, shape2_bbox).Overlaps(shape1_bbox)) + return; + + // Note: As we don't remember the penetration axis from the last iteration, and it is likely that shape2 is pushed out of + // collision relative to shape1 by comparing their COM's, we use that as an initial penetration axis: shape2.com - shape1.com + // This has been seen to improve performance by approx. 1% over using a fixed axis like (1, 0, 0). + Vec3 penetration_axis = transform_2_to_1.GetTranslation(); + + // Ensure that we do not pass in a near zero penetration axis + if (penetration_axis.IsNearZero()) + penetration_axis = Vec3::sAxisX(); + + Vec3 point1, point2; + EPAPenetrationDepth pen_depth; + EPAPenetrationDepth::EStatus status; + + // Scope to limit lifetime of SupportBuffer + { + // Create support function + SupportBuffer buffer1_excl_cvx_radius, buffer2_excl_cvx_radius; + const Support *shape1_excl_cvx_radius = shape1->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, buffer1_excl_cvx_radius, inScale1); + const Support *shape2_excl_cvx_radius = shape2->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, buffer2_excl_cvx_radius, inScale2); + + // Transform shape 2 in the space of shape 1 + TransformedConvexObject transformed2_excl_cvx_radius(transform_2_to_1, *shape2_excl_cvx_radius); + + // Perform GJK step + status = pen_depth.GetPenetrationDepthStepGJK(*shape1_excl_cvx_radius, shape1_excl_cvx_radius->GetConvexRadius() + inCollideShapeSettings.mMaxSeparationDistance, transformed2_excl_cvx_radius, shape2_excl_cvx_radius->GetConvexRadius(), inCollideShapeSettings.mCollisionTolerance, penetration_axis, point1, point2); + } + + // Check result of collision detection + switch (status) + { + case EPAPenetrationDepth::EStatus::Colliding: + break; + + case EPAPenetrationDepth::EStatus::NotColliding: + return; + + case EPAPenetrationDepth::EStatus::Indeterminate: + { + // Need to run expensive EPA algorithm + + // Create support function + SupportBuffer buffer1_incl_cvx_radius, buffer2_incl_cvx_radius; + const Support *shape1_incl_cvx_radius = shape1->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer1_incl_cvx_radius, inScale1); + const Support *shape2_incl_cvx_radius = shape2->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer2_incl_cvx_radius, inScale2); + + // Add separation distance + AddConvexRadius shape1_add_max_separation_distance(*shape1_incl_cvx_radius, inCollideShapeSettings.mMaxSeparationDistance); + + // Transform shape 2 in the space of shape 1 + TransformedConvexObject transformed2_incl_cvx_radius(transform_2_to_1, *shape2_incl_cvx_radius); + + // Perform EPA step + if (!pen_depth.GetPenetrationDepthStepEPA(shape1_add_max_separation_distance, transformed2_incl_cvx_radius, inCollideShapeSettings.mPenetrationTolerance, penetration_axis, point1, point2)) + return; + break; + } + } + + // Check if the penetration is bigger than the early out fraction + float penetration_depth = (point2 - point1).Length() - inCollideShapeSettings.mMaxSeparationDistance; + if (-penetration_depth >= ioCollector.GetEarlyOutFraction()) + return; + + // Correct point1 for the added separation distance + float penetration_axis_len = penetration_axis.Length(); + if (penetration_axis_len > 0.0f) + point1 -= penetration_axis * (inCollideShapeSettings.mMaxSeparationDistance / penetration_axis_len); + + // Convert to world space + point1 = inCenterOfMassTransform1 * point1; + point2 = inCenterOfMassTransform1 * point2; + Vec3 penetration_axis_world = inCenterOfMassTransform1.Multiply3x3(penetration_axis); + + // Create collision result + CollideShapeResult result(point1, point2, penetration_axis_world, penetration_depth, inSubShapeIDCreator1.GetID(), inSubShapeIDCreator2.GetID(), TransformedShape::sGetBodyID(ioCollector.GetContext())); + + // Gather faces + if (inCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces) + { + // Get supporting face of shape 1 + shape1->GetSupportingFace(SubShapeID(), -penetration_axis, inScale1, inCenterOfMassTransform1, result.mShape1Face); + + // Get supporting face of shape 2 + shape2->GetSupportingFace(SubShapeID(), transform_2_to_1.Multiply3x3Transposed(penetration_axis), inScale2, inCenterOfMassTransform2, result.mShape2Face); + } + + // Notify the collector + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) + ioCollector.AddHit(result); +} + +bool ConvexShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + // Note: This is a fallback routine, most convex shapes should implement a more performant version! + + JPH_PROFILE_FUNCTION(); + + // Create support function + SupportBuffer buffer; + const Support *support = GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sReplicate(1.0f)); + + // Cast ray + GJKClosestPoint gjk; + if (gjk.CastRay(inRay.mOrigin, inRay.mDirection, cDefaultCollisionTolerance, *support, ioHit.mFraction)) + { + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + + return false; +} + +void ConvexShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Note: This is a fallback routine, most convex shapes should implement a more performant version! + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // First do a normal raycast, limited to the early out fraction + RayCastResult hit; + hit.mFraction = ioCollector.GetEarlyOutFraction(); + if (CastRay(inRay, inSubShapeIDCreator, hit)) + { + // Check front side + if (inRayCastSettings.mTreatConvexAsSolid || hit.mFraction > 0.0f) + { + hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + ioCollector.AddHit(hit); + } + + // Check if we want back facing hits and the collector still accepts additional hits + if (inRayCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces && !ioCollector.ShouldEarlyOut()) + { + // Invert the ray, going from the early out fraction back to the fraction where we found our forward hit + float start_fraction = min(1.0f, ioCollector.GetEarlyOutFraction()); + float delta_fraction = hit.mFraction - start_fraction; + if (delta_fraction < 0.0f) + { + RayCast inverted_ray { inRay.mOrigin + start_fraction * inRay.mDirection, delta_fraction * inRay.mDirection }; + + // Cast another ray + RayCastResult inverted_hit; + inverted_hit.mFraction = 1.0f; + if (CastRay(inverted_ray, inSubShapeIDCreator, inverted_hit) + && inverted_hit.mFraction > 0.0f) // Ignore hits with fraction 0, this means the ray ends inside the object and we don't want to report it as a back facing hit + { + // Invert fraction and rescale it to the fraction of the original ray + inverted_hit.mFraction = hit.mFraction + (inverted_hit.mFraction - 1.0f) * delta_fraction; + inverted_hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + ioCollector.AddHit(inverted_hit); + } + } + } + } +} + +void ConvexShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // First test bounding box + if (GetLocalBounds().Contains(inPoint)) + { + // Create support function + SupportBuffer buffer; + const Support *support = GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sReplicate(1.0f)); + + // Create support function for point + PointConvexSupport point { inPoint }; + + // Test intersection + GJKClosestPoint gjk; + Vec3 v = inPoint; + if (gjk.Intersects(*support, point, cDefaultCollisionTolerance, v)) + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); + } +} + +void ConvexShape::sCastConvexVsConvex(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + // Only supported for convex shapes + JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Convex); + const ConvexShape *cast_shape = static_cast(inShapeCast.mShape); + + JPH_ASSERT(inShape->GetType() == EShapeType::Convex); + const ConvexShape *shape = static_cast(inShape); + + // Determine if we want to use the actual shape or a shrunken shape with convex radius + ConvexShape::ESupportMode support_mode = inShapeCastSettings.mUseShrunkenShapeAndConvexRadius? ConvexShape::ESupportMode::ExcludeConvexRadius : ConvexShape::ESupportMode::Default; + + // Create support function for shape to cast + SupportBuffer cast_buffer; + const Support *cast_support = cast_shape->GetSupportFunction(support_mode, cast_buffer, inShapeCast.mScale); + + // Create support function for target shape + SupportBuffer target_buffer; + const Support *target_support = shape->GetSupportFunction(support_mode, target_buffer, inScale); + + // Do a raycast against the result + EPAPenetrationDepth epa; + float fraction = ioCollector.GetEarlyOutFraction(); + Vec3 contact_point_a, contact_point_b, contact_normal; + if (epa.CastShape(inShapeCast.mCenterOfMassStart, inShapeCast.mDirection, inShapeCastSettings.mCollisionTolerance, inShapeCastSettings.mPenetrationTolerance, *cast_support, *target_support, cast_support->GetConvexRadius(), target_support->GetConvexRadius(), inShapeCastSettings.mReturnDeepestPoint, fraction, contact_point_a, contact_point_b, contact_normal) + && (inShapeCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces + || contact_normal.Dot(inShapeCast.mDirection) > 0.0f)) // Test if backfacing + { + // Convert to world space + contact_point_a = inCenterOfMassTransform2 * contact_point_a; + contact_point_b = inCenterOfMassTransform2 * contact_point_b; + Vec3 contact_normal_world = inCenterOfMassTransform2.Multiply3x3(contact_normal); + + ShapeCastResult result(fraction, contact_point_a, contact_point_b, contact_normal_world, false, inSubShapeIDCreator1.GetID(), inSubShapeIDCreator2.GetID(), TransformedShape::sGetBodyID(ioCollector.GetContext())); + + // Early out if this hit is deeper than the collector's early out value + if (fraction == 0.0f && -result.mPenetrationDepth >= ioCollector.GetEarlyOutFraction()) + return; + + // Gather faces + if (inShapeCastSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces) + { + // Get supporting face of shape 1 + Mat44 transform_1_to_2 = inShapeCast.mCenterOfMassStart; + transform_1_to_2.SetTranslation(transform_1_to_2.GetTranslation() + fraction * inShapeCast.mDirection); + cast_shape->GetSupportingFace(SubShapeID(), transform_1_to_2.Multiply3x3Transposed(-contact_normal), inShapeCast.mScale, inCenterOfMassTransform2 * transform_1_to_2, result.mShape1Face); + + // Get supporting face of shape 2 + shape->GetSupportingFace(SubShapeID(), contact_normal, inScale, inCenterOfMassTransform2, result.mShape2Face); + } + + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) + ioCollector.AddHit(result); + } +} + +class ConvexShape::CSGetTrianglesContext +{ +public: + CSGetTrianglesContext(const ConvexShape *inShape, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) : + mLocalToWorld(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale)), + mIsInsideOut(ScaleHelpers::IsInsideOut(inScale)) + { + mSupport = inShape->GetSupportFunction(ESupportMode::IncludeConvexRadius, mSupportBuffer, Vec3::sReplicate(1.0f)); + } + + SupportBuffer mSupportBuffer; + const Support * mSupport; + Mat44 mLocalToWorld; + bool mIsInsideOut; + size_t mCurrentVertex = 0; +}; + +void ConvexShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + static_assert(sizeof(CSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(&ioContext, alignof(CSGetTrianglesContext))); + + new (&ioContext) CSGetTrianglesContext(this, inPositionCOM, inRotation, inScale); +} + +int ConvexShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested); + + CSGetTrianglesContext &context = (CSGetTrianglesContext &)ioContext; + + int total_num_vertices = min(inMaxTrianglesRequested * 3, int(sUnitSphereTriangles.size() - context.mCurrentVertex)); + + if (context.mIsInsideOut) + { + // Store triangles flipped + for (const Vec3 *v = sUnitSphereTriangles.data() + context.mCurrentVertex, *v_end = v + total_num_vertices; v < v_end; v += 3) + { + (context.mLocalToWorld * context.mSupport->GetSupport(v[0])).StoreFloat3(outTriangleVertices++); + (context.mLocalToWorld * context.mSupport->GetSupport(v[2])).StoreFloat3(outTriangleVertices++); + (context.mLocalToWorld * context.mSupport->GetSupport(v[1])).StoreFloat3(outTriangleVertices++); + } + } + else + { + // Store triangles + for (const Vec3 *v = sUnitSphereTriangles.data() + context.mCurrentVertex, *v_end = v + total_num_vertices; v < v_end; v += 3) + { + (context.mLocalToWorld * context.mSupport->GetSupport(v[0])).StoreFloat3(outTriangleVertices++); + (context.mLocalToWorld * context.mSupport->GetSupport(v[1])).StoreFloat3(outTriangleVertices++); + (context.mLocalToWorld * context.mSupport->GetSupport(v[2])).StoreFloat3(outTriangleVertices++); + } + } + + context.mCurrentVertex += total_num_vertices; + int total_num_triangles = total_num_vertices / 3; + + // Store materials + if (outMaterials != nullptr) + { + const PhysicsMaterial *material = GetMaterial(); + for (const PhysicsMaterial **m = outMaterials, **m_end = outMaterials + total_num_triangles; m < m_end; ++m) + *m = material; + } + + return total_num_triangles; +} + +void ConvexShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + // Calculate total volume + Vec3 abs_scale = inScale.Abs(); + Vec3 extent = GetLocalBounds().GetExtent() * abs_scale; + outTotalVolume = 8.0f * extent.GetX() * extent.GetY() * extent.GetZ(); + + // Points of the bounding box + Vec3 points[] = + { + Vec3(-1, -1, -1), + Vec3( 1, -1, -1), + Vec3(-1, 1, -1), + Vec3( 1, 1, -1), + Vec3(-1, -1, 1), + Vec3( 1, -1, 1), + Vec3(-1, 1, 1), + Vec3( 1, 1, 1), + }; + + // Faces of the bounding box + using Face = int[5]; + #define MAKE_FACE(a, b, c, d) { a, b, c, d, ((1 << a) | (1 << b) | (1 << c) | (1 << d)) } // Last int is a bit mask that indicates which indices are used + Face faces[] = + { + MAKE_FACE(0, 2, 3, 1), + MAKE_FACE(4, 6, 2, 0), + MAKE_FACE(4, 5, 7, 6), + MAKE_FACE(1, 3, 7, 5), + MAKE_FACE(2, 6, 7, 3), + MAKE_FACE(0, 1, 5, 4), + }; + + PolyhedronSubmergedVolumeCalculator::Point *buffer = (PolyhedronSubmergedVolumeCalculator::Point *)JPH_STACK_ALLOC(8 * sizeof(PolyhedronSubmergedVolumeCalculator::Point)); + PolyhedronSubmergedVolumeCalculator submerged_vol_calc(inCenterOfMassTransform * Mat44::sScale(extent), points, sizeof(Vec3), 8, inSurface, buffer JPH_IF_DEBUG_RENDERER(, inBaseOffset)); + + if (submerged_vol_calc.AreAllAbove()) + { + // We're above the water + outSubmergedVolume = 0.0f; + outCenterOfBuoyancy = Vec3::sZero(); + } + else if (submerged_vol_calc.AreAllBelow()) + { + // We're fully submerged + outSubmergedVolume = outTotalVolume; + outCenterOfBuoyancy = inCenterOfMassTransform.GetTranslation(); + } + else + { + // Calculate submerged volume + int reference_point_bit = 1 << submerged_vol_calc.GetReferencePointIdx(); + for (const Face &f : faces) + { + // Test if this face includes the reference point + if ((f[4] & reference_point_bit) == 0) + { + // Triangulate the face (a quad) + submerged_vol_calc.AddFace(f[0], f[1], f[2]); + submerged_vol_calc.AddFace(f[0], f[2], f[3]); + } + } + + submerged_vol_calc.GetResult(outSubmergedVolume, outCenterOfBuoyancy); + } +} + +#ifdef JPH_DEBUG_RENDERER +void ConvexShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const +{ + // Get the support function with convex radius + SupportBuffer buffer; + const Support *support = GetSupportFunction(ESupportMode::ExcludeConvexRadius, buffer, inScale); + AddConvexRadius add_convex(*support, support->GetConvexRadius()); + + // Draw the shape + DebugRenderer::GeometryRef geometry = inRenderer->CreateTriangleGeometryForConvex([&add_convex](Vec3Arg inDirection) { return add_convex.GetSupport(inDirection); }); + AABox bounds = geometry->mBounds.Transformed(inCenterOfMassTransform); + float lod_scale_sq = geometry->mBounds.GetExtent().LengthSq(); + inRenderer->DrawGeometry(inCenterOfMassTransform, bounds, lod_scale_sq, inColor, geometry); + + if (inDrawSupportDirection) + { + // Iterate on all directions and draw the support point and an arrow in the direction that was sampled to test if the support points make sense + for (Vec3 v : Vec3::sUnitSphere) + { + Vec3 direction = 0.05f * v; + Vec3 pos = add_convex.GetSupport(direction); + RVec3 from = inCenterOfMassTransform * pos; + RVec3 to = inCenterOfMassTransform * (pos + direction); + inRenderer->DrawMarker(from, Color::sWhite, 0.001f); + inRenderer->DrawArrow(from, to, Color::sWhite, 0.001f); + } + } +} + +void ConvexShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + // Sample directions and map which faces belong to which directions + using FaceToDirection = UnorderedMap>; + FaceToDirection faces; + for (Vec3 v : Vec3::sUnitSphere) + { + Vec3 direction = 0.05f * v; + + SupportingFace face; + GetSupportingFace(SubShapeID(), direction, inScale, Mat44::sIdentity(), face); + + if (!face.empty()) + { + JPH_ASSERT(face.size() >= 2, "The GetSupportingFace function should either return nothing or at least an edge"); + faces[face].push_back(direction); + } + } + + // Draw each face in a unique color and draw corresponding directions + int color_it = 0; + for (FaceToDirection::value_type &ftd : faces) + { + Color color = Color::sGetDistinctColor(color_it++); + + // Create copy of face (key in map is read only) + SupportingFace face = ftd.first; + + // Displace the face a little bit forward so it is easier to see + Vec3 normal = face.size() >= 3? (face[2] - face[1]).Cross(face[0] - face[1]).Normalized() : Vec3::sZero(); + Vec3 displacement = 0.001f * normal; + + // Transform face to world space and calculate center of mass + Vec3 com_ls = Vec3::sZero(); + for (Vec3 &v : face) + { + v = inCenterOfMassTransform.Multiply3x3(v + displacement); + com_ls += v; + } + RVec3 com = inCenterOfMassTransform.GetTranslation() + com_ls / (float)face.size(); + + // Draw the polygon and directions + inRenderer->DrawWirePolygon(RMat44::sTranslation(inCenterOfMassTransform.GetTranslation()), face, color, face.size() >= 3? 0.001f : 0.0f); + if (face.size() >= 3) + inRenderer->DrawArrow(com, com + inCenterOfMassTransform.Multiply3x3(normal), color, 0.01f); + for (Vec3 &v : ftd.second) + inRenderer->DrawArrow(com, com + inCenterOfMassTransform.Multiply3x3(-v), color, 0.001f); + } +} +#endif // JPH_DEBUG_RENDERER + +void ConvexShape::SaveBinaryState(StreamOut &inStream) const +{ + Shape::SaveBinaryState(inStream); + + inStream.Write(mDensity); +} + +void ConvexShape::RestoreBinaryState(StreamIn &inStream) +{ + Shape::RestoreBinaryState(inStream); + + inStream.Read(mDensity); +} + +void ConvexShape::SaveMaterialState(PhysicsMaterialList &outMaterials) const +{ + outMaterials.clear(); + outMaterials.push_back(mMaterial); +} + +void ConvexShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) +{ + JPH_ASSERT(inNumMaterials == 1); + mMaterial = inMaterials[0]; +} + +void ConvexShape::sRegister() +{ + for (EShapeSubType s1 : sConvexSubShapeTypes) + for (EShapeSubType s2 : sConvexSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(s1, s2, sCollideConvexVsConvex); + CollisionDispatch::sRegisterCastShape(s1, s2, sCastConvexVsConvex); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexShape.h new file mode 100644 index 000000000000..a4eab8755342 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexShape.h @@ -0,0 +1,150 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; + +/// Class that constructs a ConvexShape (abstract) +class JPH_EXPORT ConvexShapeSettings : public ShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, ConvexShapeSettings) + +public: + /// Constructor + ConvexShapeSettings() = default; + explicit ConvexShapeSettings(const PhysicsMaterial *inMaterial) : mMaterial(inMaterial) { } + + /// Set the density of the object in kg / m^3 + void SetDensity(float inDensity) { mDensity = inDensity; } + + // Properties + RefConst mMaterial; ///< Material assigned to this shape + float mDensity = 1000.0f; ///< Uniform density of the interior of the convex object (kg / m^3) +}; + +/// Base class for all convex shapes. Defines a virtual interface. +class JPH_EXPORT ConvexShape : public Shape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit ConvexShape(EShapeSubType inSubType) : Shape(EShapeType::Convex, inSubType) { } + ConvexShape(EShapeSubType inSubType, const ConvexShapeSettings &inSettings, ShapeResult &outResult) : Shape(EShapeType::Convex, inSubType, inSettings, outResult), mMaterial(inSettings.mMaterial), mDensity(inSettings.mDensity) { } + ConvexShape(EShapeSubType inSubType, const PhysicsMaterial *inMaterial) : Shape(EShapeType::Convex, inSubType), mMaterial(inMaterial) { } + + // See Shape::GetSubShapeIDBitsRecursive + virtual uint GetSubShapeIDBitsRecursive() const override { return 0; } // Convex shapes don't have sub shapes + + // See Shape::GetMaterial + virtual const PhysicsMaterial * GetMaterial([[maybe_unused]] const SubShapeID &inSubShapeID) const override { JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); return GetMaterial(); } + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + + /// Function that provides an interface for GJK + class Support + { + public: + /// Warning: Virtual destructor will not be called on this object! + virtual ~Support() = default; + + /// Calculate the support vector for this convex shape (includes / excludes the convex radius depending on how this was obtained). + /// Support vector is relative to the center of mass of the shape. + virtual Vec3 GetSupport(Vec3Arg inDirection) const = 0; + + /// Convex radius of shape. Collision detection on penetrating shapes is much more expensive, + /// so you can add a radius around objects to increase the shape. This makes it far less likely that they will actually penetrate. + virtual float GetConvexRadius() const = 0; + }; + + /// Buffer to hold a Support object, used to avoid dynamic memory allocations + class alignas(16) SupportBuffer + { + public: + uint8 mData[4160]; + }; + + /// How the GetSupport function should behave + enum class ESupportMode + { + ExcludeConvexRadius, ///< Return the shape excluding the convex radius, Support::GetConvexRadius will return the convex radius if there is one, but adding this radius may not result in the most accurate/efficient representation of shapes with sharp edges + IncludeConvexRadius, ///< Return the shape including the convex radius, Support::GetSupport includes the convex radius if there is one, Support::GetConvexRadius will return 0 + Default, ///< Use both Support::GetSupport add Support::GetConvexRadius to get a support point that matches the original shape as accurately/efficiently as possible + }; + + /// Returns an object that provides the GetSupport function for this shape. + /// inMode determines if this support function includes or excludes the convex radius. + /// of the values returned by the GetSupport function. This improves numerical accuracy of the results. + /// inScale scales this shape in local space. + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const = 0; + + /// Material of the shape + void SetMaterial(const PhysicsMaterial *inMaterial) { mMaterial = inMaterial; } + const PhysicsMaterial * GetMaterial() const { return mMaterial != nullptr? mMaterial : PhysicsMaterial::sDefault; } + + /// Set density of the shape (kg / m^3) + void SetDensity(float inDensity) { mDensity = inDensity; } + + /// Get density of the shape (kg / m^3) + float GetDensity() const { return mDensity; } + +#ifdef JPH_DEBUG_RENDERER + // See Shape::DrawGetSupportFunction + virtual void DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override; + + // See Shape::DrawGetSupportingFace + virtual void DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void SaveMaterialState(PhysicsMaterialList &outMaterials) const override; + virtual void RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + + /// Vertex list that forms a unit sphere + static const StaticArray sUnitSphereTriangles; + +private: + // Class for GetTrianglesStart/Next + class CSGetTrianglesContext; + + // Helper functions called by CollisionDispatch + static void sCollideConvexVsConvex(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 sCastConvexVsConvex(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + // Properties + RefConst mMaterial; ///< Material assigned to this shape + float mDensity = 1000.0f; ///< Uniform density of the interior of the convex object (kg / m^3) +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.cpp new file mode 100644 index 000000000000..06fc5ec7184a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.cpp @@ -0,0 +1,404 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(CylinderShapeSettings) +{ + JPH_ADD_BASE_CLASS(CylinderShapeSettings, ConvexShapeSettings) + + JPH_ADD_ATTRIBUTE(CylinderShapeSettings, mHalfHeight) + JPH_ADD_ATTRIBUTE(CylinderShapeSettings, mRadius) + JPH_ADD_ATTRIBUTE(CylinderShapeSettings, mConvexRadius) +} + +// Approximation of top face with 8 vertices +static const Vec3 cCylinderTopFace[] = +{ + Vec3(0.0f, 1.0f, 1.0f), + Vec3(0.707106769f, 1.0f, 0.707106769f), + Vec3(1.0f, 1.0f, 0.0f), + Vec3(0.707106769f, 1.0f, -0.707106769f), + Vec3(-0.0f, 1.0f, -1.0f), + Vec3(-0.707106769f, 1.0f, -0.707106769f), + Vec3(-1.0f, 1.0f, 0.0f), + Vec3(-0.707106769f, 1.0f, 0.707106769f) +}; + +static const StaticArray sUnitCylinderTriangles = []() { + StaticArray verts; + + const Vec3 bottom_offset(0.0f, -2.0f, 0.0f); + + int num_verts = sizeof(cCylinderTopFace) / sizeof(Vec3); + for (int i = 0; i < num_verts; ++i) + { + Vec3 t1 = cCylinderTopFace[i]; + Vec3 t2 = cCylinderTopFace[(i + 1) % num_verts]; + Vec3 b1 = cCylinderTopFace[i] + bottom_offset; + Vec3 b2 = cCylinderTopFace[(i + 1) % num_verts] + bottom_offset; + + // Top + verts.emplace_back(0.0f, 1.0f, 0.0f); + verts.push_back(t1); + verts.push_back(t2); + + // Bottom + verts.emplace_back(0.0f, -1.0f, 0.0f); + verts.push_back(b2); + verts.push_back(b1); + + // Side + verts.push_back(t1); + verts.push_back(b1); + verts.push_back(t2); + + verts.push_back(t2); + verts.push_back(b1); + verts.push_back(b2); + } + + return verts; +}(); + +ShapeSettings::ShapeResult CylinderShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new CylinderShape(*this, mCachedResult); + return mCachedResult; +} + +CylinderShape::CylinderShape(const CylinderShapeSettings &inSettings, ShapeResult &outResult) : + ConvexShape(EShapeSubType::Cylinder, inSettings, outResult), + mHalfHeight(inSettings.mHalfHeight), + mRadius(inSettings.mRadius), + mConvexRadius(inSettings.mConvexRadius) +{ + if (inSettings.mHalfHeight < inSettings.mConvexRadius) + { + outResult.SetError("Invalid height"); + return; + } + + if (inSettings.mRadius < inSettings.mConvexRadius) + { + outResult.SetError("Invalid radius"); + return; + } + + if (inSettings.mConvexRadius < 0.0f) + { + outResult.SetError("Invalid convex radius"); + return; + } + + outResult.Set(this); +} + +CylinderShape::CylinderShape(float inHalfHeight, float inRadius, float inConvexRadius, const PhysicsMaterial *inMaterial) : + ConvexShape(EShapeSubType::Cylinder, inMaterial), + mHalfHeight(inHalfHeight), + mRadius(inRadius), + mConvexRadius(inConvexRadius) +{ + JPH_ASSERT(inHalfHeight >= inConvexRadius); + JPH_ASSERT(inRadius >= inConvexRadius); + JPH_ASSERT(inConvexRadius >= 0.0f); +} + +class CylinderShape::Cylinder final : public Support +{ +public: + Cylinder(float inHalfHeight, float inRadius, float inConvexRadius) : + mHalfHeight(inHalfHeight), + mRadius(inRadius), + mConvexRadius(inConvexRadius) + { + static_assert(sizeof(Cylinder) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(Cylinder))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + // Support mapping, taken from: + // A Fast and Robust GJK Implementation for Collision Detection of Convex Objects - Gino van den Bergen + // page 8 + float x = inDirection.GetX(), y = inDirection.GetY(), z = inDirection.GetZ(); + float o = sqrt(Square(x) + Square(z)); + if (o > 0.0f) + return Vec3((mRadius * x) / o, Sign(y) * mHalfHeight, (mRadius * z) / o); + else + return Vec3(0, Sign(y) * mHalfHeight, 0); + } + + virtual float GetConvexRadius() const override + { + return mConvexRadius; + } + +private: + float mHalfHeight; + float mRadius; + float mConvexRadius; +}; + +const ConvexShape::Support *CylinderShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + // Get scaled cylinder + Vec3 abs_scale = inScale.Abs(); + float scale_xz = abs_scale.GetX(); + float scale_y = abs_scale.GetY(); + float scaled_half_height = scale_y * mHalfHeight; + float scaled_radius = scale_xz * mRadius; + float scaled_convex_radius = ScaleHelpers::ScaleConvexRadius(mConvexRadius, inScale); + + switch (inMode) + { + case ESupportMode::IncludeConvexRadius: + case ESupportMode::Default: + return new (&inBuffer) Cylinder(scaled_half_height, scaled_radius, 0.0f); + + case ESupportMode::ExcludeConvexRadius: + return new (&inBuffer) Cylinder(scaled_half_height - scaled_convex_radius, scaled_radius - scaled_convex_radius, scaled_convex_radius); + } + + JPH_ASSERT(false); + return nullptr; +} + +void CylinderShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + JPH_ASSERT(IsValidScale(inScale)); + + // Get scaled cylinder + Vec3 abs_scale = inScale.Abs(); + float scale_xz = abs_scale.GetX(); + float scale_y = abs_scale.GetY(); + float scaled_half_height = scale_y * mHalfHeight; + float scaled_radius = scale_xz * mRadius; + + float x = inDirection.GetX(), y = inDirection.GetY(), z = inDirection.GetZ(); + float o = sqrt(Square(x) + Square(z)); + + // If o / |y| > scaled_radius / scaled_half_height, we're hitting the side + if (o * scaled_half_height > scaled_radius * abs(y)) + { + // Hitting side + float f = -scaled_radius / o; + float vx = x * f; + float vz = z * f; + outVertices.push_back(inCenterOfMassTransform * Vec3(vx, scaled_half_height, vz)); + outVertices.push_back(inCenterOfMassTransform * Vec3(vx, -scaled_half_height, vz)); + } + else + { + // Hitting top or bottom + Vec3 multiplier = y < 0.0f? Vec3(scaled_radius, scaled_half_height, scaled_radius) : Vec3(-scaled_radius, -scaled_half_height, scaled_radius); + Mat44 transform = inCenterOfMassTransform.PreScaled(multiplier); + for (const Vec3 &v : cCylinderTopFace) + outVertices.push_back(transform * v); + } +} + +MassProperties CylinderShape::GetMassProperties() const +{ + MassProperties p; + + // Mass is surface of circle * height + float radius_sq = Square(mRadius); + float height = 2.0f * mHalfHeight; + p.mMass = JPH_PI * radius_sq * height * GetDensity(); + + // Inertia according to https://en.wikipedia.org/wiki/List_of_moments_of_inertia: + float inertia_y = radius_sq * p.mMass * 0.5f; + float inertia_x = inertia_y * 0.5f + p.mMass * height * height / 12.0f; + float inertia_z = inertia_x; + + // Set inertia + p.mInertia = Mat44::sScale(Vec3(inertia_x, inertia_y, inertia_z)); + + return p; +} + +Vec3 CylinderShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + // Calculate distance to infinite cylinder surface + Vec3 local_surface_position_xz(inLocalSurfacePosition.GetX(), 0, inLocalSurfacePosition.GetZ()); + float local_surface_position_xz_len = local_surface_position_xz.Length(); + float distance_to_curved_surface = abs(local_surface_position_xz_len - mRadius); + + // Calculate distance to top or bottom plane + float distance_to_top_or_bottom = abs(abs(inLocalSurfacePosition.GetY()) - mHalfHeight); + + // Return normal according to closest surface + if (distance_to_curved_surface < distance_to_top_or_bottom) + return local_surface_position_xz / local_surface_position_xz_len; + else + return inLocalSurfacePosition.GetY() > 0.0f? Vec3::sAxisY() : -Vec3::sAxisY(); +} + +AABox CylinderShape::GetLocalBounds() const +{ + Vec3 extent = Vec3(mRadius, mHalfHeight, mRadius); + return AABox(-extent, extent); +} + +#ifdef JPH_DEBUG_RENDERER +void CylinderShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + inRenderer->DrawCylinder(inCenterOfMassTransform * Mat44::sScale(inScale.Abs()), mHalfHeight, mRadius, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode); +} +#endif // JPH_DEBUG_RENDERER + +bool CylinderShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + // Test ray against capsule + float fraction = RayCylinder(inRay.mOrigin, inRay.mDirection, mHalfHeight, mRadius); + if (fraction < ioHit.mFraction) + { + ioHit.mFraction = fraction; + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + return false; +} + +void CylinderShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Check if the point is in the cylinder + if (abs(inPoint.GetY()) <= mHalfHeight // Within the height + && Square(inPoint.GetX()) + Square(inPoint.GetZ()) <= Square(mRadius)) // Within the radius + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); +} + +void CylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation(); + + // Get scaled cylinder + Vec3 abs_scale = inScale.Abs(); + float half_height = abs_scale.GetY() * mHalfHeight; + float radius = abs_scale.GetX() * mRadius; + + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) + { + Vec3 local_pos = inverse_transform * v.GetPosition(); + + // Calculate penetration into side surface + Vec3 side_normal = local_pos; + side_normal.SetY(0.0f); + float side_normal_length = side_normal.Length(); + float side_penetration = radius - side_normal_length; + + // Calculate penetration into top or bottom plane + float top_penetration = half_height - abs(local_pos.GetY()); + + Vec3 point, normal; + if (side_penetration < 0.0f && top_penetration < 0.0f) + { + // We're outside the cylinder height and radius + point = side_normal * (radius / side_normal_length) + Vec3(0, half_height * Sign(local_pos.GetY()), 0); + normal = (local_pos - point).NormalizedOr(Vec3::sAxisY()); + } + else if (side_penetration < top_penetration) + { + // Side surface is closest + normal = side_normal_length > 0.0f? side_normal / side_normal_length : Vec3::sAxisX(); + point = radius * normal; + } + else + { + // Top or bottom plane is closest + normal = Vec3(0, Sign(local_pos.GetY()), 0); + point = half_height * normal; + } + + // Calculate penetration + Plane plane = Plane::sFromPointAndNormal(point, normal); + float penetration = -plane.SignedDistance(local_pos); + if (v.UpdatePenetration(penetration)) + v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); + } +} + +void CylinderShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + Mat44 unit_cylinder_transform(Vec4(mRadius, 0, 0, 0), Vec4(0, mHalfHeight, 0, 0), Vec4(0, 0, mRadius, 0), Vec4(0, 0, 0, 1)); + new (&ioContext) GetTrianglesContextVertexList(inPositionCOM, inRotation, inScale, unit_cylinder_transform, sUnitCylinderTriangles.data(), sUnitCylinderTriangles.size(), GetMaterial()); +} + +int CylinderShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + return ((GetTrianglesContextVertexList &)ioContext).GetTrianglesNext(inMaxTrianglesRequested, outTriangleVertices, outMaterials); +} + +void CylinderShape::SaveBinaryState(StreamOut &inStream) const +{ + ConvexShape::SaveBinaryState(inStream); + + inStream.Write(mHalfHeight); + inStream.Write(mRadius); + inStream.Write(mConvexRadius); +} + +void CylinderShape::RestoreBinaryState(StreamIn &inStream) +{ + ConvexShape::RestoreBinaryState(inStream); + + inStream.Read(mHalfHeight); + inStream.Read(mRadius); + inStream.Read(mConvexRadius); +} + +bool CylinderShape::IsValidScale(Vec3Arg inScale) const +{ + return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScaleXZ(inScale.Abs()); +} + +Vec3 CylinderShape::MakeScaleValid(Vec3Arg inScale) const +{ + Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale); + + return scale.GetSign() * ScaleHelpers::MakeUniformScaleXZ(scale.Abs()); +} + +void CylinderShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Cylinder); + f.mConstruct = []() -> Shape * { return new CylinderShape; }; + f.mColor = Color::sGreen; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.h new file mode 100644 index 000000000000..302c97565b0b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.h @@ -0,0 +1,126 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a CylinderShape +class JPH_EXPORT CylinderShapeSettings final : public ConvexShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, CylinderShapeSettings) + +public: + /// Default constructor for deserialization + CylinderShapeSettings() = default; + + /// Create a shape centered around the origin with one top at (0, -inHalfHeight, 0) and the other at (0, inHalfHeight, 0) and radius inRadius. + /// (internally the convex radius will be subtracted from the cylinder the total cylinder will not grow with the convex radius, but the edges of the cylinder will be rounded a bit). + CylinderShapeSettings(float inHalfHeight, float inRadius, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mHalfHeight(inHalfHeight), mRadius(inRadius), mConvexRadius(inConvexRadius) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + float mHalfHeight = 0.0f; + float mRadius = 0.0f; + float mConvexRadius = 0.0f; +}; + +/// A cylinder +class JPH_EXPORT CylinderShape final : public ConvexShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + CylinderShape() : ConvexShape(EShapeSubType::Cylinder) { } + CylinderShape(const CylinderShapeSettings &inSettings, ShapeResult &outResult); + + /// Create a shape centered around the origin with one top at (0, -inHalfHeight, 0) and the other at (0, inHalfHeight, 0) and radius inRadius. + /// (internally the convex radius will be subtracted from the cylinder the total cylinder will not grow with the convex radius, but the edges of the cylinder will be rounded a bit). + CylinderShape(float inHalfHeight, float inRadius, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr); + + /// Get half height of cylinder + float GetHalfHeight() const { return mHalfHeight; } + + /// Get radius of cylinder + float GetRadius() const { return mRadius; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return min(mHalfHeight, mRadius); } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See ConvexShape::GetSupportFunction + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + using ConvexShape::CastRay; + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return 2.0f * JPH_PI * mHalfHeight * Square(mRadius); } + + /// Get the convex radius of this cylinder + float GetConvexRadius() const { return mConvexRadius; } + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // See Shape::MakeScaleValid + virtual Vec3 MakeScaleValid(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Class for GetSupportFunction + class Cylinder; + + float mHalfHeight = 0.0f; + float mRadius = 0.0f; + float mConvexRadius = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/DecoratedShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/DecoratedShape.cpp new file mode 100644 index 000000000000..339f78363761 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/DecoratedShape.cpp @@ -0,0 +1,87 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(DecoratedShapeSettings) +{ + JPH_ADD_BASE_CLASS(DecoratedShapeSettings, ShapeSettings) + + JPH_ADD_ATTRIBUTE(DecoratedShapeSettings, mInnerShape) +} + +DecoratedShape::DecoratedShape(EShapeSubType inSubType, const DecoratedShapeSettings &inSettings, ShapeResult &outResult) : + Shape(EShapeType::Decorated, inSubType, inSettings, outResult) +{ + // Check that there's a shape + if (inSettings.mInnerShape == nullptr && inSettings.mInnerShapePtr == nullptr) + { + outResult.SetError("Inner shape is null!"); + return; + } + + if (inSettings.mInnerShapePtr != nullptr) + { + // Use provided shape + mInnerShape = inSettings.mInnerShapePtr; + } + else + { + // Create child shape + ShapeResult child_result = inSettings.mInnerShape->Create(); + if (!child_result.IsValid()) + { + outResult = child_result; + return; + } + mInnerShape = child_result.Get(); + } +} + +const PhysicsMaterial *DecoratedShape::GetMaterial(const SubShapeID &inSubShapeID) const +{ + return mInnerShape->GetMaterial(inSubShapeID); +} + +void DecoratedShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + mInnerShape->GetSupportingFace(inSubShapeID, inDirection, inScale, inCenterOfMassTransform, outVertices); +} + +uint64 DecoratedShape::GetSubShapeUserData(const SubShapeID &inSubShapeID) const +{ + return mInnerShape->GetSubShapeUserData(inSubShapeID); +} + +void DecoratedShape::SaveSubShapeState(ShapeList &outSubShapes) const +{ + outSubShapes.clear(); + outSubShapes.push_back(mInnerShape); +} + +void DecoratedShape::RestoreSubShapeState(const ShapeRefC *inSubShapes, uint inNumShapes) +{ + JPH_ASSERT(inNumShapes == 1); + mInnerShape = inSubShapes[0]; +} + +Shape::Stats DecoratedShape::GetStatsRecursive(VisitedShapes &ioVisitedShapes) const +{ + // Get own stats + Stats stats = Shape::GetStatsRecursive(ioVisitedShapes); + + // Add child stats + Stats child_stats = mInnerShape->GetStatsRecursive(ioVisitedShapes); + stats.mSizeBytes += child_stats.mSizeBytes; + stats.mNumTriangles += child_stats.mNumTriangles; + + return stats; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/DecoratedShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/DecoratedShape.h new file mode 100644 index 000000000000..5ab8748fc91c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/DecoratedShape.h @@ -0,0 +1,80 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a DecoratedShape +class JPH_EXPORT DecoratedShapeSettings : public ShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, DecoratedShapeSettings) + +public: + /// Default constructor for deserialization + DecoratedShapeSettings() = default; + + /// Constructor that decorates another shape + explicit DecoratedShapeSettings(const ShapeSettings *inShape) : mInnerShape(inShape) { } + explicit DecoratedShapeSettings(const Shape *inShape) : mInnerShapePtr(inShape) { } + + RefConst mInnerShape; ///< Sub shape (either this or mShapePtr needs to be filled up) + RefConst mInnerShapePtr; ///< Sub shape (either this or mShape needs to be filled up) +}; + +/// Base class for shapes that decorate another shape with extra functionality (e.g. scale, translation etc.) +class JPH_EXPORT DecoratedShape : public Shape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit DecoratedShape(EShapeSubType inSubType) : Shape(EShapeType::Decorated, inSubType) { } + DecoratedShape(EShapeSubType inSubType, const Shape *inInnerShape) : Shape(EShapeType::Decorated, inSubType), mInnerShape(inInnerShape) { } + DecoratedShape(EShapeSubType inSubType, const DecoratedShapeSettings &inSettings, ShapeResult &outResult); + + /// Access to the decorated inner shape + const Shape * GetInnerShape() const { return mInnerShape; } + + // See Shape::MustBeStatic + virtual bool MustBeStatic() const override { return mInnerShape->MustBeStatic(); } + + // See Shape::GetCenterOfMass + virtual Vec3 GetCenterOfMass() const override { return mInnerShape->GetCenterOfMass(); } + + // See Shape::GetSubShapeIDBitsRecursive + virtual uint GetSubShapeIDBitsRecursive() const override { return mInnerShape->GetSubShapeIDBitsRecursive(); } + + // See Shape::GetLeafShape + virtual const Shape * GetLeafShape(const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const override { return mInnerShape->GetLeafShape(inSubShapeID, outRemainder); } + + // See Shape::GetMaterial + virtual const PhysicsMaterial * GetMaterial(const SubShapeID &inSubShapeID) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See Shape::GetSubShapeUserData + virtual uint64 GetSubShapeUserData(const SubShapeID &inSubShapeID) const override; + + // See Shape + virtual void SaveSubShapeState(ShapeList &outSubShapes) const override; + virtual void RestoreSubShapeState(const ShapeRefC *inSubShapes, uint inNumShapes) override; + + // See Shape::GetStatsRecursive + virtual Stats GetStatsRecursive(VisitedShapes &ioVisitedShapes) const override; + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override { return mInnerShape->IsValidScale(inScale); } + + // See Shape::MakeScaleValid + virtual Vec3 MakeScaleValid(Vec3Arg inScale) const override { return mInnerShape->MakeScaleValid(inScale); } + +protected: + RefConst mInnerShape; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.cpp new file mode 100644 index 000000000000..8cb81f68d038 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.cpp @@ -0,0 +1,65 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(EmptyShapeSettings) +{ + JPH_ADD_BASE_CLASS(EmptyShapeSettings, ShapeSettings) + + JPH_ADD_ATTRIBUTE(EmptyShapeSettings, mCenterOfMass) +} + +ShapeSettings::ShapeResult EmptyShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + new EmptyShape(*this, mCachedResult); + + return mCachedResult; +} + +MassProperties EmptyShape::GetMassProperties() const +{ + MassProperties mass_properties; + mass_properties.mMass = 1.0f; + mass_properties.mInertia = Mat44::sIdentity(); + return mass_properties; +} + +#ifdef JPH_DEBUG_RENDERER +void EmptyShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, [[maybe_unused]] bool inUseMaterialColors, [[maybe_unused]] bool inDrawWireframe) const +{ + inRenderer->DrawMarker(inCenterOfMassTransform.GetTranslation(), inColor, abs(inScale.GetX()) * 0.1f); +} +#endif // JPH_DEBUG_RENDERER + +void EmptyShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Empty); + f.mConstruct = []() -> Shape * { return new EmptyShape; }; + f.mColor = Color::sBlack; + + auto collide_empty = []([[maybe_unused]] const Shape *inShape1, [[maybe_unused]] const Shape *inShape2, [[maybe_unused]] Vec3Arg inScale1, [[maybe_unused]] Vec3Arg inScale2, [[maybe_unused]] Mat44Arg inCenterOfMassTransform1, [[maybe_unused]] Mat44Arg inCenterOfMassTransform2, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator1, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator2, [[maybe_unused]] const CollideShapeSettings &inCollideShapeSettings, [[maybe_unused]] CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter) { /* Do Nothing */ }; + auto cast_empty = []([[maybe_unused]] const ShapeCast &inShapeCast, [[maybe_unused]] const ShapeCastSettings &inShapeCastSettings, [[maybe_unused]] const Shape *inShape, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, [[maybe_unused]] Mat44Arg inCenterOfMassTransform2, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator1, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator2, [[maybe_unused]] CastShapeCollector &ioCollector) { /* Do nothing */ }; + + for (const EShapeSubType s : sAllSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Empty, s, collide_empty); + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Empty, collide_empty); + + CollisionDispatch::sRegisterCastShape(EShapeSubType::Empty, s, cast_empty); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Empty, cast_empty); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.h new file mode 100644 index 000000000000..f3e7bdcf6ae6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.h @@ -0,0 +1,75 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that constructs an EmptyShape +class JPH_EXPORT EmptyShapeSettings final : public ShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, EmptyShapeSettings) + +public: + EmptyShapeSettings() = default; + explicit EmptyShapeSettings(Vec3Arg inCenterOfMass) : mCenterOfMass(inCenterOfMass) { } + + ShapeResult Create() const override; + + Vec3 mCenterOfMass = Vec3::sZero(); ///< Determines the center of mass for this shape +}; + +/// An empty shape that has no volume and collides with nothing. +/// +/// Possible use cases: +/// - As a placeholder for a shape that will be created later. E.g. if you first need to create a body and only then know what shape it will have. +/// - If you need a kinematic body to attach a constraint to, but you don't want the body to collide with anything. +/// +/// Note that, if possible, you should also put your body in an ObjectLayer that doesn't collide with anything. +/// This ensures that collisions will be filtered out at broad phase level instead of at narrow phase level, this is more efficient. +class JPH_EXPORT EmptyShape final : public Shape +{ +public: + // Constructor + EmptyShape() : Shape(EShapeType::Empty, EShapeSubType::Empty) { } + explicit EmptyShape(Vec3Arg inCenterOfMass) : Shape(EShapeType::Empty, EShapeSubType::Empty), mCenterOfMass(inCenterOfMass) { } + EmptyShape(const EmptyShapeSettings &inSettings, ShapeResult &outResult) : Shape(EShapeType::Empty, EShapeSubType::Empty, inSettings, outResult), mCenterOfMass(inSettings.mCenterOfMass) { outResult.Set(this); } + + // See: Shape + Vec3 GetCenterOfMass() const override { return mCenterOfMass; } + AABox GetLocalBounds() const override { return { Vec3::sZero(), Vec3::sZero() }; } + uint GetSubShapeIDBitsRecursive() const override { return 0; } + float GetInnerRadius() const override { return 0.0f; } + MassProperties GetMassProperties() const override; + const PhysicsMaterial * GetMaterial([[maybe_unused]] const SubShapeID &inSubShapeID) const override { return PhysicsMaterial::sDefault; } + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override { return Vec3::sZero(); } + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy +#ifdef JPH_DEBUG_RENDERER // Not using JPH_IF_DEBUG_RENDERER for Doxygen + , RVec3Arg inBaseOffset +#endif + ) const override { outTotalVolume = 0.0f; outSubmergedVolume = 0.0f; outCenterOfBuoyancy = Vec3::sZero(); } +#ifdef JPH_DEBUG_RENDERER + virtual void Draw([[maybe_unused]] DebugRenderer *inRenderer, [[maybe_unused]] RMat44Arg inCenterOfMassTransform, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] ColorArg inColor, [[maybe_unused]] bool inUseMaterialColors, [[maybe_unused]] bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + virtual bool CastRay([[maybe_unused]] const RayCast &inRay, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator, [[maybe_unused]] RayCastResult &ioHit) const override { return false; } + virtual void CastRay([[maybe_unused]] const RayCast &inRay, [[maybe_unused]] const RayCastSettings &inRayCastSettings, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator, [[maybe_unused]] CastRayCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter = { }) const override { /* Do nothing */ } + virtual void CollidePoint([[maybe_unused]] Vec3Arg inPoint, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator, [[maybe_unused]] CollidePointCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter = { }) const override { /* Do nothing */ } + virtual void CollideSoftBodyVertices([[maybe_unused]] Mat44Arg inCenterOfMassTransform, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] const CollideSoftBodyVertexIterator &inVertices, [[maybe_unused]] uint inNumVertices, [[maybe_unused]] int inCollidingShapeIndex) const override { /* Do nothing */ } + virtual void GetTrianglesStart([[maybe_unused]] GetTrianglesContext &ioContext, [[maybe_unused]] const AABox &inBox, [[maybe_unused]] Vec3Arg inPositionCOM, [[maybe_unused]] QuatArg inRotation, [[maybe_unused]] Vec3Arg inScale) const override { /* Do nothing */ } + virtual int GetTrianglesNext([[maybe_unused]] GetTrianglesContext &ioContext, [[maybe_unused]] int inMaxTrianglesRequested, [[maybe_unused]] Float3 *outTriangleVertices, [[maybe_unused]] const PhysicsMaterial **outMaterials = nullptr) const override { return 0; } + Stats GetStats() const override { return { sizeof(*this), 0 }; } + float GetVolume() const override { return 0.0f; } + bool IsValidScale([[maybe_unused]] Vec3Arg inScale) const override { return true; } + + // Register shape functions with the registry + static void sRegister(); + +private: + Vec3 mCenterOfMass = Vec3::sZero(); +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/GetTrianglesContext.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/GetTrianglesContext.h new file mode 100644 index 000000000000..df68ae2f55be --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/GetTrianglesContext.h @@ -0,0 +1,248 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsMaterial; + +/// Implementation of GetTrianglesStart/Next that uses a fixed list of vertices for the triangles. These are transformed into world space when getting the triangles. +class GetTrianglesContextVertexList +{ +public: + /// Constructor, to be called in GetTrianglesStart + GetTrianglesContextVertexList(Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, Mat44Arg inLocalTransform, const Vec3 *inTriangleVertices, size_t inNumTriangleVertices, const PhysicsMaterial *inMaterial) : + mLocalToWorld(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale) * inLocalTransform), + mTriangleVertices(inTriangleVertices), + mNumTriangleVertices(inNumTriangleVertices), + mMaterial(inMaterial), + mIsInsideOut(ScaleHelpers::IsInsideOut(inScale)) + { + static_assert(sizeof(GetTrianglesContextVertexList) <= sizeof(Shape::GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(this, alignof(GetTrianglesContextVertexList))); + JPH_ASSERT(inNumTriangleVertices % 3 == 0); + } + + /// @see Shape::GetTrianglesNext + int GetTrianglesNext(int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) + { + JPH_ASSERT(inMaxTrianglesRequested >= Shape::cGetTrianglesMinTrianglesRequested); + + int total_num_vertices = min(inMaxTrianglesRequested * 3, int(mNumTriangleVertices - mCurrentVertex)); + + if (mIsInsideOut) + { + // Store triangles flipped + for (const Vec3 *v = mTriangleVertices + mCurrentVertex, *v_end = v + total_num_vertices; v < v_end; v += 3) + { + (mLocalToWorld * v[0]).StoreFloat3(outTriangleVertices++); + (mLocalToWorld * v[2]).StoreFloat3(outTriangleVertices++); + (mLocalToWorld * v[1]).StoreFloat3(outTriangleVertices++); + } + } + else + { + // Store triangles + for (const Vec3 *v = mTriangleVertices + mCurrentVertex, *v_end = v + total_num_vertices; v < v_end; v += 3) + { + (mLocalToWorld * v[0]).StoreFloat3(outTriangleVertices++); + (mLocalToWorld * v[1]).StoreFloat3(outTriangleVertices++); + (mLocalToWorld * v[2]).StoreFloat3(outTriangleVertices++); + } + } + + // Update the current vertex to point to the next vertex to get + mCurrentVertex += total_num_vertices; + int total_num_triangles = total_num_vertices / 3; + + // Store materials + if (outMaterials != nullptr) + for (const PhysicsMaterial **m = outMaterials, **m_end = outMaterials + total_num_triangles; m < m_end; ++m) + *m = mMaterial; + + return total_num_triangles; + } + + /// Helper function that creates a vertex list of a half unit sphere (top part) + template + static void sCreateHalfUnitSphereTop(A &ioVertices, int inDetailLevel) + { + sCreateUnitSphereHelper(ioVertices, Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), inDetailLevel); + sCreateUnitSphereHelper(ioVertices, Vec3::sAxisY(), -Vec3::sAxisX(), Vec3::sAxisZ(), inDetailLevel); + sCreateUnitSphereHelper(ioVertices, Vec3::sAxisY(), Vec3::sAxisX(), -Vec3::sAxisZ(), inDetailLevel); + sCreateUnitSphereHelper(ioVertices, -Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), inDetailLevel); + } + + /// Helper function that creates a vertex list of a half unit sphere (bottom part) + template + static void sCreateHalfUnitSphereBottom(A &ioVertices, int inDetailLevel) + { + sCreateUnitSphereHelper(ioVertices, -Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), inDetailLevel); + sCreateUnitSphereHelper(ioVertices, -Vec3::sAxisY(), Vec3::sAxisX(), Vec3::sAxisZ(), inDetailLevel); + sCreateUnitSphereHelper(ioVertices, Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), inDetailLevel); + sCreateUnitSphereHelper(ioVertices, -Vec3::sAxisY(), -Vec3::sAxisX(), -Vec3::sAxisZ(), inDetailLevel); + } + + /// Helper function that creates an open cylinder of half height 1 and radius 1 + template + static void sCreateUnitOpenCylinder(A &ioVertices, int inDetailLevel) + { + const Vec3 bottom_offset(0.0f, -2.0f, 0.0f); + int num_verts = 4 * (1 << inDetailLevel); + for (int i = 0; i < num_verts; ++i) + { + float angle1 = 2.0f * JPH_PI * (float(i) / num_verts); + float angle2 = 2.0f * JPH_PI * (float(i + 1) / num_verts); + + Vec3 t1(Sin(angle1), 1.0f, Cos(angle1)); + Vec3 t2(Sin(angle2), 1.0f, Cos(angle2)); + Vec3 b1 = t1 + bottom_offset; + Vec3 b2 = t2 + bottom_offset; + + ioVertices.push_back(t1); + ioVertices.push_back(b1); + ioVertices.push_back(t2); + + ioVertices.push_back(t2); + ioVertices.push_back(b1); + ioVertices.push_back(b2); + } + } + +private: + /// Recursive helper function for creating a sphere + template + static void sCreateUnitSphereHelper(A &ioVertices, Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, int inLevel) + { + Vec3 center1 = (inV1 + inV2).Normalized(); + Vec3 center2 = (inV2 + inV3).Normalized(); + Vec3 center3 = (inV3 + inV1).Normalized(); + + if (inLevel > 0) + { + int new_level = inLevel - 1; + sCreateUnitSphereHelper(ioVertices, inV1, center1, center3, new_level); + sCreateUnitSphereHelper(ioVertices, center1, center2, center3, new_level); + sCreateUnitSphereHelper(ioVertices, center1, inV2, center2, new_level); + sCreateUnitSphereHelper(ioVertices, center3, center2, inV3, new_level); + } + else + { + ioVertices.push_back(inV1); + ioVertices.push_back(inV2); + ioVertices.push_back(inV3); + } + } + + Mat44 mLocalToWorld; + const Vec3 * mTriangleVertices; + size_t mNumTriangleVertices; + size_t mCurrentVertex = 0; + const PhysicsMaterial * mMaterial; + bool mIsInsideOut; +}; + +/// Implementation of GetTrianglesStart/Next that uses a multiple fixed lists of vertices for the triangles. These are transformed into world space when getting the triangles. +class GetTrianglesContextMultiVertexList +{ +public: + /// Constructor, to be called in GetTrianglesStart + GetTrianglesContextMultiVertexList(bool inIsInsideOut, const PhysicsMaterial *inMaterial) : + mMaterial(inMaterial), + mIsInsideOut(inIsInsideOut) + { + static_assert(sizeof(GetTrianglesContextMultiVertexList) <= sizeof(Shape::GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(this, alignof(GetTrianglesContextMultiVertexList))); + } + + /// Add a mesh part and its transform + void AddPart(Mat44Arg inLocalToWorld, const Vec3 *inTriangleVertices, size_t inNumTriangleVertices) + { + JPH_ASSERT(inNumTriangleVertices % 3 == 0); + + mParts.push_back({ inLocalToWorld, inTriangleVertices, inNumTriangleVertices }); + } + + /// @see Shape::GetTrianglesNext + int GetTrianglesNext(int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) + { + JPH_ASSERT(inMaxTrianglesRequested >= Shape::cGetTrianglesMinTrianglesRequested); + + int total_num_vertices = 0; + int max_vertices_requested = inMaxTrianglesRequested * 3; + + // Loop over parts + for (; mCurrentPart < mParts.size(); ++mCurrentPart) + { + const Part &part = mParts[mCurrentPart]; + + // Calculate how many vertices to take from this part + int part_num_vertices = min(max_vertices_requested, int(part.mNumTriangleVertices - mCurrentVertex)); + if (part_num_vertices == 0) + break; + + max_vertices_requested -= part_num_vertices; + total_num_vertices += part_num_vertices; + + if (mIsInsideOut) + { + // Store triangles flipped + for (const Vec3 *v = part.mTriangleVertices + mCurrentVertex, *v_end = v + part_num_vertices; v < v_end; v += 3) + { + (part.mLocalToWorld * v[0]).StoreFloat3(outTriangleVertices++); + (part.mLocalToWorld * v[2]).StoreFloat3(outTriangleVertices++); + (part.mLocalToWorld * v[1]).StoreFloat3(outTriangleVertices++); + } + } + else + { + // Store triangles + for (const Vec3 *v = part.mTriangleVertices + mCurrentVertex, *v_end = v + part_num_vertices; v < v_end; v += 3) + { + (part.mLocalToWorld * v[0]).StoreFloat3(outTriangleVertices++); + (part.mLocalToWorld * v[1]).StoreFloat3(outTriangleVertices++); + (part.mLocalToWorld * v[2]).StoreFloat3(outTriangleVertices++); + } + } + + // Update the current vertex to point to the next vertex to get + mCurrentVertex += part_num_vertices; + + // Check if we completed this part + if (mCurrentVertex < part.mNumTriangleVertices) + break; + + // Reset current vertex for the next part + mCurrentVertex = 0; + } + + int total_num_triangles = total_num_vertices / 3; + + // Store materials + if (outMaterials != nullptr) + for (const PhysicsMaterial **m = outMaterials, **m_end = outMaterials + total_num_triangles; m < m_end; ++m) + *m = mMaterial; + + return total_num_triangles; + } + +private: + struct Part + { + Mat44 mLocalToWorld; + const Vec3 * mTriangleVertices; + size_t mNumTriangleVertices; + }; + + StaticArray mParts; + uint mCurrentPart = 0; + size_t mCurrentVertex = 0; + const PhysicsMaterial * mMaterial; + bool mIsInsideOut; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp new file mode 100644 index 000000000000..fcecdaf92dc0 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp @@ -0,0 +1,2714 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//#define JPH_DEBUG_HEIGHT_FIELD + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_DEBUG_RENDERER +bool HeightFieldShape::sDrawTriangleOutlines = false; +#endif // JPH_DEBUG_RENDERER + +using namespace HeightFieldShapeConstants; + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(HeightFieldShapeSettings) +{ + JPH_ADD_BASE_CLASS(HeightFieldShapeSettings, ShapeSettings) + + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mHeightSamples) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mOffset) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mScale) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMinHeightValue) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaxHeightValue) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaterialsCapacity) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mSampleCount) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mBlockSize) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mBitsPerSample) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaterialIndices) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaterials) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mActiveEdgeCosThresholdAngle) +} + +const uint HeightFieldShape::sGridOffsets[] = +{ + 0, // level: 0, max x/y: 0, offset: 0 + 1, // level: 1, max x/y: 1, offset: 1 + 5, // level: 2, max x/y: 3, offset: 1 + 4 + 21, // level: 3, max x/y: 7, offset: 1 + 4 + 16 + 85, // level: 4, max x/y: 15, offset: 1 + 4 + 16 + 64 + 341, // level: 5, max x/y: 31, offset: 1 + 4 + 16 + 64 + 256 + 1365, // level: 6, max x/y: 63, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 5461, // level: 7, max x/y: 127, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + 21845, // level: 8, max x/y: 255, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ... + 87381, // level: 9, max x/y: 511, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ... + 349525, // level: 10, max x/y: 1023, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ... + 1398101, // level: 11, max x/y: 2047, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ... + 5592405, // level: 12, max x/y: 4095, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ... + 22369621, // level: 13, max x/y: 8191, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ... + 89478485, // level: 14, max x/y: 16383, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ... +}; + +HeightFieldShapeSettings::HeightFieldShapeSettings(const float *inSamples, Vec3Arg inOffset, Vec3Arg inScale, uint32 inSampleCount, const uint8 *inMaterialIndices, const PhysicsMaterialList &inMaterialList) : + mOffset(inOffset), + mScale(inScale), + mSampleCount(inSampleCount) +{ + mHeightSamples.assign(inSamples, inSamples + Square(inSampleCount)); + + if (!inMaterialList.empty() && inMaterialIndices != nullptr) + { + mMaterialIndices.assign(inMaterialIndices, inMaterialIndices + Square(inSampleCount - 1)); + mMaterials = inMaterialList; + } + else + { + JPH_ASSERT(inMaterialList.empty()); + JPH_ASSERT(inMaterialIndices == nullptr); + } +} + +ShapeSettings::ShapeResult HeightFieldShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new HeightFieldShape(*this, mCachedResult); + return mCachedResult; +} + +void HeightFieldShapeSettings::DetermineMinAndMaxSample(float &outMinValue, float &outMaxValue, float &outQuantizationScale) const +{ + // Determine min and max value + outMinValue = mMinHeightValue; + outMaxValue = mMaxHeightValue; + for (float h : mHeightSamples) + if (h != cNoCollisionValue) + { + outMinValue = min(outMinValue, h); + outMaxValue = max(outMaxValue, h); + } + + // Prevent dividing by zero by setting a minimal height difference + float height_diff = max(outMaxValue - outMinValue, 1.0e-6f); + + // Calculate the scale factor to quantize to 16 bits + outQuantizationScale = float(cMaxHeightValue16) / height_diff; +} + +uint32 HeightFieldShapeSettings::CalculateBitsPerSampleForError(float inMaxError) const +{ + // Start with 1 bit per sample + uint32 bits_per_sample = 1; + + // Determine total range + float min_value, max_value, scale; + DetermineMinAndMaxSample(min_value, max_value, scale); + if (min_value < max_value) + { + // Loop over all blocks + for (uint y = 0; y < mSampleCount; y += mBlockSize) + for (uint x = 0; x < mSampleCount; x += mBlockSize) + { + // Determine min and max block value + take 1 sample border just like we do while building the hierarchical grids + float block_min_value = FLT_MAX, block_max_value = -FLT_MAX; + for (uint bx = x; bx < min(x + mBlockSize + 1, mSampleCount); ++bx) + for (uint by = y; by < min(y + mBlockSize + 1, mSampleCount); ++by) + { + float h = mHeightSamples[by * mSampleCount + bx]; + if (h != cNoCollisionValue) + { + block_min_value = min(block_min_value, h); + block_max_value = max(block_max_value, h); + } + } + + if (block_min_value < block_max_value) + { + // Quantize then dequantize block min/max value + block_min_value = min_value + floor((block_min_value - min_value) * scale) / scale; + block_max_value = min_value + ceil((block_max_value - min_value) * scale) / scale; + float block_height = block_max_value - block_min_value; + + // Loop over the block again + for (uint bx = x; bx < x + mBlockSize; ++bx) + for (uint by = y; by < y + mBlockSize; ++by) + { + // Get the height + float height = mHeightSamples[by * mSampleCount + bx]; + if (height != cNoCollisionValue) + { + for (;;) + { + // Determine bitmask for sample + uint32 sample_mask = (1 << bits_per_sample) - 1; + + // Quantize + float quantized_height = floor((height - block_min_value) * float(sample_mask) / block_height); + quantized_height = Clamp(quantized_height, 0.0f, float(sample_mask - 1)); + + // Dequantize and check error + float dequantized_height = block_min_value + (quantized_height + 0.5f) * block_height / float(sample_mask); + if (abs(dequantized_height - height) <= inMaxError) + break; + + // Not accurate enough, increase bits per sample + bits_per_sample++; + + // Don't go above 8 bits per sample + if (bits_per_sample == 8) + return bits_per_sample; + } + } + } + } + } + + } + + return bits_per_sample; +} + +void HeightFieldShape::CalculateActiveEdges(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, uint inHeightsStartX, uint inHeightsStartY, intptr_t inHeightsStride, float inHeightsScale, float inActiveEdgeCosThresholdAngle, TempAllocator &inAllocator) +{ + // Allocate temporary buffer for normals + uint normals_size = 2 * inSizeX * inSizeY * sizeof(Vec3); + Vec3 *normals = (Vec3 *)inAllocator.Allocate(normals_size); + JPH_SCOPE_EXIT([&inAllocator, normals, normals_size]{ inAllocator.Free(normals, normals_size); }); + + // Calculate triangle normals and make normals zero for triangles that are missing + Vec3 *out_normal = normals; + for (uint y = 0; y < inSizeY; ++y) + for (uint x = 0; x < inSizeX; ++x) + { + // Get height on diagonal + const float *height_samples = inHeights + (inY - inHeightsStartY + y) * inHeightsStride + (inX - inHeightsStartX + x); + float x1y1_h = height_samples[0]; + float x2y2_h = height_samples[inHeightsStride + 1]; + if (x1y1_h != cNoCollisionValue && x2y2_h != cNoCollisionValue) + { + // Calculate normal for lower left triangle (e.g. T1A) + float x1y2_h = height_samples[inHeightsStride]; + if (x1y2_h != cNoCollisionValue) + { + Vec3 x2y2_minus_x1y2(mScale.GetX(), inHeightsScale * (x2y2_h - x1y2_h), 0); + Vec3 x1y1_minus_x1y2(0, inHeightsScale * (x1y1_h - x1y2_h), -mScale.GetZ()); + out_normal[0] = x2y2_minus_x1y2.Cross(x1y1_minus_x1y2).Normalized(); + } + else + out_normal[0] = Vec3::sZero(); + + // Calculate normal for upper right triangle (e.g. T1B) + float x2y1_h = height_samples[1]; + if (x2y1_h != cNoCollisionValue) + { + Vec3 x1y1_minus_x2y1(-mScale.GetX(), inHeightsScale * (x1y1_h - x2y1_h), 0); + Vec3 x2y2_minus_x2y1(0, inHeightsScale * (x2y2_h - x2y1_h), mScale.GetZ()); + out_normal[1] = x1y1_minus_x2y1.Cross(x2y2_minus_x2y1).Normalized(); + } + else + out_normal[1] = Vec3::sZero(); + } + else + { + out_normal[0] = Vec3::sZero(); + out_normal[1] = Vec3::sZero(); + } + + out_normal += 2; + } + + // Calculate active edges + const Vec3 *in_normal = normals; + uint global_bit_pos = 3 * (inY * (mSampleCount - 1) + inX); + for (uint y = 0; y < inSizeY; ++y) + { + for (uint x = 0; x < inSizeX; ++x) + { + // Get vertex heights + const float *height_samples = inHeights + (inY - inHeightsStartY + y) * inHeightsStride + (inX - inHeightsStartX + x); + float x1y1_h = height_samples[0]; + float x1y2_h = height_samples[inHeightsStride]; + float x2y2_h = height_samples[inHeightsStride + 1]; + bool x1y1_valid = x1y1_h != cNoCollisionValue; + bool x1y2_valid = x1y2_h != cNoCollisionValue; + bool x2y2_valid = x2y2_h != cNoCollisionValue; + + // Calculate the edge flags (3 bits) + // See diagram in the next function for the edge numbering + uint16 edge_mask = 0b111; + uint16 edge_flags = 0; + + // Edge 0 + if (x == 0) + edge_mask &= 0b110; // We need normal x - 1 which we didn't calculate, don't update this edge + else if (x1y1_valid && x1y2_valid) + { + Vec3 edge0_direction(0, inHeightsScale * (x1y2_h - x1y1_h), mScale.GetZ()); + if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[-1], edge0_direction, inActiveEdgeCosThresholdAngle)) + edge_flags |= 0b001; + } + + // Edge 1 + if (y == inSizeY - 1) + edge_mask &= 0b101; // We need normal y + 1 which we didn't calculate, don't update this edge + else if (x1y2_valid && x2y2_valid) + { + Vec3 edge1_direction(mScale.GetX(), inHeightsScale * (x2y2_h - x1y2_h), 0); + if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[2 * inSizeX + 1], edge1_direction, inActiveEdgeCosThresholdAngle)) + edge_flags |= 0b010; + } + + // Edge 2 + if (x1y1_valid && x2y2_valid) + { + Vec3 edge2_direction(-mScale.GetX(), inHeightsScale * (x1y1_h - x2y2_h), -mScale.GetZ()); + if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[1], edge2_direction, inActiveEdgeCosThresholdAngle)) + edge_flags |= 0b100; + } + + // Store the edge flags in the array + uint byte_pos = global_bit_pos >> 3; + uint bit_pos = global_bit_pos & 0b111; + JPH_ASSERT(byte_pos < mActiveEdgesSize); + uint8 *edge_flags_ptr = &mActiveEdges[byte_pos]; + uint16 combined_edge_flags = uint16(edge_flags_ptr[0]) | uint16(uint16(edge_flags_ptr[1]) << 8); + combined_edge_flags &= ~(edge_mask << bit_pos); + combined_edge_flags |= edge_flags << bit_pos; + edge_flags_ptr[0] = uint8(combined_edge_flags); + edge_flags_ptr[1] = uint8(combined_edge_flags >> 8); + + in_normal += 2; + global_bit_pos += 3; + } + + global_bit_pos += 3 * (mSampleCount - 1 - inSizeX); + } +} + +void HeightFieldShape::CalculateActiveEdges(const HeightFieldShapeSettings &inSettings) +{ + /* + Store active edges. The triangles are organized like this: + x ---> + + y + + + | \ T1B | \ T2B + | e0 e2 | \ + | | T1A \ | T2A \ + V +--e1---+-------+ + | \ T3B | \ T4B + | \ | \ + | T3A \ | T4A \ + +-------+-------+ + We store active edges e0 .. e2 as bits 0 .. 2. + We store triangles horizontally then vertically (order T1A, T2A, T3A and T4A). + The top edge and right edge of the heightfield are always active so we do not need to store them, + therefore we only need to store (mSampleCount - 1)^2 * 3-bit + The triangles T1B, T2B, T3B and T4B do not need to be stored, their active edges can be constructed from adjacent triangles. + Add 1 byte padding so we can always read 1 uint16 to get the bits that cross an 8 bit boundary + */ + + // Make all edges active (if mSampleCount is bigger than inSettings.mSampleCount we need to fill up the padding, + // also edges at x = 0 and y = inSettings.mSampleCount - 1 are not updated) + memset(mActiveEdges, 0xff, mActiveEdgesSize); + + // Now clear the edges that are not active + TempAllocatorMalloc allocator; + CalculateActiveEdges(0, 0, inSettings.mSampleCount - 1, inSettings.mSampleCount - 1, inSettings.mHeightSamples.data(), 0, 0, inSettings.mSampleCount, inSettings.mScale.GetY(), inSettings.mActiveEdgeCosThresholdAngle, allocator); +} + +void HeightFieldShape::StoreMaterialIndices(const HeightFieldShapeSettings &inSettings) +{ + // We need to account for any rounding of the sample count to the nearest block size + uint in_count_min_1 = inSettings.mSampleCount - 1; + uint out_count_min_1 = mSampleCount - 1; + + mNumBitsPerMaterialIndex = 32 - CountLeadingZeros(max((uint32)mMaterials.size(), inSettings.mMaterialsCapacity) - 1); + mMaterialIndices.resize(((Square(out_count_min_1) * mNumBitsPerMaterialIndex + 7) >> 3) + 1, 0); // Add 1 byte so we don't read out of bounds when reading an uint16 + + if (mMaterials.size() > 1) + for (uint y = 0; y < out_count_min_1; ++y) + for (uint x = 0; x < out_count_min_1; ++x) + { + // Read material + uint16 material_index = x < in_count_min_1 && y < in_count_min_1? uint16(inSettings.mMaterialIndices[x + y * in_count_min_1]) : 0; + + // Calculate byte and bit position where the material index needs to go + uint sample_pos = x + y * out_count_min_1; + uint bit_pos = sample_pos * mNumBitsPerMaterialIndex; + uint byte_pos = bit_pos >> 3; + bit_pos &= 0b111; + + // Write the material index + material_index <<= bit_pos; + JPH_ASSERT(byte_pos + 1 < mMaterialIndices.size()); + mMaterialIndices[byte_pos] |= uint8(material_index); + mMaterialIndices[byte_pos + 1] |= uint8(material_index >> 8); + } +} + +void HeightFieldShape::CacheValues() +{ + mSampleMask = uint8((uint32(1) << mBitsPerSample) - 1); +} + +void HeightFieldShape::AllocateBuffers() +{ + uint num_blocks = GetNumBlocks(); + uint max_stride = (num_blocks + 1) >> 1; + mRangeBlocksSize = sGridOffsets[sGetMaxLevel(num_blocks) - 1] + Square(max_stride); + mHeightSamplesSize = (mSampleCount * mSampleCount * mBitsPerSample + 7) / 8 + 1; + mActiveEdgesSize = (Square(mSampleCount - 1) * 3 + 7) / 8 + 1; // See explanation at HeightFieldShape::CalculateActiveEdges + + JPH_ASSERT(mRangeBlocks == nullptr && mHeightSamples == nullptr && mActiveEdges == nullptr); + void *data = AlignedAllocate(mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize, alignof(RangeBlock)); + mRangeBlocks = reinterpret_cast(data); + mHeightSamples = reinterpret_cast(mRangeBlocks + mRangeBlocksSize); + mActiveEdges = mHeightSamples + mHeightSamplesSize; +} + +HeightFieldShape::HeightFieldShape(const HeightFieldShapeSettings &inSettings, ShapeResult &outResult) : + Shape(EShapeType::HeightField, EShapeSubType::HeightField, inSettings, outResult), + mOffset(inSettings.mOffset), + mScale(inSettings.mScale), + mSampleCount(((inSettings.mSampleCount + inSettings.mBlockSize - 1) / inSettings.mBlockSize) * inSettings.mBlockSize), // Round sample count to nearest block size + mBlockSize(inSettings.mBlockSize), + mBitsPerSample(uint8(inSettings.mBitsPerSample)) +{ + CacheValues(); + + // Reserve a bigger materials list if requested + if (inSettings.mMaterialsCapacity > 0) + mMaterials.reserve(inSettings.mMaterialsCapacity); + mMaterials = inSettings.mMaterials; + + // Check block size + if (mBlockSize < 2 || mBlockSize > 8) + { + outResult.SetError("HeightFieldShape: Block size must be in the range [2, 8]!"); + return; + } + + // Check bits per sample + if (inSettings.mBitsPerSample < 1 || inSettings.mBitsPerSample > 8) + { + outResult.SetError("HeightFieldShape: Bits per sample must be in the range [1, 8]!"); + return; + } + + // We stop at mBlockSize x mBlockSize height sample blocks + uint num_blocks = GetNumBlocks(); + + // We want at least 1 grid layer + if (num_blocks < 2) + { + outResult.SetError("HeightFieldShape: Sample count too low!"); + return; + } + + // Check that we don't overflow our 32 bit 'properties' + if (num_blocks > (1 << cNumBitsXY)) + { + outResult.SetError("HeightFieldShape: Sample count too high!"); + return; + } + + // Check if we're not exceeding the amount of sub shape id bits + if (GetSubShapeIDBitsRecursive() > SubShapeID::MaxBits) + { + outResult.SetError("HeightFieldShape: Size exceeds the amount of available sub shape ID bits!"); + return; + } + + if (!mMaterials.empty()) + { + // Validate materials + if (mMaterials.size() > 256) + { + outResult.SetError("Supporting max 256 materials per height field"); + return; + } + for (uint8 s : inSettings.mMaterialIndices) + if (s >= mMaterials.size()) + { + outResult.SetError(StringFormat("Material %u is beyond material list (size: %u)", s, (uint)mMaterials.size())); + return; + } + } + else + { + // No materials assigned, validate that no materials have been specified + if (!inSettings.mMaterialIndices.empty()) + { + outResult.SetError("No materials present, mMaterialIndices should be empty"); + return; + } + } + + // Determine range + float min_value, max_value, scale; + inSettings.DetermineMinAndMaxSample(min_value, max_value, scale); + if (min_value > max_value) + { + // If there is no collision with this heightmap, leave everything empty + mMaterials.clear(); + outResult.Set(this); + return; + } + + // Allocate space for this shape + AllocateBuffers(); + + // Quantize to uint16 + Array quantized_samples; + quantized_samples.reserve(mSampleCount * mSampleCount); + for (uint y = 0; y < inSettings.mSampleCount; ++y) + { + for (uint x = 0; x < inSettings.mSampleCount; ++x) + { + float h = inSettings.mHeightSamples[x + y * inSettings.mSampleCount]; + if (h == cNoCollisionValue) + { + quantized_samples.push_back(cNoCollisionValue16); + } + else + { + // Floor the quantized height to get a lower bound for the quantized value + int quantized_height = (int)floor(scale * (h - min_value)); + + // Ensure that the height says below the max height value so we can safely add 1 to get the upper bound for the quantized value + quantized_height = Clamp(quantized_height, 0, int(cMaxHeightValue16 - 1)); + + quantized_samples.push_back(uint16(quantized_height)); + } + } + // Pad remaining columns with no collision + for (uint x = inSettings.mSampleCount; x < mSampleCount; ++x) + quantized_samples.push_back(cNoCollisionValue16); + } + // Pad remaining rows with no collision + for (uint y = inSettings.mSampleCount; y < mSampleCount; ++y) + for (uint x = 0; x < mSampleCount; ++x) + quantized_samples.push_back(cNoCollisionValue16); + + // Update offset and scale to account for the compression to uint16 + if (min_value <= max_value) // Only when there was collision + { + // In GetPosition we always add 0.5 to the quantized sample in order to reduce the average error. + // We want to be able to exactly quantize min_value (this is important in case the heightfield is entirely flat) so we subtract that value from min_value. + min_value -= 0.5f / (scale * mSampleMask); + + mOffset.SetY(mOffset.GetY() + mScale.GetY() * min_value); + } + mScale.SetY(mScale.GetY() / scale); + + // Calculate amount of grids + uint max_level = sGetMaxLevel(num_blocks); + + // Temporary data structure used during creating of a hierarchy of grids + struct Range + { + uint16 mMin; + uint16 mMax; + }; + + // Reserve size for temporary range data + reserve 1 extra for a 1x1 grid that we won't store but use for calculating the bounding box + Array> ranges; + ranges.resize(max_level + 1); + + // Calculate highest detail grid by combining mBlockSize x mBlockSize height samples + Array *cur_range_vector = &ranges.back(); + uint num_blocks_pow2 = GetNextPowerOf2(num_blocks); // We calculate the range blocks as if the heightfield was a power of 2, when we save the range blocks we'll ignore the extra samples (this makes downsampling easier) + cur_range_vector->resize(num_blocks_pow2 * num_blocks_pow2); + Range *range_dst = &cur_range_vector->front(); + for (uint y = 0; y < num_blocks_pow2; ++y) + for (uint x = 0; x < num_blocks_pow2; ++x) + { + range_dst->mMin = 0xffff; + range_dst->mMax = 0; + uint max_bx = x == num_blocks_pow2 - 1? mBlockSize : mBlockSize + 1; // for interior blocks take 1 more because the triangles connect to the next block so we must include their height too + uint max_by = y == num_blocks_pow2 - 1? mBlockSize : mBlockSize + 1; + for (uint by = 0; by < max_by; ++by) + for (uint bx = 0; bx < max_bx; ++bx) + { + uint sx = x * mBlockSize + bx; + uint sy = y * mBlockSize + by; + if (sx < mSampleCount && sy < mSampleCount) + { + uint16 h = quantized_samples[sy * mSampleCount + sx]; + if (h != cNoCollisionValue16) + { + range_dst->mMin = min(range_dst->mMin, h); + range_dst->mMax = max(range_dst->mMax, uint16(h + 1)); // Add 1 to the max so we know the real value is between mMin and mMax + } + } + } + ++range_dst; + } + + // Calculate remaining grids + for (uint n = num_blocks_pow2 >> 1; n >= 1; n >>= 1) + { + // Get source buffer + const Range *range_src = &cur_range_vector->front(); + + // Previous array element + --cur_range_vector; + + // Make space for this grid + cur_range_vector->resize(n * n); + + // Get target buffer + range_dst = &cur_range_vector->front(); + + // Combine the results of 2x2 ranges + for (uint y = 0; y < n; ++y) + for (uint x = 0; x < n; ++x) + { + range_dst->mMin = 0xffff; + range_dst->mMax = 0; + for (uint by = 0; by < 2; ++by) + for (uint bx = 0; bx < 2; ++bx) + { + const Range &r = range_src[(y * 2 + by) * n * 2 + x * 2 + bx]; + range_dst->mMin = min(range_dst->mMin, r.mMin); + range_dst->mMax = max(range_dst->mMax, r.mMax); + } + ++range_dst; + } + } + JPH_ASSERT(cur_range_vector == &ranges.front()); + + // Store global range for bounding box calculation + mMinSample = ranges[0][0].mMin; + mMaxSample = ranges[0][0].mMax; + +#ifdef JPH_ENABLE_ASSERTS + // Validate that we did not lose range along the way + uint16 minv = 0xffff, maxv = 0; + for (uint16 v : quantized_samples) + if (v != cNoCollisionValue16) + { + minv = min(minv, v); + maxv = max(maxv, uint16(v + 1)); + } + JPH_ASSERT(mMinSample == minv && mMaxSample == maxv); +#endif + + // Now erase the first element, we need a 2x2 grid to start with + ranges.erase(ranges.begin()); + + // Create blocks + uint max_stride = (num_blocks + 1) >> 1; + RangeBlock *current_block = mRangeBlocks; + for (uint level = 0; level < ranges.size(); ++level) + { + JPH_ASSERT(uint(current_block - mRangeBlocks) == sGridOffsets[level]); + + uint in_n = 1 << level; + uint out_n = min(in_n, max_stride); // At the most detailed level we store a non-power of 2 number of blocks + + for (uint y = 0; y < out_n; ++y) + for (uint x = 0; x < out_n; ++x) + { + // Convert from 2x2 Range structure to 1 RangeBlock structure + RangeBlock &rb = *current_block++; + for (uint by = 0; by < 2; ++by) + for (uint bx = 0; bx < 2; ++bx) + { + uint src_pos = (y * 2 + by) * 2 * in_n + (x * 2 + bx); + uint dst_pos = by * 2 + bx; + rb.mMin[dst_pos] = ranges[level][src_pos].mMin; + rb.mMax[dst_pos] = ranges[level][src_pos].mMax; + } + } + } + JPH_ASSERT(uint32(current_block - mRangeBlocks) == mRangeBlocksSize); + + // Quantize height samples + memset(mHeightSamples, 0, mHeightSamplesSize); + int sample = 0; + for (uint y = 0; y < mSampleCount; ++y) + for (uint x = 0; x < mSampleCount; ++x) + { + uint32 output_value; + + float h = x < inSettings.mSampleCount && y < inSettings.mSampleCount? inSettings.mHeightSamples[x + y * inSettings.mSampleCount] : cNoCollisionValue; + if (h == cNoCollisionValue) + { + // No collision + output_value = mSampleMask; + } + else + { + // Get range of block so we know what range to compress to + uint bx = x / mBlockSize; + uint by = y / mBlockSize; + const Range &range = ranges.back()[by * num_blocks_pow2 + bx]; + JPH_ASSERT(range.mMin < range.mMax); + + // Quantize to mBitsPerSample bits, note that mSampleMask is reserved for indicating that there's no collision. + // We divide the range into mSampleMask segments and use the mid points of these segments as the quantized values. + // This results in a lower error than if we had quantized our data using the lowest point of all these segments. + float h_min = min_value + range.mMin / scale; + float h_delta = float(range.mMax - range.mMin) / scale; + float quantized_height = floor((h - h_min) * float(mSampleMask) / h_delta); + output_value = uint32(Clamp((int)quantized_height, 0, int(mSampleMask) - 1)); // mSampleMask is reserved as 'no collision value' + } + + // Store the sample + uint byte_pos = sample >> 3; + uint bit_pos = sample & 0b111; + output_value <<= bit_pos; + JPH_ASSERT(byte_pos + 1 < mHeightSamplesSize); + mHeightSamples[byte_pos] |= uint8(output_value); + mHeightSamples[byte_pos + 1] |= uint8(output_value >> 8); + sample += inSettings.mBitsPerSample; + } + + // Calculate the active edges + CalculateActiveEdges(inSettings); + + // Compress material indices + if (mMaterials.size() > 1 || inSettings.mMaterialsCapacity > 1) + StoreMaterialIndices(inSettings); + + outResult.Set(this); +} + +HeightFieldShape::~HeightFieldShape() +{ + if (mRangeBlocks != nullptr) + AlignedFree(mRangeBlocks); +} + +Ref HeightFieldShape::Clone() const +{ + Ref clone = new HeightFieldShape; + clone->SetUserData(GetUserData()); + + clone->mOffset = mOffset; + clone->mScale = mScale; + clone->mSampleCount = mSampleCount; + clone->mBlockSize = mBlockSize; + clone->mBitsPerSample = mBitsPerSample; + clone->mSampleMask = mSampleMask; + clone->mMinSample = mMinSample; + clone->mMaxSample = mMaxSample; + + clone->AllocateBuffers(); + memcpy(clone->mRangeBlocks, mRangeBlocks, mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize); // Copy the entire buffer in 1 go + + clone->mMaterials.reserve(mMaterials.capacity()); // Ensure we keep the capacity of the original + clone->mMaterials = mMaterials; + clone->mMaterialIndices = mMaterialIndices; + clone->mNumBitsPerMaterialIndex = mNumBitsPerMaterialIndex; + +#ifdef JPH_DEBUG_RENDERER + clone->mGeometry = mGeometry; + clone->mCachedUseMaterialColors = mCachedUseMaterialColors; +#endif // JPH_DEBUG_RENDERER + + return clone; +} + +inline void HeightFieldShape::sGetRangeBlockOffsetAndStride(uint inNumBlocks, uint inMaxLevel, uint &outRangeBlockOffset, uint &outRangeBlockStride) +{ + outRangeBlockOffset = sGridOffsets[inMaxLevel - 1]; + outRangeBlockStride = (inNumBlocks + 1) >> 1; +} + +inline void HeightFieldShape::GetRangeBlock(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, RangeBlock *&outBlock, uint &outIndexInBlock) +{ + JPH_ASSERT(inBlockX < GetNumBlocks() && inBlockY < GetNumBlocks()); + + // Convert to location of range block + uint rbx = inBlockX >> 1; + uint rby = inBlockY >> 1; + outIndexInBlock = ((inBlockY & 1) << 1) + (inBlockX & 1); + + uint offset = inRangeBlockOffset + rby * inRangeBlockStride + rbx; + JPH_ASSERT(offset < mRangeBlocksSize); + outBlock = mRangeBlocks + offset; +} + +inline void HeightFieldShape::GetBlockOffsetAndScale(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, float &outBlockOffset, float &outBlockScale) const +{ + JPH_ASSERT(inBlockX < GetNumBlocks() && inBlockY < GetNumBlocks()); + + // Convert to location of range block + uint rbx = inBlockX >> 1; + uint rby = inBlockY >> 1; + uint n = ((inBlockY & 1) << 1) + (inBlockX & 1); + + // Calculate offset and scale + uint offset = inRangeBlockOffset + rby * inRangeBlockStride + rbx; + JPH_ASSERT(offset < mRangeBlocksSize); + const RangeBlock &block = mRangeBlocks[offset]; + outBlockOffset = float(block.mMin[n]); + outBlockScale = float(block.mMax[n] - block.mMin[n]) / float(mSampleMask); +} + +inline uint8 HeightFieldShape::GetHeightSample(uint inX, uint inY) const +{ + JPH_ASSERT(inX < mSampleCount); + JPH_ASSERT(inY < mSampleCount); + + // Determine bit position of sample + uint sample = (inY * mSampleCount + inX) * uint(mBitsPerSample); + uint byte_pos = sample >> 3; + uint bit_pos = sample & 0b111; + + // Fetch the height sample value + JPH_ASSERT(byte_pos + 1 < mHeightSamplesSize); + const uint8 *height_samples = mHeightSamples + byte_pos; + uint16 height_sample = uint16(height_samples[0]) | uint16(uint16(height_samples[1]) << 8); + return uint8(height_sample >> bit_pos) & mSampleMask; +} + +inline Vec3 HeightFieldShape::GetPosition(uint inX, uint inY, float inBlockOffset, float inBlockScale, bool &outNoCollision) const +{ + // Get quantized value + uint8 height_sample = GetHeightSample(inX, inY); + outNoCollision = height_sample == mSampleMask; + + // Add 0.5 to the quantized value to minimize the error (see constructor) + return mOffset + mScale * Vec3(float(inX), inBlockOffset + (0.5f + height_sample) * inBlockScale, float(inY)); +} + +Vec3 HeightFieldShape::GetPosition(uint inX, uint inY) const +{ + // Test if there are any samples + if (mHeightSamplesSize == 0) + return mOffset + mScale * Vec3(float(inX), 0.0f, float(inY)); + + // Get block location + uint bx = inX / mBlockSize; + uint by = inY / mBlockSize; + + // Calculate offset and stride + uint num_blocks = GetNumBlocks(); + uint range_block_offset, range_block_stride; + sGetRangeBlockOffsetAndStride(num_blocks, sGetMaxLevel(num_blocks), range_block_offset, range_block_stride); + + float offset, scale; + GetBlockOffsetAndScale(bx, by, range_block_offset, range_block_stride, offset, scale); + + bool no_collision; + return GetPosition(inX, inY, offset, scale, no_collision); +} + +bool HeightFieldShape::IsNoCollision(uint inX, uint inY) const +{ + return mHeightSamplesSize == 0 || GetHeightSample(inX, inY) == mSampleMask; +} + +bool HeightFieldShape::ProjectOntoSurface(Vec3Arg inLocalPosition, Vec3 &outSurfacePosition, SubShapeID &outSubShapeID) const +{ + // Check if we have collision + if (mHeightSamplesSize == 0) + return false; + + // Convert coordinate to integer space + Vec3 integer_space = (inLocalPosition - mOffset) / mScale; + + // Get x coordinate and fraction + float x_frac = integer_space.GetX(); + if (x_frac < 0.0f || x_frac >= mSampleCount - 1) + return false; + uint x = (uint)floor(x_frac); + x_frac -= x; + + // Get y coordinate and fraction + float y_frac = integer_space.GetZ(); + if (y_frac < 0.0f || y_frac >= mSampleCount - 1) + return false; + uint y = (uint)floor(y_frac); + y_frac -= y; + + // If one of the diagonal points doesn't have collision, we don't have a height at this location + if (IsNoCollision(x, y) || IsNoCollision(x + 1, y + 1)) + return false; + + if (y_frac >= x_frac) + { + // Left bottom triangle, test the 3rd point + if (IsNoCollision(x, y + 1)) + return false; + + // Interpolate height value + Vec3 v1 = GetPosition(x, y); + Vec3 v2 = GetPosition(x, y + 1); + Vec3 v3 = GetPosition(x + 1, y + 1); + outSurfacePosition = v1 + y_frac * (v2 - v1) + x_frac * (v3 - v2); + SubShapeIDCreator creator; + outSubShapeID = EncodeSubShapeID(creator, x, y, 0); + return true; + } + else + { + // Right top triangle, test the third point + if (IsNoCollision(x + 1, y)) + return false; + + // Interpolate height value + Vec3 v1 = GetPosition(x, y); + Vec3 v2 = GetPosition(x + 1, y + 1); + Vec3 v3 = GetPosition(x + 1, y); + outSurfacePosition = v1 + y_frac * (v2 - v3) + x_frac * (v3 - v1); + SubShapeIDCreator creator; + outSubShapeID = EncodeSubShapeID(creator, x, y, 1); + return true; + } +} + +void HeightFieldShape::GetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, float *outHeights, intptr_t inHeightsStride) const +{ + if (inSizeX == 0 || inSizeY == 0) + return; + + JPH_ASSERT(inX % mBlockSize == 0 && inY % mBlockSize == 0); + JPH_ASSERT(inX < mSampleCount && inY < mSampleCount); + JPH_ASSERT(inX + inSizeX <= mSampleCount && inY + inSizeY <= mSampleCount); + + // Test if there are any samples + if (mHeightSamplesSize == 0) + { + // No samples, return the offset + float offset = mOffset.GetY(); + for (uint y = 0; y < inSizeY; ++y, outHeights += inHeightsStride) + for (uint x = 0; x < inSizeX; ++x) + outHeights[x] = offset; + } + else + { + // Calculate offset and stride + uint num_blocks = GetNumBlocks(); + uint range_block_offset, range_block_stride; + sGetRangeBlockOffsetAndStride(num_blocks, sGetMaxLevel(num_blocks), range_block_offset, range_block_stride); + + // Loop over blocks + uint block_start_x = inX / mBlockSize; + uint block_start_y = inY / mBlockSize; + uint num_blocks_x = inSizeX / mBlockSize; + uint num_blocks_y = inSizeY / mBlockSize; + for (uint block_y = 0; block_y < num_blocks_y; ++block_y) + for (uint block_x = 0; block_x < num_blocks_x; ++block_x) + { + // Get offset and scale for block + float offset, scale; + GetBlockOffsetAndScale(block_start_x + block_x, block_start_y + block_y, range_block_offset, range_block_stride, offset, scale); + + // Adjust by global offset and scale + // Note: This is the math applied in GetPosition() written out to reduce calculations in the inner loop + scale *= mScale.GetY(); + offset = mOffset.GetY() + mScale.GetY() * offset + 0.5f * scale; + + // Loop over samples in block + for (uint sample_y = 0; sample_y < mBlockSize; ++sample_y) + for (uint sample_x = 0; sample_x < mBlockSize; ++sample_x) + { + // Calculate output coordinate + uint output_x = block_x * mBlockSize + sample_x; + uint output_y = block_y * mBlockSize + sample_y; + + // Get quantized value + uint8 height_sample = GetHeightSample(inX + output_x, inY + output_y); + + // Dequantize + float h = height_sample != mSampleMask? offset + height_sample * scale : cNoCollisionValue; + outHeights[output_y * inHeightsStride + output_x] = h; + } + } + } +} + +void HeightFieldShape::SetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, intptr_t inHeightsStride, TempAllocator &inAllocator, float inActiveEdgeCosThresholdAngle) +{ + if (inSizeX == 0 || inSizeY == 0) + return; + + JPH_ASSERT(mHeightSamplesSize > 0); + JPH_ASSERT(inX % mBlockSize == 0 && inY % mBlockSize == 0); + JPH_ASSERT(inX < mSampleCount && inY < mSampleCount); + JPH_ASSERT(inX + inSizeX <= mSampleCount && inY + inSizeY <= mSampleCount); + + // If we have a block in negative x/y direction, we will affect its range so we need to take it into account + bool need_temp_heights = false; + uint affected_x = inX; + uint affected_y = inY; + uint affected_size_x = inSizeX; + uint affected_size_y = inSizeY; + if (inX > 0) { affected_x -= mBlockSize; affected_size_x += mBlockSize; need_temp_heights = true; } + if (inY > 0) { affected_y -= mBlockSize; affected_size_y += mBlockSize; need_temp_heights = true; } + + // If we have a block in positive x/y direction, our ranges are affected by it so we need to take it into account + uint heights_size_x = affected_size_x; + uint heights_size_y = affected_size_y; + if (inX + inSizeX < mSampleCount) { heights_size_x += mBlockSize; need_temp_heights = true; } + if (inY + inSizeY < mSampleCount) { heights_size_y += mBlockSize; need_temp_heights = true; } + + // Get heights for affected area + const float *heights; + intptr_t heights_stride; + float *temp_heights; + if (need_temp_heights) + { + // Fetch the surrounding height data (note we're forced to recompress this data with a potentially different range so there will be some precision loss here) + temp_heights = (float *)inAllocator.Allocate(heights_size_x * heights_size_y * sizeof(float)); + heights = temp_heights; + heights_stride = heights_size_x; + + // We need to fill in the following areas: + // + // +-----------------+ + // | 2 | + // |---+---------+---| + // | | | | + // | 3 | 1 | 4 | + // | | | | + // |---+---------+---| + // | 5 | + // +-----------------+ + // + // 1. The area that is affected by the new heights (we just copy these) + // 2-5. These areas are either needed to calculate the range of the affected blocks or they need to be recompressed with a different range + uint offset_x = inX - affected_x; + uint offset_y = inY - affected_y; + + // Area 2 + GetHeights(affected_x, affected_y, heights_size_x, offset_y, temp_heights, heights_size_x); + float *area3_start = temp_heights + offset_y * heights_size_x; + + // Area 3 + GetHeights(affected_x, inY, offset_x, inSizeY, area3_start, heights_size_x); + + // Area 1 + float *area1_start = area3_start + offset_x; + for (uint y = 0; y < inSizeY; ++y, area1_start += heights_size_x, inHeights += inHeightsStride) + memcpy(area1_start, inHeights, inSizeX * sizeof(float)); + + // Area 4 + uint area4_x = inX + inSizeX; + GetHeights(area4_x, inY, affected_x + heights_size_x - area4_x, inSizeY, area3_start + area4_x - affected_x, heights_size_x); + + // Area 5 + uint area5_y = inY + inSizeY; + float *area5_start = temp_heights + (area5_y - affected_y) * heights_size_x; + GetHeights(affected_x, area5_y, heights_size_x, affected_y + heights_size_y - area5_y, area5_start, heights_size_x); + } + else + { + // We can directly use the input buffer because there are no extra edges to take into account + heights = inHeights; + heights_stride = inHeightsStride; + temp_heights = nullptr; + } + + // Calculate offset and stride + uint num_blocks = GetNumBlocks(); + uint range_block_offset, range_block_stride; + uint max_level = sGetMaxLevel(num_blocks); + sGetRangeBlockOffsetAndStride(num_blocks, max_level, range_block_offset, range_block_stride); + + // Loop over blocks + uint block_start_x = affected_x / mBlockSize; + uint block_start_y = affected_y / mBlockSize; + uint num_blocks_x = affected_size_x / mBlockSize; + uint num_blocks_y = affected_size_y / mBlockSize; + for (uint block_y = 0, sample_start_y = 0; block_y < num_blocks_y; ++block_y, sample_start_y += mBlockSize) + for (uint block_x = 0, sample_start_x = 0; block_x < num_blocks_x; ++block_x, sample_start_x += mBlockSize) + { + // Determine quantized min and max value for block + // Note that we need to include 1 extra row in the positive x/y direction to account for connecting triangles + int min_value = 0xffff; + int max_value = 0; + uint sample_x_end = min(sample_start_x + mBlockSize + 1, mSampleCount - affected_x); + uint sample_y_end = min(sample_start_y + mBlockSize + 1, mSampleCount - affected_y); + for (uint sample_y = sample_start_y; sample_y < sample_y_end; ++sample_y) + for (uint sample_x = sample_start_x; sample_x < sample_x_end; ++sample_x) + { + float h = heights[sample_y * heights_stride + sample_x]; + if (h != cNoCollisionValue) + { + int quantized_height = Clamp((int)floor((h - mOffset.GetY()) / mScale.GetY()), 0, int(cMaxHeightValue16 - 1)); + min_value = min(min_value, quantized_height); + max_value = max(max_value, quantized_height + 1); + } + } + if (min_value > max_value) + min_value = max_value = cNoCollisionValue16; + + // Update range for block + RangeBlock *range_block; + uint index_in_block; + GetRangeBlock(block_start_x + block_x, block_start_y + block_y, range_block_offset, range_block_stride, range_block, index_in_block); + range_block->mMin[index_in_block] = uint16(min_value); + range_block->mMax[index_in_block] = uint16(max_value); + + // Get offset and scale for block + float offset_block = float(min_value); + float scale_block = float(max_value - min_value) / float(mSampleMask); + + // Calculate scale and offset using the formula used in GetPosition() solved for the quantized height (excluding 0.5 because we round down while quantizing) + float scale = scale_block * mScale.GetY(); + float offset = mOffset.GetY() + offset_block * mScale.GetY(); + + // Loop over samples in block + sample_x_end = sample_start_x + mBlockSize; + sample_y_end = sample_start_y + mBlockSize; + for (uint sample_y = sample_start_y; sample_y < sample_y_end; ++sample_y) + for (uint sample_x = sample_start_x; sample_x < sample_x_end; ++sample_x) + { + // Quantize height + float h = heights[sample_y * heights_stride + sample_x]; + uint8 quantized_height = h != cNoCollisionValue? uint8(Clamp((int)floor((h - offset) / scale), 0, int(mSampleMask) - 1)) : mSampleMask; + + // Determine bit position of sample + uint sample = ((affected_y + sample_y) * mSampleCount + affected_x + sample_x) * uint(mBitsPerSample); + uint byte_pos = sample >> 3; + uint bit_pos = sample & 0b111; + + // Update the height value sample + JPH_ASSERT(byte_pos + 1 < mHeightSamplesSize); + uint8 *height_samples = mHeightSamples + byte_pos; + uint16 height_sample = uint16(height_samples[0]) | uint16(uint16(height_samples[1]) << 8); + height_sample &= ~(uint16(mSampleMask) << bit_pos); + height_sample |= uint16(quantized_height) << bit_pos; + height_samples[0] = uint8(height_sample); + height_samples[1] = uint8(height_sample >> 8); + } + } + + // Update active edges + // Note that we must take an extra row on all sides to account for connecting triangles + uint ae_x = inX > 1? inX - 2 : 0; + uint ae_y = inY > 1? inY - 2 : 0; + uint ae_sx = min(inX + inSizeX + 1, mSampleCount - 1) - ae_x; + uint ae_sy = min(inY + inSizeY + 1, mSampleCount - 1) - ae_y; + CalculateActiveEdges(ae_x, ae_y, ae_sx, ae_sy, heights, affected_x, affected_y, heights_stride, 1.0f, inActiveEdgeCosThresholdAngle, inAllocator); + + // Free temporary buffer + if (temp_heights != nullptr) + inAllocator.Free(temp_heights, heights_size_x * heights_size_y * sizeof(float)); + + // Update hierarchy of range blocks + while (max_level > 1) + { + // Get offset and stride for destination blocks + uint dst_range_block_offset, dst_range_block_stride; + sGetRangeBlockOffsetAndStride(num_blocks >> 1, max_level - 1, dst_range_block_offset, dst_range_block_stride); + + // We'll be processing 2x2 blocks below so we need the start coordinates to be even and we extend the number of blocks to correct for that + if (block_start_x & 1) { --block_start_x; ++num_blocks_x; } + if (block_start_y & 1) { --block_start_y; ++num_blocks_y; } + + // Loop over all affected blocks + uint block_end_x = block_start_x + num_blocks_x; + uint block_end_y = block_start_y + num_blocks_y; + for (uint block_y = block_start_y; block_y < block_end_y; block_y += 2) + for (uint block_x = block_start_x; block_x < block_end_x; block_x += 2) + { + // Get source range block + RangeBlock *src_range_block; + uint index_in_src_block; + GetRangeBlock(block_x, block_y, range_block_offset, range_block_stride, src_range_block, index_in_src_block); + + // Determine quantized min and max value for the entire 2x2 block + uint16 min_value = 0xffff; + uint16 max_value = 0; + for (uint i = 0; i < 4; ++i) + if (src_range_block->mMin[i] != cNoCollisionValue16) + { + min_value = min(min_value, src_range_block->mMin[i]); + max_value = max(max_value, src_range_block->mMax[i]); + } + + // Write to destination block + RangeBlock *dst_range_block; + uint index_in_dst_block; + GetRangeBlock(block_x >> 1, block_y >> 1, dst_range_block_offset, dst_range_block_stride, dst_range_block, index_in_dst_block); + dst_range_block->mMin[index_in_dst_block] = uint16(min_value); + dst_range_block->mMax[index_in_dst_block] = uint16(max_value); + } + + // Go up one level + --max_level; + num_blocks >>= 1; + block_start_x >>= 1; + block_start_y >>= 1; + num_blocks_x = min((num_blocks_x + 1) >> 1, num_blocks); + num_blocks_y = min((num_blocks_y + 1) >> 1, num_blocks); + + // Update stride and offset for source to old destination + range_block_offset = dst_range_block_offset; + range_block_stride = dst_range_block_stride; + } + + // Calculate new min and max sample for the entire height field + mMinSample = 0xffff; + mMaxSample = 0; + for (uint i = 0; i < 4; ++i) + if (mRangeBlocks[0].mMin[i] != cNoCollisionValue16) + { + mMinSample = min(mMinSample, mRangeBlocks[0].mMin[i]); + mMaxSample = max(mMaxSample, mRangeBlocks[0].mMax[i]); + } + +#ifdef JPH_DEBUG_RENDERER + // Invalidate temporary rendering data + mGeometry.clear(); +#endif +} + +void HeightFieldShape::GetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, uint8 *outMaterials, intptr_t inMaterialsStride) const +{ + if (inSizeX == 0 || inSizeY == 0) + return; + + if (mMaterialIndices.empty()) + { + // Return all 0's + for (uint y = 0; y < inSizeY; ++y) + { + uint8 *out_indices = outMaterials + y * inMaterialsStride; + for (uint x = 0; x < inSizeX; ++x) + *out_indices++ = 0; + } + return; + } + + JPH_ASSERT(inX < mSampleCount && inY < mSampleCount); + JPH_ASSERT(inX + inSizeX < mSampleCount && inY + inSizeY < mSampleCount); + + uint count_min_1 = mSampleCount - 1; + uint16 material_index_mask = uint16((1 << mNumBitsPerMaterialIndex) - 1); + + for (uint y = 0; y < inSizeY; ++y) + { + // Calculate input position + uint bit_pos = (inX + (inY + y) * count_min_1) * mNumBitsPerMaterialIndex; + const uint8 *in_indices = mMaterialIndices.data() + (bit_pos >> 3); + bit_pos &= 0b111; + + // Calculate output position + uint8 *out_indices = outMaterials + y * inMaterialsStride; + + for (uint x = 0; x < inSizeX; ++x) + { + // Get material index + uint16 material_index = uint16(in_indices[0]) + uint16(uint16(in_indices[1]) << 8); + material_index >>= bit_pos; + material_index &= material_index_mask; + *out_indices = uint8(material_index); + + // Go to the next index + bit_pos += mNumBitsPerMaterialIndex; + in_indices += bit_pos >> 3; + bit_pos &= 0b111; + ++out_indices; + } + } +} + +bool HeightFieldShape::SetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, const uint8 *inMaterials, intptr_t inMaterialsStride, const PhysicsMaterialList *inMaterialList, TempAllocator &inAllocator) +{ + if (inSizeX == 0 || inSizeY == 0) + return true; + + JPH_ASSERT(inX < mSampleCount && inY < mSampleCount); + JPH_ASSERT(inX + inSizeX < mSampleCount && inY + inSizeY < mSampleCount); + + // Remap materials + uint material_remap_table_size = uint(inMaterialList != nullptr? inMaterialList->size() : mMaterials.size()); + uint8 *material_remap_table = (uint8 *)inAllocator.Allocate(material_remap_table_size); + JPH_SCOPE_EXIT([&inAllocator, material_remap_table, material_remap_table_size]{ inAllocator.Free(material_remap_table, material_remap_table_size); }); + if (inMaterialList != nullptr) + { + // Conservatively reserve more space if the incoming material list is bigger + if (inMaterialList->size() > mMaterials.size()) + mMaterials.reserve(inMaterialList->size()); + + // Create a remap table + uint8 *remap_entry = material_remap_table; + for (const PhysicsMaterial *material : *inMaterialList) + { + // Try to find it in the existing list + PhysicsMaterialList::const_iterator it = std::find(mMaterials.begin(), mMaterials.end(), material); + if (it != mMaterials.end()) + { + // Found it, calculate index + *remap_entry = uint8(it - mMaterials.begin()); + } + else + { + // Not found, add it + if (mMaterials.size() >= 256) + { + // We can't have more than 256 materials since we use uint8 as indices + return false; + } + *remap_entry = uint8(mMaterials.size()); + mMaterials.push_back(material); + } + ++remap_entry; + } + } + else + { + // No remapping + for (uint i = 0; i < material_remap_table_size; ++i) + material_remap_table[i] = uint8(i); + } + + if (mMaterials.size() == 1) + { + // Only 1 material, we don't need to store the material indices + return true; + } + + // Check if we need to resize the material indices array + uint count_min_1 = mSampleCount - 1; + uint32 new_bits_per_material_index = 32 - CountLeadingZeros((uint32)mMaterials.size() - 1); + JPH_ASSERT(mNumBitsPerMaterialIndex <= 8 && new_bits_per_material_index <= 8); + if (new_bits_per_material_index > mNumBitsPerMaterialIndex) + { + // Resize the material indices array + mMaterialIndices.resize(((Square(count_min_1) * new_bits_per_material_index + 7) >> 3) + 1, 0); // Add 1 byte so we don't read out of bounds when reading an uint16 + + // Calculate old and new mask + uint16 old_material_index_mask = uint16((1 << mNumBitsPerMaterialIndex) - 1); + uint16 new_material_index_mask = uint16((1 << new_bits_per_material_index) - 1); + + // Loop through the array backwards to avoid overwriting data + int in_bit_pos = (count_min_1 * count_min_1 - 1) * mNumBitsPerMaterialIndex; + const uint8 *in_indices = mMaterialIndices.data() + (in_bit_pos >> 3); + in_bit_pos &= 0b111; + int out_bit_pos = (count_min_1 * count_min_1 - 1) * new_bits_per_material_index; + uint8 *out_indices = mMaterialIndices.data() + (out_bit_pos >> 3); + out_bit_pos &= 0b111; + + while (out_indices >= mMaterialIndices.data()) + { + // Read the material index + uint16 material_index = uint16(in_indices[0]) + uint16(uint16(in_indices[1]) << 8); + material_index >>= in_bit_pos; + material_index &= old_material_index_mask; + + // Write the material index + uint16 output_data = uint16(out_indices[0]) + uint16(uint16(out_indices[1]) << 8); + output_data &= ~(new_material_index_mask << out_bit_pos); + output_data |= material_index << out_bit_pos; + out_indices[0] = uint8(output_data); + out_indices[1] = uint8(output_data >> 8); + + // Go to the previous index + in_bit_pos -= int(mNumBitsPerMaterialIndex); + in_indices += in_bit_pos >> 3; + in_bit_pos &= 0b111; + out_bit_pos -= int(new_bits_per_material_index); + out_indices += out_bit_pos >> 3; + out_bit_pos &= 0b111; + } + + // Accept the new bits per material index + mNumBitsPerMaterialIndex = new_bits_per_material_index; + } + + uint16 material_index_mask = uint16((1 << mNumBitsPerMaterialIndex) - 1); + for (uint y = 0; y < inSizeY; ++y) + { + // Calculate input position + const uint8 *in_indices = inMaterials + y * inMaterialsStride; + + // Calculate output position + uint bit_pos = (inX + (inY + y) * count_min_1) * mNumBitsPerMaterialIndex; + uint8 *out_indices = mMaterialIndices.data() + (bit_pos >> 3); + bit_pos &= 0b111; + + for (uint x = 0; x < inSizeX; ++x) + { + // Update material + uint16 output_data = uint16(out_indices[0]) + uint16(uint16(out_indices[1]) << 8); + output_data &= ~(material_index_mask << bit_pos); + output_data |= material_remap_table[*in_indices] << bit_pos; + out_indices[0] = uint8(output_data); + out_indices[1] = uint8(output_data >> 8); + + // Go to the next index + in_indices++; + bit_pos += mNumBitsPerMaterialIndex; + out_indices += bit_pos >> 3; + bit_pos &= 0b111; + } + } + + return true; +} + +MassProperties HeightFieldShape::GetMassProperties() const +{ + // Object should always be static, return default mass properties + return MassProperties(); +} + +const PhysicsMaterial *HeightFieldShape::GetMaterial(uint inX, uint inY) const +{ + if (mMaterials.empty()) + return PhysicsMaterial::sDefault; + if (mMaterials.size() == 1) + return mMaterials[0]; + + uint count_min_1 = mSampleCount - 1; + JPH_ASSERT(inX < count_min_1); + JPH_ASSERT(inY < count_min_1); + + // Calculate at which bit the material index starts + uint bit_pos = (inX + inY * count_min_1) * mNumBitsPerMaterialIndex; + uint byte_pos = bit_pos >> 3; + bit_pos &= 0b111; + + // Read the material index + JPH_ASSERT(byte_pos + 1 < mMaterialIndices.size()); + const uint8 *material_indices = mMaterialIndices.data() + byte_pos; + uint16 material_index = uint16(material_indices[0]) + uint16(uint16(material_indices[1]) << 8); + material_index >>= bit_pos; + material_index &= (1 << mNumBitsPerMaterialIndex) - 1; + + // Return the material + return mMaterials[material_index]; +} + +uint HeightFieldShape::GetSubShapeIDBits() const +{ + // Need to store X, Y and 1 extra bit to specify the triangle number in the quad + return 2 * (32 - CountLeadingZeros(mSampleCount - 1)) + 1; +} + +SubShapeID HeightFieldShape::EncodeSubShapeID(const SubShapeIDCreator &inCreator, uint inX, uint inY, uint inTriangle) const +{ + return inCreator.PushID((inX + inY * mSampleCount) * 2 + inTriangle, GetSubShapeIDBits()).GetID(); +} + +void HeightFieldShape::DecodeSubShapeID(const SubShapeID &inSubShapeID, uint &outX, uint &outY, uint &outTriangle) const +{ + // Decode sub shape id + SubShapeID remainder; + uint32 id = inSubShapeID.PopID(GetSubShapeIDBits(), remainder); + JPH_ASSERT(remainder.IsEmpty(), "Invalid subshape ID"); + + // Get triangle index + outTriangle = id & 1; + id >>= 1; + + // Fetch the x and y coordinate + outX = id % mSampleCount; + outY = id / mSampleCount; +} + +void HeightFieldShape::GetSubShapeCoordinates(const SubShapeID &inSubShapeID, uint &outX, uint &outY, uint &outTriangleIndex) const +{ + DecodeSubShapeID(inSubShapeID, outX, outY, outTriangleIndex); +} + +const PhysicsMaterial *HeightFieldShape::GetMaterial(const SubShapeID &inSubShapeID) const +{ + // Decode ID + uint x, y, triangle; + DecodeSubShapeID(inSubShapeID, x, y, triangle); + + // Fetch the material + return GetMaterial(x, y); +} + +Vec3 HeightFieldShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + // Decode ID + uint x, y, triangle; + DecodeSubShapeID(inSubShapeID, x, y, triangle); + + // Fetch vertices that both triangles share + Vec3 x1y1 = GetPosition(x, y); + Vec3 x2y2 = GetPosition(x + 1, y + 1); + + // Get normal depending on which triangle was selected + Vec3 normal; + if (triangle == 0) + { + Vec3 x1y2 = GetPosition(x, y + 1); + normal = (x2y2 - x1y2).Cross(x1y1 - x1y2); + } + else + { + Vec3 x2y1 = GetPosition(x + 1, y); + normal = (x1y1 - x2y1).Cross(x2y2 - x2y1); + } + + return normal.Normalized(); +} + +void HeightFieldShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + // Decode ID + uint x, y, triangle; + DecodeSubShapeID(inSubShapeID, x, y, triangle); + + // Fetch the triangle + outVertices.resize(3); + outVertices[0] = GetPosition(x, y); + Vec3 v2 = GetPosition(x + 1, y + 1); + if (triangle == 0) + { + outVertices[1] = GetPosition(x, y + 1); + outVertices[2] = v2; + } + else + { + outVertices[1] = v2; + outVertices[2] = GetPosition(x + 1, y); + } + + // Flip triangle if scaled inside out + if (ScaleHelpers::IsInsideOut(inScale)) + std::swap(outVertices[1], outVertices[2]); + + // Transform to world space + Mat44 transform = inCenterOfMassTransform.PreScaled(inScale); + for (Vec3 &v : outVertices) + v = transform * v; +} + +inline uint8 HeightFieldShape::GetEdgeFlags(uint inX, uint inY, uint inTriangle) const +{ + JPH_ASSERT(inX < mSampleCount - 1 && inY < mSampleCount - 1); + + if (inTriangle == 0) + { + // The edge flags for this triangle are directly stored, find the right 3 bits + uint bit_pos = 3 * (inX + inY * (mSampleCount - 1)); + uint byte_pos = bit_pos >> 3; + bit_pos &= 0b111; + JPH_ASSERT(byte_pos + 1 < mActiveEdgesSize); + const uint8 *active_edges = mActiveEdges + byte_pos; + uint16 edge_flags = uint16(active_edges[0]) + uint16(uint16(active_edges[1]) << 8); + return uint8(edge_flags >> bit_pos) & 0b111; + } + else + { + // We don't store this triangle directly, we need to look at our three neighbours to construct the edge flags + uint8 edge0 = (GetEdgeFlags(inX, inY, 0) & 0b100) != 0? 0b001 : 0; // Diagonal edge + uint8 edge1 = inX == mSampleCount - 2 || (GetEdgeFlags(inX + 1, inY, 0) & 0b001) != 0? 0b010 : 0; // Vertical edge + uint8 edge2 = inY == 0 || (GetEdgeFlags(inX, inY - 1, 0) & 0b010) != 0? 0b100 : 0; // Horizontal edge + return edge0 | edge1 | edge2; + } +} + +AABox HeightFieldShape::GetLocalBounds() const +{ + if (mMinSample == cNoCollisionValue16) + { + // This whole height field shape doesn't have any collision, return the center point + Vec3 center = mOffset + 0.5f * mScale * Vec3(float(mSampleCount - 1), 0.0f, float(mSampleCount - 1)); + return AABox(center, center); + } + else + { + // Bounding box based on min and max sample height + Vec3 bmin = mOffset + mScale * Vec3(0.0f, float(mMinSample), 0.0f); + Vec3 bmax = mOffset + mScale * Vec3(float(mSampleCount - 1), float(mMaxSample), float(mSampleCount - 1)); + return AABox(bmin, bmax); + } +} + +#ifdef JPH_DEBUG_RENDERER +void HeightFieldShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + // Don't draw anything if we don't have any collision + if (mHeightSamplesSize == 0) + return; + + // Reset the batch if we switch coloring mode + if (mCachedUseMaterialColors != inUseMaterialColors) + { + mGeometry.clear(); + mCachedUseMaterialColors = inUseMaterialColors; + } + + if (mGeometry.empty()) + { + // Divide terrain in triangle batches of max 64x64x2 triangles to allow better culling of the terrain + uint32 block_size = min(mSampleCount, 64); + for (uint32 by = 0; by < mSampleCount; by += block_size) + for (uint32 bx = 0; bx < mSampleCount; bx += block_size) + { + // Create vertices for a block + Array triangles; + triangles.resize(block_size * block_size * 2); + DebugRenderer::Triangle *out_tri = &triangles[0]; + for (uint32 y = by, max_y = min(by + block_size, mSampleCount - 1); y < max_y; ++y) + for (uint32 x = bx, max_x = min(bx + block_size, mSampleCount - 1); x < max_x; ++x) + if (!IsNoCollision(x, y) && !IsNoCollision(x + 1, y + 1)) + { + Vec3 x1y1 = GetPosition(x, y); + Vec3 x2y2 = GetPosition(x + 1, y + 1); + Color color = inUseMaterialColors? GetMaterial(x, y)->GetDebugColor() : Color::sWhite; + + if (!IsNoCollision(x, y + 1)) + { + Vec3 x1y2 = GetPosition(x, y + 1); + + x1y1.StoreFloat3(&out_tri->mV[0].mPosition); + x1y2.StoreFloat3(&out_tri->mV[1].mPosition); + x2y2.StoreFloat3(&out_tri->mV[2].mPosition); + + Vec3 normal = (x2y2 - x1y2).Cross(x1y1 - x1y2).Normalized(); + for (DebugRenderer::Vertex &v : out_tri->mV) + { + v.mColor = color; + v.mUV = Float2(0, 0); + normal.StoreFloat3(&v.mNormal); + } + + ++out_tri; + } + + if (!IsNoCollision(x + 1, y)) + { + Vec3 x2y1 = GetPosition(x + 1, y); + + x1y1.StoreFloat3(&out_tri->mV[0].mPosition); + x2y2.StoreFloat3(&out_tri->mV[1].mPosition); + x2y1.StoreFloat3(&out_tri->mV[2].mPosition); + + Vec3 normal = (x1y1 - x2y1).Cross(x2y2 - x2y1).Normalized(); + for (DebugRenderer::Vertex &v : out_tri->mV) + { + v.mColor = color; + v.mUV = Float2(0, 0); + normal.StoreFloat3(&v.mNormal); + } + + ++out_tri; + } + } + + // Resize triangles array to actual amount of triangles written + size_t num_triangles = out_tri - &triangles[0]; + triangles.resize(num_triangles); + + // Create batch + if (num_triangles > 0) + mGeometry.push_back(new DebugRenderer::Geometry(inRenderer->CreateTriangleBatch(triangles), DebugRenderer::sCalculateBounds(&triangles[0].mV[0], int(3 * num_triangles)))); + } + } + + // Get transform including scale + RMat44 transform = inCenterOfMassTransform.PreScaled(inScale); + + // Test if the shape is scaled inside out + DebugRenderer::ECullMode cull_mode = ScaleHelpers::IsInsideOut(inScale)? DebugRenderer::ECullMode::CullFrontFace : DebugRenderer::ECullMode::CullBackFace; + + // Determine the draw mode + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + + // Draw the geometry + for (const DebugRenderer::GeometryRef &b : mGeometry) + inRenderer->DrawGeometry(transform, inColor, b, cull_mode, DebugRenderer::ECastShadow::On, draw_mode); + + if (sDrawTriangleOutlines) + { + struct Visitor + { + JPH_INLINE explicit Visitor(const HeightFieldShape *inShape, DebugRenderer *inRenderer, RMat44Arg inTransform) : + mShape(inShape), + mRenderer(inRenderer), + mTransform(inTransform) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return false; + } + + JPH_INLINE bool ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + UVec4 valid = Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY); + return CountAndSortTrues(valid, ioProperties); + } + + JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) const + { + // Determine active edges + uint8 active_edges = mShape->GetEdgeFlags(inX, inY, inTriangle); + + // Loop through edges + Vec3 v[] = { inV0, inV1, inV2 }; + for (uint edge_idx = 0; edge_idx < 3; ++edge_idx) + { + RVec3 v1 = mTransform * v[edge_idx]; + RVec3 v2 = mTransform * v[(edge_idx + 1) % 3]; + + // Draw active edge as a green arrow, other edges as grey + if (active_edges & (1 << edge_idx)) + mRenderer->DrawArrow(v1, v2, Color::sGreen, 0.01f); + else + mRenderer->DrawLine(v1, v2, Color::sGrey); + } + } + + const HeightFieldShape *mShape; + DebugRenderer * mRenderer; + RMat44 mTransform; + }; + + Visitor visitor(this, inRenderer, inCenterOfMassTransform.PreScaled(inScale)); + WalkHeightField(visitor); + } +} +#endif // JPH_DEBUG_RENDERER + +class HeightFieldShape::DecodingContext +{ +public: + JPH_INLINE explicit DecodingContext(const HeightFieldShape *inShape) : + mShape(inShape) + { + static_assert(sizeof(sGridOffsets) / sizeof(uint) == cNumBitsXY + 1, "Offsets array is not long enough"); + + // Construct root stack entry + mPropertiesStack[0] = 0; // level: 0, x: 0, y: 0 + } + + template + JPH_INLINE void WalkHeightField(Visitor &ioVisitor) + { + // Early out if there's no collision + if (mShape->mHeightSamplesSize == 0) + return; + + // Assert that an inside-out bounding box does not collide + JPH_IF_ENABLE_ASSERTS(UVec4 dummy = UVec4::sReplicate(0);) + JPH_ASSERT(ioVisitor.VisitRangeBlock(Vec4::sReplicate(-1.0e6f), Vec4::sReplicate(1.0e6f), Vec4::sReplicate(-1.0e6f), Vec4::sReplicate(1.0e6f), Vec4::sReplicate(-1.0e6f), Vec4::sReplicate(1.0e6f), dummy, 0) == 0); + + // Precalculate values relating to sample count + uint32 sample_count = mShape->mSampleCount; + UVec4 sample_count_min_1 = UVec4::sReplicate(sample_count - 1); + + // Precalculate values relating to block size + uint32 block_size = mShape->mBlockSize; + uint32 block_size_plus_1 = block_size + 1; + uint num_blocks = mShape->GetNumBlocks(); + uint num_blocks_min_1 = num_blocks - 1; + uint max_level = HeightFieldShape::sGetMaxLevel(num_blocks); + uint32 max_stride = (num_blocks + 1) >> 1; + + // Precalculate range block offset and stride for GetBlockOffsetAndScale + uint range_block_offset, range_block_stride; + sGetRangeBlockOffsetAndStride(num_blocks, max_level, range_block_offset, range_block_stride); + + // Allocate space for vertices and 'no collision' flags + int array_size = Square(block_size_plus_1); + Vec3 *vertices = reinterpret_cast(JPH_STACK_ALLOC(array_size * sizeof(Vec3))); + bool *no_collision = reinterpret_cast(JPH_STACK_ALLOC(array_size * sizeof(bool))); + + // Splat offsets + Vec4 ox = mShape->mOffset.SplatX(); + Vec4 oy = mShape->mOffset.SplatY(); + Vec4 oz = mShape->mOffset.SplatZ(); + + // Splat scales + Vec4 sx = mShape->mScale.SplatX(); + Vec4 sy = mShape->mScale.SplatY(); + Vec4 sz = mShape->mScale.SplatZ(); + + do + { + // Decode properties + uint32 properties_top = mPropertiesStack[mTop]; + uint32 x = properties_top & cMaskBitsXY; + uint32 y = (properties_top >> cNumBitsXY) & cMaskBitsXY; + uint32 level = properties_top >> cLevelShift; + + if (level >= max_level) + { + // Determine actual range of samples (minus one because we eventually want to iterate over the triangles, not the samples) + uint32 min_x = x * block_size; + uint32 max_x = min_x + block_size; + uint32 min_y = y * block_size; + uint32 max_y = min_y + block_size; + + // Decompress vertices of block at (x, y) + Vec3 *dst_vertex = vertices; + bool *dst_no_collision = no_collision; + float block_offset, block_scale; + mShape->GetBlockOffsetAndScale(x, y, range_block_offset, range_block_stride, block_offset, block_scale); + for (uint32 v_y = min_y; v_y < max_y; ++v_y) + { + for (uint32 v_x = min_x; v_x < max_x; ++v_x) + { + *dst_vertex = mShape->GetPosition(v_x, v_y, block_offset, block_scale, *dst_no_collision); + ++dst_vertex; + ++dst_no_collision; + } + + // Skip last column, these values come from a different block + ++dst_vertex; + ++dst_no_collision; + } + + // Decompress block (x + 1, y) + uint32 max_x_decrement = 0; + if (x < num_blocks_min_1) + { + dst_vertex = vertices + block_size; + dst_no_collision = no_collision + block_size; + mShape->GetBlockOffsetAndScale(x + 1, y, range_block_offset, range_block_stride, block_offset, block_scale); + for (uint32 v_y = min_y; v_y < max_y; ++v_y) + { + *dst_vertex = mShape->GetPosition(max_x, v_y, block_offset, block_scale, *dst_no_collision); + dst_vertex += block_size_plus_1; + dst_no_collision += block_size_plus_1; + } + } + else + max_x_decrement = 1; // We don't have a next block, one less triangle to test + + // Decompress block (x, y + 1) + if (y < num_blocks_min_1) + { + uint start = block_size * block_size_plus_1; + dst_vertex = vertices + start; + dst_no_collision = no_collision + start; + mShape->GetBlockOffsetAndScale(x, y + 1, range_block_offset, range_block_stride, block_offset, block_scale); + for (uint32 v_x = min_x; v_x < max_x; ++v_x) + { + *dst_vertex = mShape->GetPosition(v_x, max_y, block_offset, block_scale, *dst_no_collision); + ++dst_vertex; + ++dst_no_collision; + } + + // Decompress single sample of block at (x + 1, y + 1) + if (x < num_blocks_min_1) + { + mShape->GetBlockOffsetAndScale(x + 1, y + 1, range_block_offset, range_block_stride, block_offset, block_scale); + *dst_vertex = mShape->GetPosition(max_x, max_y, block_offset, block_scale, *dst_no_collision); + } + } + else + --max_y; // We don't have a next block, one less triangle to test + + // Update max_x (we've been using it so we couldn't update it earlier) + max_x -= max_x_decrement; + + // We're going to divide the vertices in 4 blocks to do one more runtime sub-division, calculate the ranges of those blocks + struct Range + { + uint32 mMinX, mMinY, mNumTrianglesX, mNumTrianglesY; + }; + uint32 half_block_size = block_size >> 1; + uint32 block_size_x = max_x - min_x - half_block_size; + uint32 block_size_y = max_y - min_y - half_block_size; + Range ranges[] = + { + { 0, 0, half_block_size, half_block_size }, + { half_block_size, 0, block_size_x, half_block_size }, + { 0, half_block_size, half_block_size, block_size_y }, + { half_block_size, half_block_size, block_size_x, block_size_y }, + }; + + // Calculate the min and max of each of the blocks + Mat44 block_min, block_max; + for (int block = 0; block < 4; ++block) + { + // Get the range for this block + const Range &range = ranges[block]; + uint32 start = range.mMinX + range.mMinY * block_size_plus_1; + uint32 size_x_plus_1 = range.mNumTrianglesX + 1; + uint32 size_y_plus_1 = range.mNumTrianglesY + 1; + + // Calculate where to start reading + const Vec3 *src_vertex = vertices + start; + const bool *src_no_collision = no_collision + start; + uint32 stride = block_size_plus_1 - size_x_plus_1; + + // Start range with a very large inside-out box + Vec3 value_min = Vec3::sReplicate(1.0e30f); + Vec3 value_max = Vec3::sReplicate(-1.0e30f); + + // Loop over the samples to determine the min and max of this block + for (uint32 block_y = 0; block_y < size_y_plus_1; ++block_y) + { + for (uint32 block_x = 0; block_x < size_x_plus_1; ++block_x) + { + if (!*src_no_collision) + { + value_min = Vec3::sMin(value_min, *src_vertex); + value_max = Vec3::sMax(value_max, *src_vertex); + } + ++src_vertex; + ++src_no_collision; + } + src_vertex += stride; + src_no_collision += stride; + } + block_min.SetColumn4(block, Vec4(value_min)); + block_max.SetColumn4(block, Vec4(value_max)); + } + + #ifdef JPH_DEBUG_HEIGHT_FIELD + // Draw the bounding boxes of the sub-nodes + for (int block = 0; block < 4; ++block) + { + AABox bounds(block_min.GetColumn3(block), block_max.GetColumn3(block)); + if (bounds.IsValid()) + DebugRenderer::sInstance->DrawWireBox(bounds, Color::sYellow); + } + #endif // JPH_DEBUG_HEIGHT_FIELD + + // Transpose so we have the mins and maxes of each of the blocks in rows instead of columns + Mat44 transposed_min = block_min.Transposed(); + Mat44 transposed_max = block_max.Transposed(); + + // Check which blocks collide + // Note: At this point we don't use our own stack but we do allow the visitor to use its own stack + // to store collision distances so that we can still early out when no closer hits have been found. + UVec4 colliding_blocks(0, 1, 2, 3); + int num_results = ioVisitor.VisitRangeBlock(transposed_min.GetColumn4(0), transposed_min.GetColumn4(1), transposed_min.GetColumn4(2), transposed_max.GetColumn4(0), transposed_max.GetColumn4(1), transposed_max.GetColumn4(2), colliding_blocks, mTop); + + // Loop through the results backwards (closest first) + int result = num_results - 1; + while (result >= 0) + { + // Calculate the min and max of this block + uint32 block = colliding_blocks[result]; + const Range &range = ranges[block]; + uint32 block_min_x = min_x + range.mMinX; + uint32 block_max_x = block_min_x + range.mNumTrianglesX; + uint32 block_min_y = min_y + range.mMinY; + uint32 block_max_y = block_min_y + range.mNumTrianglesY; + + // Loop triangles + for (uint32 v_y = block_min_y; v_y < block_max_y; ++v_y) + for (uint32 v_x = block_min_x; v_x < block_max_x; ++v_x) + { + // Get first vertex + const int offset = (v_y - min_y) * block_size_plus_1 + (v_x - min_x); + const Vec3 *start_vertex = vertices + offset; + const bool *start_no_collision = no_collision + offset; + + // Check if vertices shared by both triangles have collision + if (!start_no_collision[0] && !start_no_collision[block_size_plus_1 + 1]) + { + // Loop 2 triangles + for (uint t = 0; t < 2; ++t) + { + // Determine triangle vertices + Vec3 v0, v1, v2; + if (t == 0) + { + // Check third vertex + if (start_no_collision[block_size_plus_1]) + continue; + + // Get vertices for triangle + v0 = start_vertex[0]; + v1 = start_vertex[block_size_plus_1]; + v2 = start_vertex[block_size_plus_1 + 1]; + } + else + { + // Check third vertex + if (start_no_collision[1]) + continue; + + // Get vertices for triangle + v0 = start_vertex[0]; + v1 = start_vertex[block_size_plus_1 + 1]; + v2 = start_vertex[1]; + } + + #ifdef JPH_DEBUG_HEIGHT_FIELD + DebugRenderer::sInstance->DrawWireTriangle(RVec3(v0), RVec3(v1), RVec3(v2), Color::sWhite); + #endif + + // Call visitor + ioVisitor.VisitTriangle(v_x, v_y, t, v0, v1, v2); + + // Check if we're done + if (ioVisitor.ShouldAbort()) + return; + } + } + } + + // Fetch next block until we find one that the visitor wants to see + do + --result; + while (result >= 0 && !ioVisitor.ShouldVisitRangeBlock(mTop + result)); + } + } + else + { + // Visit child grid + uint32 stride = min(1U << level, max_stride); // At the most detailed level we store a non-power of 2 number of blocks + uint32 offset = sGridOffsets[level] + stride * y + x; + + // Decode min/max height + JPH_ASSERT(offset < mShape->mRangeBlocksSize); + UVec4 block = UVec4::sLoadInt4Aligned(reinterpret_cast(&mShape->mRangeBlocks[offset])); + Vec4 bounds_miny = oy + sy * block.Expand4Uint16Lo().ToFloat(); + Vec4 bounds_maxy = oy + sy * block.Expand4Uint16Hi().ToFloat(); + + // Calculate size of one cell at this grid level + UVec4 internal_cell_size = UVec4::sReplicate(block_size << (max_level - level - 1)); // subtract 1 from level because we have an internal grid of 2x2 + + // Calculate min/max x and z + UVec4 two_x = UVec4::sReplicate(2 * x); // multiply by two because we have an internal grid of 2x2 + Vec4 bounds_minx = ox + sx * (internal_cell_size * (two_x + UVec4(0, 1, 0, 1))).ToFloat(); + Vec4 bounds_maxx = ox + sx * UVec4::sMin(internal_cell_size * (two_x + UVec4(1, 2, 1, 2)), sample_count_min_1).ToFloat(); + + UVec4 two_y = UVec4::sReplicate(2 * y); + Vec4 bounds_minz = oz + sz * (internal_cell_size * (two_y + UVec4(0, 0, 1, 1))).ToFloat(); + Vec4 bounds_maxz = oz + sz * UVec4::sMin(internal_cell_size * (two_y + UVec4(1, 1, 2, 2)), sample_count_min_1).ToFloat(); + + // Calculate properties of child blocks + UVec4 properties = UVec4::sReplicate(((level + 1) << cLevelShift) + (y << (cNumBitsXY + 1)) + (x << 1)) + UVec4(0, 1, 1 << cNumBitsXY, (1 << cNumBitsXY) + 1); + + #ifdef JPH_DEBUG_HEIGHT_FIELD + // Draw boxes + for (int i = 0; i < 4; ++i) + { + AABox b(Vec3(bounds_minx[i], bounds_miny[i], bounds_minz[i]), Vec3(bounds_maxx[i], bounds_maxy[i], bounds_maxz[i])); + if (b.IsValid()) + DebugRenderer::sInstance->DrawWireBox(b, Color::sGreen); + } + #endif + + // Check which sub nodes to visit + int num_results = ioVisitor.VisitRangeBlock(bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz, properties, mTop); + + // Push them onto the stack + JPH_ASSERT(mTop + 4 < cStackSize); + properties.StoreInt4(&mPropertiesStack[mTop]); + mTop += num_results; + } + + // Check if we're done + if (ioVisitor.ShouldAbort()) + return; + + // Fetch next node until we find one that the visitor wants to see + do + --mTop; + while (mTop >= 0 && !ioVisitor.ShouldVisitRangeBlock(mTop)); + } + while (mTop >= 0); + } + + // This can be used to have the visitor early out (ioVisitor.ShouldAbort() returns true) and later continue again (call WalkHeightField() again) + JPH_INLINE bool IsDoneWalking() const + { + return mTop < 0; + } + +private: + const HeightFieldShape * mShape; + int mTop = 0; + uint32 mPropertiesStack[cStackSize]; +}; + +template +void HeightFieldShape::WalkHeightField(Visitor &ioVisitor) const +{ + DecodingContext ctx(this); + ctx.WalkHeightField(ioVisitor); +} + +bool HeightFieldShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor + { + JPH_INLINE explicit Visitor(const HeightFieldShape *inShape, const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) : + mHit(ioHit), + mRayOrigin(inRay.mOrigin), + mRayDirection(inRay.mDirection), + mRayInvDirection(inRay.mDirection), + mShape(inShape), + mSubShapeIDCreator(inSubShapeIDCreator) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return mHit.mFraction <= 0.0f; + } + + JPH_INLINE bool ShouldVisitRangeBlock(int inStackTop) const + { + return mDistanceStack[inStackTop] < mHit.mFraction; + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test bounds of 4 children + Vec4 distance = RayAABox4(mRayOrigin, mRayInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mHit.mFraction, ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + float fraction = RayTriangle(mRayOrigin, mRayDirection, inV0, inV1, inV2); + if (fraction < mHit.mFraction) + { + // It's a closer hit + mHit.mFraction = fraction; + mHit.mSubShapeID2 = mShape->EncodeSubShapeID(mSubShapeIDCreator, inX, inY, inTriangle); + mReturnValue = true; + } + } + + RayCastResult & mHit; + Vec3 mRayOrigin; + Vec3 mRayDirection; + RayInvDirection mRayInvDirection; + const HeightFieldShape *mShape; + SubShapeIDCreator mSubShapeIDCreator; + bool mReturnValue = false; + float mDistanceStack[cStackSize]; + }; + + Visitor visitor(this, inRay, inSubShapeIDCreator, ioHit); + WalkHeightField(visitor); + + return visitor.mReturnValue; +} + +void HeightFieldShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + struct Visitor + { + JPH_INLINE explicit Visitor(const HeightFieldShape *inShape, const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector) : + mCollector(ioCollector), + mRayOrigin(inRay.mOrigin), + mRayDirection(inRay.mDirection), + mRayInvDirection(inRay.mDirection), + mBackFaceMode(inRayCastSettings.mBackFaceModeTriangles), + mShape(inShape), + mSubShapeIDCreator(inSubShapeIDCreator) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitRangeBlock(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetEarlyOutFraction(); + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test bounds of 4 children + Vec4 distance = RayAABox4(mRayOrigin, mRayInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) const + { + // Back facing check + if (mBackFaceMode == EBackFaceMode::IgnoreBackFaces && (inV2 - inV0).Cross(inV1 - inV0).Dot(mRayDirection) < 0) + return; + + // Check the triangle + float fraction = RayTriangle(mRayOrigin, mRayDirection, inV0, inV1, inV2); + if (fraction < mCollector.GetEarlyOutFraction()) + { + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(mCollector.GetContext()); + hit.mFraction = fraction; + hit.mSubShapeID2 = mShape->EncodeSubShapeID(mSubShapeIDCreator, inX, inY, inTriangle); + mCollector.AddHit(hit); + } + } + + CastRayCollector & mCollector; + Vec3 mRayOrigin; + Vec3 mRayDirection; + RayInvDirection mRayInvDirection; + EBackFaceMode mBackFaceMode; + const HeightFieldShape *mShape; + SubShapeIDCreator mSubShapeIDCreator; + float mDistanceStack[cStackSize]; + }; + + Visitor visitor(this, inRay, inRayCastSettings, inSubShapeIDCreator, ioCollector); + WalkHeightField(visitor); +} + +void HeightFieldShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // A height field doesn't have volume, so we can't test insideness +} + +void HeightFieldShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CollideSoftBodyVerticesVsTriangles + { + using CollideSoftBodyVerticesVsTriangles::CollideSoftBodyVerticesVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return false; + } + + JPH_INLINE bool ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const + { + return mDistanceStack[inStackTop] < mClosestDistanceSq; + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Get distance to vertex + Vec4 dist_sq = AABox4DistanceSqToPoint(mLocalPosition, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Clear distance for invalid bounds + dist_sq = Vec4::sSelect(Vec4::sReplicate(FLT_MAX), dist_sq, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY)); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(dist_sq, mClosestDistanceSq, ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle([[maybe_unused]] uint inX, [[maybe_unused]] uint inY, [[maybe_unused]] uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + ProcessTriangle(inV0, inV1, inV2); + } + + float mDistanceStack[cStackSize]; + }; + + Visitor visitor(inCenterOfMassTransform, inScale); + + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) + { + visitor.StartVertex(v); + WalkHeightField(visitor); + visitor.FinishVertex(v, inCollidingShapeIndex); + } +} + +void HeightFieldShape::sCastConvexVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastConvexVsTriangles + { + using CastConvexVsTriangles::CastConvexVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitRangeBlock(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction(); + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Enlarge them by the casted shape's box extents + AABox4EnlargeWithExtent(mBoxExtent, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test bounds of 4 children + Vec4 distance = RayAABox4(mBoxCenter, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Clear distance for invalid bounds + distance = Vec4::sSelect(Vec4::sReplicate(FLT_MAX), distance, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY)); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + // Create sub shape id for this part + SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle); + + // Determine active edges + uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle); + + Cast(inV0, inV1, inV2, active_edges, triangle_sub_shape_id); + } + + const HeightFieldShape * mShape2; + RayInvDirection mInvDirection; + Vec3 mBoxCenter; + Vec3 mBoxExtent; + SubShapeIDCreator mSubShapeIDCreator2; + float mDistanceStack[cStackSize]; + }; + + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::HeightField); + const HeightFieldShape *shape = static_cast(inShape); + + Visitor visitor(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + visitor.mShape2 = shape; + visitor.mInvDirection.Set(inShapeCast.mDirection); + visitor.mBoxCenter = inShapeCast.mShapeWorldBounds.GetCenter(); + visitor.mBoxExtent = inShapeCast.mShapeWorldBounds.GetExtent(); + visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2; + shape->WalkHeightField(visitor); +} + +void HeightFieldShape::sCastSphereVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastSphereVsTriangles + { + using CastSphereVsTriangles::CastSphereVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitRangeBlock(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction(); + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Enlarge them by the radius of the sphere + AABox4EnlargeWithExtent(Vec3::sReplicate(mRadius), bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test bounds of 4 children + Vec4 distance = RayAABox4(mStart, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Clear distance for invalid bounds + distance = Vec4::sSelect(Vec4::sReplicate(FLT_MAX), distance, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY)); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + // Create sub shape id for this part + SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle); + + // Determine active edges + uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle); + + Cast(inV0, inV1, inV2, active_edges, triangle_sub_shape_id); + } + + const HeightFieldShape * mShape2; + RayInvDirection mInvDirection; + SubShapeIDCreator mSubShapeIDCreator2; + float mDistanceStack[cStackSize]; + }; + + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::HeightField); + const HeightFieldShape *shape = static_cast(inShape); + + Visitor visitor(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + visitor.mShape2 = shape; + visitor.mInvDirection.Set(inShapeCast.mDirection); + visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2; + shape->WalkHeightField(visitor); +} + +struct HeightFieldShape::HSGetTrianglesContext +{ + HSGetTrianglesContext(const HeightFieldShape *inShape, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) : + mDecodeCtx(inShape), + mShape(inShape), + mLocalBox(Mat44::sInverseRotationTranslation(inRotation, inPositionCOM), inBox), + mHeightFieldScale(inScale), + mLocalToWorld(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale)), + mIsInsideOut(ScaleHelpers::IsInsideOut(inScale)) + { + } + + bool ShouldAbort() const + { + return mShouldAbort; + } + + bool ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const + { + return true; + } + + int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mHeightFieldScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which nodes collide + UVec4 collides = AABox4VsBox(mLocalBox, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Filter out invalid bounding boxes + collides = UVec4::sAnd(collides, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY)); + + return CountAndSortTrues(collides, ioProperties); + } + + void VisitTriangle(uint inX, uint inY, [[maybe_unused]] uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + // When the buffer is full and we cannot process the triangles, abort the height field walk. The next time GetTrianglesNext is called we will continue here. + if (mNumTrianglesFound + 1 > mMaxTrianglesRequested) + { + mShouldAbort = true; + return; + } + + // Store vertices as Float3 + if (mIsInsideOut) + { + // Reverse vertices + (mLocalToWorld * inV0).StoreFloat3(mTriangleVertices++); + (mLocalToWorld * inV2).StoreFloat3(mTriangleVertices++); + (mLocalToWorld * inV1).StoreFloat3(mTriangleVertices++); + } + else + { + // Normal scale + (mLocalToWorld * inV0).StoreFloat3(mTriangleVertices++); + (mLocalToWorld * inV1).StoreFloat3(mTriangleVertices++); + (mLocalToWorld * inV2).StoreFloat3(mTriangleVertices++); + } + + // Decode material + if (mMaterials != nullptr) + *mMaterials++ = mShape->GetMaterial(inX, inY); + + // Accumulate triangles found + mNumTrianglesFound++; + } + + DecodingContext mDecodeCtx; + const HeightFieldShape * mShape; + OrientedBox mLocalBox; + Vec3 mHeightFieldScale; + Mat44 mLocalToWorld; + int mMaxTrianglesRequested; + Float3 * mTriangleVertices; + int mNumTrianglesFound; + const PhysicsMaterial ** mMaterials; + bool mShouldAbort; + bool mIsInsideOut; +}; + +void HeightFieldShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + static_assert(sizeof(HSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(&ioContext, alignof(HSGetTrianglesContext))); + + new (&ioContext) HSGetTrianglesContext(this, inBox, inPositionCOM, inRotation, inScale); +} + +int HeightFieldShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + static_assert(cGetTrianglesMinTrianglesRequested >= 1, "cGetTrianglesMinTrianglesRequested is too small"); + JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested); + + // Check if we're done + HSGetTrianglesContext &context = (HSGetTrianglesContext &)ioContext; + if (context.mDecodeCtx.IsDoneWalking()) + return 0; + + // Store parameters on context + context.mMaxTrianglesRequested = inMaxTrianglesRequested; + context.mTriangleVertices = outTriangleVertices; + context.mMaterials = outMaterials; + context.mShouldAbort = false; // Reset the abort flag + context.mNumTrianglesFound = 0; + + // Continue (or start) walking the height field + context.mDecodeCtx.WalkHeightField(context); + return context.mNumTrianglesFound; +} + +void HeightFieldShape::sCollideConvexVsHeightField(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, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + // Get the shapes + JPH_ASSERT(inShape1->GetType() == EShapeType::Convex); + JPH_ASSERT(inShape2->GetType() == EShapeType::HeightField); + const ConvexShape *shape1 = static_cast(inShape1); + const HeightFieldShape *shape2 = static_cast(inShape2); + + struct Visitor : public CollideConvexVsTriangles + { + using CollideConvexVsTriangles::CollideConvexVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which nodes collide + UVec4 collides = AABox4VsBox(mBoundsOf1InSpaceOf2, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Filter out invalid bounding boxes + collides = UVec4::sAnd(collides, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY)); + + return CountAndSortTrues(collides, ioProperties); + } + + JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + // Create ID for triangle + SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle); + + // Determine active edges + uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle); + + Collide(inV0, inV1, inV2, active_edges, triangle_sub_shape_id); + } + + const HeightFieldShape * mShape2; + SubShapeIDCreator mSubShapeIDCreator2; + }; + + Visitor visitor(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + visitor.mShape2 = shape2; + visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2; + shape2->WalkHeightField(visitor); +} + +void HeightFieldShape::sCollideSphereVsHeightField(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, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + // Get the shapes + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Sphere); + JPH_ASSERT(inShape2->GetType() == EShapeType::HeightField); + const SphereShape *shape1 = static_cast(inShape1); + const HeightFieldShape *shape2 = static_cast(inShape2); + + struct Visitor : public CollideSphereVsTriangles + { + using CollideSphereVsTriangles::CollideSphereVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which nodes collide + UVec4 collides = AABox4VsSphere(mSphereCenterIn2, mRadiusPlusMaxSeparationSq, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Filter out invalid bounding boxes + collides = UVec4::sAnd(collides, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY)); + + return CountAndSortTrues(collides, ioProperties); + } + + JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + // Create ID for triangle + SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle); + + // Determine active edges + uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle); + + Collide(inV0, inV1, inV2, active_edges, triangle_sub_shape_id); + } + + const HeightFieldShape * mShape2; + SubShapeIDCreator mSubShapeIDCreator2; + }; + + Visitor visitor(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + visitor.mShape2 = shape2; + visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2; + shape2->WalkHeightField(visitor); +} + +void HeightFieldShape::SaveBinaryState(StreamOut &inStream) const +{ + Shape::SaveBinaryState(inStream); + + inStream.Write(mOffset); + inStream.Write(mScale); + inStream.Write(mSampleCount); + inStream.Write(mBlockSize); + inStream.Write(mBitsPerSample); + inStream.Write(mMinSample); + inStream.Write(mMaxSample); + inStream.Write(mMaterialIndices); + inStream.Write(mNumBitsPerMaterialIndex); + + if (mRangeBlocks != nullptr) + { + inStream.Write(true); + inStream.WriteBytes(mRangeBlocks, mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize); + } + else + { + inStream.Write(false); + } +} + +void HeightFieldShape::RestoreBinaryState(StreamIn &inStream) +{ + Shape::RestoreBinaryState(inStream); + + inStream.Read(mOffset); + inStream.Read(mScale); + inStream.Read(mSampleCount); + inStream.Read(mBlockSize); + inStream.Read(mBitsPerSample); + inStream.Read(mMinSample); + inStream.Read(mMaxSample); + inStream.Read(mMaterialIndices); + inStream.Read(mNumBitsPerMaterialIndex); + + // We don't have the exact number of reserved materials anymore, but ensure that our array is big enough + // TODO: Next time when we bump the binary serialization format of this class we should store the capacity and allocate the right amount, for now we accept a little bit of waste + mMaterials.reserve(PhysicsMaterialList::size_type(1) << mNumBitsPerMaterialIndex); + + CacheValues(); + + bool has_heights = false; + inStream.Read(has_heights); + if (has_heights) + { + AllocateBuffers(); + inStream.ReadBytes(mRangeBlocks, mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize); + } +} + +void HeightFieldShape::SaveMaterialState(PhysicsMaterialList &outMaterials) const +{ + outMaterials = mMaterials; +} + +void HeightFieldShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) +{ + mMaterials.assign(inMaterials, inMaterials + inNumMaterials); +} + +Shape::Stats HeightFieldShape::GetStats() const +{ + return Stats( + sizeof(*this) + + mMaterials.size() * sizeof(Ref) + + mRangeBlocksSize * sizeof(RangeBlock) + + mHeightSamplesSize * sizeof(uint8) + + mActiveEdgesSize * sizeof(uint8) + + mMaterialIndices.size() * sizeof(uint8), + mHeightSamplesSize == 0? 0 : Square(mSampleCount - 1) * 2); +} + +void HeightFieldShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::HeightField); + f.mConstruct = []() -> Shape * { return new HeightFieldShape; }; + f.mColor = Color::sPurple; + + for (EShapeSubType s : sConvexSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::HeightField, sCollideConvexVsHeightField); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::HeightField, sCastConvexVsHeightField); + + CollisionDispatch::sRegisterCastShape(EShapeSubType::HeightField, s, CollisionDispatch::sReversedCastShape); + CollisionDispatch::sRegisterCollideShape(EShapeSubType::HeightField, s, CollisionDispatch::sReversedCollideShape); + } + + // Specialized collision functions + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Sphere, EShapeSubType::HeightField, sCollideSphereVsHeightField); + CollisionDispatch::sRegisterCastShape(EShapeSubType::Sphere, EShapeSubType::HeightField, sCastSphereVsHeightField); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.h new file mode 100644 index 000000000000..16cbb76367bd --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.h @@ -0,0 +1,380 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +class ConvexShape; +class CollideShapeSettings; +class TempAllocator; + +/// Constants for HeightFieldShape, this was moved out of the HeightFieldShape because of a linker bug +namespace HeightFieldShapeConstants +{ + /// Value used to create gaps in the height field + constexpr float cNoCollisionValue = FLT_MAX; + + /// Stack size to use during WalkHeightField + constexpr int cStackSize = 128; + + /// A position in the hierarchical grid is defined by a level (which grid), x and y position. We encode this in a single uint32 as: level << 28 | y << 14 | x + constexpr uint cNumBitsXY = 14; + constexpr uint cMaskBitsXY = (1 << cNumBitsXY) - 1; + constexpr uint cLevelShift = 2 * cNumBitsXY; + + /// When height samples are converted to 16 bit: + constexpr uint16 cNoCollisionValue16 = 0xffff; ///< This is the magic value for 'no collision' + constexpr uint16 cMaxHeightValue16 = 0xfffe; ///< This is the maximum allowed height value +}; + +/// Class that constructs a HeightFieldShape +class JPH_EXPORT HeightFieldShapeSettings final : public ShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, HeightFieldShapeSettings) + +public: + /// Default constructor for deserialization + HeightFieldShapeSettings() = default; + + /// Create a height field shape of inSampleCount * inSampleCount vertices. + /// The height field is a surface defined by: inOffset + inScale * (x, inSamples[y * inSampleCount + x], y). + /// where x and y are integers in the range x and y e [0, inSampleCount - 1]. + /// inSampleCount: inSampleCount / mBlockSize must be minimally 2 and a power of 2 is the most efficient in terms of performance and storage. + /// inSamples: inSampleCount^2 vertices. + /// inMaterialIndices: (inSampleCount - 1)^2 indices that index into inMaterialList. + HeightFieldShapeSettings(const float *inSamples, Vec3Arg inOffset, Vec3Arg inScale, uint32 inSampleCount, const uint8 *inMaterialIndices = nullptr, const PhysicsMaterialList &inMaterialList = PhysicsMaterialList()); + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + /// Determine the minimal and maximal value of mHeightSamples (will ignore cNoCollisionValue) + /// @param outMinValue The minimal value of mHeightSamples or FLT_MAX if no samples have collision + /// @param outMaxValue The maximal value of mHeightSamples or -FLT_MAX if no samples have collision + /// @param outQuantizationScale (value - outMinValue) * outQuantizationScale quantizes a height sample to 16 bits + void DetermineMinAndMaxSample(float &outMinValue, float &outMaxValue, float &outQuantizationScale) const; + + /// Given mBlockSize, mSampleCount and mHeightSamples, calculate the amount of bits needed to stay below absolute error inMaxError + /// @param inMaxError Maximum allowed error in mHeightSamples after compression (note that this does not take mScale.Y into account) + /// @return Needed bits per sample in the range [1, 8]. + uint32 CalculateBitsPerSampleForError(float inMaxError) const; + + /// The height field is a surface defined by: mOffset + mScale * (x, mHeightSamples[y * mSampleCount + x], y). + /// where x and y are integers in the range x and y e [0, mSampleCount - 1]. + Vec3 mOffset = Vec3::sZero(); + Vec3 mScale = Vec3::sReplicate(1.0f); + uint32 mSampleCount = 0; + + /// Artificial minimal value of mHeightSamples, used for compression and can be used to update the terrain after creating with lower height values. If there are any lower values in mHeightSamples, this value will be ignored. + float mMinHeightValue = FLT_MAX; + + /// Artificial maximum value of mHeightSamples, used for compression and can be used to update the terrain after creating with higher height values. If there are any higher values in mHeightSamples, this value will be ignored. + float mMaxHeightValue = -FLT_MAX; + + /// When bigger than mMaterials.size() the internal material list will be preallocated to support this number of materials. + /// This avoids reallocations when calling HeightFieldShape::SetMaterials with new materials later. + uint32 mMaterialsCapacity = 0; + + /// The heightfield is divided in blocks of mBlockSize * mBlockSize * 2 triangles and the acceleration structure culls blocks only, + /// bigger block sizes reduce memory consumption but also reduce query performance. Sensible values are [2, 8], does not need to be + /// a power of 2. Note that at run-time we'll perform one more grid subdivision, so the effective block size is half of what is provided here. + uint32 mBlockSize = 2; + + /// How many bits per sample to use to compress the height field. Can be in the range [1, 8]. + /// Note that each sample is compressed relative to the min/max value of its block of mBlockSize * mBlockSize pixels so the effective precision is higher. + /// Also note that increasing mBlockSize saves more memory than reducing the amount of bits per sample. + uint32 mBitsPerSample = 8; + + /// An array of mSampleCount^2 height samples. Samples are stored in row major order, so the sample at (x, y) is at index y * mSampleCount + x. + Array mHeightSamples; + + /// An array of (mSampleCount - 1)^2 material indices. + Array mMaterialIndices; + + /// The materials of square at (x, y) is: mMaterials[mMaterialIndices[x + y * (mSampleCount - 1)]] + PhysicsMaterialList mMaterials; + + /// Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive). + /// Setting this value too small can cause ghost collisions with edges, setting it too big can cause depenetration artifacts (objects not depenetrating quickly). + /// Valid ranges are between cos(0 degrees) and cos(90 degrees). The default value is cos(5 degrees). + float mActiveEdgeCosThresholdAngle = 0.996195f; // cos(5 degrees) +}; + +/// A height field shape. Cannot be used as a dynamic object. +/// +/// Note: If you're using HeightFieldShape and are querying data while modifying the shape you'll have a race condition. +/// In this case it is best to create a new HeightFieldShape using the Clone function. You replace the shape on a body using BodyInterface::SetShape. +/// If a query is still working on the old shape, it will have taken a reference and keep the old shape alive until the query finishes. +class JPH_EXPORT HeightFieldShape final : public Shape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + HeightFieldShape() : Shape(EShapeType::HeightField, EShapeSubType::HeightField) { } + HeightFieldShape(const HeightFieldShapeSettings &inSettings, ShapeResult &outResult); + virtual ~HeightFieldShape() override; + + /// Clone this shape. Can be used to avoid race conditions. See the documentation of this class for more information. + Ref Clone() const; + + // See Shape::MustBeStatic + virtual bool MustBeStatic() const override { return true; } + + /// Get the size of the height field. Note that this will always be rounded up to the nearest multiple of GetBlockSize(). + inline uint GetSampleCount() const { return mSampleCount; } + + /// Get the size of a block + inline uint GetBlockSize() const { return mBlockSize; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetSubShapeIDBitsRecursive + virtual uint GetSubShapeIDBitsRecursive() const override { return GetSubShapeIDBits(); } + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return 0.0f; } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetMaterial + virtual const PhysicsMaterial * GetMaterial(const SubShapeID &inSubShapeID) const override; + + /// Overload to get the material at a particular location + const PhysicsMaterial * GetMaterial(uint inX, uint inY) const; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override { JPH_ASSERT(false, "Not supported"); } + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + /// Get height field position at sampled location (inX, inY). + /// where inX and inY are integers in the range inX e [0, mSampleCount - 1] and inY e [0, mSampleCount - 1]. + Vec3 GetPosition(uint inX, uint inY) const; + + /// Check if height field at sampled location (inX, inY) has collision (has a hole or not) + bool IsNoCollision(uint inX, uint inY) const; + + /// Projects inLocalPosition (a point in the space of the shape) along the Y axis onto the surface and returns it in outSurfacePosition. + /// When there is no surface position (because of a hole or because the point is outside the heightfield) the function will return false. + bool ProjectOntoSurface(Vec3Arg inLocalPosition, Vec3 &outSurfacePosition, SubShapeID &outSubShapeID) const; + + /// Returns the coordinates of the triangle that a sub shape ID represents + /// @param inSubShapeID The sub shape ID to decode + /// @param outX X coordinate of the triangle (in the range [0, mSampleCount - 2]) + /// @param outY Y coordinate of the triangle (in the range [0, mSampleCount - 2]) + /// @param outTriangleIndex Triangle within the quad (0 = lower triangle or 1 = upper triangle) + void GetSubShapeCoordinates(const SubShapeID &inSubShapeID, uint &outX, uint &outY, uint &outTriangleIndex) const; + + /// Get the range of height values that this height field can encode. Can be used to determine the allowed range when setting the height values with SetHeights. + float GetMinHeightValue() const { return mOffset.GetY(); } + float GetMaxHeightValue() const { return mOffset.GetY() + mScale.GetY() * HeightFieldShapeConstants::cMaxHeightValue16; } + + /// Get the height values of a block of data. + /// Note that the height values are decompressed so will be slightly different from what the shape was originally created with. + /// @param inX Start X position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1] + /// @param inY Start Y position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1] + /// @param inSizeX Number of samples in X direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inX] + /// @param inSizeY Number of samples in Y direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inY] + /// @param outHeights Returned height values, must be at least inSizeX * inSizeY floats. Values are returned in x-major order and can be cNoCollisionValue. + /// @param inHeightsStride Stride in floats between two consecutive rows of outHeights (can be negative if the data is upside down). + void GetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, float *outHeights, intptr_t inHeightsStride) const; + + /// Set the height values of a block of data. + /// Note that this requires decompressing and recompressing a border of size mBlockSize in the negative x/y direction so will cause some precision loss. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + /// @param inX Start X position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1] + /// @param inY Start Y position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1] + /// @param inSizeX Number of samples in X direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inX] + /// @param inSizeY Number of samples in Y direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inY] + /// @param inHeights The new height values to set, must be an array of inSizeX * inSizeY floats, can be cNoCollisionValue. Values outside of the range [GetMinHeightValue(), GetMaxHeightValue()] will be clamped. + /// @param inHeightsStride Stride in floats between two consecutive rows of inHeights (can be negative if the data is upside down). + /// @param inAllocator Allocator to use for temporary memory + /// @param inActiveEdgeCosThresholdAngle Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive). + void SetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, intptr_t inHeightsStride, TempAllocator &inAllocator, float inActiveEdgeCosThresholdAngle = 0.996195f); + + /// Get the current list of materials, the indices returned by GetMaterials() will index into this list. + const PhysicsMaterialList & GetMaterialList() const { return mMaterials; } + + /// Get the material indices of a block of data. + /// @param inX Start X position, must in the range [0, mSampleCount - 1] + /// @param inY Start Y position, must in the range [0, mSampleCount - 1] + /// @param inSizeX Number of samples in X direction + /// @param inSizeY Number of samples in Y direction + /// @param outMaterials Returned material indices, must be at least inSizeX * inSizeY uint8s. Values are returned in x-major order. + /// @param inMaterialsStride Stride in uint8s between two consecutive rows of outMaterials (can be negative if the data is upside down). + void GetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, uint8 *outMaterials, intptr_t inMaterialsStride) const; + + /// Set the material indices of a block of data. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + /// @param inX Start X position, must in the range [0, mSampleCount - 1] + /// @param inY Start Y position, must in the range [0, mSampleCount - 1] + /// @param inSizeX Number of samples in X direction + /// @param inSizeY Number of samples in Y direction + /// @param inMaterials The new material indices, must be at least inSizeX * inSizeY uint8s. Values are returned in x-major order. + /// @param inMaterialsStride Stride in uint8s between two consecutive rows of inMaterials (can be negative if the data is upside down). + /// @param inMaterialList The material list to use for the new material indices or nullptr if the material list should not be updated + /// @param inAllocator Allocator to use for temporary memory + /// @return True if the material indices were set, false if the total number of materials exceeded 256 + bool SetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, const uint8 *inMaterials, intptr_t inMaterialsStride, const PhysicsMaterialList *inMaterialList, TempAllocator &inAllocator); + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void SaveMaterialState(PhysicsMaterialList &outMaterials) const override; + virtual void RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) override; + + // See Shape::GetStats + virtual Stats GetStats() const override; + + // See Shape::GetVolume + virtual float GetVolume() const override { return 0; } + +#ifdef JPH_DEBUG_RENDERER + // Settings + static bool sDrawTriangleOutlines; +#endif // JPH_DEBUG_RENDERER + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + class DecodingContext; ///< Context class for walking through all nodes of a heightfield + struct HSGetTrianglesContext; ///< Context class for GetTrianglesStart/Next + + /// Calculate commonly used values and store them in the shape + void CacheValues(); + + /// Allocate the mRangeBlocks, mHeightSamples and mActiveEdges buffers as a single data block + void AllocateBuffers(); + + /// Calculate bit mask for all active edges in the heightfield for a specific region + void CalculateActiveEdges(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, uint inHeightsStartX, uint inHeightsStartY, intptr_t inHeightsStride, float inHeightsScale, float inActiveEdgeCosThresholdAngle, TempAllocator &inAllocator); + + /// Calculate bit mask for all active edges in the heightfield + void CalculateActiveEdges(const HeightFieldShapeSettings &inSettings); + + /// Store material indices in the least amount of bits per index possible + void StoreMaterialIndices(const HeightFieldShapeSettings &inSettings); + + /// Get the amount of horizontal/vertical blocks + inline uint GetNumBlocks() const { return mSampleCount / mBlockSize; } + + /// Get the maximum level (amount of grids) of the tree + static inline uint sGetMaxLevel(uint inNumBlocks) { return 32 - CountLeadingZeros(inNumBlocks - 1); } + + /// Get the range block offset and stride for GetBlockOffsetAndScale + static inline void sGetRangeBlockOffsetAndStride(uint inNumBlocks, uint inMaxLevel, uint &outRangeBlockOffset, uint &outRangeBlockStride); + + /// For block (inBlockX, inBlockY) get the offset and scale needed to decode a uint8 height sample to a uint16 + inline void GetBlockOffsetAndScale(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, float &outBlockOffset, float &outBlockScale) const; + + /// Get the height sample at position (inX, inY) + inline uint8 GetHeightSample(uint inX, uint inY) const; + + /// Faster version of GetPosition when block offset and scale are already known + inline Vec3 GetPosition(uint inX, uint inY, float inBlockOffset, float inBlockScale, bool &outNoCollision) const; + + /// Determine amount of bits needed to encode sub shape id + uint GetSubShapeIDBits() const; + + /// En/decode a sub shape ID. inX and inY specify the coordinate of the triangle. inTriangle == 0 is the lower triangle, inTriangle == 1 is the upper triangle. + inline SubShapeID EncodeSubShapeID(const SubShapeIDCreator &inCreator, uint inX, uint inY, uint inTriangle) const; + inline void DecodeSubShapeID(const SubShapeID &inSubShapeID, uint &outX, uint &outY, uint &outTriangle) const; + + /// Get the edge flags for a triangle + inline uint8 GetEdgeFlags(uint inX, uint inY, uint inTriangle) const; + + // Helper functions called by CollisionDispatch + static void sCollideConvexVsHeightField(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 sCollideSphereVsHeightField(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 sCastConvexVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastSphereVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + /// Visit the entire height field using a visitor pattern + /// Note: Used to be inlined but this triggers a bug in MSVC where it will not free the memory allocated by alloca which causes a stack overflow when WalkHeightField is called in a loop (clang does it correct) + template + void WalkHeightField(Visitor &ioVisitor) const; + + /// A block of 2x2 ranges used to form a hierarchical grid, ordered left top, right top, left bottom, right bottom + struct alignas(16) RangeBlock + { + uint16 mMin[4]; + uint16 mMax[4]; + }; + + /// For block (inBlockX, inBlockY) get the range block and the entry in the range block + inline void GetRangeBlock(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, RangeBlock *&outBlock, uint &outIndexInBlock); + + /// Offset of first RangedBlock in grid per level + static const uint sGridOffsets[]; + + /// The height field is a surface defined by: mOffset + mScale * (x, mHeightSamples[y * mSampleCount + x], y). + /// where x and y are integers in the range x and y e [0, mSampleCount - 1]. + Vec3 mOffset = Vec3::sZero(); + Vec3 mScale = Vec3::sReplicate(1.0f); + + /// Height data + uint32 mSampleCount = 0; ///< See HeightFieldShapeSettings::mSampleCount + uint32 mBlockSize = 2; ///< See HeightFieldShapeSettings::mBlockSize + uint32 mHeightSamplesSize = 0; ///< Size of mHeightSamples in bytes + uint32 mRangeBlocksSize = 0; ///< Size of mRangeBlocks in elements + uint32 mActiveEdgesSize = 0; ///< Size of mActiveEdges in bytes + uint8 mBitsPerSample = 8; ///< See HeightFieldShapeSettings::mBitsPerSample + uint8 mSampleMask = 0xff; ///< All bits set for a sample: (1 << mBitsPerSample) - 1, used to indicate that there's no collision + uint16 mMinSample = HeightFieldShapeConstants::cNoCollisionValue16; ///< Min and max value in mHeightSamples quantized to 16 bit, for calculating bounding box + uint16 mMaxSample = HeightFieldShapeConstants::cNoCollisionValue16; + RangeBlock * mRangeBlocks = nullptr; ///< Hierarchical grid of range data describing the height variations within 1 block. The grid for level starts at offset sGridOffsets[] + uint8 * mHeightSamples = nullptr; ///< mBitsPerSample-bit height samples. Value [0, mMaxHeightValue] maps to highest detail grid in mRangeBlocks [mMin, mMax]. mNoCollisionValue is reserved to indicate no collision. + uint8 * mActiveEdges = nullptr; ///< (mSampleCount - 1)^2 * 3-bit active edge flags. + + /// Materials + PhysicsMaterialList mMaterials; ///< The materials of square at (x, y) is: mMaterials[mMaterialIndices[x + y * (mSampleCount - 1)]] + Array mMaterialIndices; ///< Compressed to the minimum amount of bits per material index (mSampleCount - 1) * (mSampleCount - 1) * mNumBitsPerMaterialIndex bits of data + uint32 mNumBitsPerMaterialIndex = 0; ///< Number of bits per material index + +#ifdef JPH_DEBUG_RENDERER + /// Temporary rendering data + mutable Array mGeometry; + mutable bool mCachedUseMaterialColors = false; ///< This is used to regenerate the triangle batch if the drawing settings change +#endif // JPH_DEBUG_RENDERER +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.cpp new file mode 100644 index 000000000000..852021a0407d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.cpp @@ -0,0 +1,1266 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_DEBUG_RENDERER +bool MeshShape::sDrawTriangleGroups = false; +bool MeshShape::sDrawTriangleOutlines = false; +#endif // JPH_DEBUG_RENDERER + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(MeshShapeSettings) +{ + JPH_ADD_BASE_CLASS(MeshShapeSettings, ShapeSettings) + + JPH_ADD_ATTRIBUTE(MeshShapeSettings, mTriangleVertices) + JPH_ADD_ATTRIBUTE(MeshShapeSettings, mIndexedTriangles) + JPH_ADD_ATTRIBUTE(MeshShapeSettings, mMaterials) + JPH_ADD_ATTRIBUTE(MeshShapeSettings, mMaxTrianglesPerLeaf) + JPH_ADD_ATTRIBUTE(MeshShapeSettings, mActiveEdgeCosThresholdAngle) + JPH_ADD_ATTRIBUTE(MeshShapeSettings, mPerTriangleUserData) +} + +// Codecs this mesh shape is using +using TriangleCodec = TriangleCodecIndexed8BitPackSOA4Flags; +using NodeCodec = NodeCodecQuadTreeHalfFloat; + +// Get header for tree +static JPH_INLINE const NodeCodec::Header *sGetNodeHeader(const ByteBuffer &inTree) +{ + return inTree.Get(0); +} + +// Get header for triangles +static JPH_INLINE const TriangleCodec::TriangleHeader *sGetTriangleHeader(const ByteBuffer &inTree) +{ + return inTree.Get(NodeCodec::HeaderSize); +} + +MeshShapeSettings::MeshShapeSettings(const TriangleList &inTriangles, PhysicsMaterialList inMaterials) : + mMaterials(std::move(inMaterials)) +{ + Indexify(inTriangles, mTriangleVertices, mIndexedTriangles); + + Sanitize(); +} + +MeshShapeSettings::MeshShapeSettings(VertexList inVertices, IndexedTriangleList inTriangles, PhysicsMaterialList inMaterials) : + mTriangleVertices(std::move(inVertices)), + mIndexedTriangles(std::move(inTriangles)), + mMaterials(std::move(inMaterials)) +{ + Sanitize(); +} + +void MeshShapeSettings::Sanitize() +{ + // Remove degenerate and duplicate triangles + UnorderedSet triangles; + triangles.reserve(UnorderedSet::size_type(mIndexedTriangles.size())); + TriangleCodec::ValidationContext validation_ctx(mIndexedTriangles, mTriangleVertices); + for (int t = (int)mIndexedTriangles.size() - 1; t >= 0; --t) + { + const IndexedTriangle &tri = mIndexedTriangles[t]; + + if (tri.IsDegenerate(mTriangleVertices) // Degenerate triangle + || validation_ctx.IsDegenerate(tri) // Triangle is degenerate in the quantized space + || !triangles.insert(tri.GetLowestIndexFirst()).second) // Duplicate triangle + { + // The order of triangles doesn't matter (gets reordered while building the tree), so we can just swap the last triangle into this slot + mIndexedTriangles[t] = mIndexedTriangles.back(); + mIndexedTriangles.pop_back(); + } + } +} + +ShapeSettings::ShapeResult MeshShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new MeshShape(*this, mCachedResult); + return mCachedResult; +} + +MeshShape::MeshShape(const MeshShapeSettings &inSettings, ShapeResult &outResult) : + Shape(EShapeType::Mesh, EShapeSubType::Mesh, inSettings, outResult) +{ + // Check if there are any triangles + if (inSettings.mIndexedTriangles.empty()) + { + outResult.SetError("Need triangles to create a mesh shape!"); + return; + } + + // Check triangles + TriangleCodec::ValidationContext validation_ctx(inSettings.mIndexedTriangles, inSettings.mTriangleVertices); + for (int t = (int)inSettings.mIndexedTriangles.size() - 1; t >= 0; --t) + { + const IndexedTriangle &triangle = inSettings.mIndexedTriangles[t]; + if (triangle.IsDegenerate(inSettings.mTriangleVertices) + || validation_ctx.IsDegenerate(triangle)) + { + outResult.SetError(StringFormat("Triangle %d is degenerate!", t)); + return; + } + else + { + // Check vertex indices + for (uint32 idx : triangle.mIdx) + if (idx >= inSettings.mTriangleVertices.size()) + { + outResult.SetError(StringFormat("Vertex index %u is beyond vertex list (size: %u)", idx, (uint)inSettings.mTriangleVertices.size())); + return; + } + } + } + + // Copy materials + mMaterials = inSettings.mMaterials; + if (!mMaterials.empty()) + { + // Validate materials + if (mMaterials.size() > (1 << FLAGS_MATERIAL_BITS)) + { + outResult.SetError(StringFormat("Supporting max %d materials per mesh", 1 << FLAGS_MATERIAL_BITS)); + return; + } + for (const IndexedTriangle &t : inSettings.mIndexedTriangles) + if (t.mMaterialIndex >= mMaterials.size()) + { + outResult.SetError(StringFormat("Triangle material %u is beyond material list (size: %u)", t.mMaterialIndex, (uint)mMaterials.size())); + return; + } + } + else + { + // No materials assigned, validate that all triangles use material index 0 + for (const IndexedTriangle &t : inSettings.mIndexedTriangles) + if (t.mMaterialIndex != 0) + { + outResult.SetError("No materials present, all triangles should have material index 0"); + return; + } + } + + // Check max triangles + if (inSettings.mMaxTrianglesPerLeaf < 1 || inSettings.mMaxTrianglesPerLeaf > MaxTrianglesPerLeaf) + { + outResult.SetError("Invalid max triangles per leaf"); + return; + } + + // Fill in active edge bits + IndexedTriangleList indexed_triangles = inSettings.mIndexedTriangles; // Copy indices since we're adding the 'active edge' flag + sFindActiveEdges(inSettings, indexed_triangles); + + // Create triangle splitter + TriangleSplitterBinning splitter(inSettings.mTriangleVertices, indexed_triangles); + + // Build tree + AABBTreeBuilder builder(splitter, inSettings.mMaxTrianglesPerLeaf); + AABBTreeBuilderStats builder_stats; + const AABBTreeBuilder::Node *root = builder.Build(builder_stats); + + // Convert to buffer + AABBTreeToBuffer buffer; + const char *error = nullptr; + if (!buffer.Convert(builder.GetTriangles(), builder.GetNodes(), inSettings.mTriangleVertices, root, inSettings.mPerTriangleUserData, error)) + { + outResult.SetError(error); + return; + } + + // Move data to this class + mTree.swap(buffer.GetBuffer()); + + // Check if we're not exceeding the amount of sub shape id bits + if (GetSubShapeIDBitsRecursive() > SubShapeID::MaxBits) + { + outResult.SetError("Mesh is too big and exceeds the amount of available sub shape ID bits"); + return; + } + + outResult.Set(this); +} + +void MeshShape::sFindActiveEdges(const MeshShapeSettings &inSettings, IndexedTriangleList &ioIndices) +{ + // A struct to hold the two vertex indices of an edge + struct Edge + { + Edge(int inIdx1, int inIdx2) : mIdx1(min(inIdx1, inIdx2)), mIdx2(max(inIdx1, inIdx2)) { } + + uint GetIndexInTriangle(const IndexedTriangle &inTriangle) const + { + for (uint edge_idx = 0; edge_idx < 3; ++edge_idx) + { + Edge edge(inTriangle.mIdx[edge_idx], inTriangle.mIdx[(edge_idx + 1) % 3]); + if (*this == edge) + return edge_idx; + } + + JPH_ASSERT(false); + return ~uint(0); + } + + bool operator == (const Edge &inRHS) const + { + return mIdx1 == inRHS.mIdx1 && mIdx2 == inRHS.mIdx2; + } + + uint64 GetHash() const + { + static_assert(sizeof(*this) == 2 * sizeof(int), "No padding expected"); + return HashBytes(this, sizeof(*this)); + } + + int mIdx1; + int mIdx2; + }; + + // A struct to hold the triangles that are connected to an edge + struct TriangleIndices + { + uint mNumTriangles = 0; + uint mTriangleIndices[2]; + }; + + // Build a list of edge to triangles + using EdgeToTriangle = UnorderedMap; + EdgeToTriangle edge_to_triangle; + edge_to_triangle.reserve(EdgeToTriangle::size_type(ioIndices.size() * 3)); + for (uint triangle_idx = 0; triangle_idx < ioIndices.size(); ++triangle_idx) + { + IndexedTriangle &triangle = ioIndices[triangle_idx]; + for (uint edge_idx = 0; edge_idx < 3; ++edge_idx) + { + Edge edge(triangle.mIdx[edge_idx], triangle.mIdx[(edge_idx + 1) % 3]); + EdgeToTriangle::iterator edge_to_triangle_it = edge_to_triangle.try_emplace(edge, TriangleIndices()).first; + TriangleIndices &indices = edge_to_triangle_it->second; + if (indices.mNumTriangles < 2) + { + // Store index of triangle that connects to this edge + indices.mTriangleIndices[indices.mNumTriangles] = triangle_idx; + indices.mNumTriangles++; + } + else + { + // 3 or more triangles share an edge, mark this edge as active + uint32 mask = 1 << (edge_idx + FLAGS_ACTIVE_EGDE_SHIFT); + JPH_ASSERT((triangle.mMaterialIndex & mask) == 0); + triangle.mMaterialIndex |= mask; + } + } + } + + // Walk over all edges and determine which ones are active + for (const EdgeToTriangle::value_type &edge : edge_to_triangle) + { + uint num_active = 0; + if (edge.second.mNumTriangles == 1) + { + // Edge is not shared, it is an active edge + num_active = 1; + } + else if (edge.second.mNumTriangles == 2) + { + // Simple shared edge, determine if edge is active based on the two adjacent triangles + const IndexedTriangle &triangle1 = ioIndices[edge.second.mTriangleIndices[0]]; + const IndexedTriangle &triangle2 = ioIndices[edge.second.mTriangleIndices[1]]; + + // Find which edge this is for both triangles + uint edge_idx1 = edge.first.GetIndexInTriangle(triangle1); + uint edge_idx2 = edge.first.GetIndexInTriangle(triangle2); + + // Construct a plane for triangle 1 (e1 = edge vertex 1, e2 = edge vertex 2, op = opposing vertex) + Vec3 triangle1_e1 = Vec3(inSettings.mTriangleVertices[triangle1.mIdx[edge_idx1]]); + Vec3 triangle1_e2 = Vec3(inSettings.mTriangleVertices[triangle1.mIdx[(edge_idx1 + 1) % 3]]); + Vec3 triangle1_op = Vec3(inSettings.mTriangleVertices[triangle1.mIdx[(edge_idx1 + 2) % 3]]); + Plane triangle1_plane = Plane::sFromPointsCCW(triangle1_e1, triangle1_e2, triangle1_op); + + // Construct a plane for triangle 2 + Vec3 triangle2_e1 = Vec3(inSettings.mTriangleVertices[triangle2.mIdx[edge_idx2]]); + Vec3 triangle2_e2 = Vec3(inSettings.mTriangleVertices[triangle2.mIdx[(edge_idx2 + 1) % 3]]); + Vec3 triangle2_op = Vec3(inSettings.mTriangleVertices[triangle2.mIdx[(edge_idx2 + 2) % 3]]); + Plane triangle2_plane = Plane::sFromPointsCCW(triangle2_e1, triangle2_e2, triangle2_op); + + // Determine if the edge is active + num_active = ActiveEdges::IsEdgeActive(triangle1_plane.GetNormal(), triangle2_plane.GetNormal(), triangle1_e2 - triangle1_e1, inSettings.mActiveEdgeCosThresholdAngle)? 2 : 0; + } + else + { + // More edges incoming, we've already marked all edges beyond the 2nd as active + num_active = 2; + } + + // Mark edges of all original triangles active + for (uint i = 0; i < num_active; ++i) + { + uint triangle_idx = edge.second.mTriangleIndices[i]; + IndexedTriangle &triangle = ioIndices[triangle_idx]; + uint edge_idx = edge.first.GetIndexInTriangle(triangle); + uint32 mask = 1 << (edge_idx + FLAGS_ACTIVE_EGDE_SHIFT); + JPH_ASSERT((triangle.mMaterialIndex & mask) == 0); + triangle.mMaterialIndex |= mask; + } + } +} + +MassProperties MeshShape::GetMassProperties() const +{ + // We cannot calculate the volume for an arbitrary mesh, so we return invalid mass properties. + // If you want your mesh to be dynamic, then you should provide the mass properties yourself when + // creating a Body: + // + // BodyCreationSettings::mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided; + // BodyCreationSettings::mMassPropertiesOverride.SetMassAndInertiaOfSolidBox(Vec3::sReplicate(1.0f), 1000.0f); + // + // Note that for a mesh shape to simulate properly, it is best if the mesh is manifold + // (i.e. closed, all edges shared by only two triangles, consistent winding order). + return MassProperties(); +} + +void MeshShape::DecodeSubShapeID(const SubShapeID &inSubShapeID, const void *&outTriangleBlock, uint32 &outTriangleIndex) const +{ + // Get block + SubShapeID triangle_idx_subshape_id; + uint32 block_id = inSubShapeID.PopID(NodeCodec::DecodingContext::sTriangleBlockIDBits(sGetNodeHeader(mTree)), triangle_idx_subshape_id); + outTriangleBlock = NodeCodec::DecodingContext::sGetTriangleBlockStart(&mTree[0], block_id); + + // Fetch the triangle index + SubShapeID remainder; + outTriangleIndex = triangle_idx_subshape_id.PopID(NumTriangleBits, remainder); + JPH_ASSERT(remainder.IsEmpty(), "Invalid subshape ID"); +} + +uint MeshShape::GetMaterialIndex(const SubShapeID &inSubShapeID) const +{ + // Decode ID + const void *block_start; + uint32 triangle_idx; + DecodeSubShapeID(inSubShapeID, block_start, triangle_idx); + + // Fetch the flags + uint8 flags = TriangleCodec::DecodingContext::sGetFlags(block_start, triangle_idx); + return flags & FLAGS_MATERIAL_MASK; +} + +const PhysicsMaterial *MeshShape::GetMaterial(const SubShapeID &inSubShapeID) const +{ + // Return the default material if there are no materials on this shape + if (mMaterials.empty()) + return PhysicsMaterial::sDefault; + + return mMaterials[GetMaterialIndex(inSubShapeID)]; +} + +Vec3 MeshShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + // Decode ID + const void *block_start; + uint32 triangle_idx; + DecodeSubShapeID(inSubShapeID, block_start, triangle_idx); + + // Decode triangle + Vec3 v1, v2, v3; + const TriangleCodec::DecodingContext triangle_ctx(sGetTriangleHeader(mTree)); + triangle_ctx.GetTriangle(block_start, triangle_idx, v1, v2, v3); + + // Calculate normal + return (v3 - v2).Cross(v1 - v2).Normalized(); +} + +void MeshShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + // Decode ID + const void *block_start; + uint32 triangle_idx; + DecodeSubShapeID(inSubShapeID, block_start, triangle_idx); + + // Decode triangle + const TriangleCodec::DecodingContext triangle_ctx(sGetTriangleHeader(mTree)); + outVertices.resize(3); + triangle_ctx.GetTriangle(block_start, triangle_idx, outVertices[0], outVertices[1], outVertices[2]); + + // Flip triangle if scaled inside out + if (ScaleHelpers::IsInsideOut(inScale)) + std::swap(outVertices[1], outVertices[2]); + + // Calculate transform with scale + Mat44 transform = inCenterOfMassTransform.PreScaled(inScale); + + // Transform to world space + for (Vec3 &v : outVertices) + v = transform * v; +} + +AABox MeshShape::GetLocalBounds() const +{ + const NodeCodec::Header *header = sGetNodeHeader(mTree); + return AABox(Vec3::sLoadFloat3Unsafe(header->mRootBoundsMin), Vec3::sLoadFloat3Unsafe(header->mRootBoundsMax)); +} + +uint MeshShape::GetSubShapeIDBitsRecursive() const +{ + return NodeCodec::DecodingContext::sTriangleBlockIDBits(sGetNodeHeader(mTree)) + NumTriangleBits; +} + +template +JPH_INLINE void MeshShape::WalkTree(Visitor &ioVisitor) const +{ + const NodeCodec::Header *header = sGetNodeHeader(mTree); + NodeCodec::DecodingContext node_ctx(header); + + const TriangleCodec::DecodingContext triangle_ctx(sGetTriangleHeader(mTree)); + const uint8 *buffer_start = &mTree[0]; + node_ctx.WalkTree(buffer_start, triangle_ctx, ioVisitor); +} + +template +JPH_INLINE void MeshShape::WalkTreePerTriangle(const SubShapeIDCreator &inSubShapeIDCreator2, Visitor &ioVisitor) const +{ + struct ChainedVisitor + { + JPH_INLINE ChainedVisitor(Visitor &ioVisitor, const SubShapeIDCreator &inSubShapeIDCreator2, uint inTriangleBlockIDBits) : + mVisitor(ioVisitor), + mSubShapeIDCreator2(inSubShapeIDCreator2), + mTriangleBlockIDBits(inTriangleBlockIDBits) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return mVisitor.ShouldAbort(); + } + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mVisitor.ShouldVisitNode(inStackTop); + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + return mVisitor.VisitNodes(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, ioProperties, inStackTop); + } + + JPH_INLINE void VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, uint32 inTriangleBlockID) + { + // Create ID for triangle block + SubShapeIDCreator block_sub_shape_id = mSubShapeIDCreator2.PushID(inTriangleBlockID, mTriangleBlockIDBits); + + // Decode vertices and flags + JPH_ASSERT(inNumTriangles <= MaxTrianglesPerLeaf); + Vec3 vertices[MaxTrianglesPerLeaf * 3]; + uint8 flags[MaxTrianglesPerLeaf]; + ioContext.Unpack(inTriangles, inNumTriangles, vertices, flags); + + int triangle_idx = 0; + for (const Vec3 *v = vertices, *v_end = vertices + inNumTriangles * 3; v < v_end; v += 3, triangle_idx++) + { + // Determine active edges + uint8 active_edges = (flags[triangle_idx] >> FLAGS_ACTIVE_EGDE_SHIFT) & FLAGS_ACTIVE_EDGE_MASK; + + // Create ID for triangle + SubShapeIDCreator triangle_sub_shape_id = block_sub_shape_id.PushID(triangle_idx, NumTriangleBits); + + mVisitor.VisitTriangle(v[0], v[1], v[2], active_edges, triangle_sub_shape_id.GetID()); + + // Check if we should early out now + if (mVisitor.ShouldAbort()) + break; + } + } + + Visitor & mVisitor; + SubShapeIDCreator mSubShapeIDCreator2; + uint mTriangleBlockIDBits; + }; + + ChainedVisitor visitor(ioVisitor, inSubShapeIDCreator2, NodeCodec::DecodingContext::sTriangleBlockIDBits(sGetNodeHeader(mTree))); + WalkTree(visitor); +} + +#ifdef JPH_DEBUG_RENDERER +void MeshShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + // Reset the batch if we switch coloring mode + if (mCachedTrianglesColoredPerGroup != sDrawTriangleGroups || mCachedUseMaterialColors != inUseMaterialColors) + { + mGeometry = nullptr; + mCachedTrianglesColoredPerGroup = sDrawTriangleGroups; + mCachedUseMaterialColors = inUseMaterialColors; + } + + if (mGeometry == nullptr) + { + struct Visitor + { + JPH_INLINE bool ShouldAbort() const + { + return false; + } + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + UVec4 valid = UVec4::sOr(UVec4::sOr(Vec4::sLess(inBoundsMinX, inBoundsMaxX), Vec4::sLess(inBoundsMinY, inBoundsMaxY)), Vec4::sLess(inBoundsMinZ, inBoundsMaxZ)); + return CountAndSortTrues(valid, ioProperties); + } + + JPH_INLINE void VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, [[maybe_unused]] uint32 inTriangleBlockID) + { + JPH_ASSERT(inNumTriangles <= MaxTrianglesPerLeaf); + Vec3 vertices[MaxTrianglesPerLeaf * 3]; + ioContext.Unpack(inTriangles, inNumTriangles, vertices); + + if (mDrawTriangleGroups || !mUseMaterialColors || mMaterials.empty()) + { + // Single color for mesh + Color color = mDrawTriangleGroups? Color::sGetDistinctColor(mColorIdx++) : (mUseMaterialColors? PhysicsMaterial::sDefault->GetDebugColor() : Color::sWhite); + for (const Vec3 *v = vertices, *v_end = vertices + inNumTriangles * 3; v < v_end; v += 3) + mTriangles.push_back({ v[0], v[1], v[2], color }); + } + else + { + // Per triangle color + uint8 flags[MaxTrianglesPerLeaf]; + TriangleCodec::DecodingContext::sGetFlags(inTriangles, inNumTriangles, flags); + + const uint8 *f = flags; + for (const Vec3 *v = vertices, *v_end = vertices + inNumTriangles * 3; v < v_end; v += 3, f++) + mTriangles.push_back({ v[0], v[1], v[2], mMaterials[*f & FLAGS_MATERIAL_MASK]->GetDebugColor() }); + } + } + + Array & mTriangles; + const PhysicsMaterialList & mMaterials; + bool mUseMaterialColors; + bool mDrawTriangleGroups; + int mColorIdx = 0; + }; + + Array triangles; + Visitor visitor { triangles, mMaterials, mCachedUseMaterialColors, mCachedTrianglesColoredPerGroup }; + WalkTree(visitor); + mGeometry = new DebugRenderer::Geometry(inRenderer->CreateTriangleBatch(triangles), GetLocalBounds()); + } + + // Test if the shape is scaled inside out + DebugRenderer::ECullMode cull_mode = ScaleHelpers::IsInsideOut(inScale)? DebugRenderer::ECullMode::CullFrontFace : DebugRenderer::ECullMode::CullBackFace; + + // Determine the draw mode + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + + // Draw the geometry + inRenderer->DrawGeometry(inCenterOfMassTransform * Mat44::sScale(inScale), inColor, mGeometry, cull_mode, DebugRenderer::ECastShadow::On, draw_mode); + + if (sDrawTriangleOutlines) + { + struct Visitor + { + JPH_INLINE Visitor(DebugRenderer *inRenderer, RMat44Arg inTransform) : + mRenderer(inRenderer), + mTransform(inTransform) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return false; + } + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + UVec4 valid = UVec4::sOr(UVec4::sOr(Vec4::sLess(inBoundsMinX, inBoundsMaxX), Vec4::sLess(inBoundsMinY, inBoundsMaxY)), Vec4::sLess(inBoundsMinZ, inBoundsMaxZ)); + return CountAndSortTrues(valid, ioProperties); + } + + JPH_INLINE void VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, uint32 inTriangleBlockID) + { + // Decode vertices and flags + JPH_ASSERT(inNumTriangles <= MaxTrianglesPerLeaf); + Vec3 vertices[MaxTrianglesPerLeaf * 3]; + uint8 flags[MaxTrianglesPerLeaf]; + ioContext.Unpack(inTriangles, inNumTriangles, vertices, flags); + + // Loop through triangles + const uint8 *f = flags; + for (Vec3 *v = vertices, *v_end = vertices + inNumTriangles * 3; v < v_end; v += 3, ++f) + { + // Loop through edges + for (uint edge_idx = 0; edge_idx < 3; ++edge_idx) + { + RVec3 v1 = mTransform * v[edge_idx]; + RVec3 v2 = mTransform * v[(edge_idx + 1) % 3]; + + // Draw active edge as a green arrow, other edges as grey + if (*f & (1 << (edge_idx + FLAGS_ACTIVE_EGDE_SHIFT))) + mRenderer->DrawArrow(v1, v2, Color::sGreen, 0.01f); + else + mRenderer->DrawLine(v1, v2, Color::sGrey); + } + } + } + + DebugRenderer * mRenderer; + RMat44 mTransform; + }; + + Visitor visitor { inRenderer, inCenterOfMassTransform.PreScaled(inScale) }; + WalkTree(visitor); + } +} +#endif // JPH_DEBUG_RENDERER + +bool MeshShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor + { + JPH_INLINE explicit Visitor(RayCastResult &ioHit) : + mHit(ioHit) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return mHit.mFraction <= 0.0f; + } + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mDistanceStack[inStackTop] < mHit.mFraction; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test bounds of 4 children + Vec4 distance = RayAABox4(mRayOrigin, mRayInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mHit.mFraction, ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, uint32 inTriangleBlockID) + { + // Test against triangles + uint32 triangle_idx; + float fraction = ioContext.TestRay(mRayOrigin, mRayDirection, inTriangles, inNumTriangles, mHit.mFraction, triangle_idx); + if (fraction < mHit.mFraction) + { + mHit.mFraction = fraction; + mHit.mSubShapeID2 = mSubShapeIDCreator.PushID(inTriangleBlockID, mTriangleBlockIDBits).PushID(triangle_idx, NumTriangleBits).GetID(); + mReturnValue = true; + } + } + + RayCastResult & mHit; + Vec3 mRayOrigin; + Vec3 mRayDirection; + RayInvDirection mRayInvDirection; + uint mTriangleBlockIDBits; + SubShapeIDCreator mSubShapeIDCreator; + bool mReturnValue = false; + float mDistanceStack[NodeCodec::StackSize]; + }; + + Visitor visitor(ioHit); + visitor.mRayOrigin = inRay.mOrigin; + visitor.mRayDirection = inRay.mDirection; + visitor.mRayInvDirection.Set(inRay.mDirection); + visitor.mTriangleBlockIDBits = NodeCodec::DecodingContext::sTriangleBlockIDBits(sGetNodeHeader(mTree)); + visitor.mSubShapeIDCreator = inSubShapeIDCreator; + WalkTree(visitor); + + return visitor.mReturnValue; +} + +void MeshShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + struct Visitor + { + JPH_INLINE explicit Visitor(CastRayCollector &ioCollector) : + mCollector(ioCollector) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetEarlyOutFraction(); + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test bounds of 4 children + Vec4 distance = RayAABox4(mRayOrigin, mRayInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, [[maybe_unused]] uint8 inActiveEdges, SubShapeID inSubShapeID2) + { + // Back facing check + if (mBackFaceMode == EBackFaceMode::IgnoreBackFaces && (inV2 - inV0).Cross(inV1 - inV0).Dot(mRayDirection) < 0) + return; + + // Check the triangle + float fraction = RayTriangle(mRayOrigin, mRayDirection, inV0, inV1, inV2); + if (fraction < mCollector.GetEarlyOutFraction()) + { + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(mCollector.GetContext()); + hit.mFraction = fraction; + hit.mSubShapeID2 = inSubShapeID2; + mCollector.AddHit(hit); + } + } + + CastRayCollector & mCollector; + Vec3 mRayOrigin; + Vec3 mRayDirection; + RayInvDirection mRayInvDirection; + EBackFaceMode mBackFaceMode; + float mDistanceStack[NodeCodec::StackSize]; + }; + + Visitor visitor(ioCollector); + visitor.mBackFaceMode = inRayCastSettings.mBackFaceModeTriangles; + visitor.mRayOrigin = inRay.mOrigin; + visitor.mRayDirection = inRay.mDirection; + visitor.mRayInvDirection.Set(inRay.mDirection); + WalkTreePerTriangle(inSubShapeIDCreator, visitor); +} + +void MeshShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + sCollidePointUsingRayCast(*this, inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void MeshShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CollideSoftBodyVerticesVsTriangles + { + using CollideSoftBodyVerticesVsTriangles::CollideSoftBodyVerticesVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return false; + } + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return mDistanceStack[inStackTop] < mClosestDistanceSq; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Get distance to vertex + Vec4 dist_sq = AABox4DistanceSqToPoint(mLocalPosition, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(dist_sq, mClosestDistanceSq, ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, [[maybe_unused]] uint8 inActiveEdges, [[maybe_unused]] SubShapeID inSubShapeID2) + { + ProcessTriangle(inV0, inV1, inV2); + } + + float mDistanceStack[NodeCodec::StackSize]; + }; + + Visitor visitor(inCenterOfMassTransform, inScale); + + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) + { + visitor.StartVertex(v); + WalkTreePerTriangle(SubShapeIDCreator(), visitor); + visitor.FinishVertex(v, inCollidingShapeIndex); + } +} + +void MeshShape::sCastConvexVsMesh(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastConvexVsTriangles + { + using CastConvexVsTriangles::CastConvexVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction(); + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Enlarge them by the casted shape's box extents + AABox4EnlargeWithExtent(mBoxExtent, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test bounds of 4 children + Vec4 distance = RayAABox4(mBoxCenter, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2) + { + Cast(inV0, inV1, inV2, inActiveEdges, inSubShapeID2); + } + + RayInvDirection mInvDirection; + Vec3 mBoxCenter; + Vec3 mBoxExtent; + float mDistanceStack[NodeCodec::StackSize]; + }; + + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Mesh); + const MeshShape *shape = static_cast(inShape); + + Visitor visitor(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + visitor.mInvDirection.Set(inShapeCast.mDirection); + visitor.mBoxCenter = inShapeCast.mShapeWorldBounds.GetCenter(); + visitor.mBoxExtent = inShapeCast.mShapeWorldBounds.GetExtent(); + shape->WalkTreePerTriangle(inSubShapeIDCreator2, visitor); +} + +void MeshShape::sCastSphereVsMesh(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastSphereVsTriangles + { + using CastSphereVsTriangles::CastSphereVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction(); + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Enlarge them by the radius of the sphere + AABox4EnlargeWithExtent(Vec3::sReplicate(mRadius), bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test bounds of 4 children + Vec4 distance = RayAABox4(mStart, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2) + { + Cast(inV0, inV1, inV2, inActiveEdges, inSubShapeID2); + } + + RayInvDirection mInvDirection; + float mDistanceStack[NodeCodec::StackSize]; + }; + + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Mesh); + const MeshShape *shape = static_cast(inShape); + + Visitor visitor(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + visitor.mInvDirection.Set(inShapeCast.mDirection); + shape->WalkTreePerTriangle(inSubShapeIDCreator2, visitor); +} + +struct MeshShape::MSGetTrianglesContext +{ + JPH_INLINE MSGetTrianglesContext(const MeshShape *inShape, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) : + mDecodeCtx(sGetNodeHeader(inShape->mTree)), + mShape(inShape), + mLocalBox(Mat44::sInverseRotationTranslation(inRotation, inPositionCOM), inBox), + mMeshScale(inScale), + mLocalToWorld(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale)), + mIsInsideOut(ScaleHelpers::IsInsideOut(inScale)) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return mShouldAbort; + } + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mMeshScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which nodes collide + UVec4 collides = AABox4VsBox(mLocalBox, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + return CountAndSortTrues(collides, ioProperties); + } + + JPH_INLINE void VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, [[maybe_unused]] uint32 inTriangleBlockID) + { + // When the buffer is full and we cannot process the triangles, abort the tree walk. The next time GetTrianglesNext is called we will continue here. + if (mNumTrianglesFound + inNumTriangles > mMaxTrianglesRequested) + { + mShouldAbort = true; + return; + } + + // Decode vertices + JPH_ASSERT(inNumTriangles <= MaxTrianglesPerLeaf); + Vec3 vertices[MaxTrianglesPerLeaf * 3]; + ioContext.Unpack(inTriangles, inNumTriangles, vertices); + + // Store vertices as Float3 + if (mIsInsideOut) + { + // Scaled inside out, flip the triangles + for (const Vec3 *v = vertices, *v_end = v + 3 * inNumTriangles; v < v_end; v += 3) + { + (mLocalToWorld * v[0]).StoreFloat3(mTriangleVertices++); + (mLocalToWorld * v[2]).StoreFloat3(mTriangleVertices++); + (mLocalToWorld * v[1]).StoreFloat3(mTriangleVertices++); + } + } + else + { + // Normal scale + for (const Vec3 *v = vertices, *v_end = v + 3 * inNumTriangles; v < v_end; ++v) + (mLocalToWorld * *v).StoreFloat3(mTriangleVertices++); + } + + if (mMaterials != nullptr) + { + if (mShape->mMaterials.empty()) + { + // No materials, output default + const PhysicsMaterial *default_material = PhysicsMaterial::sDefault; + for (int m = 0; m < inNumTriangles; ++m) + *mMaterials++ = default_material; + } + else + { + // Decode triangle flags + uint8 flags[MaxTrianglesPerLeaf]; + TriangleCodec::DecodingContext::sGetFlags(inTriangles, inNumTriangles, flags); + + // Store materials + for (const uint8 *f = flags, *f_end = f + inNumTriangles; f < f_end; ++f) + *mMaterials++ = mShape->mMaterials[*f & FLAGS_MATERIAL_MASK].GetPtr(); + } + } + + // Accumulate triangles found + mNumTrianglesFound += inNumTriangles; + } + + NodeCodec::DecodingContext mDecodeCtx; + const MeshShape * mShape; + OrientedBox mLocalBox; + Vec3 mMeshScale; + Mat44 mLocalToWorld; + int mMaxTrianglesRequested; + Float3 * mTriangleVertices; + int mNumTrianglesFound; + const PhysicsMaterial ** mMaterials; + bool mShouldAbort; + bool mIsInsideOut; +}; + +void MeshShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + static_assert(sizeof(MSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(&ioContext, alignof(MSGetTrianglesContext))); + + new (&ioContext) MSGetTrianglesContext(this, inBox, inPositionCOM, inRotation, inScale); +} + +int MeshShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + static_assert(cGetTrianglesMinTrianglesRequested >= MaxTrianglesPerLeaf, "cGetTrianglesMinTrianglesRequested is too small"); + JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested); + + // Check if we're done + MSGetTrianglesContext &context = (MSGetTrianglesContext &)ioContext; + if (context.mDecodeCtx.IsDoneWalking()) + return 0; + + // Store parameters on context + context.mMaxTrianglesRequested = inMaxTrianglesRequested; + context.mTriangleVertices = outTriangleVertices; + context.mMaterials = outMaterials; + context.mShouldAbort = false; // Reset the abort flag + context.mNumTrianglesFound = 0; + + // Continue (or start) walking the tree + const TriangleCodec::DecodingContext triangle_ctx(sGetTriangleHeader(mTree)); + const uint8 *buffer_start = &mTree[0]; + context.mDecodeCtx.WalkTree(buffer_start, triangle_ctx, context); + return context.mNumTrianglesFound; +} + +void MeshShape::sCollideConvexVsMesh(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, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + // Get the shapes + JPH_ASSERT(inShape1->GetType() == EShapeType::Convex); + JPH_ASSERT(inShape2->GetType() == EShapeType::Mesh); + const ConvexShape *shape1 = static_cast(inShape1); + const MeshShape *shape2 = static_cast(inShape2); + + struct Visitor : public CollideConvexVsTriangles + { + using CollideConvexVsTriangles::CollideConvexVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which nodes collide + UVec4 collides = AABox4VsBox(mBoundsOf1InSpaceOf2, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + return CountAndSortTrues(collides, ioProperties); + } + + JPH_INLINE void VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2) + { + Collide(inV0, inV1, inV2, inActiveEdges, inSubShapeID2); + } + }; + + Visitor visitor(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + shape2->WalkTreePerTriangle(inSubShapeIDCreator2, visitor); +} + +void MeshShape::sCollideSphereVsMesh(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, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + // Get the shapes + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Sphere); + JPH_ASSERT(inShape2->GetType() == EShapeType::Mesh); + const SphereShape *shape1 = static_cast(inShape1); + const MeshShape *shape2 = static_cast(inShape2); + + struct Visitor : public CollideSphereVsTriangles + { + using CollideSphereVsTriangles::CollideSphereVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which nodes collide + UVec4 collides = AABox4VsSphere(mSphereCenterIn2, mRadiusPlusMaxSeparationSq, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + return CountAndSortTrues(collides, ioProperties); + } + + JPH_INLINE void VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2) + { + Collide(inV0, inV1, inV2, inActiveEdges, inSubShapeID2); + } + }; + + Visitor visitor(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + shape2->WalkTreePerTriangle(inSubShapeIDCreator2, visitor); +} + +void MeshShape::SaveBinaryState(StreamOut &inStream) const +{ + Shape::SaveBinaryState(inStream); + + inStream.Write(static_cast(mTree)); // Make sure we use the Array<> overload +} + +void MeshShape::RestoreBinaryState(StreamIn &inStream) +{ + Shape::RestoreBinaryState(inStream); + + inStream.Read(static_cast(mTree)); // Make sure we use the Array<> overload +} + +void MeshShape::SaveMaterialState(PhysicsMaterialList &outMaterials) const +{ + outMaterials = mMaterials; +} + +void MeshShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) +{ + mMaterials.assign(inMaterials, inMaterials + inNumMaterials); +} + +Shape::Stats MeshShape::GetStats() const +{ + // Walk the tree to count the triangles + struct Visitor + { + JPH_INLINE bool ShouldAbort() const + { + return false; + } + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Visit all valid children + UVec4 valid = UVec4::sOr(UVec4::sOr(Vec4::sLess(inBoundsMinX, inBoundsMaxX), Vec4::sLess(inBoundsMinY, inBoundsMaxY)), Vec4::sLess(inBoundsMinZ, inBoundsMaxZ)); + return CountAndSortTrues(valid, ioProperties); + } + + JPH_INLINE void VisitTriangles([[maybe_unused]] const TriangleCodec::DecodingContext &ioContext, [[maybe_unused]] const void *inTriangles, int inNumTriangles, [[maybe_unused]] uint32 inTriangleBlockID) + { + mNumTriangles += inNumTriangles; + } + + uint mNumTriangles = 0; + }; + + Visitor visitor; + WalkTree(visitor); + + return Stats(sizeof(*this) + mMaterials.size() * sizeof(Ref) + mTree.size() * sizeof(uint8), visitor.mNumTriangles); +} + +uint32 MeshShape::GetTriangleUserData(const SubShapeID &inSubShapeID) const +{ + // Decode ID + const void *block_start; + uint32 triangle_idx; + DecodeSubShapeID(inSubShapeID, block_start, triangle_idx); + + // Decode triangle + const TriangleCodec::DecodingContext triangle_ctx(sGetTriangleHeader(mTree)); + return triangle_ctx.GetUserData(block_start, triangle_idx); +} + +void MeshShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Mesh); + f.mConstruct = []() -> Shape * { return new MeshShape; }; + f.mColor = Color::sRed; + + for (EShapeSubType s : sConvexSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Mesh, sCollideConvexVsMesh); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Mesh, sCastConvexVsMesh); + + CollisionDispatch::sRegisterCastShape(EShapeSubType::Mesh, s, CollisionDispatch::sReversedCastShape); + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Mesh, s, CollisionDispatch::sReversedCollideShape); + } + + // Specialized collision functions + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Sphere, EShapeSubType::Mesh, sCollideSphereVsMesh); + CollisionDispatch::sRegisterCastShape(EShapeSubType::Sphere, EShapeSubType::Mesh, sCastSphereVsMesh); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.h new file mode 100644 index 000000000000..a211b7a0ad3c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.h @@ -0,0 +1,217 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +class ConvexShape; +class CollideShapeSettings; + +/// Class that constructs a MeshShape +class JPH_EXPORT MeshShapeSettings final : public ShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, MeshShapeSettings) + +public: + /// Default constructor for deserialization + MeshShapeSettings() = default; + + /// Create a mesh shape. + MeshShapeSettings(const TriangleList &inTriangles, PhysicsMaterialList inMaterials = PhysicsMaterialList()); + MeshShapeSettings(VertexList inVertices, IndexedTriangleList inTriangles, PhysicsMaterialList inMaterials = PhysicsMaterialList()); + + /// Sanitize the mesh data. Remove duplicate and degenerate triangles. This is called automatically when constructing the MeshShapeSettings with a list of (indexed-) triangles. + void Sanitize(); + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + /// Vertices belonging to mIndexedTriangles + VertexList mTriangleVertices; + + /// Original list of indexed triangles (triangles will be reordered internally in the mesh shape). + /// Triangles must be provided in counter clockwise order. + /// Degenerate triangles will automatically be removed during mesh creation but no other mesh simplifications are performed, use an external library if this is desired. + /// For simulation, the triangles are considered to be single sided. + /// For ray casts you can choose to make triangles double sided by setting RayCastSettings::mBackFaceMode to EBackFaceMode::CollideWithBackFaces. + /// For collide shape tests you can use CollideShapeSettings::mBackFaceMode and for shape casts you can use ShapeCastSettings::mBackFaceModeTriangles. + IndexedTriangleList mIndexedTriangles; + + /// Materials assigned to the triangles. Each triangle specifies which material it uses through its mMaterialIndex + PhysicsMaterialList mMaterials; + + /// Maximum number of triangles in each leaf of the axis aligned box tree. This is a balance between memory and performance. Can be in the range [1, MeshShape::MaxTrianglesPerLeaf]. + /// Sensible values are between 4 (for better performance) and 8 (for less memory usage). + uint mMaxTrianglesPerLeaf = 8; + + /// Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive). + /// Setting this value too small can cause ghost collisions with edges, setting it too big can cause depenetration artifacts (objects not depenetrating quickly). + /// Valid ranges are between cos(0 degrees) and cos(90 degrees). The default value is cos(5 degrees). + float mActiveEdgeCosThresholdAngle = 0.996195f; // cos(5 degrees) + + /// When true, we store the user data coming from Triangle::mUserData or IndexedTriangle::mUserData in the mesh shape. + /// This can be used to store additional data like the original index of the triangle in the mesh. + /// Can be retrieved using MeshShape::GetTriangleUserData. + /// Turning this on increases the memory used by the MeshShape by roughly 25%. + bool mPerTriangleUserData = false; +}; + +/// A mesh shape, consisting of triangles. Mesh shapes are mostly used for static geometry. +/// They can be used by dynamic or kinematic objects but only if they don't collide with other mesh or heightfield shapes as those collisions are currently not supported. +/// Note that if you make a mesh shape a dynamic or kinematic object, you need to provide a mass yourself as mesh shapes don't need to form a closed hull so don't have a well defined volume from which the mass can be calculated. +class JPH_EXPORT MeshShape final : public Shape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + MeshShape() : Shape(EShapeType::Mesh, EShapeSubType::Mesh) { } + MeshShape(const MeshShapeSettings &inSettings, ShapeResult &outResult); + + // See Shape::MustBeStatic + virtual bool MustBeStatic() const override { return true; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetSubShapeIDBitsRecursive + virtual uint GetSubShapeIDBitsRecursive() const override; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return 0.0f; } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetMaterial + virtual const PhysicsMaterial * GetMaterial(const SubShapeID &inSubShapeID) const override; + + /// Get the list of all materials + const PhysicsMaterialList & GetMaterialList() const { return mMaterials; } + + /// Determine which material index a particular sub shape uses (note that if there are no materials this function will return 0 so check the array size) + /// Note: This could for example be used to create a decorator shape around a mesh shape that overrides the GetMaterial call to replace a material with another material. + uint GetMaterialIndex(const SubShapeID &inSubShapeID) const; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + /// See: Shape::CollidePoint + /// Note that for CollidePoint to work for a mesh shape, the mesh needs to be closed (a manifold) or multiple non-intersecting manifolds. Triangles may be facing the interior of the manifold. + /// Insideness is tested by counting the amount of triangles encountered when casting an infinite ray from inPoint. If the number of hits is odd we're inside, if it's even we're outside. + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override { JPH_ASSERT(false, "Not supported"); } + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void SaveMaterialState(PhysicsMaterialList &outMaterials) const override; + virtual void RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) override; + + // See Shape::GetStats + virtual Stats GetStats() const override; + + // See Shape::GetVolume + virtual float GetVolume() const override { return 0; } + + // When MeshShape::mPerTriangleUserData is true, this function can be used to retrieve the user data that was stored in the mesh shape. + uint32 GetTriangleUserData(const SubShapeID &inSubShapeID) const; + +#ifdef JPH_DEBUG_RENDERER + // Settings + static bool sDrawTriangleGroups; + static bool sDrawTriangleOutlines; +#endif // JPH_DEBUG_RENDERER + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + struct MSGetTrianglesContext; ///< Context class for GetTrianglesStart/Next + + static constexpr int NumTriangleBits = 3; ///< How many bits to reserve to encode the triangle index + static constexpr int MaxTrianglesPerLeaf = 1 << NumTriangleBits; ///< Number of triangles that are stored max per leaf aabb node + + /// Find and flag active edges + static void sFindActiveEdges(const MeshShapeSettings &inSettings, IndexedTriangleList &ioIndices); + + /// Visit the entire tree using a visitor pattern + template + void WalkTree(Visitor &ioVisitor) const; + + /// Same as above but with a callback per triangle instead of per block of triangles + template + void WalkTreePerTriangle(const SubShapeIDCreator &inSubShapeIDCreator2, Visitor &ioVisitor) const; + + /// Decode a sub shape ID + inline void DecodeSubShapeID(const SubShapeID &inSubShapeID, const void *&outTriangleBlock, uint32 &outTriangleIndex) const; + + // Helper functions called by CollisionDispatch + static void sCollideConvexVsMesh(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 sCollideSphereVsMesh(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 sCastConvexVsMesh(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastSphereVsMesh(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + /// Materials assigned to the triangles. Each triangle specifies which material it uses through its mMaterialIndex + PhysicsMaterialList mMaterials; + + ByteBuffer mTree; ///< Resulting packed data structure + + /// 8 bit flags stored per triangle + enum ETriangleFlags + { + /// Material index + FLAGS_MATERIAL_BITS = 5, + FLAGS_MATERIAL_MASK = (1 << FLAGS_MATERIAL_BITS) - 1, + + /// Active edge bits + FLAGS_ACTIVE_EGDE_SHIFT = FLAGS_MATERIAL_BITS, + FLAGS_ACTIVE_EDGE_BITS = 3, + FLAGS_ACTIVE_EDGE_MASK = (1 << FLAGS_ACTIVE_EDGE_BITS) - 1 + }; + +#ifdef JPH_DEBUG_RENDERER + mutable DebugRenderer::GeometryRef mGeometry; ///< Debug rendering data + mutable bool mCachedTrianglesColoredPerGroup = false; ///< This is used to regenerate the triangle batch if the drawing settings change + mutable bool mCachedUseMaterialColors = false; ///< This is used to regenerate the triangle batch if the drawing settings change +#endif // JPH_DEBUG_RENDERER +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp new file mode 100644 index 000000000000..fdedc9a31715 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp @@ -0,0 +1,589 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(MutableCompoundShapeSettings) +{ + JPH_ADD_BASE_CLASS(MutableCompoundShapeSettings, CompoundShapeSettings) +} + +ShapeSettings::ShapeResult MutableCompoundShapeSettings::Create() const +{ + // Build a mutable compound shape + if (mCachedResult.IsEmpty()) + Ref shape = new MutableCompoundShape(*this, mCachedResult); + + return mCachedResult; +} + +MutableCompoundShape::MutableCompoundShape(const MutableCompoundShapeSettings &inSettings, ShapeResult &outResult) : + CompoundShape(EShapeSubType::MutableCompound, inSettings, outResult) +{ + mSubShapes.reserve(inSettings.mSubShapes.size()); + for (const CompoundShapeSettings::SubShapeSettings &shape : inSettings.mSubShapes) + { + // Start constructing the runtime sub shape + SubShape out_shape; + if (!out_shape.FromSettings(shape, outResult)) + return; + + mSubShapes.push_back(out_shape); + } + + AdjustCenterOfMass(); + + CalculateSubShapeBounds(0, (uint)mSubShapes.size()); + + // Check if we're not exceeding the amount of sub shape id bits + if (GetSubShapeIDBitsRecursive() > SubShapeID::MaxBits) + { + outResult.SetError("Compound hierarchy is too deep and exceeds the amount of available sub shape ID bits"); + return; + } + + outResult.Set(this); +} + +Ref MutableCompoundShape::Clone() const +{ + Ref clone = new MutableCompoundShape(); + clone->SetUserData(GetUserData()); + + clone->mCenterOfMass = mCenterOfMass; + clone->mLocalBounds = mLocalBounds; + clone->mSubShapes = mSubShapes; + clone->mInnerRadius = mInnerRadius; + clone->mSubShapeBounds = mSubShapeBounds; + + return clone; +} + +void MutableCompoundShape::AdjustCenterOfMass() +{ + // First calculate the delta of the center of mass + float mass = 0.0f; + Vec3 center_of_mass = Vec3::sZero(); + for (const CompoundShape::SubShape &sub_shape : mSubShapes) + { + MassProperties child = sub_shape.mShape->GetMassProperties(); + mass += child.mMass; + center_of_mass += sub_shape.GetPositionCOM() * child.mMass; + } + if (mass > 0.0f) + center_of_mass /= mass; + + // Now adjust all shapes to recenter around center of mass + for (CompoundShape::SubShape &sub_shape : mSubShapes) + sub_shape.SetPositionCOM(sub_shape.GetPositionCOM() - center_of_mass); + + // Update bounding boxes + for (Bounds &bounds : mSubShapeBounds) + { + Vec4 xxxx = center_of_mass.SplatX(); + Vec4 yyyy = center_of_mass.SplatY(); + Vec4 zzzz = center_of_mass.SplatZ(); + bounds.mMinX -= xxxx; + bounds.mMinY -= yyyy; + bounds.mMinZ -= zzzz; + bounds.mMaxX -= xxxx; + bounds.mMaxY -= yyyy; + bounds.mMaxZ -= zzzz; + } + mLocalBounds.Translate(-center_of_mass); + + // And adjust the center of mass for this shape in the opposite direction + mCenterOfMass += center_of_mass; +} + +void MutableCompoundShape::CalculateLocalBounds() +{ + uint num_blocks = GetNumBlocks(); + if (num_blocks > 0) + { + // Initialize min/max for first block + const Bounds *bounds = mSubShapeBounds.data(); + Vec4 min_x = bounds->mMinX; + Vec4 min_y = bounds->mMinY; + Vec4 min_z = bounds->mMinZ; + Vec4 max_x = bounds->mMaxX; + Vec4 max_y = bounds->mMaxY; + Vec4 max_z = bounds->mMaxZ; + + // Accumulate other blocks + const Bounds *bounds_end = bounds + num_blocks; + for (++bounds; bounds < bounds_end; ++bounds) + { + min_x = Vec4::sMin(min_x, bounds->mMinX); + min_y = Vec4::sMin(min_y, bounds->mMinY); + min_z = Vec4::sMin(min_z, bounds->mMinZ); + max_x = Vec4::sMax(max_x, bounds->mMaxX); + max_y = Vec4::sMax(max_y, bounds->mMaxY); + max_z = Vec4::sMax(max_z, bounds->mMaxZ); + } + + // Calculate resulting bounding box + mLocalBounds.mMin.SetX(min_x.ReduceMin()); + mLocalBounds.mMin.SetY(min_y.ReduceMin()); + mLocalBounds.mMin.SetZ(min_z.ReduceMin()); + mLocalBounds.mMax.SetX(max_x.ReduceMax()); + mLocalBounds.mMax.SetY(max_y.ReduceMax()); + mLocalBounds.mMax.SetZ(max_z.ReduceMax()); + } + else + { + // There are no subshapes, set the bounding box to invalid + mLocalBounds.SetEmpty(); + } + + // Cache the inner radius as it can take a while to recursively iterate over all sub shapes + CalculateInnerRadius(); +} + +void MutableCompoundShape::EnsureSubShapeBoundsCapacity() +{ + // Check if we have enough space + uint new_capacity = ((uint)mSubShapes.size() + 3) >> 2; + if (mSubShapeBounds.size() < new_capacity) + mSubShapeBounds.resize(new_capacity); +} + +void MutableCompoundShape::CalculateSubShapeBounds(uint inStartIdx, uint inNumber) +{ + // Ensure that we have allocated the required space for mSubShapeBounds + EnsureSubShapeBoundsCapacity(); + + // Loop over blocks of 4 sub shapes + for (uint sub_shape_idx_start = inStartIdx & ~uint(3), sub_shape_idx_end = inStartIdx + inNumber; sub_shape_idx_start < sub_shape_idx_end; sub_shape_idx_start += 4) + { + Mat44 bounds_min; + Mat44 bounds_max; + + AABox sub_shape_bounds; + for (uint col = 0; col < 4; ++col) + { + uint sub_shape_idx = sub_shape_idx_start + col; + if (sub_shape_idx < mSubShapes.size()) // else reuse sub_shape_bounds from previous iteration + { + const SubShape &sub_shape = mSubShapes[sub_shape_idx]; + + // Transform the shape's bounds into our local space + Mat44 transform = Mat44::sRotationTranslation(sub_shape.GetRotation(), sub_shape.GetPositionCOM()); + + // Get the bounding box + sub_shape_bounds = sub_shape.mShape->GetWorldSpaceBounds(transform, Vec3::sReplicate(1.0f)); + } + + // Put the bounds as columns in a matrix + bounds_min.SetColumn3(col, sub_shape_bounds.mMin); + bounds_max.SetColumn3(col, sub_shape_bounds.mMax); + } + + // Transpose to go to structure of arrays format + Mat44 bounds_min_t = bounds_min.Transposed(); + Mat44 bounds_max_t = bounds_max.Transposed(); + + // Store in our bounds array + Bounds &bounds = mSubShapeBounds[sub_shape_idx_start >> 2]; + bounds.mMinX = bounds_min_t.GetColumn4(0); + bounds.mMinY = bounds_min_t.GetColumn4(1); + bounds.mMinZ = bounds_min_t.GetColumn4(2); + bounds.mMaxX = bounds_max_t.GetColumn4(0); + bounds.mMaxY = bounds_max_t.GetColumn4(1); + bounds.mMaxZ = bounds_max_t.GetColumn4(2); + } + + CalculateLocalBounds(); +} + +uint MutableCompoundShape::AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData) +{ + SubShape sub_shape; + sub_shape.mShape = inShape; + sub_shape.mUserData = inUserData; + sub_shape.SetTransform(inPosition, inRotation, mCenterOfMass); + mSubShapes.push_back(sub_shape); + uint shape_idx = (uint)mSubShapes.size() - 1; + + CalculateSubShapeBounds(shape_idx, 1); + + return shape_idx; +} + +void MutableCompoundShape::RemoveShape(uint inIndex) +{ + mSubShapes.erase(mSubShapes.begin() + inIndex); + + uint num_bounds = (uint)mSubShapes.size() - inIndex; + if (num_bounds > 0) + CalculateSubShapeBounds(inIndex, num_bounds); + else + CalculateLocalBounds(); +} + +void MutableCompoundShape::ModifyShape(uint inIndex, Vec3Arg inPosition, QuatArg inRotation) +{ + SubShape &sub_shape = mSubShapes[inIndex]; + sub_shape.SetTransform(inPosition, inRotation, mCenterOfMass); + + CalculateSubShapeBounds(inIndex, 1); +} + +void MutableCompoundShape::ModifyShape(uint inIndex, Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape) +{ + SubShape &sub_shape = mSubShapes[inIndex]; + sub_shape.mShape = inShape; + sub_shape.SetTransform(inPosition, inRotation, mCenterOfMass); + + CalculateSubShapeBounds(inIndex, 1); +} + +void MutableCompoundShape::ModifyShapes(uint inStartIndex, uint inNumber, const Vec3 *inPositions, const Quat *inRotations, uint inPositionStride, uint inRotationStride) +{ + JPH_ASSERT(inStartIndex + inNumber <= mSubShapes.size()); + + const Vec3 *pos = inPositions; + const Quat *rot = inRotations; + for (SubShape *dest = &mSubShapes[inStartIndex], *dest_end = dest + inNumber; dest < dest_end; ++dest) + { + // Update transform + dest->SetTransform(*pos, *rot, mCenterOfMass); + + // Advance pointer in position / rotation buffer + pos = reinterpret_cast(reinterpret_cast(pos) + inPositionStride); + rot = reinterpret_cast(reinterpret_cast(rot) + inRotationStride); + } + + CalculateSubShapeBounds(inStartIndex, inNumber); +} + +template +inline void MutableCompoundShape::WalkSubShapes(Visitor &ioVisitor) const +{ + // Loop over all blocks of 4 bounding boxes + for (uint block = 0, num_blocks = GetNumBlocks(); block < num_blocks; ++block) + { + // Test the bounding boxes + const Bounds &bounds = mSubShapeBounds[block]; + typename Visitor::Result result = ioVisitor.TestBlock(bounds.mMinX, bounds.mMinY, bounds.mMinZ, bounds.mMaxX, bounds.mMaxY, bounds.mMaxZ); + + // Check if any of the bounding boxes collided + if (ioVisitor.ShouldVisitBlock(result)) + { + // Go through the individual boxes + uint sub_shape_start_idx = block << 2; + for (uint col = 0, max_col = min(4, (uint)mSubShapes.size() - sub_shape_start_idx); col < max_col; ++col) // Don't read beyond the end of the subshapes array + if (ioVisitor.ShouldVisitSubShape(result, col)) // Because the early out fraction can change, we need to retest every shape + { + // Test sub shape + uint sub_shape_idx = sub_shape_start_idx + col; + const SubShape &sub_shape = mSubShapes[sub_shape_idx]; + ioVisitor.VisitShape(sub_shape, sub_shape_idx); + + // If no better collision is available abort + if (ioVisitor.ShouldAbort()) + break; + } + } + } +} + +bool MutableCompoundShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastRayVisitor + { + using CastRayVisitor::CastRayVisitor; + + using Result = Vec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(Vec4Arg inResult) const + { + UVec4 closer = Vec4::sLess(inResult, Vec4::sReplicate(mHit.mFraction)); + return closer.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(Vec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] < mHit.mFraction; + } + }; + + Visitor visitor(inRay, this, inSubShapeIDCreator, ioHit); + WalkSubShapes(visitor); + return visitor.mReturnValue; +} + +void MutableCompoundShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + struct Visitor : public CastRayVisitorCollector + { + using CastRayVisitorCollector::CastRayVisitorCollector; + + using Result = Vec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(Vec4Arg inResult) const + { + UVec4 closer = Vec4::sLess(inResult, Vec4::sReplicate(mCollector.GetEarlyOutFraction())); + return closer.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(Vec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] < mCollector.GetEarlyOutFraction(); + } + }; + + Visitor visitor(inRay, inRayCastSettings, this, inSubShapeIDCreator, ioCollector, inShapeFilter); + WalkSubShapes(visitor); +} + +void MutableCompoundShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + struct Visitor : public CollidePointVisitor + { + using CollidePointVisitor::CollidePointVisitor; + + using Result = UVec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(UVec4Arg inResult) const + { + return inResult.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] != 0; + } + }; + + Visitor visitor(inPoint, this, inSubShapeIDCreator, ioCollector, inShapeFilter); + WalkSubShapes(visitor); +} + +void MutableCompoundShape::sCastShapeVsCompound(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastShapeVisitor + { + using CastShapeVisitor::CastShapeVisitor; + + using Result = Vec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(Vec4Arg inResult) const + { + UVec4 closer = Vec4::sLess(inResult, Vec4::sReplicate(mCollector.GetPositiveEarlyOutFraction())); + return closer.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(Vec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] < mCollector.GetPositiveEarlyOutFraction(); + } + }; + + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::MutableCompound); + const MutableCompoundShape *shape = static_cast(inShape); + + Visitor visitor(inShapeCast, inShapeCastSettings, shape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); + shape->WalkSubShapes(visitor); +} + +void MutableCompoundShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + struct Visitor : public CollectTransformedShapesVisitor + { + using CollectTransformedShapesVisitor::CollectTransformedShapesVisitor; + + using Result = UVec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(UVec4Arg inResult) const + { + return inResult.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] != 0; + } + }; + + Visitor visitor(inBox, this, inPositionCOM, inRotation, inScale, inSubShapeIDCreator, ioCollector, inShapeFilter); + WalkSubShapes(visitor); +} + +int MutableCompoundShape::GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const +{ + JPH_PROFILE_FUNCTION(); + + GetIntersectingSubShapesVisitorMC visitor(inBox, outSubShapeIndices, inMaxSubShapeIndices); + WalkSubShapes(visitor); + return visitor.GetNumResults(); +} + +int MutableCompoundShape::GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const +{ + JPH_PROFILE_FUNCTION(); + + GetIntersectingSubShapesVisitorMC visitor(inBox, outSubShapeIndices, inMaxSubShapeIndices); + WalkSubShapes(visitor); + return visitor.GetNumResults(); +} + +void MutableCompoundShape::sCollideCompoundVsShape(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) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::MutableCompound); + const MutableCompoundShape *shape1 = static_cast(inShape1); + + struct Visitor : public CollideCompoundVsShapeVisitor + { + using CollideCompoundVsShapeVisitor::CollideCompoundVsShapeVisitor; + + using Result = UVec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(UVec4Arg inResult) const + { + return inResult.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] != 0; + } + }; + + Visitor visitor(shape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); + shape1->WalkSubShapes(visitor); +} + +void MutableCompoundShape::sCollideShapeVsCompound(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) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::MutableCompound); + const MutableCompoundShape *shape2 = static_cast(inShape2); + + struct Visitor : public CollideShapeVsCompoundVisitor + { + using CollideShapeVsCompoundVisitor::CollideShapeVsCompoundVisitor; + + using Result = UVec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(UVec4Arg inResult) const + { + return inResult.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] != 0; + } + }; + + Visitor visitor(inShape1, shape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); + shape2->WalkSubShapes(visitor); +} + +void MutableCompoundShape::SaveBinaryState(StreamOut &inStream) const +{ + CompoundShape::SaveBinaryState(inStream); + + // Write bounds + uint bounds_size = (((uint)mSubShapes.size() + 3) >> 2) * sizeof(Bounds); + inStream.WriteBytes(mSubShapeBounds.data(), bounds_size); +} + +void MutableCompoundShape::RestoreBinaryState(StreamIn &inStream) +{ + CompoundShape::RestoreBinaryState(inStream); + + // Ensure that we have allocated the required space for mSubShapeBounds + EnsureSubShapeBoundsCapacity(); + + // Read bounds + uint bounds_size = (((uint)mSubShapes.size() + 3) >> 2) * sizeof(Bounds); + inStream.ReadBytes(mSubShapeBounds.data(), bounds_size); +} + +void MutableCompoundShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::MutableCompound); + f.mConstruct = []() -> Shape * { return new MutableCompoundShape; }; + f.mColor = Color::sDarkOrange; + + for (EShapeSubType s : sAllSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(EShapeSubType::MutableCompound, s, sCollideCompoundVsShape); + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::MutableCompound, sCollideShapeVsCompound); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::MutableCompound, sCastShapeVsCompound); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.h new file mode 100644 index 000000000000..c3f6ff296496 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.h @@ -0,0 +1,169 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; + +/// Class that constructs a MutableCompoundShape. +class JPH_EXPORT MutableCompoundShapeSettings final : public CompoundShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, MutableCompoundShapeSettings) + +public: + // See: ShapeSettings + virtual ShapeResult Create() const override; +}; + +/// A compound shape, sub shapes can be rotated and translated. +/// This shape is optimized for adding / removing and changing the rotation / translation of sub shapes but is less efficient in querying. +/// Shifts all child objects so that they're centered around the center of mass (which needs to be kept up to date by calling AdjustCenterOfMass). +/// +/// Note: If you're using MutableCompoundShape and are querying data while modifying the shape you'll have a race condition. +/// In this case it is best to create a new MutableCompoundShape using the Clone function. You replace the shape on a body using BodyInterface::SetShape. +/// If a query is still working on the old shape, it will have taken a reference and keep the old shape alive until the query finishes. +class JPH_EXPORT MutableCompoundShape final : public CompoundShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + MutableCompoundShape() : CompoundShape(EShapeSubType::MutableCompound) { } + MutableCompoundShape(const MutableCompoundShapeSettings &inSettings, ShapeResult &outResult); + + /// Clone this shape. Can be used to avoid race conditions. See the documentation of this class for more information. + Ref Clone() const; + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See Shape::CollectTransformedShapes + virtual void CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override; + + // See: CompoundShape::GetIntersectingSubShapes + virtual int GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const override; + + // See: CompoundShape::GetIntersectingSubShapes + virtual int GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this) + mSubShapes.size() * sizeof(SubShape) + mSubShapeBounds.size() * sizeof(Bounds), 0); } + + ///@{ + /// @name Mutating shapes. Note that this is not thread safe, so you need to ensure that any bodies that use this shape are locked at the time of modification using BodyLockWrite. After modification you need to call BodyInterface::NotifyShapeChanged to update the broadphase and collision caches. + + /// Adding a new shape. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + /// @return The index of the newly added shape + uint AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData = 0); + + /// Remove a shape by index. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + void RemoveShape(uint inIndex); + + /// Modify the position / orientation of a shape. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + void ModifyShape(uint inIndex, Vec3Arg inPosition, QuatArg inRotation); + + /// Modify the position / orientation and shape at the same time. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + void ModifyShape(uint inIndex, Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape); + + /// @brief Batch set positions / orientations, this avoids duplicate work due to bounding box calculation. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + /// @param inStartIndex Index of first shape to update + /// @param inNumber Number of shapes to update + /// @param inPositions A list of positions with arbitrary stride + /// @param inRotations A list of orientations with arbitrary stride + /// @param inPositionStride The position stride (the number of bytes between the first and second element) + /// @param inRotationStride The orientation stride (the number of bytes between the first and second element) + void ModifyShapes(uint inStartIndex, uint inNumber, const Vec3 *inPositions, const Quat *inRotations, uint inPositionStride = sizeof(Vec3), uint inRotationStride = sizeof(Quat)); + + /// Recalculate the center of mass and shift all objects so they're centered around it + /// (this needs to be done of dynamic bodies and if the center of mass changes significantly due to adding / removing / repositioning sub shapes or else the simulation will look unnatural) + /// Note that after adjusting the center of mass of an object you need to call BodyInterface::NotifyShapeChanged and Constraint::NotifyShapeChanged on the relevant bodies / constraints. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + void AdjustCenterOfMass(); + + ///@} + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Visitor for GetIntersectingSubShapes + template + struct GetIntersectingSubShapesVisitorMC : public GetIntersectingSubShapesVisitor + { + using GetIntersectingSubShapesVisitor::GetIntersectingSubShapesVisitor; + + using Result = UVec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return GetIntersectingSubShapesVisitor::TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(UVec4Arg inResult) const + { + return inResult.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] != 0; + } + }; + + /// Get the number of blocks of 4 bounding boxes + inline uint GetNumBlocks() const { return ((uint)mSubShapes.size() + 3) >> 2; } + + /// Ensure that the mSubShapeBounds has enough space to store bounding boxes equivalent to the number of shapes in mSubShapes + void EnsureSubShapeBoundsCapacity(); + + /// Update mSubShapeBounds + /// @param inStartIdx First sub shape to update + /// @param inNumber Number of shapes to update + void CalculateSubShapeBounds(uint inStartIdx, uint inNumber); + + /// Calculate mLocalBounds from mSubShapeBounds + void CalculateLocalBounds(); + + template + JPH_INLINE void WalkSubShapes(Visitor &ioVisitor) const; ///< Walk the sub shapes and call Visitor::VisitShape for each sub shape encountered + + // Helper functions called by CollisionDispatch + static void sCollideCompoundVsShape(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 sCollideShapeVsCompound(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 sCastShapeVsCompound(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + struct Bounds + { + Vec4 mMinX; + Vec4 mMinY; + Vec4 mMinZ; + Vec4 mMaxX; + Vec4 mMaxY; + Vec4 mMaxZ; + }; + + Array mSubShapeBounds; ///< Bounding boxes of all sub shapes in SOA format (in blocks of 4 boxes), MinX 0..3, MinY 0..3, MinZ 0..3, MaxX 0..3, MaxY 0..3, MaxZ 0..3, MinX 4..7, MinY 4..7, ... +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp new file mode 100644 index 000000000000..8996b46c440a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp @@ -0,0 +1,217 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(OffsetCenterOfMassShapeSettings) +{ + JPH_ADD_BASE_CLASS(OffsetCenterOfMassShapeSettings, DecoratedShapeSettings) + + JPH_ADD_ATTRIBUTE(OffsetCenterOfMassShapeSettings, mOffset) +} + +ShapeSettings::ShapeResult OffsetCenterOfMassShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new OffsetCenterOfMassShape(*this, mCachedResult); + return mCachedResult; +} + +OffsetCenterOfMassShape::OffsetCenterOfMassShape(const OffsetCenterOfMassShapeSettings &inSettings, ShapeResult &outResult) : + DecoratedShape(EShapeSubType::OffsetCenterOfMass, inSettings, outResult), + mOffset(inSettings.mOffset) +{ + if (outResult.HasError()) + return; + + outResult.Set(this); +} + +AABox OffsetCenterOfMassShape::GetLocalBounds() const +{ + AABox bounds = mInnerShape->GetLocalBounds(); + bounds.mMin -= mOffset; + bounds.mMax -= mOffset; + return bounds; +} + +AABox OffsetCenterOfMassShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + return mInnerShape->GetWorldSpaceBounds(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale); +} + +TransformedShape OffsetCenterOfMassShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const +{ + // We don't use any bits in the sub shape ID + outRemainder = inSubShapeID; + + TransformedShape ts(RVec3(inPositionCOM - inRotation * (inScale * mOffset)), inRotation, mInnerShape, BodyID()); + ts.SetShapeScale(inScale); + return ts; +} + +Vec3 OffsetCenterOfMassShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + // Transform surface position to local space and pass call on + return mInnerShape->GetSurfaceNormal(inSubShapeID, inLocalSurfacePosition + mOffset); +} + +void OffsetCenterOfMassShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + mInnerShape->GetSupportingFace(inSubShapeID, inDirection, inScale, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), outVertices); +} + +void OffsetCenterOfMassShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + mInnerShape->GetSubmergedVolume(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset)); +} + +#ifdef JPH_DEBUG_RENDERER +void OffsetCenterOfMassShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + mInnerShape->Draw(inRenderer, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inColor, inUseMaterialColors, inDrawWireframe); +} + +void OffsetCenterOfMassShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const +{ + mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inColor, inDrawSupportDirection); +} + +void OffsetCenterOfMassShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale); +} +#endif // JPH_DEBUG_RENDERER + +bool OffsetCenterOfMassShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + // Transform the ray to local space + RayCast ray = inRay; + ray.mOrigin += mOffset; + + return mInnerShape->CastRay(ray, inSubShapeIDCreator, ioHit); +} + +void OffsetCenterOfMassShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Transform the ray to local space + RayCast ray = inRay; + ray.mOrigin += mOffset; + + return mInnerShape->CastRay(ray, inRayCastSettings, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void OffsetCenterOfMassShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Pass the point on to the inner shape in local space + mInnerShape->CollidePoint(inPoint + mOffset, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void OffsetCenterOfMassShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inVertices, inNumVertices, inCollidingShapeIndex); +} + +void OffsetCenterOfMassShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + mInnerShape->CollectTransformedShapes(inBox, inPositionCOM - inRotation * (inScale * mOffset), inRotation, inScale, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void OffsetCenterOfMassShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const +{ + mInnerShape->TransformShape(inCenterOfMassTransform.PreTranslated(-mOffset), ioCollector); +} + +void OffsetCenterOfMassShape::sCollideOffsetCenterOfMassVsShape(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) +{ + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::OffsetCenterOfMass); + const OffsetCenterOfMassShape *shape1 = static_cast(inShape1); + + CollisionDispatch::sCollideShapeVsShape(shape1->mInnerShape, inShape2, inScale1, inScale2, inCenterOfMassTransform1.PreTranslated(-inScale1 * shape1->mOffset), inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); +} + +void OffsetCenterOfMassShape::sCollideShapeVsOffsetCenterOfMass(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) +{ + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::OffsetCenterOfMass); + const OffsetCenterOfMassShape *shape2 = static_cast(inShape2); + + CollisionDispatch::sCollideShapeVsShape(inShape1, shape2->mInnerShape, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2.PreTranslated(-inScale2 * shape2->mOffset), inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); +} + +void OffsetCenterOfMassShape::sCastOffsetCenterOfMassVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + // Fetch offset center of mass shape from cast shape + JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::OffsetCenterOfMass); + const OffsetCenterOfMassShape *shape1 = static_cast(inShapeCast.mShape); + + // Transform the shape cast and update the shape + ShapeCast shape_cast(shape1->mInnerShape, inShapeCast.mScale, inShapeCast.mCenterOfMassStart.PreTranslated(-inShapeCast.mScale * shape1->mOffset), inShapeCast.mDirection); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); +} + +void OffsetCenterOfMassShape::sCastShapeVsOffsetCenterOfMass(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::OffsetCenterOfMass); + const OffsetCenterOfMassShape *shape = static_cast(inShape); + + // Transform the shape cast + ShapeCast shape_cast = inShapeCast.PostTransformed(Mat44::sTranslation(inScale * shape->mOffset)); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, shape->mInnerShape, inScale, inShapeFilter, inCenterOfMassTransform2.PreTranslated(-inScale * shape->mOffset), inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); +} + +void OffsetCenterOfMassShape::SaveBinaryState(StreamOut &inStream) const +{ + DecoratedShape::SaveBinaryState(inStream); + + inStream.Write(mOffset); +} + +void OffsetCenterOfMassShape::RestoreBinaryState(StreamIn &inStream) +{ + DecoratedShape::RestoreBinaryState(inStream); + + inStream.Read(mOffset); +} + +void OffsetCenterOfMassShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::OffsetCenterOfMass); + f.mConstruct = []() -> Shape * { return new OffsetCenterOfMassShape; }; + f.mColor = Color::sCyan; + + for (EShapeSubType s : sAllSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(EShapeSubType::OffsetCenterOfMass, s, sCollideOffsetCenterOfMassVsShape); + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::OffsetCenterOfMass, sCollideShapeVsOffsetCenterOfMass); + CollisionDispatch::sRegisterCastShape(EShapeSubType::OffsetCenterOfMass, s, sCastOffsetCenterOfMassVsShape); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::OffsetCenterOfMass, sCastShapeVsOffsetCenterOfMass); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h new file mode 100644 index 000000000000..7ba6a5a7a01d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h @@ -0,0 +1,140 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; + +/// Class that constructs an OffsetCenterOfMassShape +class JPH_EXPORT OffsetCenterOfMassShapeSettings final : public DecoratedShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, OffsetCenterOfMassShapeSettings) + +public: + /// Constructor + OffsetCenterOfMassShapeSettings() = default; + + /// Construct with shape settings, can be serialized. + OffsetCenterOfMassShapeSettings(Vec3Arg inOffset, const ShapeSettings *inShape) : DecoratedShapeSettings(inShape), mOffset(inOffset) { } + + /// Variant that uses a concrete shape, which means this object cannot be serialized. + OffsetCenterOfMassShapeSettings(Vec3Arg inOffset, const Shape *inShape): DecoratedShapeSettings(inShape), mOffset(inOffset) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + Vec3 mOffset; ///< Offset to be applied to the center of mass of the child shape +}; + +/// This shape will shift the center of mass of a child shape, it can e.g. be used to lower the center of mass of an unstable object like a boat to make it stable +class JPH_EXPORT OffsetCenterOfMassShape final : public DecoratedShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + OffsetCenterOfMassShape() : DecoratedShape(EShapeSubType::OffsetCenterOfMass) { } + OffsetCenterOfMassShape(const OffsetCenterOfMassShapeSettings &inSettings, ShapeResult &outResult); + OffsetCenterOfMassShape(const Shape *inShape, Vec3Arg inOffset) : DecoratedShape(EShapeSubType::OffsetCenterOfMass, inShape), mOffset(inOffset) { } + + /// Access the offset that is applied to the center of mass + Vec3 GetOffset() const { return mOffset; } + + // See Shape::GetCenterOfMass + virtual Vec3 GetCenterOfMass() const override { return mInnerShape->GetCenterOfMass() + mOffset; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mInnerShape->GetInnerRadius(); } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override + { + MassProperties mp = mInnerShape->GetMassProperties(); + mp.Translate(mOffset); + return mp; + } + + // See Shape::GetSubShapeTransformedShape + virtual TransformedShape GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; + + // See Shape::DrawGetSupportFunction + virtual void DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override; + + // See Shape::DrawGetSupportingFace + virtual void DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::CollectTransformedShapes + virtual void CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override; + + // See Shape::TransformShape + virtual void TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); } + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); return 0; } + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return mInnerShape->GetVolume(); } + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Helper functions called by CollisionDispatch + static void sCollideOffsetCenterOfMassVsShape(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 sCollideShapeVsOffsetCenterOfMass(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 sCastOffsetCenterOfMassVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastShapeVsOffsetCenterOfMass(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + Vec3 mOffset; ///< Offset of the center of mass +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.cpp new file mode 100644 index 000000000000..c1401fd7fcc8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.cpp @@ -0,0 +1,541 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PlaneShapeSettings) +{ + JPH_ADD_BASE_CLASS(PlaneShapeSettings, ShapeSettings) + + JPH_ADD_ATTRIBUTE(PlaneShapeSettings, mPlane) + JPH_ADD_ATTRIBUTE(PlaneShapeSettings, mMaterial) + JPH_ADD_ATTRIBUTE(PlaneShapeSettings, mHalfExtent) +} + +ShapeSettings::ShapeResult PlaneShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new PlaneShape(*this, mCachedResult); + return mCachedResult; +} + +inline static void sPlaneGetOrthogonalBasis(Vec3Arg inNormal, Vec3 &outPerp1, Vec3 &outPerp2) +{ + outPerp1 = inNormal.Cross(Vec3::sAxisY()).NormalizedOr(Vec3::sAxisX()); + outPerp2 = outPerp1.Cross(inNormal).Normalized(); + outPerp1 = inNormal.Cross(outPerp2); +} + +void PlaneShape::GetVertices(Vec3 *outVertices) const +{ + // Create orthogonal basis + Vec3 normal = mPlane.GetNormal(); + Vec3 perp1, perp2; + sPlaneGetOrthogonalBasis(normal, perp1, perp2); + + // Scale basis + perp1 *= mHalfExtent; + perp2 *= mHalfExtent; + + // Calculate corners + Vec3 point = -normal * mPlane.GetConstant(); + outVertices[0] = point + perp1 + perp2; + outVertices[1] = point + perp1 - perp2; + outVertices[2] = point - perp1 - perp2; + outVertices[3] = point - perp1 + perp2; +} + +void PlaneShape::CalculateLocalBounds() +{ + // Get the vertices of the plane + Vec3 vertices[4]; + GetVertices(vertices); + + // Encapsulate the vertices and a point mHalfExtent behind the plane + mLocalBounds = AABox(); + Vec3 normal = mPlane.GetNormal(); + for (const Vec3 &v : vertices) + { + mLocalBounds.Encapsulate(v); + mLocalBounds.Encapsulate(v - mHalfExtent * normal); + } +} + +PlaneShape::PlaneShape(const PlaneShapeSettings &inSettings, ShapeResult &outResult) : + Shape(EShapeType::Plane, EShapeSubType::Plane, inSettings, outResult), + mPlane(inSettings.mPlane), + mMaterial(inSettings.mMaterial), + mHalfExtent(inSettings.mHalfExtent) +{ + if (!mPlane.GetNormal().IsNormalized()) + { + outResult.SetError("Plane normal needs to be normalized!"); + return; + } + + CalculateLocalBounds(); + + outResult.Set(this); +} + +MassProperties PlaneShape::GetMassProperties() const +{ + // Object should always be static, return default mass properties + return MassProperties(); +} + +void PlaneShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + // Get the vertices of the plane + Vec3 vertices[4]; + GetVertices(vertices); + + // Reverse if scale is inside out + if (ScaleHelpers::IsInsideOut(inScale)) + { + std::swap(vertices[0], vertices[3]); + std::swap(vertices[1], vertices[2]); + } + + // Transform them to world space + outVertices.clear(); + Mat44 com = inCenterOfMassTransform.PreScaled(inScale); + for (const Vec3 &v : vertices) + outVertices.push_back(com * v); +} + +#ifdef JPH_DEBUG_RENDERER +void PlaneShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + // Get the vertices of the plane + Vec3 local_vertices[4]; + GetVertices(local_vertices); + + // Reverse if scale is inside out + if (ScaleHelpers::IsInsideOut(inScale)) + { + std::swap(local_vertices[0], local_vertices[3]); + std::swap(local_vertices[1], local_vertices[2]); + } + + // Transform them to world space + RMat44 com = inCenterOfMassTransform.PreScaled(inScale); + RVec3 vertices[4]; + for (uint i = 0; i < 4; ++i) + vertices[i] = com * local_vertices[i]; + + // Determine the color + Color color = inUseMaterialColors? GetMaterial(SubShapeID())->GetDebugColor() : inColor; + + // Draw the plane + if (inDrawWireframe) + { + inRenderer->DrawWireTriangle(vertices[0], vertices[1], vertices[2], color); + inRenderer->DrawWireTriangle(vertices[0], vertices[2], vertices[3], color); + } + else + { + inRenderer->DrawTriangle(vertices[0], vertices[1], vertices[2], color, DebugRenderer::ECastShadow::On); + inRenderer->DrawTriangle(vertices[0], vertices[2], vertices[3], color, DebugRenderer::ECastShadow::On); + } +} +#endif // JPH_DEBUG_RENDERER + +bool PlaneShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + JPH_PROFILE_FUNCTION(); + + // Test starting inside of negative half space + float distance = mPlane.SignedDistance(inRay.mOrigin); + if (distance <= 0.0f) + { + ioHit.mFraction = 0.0f; + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + + // Test ray parallel to plane + float dot = inRay.mDirection.Dot(mPlane.GetNormal()); + if (dot == 0.0f) + return false; + + // Calculate hit fraction + float fraction = -distance / dot; + if (fraction >= 0.0f && fraction < ioHit.mFraction) + { + ioHit.mFraction = fraction; + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + + return false; +} + +void PlaneShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Inside solid half space? + float distance = mPlane.SignedDistance(inRay.mOrigin); + if (inRayCastSettings.mTreatConvexAsSolid + && distance <= 0.0f // Inside plane + && ioCollector.GetEarlyOutFraction() > 0.0f) // Willing to accept hits at fraction 0 + { + // Hit at fraction 0 + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + hit.mFraction = 0.0f; + hit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + ioCollector.AddHit(hit); + } + + float dot = inRay.mDirection.Dot(mPlane.GetNormal()); + if (dot != 0.0f // Parallel ray will not hit plane + && (inRayCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces || dot < 0.0f)) // Back face culling + { + // Calculate hit with plane + float fraction = -distance / dot; + if (fraction >= 0.0f && fraction < ioCollector.GetEarlyOutFraction()) + { + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + hit.mFraction = fraction; + hit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + ioCollector.AddHit(hit); + } + } +} + +void PlaneShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Check if the point is inside the plane + if (mPlane.SignedDistance(inPoint) < 0.0f) + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); +} + +void PlaneShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + JPH_PROFILE_FUNCTION(); + + // Convert plane to world space + Plane plane = mPlane.Scaled(inScale).GetTransformed(inCenterOfMassTransform); + + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) + { + // Calculate penetration + float penetration = -plane.SignedDistance(v.GetPosition()); + if (v.UpdatePenetration(penetration)) + v.SetCollision(plane, inCollidingShapeIndex); + } +} + +// This is a version of GetSupportingFace that returns a face that is large enough to cover the shape we're colliding with but not as large as the regular GetSupportedFace to avoid numerical precision issues +inline static void sGetSupportingFace(const ConvexShape *inShape, Vec3Arg inShapeCOM, const Plane &inPlane, Mat44Arg inPlaneToWorld, ConvexShape::SupportingFace &outPlaneFace) +{ + // Project COM of shape onto plane + Plane world_plane = inPlane.GetTransformed(inPlaneToWorld); + Vec3 center = world_plane.ProjectPointOnPlane(inShapeCOM); + + // Create orthogonal basis for the plane + Vec3 normal = world_plane.GetNormal(); + Vec3 perp1, perp2; + sPlaneGetOrthogonalBasis(normal, perp1, perp2); + + // Base the size of the face on the bounding box of the shape, ensuring that it is large enough to cover the entire shape + float size = inShape->GetLocalBounds().GetSize().Length(); + perp1 *= size; + perp2 *= size; + + // Emit the vertices + outPlaneFace.resize(4); + outPlaneFace[0] = center + perp1 + perp2; + outPlaneFace[1] = center + perp1 - perp2; + outPlaneFace[2] = center - perp1 - perp2; + outPlaneFace[3] = center - perp1 + perp2; +} + +void PlaneShape::sCastConvexVsPlane(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + // Get the shapes + JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Convex); + JPH_ASSERT(inShape->GetType() == EShapeType::Plane); + const ConvexShape *convex_shape = static_cast(inShapeCast.mShape); + const PlaneShape *plane_shape = static_cast(inShape); + + // Shape cast is provided relative to COM of inShape, so all we need to do is transform our plane with inScale + Plane plane = plane_shape->mPlane.Scaled(inScale); + Vec3 normal = plane.GetNormal(); + + // Get support function + ConvexShape::SupportBuffer shape1_support_buffer; + const ConvexShape::Support *shape1_support = convex_shape->GetSupportFunction(ConvexShape::ESupportMode::Default, shape1_support_buffer, inShapeCast.mScale); + + // Get the support point of the convex shape in the opposite direction of the plane normal in our local space + Vec3 normal_in_convex_shape_space = inShapeCast.mCenterOfMassStart.Multiply3x3Transposed(normal); + Vec3 support_point = inShapeCast.mCenterOfMassStart * shape1_support->GetSupport(-normal_in_convex_shape_space); + float signed_distance = plane.SignedDistance(support_point); + float convex_radius = shape1_support->GetConvexRadius(); + float penetration_depth = -signed_distance + convex_radius; + float dot = inShapeCast.mDirection.Dot(normal); + + // Collision output + Mat44 com_hit; + Vec3 point1, point2; + float fraction; + + // Do we start in collision? + if (penetration_depth > 0.0f) + { + // Back face culling? + if (inShapeCastSettings.mBackFaceModeConvex == EBackFaceMode::IgnoreBackFaces && dot > 0.0f) + return; + + // Shallower hit? + if (penetration_depth <= -ioCollector.GetEarlyOutFraction()) + return; + + // We're hitting at fraction 0 + fraction = 0.0f; + + // Get contact point + com_hit = inCenterOfMassTransform2; + point1 = inCenterOfMassTransform2 * (support_point - normal * convex_radius); + point2 = inCenterOfMassTransform2 * (support_point - normal * signed_distance); + } + else if (dot < 0.0f) // Moving towards the plane? + { + // Calculate hit fraction + fraction = penetration_depth / dot; + JPH_ASSERT(fraction >= 0.0f); + + // Further than early out fraction? + if (fraction >= ioCollector.GetEarlyOutFraction()) + return; + + // Get contact point + com_hit = inCenterOfMassTransform2.PostTranslated(fraction * inShapeCast.mDirection); + point1 = point2 = com_hit * (support_point - normal * convex_radius); + } + else + { + // Moving away from the plane + return; + } + + // Create cast result + Vec3 penetration_axis_world = com_hit.Multiply3x3(-normal); + bool back_facing = dot > 0.0f; + ShapeCastResult result(fraction, point1, point2, penetration_axis_world, back_facing, inSubShapeIDCreator1.GetID(), inSubShapeIDCreator2.GetID(), TransformedShape::sGetBodyID(ioCollector.GetContext())); + + // Gather faces + if (inShapeCastSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces) + { + // Get supporting face of convex shape + Mat44 shape_to_world = com_hit * inShapeCast.mCenterOfMassStart; + convex_shape->GetSupportingFace(SubShapeID(), normal_in_convex_shape_space, inShapeCast.mScale, shape_to_world, result.mShape1Face); + + // Get supporting face of plane + if (!result.mShape1Face.empty()) + sGetSupportingFace(convex_shape, shape_to_world.GetTranslation(), plane, inCenterOfMassTransform2, result.mShape2Face); + } + + // Notify the collector + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) + ioCollector.AddHit(result); +} + +struct PlaneShape::PSGetTrianglesContext +{ + Float3 mVertices[4]; + bool mDone = false; +}; + +void PlaneShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + static_assert(sizeof(PSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(&ioContext, alignof(PSGetTrianglesContext))); + + PSGetTrianglesContext *context = new (&ioContext) PSGetTrianglesContext(); + + // Get the vertices of the plane + Vec3 vertices[4]; + GetVertices(vertices); + + // Reverse if scale is inside out + if (ScaleHelpers::IsInsideOut(inScale)) + { + std::swap(vertices[0], vertices[3]); + std::swap(vertices[1], vertices[2]); + } + + // Transform them to world space + Mat44 com = Mat44::sRotationTranslation(inRotation, inPositionCOM).PreScaled(inScale); + for (uint i = 0; i < 4; ++i) + (com * vertices[i]).StoreFloat3(&context->mVertices[i]); +} + +int PlaneShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + static_assert(cGetTrianglesMinTrianglesRequested >= 2, "cGetTrianglesMinTrianglesRequested is too small"); + JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested); + + // Check if we're done + PSGetTrianglesContext &context = (PSGetTrianglesContext &)ioContext; + if (context.mDone) + return 0; + context.mDone = true; + + // 1st triangle + outTriangleVertices[0] = context.mVertices[0]; + outTriangleVertices[1] = context.mVertices[1]; + outTriangleVertices[2] = context.mVertices[2]; + + // 2nd triangle + outTriangleVertices[3] = context.mVertices[0]; + outTriangleVertices[4] = context.mVertices[2]; + outTriangleVertices[5] = context.mVertices[3]; + + if (outMaterials != nullptr) + { + // Get material + const PhysicsMaterial *material = GetMaterial(SubShapeID()); + outMaterials[0] = material; + outMaterials[1] = material; + } + + return 2; +} + +void PlaneShape::sCollideConvexVsPlane(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, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + // Get the shapes + JPH_ASSERT(inShape1->GetType() == EShapeType::Convex); + JPH_ASSERT(inShape2->GetType() == EShapeType::Plane); + const ConvexShape *shape1 = static_cast(inShape1); + const PlaneShape *shape2 = static_cast(inShape2); + + // Transform the plane to the space of the convex shape + Plane scaled_plane = shape2->mPlane.Scaled(inScale2); + Plane plane = scaled_plane.GetTransformed(inCenterOfMassTransform1.InversedRotationTranslation() * inCenterOfMassTransform2); + Vec3 normal = plane.GetNormal(); + + // Get support function + ConvexShape::SupportBuffer shape1_support_buffer; + const ConvexShape::Support *shape1_support = shape1->GetSupportFunction(ConvexShape::ESupportMode::Default, shape1_support_buffer, inScale1); + + // Get the support point of the convex shape in the opposite direction of the plane normal + Vec3 support_point = shape1_support->GetSupport(-normal); + float signed_distance = plane.SignedDistance(support_point); + float convex_radius = shape1_support->GetConvexRadius(); + float penetration_depth = -signed_distance + convex_radius; + if (penetration_depth > -inCollideShapeSettings.mMaxSeparationDistance) + { + // Get contact point + Vec3 point1 = inCenterOfMassTransform1 * (support_point - normal * convex_radius); + Vec3 point2 = inCenterOfMassTransform1 * (support_point - normal * signed_distance); + Vec3 penetration_axis_world = inCenterOfMassTransform1.Multiply3x3(-normal); + + // Create collision result + CollideShapeResult result(point1, point2, penetration_axis_world, penetration_depth, inSubShapeIDCreator1.GetID(), inSubShapeIDCreator2.GetID(), TransformedShape::sGetBodyID(ioCollector.GetContext())); + + // Gather faces + if (inCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces) + { + // Get supporting face of shape 1 + shape1->GetSupportingFace(SubShapeID(), normal, inScale1, inCenterOfMassTransform1, result.mShape1Face); + + // Get supporting face of shape 2 + if (!result.mShape1Face.empty()) + sGetSupportingFace(shape1, inCenterOfMassTransform1.GetTranslation(), scaled_plane, inCenterOfMassTransform2, result.mShape2Face); + } + + // Notify the collector + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) + ioCollector.AddHit(result); + } +} + +void PlaneShape::SaveBinaryState(StreamOut &inStream) const +{ + Shape::SaveBinaryState(inStream); + + inStream.Write(mPlane); + inStream.Write(mHalfExtent); +} + +void PlaneShape::RestoreBinaryState(StreamIn &inStream) +{ + Shape::RestoreBinaryState(inStream); + + inStream.Read(mPlane); + inStream.Read(mHalfExtent); + + CalculateLocalBounds(); +} + +void PlaneShape::SaveMaterialState(PhysicsMaterialList &outMaterials) const +{ + outMaterials = { mMaterial }; +} + +void PlaneShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) +{ + JPH_ASSERT(inNumMaterials == 1); + mMaterial = inMaterials[0]; +} + +void PlaneShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Plane); + f.mConstruct = []() -> Shape * { return new PlaneShape; }; + f.mColor = Color::sDarkRed; + + for (EShapeSubType s : sConvexSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Plane, sCollideConvexVsPlane); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Plane, sCastConvexVsPlane); + + CollisionDispatch::sRegisterCastShape(EShapeSubType::Plane, s, CollisionDispatch::sReversedCastShape); + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Plane, s, CollisionDispatch::sReversedCollideShape); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.h new file mode 100644 index 000000000000..c9a7810b7445 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.h @@ -0,0 +1,143 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; + +/// Class that constructs a PlaneShape +class JPH_EXPORT PlaneShapeSettings final : public ShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PlaneShapeSettings) + +public: + /// Default constructor for deserialization + PlaneShapeSettings() = default; + + /// Create a plane shape. + PlaneShapeSettings(const Plane &inPlane, const PhysicsMaterial *inMaterial = nullptr, float inHalfExtent = cDefaultHalfExtent) : mPlane(inPlane), mMaterial(inMaterial), mHalfExtent(inHalfExtent) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + Plane mPlane; ///< Plane that describes the shape. The negative half space is considered solid. + + RefConst mMaterial; ///< Surface material of the plane + + static constexpr float cDefaultHalfExtent = 1000.0f; ///< Default half-extent of the plane (total size along 1 axis will be 2 * half-extent) + + float mHalfExtent = cDefaultHalfExtent; ///< The bounding box of this plane will run from [-half_extent, half_extent]. Keep this as low as possible for better broad phase performance. +}; + +/// A plane shape. The negative half space is considered solid. Planes cannot be dynamic objects, only static or kinematic. +/// The plane is considered an infinite shape, but testing collision outside of its bounding box (defined by the half-extent parameter) will not return a collision result. +/// At the edge of the bounding box collision with the plane will be inconsistent. If you need something of a well defined size, a box shape may be better. +class JPH_EXPORT PlaneShape final : public Shape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + PlaneShape() : Shape(EShapeType::Plane, EShapeSubType::Plane) { } + PlaneShape(const Plane &inPlane, const PhysicsMaterial *inMaterial = nullptr, float inHalfExtent = PlaneShapeSettings::cDefaultHalfExtent) : Shape(EShapeType::Plane, EShapeSubType::Plane), mPlane(inPlane), mMaterial(inMaterial), mHalfExtent(inHalfExtent) { CalculateLocalBounds(); } + PlaneShape(const PlaneShapeSettings &inSettings, ShapeResult &outResult); + + /// Get the plane + const Plane & GetPlane() const { return mPlane; } + + /// Get the half-extent of the bounding box of the plane + float GetHalfExtent() const { return mHalfExtent; } + + // See Shape::MustBeStatic + virtual bool MustBeStatic() const override { return true; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override { return mLocalBounds; } + + // See Shape::GetSubShapeIDBitsRecursive + virtual uint GetSubShapeIDBitsRecursive() const override { return 0; } + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return 0.0f; } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetMaterial + virtual const PhysicsMaterial * GetMaterial(const SubShapeID &inSubShapeID) const override { JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); return mMaterial != nullptr? mMaterial : PhysicsMaterial::sDefault; } + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override { JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); return mPlane.GetNormal(); } + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override { JPH_ASSERT(false, "Not supported"); } + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void SaveMaterialState(PhysicsMaterialList &outMaterials) const override; + virtual void RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return 0; } + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + struct PSGetTrianglesContext; ///< Context class for GetTrianglesStart/Next + + // Get 4 vertices that form the plane + void GetVertices(Vec3 *outVertices) const; + + // Cache the local bounds + void CalculateLocalBounds(); + + // Helper functions called by CollisionDispatch + static void sCollideConvexVsPlane(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 sCastConvexVsPlane(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + Plane mPlane; + RefConst mMaterial; + float mHalfExtent; + AABox mLocalBounds; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PolyhedronSubmergedVolumeCalculator.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PolyhedronSubmergedVolumeCalculator.h new file mode 100644 index 000000000000..96e69f0b19be --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PolyhedronSubmergedVolumeCalculator.h @@ -0,0 +1,319 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +/// This class calculates the intersection between a fluid surface and a polyhedron and returns the submerged volume and its center of buoyancy +/// Construct this class and then one by one add all faces of the polyhedron using the AddFace function. After all faces have been added the result +/// can be gotten through GetResult. +class PolyhedronSubmergedVolumeCalculator +{ +private: + // Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 4 vertices submerged + // inV1 .. inV4 are submerged + inline static void sTetrahedronVolume4(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, Vec3Arg inV4, float &outVolumeTimes6, Vec3 &outCenterTimes4) + { + // Calculate center of mass and mass of this tetrahedron, + // see: https://en.wikipedia.org/wiki/Tetrahedron#Volume + outVolumeTimes6 = max((inV1 - inV4).Dot((inV2 - inV4).Cross(inV3 - inV4)), 0.0f); // All contributions should be positive because we use a reference point that is on the surface of the hull + outCenterTimes4 = inV1 + inV2 + inV3 + inV4; + } + + // Get the intersection point with a plane. + // inV1 is inD1 distance away from the plane, inV2 is inD2 distance away from the plane + inline static Vec3 sGetPlaneIntersection(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2) + { + JPH_ASSERT(Sign(inD1) != Sign(inD2), "Assuming both points are on opposite ends of the plane"); + float delta = inD1 - inD2; + if (abs(delta) < 1.0e-6f) + return inV1; // Parallel to plane, just pick a point + else + return inV1 + inD1 * (inV2 - inV1) / delta; + } + + // Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 1 vertex submerged + // inV1 is submerged, inV2 .. inV4 are not + // inD1 .. inD4 are the distances from the points to the plane + inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume1(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4) + { + // A tetrahedron with 1 point submerged is cut along 3 edges forming a new tetrahedron + Vec3 v2 = sGetPlaneIntersection(inV1, inD1, inV2, inD2); + Vec3 v3 = sGetPlaneIntersection(inV1, inD1, inV3, inD3); + Vec3 v4 = sGetPlaneIntersection(inV1, inD1, inV4, inD4); + + #ifdef JPH_DEBUG_RENDERER + // Draw intersection between tetrahedron and surface + if (Shape::sDrawSubmergedVolumes) + { + RVec3 v2w = mBaseOffset + v2; + RVec3 v3w = mBaseOffset + v3; + RVec3 v4w = mBaseOffset + v4; + + DebugRenderer::sInstance->DrawTriangle(v4w, v3w, v2w, Color::sGreen); + DebugRenderer::sInstance->DrawWireTriangle(v4w, v3w, v2w, Color::sWhite); + } + #endif // JPH_DEBUG_RENDERER + + sTetrahedronVolume4(inV1, v2, v3, v4, outVolumeTimes6, outCenterTimes4); + } + + // Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 2 vertices submerged + // inV1, inV2 are submerged, inV3, inV4 are not + // inD1 .. inD4 are the distances from the points to the plane + inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume2(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4) + { + // A tetrahedron with 2 points submerged is cut along 4 edges forming a quad + Vec3 c = sGetPlaneIntersection(inV1, inD1, inV3, inD3); + Vec3 d = sGetPlaneIntersection(inV1, inD1, inV4, inD4); + Vec3 e = sGetPlaneIntersection(inV2, inD2, inV4, inD4); + Vec3 f = sGetPlaneIntersection(inV2, inD2, inV3, inD3); + + #ifdef JPH_DEBUG_RENDERER + // Draw intersection between tetrahedron and surface + if (Shape::sDrawSubmergedVolumes) + { + RVec3 cw = mBaseOffset + c; + RVec3 dw = mBaseOffset + d; + RVec3 ew = mBaseOffset + e; + RVec3 fw = mBaseOffset + f; + + DebugRenderer::sInstance->DrawTriangle(cw, ew, dw, Color::sGreen); + DebugRenderer::sInstance->DrawTriangle(cw, fw, ew, Color::sGreen); + DebugRenderer::sInstance->DrawWireTriangle(cw, ew, dw, Color::sWhite); + DebugRenderer::sInstance->DrawWireTriangle(cw, fw, ew, Color::sWhite); + } + #endif // JPH_DEBUG_RENDERER + + // We pick point c as reference (which is on the cut off surface) + // This leaves us with three tetrahedrons to sum up (any faces that are in the same plane as c will have zero volume) + Vec3 center1, center2, center3; + float volume1, volume2, volume3; + sTetrahedronVolume4(e, f, inV2, c, volume1, center1); + sTetrahedronVolume4(e, inV1, d, c, volume2, center2); + sTetrahedronVolume4(e, inV2, inV1, c, volume3, center3); + + // Tally up the totals + outVolumeTimes6 = volume1 + volume2 + volume3; + outCenterTimes4 = outVolumeTimes6 > 0.0f? (volume1 * center1 + volume2 * center2 + volume3 * center3) / outVolumeTimes6 : Vec3::sZero(); + } + + // Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 3 vertices submerged + // inV1, inV2, inV3 are submerged, inV4 is not + // inD1 .. inD4 are the distances from the points to the plane + inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume3(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4) + { + // A tetrahedron with 1 point above the surface is cut along 3 edges forming a new tetrahedron + Vec3 v1 = sGetPlaneIntersection(inV1, inD1, inV4, inD4); + Vec3 v2 = sGetPlaneIntersection(inV2, inD2, inV4, inD4); + Vec3 v3 = sGetPlaneIntersection(inV3, inD3, inV4, inD4); + + #ifdef JPH_DEBUG_RENDERER + // Draw intersection between tetrahedron and surface + if (Shape::sDrawSubmergedVolumes) + { + RVec3 v1w = mBaseOffset + v1; + RVec3 v2w = mBaseOffset + v2; + RVec3 v3w = mBaseOffset + v3; + + DebugRenderer::sInstance->DrawTriangle(v3w, v2w, v1w, Color::sGreen); + DebugRenderer::sInstance->DrawWireTriangle(v3w, v2w, v1w, Color::sWhite); + } + #endif // JPH_DEBUG_RENDERER + + Vec3 dry_center, total_center; + float dry_volume, total_volume; + + // We first calculate the part that is above the surface + sTetrahedronVolume4(v1, v2, v3, inV4, dry_volume, dry_center); + + // Calculate the total volume + sTetrahedronVolume4(inV1, inV2, inV3, inV4, total_volume, total_center); + + // From this we can calculate the center and volume of the submerged part + outVolumeTimes6 = max(total_volume - dry_volume, 0.0f); + outCenterTimes4 = outVolumeTimes6 > 0.0f? (total_center * total_volume - dry_center * dry_volume) / outVolumeTimes6 : Vec3::sZero(); + } + +public: + /// A helper class that contains cached information about a polyhedron vertex + class Point + { + public: + Vec3 mPosition; ///< World space position of vertex + float mDistanceToSurface; ///< Signed distance to the surface (> 0 is above, < 0 is below) + bool mAboveSurface; ///< If the point is above the surface (mDistanceToSurface > 0) + }; + + /// Constructor + /// @param inTransform Transform to transform all incoming points with + /// @param inPoints Array of points that are part of the polyhedron + /// @param inPointStride Amount of bytes between each point (should usually be sizeof(Vec3)) + /// @param inNumPoints The amount of points + /// @param inSurface The plane that forms the fluid surface (normal should point up) + /// @param ioBuffer A temporary buffer of Point's that should have inNumPoints entries and should stay alive while this class is alive +#ifdef JPH_DEBUG_RENDERER + /// @param inBaseOffset The offset to transform inTransform to world space (in double precision mode this can be used to shift the whole operation closer to the origin). Only used for debug drawing. +#endif // JPH_DEBUG_RENDERER + PolyhedronSubmergedVolumeCalculator(const Mat44 &inTransform, const Vec3 *inPoints, int inPointStride, int inNumPoints, const Plane &inSurface, Point *ioBuffer +#ifdef JPH_DEBUG_RENDERER // Not using JPH_IF_DEBUG_RENDERER for Doxygen + , RVec3 inBaseOffset +#endif // JPH_DEBUG_RENDERER + ) : + mPoints(ioBuffer) +#ifdef JPH_DEBUG_RENDERER + , mBaseOffset(inBaseOffset) +#endif // JPH_DEBUG_RENDERER + { + // Convert the points to world space and determine the distance to the surface + float reference_dist = FLT_MAX; + for (int p = 0; p < inNumPoints; ++p) + { + // Calculate values + Vec3 transformed_point = inTransform * *reinterpret_cast(reinterpret_cast(inPoints) + p * inPointStride); + float dist = inSurface.SignedDistance(transformed_point); + bool above = dist >= 0.0f; + + // Keep track if all are above or below + mAllAbove &= above; + mAllBelow &= !above; + + // Calculate lowest point, we use this to create tetrahedrons out of all faces + if (reference_dist > dist) + { + mReferencePointIdx = p; + reference_dist = dist; + } + + // Store values + ioBuffer->mPosition = transformed_point; + ioBuffer->mDistanceToSurface = dist; + ioBuffer->mAboveSurface = above; + ++ioBuffer; + } + } + + /// Check if all points are above the surface. Should be used as early out. + inline bool AreAllAbove() const + { + return mAllAbove; + } + + /// Check if all points are below the surface. Should be used as early out. + inline bool AreAllBelow() const + { + return mAllBelow; + } + + /// Get the lowest point of the polyhedron. Used to form the 4th vertex to make a tetrahedron out of a polyhedron face. + inline int GetReferencePointIdx() const + { + return mReferencePointIdx; + } + + /// Add a polyhedron face. Supply the indices of the points that form the face (in counter clockwise order). + void AddFace(int inIdx1, int inIdx2, int inIdx3) + { + JPH_ASSERT(inIdx1 != mReferencePointIdx && inIdx2 != mReferencePointIdx && inIdx3 != mReferencePointIdx, "A face using the reference point will not contribute to the volume"); + + // Find the points + const Point &ref = mPoints[mReferencePointIdx]; + const Point &p1 = mPoints[inIdx1]; + const Point &p2 = mPoints[inIdx2]; + const Point &p3 = mPoints[inIdx3]; + + // Determine which vertices are submerged + uint code = (p1.mAboveSurface? 0 : 0b001) | (p2.mAboveSurface? 0 : 0b010) | (p3.mAboveSurface? 0 : 0b100); + + float volume; + Vec3 center; + switch (code) + { + case 0b000: + // One point submerged + sTetrahedronVolume1(ref.mPosition, ref.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, volume, center); + break; + + case 0b001: + // Two points submerged + sTetrahedronVolume2(ref.mPosition, ref.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, volume, center); + break; + + case 0b010: + // Two points submerged + sTetrahedronVolume2(ref.mPosition, ref.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, volume, center); + break; + + case 0b100: + // Two points submerged + sTetrahedronVolume2(ref.mPosition, ref.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, volume, center); + break; + + case 0b011: + // Three points submerged + sTetrahedronVolume3(ref.mPosition, ref.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, volume, center); + break; + + case 0b101: + // Three points submerged + sTetrahedronVolume3(ref.mPosition, ref.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, volume, center); + break; + + case 0b110: + // Three points submerged + sTetrahedronVolume3(ref.mPosition, ref.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, volume, center); + break; + + case 0b111: + // Four points submerged + sTetrahedronVolume4(ref.mPosition, p3.mPosition, p2.mPosition, p1.mPosition, volume, center); + break; + + default: + // Should not be possible + JPH_ASSERT(false); + volume = 0.0f; + center = Vec3::sZero(); + break; + } + + mSubmergedVolume += volume; + mCenterOfBuoyancy += volume * center; + } + + /// Call after all faces have been added. Returns the submerged volume and the center of buoyancy for the submerged volume. + void GetResult(float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const + { + outCenterOfBuoyancy = mSubmergedVolume > 0.0f? mCenterOfBuoyancy / (4.0f * mSubmergedVolume) : Vec3::sZero(); // Do this before dividing submerged volume by 6 to get correct weight factor + outSubmergedVolume = mSubmergedVolume / 6.0f; + } + +private: + // The precalculated points for this polyhedron + const Point * mPoints; + + // If all points are above/below the surface + bool mAllBelow = true; + bool mAllAbove = true; + + // The lowest point + int mReferencePointIdx = 0; + + // Aggregator for submerged volume and center of buoyancy + float mSubmergedVolume = 0.0f; + Vec3 mCenterOfBuoyancy = Vec3::sZero(); + +#ifdef JPH_DEBUG_RENDERER + // Base offset used for drawing + RVec3 mBaseOffset; +#endif +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp new file mode 100644 index 000000000000..66b8bea1eb2a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp @@ -0,0 +1,333 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(RotatedTranslatedShapeSettings) +{ + JPH_ADD_BASE_CLASS(RotatedTranslatedShapeSettings, DecoratedShapeSettings) + + JPH_ADD_ATTRIBUTE(RotatedTranslatedShapeSettings, mPosition) + JPH_ADD_ATTRIBUTE(RotatedTranslatedShapeSettings, mRotation) +} + +ShapeSettings::ShapeResult RotatedTranslatedShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new RotatedTranslatedShape(*this, mCachedResult); + return mCachedResult; +} + +RotatedTranslatedShape::RotatedTranslatedShape(const RotatedTranslatedShapeSettings &inSettings, ShapeResult &outResult) : + DecoratedShape(EShapeSubType::RotatedTranslated, inSettings, outResult) +{ + if (outResult.HasError()) + return; + + // Calculate center of mass position + mCenterOfMass = inSettings.mPosition + inSettings.mRotation * mInnerShape->GetCenterOfMass(); + + // Store rotation (position is always zero because we center around the center of mass) + mRotation = inSettings.mRotation; + mIsRotationIdentity = mRotation.IsClose(Quat::sIdentity()); + + outResult.Set(this); +} + +RotatedTranslatedShape::RotatedTranslatedShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape) : + DecoratedShape(EShapeSubType::RotatedTranslated, inShape) +{ + // Calculate center of mass position + mCenterOfMass = inPosition + inRotation * mInnerShape->GetCenterOfMass(); + + // Store rotation (position is always zero because we center around the center of mass) + mRotation = inRotation; + mIsRotationIdentity = mRotation.IsClose(Quat::sIdentity()); +} + +MassProperties RotatedTranslatedShape::GetMassProperties() const +{ + // Rotate inertia of child into place + MassProperties p = mInnerShape->GetMassProperties(); + p.Rotate(Mat44::sRotation(mRotation)); + return p; +} + +AABox RotatedTranslatedShape::GetLocalBounds() const +{ + return mInnerShape->GetLocalBounds().Transformed(Mat44::sRotation(mRotation)); +} + +AABox RotatedTranslatedShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + Mat44 transform = inCenterOfMassTransform * Mat44::sRotation(mRotation); + return mInnerShape->GetWorldSpaceBounds(transform, TransformScale(inScale)); +} + +TransformedShape RotatedTranslatedShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const +{ + // We don't use any bits in the sub shape ID + outRemainder = inSubShapeID; + + TransformedShape ts(RVec3(inPositionCOM), inRotation * mRotation, mInnerShape, BodyID()); + ts.SetShapeScale(TransformScale(inScale)); + return ts; +} + +Vec3 RotatedTranslatedShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + // Transform surface position to local space and pass call on + Mat44 transform = Mat44::sRotation(mRotation.Conjugated()); + Vec3 normal = mInnerShape->GetSurfaceNormal(inSubShapeID, transform * inLocalSurfacePosition); + + // Transform normal to this shape's space + return transform.Multiply3x3Transposed(normal); +} + +void RotatedTranslatedShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + Mat44 transform = Mat44::sRotation(mRotation); + mInnerShape->GetSupportingFace(inSubShapeID, transform.Multiply3x3Transposed(inDirection), TransformScale(inScale), inCenterOfMassTransform * transform, outVertices); +} + +void RotatedTranslatedShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + // Get center of mass transform of child + Mat44 transform = inCenterOfMassTransform * Mat44::sRotation(mRotation); + + // Recurse to child + mInnerShape->GetSubmergedVolume(transform, TransformScale(inScale), inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset)); +} + +#ifdef JPH_DEBUG_RENDERER +void RotatedTranslatedShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + mInnerShape->Draw(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale), inColor, inUseMaterialColors, inDrawWireframe); +} + +void RotatedTranslatedShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const +{ + mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale), inColor, inDrawSupportDirection); +} + +void RotatedTranslatedShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale)); +} +#endif // JPH_DEBUG_RENDERER + +bool RotatedTranslatedShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + // Transform the ray + Mat44 transform = Mat44::sRotation(mRotation.Conjugated()); + RayCast ray = inRay.Transformed(transform); + + return mInnerShape->CastRay(ray, inSubShapeIDCreator, ioHit); +} + +void RotatedTranslatedShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Transform the ray + Mat44 transform = Mat44::sRotation(mRotation.Conjugated()); + RayCast ray = inRay.Transformed(transform); + + return mInnerShape->CastRay(ray, inRayCastSettings, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void RotatedTranslatedShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Transform the point + Mat44 transform = Mat44::sRotation(mRotation.Conjugated()); + mInnerShape->CollidePoint(transform * inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void RotatedTranslatedShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform * Mat44::sRotation(mRotation), inScale, inVertices, inNumVertices, inCollidingShapeIndex); +} + +void RotatedTranslatedShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + mInnerShape->CollectTransformedShapes(inBox, inPositionCOM, inRotation * mRotation, TransformScale(inScale), inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void RotatedTranslatedShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const +{ + mInnerShape->TransformShape(inCenterOfMassTransform * Mat44::sRotation(mRotation), ioCollector); +} + +void RotatedTranslatedShape::sCollideRotatedTranslatedVsShape(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) +{ + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape1 = static_cast(inShape1); + + // Get world transform of 1 + Mat44 transform1 = inCenterOfMassTransform1 * Mat44::sRotation(shape1->mRotation); + + CollisionDispatch::sCollideShapeVsShape(shape1->mInnerShape, inShape2, shape1->TransformScale(inScale1), inScale2, transform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); +} + +void RotatedTranslatedShape::sCollideShapeVsRotatedTranslated(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) +{ + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape2 = static_cast(inShape2); + + // Get world transform of 2 + Mat44 transform2 = inCenterOfMassTransform2 * Mat44::sRotation(shape2->mRotation); + + CollisionDispatch::sCollideShapeVsShape(inShape1, shape2->mInnerShape, inScale1, shape2->TransformScale(inScale2), inCenterOfMassTransform1, transform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); +} + +void RotatedTranslatedShape::sCollideRotatedTranslatedVsRotatedTranslated(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) +{ + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape1 = static_cast(inShape1); + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape2 = static_cast(inShape2); + + // Get world transform of 1 and 2 + Mat44 transform1 = inCenterOfMassTransform1 * Mat44::sRotation(shape1->mRotation); + Mat44 transform2 = inCenterOfMassTransform2 * Mat44::sRotation(shape2->mRotation); + + CollisionDispatch::sCollideShapeVsShape(shape1->mInnerShape, shape2->mInnerShape, shape1->TransformScale(inScale1), shape2->TransformScale(inScale2), transform1, transform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); +} + +void RotatedTranslatedShape::sCastRotatedTranslatedVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + // Fetch rotated translated shape from cast shape + JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape1 = static_cast(inShapeCast.mShape); + + // Transform the shape cast and update the shape + Mat44 transform = inShapeCast.mCenterOfMassStart * Mat44::sRotation(shape1->mRotation); + Vec3 scale = shape1->TransformScale(inShapeCast.mScale); + ShapeCast shape_cast(shape1->mInnerShape, scale, transform, inShapeCast.mDirection); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); +} + +void RotatedTranslatedShape::sCastShapeVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape = static_cast(inShape); + + // Determine the local transform + Mat44 local_transform = Mat44::sRotation(shape->mRotation); + + // Transform the shape cast + ShapeCast shape_cast = inShapeCast.PostTransformed(local_transform.Transposed3x3()); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, shape->mInnerShape, shape->TransformScale(inScale), inShapeFilter, inCenterOfMassTransform2 * local_transform, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); +} + +void RotatedTranslatedShape::sCastRotatedTranslatedVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape1 = static_cast(inShapeCast.mShape); + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape2 = static_cast(inShape); + + // Determine the local transform of shape 2 + Mat44 local_transform2 = Mat44::sRotation(shape2->mRotation); + Mat44 local_transform2_transposed = local_transform2.Transposed3x3(); + + // Transform the shape cast and update the shape + Mat44 transform = (local_transform2_transposed * inShapeCast.mCenterOfMassStart) * Mat44::sRotation(shape1->mRotation); + Vec3 scale = shape1->TransformScale(inShapeCast.mScale); + ShapeCast shape_cast(shape1->mInnerShape, scale, transform, local_transform2_transposed.Multiply3x3(inShapeCast.mDirection)); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, shape2->mInnerShape, shape2->TransformScale(inScale), inShapeFilter, inCenterOfMassTransform2 * local_transform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); +} + +void RotatedTranslatedShape::SaveBinaryState(StreamOut &inStream) const +{ + DecoratedShape::SaveBinaryState(inStream); + + inStream.Write(mCenterOfMass); + inStream.Write(mRotation); +} + +void RotatedTranslatedShape::RestoreBinaryState(StreamIn &inStream) +{ + DecoratedShape::RestoreBinaryState(inStream); + + inStream.Read(mCenterOfMass); + inStream.Read(mRotation); + mIsRotationIdentity = mRotation.IsClose(Quat::sIdentity()); +} + +bool RotatedTranslatedShape::IsValidScale(Vec3Arg inScale) const +{ + if (!Shape::IsValidScale(inScale)) + return false; + + if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(inScale)) + return mInnerShape->IsValidScale(inScale); + + if (!ScaleHelpers::CanScaleBeRotated(mRotation, inScale)) + return false; + + return mInnerShape->IsValidScale(ScaleHelpers::RotateScale(mRotation, inScale)); +} + +Vec3 RotatedTranslatedShape::MakeScaleValid(Vec3Arg inScale) const +{ + Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale); + + if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(scale)) + return mInnerShape->MakeScaleValid(scale); + + if (ScaleHelpers::CanScaleBeRotated(mRotation, scale)) + return ScaleHelpers::RotateScale(mRotation.Conjugated(), mInnerShape->MakeScaleValid(ScaleHelpers::RotateScale(mRotation, scale))); + + Vec3 abs_uniform_scale = ScaleHelpers::MakeUniformScale(scale.Abs()); + Vec3 uniform_scale = scale.GetSign() * abs_uniform_scale; + if (ScaleHelpers::CanScaleBeRotated(mRotation, uniform_scale)) + return uniform_scale; + + return Sign(scale.GetX()) * abs_uniform_scale; +} + +void RotatedTranslatedShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::RotatedTranslated); + f.mConstruct = []() -> Shape * { return new RotatedTranslatedShape; }; + f.mColor = Color::sBlue; + + for (EShapeSubType s : sAllSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(EShapeSubType::RotatedTranslated, s, sCollideRotatedTranslatedVsShape); + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::RotatedTranslated, sCollideShapeVsRotatedTranslated); + CollisionDispatch::sRegisterCastShape(EShapeSubType::RotatedTranslated, s, sCastRotatedTranslatedVsShape); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::RotatedTranslated, sCastShapeVsRotatedTranslated); + } + + CollisionDispatch::sRegisterCollideShape(EShapeSubType::RotatedTranslated, EShapeSubType::RotatedTranslated, sCollideRotatedTranslatedVsRotatedTranslated); + CollisionDispatch::sRegisterCastShape(EShapeSubType::RotatedTranslated, EShapeSubType::RotatedTranslated, sCastRotatedTranslatedVsRotatedTranslated); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h new file mode 100644 index 000000000000..2978a9559cbe --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h @@ -0,0 +1,161 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; + +/// Class that constructs a RotatedTranslatedShape +class JPH_EXPORT RotatedTranslatedShapeSettings final : public DecoratedShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, RotatedTranslatedShapeSettings) + +public: + /// Constructor + RotatedTranslatedShapeSettings() = default; + + /// Construct with shape settings, can be serialized. + RotatedTranslatedShapeSettings(Vec3Arg inPosition, QuatArg inRotation, const ShapeSettings *inShape) : DecoratedShapeSettings(inShape), mPosition(inPosition), mRotation(inRotation) { } + + /// Variant that uses a concrete shape, which means this object cannot be serialized. + RotatedTranslatedShapeSettings(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape): DecoratedShapeSettings(inShape), mPosition(inPosition), mRotation(inRotation) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + Vec3 mPosition; ///< Position of the sub shape + Quat mRotation; ///< Rotation of the sub shape +}; + +/// A rotated translated shape will rotate and translate a child shape. +/// Shifts the child object so that it is centered around the center of mass. +class JPH_EXPORT RotatedTranslatedShape final : public DecoratedShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + RotatedTranslatedShape() : DecoratedShape(EShapeSubType::RotatedTranslated) { } + RotatedTranslatedShape(const RotatedTranslatedShapeSettings &inSettings, ShapeResult &outResult); + RotatedTranslatedShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape); + + /// Access the rotation that is applied to the inner shape + Quat GetRotation() const { return mRotation; } + + /// Access the translation that has been applied to the inner shape + Vec3 GetPosition() const { return mCenterOfMass - mRotation * mInnerShape->GetCenterOfMass(); } + + // See Shape::GetCenterOfMass + virtual Vec3 GetCenterOfMass() const override { return mCenterOfMass; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mInnerShape->GetInnerRadius(); } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSubShapeTransformedShape + virtual TransformedShape GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; + + // See Shape::DrawGetSupportFunction + virtual void DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override; + + // See Shape::DrawGetSupportingFace + virtual void DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::CollectTransformedShapes + virtual void CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override; + + // See Shape::TransformShape + virtual void TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); } + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); return 0; } + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return mInnerShape->GetVolume(); } + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // See Shape::MakeScaleValid + virtual Vec3 MakeScaleValid(Vec3Arg inScale) const override; + + /// Transform the scale to the local space of the child shape + inline Vec3 TransformScale(Vec3Arg inScale) const + { + // We don't need to transform uniform scale or if the rotation is identity + if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(inScale)) + return inScale; + + return ScaleHelpers::RotateScale(mRotation, inScale); + } + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Helper functions called by CollisionDispatch + static void sCollideRotatedTranslatedVsShape(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 sCollideShapeVsRotatedTranslated(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 sCollideRotatedTranslatedVsRotatedTranslated(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 sCastRotatedTranslatedVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastShapeVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastRotatedTranslatedVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + bool mIsRotationIdentity; ///< If mRotation is close to identity (put here because it falls in padding bytes) + Vec3 mCenterOfMass; ///< Position of the center of mass + Quat mRotation; ///< Rotation of the child shape +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaleHelpers.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaleHelpers.h new file mode 100644 index 000000000000..4035dd77ae5a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaleHelpers.h @@ -0,0 +1,83 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Helper functions to get properties of a scaling vector +namespace ScaleHelpers +{ + /// Minimum valid scale value. This is used to prevent division by zero when scaling a shape with a zero scale. + static constexpr float cMinScale = 1.0e-6f; + + /// The tolerance used to check if components of the scale vector are the same + static constexpr float cScaleToleranceSq = 1.0e-8f; + + /// Test if a scale is identity + inline bool IsNotScaled(Vec3Arg inScale) { return inScale.IsClose(Vec3::sReplicate(1.0f), cScaleToleranceSq); } + + /// Test if a scale is uniform + inline bool IsUniformScale(Vec3Arg inScale) { return inScale.Swizzle().IsClose(inScale, cScaleToleranceSq); } + + /// Test if a scale is uniform in XZ + inline bool IsUniformScaleXZ(Vec3Arg inScale) { return inScale.Swizzle().IsClose(inScale, ScaleHelpers::cScaleToleranceSq); } + + /// Scale the convex radius of an object + inline float ScaleConvexRadius(float inConvexRadius, Vec3Arg inScale) { return min(inConvexRadius * inScale.Abs().ReduceMin(), cDefaultConvexRadius); } + + /// Test if a scale flips an object inside out (which requires flipping all normals and polygon windings) + inline bool IsInsideOut(Vec3Arg inScale) { return (CountBits(Vec3::sLess(inScale, Vec3::sZero()).GetTrues() & 0x7) & 1) != 0; } + + /// Test if any of the components of the scale have a value below cMinScale + inline bool IsZeroScale(Vec3Arg inScale) { return Vec3::sLess(inScale.Abs(), Vec3::sReplicate(cMinScale)).TestAnyXYZTrue(); } + + /// Ensure that the scale for each component is at least cMinScale + inline Vec3 MakeNonZeroScale(Vec3Arg inScale) { return inScale.GetSign() * Vec3::sMax(inScale.Abs(), Vec3::sReplicate(cMinScale)); } + + /// Get the average scale if inScale, used to make the scale uniform when a shape doesn't support non-uniform scale + inline Vec3 MakeUniformScale(Vec3Arg inScale) { return Vec3::sReplicate((inScale.GetX() + inScale.GetY() + inScale.GetZ()) / 3.0f); } + + /// Average the scale in XZ, used to make the scale uniform when a shape doesn't support non-uniform scale in the XZ plane + inline Vec3 MakeUniformScaleXZ(Vec3Arg inScale) { return 0.5f * (inScale + inScale.Swizzle()); } + + /// Checks in scale can be rotated to child shape + /// @param inRotation Rotation of child shape + /// @param inScale Scale in local space of parent shape + /// @return True if the scale is valid (no shearing introduced) + inline bool CanScaleBeRotated(QuatArg inRotation, Vec3Arg inScale) + { + // inScale is a scale in local space of the shape, so the transform for the shape (ignoring translation) is: T = Mat44::sScale(inScale) * mRotation. + // when we pass the scale to the child it needs to be local to the child, so we want T = mRotation * Mat44::sScale(ChildScale). + // Solving for ChildScale: ChildScale = mRotation^-1 * Mat44::sScale(inScale) * mRotation = mRotation^T * Mat44::sScale(inScale) * mRotation + // If any of the off diagonal elements are non-zero, it means the scale / rotation is not compatible. + Mat44 r = Mat44::sRotation(inRotation); + Mat44 child_scale = r.Multiply3x3LeftTransposed(r.PostScaled(inScale)); + + // Get the columns, but zero the diagonal + Vec4 zero = Vec4::sZero(); + Vec4 c0 = Vec4::sSelect(child_scale.GetColumn4(0), zero, UVec4(0xffffffff, 0, 0, 0)).Abs(); + Vec4 c1 = Vec4::sSelect(child_scale.GetColumn4(1), zero, UVec4(0, 0xffffffff, 0, 0)).Abs(); + Vec4 c2 = Vec4::sSelect(child_scale.GetColumn4(2), zero, UVec4(0, 0, 0xffffffff, 0)).Abs(); + + // Check if all elements are less than epsilon + Vec4 epsilon = Vec4::sReplicate(1.0e-6f); + return UVec4::sAnd(UVec4::sAnd(Vec4::sLess(c0, epsilon), Vec4::sLess(c1, epsilon)), Vec4::sLess(c2, epsilon)).TestAllTrue(); + } + + /// Adjust scale for rotated child shape + /// @param inRotation Rotation of child shape + /// @param inScale Scale in local space of parent shape + /// @return Rotated scale + inline Vec3 RotateScale(QuatArg inRotation, Vec3Arg inScale) + { + // Get the diagonal of mRotation^T * Mat44::sScale(inScale) * mRotation (see comment at CanScaleBeRotated) + Mat44 r = Mat44::sRotation(inRotation); + return r.Multiply3x3LeftTransposed(r.PostScaled(inScale)).GetDiagonal3(); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaledShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaledShape.cpp new file mode 100644 index 000000000000..1511813892bf --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaledShape.cpp @@ -0,0 +1,238 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ScaledShapeSettings) +{ + JPH_ADD_BASE_CLASS(ScaledShapeSettings, DecoratedShapeSettings) + + JPH_ADD_ATTRIBUTE(ScaledShapeSettings, mScale) +} + +ShapeSettings::ShapeResult ScaledShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new ScaledShape(*this, mCachedResult); + return mCachedResult; +} + +ScaledShape::ScaledShape(const ScaledShapeSettings &inSettings, ShapeResult &outResult) : + DecoratedShape(EShapeSubType::Scaled, inSettings, outResult), + mScale(inSettings.mScale) +{ + if (outResult.HasError()) + return; + + if (ScaleHelpers::IsZeroScale(inSettings.mScale)) + { + outResult.SetError("Can't use zero scale!"); + return; + } + + outResult.Set(this); +} + +MassProperties ScaledShape::GetMassProperties() const +{ + MassProperties p = mInnerShape->GetMassProperties(); + p.Scale(mScale); + return p; +} + +AABox ScaledShape::GetLocalBounds() const +{ + return mInnerShape->GetLocalBounds().Scaled(mScale); +} + +AABox ScaledShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + return mInnerShape->GetWorldSpaceBounds(inCenterOfMassTransform, inScale * mScale); +} + +TransformedShape ScaledShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const +{ + // We don't use any bits in the sub shape ID + outRemainder = inSubShapeID; + + TransformedShape ts(RVec3(inPositionCOM), inRotation, mInnerShape, BodyID()); + ts.SetShapeScale(inScale * mScale); + return ts; +} + +Vec3 ScaledShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + // Transform the surface point to local space and pass the query on + Vec3 normal = mInnerShape->GetSurfaceNormal(inSubShapeID, inLocalSurfacePosition / mScale); + + // Need to transform the plane normals using inScale + // Transforming a direction with matrix M is done through multiplying by (M^-1)^T + // In this case M is a diagonal matrix with the scale vector, so we need to multiply our normal by 1 / scale and renormalize afterwards + return (normal / mScale).Normalized(); +} + +void ScaledShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + mInnerShape->GetSupportingFace(inSubShapeID, inDirection, inScale * mScale, inCenterOfMassTransform, outVertices); +} + +void ScaledShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + mInnerShape->GetSubmergedVolume(inCenterOfMassTransform, inScale * mScale, inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset)); +} + +#ifdef JPH_DEBUG_RENDERER +void ScaledShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + mInnerShape->Draw(inRenderer, inCenterOfMassTransform, inScale * mScale, inColor, inUseMaterialColors, inDrawWireframe); +} + +void ScaledShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const +{ + mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform, inScale * mScale, inColor, inDrawSupportDirection); +} + +void ScaledShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform, inScale * mScale); +} +#endif // JPH_DEBUG_RENDERER + +bool ScaledShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + Vec3 inv_scale = mScale.Reciprocal(); + RayCast scaled_ray { inv_scale * inRay.mOrigin, inv_scale * inRay.mDirection }; + return mInnerShape->CastRay(scaled_ray, inSubShapeIDCreator, ioHit); +} + +void ScaledShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + Vec3 inv_scale = mScale.Reciprocal(); + RayCast scaled_ray { inv_scale * inRay.mOrigin, inv_scale * inRay.mDirection }; + return mInnerShape->CastRay(scaled_ray, inRayCastSettings, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void ScaledShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + Vec3 inv_scale = mScale.Reciprocal(); + mInnerShape->CollidePoint(inv_scale * inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void ScaledShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform, inScale * mScale, inVertices, inNumVertices, inCollidingShapeIndex); +} + +void ScaledShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + mInnerShape->CollectTransformedShapes(inBox, inPositionCOM, inRotation, inScale * mScale, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void ScaledShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const +{ + mInnerShape->TransformShape(inCenterOfMassTransform * Mat44::sScale(mScale), ioCollector); +} + +void ScaledShape::SaveBinaryState(StreamOut &inStream) const +{ + DecoratedShape::SaveBinaryState(inStream); + + inStream.Write(mScale); +} + +void ScaledShape::RestoreBinaryState(StreamIn &inStream) +{ + DecoratedShape::RestoreBinaryState(inStream); + + inStream.Read(mScale); +} + +float ScaledShape::GetVolume() const +{ + return abs(mScale.GetX() * mScale.GetY() * mScale.GetZ()) * mInnerShape->GetVolume(); +} + +bool ScaledShape::IsValidScale(Vec3Arg inScale) const +{ + return mInnerShape->IsValidScale(inScale * mScale); +} + +Vec3 ScaledShape::MakeScaleValid(Vec3Arg inScale) const +{ + return mInnerShape->MakeScaleValid(mScale * inScale) / mScale; +} + +void ScaledShape::sCollideScaledVsShape(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) +{ + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Scaled); + const ScaledShape *shape1 = static_cast(inShape1); + + CollisionDispatch::sCollideShapeVsShape(shape1->GetInnerShape(), inShape2, inScale1 * shape1->GetScale(), inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); +} + +void ScaledShape::sCollideShapeVsScaled(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) +{ + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::Scaled); + const ScaledShape *shape2 = static_cast(inShape2); + + CollisionDispatch::sCollideShapeVsShape(inShape1, shape2->GetInnerShape(), inScale1, inScale2 * shape2->GetScale(), inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); +} + +void ScaledShape::sCastScaledVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::Scaled); + const ScaledShape *shape = static_cast(inShapeCast.mShape); + + ShapeCast scaled_cast(shape->GetInnerShape(), inShapeCast.mScale * shape->GetScale(), inShapeCast.mCenterOfMassStart, inShapeCast.mDirection); + CollisionDispatch::sCastShapeVsShapeLocalSpace(scaled_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); +} + +void ScaledShape::sCastShapeVsScaled(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Scaled); + const ScaledShape *shape = static_cast(inShape); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(inShapeCast, inShapeCastSettings, shape->mInnerShape, inScale * shape->mScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); +} + +void ScaledShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Scaled); + f.mConstruct = []() -> Shape * { return new ScaledShape; }; + f.mColor = Color::sYellow; + + for (EShapeSubType s : sAllSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Scaled, s, sCollideScaledVsShape); + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Scaled, sCollideShapeVsScaled); + CollisionDispatch::sRegisterCastShape(EShapeSubType::Scaled, s, sCastScaledVsShape); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Scaled, sCastShapeVsScaled); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaledShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaledShape.h new file mode 100644 index 000000000000..6da92b6ed32a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaledShape.h @@ -0,0 +1,145 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class SubShapeIDCreator; +class CollideShapeSettings; + +/// Class that constructs a ScaledShape +class JPH_EXPORT ScaledShapeSettings final : public DecoratedShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, ScaledShapeSettings) + +public: + /// Default constructor for deserialization + ScaledShapeSettings() = default; + + /// Constructor that decorates another shape with a scale + ScaledShapeSettings(const ShapeSettings *inShape, Vec3Arg inScale) : DecoratedShapeSettings(inShape), mScale(inScale) { } + + /// Variant that uses a concrete shape, which means this object cannot be serialized. + ScaledShapeSettings(const Shape *inShape, Vec3Arg inScale) : DecoratedShapeSettings(inShape), mScale(inScale) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + Vec3 mScale = Vec3(1, 1, 1); +}; + +/// A shape that scales a child shape in local space of that shape. The scale can be non-uniform and can even turn it inside out when one or three components of the scale are negative. +class JPH_EXPORT ScaledShape final : public DecoratedShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + ScaledShape() : DecoratedShape(EShapeSubType::Scaled) { } + ScaledShape(const ScaledShapeSettings &inSettings, ShapeResult &outResult); + + /// Constructor that decorates another shape with a scale + ScaledShape(const Shape *inShape, Vec3Arg inScale) : DecoratedShape(EShapeSubType::Scaled, inShape), mScale(inScale) { JPH_ASSERT(!ScaleHelpers::IsZeroScale(mScale)); } + + /// Get the scale + Vec3 GetScale() const { return mScale; } + + // See Shape::GetCenterOfMass + virtual Vec3 GetCenterOfMass() const override { return mScale * mInnerShape->GetCenterOfMass(); } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mScale.ReduceMin() * mInnerShape->GetInnerRadius(); } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSubShapeTransformedShape + virtual TransformedShape GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; + + // See Shape::DrawGetSupportFunction + virtual void DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override; + + // See Shape::DrawGetSupportingFace + virtual void DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::CollectTransformedShapes + virtual void CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override; + + // See Shape::TransformShape + virtual void TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); } + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); return 0; } + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override; + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // See Shape::MakeScaleValid + virtual Vec3 MakeScaleValid(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Helper functions called by CollisionDispatch + static void sCollideScaledVsShape(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 sCollideShapeVsScaled(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 sCastScaledVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastShapeVsScaled(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + Vec3 mScale = Vec3(1, 1, 1); +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/Shape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/Shape.cpp new file mode 100644 index 000000000000..a3c37c0dae09 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/Shape.cpp @@ -0,0 +1,325 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT_BASE(ShapeSettings) +{ + JPH_ADD_BASE_CLASS(ShapeSettings, SerializableObject) + + JPH_ADD_ATTRIBUTE(ShapeSettings, mUserData) +} + +#ifdef JPH_DEBUG_RENDERER +bool Shape::sDrawSubmergedVolumes = false; +#endif // JPH_DEBUG_RENDERER + +ShapeFunctions ShapeFunctions::sRegistry[NumSubShapeTypes]; + +const Shape *Shape::GetLeafShape([[maybe_unused]] const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const +{ + outRemainder = inSubShapeID; + return this; +} + +TransformedShape Shape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const +{ + // We have reached the leaf shape so there is no remainder + outRemainder = SubShapeID(); + + // Just return the transformed shape for this shape + TransformedShape ts(RVec3(inPositionCOM), inRotation, this, BodyID()); + ts.SetShapeScale(inScale); + return ts; +} + +void Shape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + TransformedShape ts(RVec3(inPositionCOM), inRotation, this, TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator); + ts.SetShapeScale(inScale); + ioCollector.AddHit(ts); +} + +void Shape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const +{ + Vec3 scale; + Mat44 transform = inCenterOfMassTransform.Decompose(scale); + TransformedShape ts(RVec3(transform.GetTranslation()), transform.GetQuaternion(), this, BodyID(), SubShapeIDCreator()); + ts.SetShapeScale(MakeScaleValid(scale)); + ioCollector.AddHit(ts); +} + +void Shape::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mShapeSubType); + inStream.Write(mUserData); +} + +void Shape::RestoreBinaryState(StreamIn &inStream) +{ + // Type hash read by sRestoreFromBinaryState + inStream.Read(mUserData); +} + +Shape::ShapeResult Shape::sRestoreFromBinaryState(StreamIn &inStream) +{ + ShapeResult result; + + // Read the type of the shape + EShapeSubType shape_sub_type; + inStream.Read(shape_sub_type); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to read type id"); + return result; + } + + // Construct and read the data of the shape + Ref shape = ShapeFunctions::sGet(shape_sub_type).mConstruct(); + shape->RestoreBinaryState(inStream); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to restore shape"); + return result; + } + + result.Set(shape); + return result; +} + +void Shape::SaveWithChildren(StreamOut &inStream, ShapeToIDMap &ioShapeMap, MaterialToIDMap &ioMaterialMap) const +{ + ShapeToIDMap::const_iterator shape_id_iter = ioShapeMap.find(this); + if (shape_id_iter == ioShapeMap.end()) + { + // Write shape ID of this shape + uint32 shape_id = ioShapeMap.size(); + ioShapeMap[this] = shape_id; + inStream.Write(shape_id); + + // Write the shape itself + SaveBinaryState(inStream); + + // Write the ID's of all sub shapes + ShapeList sub_shapes; + SaveSubShapeState(sub_shapes); + inStream.Write(uint32(sub_shapes.size())); + for (const Shape *shape : sub_shapes) + { + if (shape == nullptr) + inStream.Write(~uint32(0)); + else + shape->SaveWithChildren(inStream, ioShapeMap, ioMaterialMap); + } + + // Write the materials + PhysicsMaterialList materials; + SaveMaterialState(materials); + StreamUtils::SaveObjectArray(inStream, materials, &ioMaterialMap); + } + else + { + // Known shape, just write the ID + inStream.Write(shape_id_iter->second); + } +} + +Shape::ShapeResult Shape::sRestoreWithChildren(StreamIn &inStream, IDToShapeMap &ioShapeMap, IDToMaterialMap &ioMaterialMap) +{ + ShapeResult result; + + // Read ID of this shape + uint32 shape_id; + inStream.Read(shape_id); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to read shape id"); + return result; + } + + // Check nullptr shape + if (shape_id == ~uint32(0)) + { + result.Set(nullptr); + return result; + } + + // Check if we already read this shape + if (shape_id < ioShapeMap.size()) + { + result.Set(ioShapeMap[shape_id]); + return result; + } + + // Read the shape + result = sRestoreFromBinaryState(inStream); + if (result.HasError()) + return result; + JPH_ASSERT(ioShapeMap.size() == shape_id); // Assert that this is the next ID in the map + ioShapeMap.push_back(result.Get()); + + // Read the sub shapes + uint32 len; + inStream.Read(len); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to read stream"); + return result; + } + ShapeList sub_shapes; + sub_shapes.reserve(len); + for (size_t i = 0; i < len; ++i) + { + ShapeResult sub_shape_result = sRestoreWithChildren(inStream, ioShapeMap, ioMaterialMap); + if (sub_shape_result.HasError()) + return sub_shape_result; + sub_shapes.push_back(sub_shape_result.Get()); + } + result.Get()->RestoreSubShapeState(sub_shapes.data(), (uint)sub_shapes.size()); + + // Read the materials + Result mlresult = StreamUtils::RestoreObjectArray(inStream, ioMaterialMap); + if (mlresult.HasError()) + { + result.SetError(mlresult.GetError()); + return result; + } + const PhysicsMaterialList &materials = mlresult.Get(); + result.Get()->RestoreMaterialState(materials.data(), (uint)materials.size()); + + return result; +} + +Shape::Stats Shape::GetStatsRecursive(VisitedShapes &ioVisitedShapes) const +{ + Stats stats = GetStats(); + + // If shape is already visited, don't count its size again + if (!ioVisitedShapes.insert(this).second) + stats.mSizeBytes = 0; + + return stats; +} + +bool Shape::IsValidScale(Vec3Arg inScale) const +{ + return !ScaleHelpers::IsZeroScale(inScale); +} + +Vec3 Shape::MakeScaleValid(Vec3Arg inScale) const +{ + return ScaleHelpers::MakeNonZeroScale(inScale); +} + +Shape::ShapeResult Shape::ScaleShape(Vec3Arg inScale) const +{ + const Vec3 unit_scale = Vec3::sReplicate(1.0f); + + if (inScale.IsNearZero()) + { + ShapeResult result; + result.SetError("Can't use zero scale!"); + return result; + } + + // First test if we can just wrap this shape in a scaled shape + if (IsValidScale(inScale)) + { + // Test if the scale is near unit + ShapeResult result; + if (inScale.IsClose(unit_scale)) + result.Set(const_cast(this)); + else + result.Set(new ScaledShape(this, inScale)); + return result; + } + + // Collect the leaf shapes and their transforms + struct Collector : TransformedShapeCollector + { + virtual void AddHit(const ResultType &inResult) override + { + mShapes.push_back(inResult); + } + + Array mShapes; + }; + Collector collector; + TransformShape(Mat44::sScale(inScale) * Mat44::sTranslation(GetCenterOfMass()), collector); + + // Construct a compound shape + StaticCompoundShapeSettings compound; + compound.mSubShapes.reserve(collector.mShapes.size()); + for (const TransformedShape &ts : collector.mShapes) + { + const Shape *shape = ts.mShape; + + // Construct a scaled shape if scale is not unit + Vec3 scale = ts.GetShapeScale(); + if (!scale.IsClose(unit_scale)) + shape = new ScaledShape(shape, scale); + + // Add the shape + compound.AddShape(Vec3(ts.mShapePositionCOM) - ts.mShapeRotation * shape->GetCenterOfMass(), ts.mShapeRotation, shape); + } + + return compound.Create(); +} + +void Shape::sCollidePointUsingRayCast(const Shape &inShape, Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + // First test if we're inside our bounding box + AABox bounds = inShape.GetLocalBounds(); + if (bounds.Contains(inPoint)) + { + // A collector that just counts the number of hits + class HitCountCollector : public CastRayCollector + { + public: + virtual void AddHit(const RayCastResult &inResult) override + { + // Store the last sub shape ID so that we can provide something to our outer hit collector + mSubShapeID = inResult.mSubShapeID2; + + ++mHitCount; + } + + int mHitCount = 0; + SubShapeID mSubShapeID; + }; + HitCountCollector collector; + + // Configure the raycast + RayCastSettings settings; + settings.SetBackFaceMode(EBackFaceMode::CollideWithBackFaces); + + // Cast a ray that's 10% longer than the height of our bounding box + inShape.CastRay(RayCast { inPoint, 1.1f * bounds.GetSize().GetY() * Vec3::sAxisY() }, settings, inSubShapeIDCreator, collector, inShapeFilter); + + // Odd amount of hits means inside + if ((collector.mHitCount & 1) == 1) + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), collector.mSubShapeID }); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/Shape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/Shape.h new file mode 100644 index 000000000000..f43b79c3f0cc --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/Shape.h @@ -0,0 +1,466 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +struct RayCast; +class RayCastSettings; +struct ShapeCast; +class ShapeCastSettings; +class RayCastResult; +class ShapeCastResult; +class CollidePointResult; +class CollideShapeResult; +class SubShapeIDCreator; +class SubShapeID; +class PhysicsMaterial; +class TransformedShape; +class Plane; +class CollideSoftBodyVertexIterator; +class Shape; +class StreamOut; +class StreamIn; +#ifdef JPH_DEBUG_RENDERER +class DebugRenderer; +#endif // JPH_DEBUG_RENDERER + +using CastRayCollector = CollisionCollector; +using CastShapeCollector = CollisionCollector; +using CollidePointCollector = CollisionCollector; +using CollideShapeCollector = CollisionCollector; +using TransformedShapeCollector = CollisionCollector; + +using ShapeRefC = RefConst; +using ShapeList = Array; +using PhysicsMaterialRefC = RefConst; +using PhysicsMaterialList = Array; + +/// Shapes are categorized in groups, each shape can return which group it belongs to through its Shape::GetType function. +enum class EShapeType : uint8 +{ + Convex, ///< Used by ConvexShape, all shapes that use the generic convex vs convex collision detection system (box, sphere, capsule, tapered capsule, cylinder, triangle) + Compound, ///< Used by CompoundShape + Decorated, ///< Used by DecoratedShape + Mesh, ///< Used by MeshShape + HeightField, ///< Used by HeightFieldShape + SoftBody, ///< Used by SoftBodyShape + + // User defined shapes + User1, + User2, + User3, + User4, + + Plane, ///< Used by PlaneShape + Empty, ///< Used by EmptyShape +}; + +/// This enumerates all shape types, each shape can return its type through Shape::GetSubType +enum class EShapeSubType : uint8 +{ + // Convex shapes + Sphere, + Box, + Triangle, + Capsule, + TaperedCapsule, + Cylinder, + ConvexHull, + + // Compound shapes + StaticCompound, + MutableCompound, + + // Decorated shapes + RotatedTranslated, + Scaled, + OffsetCenterOfMass, + + // Other shapes + Mesh, + HeightField, + SoftBody, + + // User defined shapes + User1, + User2, + User3, + User4, + User5, + User6, + User7, + User8, + + // User defined convex shapes + UserConvex1, + UserConvex2, + UserConvex3, + UserConvex4, + UserConvex5, + UserConvex6, + UserConvex7, + UserConvex8, + + // Other shapes + Plane, + TaperedCylinder, + Empty, +}; + +// Sets of shape sub types +static constexpr EShapeSubType sAllSubShapeTypes[] = { EShapeSubType::Sphere, EShapeSubType::Box, EShapeSubType::Triangle, EShapeSubType::Capsule, EShapeSubType::TaperedCapsule, EShapeSubType::Cylinder, EShapeSubType::ConvexHull, EShapeSubType::StaticCompound, EShapeSubType::MutableCompound, EShapeSubType::RotatedTranslated, EShapeSubType::Scaled, EShapeSubType::OffsetCenterOfMass, EShapeSubType::Mesh, EShapeSubType::HeightField, EShapeSubType::SoftBody, EShapeSubType::User1, EShapeSubType::User2, EShapeSubType::User3, EShapeSubType::User4, EShapeSubType::User5, EShapeSubType::User6, EShapeSubType::User7, EShapeSubType::User8, EShapeSubType::UserConvex1, EShapeSubType::UserConvex2, EShapeSubType::UserConvex3, EShapeSubType::UserConvex4, EShapeSubType::UserConvex5, EShapeSubType::UserConvex6, EShapeSubType::UserConvex7, EShapeSubType::UserConvex8, EShapeSubType::Plane, EShapeSubType::TaperedCylinder, EShapeSubType::Empty }; +static constexpr EShapeSubType sConvexSubShapeTypes[] = { EShapeSubType::Sphere, EShapeSubType::Box, EShapeSubType::Triangle, EShapeSubType::Capsule, EShapeSubType::TaperedCapsule, EShapeSubType::Cylinder, EShapeSubType::ConvexHull, EShapeSubType::TaperedCylinder, EShapeSubType::UserConvex1, EShapeSubType::UserConvex2, EShapeSubType::UserConvex3, EShapeSubType::UserConvex4, EShapeSubType::UserConvex5, EShapeSubType::UserConvex6, EShapeSubType::UserConvex7, EShapeSubType::UserConvex8 }; +static constexpr EShapeSubType sCompoundSubShapeTypes[] = { EShapeSubType::StaticCompound, EShapeSubType::MutableCompound }; +static constexpr EShapeSubType sDecoratorSubShapeTypes[] = { EShapeSubType::RotatedTranslated, EShapeSubType::Scaled, EShapeSubType::OffsetCenterOfMass }; + +/// How many shape types we support +static constexpr uint NumSubShapeTypes = uint(std::size(sAllSubShapeTypes)); + +/// Names of sub shape types +static constexpr const char *sSubShapeTypeNames[] = { "Sphere", "Box", "Triangle", "Capsule", "TaperedCapsule", "Cylinder", "ConvexHull", "StaticCompound", "MutableCompound", "RotatedTranslated", "Scaled", "OffsetCenterOfMass", "Mesh", "HeightField", "SoftBody", "User1", "User2", "User3", "User4", "User5", "User6", "User7", "User8", "UserConvex1", "UserConvex2", "UserConvex3", "UserConvex4", "UserConvex5", "UserConvex6", "UserConvex7", "UserConvex8", "Plane", "TaperedCylinder", "Empty" }; +static_assert(std::size(sSubShapeTypeNames) == NumSubShapeTypes); + +/// Class that can construct shapes and that is serializable using the ObjectStream system. +/// Can be used to store shape data in 'uncooked' form (i.e. in a form that is still human readable and authorable). +/// Once the shape has been created using the Create() function, the data will be moved into the Shape class +/// in a form that is optimized for collision detection. After this, the ShapeSettings object is no longer needed +/// and can be destroyed. Each shape class has a derived class of the ShapeSettings object to store shape specific +/// data. +class JPH_EXPORT ShapeSettings : public SerializableObject, public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, ShapeSettings) + +public: + using ShapeResult = Result>; + + /// Create a shape according to the settings specified by this object. + virtual ShapeResult Create() const = 0; + + /// When creating a shape, the result is cached so that calling Create() again will return the same shape. + /// If you make changes to the ShapeSettings you need to call this function to clear the cached result to allow Create() to build a new shape. + void ClearCachedResult() { mCachedResult.Clear(); } + + /// User data (to be used freely by the application) + uint64 mUserData = 0; + +protected: + mutable ShapeResult mCachedResult; +}; + +/// Function table for functions on shapes +class JPH_EXPORT ShapeFunctions +{ +public: + /// Construct a shape + Shape * (*mConstruct)() = nullptr; + + /// Color of the shape when drawing + Color mColor = Color::sBlack; + + /// Get an entry in the registry for a particular sub type + static inline ShapeFunctions & sGet(EShapeSubType inSubType) { return sRegistry[int(inSubType)]; } + +private: + static ShapeFunctions sRegistry[NumSubShapeTypes]; +}; + +/// Base class for all shapes (collision volume of a body). Defines a virtual interface for collision detection. +class JPH_EXPORT Shape : public RefTarget, public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + using ShapeResult = ShapeSettings::ShapeResult; + + /// Constructor + Shape(EShapeType inType, EShapeSubType inSubType) : mShapeType(inType), mShapeSubType(inSubType) { } + Shape(EShapeType inType, EShapeSubType inSubType, const ShapeSettings &inSettings, [[maybe_unused]] ShapeResult &outResult) : mUserData(inSettings.mUserData), mShapeType(inType), mShapeSubType(inSubType) { } + + /// Destructor + virtual ~Shape() = default; + + /// Get type + inline EShapeType GetType() const { return mShapeType; } + inline EShapeSubType GetSubType() const { return mShapeSubType; } + + /// User data (to be used freely by the application) + uint64 GetUserData() const { return mUserData; } + void SetUserData(uint64 inUserData) { mUserData = inUserData; } + + /// Check if this shape can only be used to create a static body or if it can also be dynamic/kinematic + virtual bool MustBeStatic() const { return false; } + + /// All shapes are centered around their center of mass. This function returns the center of mass position that needs to be applied to transform the shape to where it was created. + virtual Vec3 GetCenterOfMass() const { return Vec3::sZero(); } + + /// Get local bounding box including convex radius, this box is centered around the center of mass rather than the world transform + virtual AABox GetLocalBounds() const = 0; + + /// Get the max number of sub shape ID bits that are needed to be able to address any leaf shape in this shape. Used mainly for checking that it is smaller or equal than SubShapeID::MaxBits. + virtual uint GetSubShapeIDBitsRecursive() const = 0; + + /// Get world space bounds including convex radius. + /// This shape is scaled by inScale in local space first. + /// This function can be overridden to return a closer fitting world space bounding box, by default it will just transform what GetLocalBounds() returns. + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const { return GetLocalBounds().Scaled(inScale).Transformed(inCenterOfMassTransform); } + + /// Get world space bounds including convex radius. + AABox GetWorldSpaceBounds(DMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const + { + // Use single precision version using the rotation only + AABox bounds = GetWorldSpaceBounds(inCenterOfMassTransform.GetRotation(), inScale); + + // Apply translation + bounds.Translate(inCenterOfMassTransform.GetTranslation()); + + return bounds; + } + + /// Returns the radius of the biggest sphere that fits entirely in the shape. In case this shape consists of multiple sub shapes, it returns the smallest sphere of the parts. + /// This can be used as a measure of how far the shape can be moved without risking going through geometry. + virtual float GetInnerRadius() const = 0; + + /// Calculate the mass and inertia of this shape + virtual MassProperties GetMassProperties() const = 0; + + /// Get the leaf shape for a particular sub shape ID. + /// @param inSubShapeID The full sub shape ID that indicates the path to the leaf shape + /// @param outRemainder What remains of the sub shape ID after removing the path to the leaf shape (could e.g. refer to a triangle within a MeshShape) + /// @return The shape or null if the sub shape ID is invalid + virtual const Shape * GetLeafShape([[maybe_unused]] const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const; + + /// Get the material assigned to a particular sub shape ID + virtual const PhysicsMaterial * GetMaterial(const SubShapeID &inSubShapeID) const = 0; + + /// Get the surface normal of a particular sub shape ID and point on surface (all vectors are relative to center of mass for this shape). + /// Note: When you have a CollideShapeResult or ShapeCastResult you should use -mPenetrationAxis.Normalized() as contact normal as GetSurfaceNormal will only return face normals (and not vertex or edge normals). + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const = 0; + + /// Type definition for a supporting face + using SupportingFace = StaticArray; + + /// Get the vertices of the face that faces inDirection the most (includes any convex radius). Note that this function can only return faces of + /// convex shapes or triangles, which is why a sub shape ID to get to that leaf must be provided. + /// @param inSubShapeID Sub shape ID of target shape + /// @param inDirection Direction that the face should be facing (in local space to this shape) + /// @param inCenterOfMassTransform Transform to transform outVertices with + /// @param inScale Scale in local space of the shape (scales relative to its center of mass) + /// @param outVertices Resulting face. The returned face can be empty if the shape doesn't have polygons to return (e.g. because it's a sphere). The face will be returned in world space. + virtual void GetSupportingFace([[maybe_unused]] const SubShapeID &inSubShapeID, [[maybe_unused]] Vec3Arg inDirection, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] Mat44Arg inCenterOfMassTransform, [[maybe_unused]] SupportingFace &outVertices) const { /* Nothing */ } + + /// Get the user data of a particular sub shape ID. Corresponds with the value stored in Shape::GetUserData of the leaf shape pointed to by inSubShapeID. + virtual uint64 GetSubShapeUserData([[maybe_unused]] const SubShapeID &inSubShapeID) const { return mUserData; } + + /// Get the direct child sub shape and its transform for a sub shape ID. + /// @param inSubShapeID Sub shape ID that indicates the path to the leaf shape + /// @param inPositionCOM The position of the center of mass of this shape + /// @param inRotation The orientation of this shape + /// @param inScale Scale in local space of the shape (scales relative to its center of mass) + /// @param outRemainder The remainder of the sub shape ID after removing the sub shape + /// @return Direct child sub shape and its transform, note that the body ID and sub shape ID will be invalid + virtual TransformedShape GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const; + + /// Gets the properties needed to do buoyancy calculations for a body using this shape + /// @param inCenterOfMassTransform Transform that takes this shape (centered around center of mass) to world space (or a desired other space) + /// @param inScale Scale in local space of the shape (scales relative to its center of mass) + /// @param inSurface The surface plane of the liquid relative to inCenterOfMassTransform + /// @param outTotalVolume On return this contains the total volume of the shape + /// @param outSubmergedVolume On return this contains the submerged volume of the shape + /// @param outCenterOfBuoyancy On return this contains the world space center of mass of the submerged volume +#ifdef JPH_DEBUG_RENDERER + /// @param inBaseOffset The offset to transform inCenterOfMassTransform to world space (in double precision mode this can be used to shift the whole operation closer to the origin). Only used for debug drawing. +#endif + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy +#ifdef JPH_DEBUG_RENDERER // Not using JPH_IF_DEBUG_RENDERER for Doxygen + , RVec3Arg inBaseOffset +#endif + ) const = 0; + +#ifdef JPH_DEBUG_RENDERER + /// Draw the shape at a particular location with a particular color (debugging purposes) + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const = 0; + + /// Draw the results of the GetSupportFunction with the convex radius added back on to show any errors introduced by this process (only relevant for convex shapes) + virtual void DrawGetSupportFunction([[maybe_unused]] DebugRenderer *inRenderer, [[maybe_unused]] RMat44Arg inCenterOfMassTransform, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] ColorArg inColor, [[maybe_unused]] bool inDrawSupportDirection) const { /* Only implemented for convex shapes */ } + + /// Draw the results of the GetSupportingFace function to show any errors introduced by this process (only relevant for convex shapes) + virtual void DrawGetSupportingFace([[maybe_unused]] DebugRenderer *inRenderer, [[maybe_unused]] RMat44Arg inCenterOfMassTransform, [[maybe_unused]] Vec3Arg inScale) const { /* Only implemented for convex shapes */ } +#endif // JPH_DEBUG_RENDERER + + /// Cast a ray against this shape, returns true if it finds a hit closer than ioHit.mFraction and updates that fraction. Otherwise ioHit is left untouched and the function returns false. + /// Note that the ray should be relative to the center of mass of this shape (i.e. subtract Shape::GetCenterOfMass() from RayCast::mOrigin if you want to cast against the shape in the space it was created). + /// Convex objects will be treated as solid (meaning if the ray starts inside, you'll get a hit fraction of 0) and back face hits against triangles are returned. + /// If you want the surface normal of the hit use GetSurfaceNormal(ioHit.mSubShapeID2, inRay.GetPointOnRay(ioHit.mFraction)). + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const = 0; + + /// Cast a ray against this shape. Allows returning multiple hits through ioCollector. Note that this version is more flexible but also slightly slower than the CastRay function that returns only a single hit. + /// If you want the surface normal of the hit use GetSurfaceNormal(collected sub shape ID, inRay.GetPointOnRay(collected faction)). + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const = 0; + + /// Check if inPoint is inside this shape. For this tests all shapes are treated as if they were solid. + /// Note that inPoint should be relative to the center of mass of this shape (i.e. subtract Shape::GetCenterOfMass() from inPoint if you want to test against the shape in the space it was created). + /// For a mesh shape, this test will only provide sensible information if the mesh is a closed manifold. + /// For each shape that collides, ioCollector will receive a hit. + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const = 0; + + /// Collides all vertices of a soft body with this shape and updates SoftBodyVertex::mCollisionPlane, SoftBodyVertex::mCollidingShapeIndex and SoftBodyVertex::mLargestPenetration if a collision with more penetration was found. + /// @param inCenterOfMassTransform Center of mass transform for this shape relative to the vertices. + /// @param inScale Scale in local space of the shape (scales relative to its center of mass) + /// @param inVertices The vertices of the soft body + /// @param inNumVertices The number of vertices in inVertices + /// @param inCollidingShapeIndex Value to store in CollideSoftBodyVertexIterator::mCollidingShapeIndex when a collision was found + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const = 0; + + /// Collect the leaf transformed shapes of all leaf shapes of this shape. + /// inBox is the world space axis aligned box which leaf shapes should collide with. + /// inPositionCOM/inRotation/inScale describes the transform of this shape. + /// inSubShapeIDCeator represents the current sub shape ID of this shape. + virtual void CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const; + + /// Transforms this shape and all of its children with inTransform, resulting shape(s) are passed to ioCollector. + /// Note that not all shapes support all transforms (especially true for scaling), the resulting shape will try to match the transform as accurately as possible. + /// @param inCenterOfMassTransform The transform (rotation, translation, scale) that the center of mass of the shape should get + /// @param ioCollector The transformed shapes will be passed to this collector + virtual void TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const; + + /// Scale this shape. Note that not all shapes support all scales, this will return a shape that matches the scale as accurately as possible. See Shape::IsValidScale for more information. + /// @param inScale The scale to use for this shape (note: this scale is applied to the entire shape in the space it was created, most other functions apply the scale in the space of the leaf shapes and from the center of mass!) + ShapeResult ScaleShape(Vec3Arg inScale) const; + + /// An opaque buffer that holds shape specific information during GetTrianglesStart/Next. + struct alignas(16) GetTrianglesContext { uint8 mData[4288]; }; + + /// This is the minimum amount of triangles that should be requested through GetTrianglesNext. + static constexpr int cGetTrianglesMinTrianglesRequested = 32; + + /// To start iterating over triangles, call this function first. + /// ioContext is a temporary buffer and should remain untouched until the last call to GetTrianglesNext. + /// inBox is the world space bounding in which you want to get the triangles. + /// inPositionCOM/inRotation/inScale describes the transform of this shape. + /// To get the actual triangles call GetTrianglesNext. + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const = 0; + + /// Call this repeatedly to get all triangles in the box. + /// outTriangleVertices should be large enough to hold 3 * inMaxTriangleRequested entries. + /// outMaterials (if it is not null) should contain inMaxTrianglesRequested entries. + /// The function returns the amount of triangles that it found (which will be <= inMaxTrianglesRequested), or 0 if there are no more triangles. + /// Note that the function can return a value < inMaxTrianglesRequested and still have more triangles to process (triangles can be returned in blocks). + /// Note that the function may return triangles outside of the requested box, only coarse culling is performed on the returned triangles. + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const = 0; + + ///@name Binary serialization of the shape. Note that this saves the 'cooked' shape in a format which will not be backwards compatible for newer library versions. + /// In this case you need to recreate the shape from the ShapeSettings object and save it again. The user is expected to call SaveBinaryState followed by SaveMaterialState and SaveSubShapeState. + /// The stream should be stored as is and the material and shape list should be saved using the applications own serialization system (e.g. by assigning an ID to each pointer). + /// When restoring data, call sRestoreFromBinaryState to get the shape and then call RestoreMaterialState and RestoreSubShapeState to restore the pointers to the external objects. + /// Alternatively you can use SaveWithChildren and sRestoreWithChildren to save and restore the shape and all its child shapes and materials in a single stream. + ///@{ + + /// Saves the contents of the shape in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const; + + /// Creates a Shape of the correct type and restores its contents from the binary stream inStream. + static ShapeResult sRestoreFromBinaryState(StreamIn &inStream); + + /// Outputs the material references that this shape has to outMaterials. + virtual void SaveMaterialState([[maybe_unused]] PhysicsMaterialList &outMaterials) const { /* By default do nothing */ } + + /// Restore the material references after calling sRestoreFromBinaryState. Note that the exact same materials need to be provided in the same order as returned by SaveMaterialState. + virtual void RestoreMaterialState([[maybe_unused]] const PhysicsMaterialRefC *inMaterials, [[maybe_unused]] uint inNumMaterials) { JPH_ASSERT(inNumMaterials == 0); } + + /// Outputs the shape references that this shape has to outSubShapes. + virtual void SaveSubShapeState([[maybe_unused]] ShapeList &outSubShapes) const { /* By default do nothing */ } + + /// Restore the shape references after calling sRestoreFromBinaryState. Note that the exact same shapes need to be provided in the same order as returned by SaveSubShapeState. + virtual void RestoreSubShapeState([[maybe_unused]] const ShapeRefC *inSubShapes, [[maybe_unused]] uint inNumShapes) { JPH_ASSERT(inNumShapes == 0); } + + using ShapeToIDMap = StreamUtils::ObjectToIDMap; + using IDToShapeMap = StreamUtils::IDToObjectMap; + using MaterialToIDMap = StreamUtils::ObjectToIDMap; + using IDToMaterialMap = StreamUtils::IDToObjectMap; + + /// Save this shape, all its children and its materials. Pass in an empty map in ioShapeMap / ioMaterialMap or reuse the same map while saving multiple shapes to the same stream in order to avoid writing duplicates. + void SaveWithChildren(StreamOut &inStream, ShapeToIDMap &ioShapeMap, MaterialToIDMap &ioMaterialMap) const; + + /// Restore a shape, all its children and materials. Pass in an empty map in ioShapeMap / ioMaterialMap or reuse the same map while reading multiple shapes from the same stream in order to restore duplicates. + static ShapeResult sRestoreWithChildren(StreamIn &inStream, IDToShapeMap &ioShapeMap, IDToMaterialMap &ioMaterialMap); + + ///@} + + /// Class that holds information about the shape that can be used for logging / data collection purposes + struct Stats + { + Stats(size_t inSizeBytes, uint inNumTriangles) : mSizeBytes(inSizeBytes), mNumTriangles(inNumTriangles) { } + + size_t mSizeBytes; ///< Amount of memory used by this shape (size in bytes) + uint mNumTriangles; ///< Number of triangles in this shape (when applicable) + }; + + /// Get stats of this shape. Use for logging / data collection purposes only. Does not add values from child shapes, use GetStatsRecursive for this. + virtual Stats GetStats() const = 0; + + using VisitedShapes = UnorderedSet; + + /// Get the combined stats of this shape and its children. + /// @param ioVisitedShapes is used to track which shapes have already been visited, to avoid calculating the wrong memory size. + virtual Stats GetStatsRecursive(VisitedShapes &ioVisitedShapes) const; + + ///< Volume of this shape (m^3). Note that for compound shapes the volume may be incorrect since child shapes can overlap which is not accounted for. + virtual float GetVolume() const = 0; + + /// Test if inScale is a valid scale for this shape. Some shapes can only be scaled uniformly, compound shapes cannot handle shapes + /// being rotated and scaled (this would cause shearing), scale can never be zero. When the scale is invalid, the function will return false. + /// + /// Here's a list of supported scales: + /// * SphereShape: Scale must be uniform (signs of scale are ignored). + /// * BoxShape: Any scale supported (signs of scale are ignored). + /// * TriangleShape: Any scale supported when convex radius is zero, otherwise only uniform scale supported. + /// * CapsuleShape: Scale must be uniform (signs of scale are ignored). + /// * TaperedCapsuleShape: Scale must be uniform (sign of Y scale can be used to flip the capsule). + /// * CylinderShape: Scale must be uniform in XZ plane, Y can scale independently (signs of scale are ignored). + /// * RotatedTranslatedShape: Scale must not cause shear in the child shape. + /// * CompoundShape: Scale must not cause shear in any of the child shapes. + virtual bool IsValidScale(Vec3Arg inScale) const; + + /// This function will make sure that if you wrap this shape in a ScaledShape that the scale is valid. + /// Note that this involves discarding components of the scale that are invalid, so the resulting scaled shape may be different than the requested scale. + /// Compare the return value of this function with the scale you passed in to detect major inconsistencies and possibly warn the user. + /// @param inScale Local space scale for this shape. + /// @return Scale that can be used to wrap this shape in a ScaledShape. IsValidScale will return true for this scale. + virtual Vec3 MakeScaleValid(Vec3Arg inScale) const; + +#ifdef JPH_DEBUG_RENDERER + /// Debug helper which draws the intersection between water and the shapes, the center of buoyancy and the submerged volume + static bool sDrawSubmergedVolumes; +#endif // JPH_DEBUG_RENDERER + +protected: + /// This function should not be called directly, it is used by sRestoreFromBinaryState. + virtual void RestoreBinaryState(StreamIn &inStream); + + /// A fallback version of CollidePoint that uses a ray cast and counts the number of hits to determine if the point is inside the shape. Odd number of hits means inside, even number of hits means outside. + static void sCollidePointUsingRayCast(const Shape &inShape, Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter); + +private: + uint64 mUserData = 0; + EShapeType mShapeType; + EShapeSubType mShapeSubType; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.cpp new file mode 100644 index 000000000000..b44dd77a1e63 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.cpp @@ -0,0 +1,347 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SphereShapeSettings) +{ + JPH_ADD_BASE_CLASS(SphereShapeSettings, ConvexShapeSettings) + + JPH_ADD_ATTRIBUTE(SphereShapeSettings, mRadius) +} + +ShapeSettings::ShapeResult SphereShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new SphereShape(*this, mCachedResult); + return mCachedResult; +} + +SphereShape::SphereShape(const SphereShapeSettings &inSettings, ShapeResult &outResult) : + ConvexShape(EShapeSubType::Sphere, inSettings, outResult), + mRadius(inSettings.mRadius) +{ + if (inSettings.mRadius <= 0.0f) + { + outResult.SetError("Invalid radius"); + return; + } + + outResult.Set(this); +} + +float SphereShape::GetScaledRadius(Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Vec3 abs_scale = inScale.Abs(); + return abs_scale.GetX() * mRadius; +} + +AABox SphereShape::GetLocalBounds() const +{ + Vec3 half_extent = Vec3::sReplicate(mRadius); + return AABox(-half_extent, half_extent); +} + +AABox SphereShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + float scaled_radius = GetScaledRadius(inScale); + Vec3 half_extent = Vec3::sReplicate(scaled_radius); + AABox bounds(-half_extent, half_extent); + bounds.Translate(inCenterOfMassTransform.GetTranslation()); + return bounds; +} + +class SphereShape::SphereNoConvex final : public Support +{ +public: + explicit SphereNoConvex(float inRadius) : + mRadius(inRadius) + { + static_assert(sizeof(SphereNoConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(SphereNoConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + return Vec3::sZero(); + } + + virtual float GetConvexRadius() const override + { + return mRadius; + } + +private: + float mRadius; +}; + +class SphereShape::SphereWithConvex final : public Support +{ +public: + explicit SphereWithConvex(float inRadius) : + mRadius(inRadius) + { + static_assert(sizeof(SphereWithConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(SphereWithConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + float len = inDirection.Length(); + return len > 0.0f? (mRadius / len) * inDirection : Vec3::sZero(); + } + + virtual float GetConvexRadius() const override + { + return 0.0f; + } + +private: + float mRadius; +}; + +const ConvexShape::Support *SphereShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const +{ + float scaled_radius = GetScaledRadius(inScale); + + switch (inMode) + { + case ESupportMode::IncludeConvexRadius: + return new (&inBuffer) SphereWithConvex(scaled_radius); + + case ESupportMode::ExcludeConvexRadius: + case ESupportMode::Default: + return new (&inBuffer) SphereNoConvex(scaled_radius); + } + + JPH_ASSERT(false); + return nullptr; +} + +MassProperties SphereShape::GetMassProperties() const +{ + MassProperties p; + + // Calculate mass + float r2 = mRadius * mRadius; + p.mMass = (4.0f / 3.0f * JPH_PI) * mRadius * r2 * GetDensity(); + + // Calculate inertia + float inertia = (2.0f / 5.0f) * p.mMass * r2; + p.mInertia = Mat44::sScale(inertia); + + return p; +} + +Vec3 SphereShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + float len = inLocalSurfacePosition.Length(); + return len != 0.0f? inLocalSurfacePosition / len : Vec3::sAxisY(); +} + +void SphereShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + float scaled_radius = GetScaledRadius(inScale); + outTotalVolume = (4.0f / 3.0f * JPH_PI) * Cubed(scaled_radius); + + float distance_to_surface = inSurface.SignedDistance(inCenterOfMassTransform.GetTranslation()); + if (distance_to_surface >= scaled_radius) + { + // Above surface + outSubmergedVolume = 0.0f; + outCenterOfBuoyancy = Vec3::sZero(); + } + else if (distance_to_surface <= -scaled_radius) + { + // Under surface + outSubmergedVolume = outTotalVolume; + outCenterOfBuoyancy = inCenterOfMassTransform.GetTranslation(); + } + else + { + // Intersecting surface + + // Calculate submerged volume, see: https://en.wikipedia.org/wiki/Spherical_cap + float h = scaled_radius - distance_to_surface; + outSubmergedVolume = (JPH_PI / 3.0f) * Square(h) * (3.0f * scaled_radius - h); + + // Calculate center of buoyancy, see: http://mathworld.wolfram.com/SphericalCap.html (eq 10) + float z = (3.0f / 4.0f) * Square(2.0f * scaled_radius - h) / (3.0f * scaled_radius - h); + outCenterOfBuoyancy = inCenterOfMassTransform.GetTranslation() - z * inSurface.GetNormal(); // Negative normal since we want the portion under the water + + #ifdef JPH_DEBUG_RENDERER + // Draw intersection between sphere and water plane + if (sDrawSubmergedVolumes) + { + Vec3 circle_center = inCenterOfMassTransform.GetTranslation() - distance_to_surface * inSurface.GetNormal(); + float circle_radius = sqrt(Square(scaled_radius) - Square(distance_to_surface)); + DebugRenderer::sInstance->DrawPie(inBaseOffset + circle_center, circle_radius, inSurface.GetNormal(), inSurface.GetNormal().GetNormalizedPerpendicular(), -JPH_PI, JPH_PI, Color::sGreen, DebugRenderer::ECastShadow::Off); + } + #endif // JPH_DEBUG_RENDERER + } + +#ifdef JPH_DEBUG_RENDERER + // Draw center of buoyancy + if (sDrawSubmergedVolumes) + DebugRenderer::sInstance->DrawWireSphere(inBaseOffset + outCenterOfBuoyancy, 0.05f, Color::sRed, 1); +#endif // JPH_DEBUG_RENDERER +} + +#ifdef JPH_DEBUG_RENDERER +void SphereShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + inRenderer->DrawUnitSphere(inCenterOfMassTransform * Mat44::sScale(mRadius * inScale.Abs().GetX()), inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode); +} +#endif // JPH_DEBUG_RENDERER + +bool SphereShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + float fraction = RaySphere(inRay.mOrigin, inRay.mDirection, Vec3::sZero(), mRadius); + if (fraction < ioHit.mFraction) + { + ioHit.mFraction = fraction; + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + return false; +} + +void SphereShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + float min_fraction, max_fraction; + int num_results = RaySphere(inRay.mOrigin, inRay.mDirection, Vec3::sZero(), mRadius, min_fraction, max_fraction); + if (num_results > 0 // Ray should intersect + && max_fraction >= 0.0f // End of ray should be inside sphere + && min_fraction < ioCollector.GetEarlyOutFraction()) // Start of ray should be before early out fraction + { + // Better hit than the current hit + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + hit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + + // Check front side hit + if (inRayCastSettings.mTreatConvexAsSolid || min_fraction > 0.0f) + { + hit.mFraction = max(0.0f, min_fraction); + ioCollector.AddHit(hit); + } + + // Check back side hit + if (inRayCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces + && num_results > 1 // Ray should have 2 intersections + && max_fraction < ioCollector.GetEarlyOutFraction()) // End of ray should be before early out fraction + { + hit.mFraction = max_fraction; + ioCollector.AddHit(hit); + } + } +} + +void SphereShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + if (inPoint.LengthSq() <= Square(mRadius)) + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); +} + +void SphereShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + Vec3 center = inCenterOfMassTransform.GetTranslation(); + float radius = GetScaledRadius(inScale); + + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) + { + // Calculate penetration + Vec3 delta = v.GetPosition() - center; + float distance = delta.Length(); + float penetration = radius - distance; + if (v.UpdatePenetration(penetration)) + { + // Calculate contact point and normal + Vec3 normal = distance > 0.0f? delta / distance : Vec3::sAxisY(); + Vec3 point = center + radius * normal; + + // Store collision + v.SetCollision(Plane::sFromPointAndNormal(point, normal), inCollidingShapeIndex); + } + } +} + +void SphereShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + float scaled_radius = GetScaledRadius(inScale); + new (&ioContext) GetTrianglesContextVertexList(inPositionCOM, inRotation, Vec3::sReplicate(1.0f), Mat44::sScale(scaled_radius), sUnitSphereTriangles.data(), sUnitSphereTriangles.size(), GetMaterial()); +} + +int SphereShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + return ((GetTrianglesContextVertexList &)ioContext).GetTrianglesNext(inMaxTrianglesRequested, outTriangleVertices, outMaterials); +} + +void SphereShape::SaveBinaryState(StreamOut &inStream) const +{ + ConvexShape::SaveBinaryState(inStream); + + inStream.Write(mRadius); +} + +void SphereShape::RestoreBinaryState(StreamIn &inStream) +{ + ConvexShape::RestoreBinaryState(inStream); + + inStream.Read(mRadius); +} + +bool SphereShape::IsValidScale(Vec3Arg inScale) const +{ + return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScale(inScale.Abs()); +} + +Vec3 SphereShape::MakeScaleValid(Vec3Arg inScale) const +{ + Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale); + + return scale.GetSign() * ScaleHelpers::MakeUniformScale(scale.Abs()); +} + +void SphereShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Sphere); + f.mConstruct = []() -> Shape * { return new SphereShape; }; + f.mColor = Color::sGreen; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.h new file mode 100644 index 000000000000..c305523396e9 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.h @@ -0,0 +1,125 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a SphereShape +class JPH_EXPORT SphereShapeSettings final : public ConvexShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, SphereShapeSettings) + +public: + /// Default constructor for deserialization + SphereShapeSettings() = default; + + /// Create a sphere with radius inRadius + SphereShapeSettings(float inRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mRadius(inRadius) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + float mRadius = 0.0f; +}; + +/// A sphere, centered around the origin. +/// Note that it is implemented as a point with convex radius. +class JPH_EXPORT SphereShape final : public ConvexShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + SphereShape() : ConvexShape(EShapeSubType::Sphere) { } + SphereShape(const SphereShapeSettings &inSettings, ShapeResult &outResult); + + /// Create a sphere with radius inRadius + SphereShape(float inRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShape(EShapeSubType::Sphere, inMaterial), mRadius(inRadius) { JPH_ASSERT(inRadius > 0.0f); } + + /// Radius of the sphere + float GetRadius() const { return mRadius; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mRadius; } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace([[maybe_unused]] const SubShapeID &inSubShapeID, [[maybe_unused]] Vec3Arg inDirection, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] Mat44Arg inCenterOfMassTransform, [[maybe_unused]] SupportingFace &outVertices) const override { /* Hit is always a single point, no point in returning anything */ } + + // See ConvexShape::GetSupportFunction + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return 4.0f / 3.0f * JPH_PI * Cubed(mRadius); } + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // See Shape::MakeScaleValid + virtual Vec3 MakeScaleValid(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Get the radius of this sphere scaled by inScale + inline float GetScaledRadius(Vec3Arg inScale) const; + + // Classes for GetSupportFunction + class SphereNoConvex; + class SphereWithConvex; + + float mRadius = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/StaticCompoundShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/StaticCompoundShape.cpp new file mode 100644 index 000000000000..eb9ec06b9e76 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/StaticCompoundShape.cpp @@ -0,0 +1,674 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(StaticCompoundShapeSettings) +{ + JPH_ADD_BASE_CLASS(StaticCompoundShapeSettings, CompoundShapeSettings) +} + +ShapeSettings::ShapeResult StaticCompoundShapeSettings::Create(TempAllocator &inTempAllocator) const +{ + if (mCachedResult.IsEmpty()) + { + if (mSubShapes.size() == 0) + { + // It's an error to create a compound with no subshapes (the compound cannot encode this) + mCachedResult.SetError("Compound needs a sub shape!"); + } + else if (mSubShapes.size() == 1) + { + // If there's only 1 part we don't need a StaticCompoundShape + const SubShapeSettings &s = mSubShapes[0]; + if (s.mPosition == Vec3::sZero() + && s.mRotation == Quat::sIdentity()) + { + // No rotation or translation, we can use the shape directly + if (s.mShapePtr != nullptr) + mCachedResult.Set(const_cast(s.mShapePtr.GetPtr())); + else if (s.mShape != nullptr) + mCachedResult = s.mShape->Create(); + else + mCachedResult.SetError("Sub shape is null!"); + } + else + { + // We can use a RotatedTranslatedShape instead + RotatedTranslatedShapeSettings settings; + settings.mPosition = s.mPosition; + settings.mRotation = s.mRotation; + settings.mInnerShape = s.mShape; + settings.mInnerShapePtr = s.mShapePtr; + Ref shape = new RotatedTranslatedShape(settings, mCachedResult); + } + } + else + { + // Build a regular compound shape + Ref shape = new StaticCompoundShape(*this, inTempAllocator, mCachedResult); + } + } + return mCachedResult; +} + +ShapeSettings::ShapeResult StaticCompoundShapeSettings::Create() const +{ + TempAllocatorMalloc allocator; + return Create(allocator); +} + +void StaticCompoundShape::Node::SetChildInvalid(uint inIndex) +{ + // Make this an invalid node + mNodeProperties[inIndex] = INVALID_NODE; + + // Make bounding box invalid + mBoundsMinX[inIndex] = HALF_FLT_MAX; + mBoundsMinY[inIndex] = HALF_FLT_MAX; + mBoundsMinZ[inIndex] = HALF_FLT_MAX; + mBoundsMaxX[inIndex] = HALF_FLT_MAX; + mBoundsMaxY[inIndex] = HALF_FLT_MAX; + mBoundsMaxZ[inIndex] = HALF_FLT_MAX; +} + +void StaticCompoundShape::Node::SetChildBounds(uint inIndex, const AABox &inBounds) +{ + mBoundsMinX[inIndex] = HalfFloatConversion::FromFloat(inBounds.mMin.GetX()); + mBoundsMinY[inIndex] = HalfFloatConversion::FromFloat(inBounds.mMin.GetY()); + mBoundsMinZ[inIndex] = HalfFloatConversion::FromFloat(inBounds.mMin.GetZ()); + mBoundsMaxX[inIndex] = HalfFloatConversion::FromFloat(inBounds.mMax.GetX()); + mBoundsMaxY[inIndex] = HalfFloatConversion::FromFloat(inBounds.mMax.GetY()); + mBoundsMaxZ[inIndex] = HalfFloatConversion::FromFloat(inBounds.mMax.GetZ()); +} + +void StaticCompoundShape::sPartition(uint *ioBodyIdx, AABox *ioBounds, int inNumber, int &outMidPoint) +{ + // Handle trivial case + if (inNumber <= 4) + { + outMidPoint = inNumber / 2; + return; + } + + // Calculate bounding box of box centers + Vec3 center_min = Vec3::sReplicate(FLT_MAX); + Vec3 center_max = Vec3::sReplicate(-FLT_MAX); + for (const AABox *b = ioBounds, *b_end = ioBounds + inNumber; b < b_end; ++b) + { + Vec3 center = b->GetCenter(); + center_min = Vec3::sMin(center_min, center); + center_max = Vec3::sMax(center_max, center); + } + + // Calculate split plane + int dimension = (center_max - center_min).GetHighestComponentIndex(); + float split = 0.5f * (center_min + center_max)[dimension]; + + // Divide bodies + int start = 0, end = inNumber; + while (start < end) + { + // Search for first element that is on the right hand side of the split plane + while (start < end && ioBounds[start].GetCenter()[dimension] < split) + ++start; + + // Search for the first element that is on the left hand side of the split plane + while (start < end && ioBounds[end - 1].GetCenter()[dimension] >= split) + --end; + + if (start < end) + { + // Swap the two elements + std::swap(ioBodyIdx[start], ioBodyIdx[end - 1]); + std::swap(ioBounds[start], ioBounds[end - 1]); + ++start; + --end; + } + } + JPH_ASSERT(start == end); + + if (start > 0 && start < inNumber) + { + // Success! + outMidPoint = start; + } + else + { + // Failed to divide bodies + outMidPoint = inNumber / 2; + } +} + +void StaticCompoundShape::sPartition4(uint *ioBodyIdx, AABox *ioBounds, int inBegin, int inEnd, int *outSplit) +{ + uint *body_idx = ioBodyIdx + inBegin; + AABox *node_bounds = ioBounds + inBegin; + int number = inEnd - inBegin; + + // Partition entire range + sPartition(body_idx, node_bounds, number, outSplit[2]); + + // Partition lower half + sPartition(body_idx, node_bounds, outSplit[2], outSplit[1]); + + // Partition upper half + sPartition(body_idx + outSplit[2], node_bounds + outSplit[2], number - outSplit[2], outSplit[3]); + + // Convert to proper range + outSplit[0] = inBegin; + outSplit[1] += inBegin; + outSplit[2] += inBegin; + outSplit[3] += outSplit[2]; + outSplit[4] = inEnd; +} + +StaticCompoundShape::StaticCompoundShape(const StaticCompoundShapeSettings &inSettings, TempAllocator &inTempAllocator, ShapeResult &outResult) : + CompoundShape(EShapeSubType::StaticCompound, inSettings, outResult) +{ + // Check that there's at least 1 shape + uint num_subshapes = (uint)inSettings.mSubShapes.size(); + if (num_subshapes < 2) + { + outResult.SetError("Compound needs at least 2 sub shapes, otherwise you should use a RotatedTranslatedShape!"); + return; + } + + // Keep track of total mass to calculate center of mass + float mass = 0.0f; + + mSubShapes.resize(num_subshapes); + for (uint i = 0; i < num_subshapes; ++i) + { + const CompoundShapeSettings::SubShapeSettings &shape = inSettings.mSubShapes[i]; + + // Start constructing the runtime sub shape + SubShape &out_shape = mSubShapes[i]; + if (!out_shape.FromSettings(shape, outResult)) + return; + + // Calculate mass properties of child + MassProperties child = out_shape.mShape->GetMassProperties(); + + // Accumulate center of mass + mass += child.mMass; + mCenterOfMass += out_shape.GetPositionCOM() * child.mMass; + } + + if (mass > 0.0f) + mCenterOfMass /= mass; + + // Cache the inner radius as it can take a while to recursively iterate over all sub shapes + CalculateInnerRadius(); + + // Temporary storage for the bounding boxes of all shapes + uint bounds_size = num_subshapes * sizeof(AABox); + AABox *bounds = (AABox *)inTempAllocator.Allocate(bounds_size); + JPH_SCOPE_EXIT([&inTempAllocator, bounds, bounds_size]{ inTempAllocator.Free(bounds, bounds_size); }); + + // Temporary storage for body indexes (we're shuffling them) + uint body_idx_size = num_subshapes * sizeof(uint); + uint *body_idx = (uint *)inTempAllocator.Allocate(body_idx_size); + JPH_SCOPE_EXIT([&inTempAllocator, body_idx, body_idx_size]{ inTempAllocator.Free(body_idx, body_idx_size); }); + + // Shift all shapes so that the center of mass is now at the origin and calculate bounds + for (uint i = 0; i < num_subshapes; ++i) + { + SubShape &shape = mSubShapes[i]; + + // Shift the shape so it's centered around our center of mass + shape.SetPositionCOM(shape.GetPositionCOM() - mCenterOfMass); + + // Transform the shape's bounds into our local space + Mat44 transform = Mat44::sRotationTranslation(shape.GetRotation(), shape.GetPositionCOM()); + AABox shape_bounds = shape.mShape->GetWorldSpaceBounds(transform, Vec3::sReplicate(1.0f)); + + // Store bounds and body index for tree construction + bounds[i] = shape_bounds; + body_idx[i] = i; + + // Update our local bounds + mLocalBounds.Encapsulate(shape_bounds); + } + + // The algorithm is a recursive tree build, but to avoid the call overhead we keep track of a stack here + struct StackEntry + { + uint32 mNodeIdx; // Node index of node that is generated + int mChildIdx; // Index of child that we're currently processing + int mSplit[5]; // Indices where the node ID's have been split to form 4 partitions + AABox mBounds; // Bounding box of this node + }; + uint stack_size = num_subshapes * sizeof(StackEntry); + StackEntry *stack = (StackEntry *)inTempAllocator.Allocate(stack_size); + JPH_SCOPE_EXIT([&inTempAllocator, stack, stack_size]{ inTempAllocator.Free(stack, stack_size); }); + int top = 0; + + // Reserve enough space so that every sub shape gets its own leaf node + uint next_node_idx = 0; + mNodes.resize(num_subshapes + (num_subshapes + 2) / 3); // = Sum(num_subshapes * 4^-i) with i = [0, Inf]. + + // Create root node + stack[0].mNodeIdx = next_node_idx++; + stack[0].mChildIdx = -1; + stack[0].mBounds = AABox(); + sPartition4(body_idx, bounds, 0, num_subshapes, stack[0].mSplit); + + for (;;) + { + StackEntry &cur_stack = stack[top]; + + // Next child + cur_stack.mChildIdx++; + + // Check if all children processed + if (cur_stack.mChildIdx >= 4) + { + // Terminate if there's nothing left to pop + if (top <= 0) + break; + + // Add our bounds to our parents bounds + StackEntry &prev_stack = stack[top - 1]; + prev_stack.mBounds.Encapsulate(cur_stack.mBounds); + + // Store this node's properties in the parent node + Node &parent_node = mNodes[prev_stack.mNodeIdx]; + parent_node.mNodeProperties[prev_stack.mChildIdx] = cur_stack.mNodeIdx; + parent_node.SetChildBounds(prev_stack.mChildIdx, cur_stack.mBounds); + + // Pop entry from stack + --top; + } + else + { + // Get low and high index to bodies to process + int low = cur_stack.mSplit[cur_stack.mChildIdx]; + int high = cur_stack.mSplit[cur_stack.mChildIdx + 1]; + int num_bodies = high - low; + + if (num_bodies == 0) + { + // Mark invalid + Node &node = mNodes[cur_stack.mNodeIdx]; + node.SetChildInvalid(cur_stack.mChildIdx); + } + else if (num_bodies == 1) + { + // Get body info + uint child_node_idx = body_idx[low]; + const AABox &child_bounds = bounds[low]; + + // Update node + Node &node = mNodes[cur_stack.mNodeIdx]; + node.mNodeProperties[cur_stack.mChildIdx] = child_node_idx | IS_SUBSHAPE; + node.SetChildBounds(cur_stack.mChildIdx, child_bounds); + + // Encapsulate bounding box in parent + cur_stack.mBounds.Encapsulate(child_bounds); + } + else + { + // Allocate new node + StackEntry &new_stack = stack[++top]; + JPH_ASSERT(top < (int)num_subshapes); + new_stack.mNodeIdx = next_node_idx++; + new_stack.mChildIdx = -1; + new_stack.mBounds = AABox(); + sPartition4(body_idx, bounds, low, high, new_stack.mSplit); + } + } + } + + // Resize nodes to actual size + JPH_ASSERT(next_node_idx <= mNodes.size()); + mNodes.resize(next_node_idx); + mNodes.shrink_to_fit(); + + // Check if we ran out of bits for addressing a node + if (next_node_idx > IS_SUBSHAPE) + { + outResult.SetError("Compound hierarchy has too many nodes"); + return; + } + + // Check if we're not exceeding the amount of sub shape id bits + if (GetSubShapeIDBitsRecursive() > SubShapeID::MaxBits) + { + outResult.SetError("Compound hierarchy is too deep and exceeds the amount of available sub shape ID bits"); + return; + } + + outResult.Set(this); +} + +template +inline void StaticCompoundShape::WalkTree(Visitor &ioVisitor) const +{ + uint32 node_stack[cStackSize]; + node_stack[0] = 0; + int top = 0; + do + { + // Test if the node is valid, the node should rarely be invalid but it is possible when testing + // a really large box against the tree that the invalid nodes will intersect with the box + uint32 node_properties = node_stack[top]; + if (node_properties != INVALID_NODE) + { + // Test if node contains triangles + bool is_node = (node_properties & IS_SUBSHAPE) == 0; + if (is_node) + { + const Node &node = mNodes[node_properties]; + + // Unpack bounds + UVec4 bounds_minxy = UVec4::sLoadInt4(reinterpret_cast(&node.mBoundsMinX[0])); + Vec4 bounds_minx = HalfFloatConversion::ToFloat(bounds_minxy); + Vec4 bounds_miny = HalfFloatConversion::ToFloat(bounds_minxy.Swizzle()); + + UVec4 bounds_minzmaxx = UVec4::sLoadInt4(reinterpret_cast(&node.mBoundsMinZ[0])); + Vec4 bounds_minz = HalfFloatConversion::ToFloat(bounds_minzmaxx); + Vec4 bounds_maxx = HalfFloatConversion::ToFloat(bounds_minzmaxx.Swizzle()); + + UVec4 bounds_maxyz = UVec4::sLoadInt4(reinterpret_cast(&node.mBoundsMaxY[0])); + Vec4 bounds_maxy = HalfFloatConversion::ToFloat(bounds_maxyz); + Vec4 bounds_maxz = HalfFloatConversion::ToFloat(bounds_maxyz.Swizzle()); + + // Load properties for 4 children + UVec4 properties = UVec4::sLoadInt4(&node.mNodeProperties[0]); + + // Check which sub nodes to visit + int num_results = ioVisitor.VisitNodes(bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz, properties, top); + + // Push them onto the stack + JPH_ASSERT(top + 4 < cStackSize); + properties.StoreInt4(&node_stack[top]); + top += num_results; + } + else + { + // Points to a sub shape + uint32 sub_shape_idx = node_properties ^ IS_SUBSHAPE; + const SubShape &sub_shape = mSubShapes[sub_shape_idx]; + + ioVisitor.VisitShape(sub_shape, sub_shape_idx); + } + + // Check if we're done + if (ioVisitor.ShouldAbort()) + break; + } + + // Fetch next node until we find one that the visitor wants to see + do + --top; + while (top >= 0 && !ioVisitor.ShouldVisitNode(top)); + } + while (top >= 0); +} + +bool StaticCompoundShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastRayVisitor + { + using CastRayVisitor::CastRayVisitor; + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mDistanceStack[inStackTop] < mHit.mFraction; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test bounds of 4 children + Vec4 distance = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mHit.mFraction, ioProperties, &mDistanceStack[inStackTop]); + } + + float mDistanceStack[cStackSize]; + }; + + Visitor visitor(inRay, this, inSubShapeIDCreator, ioHit); + WalkTree(visitor); + return visitor.mReturnValue; +} + +void StaticCompoundShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastRayVisitorCollector + { + using CastRayVisitorCollector::CastRayVisitorCollector; + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetEarlyOutFraction(); + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test bounds of 4 children + Vec4 distance = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + float mDistanceStack[cStackSize]; + }; + + Visitor visitor(inRay, inRayCastSettings, this, inSubShapeIDCreator, ioCollector, inShapeFilter); + WalkTree(visitor); +} + +void StaticCompoundShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CollidePointVisitor + { + using CollidePointVisitor::CollidePointVisitor; + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Test if point overlaps with box + UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(collides, ioProperties); + } + }; + + Visitor visitor(inPoint, this, inSubShapeIDCreator, ioCollector, inShapeFilter); + WalkTree(visitor); +} + +void StaticCompoundShape::sCastShapeVsCompound(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastShapeVisitor + { + using CastShapeVisitor::CastShapeVisitor; + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction(); + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test bounds of 4 children + Vec4 distance = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + float mDistanceStack[cStackSize]; + }; + + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::StaticCompound); + const StaticCompoundShape *shape = static_cast(inShape); + + Visitor visitor(inShapeCast, inShapeCastSettings, shape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); + shape->WalkTree(visitor); +} + +void StaticCompoundShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + struct Visitor : public CollectTransformedShapesVisitor + { + using CollectTransformedShapesVisitor::CollectTransformedShapesVisitor; + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Test which nodes collide + UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(collides, ioProperties); + } + }; + + Visitor visitor(inBox, this, inPositionCOM, inRotation, inScale, inSubShapeIDCreator, ioCollector, inShapeFilter); + WalkTree(visitor); +} + +int StaticCompoundShape::GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const +{ + JPH_PROFILE_FUNCTION(); + + GetIntersectingSubShapesVisitorSC visitor(inBox, outSubShapeIndices, inMaxSubShapeIndices); + WalkTree(visitor); + return visitor.GetNumResults(); +} + +int StaticCompoundShape::GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const +{ + JPH_PROFILE_FUNCTION(); + + GetIntersectingSubShapesVisitorSC visitor(inBox, outSubShapeIndices, inMaxSubShapeIndices); + WalkTree(visitor); + return visitor.GetNumResults(); +} + +void StaticCompoundShape::sCollideCompoundVsShape(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) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::StaticCompound); + const StaticCompoundShape *shape1 = static_cast(inShape1); + + struct Visitor : public CollideCompoundVsShapeVisitor + { + using CollideCompoundVsShapeVisitor::CollideCompoundVsShapeVisitor; + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Test which nodes collide + UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(collides, ioProperties); + } + }; + + Visitor visitor(shape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); + shape1->WalkTree(visitor); +} + +void StaticCompoundShape::sCollideShapeVsCompound(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) +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CollideShapeVsCompoundVisitor + { + using CollideShapeVsCompoundVisitor::CollideShapeVsCompoundVisitor; + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Test which nodes collide + UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(collides, ioProperties); + } + }; + + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::StaticCompound); + const StaticCompoundShape *shape2 = static_cast(inShape2); + + Visitor visitor(inShape1, shape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); + shape2->WalkTree(visitor); +} + +void StaticCompoundShape::SaveBinaryState(StreamOut &inStream) const +{ + CompoundShape::SaveBinaryState(inStream); + + inStream.Write(mNodes); +} + +void StaticCompoundShape::RestoreBinaryState(StreamIn &inStream) +{ + CompoundShape::RestoreBinaryState(inStream); + + inStream.Read(mNodes); +} + +void StaticCompoundShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::StaticCompound); + f.mConstruct = []() -> Shape * { return new StaticCompoundShape; }; + f.mColor = Color::sOrange; + + for (EShapeSubType s : sAllSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(EShapeSubType::StaticCompound, s, sCollideCompoundVsShape); + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::StaticCompound, sCollideShapeVsCompound); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::StaticCompound, sCastShapeVsCompound); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/StaticCompoundShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/StaticCompoundShape.h new file mode 100644 index 000000000000..ea0a86fd3ba4 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/StaticCompoundShape.h @@ -0,0 +1,139 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; +class TempAllocator; + +/// Class that constructs a StaticCompoundShape. Note that if you only want a compound of 1 shape, use a RotatedTranslatedShape instead. +class JPH_EXPORT StaticCompoundShapeSettings final : public CompoundShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, StaticCompoundShapeSettings) + +public: + // See: ShapeSettings + virtual ShapeResult Create() const override; + + /// Specialization of Create() function that allows specifying a temp allocator to avoid temporary memory allocations on the heap + ShapeResult Create(TempAllocator &inTempAllocator) const; +}; + +/// A compound shape, sub shapes can be rotated and translated. +/// Sub shapes cannot be modified once the shape is constructed. +/// Shifts all child objects so that they're centered around the center of mass. +class JPH_EXPORT StaticCompoundShape final : public CompoundShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + StaticCompoundShape() : CompoundShape(EShapeSubType::StaticCompound) { } + StaticCompoundShape(const StaticCompoundShapeSettings &inSettings, TempAllocator &inTempAllocator, ShapeResult &outResult); + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See Shape::CollectTransformedShapes + virtual void CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override; + + // See: CompoundShape::GetIntersectingSubShapes + virtual int GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const override; + + // See: CompoundShape::GetIntersectingSubShapes + virtual int GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this) + mSubShapes.size() * sizeof(SubShape) + mNodes.size() * sizeof(Node), 0); } + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Visitor for GetIntersectingSubShapes + template + struct GetIntersectingSubShapesVisitorSC : public GetIntersectingSubShapesVisitor + { + using GetIntersectingSubShapesVisitor::GetIntersectingSubShapesVisitor; + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test if point overlaps with box + UVec4 collides = GetIntersectingSubShapesVisitor::TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(collides, ioProperties); + } + }; + + /// Sorts ioBodyIdx spatially into 2 groups. Second groups starts at ioBodyIdx + outMidPoint. + /// After the function returns ioBodyIdx and ioBounds will be shuffled + static void sPartition(uint *ioBodyIdx, AABox *ioBounds, int inNumber, int &outMidPoint); + + /// Sorts ioBodyIdx from inBegin to (but excluding) inEnd spatially into 4 groups. + /// outSplit needs to be 5 ints long, when the function returns each group runs from outSplit[i] to (but excluding) outSplit[i + 1] + /// After the function returns ioBodyIdx and ioBounds will be shuffled + static void sPartition4(uint *ioBodyIdx, AABox *ioBounds, int inBegin, int inEnd, int *outSplit); + + // Helper functions called by CollisionDispatch + static void sCollideCompoundVsShape(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 sCollideShapeVsCompound(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 sCastShapeVsCompound(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + // Maximum size of the stack during tree walk + static constexpr int cStackSize = 128; + + template + JPH_INLINE void WalkTree(Visitor &ioVisitor) const; ///< Walk the node tree calling the Visitor::VisitNodes for each node encountered and Visitor::VisitShape for each sub shape encountered + + /// Bits used in Node::mNodeProperties + enum : uint32 + { + IS_SUBSHAPE = 0x80000000, ///< If this bit is set, the other bits index in mSubShape, otherwise in mNodes + INVALID_NODE = 0x7fffffff, ///< Signifies an invalid node + }; + + /// Node structure + struct Node + { + void SetChildBounds(uint inIndex, const AABox &inBounds); ///< Set bounding box for child inIndex to inBounds + void SetChildInvalid(uint inIndex); ///< Mark the child inIndex as invalid and set its bounding box to invalid + + HalfFloat mBoundsMinX[4]; ///< 4 child bounding boxes + HalfFloat mBoundsMinY[4]; + HalfFloat mBoundsMinZ[4]; + HalfFloat mBoundsMaxX[4]; + HalfFloat mBoundsMaxY[4]; + HalfFloat mBoundsMaxZ[4]; + uint32 mNodeProperties[4]; ///< 4 child node properties + }; + + static_assert(sizeof(Node) == 64, "Node should be 64 bytes"); + + using Nodes = Array; + + Nodes mNodes; ///< Quad tree node structure +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SubShapeID.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SubShapeID.h new file mode 100644 index 000000000000..f1e8d3713bc8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SubShapeID.h @@ -0,0 +1,138 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// @brief A sub shape id contains a path to an element (usually a triangle or other primitive type) of a compound shape +/// +/// Each sub shape knows how many bits it needs to encode its ID, so knows how many bits to take from the sub shape ID. +/// +/// For example: +/// * We have a CompoundShape A with 5 child shapes (identify sub shape using 3 bits AAA) +/// * One of its child shapes is CompoundShape B which has 3 child shapes (identify sub shape using 2 bits BB) +/// * One of its child shapes is MeshShape C which contains enough triangles to need 7 bits to identify a triangle (identify sub shape using 7 bits CCCCCCC, note that MeshShape is block based and sorts triangles spatially, you can't assume that the first triangle will have bit pattern 0000000). +/// +/// The bit pattern of the sub shape ID to identify a triangle in MeshShape C will then be CCCCCCCBBAAA. +/// +/// A sub shape ID will become invalid when the structure of the shape changes. For example, if a child shape is removed from a compound shape, the sub shape ID will no longer be valid. +/// This can be a problem when caching sub shape IDs from one frame to the next. See comments at ContactListener::OnContactPersisted / OnContactRemoved. +class SubShapeID +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Underlying storage type + using Type = uint32; + + /// Type that is bigger than the underlying storage type for operations that would otherwise overflow + using BiggerType = uint64; + + static_assert(sizeof(BiggerType) > sizeof(Type), "The calculation below assumes BiggerType is a bigger type than Type"); + + /// How many bits we can store in this ID + static constexpr uint MaxBits = 8 * sizeof(Type); + + /// Constructor + SubShapeID() = default; + + /// Get the next id in the chain of ids (pops parents before children) + Type PopID(uint inBits, SubShapeID &outRemainder) const + { + Type mask_bits = Type((BiggerType(1) << inBits) - 1); + Type fill_bits = Type(BiggerType(cEmpty) << (MaxBits - inBits)); // Fill left side bits with 1 so that if there's no remainder all bits will be set, note that we do this using a BiggerType since on intel 0xffffffff << 32 == 0xffffffff + Type v = mValue & mask_bits; + outRemainder = SubShapeID(Type(BiggerType(mValue) >> inBits) | fill_bits); + return v; + } + + /// Get the value of the path to the sub shape ID + inline Type GetValue() const + { + return mValue; + } + + /// Set the value of the sub shape ID (use with care!) + inline void SetValue(Type inValue) + { + mValue = inValue; + } + + /// Check if there is any bits of subshape ID left. + /// Note that this is not a 100% guarantee as the subshape ID could consist of all 1 bits. Use for asserts only. + inline bool IsEmpty() const + { + return mValue == cEmpty; + } + + /// Check equal + inline bool operator == (const SubShapeID &inRHS) const + { + return mValue == inRHS.mValue; + } + + /// Check not-equal + inline bool operator != (const SubShapeID &inRHS) const + { + return mValue != inRHS.mValue; + } + +private: + friend class SubShapeIDCreator; + + /// An empty SubShapeID has all bits set + static constexpr Type cEmpty = ~Type(0); + + /// Constructor + explicit SubShapeID(const Type &inValue) : mValue(inValue) { } + + /// Adds an id at a particular position in the chain + /// (this should really only be called by the SubShapeIDCreator) + void PushID(Type inValue, uint inFirstBit, uint inBits) + { + // First clear the bits + mValue &= ~(Type((BiggerType(1) << inBits) - 1) << inFirstBit); + + // Then set them to the new value + mValue |= inValue << inFirstBit; + } + + Type mValue = cEmpty; +}; + +/// A sub shape id creator can be used to create a new sub shape id by recursing through the shape +/// hierarchy and pushing new ID's onto the chain +class SubShapeIDCreator +{ +public: + /// Add a new id to the chain of id's and return it + SubShapeIDCreator PushID(uint inValue, uint inBits) const + { + JPH_ASSERT(inValue < (SubShapeID::BiggerType(1) << inBits)); + SubShapeIDCreator copy = *this; + copy.mID.PushID(inValue, mCurrentBit, inBits); + copy.mCurrentBit += inBits; + JPH_ASSERT(copy.mCurrentBit <= SubShapeID::MaxBits); + return copy; + } + + // Get the resulting sub shape ID + const SubShapeID & GetID() const + { + return mID; + } + + /// Get the number of bits that have been written to the sub shape ID so far + inline uint GetNumBitsWritten() const + { + return mCurrentBit; + } + +private: + SubShapeID mID; + uint mCurrentBit = 0; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SubShapeIDPair.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SubShapeIDPair.h new file mode 100644 index 000000000000..f33ca310e70c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SubShapeIDPair.h @@ -0,0 +1,80 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// A pair of bodies and their sub shape ID's. Can be used as a key in a map to find a contact point. +class SubShapeIDPair +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + SubShapeIDPair() = default; + SubShapeIDPair(const BodyID &inBody1ID, const SubShapeID &inSubShapeID1, const BodyID &inBody2ID, const SubShapeID &inSubShapeID2) : mBody1ID(inBody1ID), mSubShapeID1(inSubShapeID1), mBody2ID(inBody2ID), mSubShapeID2(inSubShapeID2) { } + SubShapeIDPair & operator = (const SubShapeIDPair &) = default; + SubShapeIDPair(const SubShapeIDPair &) = default; + + /// Equality operator + inline bool operator == (const SubShapeIDPair &inRHS) const + { + return UVec4::sLoadInt4(reinterpret_cast(this)) == UVec4::sLoadInt4(reinterpret_cast(&inRHS)); + } + + /// Less than operator, used to consistently order contact points for a deterministic simulation + inline bool operator < (const SubShapeIDPair &inRHS) const + { + if (mBody1ID != inRHS.mBody1ID) + return mBody1ID < inRHS.mBody1ID; + + if (mSubShapeID1.GetValue() != inRHS.mSubShapeID1.GetValue()) + return mSubShapeID1.GetValue() < inRHS.mSubShapeID1.GetValue(); + + if (mBody2ID != inRHS.mBody2ID) + return mBody2ID < inRHS.mBody2ID; + + return mSubShapeID2.GetValue() < inRHS.mSubShapeID2.GetValue(); + } + + const BodyID & GetBody1ID() const { return mBody1ID; } + const SubShapeID & GetSubShapeID1() const { return mSubShapeID1; } + const BodyID & GetBody2ID() const { return mBody2ID; } + const SubShapeID & GetSubShapeID2() const { return mSubShapeID2; } + + uint64 GetHash() const { return HashBytes(this, sizeof(SubShapeIDPair)); } + +private: + BodyID mBody1ID; + SubShapeID mSubShapeID1; + BodyID mBody2ID; + SubShapeID mSubShapeID2; +}; + +static_assert(sizeof(SubShapeIDPair) == 16, "Unexpected size"); +static_assert(alignof(SubShapeIDPair) == 4, "Assuming 4 byte aligned"); + +JPH_NAMESPACE_END + +JPH_SUPPRESS_WARNINGS_STD_BEGIN + +namespace std +{ + /// Declare std::hash for SubShapeIDPair + template <> + struct hash + { + inline size_t operator () (const JPH::SubShapeIDPair &inRHS) const + { + return static_cast(inRHS.GetHash()); + } + }; +} + +JPH_SUPPRESS_WARNINGS_STD_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp new file mode 100644 index 000000000000..a3a9e020d5a5 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp @@ -0,0 +1,453 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TaperedCapsuleShapeSettings) +{ + JPH_ADD_BASE_CLASS(TaperedCapsuleShapeSettings, ConvexShapeSettings) + + JPH_ADD_ATTRIBUTE(TaperedCapsuleShapeSettings, mHalfHeightOfTaperedCylinder) + JPH_ADD_ATTRIBUTE(TaperedCapsuleShapeSettings, mTopRadius) + JPH_ADD_ATTRIBUTE(TaperedCapsuleShapeSettings, mBottomRadius) +} + +bool TaperedCapsuleShapeSettings::IsSphere() const +{ + return max(mTopRadius, mBottomRadius) >= 2.0f * mHalfHeightOfTaperedCylinder + min(mTopRadius, mBottomRadius); +} + +ShapeSettings::ShapeResult TaperedCapsuleShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + { + Ref shape; + if (IsValid() && IsSphere()) + { + // Determine sphere center and radius + float radius, center; + if (mTopRadius > mBottomRadius) + { + radius = mTopRadius; + center = mHalfHeightOfTaperedCylinder; + } + else + { + radius = mBottomRadius; + center = -mHalfHeightOfTaperedCylinder; + } + + // Create sphere + shape = new SphereShape(radius, mMaterial); + + // Offset sphere if needed + if (abs(center) > 1.0e-6f) + { + RotatedTranslatedShapeSettings rot_trans(Vec3(0, center, 0), Quat::sIdentity(), shape); + mCachedResult = rot_trans.Create(); + } + else + mCachedResult.Set(shape); + } + else + { + // Normal tapered capsule shape + shape = new TaperedCapsuleShape(*this, mCachedResult); + } + } + return mCachedResult; +} + +TaperedCapsuleShapeSettings::TaperedCapsuleShapeSettings(float inHalfHeightOfTaperedCylinder, float inTopRadius, float inBottomRadius, const PhysicsMaterial *inMaterial) : + ConvexShapeSettings(inMaterial), + mHalfHeightOfTaperedCylinder(inHalfHeightOfTaperedCylinder), + mTopRadius(inTopRadius), + mBottomRadius(inBottomRadius) +{ +} + +TaperedCapsuleShape::TaperedCapsuleShape(const TaperedCapsuleShapeSettings &inSettings, ShapeResult &outResult) : + ConvexShape(EShapeSubType::TaperedCapsule, inSettings, outResult), + mTopRadius(inSettings.mTopRadius), + mBottomRadius(inSettings.mBottomRadius) +{ + if (mTopRadius <= 0.0f) + { + outResult.SetError("Invalid top radius"); + return; + } + + if (mBottomRadius <= 0.0f) + { + outResult.SetError("Invalid bottom radius"); + return; + } + + if (inSettings.mHalfHeightOfTaperedCylinder <= 0.0f) + { + outResult.SetError("Invalid height"); + return; + } + + // If this goes off one of the sphere ends falls totally inside the other and you should use a sphere instead + if (inSettings.IsSphere()) + { + outResult.SetError("One sphere embedded in other sphere, please use sphere shape instead"); + return; + } + + // Approximation: The center of mass is exactly half way between the top and bottom cap of the tapered capsule + mTopCenter = inSettings.mHalfHeightOfTaperedCylinder + 0.5f * (mBottomRadius - mTopRadius); + mBottomCenter = -inSettings.mHalfHeightOfTaperedCylinder + 0.5f * (mBottomRadius - mTopRadius); + + // Calculate center of mass + mCenterOfMass = Vec3(0, inSettings.mHalfHeightOfTaperedCylinder - mTopCenter, 0); + + // Calculate convex radius + mConvexRadius = min(mTopRadius, mBottomRadius); + JPH_ASSERT(mConvexRadius > 0.0f); + + // Calculate the sin and tan of the angle that the cone surface makes with the Y axis + // See: TaperedCapsuleShape.gliffy + mSinAlpha = (mBottomRadius - mTopRadius) / (mTopCenter - mBottomCenter); + JPH_ASSERT(mSinAlpha >= -1.0f && mSinAlpha <= 1.0f); + mTanAlpha = Tan(ASin(mSinAlpha)); + + outResult.Set(this); +} + +class TaperedCapsuleShape::TaperedCapsule final : public Support +{ +public: + TaperedCapsule(Vec3Arg inTopCenter, Vec3Arg inBottomCenter, float inTopRadius, float inBottomRadius, float inConvexRadius) : + mTopCenter(inTopCenter), + mBottomCenter(inBottomCenter), + mTopRadius(inTopRadius), + mBottomRadius(inBottomRadius), + mConvexRadius(inConvexRadius) + { + static_assert(sizeof(TaperedCapsule) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(TaperedCapsule))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + // Check zero vector + float len = inDirection.Length(); + if (len == 0.0f) + return mTopCenter + Vec3(0, mTopRadius, 0); // Return top + + // Check if the support of the top sphere or bottom sphere is bigger + Vec3 support_top = mTopCenter + (mTopRadius / len) * inDirection; + Vec3 support_bottom = mBottomCenter + (mBottomRadius / len) * inDirection; + if (support_top.Dot(inDirection) > support_bottom.Dot(inDirection)) + return support_top; + else + return support_bottom; + } + + virtual float GetConvexRadius() const override + { + return mConvexRadius; + } + +private: + Vec3 mTopCenter; + Vec3 mBottomCenter; + float mTopRadius; + float mBottomRadius; + float mConvexRadius; +}; + +const ConvexShape::Support *TaperedCapsuleShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + // Get scaled tapered capsule + Vec3 abs_scale = inScale.Abs(); + float scale_xz = abs_scale.GetX(); + float scale_y = inScale.GetY(); // The sign of y is important as it flips the tapered capsule + Vec3 scaled_top_center = Vec3(0, scale_y * mTopCenter, 0); + Vec3 scaled_bottom_center = Vec3(0, scale_y * mBottomCenter, 0); + float scaled_top_radius = scale_xz * mTopRadius; + float scaled_bottom_radius = scale_xz * mBottomRadius; + float scaled_convex_radius = scale_xz * mConvexRadius; + + switch (inMode) + { + case ESupportMode::IncludeConvexRadius: + return new (&inBuffer) TaperedCapsule(scaled_top_center, scaled_bottom_center, scaled_top_radius, scaled_bottom_radius, 0.0f); + + case ESupportMode::ExcludeConvexRadius: + case ESupportMode::Default: + { + // Get radii reduced by convex radius + float tr = scaled_top_radius - scaled_convex_radius; + float br = scaled_bottom_radius - scaled_convex_radius; + JPH_ASSERT(tr >= 0.0f && br >= 0.0f); + JPH_ASSERT(tr == 0.0f || br == 0.0f, "Convex radius should be that of the smallest sphere"); + return new (&inBuffer) TaperedCapsule(scaled_top_center, scaled_bottom_center, tr, br, scaled_convex_radius); + } + } + + JPH_ASSERT(false); + return nullptr; +} + +void TaperedCapsuleShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + JPH_ASSERT(IsValidScale(inScale)); + + // Check zero vector + float len = inDirection.Length(); + if (len == 0.0f) + return; + + // Get scaled tapered capsule + Vec3 abs_scale = inScale.Abs(); + float scale_xz = abs_scale.GetX(); + float scale_y = inScale.GetY(); // The sign of y is important as it flips the tapered capsule + Vec3 scaled_top_center = Vec3(0, scale_y * mTopCenter, 0); + Vec3 scaled_bottom_center = Vec3(0, scale_y * mBottomCenter, 0); + float scaled_top_radius = scale_xz * mTopRadius; + float scaled_bottom_radius = scale_xz * mBottomRadius; + + // Get support point for top and bottom sphere in the opposite of inDirection (including convex radius) + Vec3 support_top = scaled_top_center - (scaled_top_radius / len) * inDirection; + Vec3 support_bottom = scaled_bottom_center - (scaled_bottom_radius / len) * inDirection; + + // Get projection on inDirection + float proj_top = support_top.Dot(inDirection); + float proj_bottom = support_bottom.Dot(inDirection); + + // If projection is roughly equal then return line, otherwise we return nothing as there's only 1 point + if (abs(proj_top - proj_bottom) < cCapsuleProjectionSlop * len) + { + outVertices.push_back(inCenterOfMassTransform * support_top); + outVertices.push_back(inCenterOfMassTransform * support_bottom); + } +} + +MassProperties TaperedCapsuleShape::GetMassProperties() const +{ + AABox box = GetInertiaApproximation(); + + MassProperties p; + p.SetMassAndInertiaOfSolidBox(box.GetSize(), GetDensity()); + return p; +} + +Vec3 TaperedCapsuleShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + // See: TaperedCapsuleShape.gliffy + // We need to calculate ty and by in order to see if the position is on the top or bottom sphere + // sin(alpha) = by / br = ty / tr + // => by = sin(alpha) * br, ty = sin(alpha) * tr + + if (inLocalSurfacePosition.GetY() > mTopCenter + mSinAlpha * mTopRadius) + return (inLocalSurfacePosition - Vec3(0, mTopCenter, 0)).Normalized(); + else if (inLocalSurfacePosition.GetY() < mBottomCenter + mSinAlpha * mBottomRadius) + return (inLocalSurfacePosition - Vec3(0, mBottomCenter, 0)).Normalized(); + else + { + // Get perpendicular vector to the surface in the xz plane + Vec3 perpendicular = Vec3(inLocalSurfacePosition.GetX(), 0, inLocalSurfacePosition.GetZ()).NormalizedOr(Vec3::sAxisX()); + + // We know that the perpendicular has length 1 and that it needs a y component where tan(alpha) = y / 1 in order to align it to the surface + perpendicular.SetY(mTanAlpha); + return perpendicular.Normalized(); + } +} + +AABox TaperedCapsuleShape::GetLocalBounds() const +{ + float max_radius = max(mTopRadius, mBottomRadius); + return AABox(Vec3(-max_radius, mBottomCenter - mBottomRadius, -max_radius), Vec3(max_radius, mTopCenter + mTopRadius, max_radius)); +} + +AABox TaperedCapsuleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Vec3 abs_scale = inScale.Abs(); + float scale_xz = abs_scale.GetX(); + float scale_y = inScale.GetY(); // The sign of y is important as it flips the tapered capsule + Vec3 bottom_extent = Vec3::sReplicate(scale_xz * mBottomRadius); + Vec3 bottom_center = inCenterOfMassTransform * Vec3(0, scale_y * mBottomCenter, 0); + Vec3 top_extent = Vec3::sReplicate(scale_xz * mTopRadius); + Vec3 top_center = inCenterOfMassTransform * Vec3(0, scale_y * mTopCenter, 0); + Vec3 p1 = Vec3::sMin(top_center - top_extent, bottom_center - bottom_extent); + Vec3 p2 = Vec3::sMax(top_center + top_extent, bottom_center + bottom_extent); + return AABox(p1, p2); +} + +void TaperedCapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation(); + + // Get scaled tapered capsule + Vec3 abs_scale = inScale.Abs(); + float scale_y = abs_scale.GetY(); + float scale_xz = abs_scale.GetX(); + Vec3 scale_y_flip(1, Sign(inScale.GetY()), 1); + Vec3 scaled_top_center(0, scale_y * mTopCenter, 0); + Vec3 scaled_bottom_center(0, scale_y * mBottomCenter, 0); + float scaled_top_radius = scale_xz * mTopRadius; + float scaled_bottom_radius = scale_xz * mBottomRadius; + + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) + { + Vec3 local_pos = scale_y_flip * (inverse_transform * v.GetPosition()); + + Vec3 position, normal; + + // If the vertex is inside the cone starting at the top center pointing along the y-axis with angle PI/2 - alpha then the closest point is on the top sphere + // This corresponds to: Dot(y-axis, (local_pos - top_center) / |local_pos - top_center|) >= cos(PI/2 - alpha) + // <=> (local_pos - top_center).y >= sin(alpha) * |local_pos - top_center| + Vec3 top_center_to_local_pos = local_pos - scaled_top_center; + float top_center_to_local_pos_len = top_center_to_local_pos.Length(); + if (top_center_to_local_pos.GetY() >= mSinAlpha * top_center_to_local_pos_len) + { + // Top sphere + normal = top_center_to_local_pos_len != 0.0f? top_center_to_local_pos / top_center_to_local_pos_len : Vec3::sAxisY(); + position = scaled_top_center + scaled_top_radius * normal; + } + else + { + // If the vertex is outside the cone starting at the bottom center pointing along the y-axis with angle PI/2 - alpha then the closest point is on the bottom sphere + // This corresponds to: Dot(y-axis, (local_pos - bottom_center) / |local_pos - bottom_center|) <= cos(PI/2 - alpha) + // <=> (local_pos - bottom_center).y <= sin(alpha) * |local_pos - bottom_center| + Vec3 bottom_center_to_local_pos = local_pos - scaled_bottom_center; + float bottom_center_to_local_pos_len = bottom_center_to_local_pos.Length(); + if (bottom_center_to_local_pos.GetY() <= mSinAlpha * bottom_center_to_local_pos_len) + { + // Bottom sphere + normal = bottom_center_to_local_pos_len != 0.0f? bottom_center_to_local_pos / bottom_center_to_local_pos_len : -Vec3::sAxisY(); + } + else + { + // Tapered cylinder + normal = Vec3(local_pos.GetX(), 0, local_pos.GetZ()).NormalizedOr(Vec3::sAxisX()); + normal.SetY(mTanAlpha); + normal = normal.NormalizedOr(Vec3::sAxisX()); + } + position = scaled_bottom_center + scaled_bottom_radius * normal; + } + + Plane plane = Plane::sFromPointAndNormal(position, normal); + float penetration = -plane.SignedDistance(local_pos); + if (v.UpdatePenetration(penetration)) + { + // Need to flip the normal's y if capsule is flipped (this corresponds to flipping both the point and the normal around y) + plane.SetNormal(scale_y_flip * plane.GetNormal()); + + // Store collision + v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); + } + } +} + +#ifdef JPH_DEBUG_RENDERER +void TaperedCapsuleShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + if (mGeometry == nullptr) + { + SupportBuffer buffer; + const Support *support = GetSupportFunction(ESupportMode::IncludeConvexRadius, buffer, Vec3::sReplicate(1.0f)); + mGeometry = inRenderer->CreateTriangleGeometryForConvex([support](Vec3Arg inDirection) { return support->GetSupport(inDirection); }); + } + + // Preserve flip along y axis but make sure we're not inside out + Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? Vec3(-1, 1, 1) * inScale : inScale; + RMat44 world_transform = inCenterOfMassTransform * Mat44::sScale(scale); + + AABox bounds = Shape::GetWorldSpaceBounds(inCenterOfMassTransform, inScale); + + float lod_scale_sq = Square(max(mTopRadius, mBottomRadius)); + + Color color = inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor; + + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + + inRenderer->DrawGeometry(world_transform, bounds, lod_scale_sq, color, mGeometry, DebugRenderer::ECullMode::CullBackFace, DebugRenderer::ECastShadow::On, draw_mode); +} +#endif // JPH_DEBUG_RENDERER + +AABox TaperedCapsuleShape::GetInertiaApproximation() const +{ + // TODO: For now the mass and inertia is that of a box + float avg_radius = 0.5f * (mTopRadius + mBottomRadius); + return AABox(Vec3(-avg_radius, mBottomCenter - mBottomRadius, -avg_radius), Vec3(avg_radius, mTopCenter + mTopRadius, avg_radius)); +} + +void TaperedCapsuleShape::SaveBinaryState(StreamOut &inStream) const +{ + ConvexShape::SaveBinaryState(inStream); + + inStream.Write(mCenterOfMass); + inStream.Write(mTopRadius); + inStream.Write(mBottomRadius); + inStream.Write(mTopCenter); + inStream.Write(mBottomCenter); + inStream.Write(mConvexRadius); + inStream.Write(mSinAlpha); + inStream.Write(mTanAlpha); +} + +void TaperedCapsuleShape::RestoreBinaryState(StreamIn &inStream) +{ + ConvexShape::RestoreBinaryState(inStream); + + inStream.Read(mCenterOfMass); + inStream.Read(mTopRadius); + inStream.Read(mBottomRadius); + inStream.Read(mTopCenter); + inStream.Read(mBottomCenter); + inStream.Read(mConvexRadius); + inStream.Read(mSinAlpha); + inStream.Read(mTanAlpha); +} + +bool TaperedCapsuleShape::IsValidScale(Vec3Arg inScale) const +{ + return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScale(inScale.Abs()); +} + +Vec3 TaperedCapsuleShape::MakeScaleValid(Vec3Arg inScale) const +{ + Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale); + + return scale.GetSign() * ScaleHelpers::MakeUniformScale(scale.Abs()); +} + +void TaperedCapsuleShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::TaperedCapsule); + f.mConstruct = []() -> Shape * { return new TaperedCapsuleShape; }; + f.mColor = Color::sGreen; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.gliffy b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.gliffy new file mode 100644 index 000000000000..3e5221b86556 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.gliffy @@ -0,0 +1 @@ +{"contentType":"application/gliffy+json","version":"1.1","metadata":{"title":"untitled","revision":0,"exportBorder":false},"embeddedResources":{"index":0,"resources":[]},"stage":{"objects":[{"x":870,"y":406,"rotation":0,"id":62,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":62,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":"1.0,1.0","startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[1.5,5.5],[1.5,-46.196228102251325]],"lockSegments":{}}},"children":null,"constraints":{"constraints":[],"startConstraint":{"type":"StartPositionConstraint","StartPositionConstraint":{"nodeId":4,"px":0.5,"py":0.5}}},"linkMap":[]},{"x":410,"y":406,"rotation":0,"id":60,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":60,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":"1.0,1.0","startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,5.5],[0,-49.03668490108288]],"lockSegments":{}}},"children":null,"constraints":{"constraints":[],"startConstraint":{"type":"StartPositionConstraint","StartPositionConstraint":{"nodeId":0,"px":0.5,"py":0.5}}},"linkMap":[]},{"x":614,"y":385,"rotation":0,"id":58,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":58,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[-204.06126531020038,0]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":626,"y":385,"rotation":0,"id":57,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":57,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[248.0020161208372,0]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":818,"y":520,"rotation":0,"id":55,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":55,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[-48,76]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":830,"y":502,"rotation":0,"id":54,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":54,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[48,-82]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":373,"y":410,"rotation":0,"id":50,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":28,"height":23,"lockAspectRatio":false,"lockShape":false,"order":19,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

tx

","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":387,"y":392,"rotation":0,"id":49,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":26,"height":23,"lockAspectRatio":false,"lockShape":false,"order":18,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

ty

","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":722,"y":488,"rotation":0,"id":45,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":20,"height":20,"lockAspectRatio":false,"lockShape":false,"order":16,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

bx

","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":440,"y":411.5,"rotation":0,"id":40,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":29,"height":28.500000000000004,"lockAspectRatio":false,"lockShape":false,"order":13,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

α

","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":411,"y":414,"rotation":0,"id":34,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":12,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":"1.0,1.0","startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[-1,-2.5],[349,180]],"lockSegments":{}}},"children":null,"constraints":{"constraints":[],"startConstraint":{"type":"StartPositionConstraint","StartPositionConstraint":{"nodeId":0,"px":0.5,"py":0.5}}},"linkMap":[]},{"x":744,"y":613,"rotation":0,"id":32,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":11,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":"1.0,1.0","startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[-1.1368683772161603e-13,-201.00995000248122]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":394,"y":436,"rotation":0,"id":30,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":10,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":"1.0,1.0","startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[0,-26.076809620810593]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":607,"y":368.5,"rotation":0,"id":26,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":20,"height":30,"lockAspectRatio":false,"lockShape":false,"order":9,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

h

","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":401,"y":412.5,"rotation":0,"id":25,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":20,"height":30,"lockAspectRatio":false,"lockShape":false,"order":8,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

tr

","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":808.5,"y":500,"rotation":0,"id":23,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":49,"height":27,"lockAspectRatio":false,"lockShape":false,"order":7,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

br - tr

","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":402,"y":416,"rotation":0,"id":21,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":6,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[8,-4.5],[-7,21]],"lockSegments":{}}},"children":null,"constraints":{"constraints":[],"startConstraint":{"type":"StartPositionConstraint","StartPositionConstraint":{"nodeId":0,"px":0.5,"py":0.5}}},"linkMap":[]},{"x":347,"y":412,"rotation":0,"id":16,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":5,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[523.5,2.5]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":854,"y":433,"rotation":0,"id":14,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":4,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[17.5,-21.5],[-106,184]],"lockSegments":{}}},"children":null,"constraints":{"constraints":[],"startConstraint":{"type":"StartPositionConstraint","StartPositionConstraint":{"nodeId":4,"px":0.5,"py":0.5}}},"linkMap":[]},{"x":395,"y":437,"rotation":0,"id":9,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":3,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[-52,-25],[695,360]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":395,"y":384,"rotation":0,"id":7,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":2,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[-49,26],[703,-359]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":630,"y":169.99999999999997,"rotation":0,"id":4,"uid":"com.gliffy.shape.basic.basic_v1.default.circle","width":483.00000000000006,"height":483.00000000000006,"lockAspectRatio":true,"lockShape":false,"order":1,"graphic":{"type":"Shape","Shape":{"tid":"com.gliffy.stencil.ellipse.basic_v1","strokeWidth":2,"strokeColor":"#333333","fillColor":"#FFFFFF","gradient":false,"dropShadow":false,"state":0,"shadowX":0,"shadowY":0,"opacity":1}},"children":[],"linkMap":[]},{"x":380,"y":381.5,"rotation":0,"id":0,"uid":"com.gliffy.shape.basic.basic_v1.default.circle","width":60,"height":60,"lockAspectRatio":true,"lockShape":false,"order":0,"graphic":{"type":"Shape","Shape":{"tid":"com.gliffy.stencil.ellipse.basic_v1","strokeWidth":2,"strokeColor":"#333333","fillColor":"#FFFFFF","gradient":false,"dropShadow":false,"state":0,"shadowX":0,"shadowY":0,"opacity":1}},"children":[],"linkMap":[]},{"x":728,"y":612,"rotation":0,"id":41,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":29,"height":28.500000000000004,"lockAspectRatio":false,"lockShape":false,"order":14,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

α

","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":375,"y":432.5,"rotation":0,"id":42,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":29,"height":28.500000000000004,"lockAspectRatio":false,"lockShape":false,"order":15,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

α

","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":753,"y":595.5,"rotation":0,"id":53,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":20,"height":30,"lockAspectRatio":false,"lockShape":false,"order":20,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

tr

","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":794,"y":400,"rotation":0,"id":65,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":65,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[-48.01041553663117,0]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":790,"y":393.25,"rotation":0,"id":46,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":29,"height":14,"lockAspectRatio":false,"lockShape":false,"order":17,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

by

","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":818,"y":400,"rotation":0,"id":66,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":66,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[54.230987451824944,0]],"lockSegments":{}}},"children":null,"linkMap":[]}],"background":"#FFFFFF","width":1113,"height":802,"maxWidth":5000,"maxHeight":5000,"nodeIndex":69,"autoFit":true,"exportBorder":false,"gridOn":true,"snapToGrid":false,"drawingGuidesOn":false,"pageBreaksOn":false,"printGridOn":false,"printPaper":"LETTER","printShrinkToFit":false,"printPortrait":true,"shapeStyles":{"com.gliffy.shape.basic.basic_v1.default":{"fill":"#FFFFFF","stroke":"#333333","strokeWidth":2}},"lineStyles":{"global":{"dashStyle":null,"endArrow":2,"startArrow":0}},"textStyles":{},"themeData":null}} \ No newline at end of file diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h new file mode 100644 index 000000000000..5e53ee136023 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h @@ -0,0 +1,135 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a TaperedCapsuleShape +class JPH_EXPORT TaperedCapsuleShapeSettings final : public ConvexShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, TaperedCapsuleShapeSettings) + +public: + /// Default constructor for deserialization + TaperedCapsuleShapeSettings() = default; + + /// Create a tapered capsule centered around the origin with one sphere cap at (0, -inHalfHeightOfTaperedCylinder, 0) with radius inBottomRadius and the other at (0, inHalfHeightOfTaperedCylinder, 0) with radius inTopRadius + TaperedCapsuleShapeSettings(float inHalfHeightOfTaperedCylinder, float inTopRadius, float inBottomRadius, const PhysicsMaterial *inMaterial = nullptr); + + /// Check if the settings are valid + bool IsValid() const { return mTopRadius > 0.0f && mBottomRadius > 0.0f && mHalfHeightOfTaperedCylinder >= 0.0f; } + + /// Checks if the settings of this tapered capsule make this shape a sphere + bool IsSphere() const; + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + float mHalfHeightOfTaperedCylinder = 0.0f; + float mTopRadius = 0.0f; + float mBottomRadius = 0.0f; +}; + +/// A capsule with different top and bottom radii +class JPH_EXPORT TaperedCapsuleShape final : public ConvexShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + TaperedCapsuleShape() : ConvexShape(EShapeSubType::TaperedCapsule) { } + TaperedCapsuleShape(const TaperedCapsuleShapeSettings &inSettings, ShapeResult &outResult); + + /// Get top radius of the tapered capsule + inline float GetTopRadius() const { return mTopRadius; } + + /// Get bottom radius of the tapered capsule + inline float GetBottomRadius() const { return mBottomRadius; } + + /// Get half height between the top and bottom sphere center + inline float GetHalfHeight() const { return 0.5f * (mTopCenter - mBottomCenter); } + + // See Shape::GetCenterOfMass + virtual Vec3 GetCenterOfMass() const override { return mCenterOfMass; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return min(mTopRadius, mBottomRadius); } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See ConvexShape::GetSupportFunction + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return GetLocalBounds().GetVolume(); } // Volume is approximate! + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // See Shape::MakeScaleValid + virtual Vec3 MakeScaleValid(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Class for GetSupportFunction + class TaperedCapsule; + + /// Returns box that approximates the inertia + AABox GetInertiaApproximation() const; + + Vec3 mCenterOfMass = Vec3::sZero(); + float mTopRadius = 0.0f; + float mBottomRadius = 0.0f; + float mTopCenter = 0.0f; + float mBottomCenter = 0.0f; + float mConvexRadius = 0.0f; + float mSinAlpha = 0.0f; + float mTanAlpha = 0.0f; + +#ifdef JPH_DEBUG_RENDERER + mutable DebugRenderer::GeometryRef mGeometry; +#endif // JPH_DEBUG_RENDERER +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp new file mode 100644 index 000000000000..d817644fb44d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp @@ -0,0 +1,687 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +// Approximation of a face of the tapered cylinder +static const Vec3 cTaperedCylinderFace[] = +{ + Vec3(0.0f, 0.0f, 1.0f), + Vec3(0.707106769f, 0.0f, 0.707106769f), + Vec3(1.0f, 0.0f, 0.0f), + Vec3(0.707106769f, 0.0f, -0.707106769f), + Vec3(-0.0f, 0.0f, -1.0f), + Vec3(-0.707106769f, 0.0f, -0.707106769f), + Vec3(-1.0f, 0.0f, 0.0f), + Vec3(-0.707106769f, 0.0f, 0.707106769f) +}; + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TaperedCylinderShapeSettings) +{ + JPH_ADD_BASE_CLASS(TaperedCylinderShapeSettings, ConvexShapeSettings) + + JPH_ADD_ATTRIBUTE(TaperedCylinderShapeSettings, mHalfHeight) + JPH_ADD_ATTRIBUTE(TaperedCylinderShapeSettings, mTopRadius) + JPH_ADD_ATTRIBUTE(TaperedCylinderShapeSettings, mBottomRadius) + JPH_ADD_ATTRIBUTE(TaperedCylinderShapeSettings, mConvexRadius) +} + +ShapeSettings::ShapeResult TaperedCylinderShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + { + Ref shape; + if (mTopRadius == mBottomRadius) + { + // Convert to regular cylinder + CylinderShapeSettings settings; + settings.mHalfHeight = mHalfHeight; + settings.mRadius = mTopRadius; + settings.mMaterial = mMaterial; + settings.mConvexRadius = mConvexRadius; + new CylinderShape(settings, mCachedResult); + } + else + { + // Normal tapered cylinder shape + new TaperedCylinderShape(*this, mCachedResult); + } + } + return mCachedResult; +} + +TaperedCylinderShapeSettings::TaperedCylinderShapeSettings(float inHalfHeightOfTaperedCylinder, float inTopRadius, float inBottomRadius, float inConvexRadius, const PhysicsMaterial *inMaterial) : + ConvexShapeSettings(inMaterial), + mHalfHeight(inHalfHeightOfTaperedCylinder), + mTopRadius(inTopRadius), + mBottomRadius(inBottomRadius), + mConvexRadius(inConvexRadius) +{ +} + +TaperedCylinderShape::TaperedCylinderShape(const TaperedCylinderShapeSettings &inSettings, ShapeResult &outResult) : + ConvexShape(EShapeSubType::TaperedCylinder, inSettings, outResult), + mTopRadius(inSettings.mTopRadius), + mBottomRadius(inSettings.mBottomRadius), + mConvexRadius(inSettings.mConvexRadius) +{ + if (mTopRadius < 0.0f) + { + outResult.SetError("Invalid top radius"); + return; + } + + if (mBottomRadius < 0.0f) + { + outResult.SetError("Invalid bottom radius"); + return; + } + + if (inSettings.mHalfHeight <= 0.0f) + { + outResult.SetError("Invalid height"); + return; + } + + if (inSettings.mConvexRadius < 0.0f) + { + outResult.SetError("Invalid convex radius"); + return; + } + + if (inSettings.mTopRadius < inSettings.mConvexRadius) + { + outResult.SetError("Convex radius must be smaller than convex radius"); + return; + } + + if (inSettings.mBottomRadius < inSettings.mConvexRadius) + { + outResult.SetError("Convex radius must be smaller than bottom radius"); + return; + } + + // Calculate the center of mass (using wxMaxima). + // Radius of cross section for tapered cylinder from 0 to h: + // r(x):=br+x*(tr-br)/h; + // Area: + // area(x):=%pi*r(x)^2; + // Total volume of cylinder: + // volume(h):=integrate(area(x),x,0,h); + // Center of mass: + // com(br,tr,h):=integrate(x*area(x),x,0,h)/volume(h); + // Results: + // ratsimp(com(br,tr,h),br,bt); + // Non-tapered cylinder should have com = 0.5: + // ratsimp(com(r,r,h)); + // Cone with tip at origin and height h should have com = 3/4 h + // ratsimp(com(0,r,h)); + float h = 2.0f * inSettings.mHalfHeight; + float tr = mTopRadius; + float tr2 = Square(tr); + float br = mBottomRadius; + float br2 = Square(br); + float com = h * (3 * tr2 + 2 * br * tr + br2) / (4.0f * (tr2 + br * tr + br2)); + mTop = h - com; + mBottom = -com; + + outResult.Set(this); +} + +class TaperedCylinderShape::TaperedCylinder final : public Support +{ +public: + TaperedCylinder(float inTop, float inBottom, float inTopRadius, float inBottomRadius, float inConvexRadius) : + mTop(inTop), + mBottom(inBottom), + mTopRadius(inTopRadius), + mBottomRadius(inBottomRadius), + mConvexRadius(inConvexRadius) + { + static_assert(sizeof(TaperedCylinder) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(TaperedCylinder))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + float x = inDirection.GetX(), y = inDirection.GetY(), z = inDirection.GetZ(); + float o = sqrt(Square(x) + Square(z)); + if (o > 0.0f) + { + Vec3 top_support((mTopRadius * x) / o, mTop, (mTopRadius * z) / o); + Vec3 bottom_support((mBottomRadius * x) / o, mBottom, (mBottomRadius * z) / o); + return inDirection.Dot(top_support) > inDirection.Dot(bottom_support)? top_support : bottom_support; + } + else + { + if (y > 0.0f) + return Vec3(0, mTop, 0); + else + return Vec3(0, mBottom, 0); + } + } + + virtual float GetConvexRadius() const override + { + return mConvexRadius; + } + +private: + float mTop; + float mBottom; + float mTopRadius; + float mBottomRadius; + float mConvexRadius; +}; + +JPH_INLINE void TaperedCylinderShape::GetScaled(Vec3Arg inScale, float &outTop, float &outBottom, float &outTopRadius, float &outBottomRadius, float &outConvexRadius) const +{ + Vec3 abs_scale = inScale.Abs(); + float scale_xz = abs_scale.GetX(); + float scale_y = inScale.GetY(); + + outTop = scale_y * mTop; + outBottom = scale_y * mBottom; + outTopRadius = scale_xz * mTopRadius; + outBottomRadius = scale_xz * mBottomRadius; + outConvexRadius = min(abs_scale.GetY(), scale_xz) * mConvexRadius; + + // Negative Y-scale flips the top and bottom + if (outBottom > outTop) + { + std::swap(outTop, outBottom); + std::swap(outTopRadius, outBottomRadius); + } +} + +const ConvexShape::Support *TaperedCylinderShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + // Get scaled tapered cylinder + float top, bottom, top_radius, bottom_radius, convex_radius; + GetScaled(inScale, top, bottom, top_radius, bottom_radius, convex_radius); + + switch (inMode) + { + case ESupportMode::IncludeConvexRadius: + case ESupportMode::Default: + return new (&inBuffer) TaperedCylinder(top, bottom, top_radius, bottom_radius, 0.0f); + + case ESupportMode::ExcludeConvexRadius: + return new (&inBuffer) TaperedCylinder(top - convex_radius, bottom + convex_radius, top_radius - convex_radius, bottom_radius - convex_radius, convex_radius); + } + + JPH_ASSERT(false); + return nullptr; +} + +JPH_INLINE static Vec3 sCalculateSideNormalXZ(Vec3Arg inSurfacePosition) +{ + return (Vec3(1, 0, 1) * inSurfacePosition).NormalizedOr(Vec3::sAxisX()); +} + +JPH_INLINE static Vec3 sCalculateSideNormal(Vec3Arg inNormalXZ, float inTop, float inBottom, float inTopRadius, float inBottomRadius) +{ + float tan_alpha = (inBottomRadius - inTopRadius) / (inTop - inBottom); + return Vec3(inNormalXZ.GetX(), tan_alpha, inNormalXZ.GetZ()).Normalized(); +} + +void TaperedCylinderShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + JPH_ASSERT(IsValidScale(inScale)); + + // Get scaled tapered cylinder + float top, bottom, top_radius, bottom_radius, convex_radius; + GetScaled(inScale, top, bottom, top_radius, bottom_radius, convex_radius); + + // Get the normal of the side of the cylinder + Vec3 normal_xz = sCalculateSideNormalXZ(-inDirection); + Vec3 normal = sCalculateSideNormal(normal_xz, top, bottom, top_radius, bottom_radius); + + constexpr float cMinRadius = 1.0e-3f; + + // Check if the normal is closer to the side than to the top or bottom + if (abs(normal.Dot(inDirection)) > abs(inDirection.GetY())) + { + // Return the side of the cylinder + outVertices.push_back(inCenterOfMassTransform * (normal_xz * top_radius + Vec3(0, top, 0))); + outVertices.push_back(inCenterOfMassTransform * (normal_xz * bottom_radius + Vec3(0, bottom, 0))); + } + else if (inDirection.GetY() < 0.0f) + { + // Top of the cylinder + if (top_radius > cMinRadius) + { + Vec3 top_3d(0, top, 0); + for (Vec3 v : cTaperedCylinderFace) + outVertices.push_back(inCenterOfMassTransform * (top_radius * v + top_3d)); + } + } + else + { + // Bottom of the cylinder + if (bottom_radius > cMinRadius) + { + Vec3 bottom_3d(0, bottom, 0); + for (const Vec3 *v = cTaperedCylinderFace + std::size(cTaperedCylinderFace) - 1; v >= cTaperedCylinderFace; --v) + outVertices.push_back(inCenterOfMassTransform * (bottom_radius * *v + bottom_3d)); + } + } +} + +MassProperties TaperedCylinderShape::GetMassProperties() const +{ + MassProperties p; + + // Calculate mass + float density = GetDensity(); + p.mMass = GetVolume() * density; + + // Calculate inertia of a tapered cylinder (using wxMaxima) + // Radius: + // r(x):=br+(x-b)*(tr-br)/(t-b); + // Where t=top, b=bottom, tr=top radius, br=bottom radius + // Area of the cross section of the cylinder at x: + // area(x):=%pi*r(x)^2; + // Inertia x slice at x (using inertia of a solid disc, see https://en.wikipedia.org/wiki/List_of_moments_of_inertia, note needs to be multiplied by density): + // dix(x):=area(x)*r(x)^2/4; + // Inertia y slice at y (note needs to be multiplied by density) + // diy(x):=area(x)*r(x)^2/2; + // Volume: + // volume(b,t):=integrate(area(x),x,b,t); + // The constant density (note that we have this through GetDensity() so we'll use that instead): + // density(b,t):=m/volume(b,t); + // Inertia tensor element xx, note that we use the parallel axis theorem to move the inertia: Ixx' = Ixx + m translation^2, also note we multiply by density here: + // Ixx(br,tr,b,t):=integrate(dix(x)+area(x)*x^2,x,b,t)*density(b,t); + // Inertia tensor element yy: + // Iyy(br,tr,b,t):=integrate(diy(x),x,b,t)*density(b,t); + // Note that we can simplfy Ixx by using: + // Ixx_delta(br,tr,b,t):=Ixx(br,tr,b,t)-Iyy(br,tr,b,t)/2; + // For a cylinder this formula matches what is listed on the wiki: + // factor(Ixx(r,r,-h/2,h/2)); + // factor(Iyy(r,r,-h/2,h/2)); + // For a cone with tip at origin too: + // factor(Ixx(0,r,0,h)); + // factor(Iyy(0,r,0,h)); + // Now for the tapered cylinder: + // rat(Ixx(br,tr,b,t),br,bt); + // rat(Iyy(br,tr,b,t),br,bt); + // rat(Ixx_delta(br,tr,b,t),br,bt); + float t = mTop; + float t2 = Square(t); + float t3 = t * t2; + + float b = mBottom; + float b2 = Square(b); + float b3 = b * b2; + + float br = mBottomRadius; + float br2 = Square(br); + float br3 = br * br2; + float br4 = Square(br2); + + float tr = mTopRadius; + float tr2 = Square(tr); + float tr3 = tr * tr2; + float tr4 = Square(tr2); + + float inertia_y = (JPH_PI / 10.0f) * density * (t - b) * (br4 + tr * br3 + tr2 * br2 + tr3 * br + tr4); + float inertia_x_delta = (JPH_PI / 30.0f) * density * ((t3 + 2 * b * t2 + 3 * b2 * t - 6 * b3) * br2 + (3 * t3 + b * t2 - b2 * t - 3 * b3) * tr * br + (6 * t3 - 3 * b * t2 - 2 * b2 * t - b3) * tr2); + float inertia_x = inertia_x_delta + inertia_y / 2; + float inertia_z = inertia_x; + p.mInertia = Mat44::sScale(Vec3(inertia_x, inertia_y, inertia_z)); + return p; +} + +Vec3 TaperedCylinderShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + constexpr float cEpsilon = 1.0e-5f; + + if (inLocalSurfacePosition.GetY() > mTop - cEpsilon) + return Vec3(0, 1, 0); + else if (inLocalSurfacePosition.GetY() < mBottom + cEpsilon) + return Vec3(0, -1, 0); + else + return sCalculateSideNormal(sCalculateSideNormalXZ(inLocalSurfacePosition), mTop, mBottom, mTopRadius, mBottomRadius); +} + +AABox TaperedCylinderShape::GetLocalBounds() const +{ + float max_radius = max(mTopRadius, mBottomRadius); + return AABox(Vec3(-max_radius, mBottom, -max_radius), Vec3(max_radius, mTop, max_radius)); +} + +void TaperedCylinderShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Check if the point is in the tapered cylinder + if (inPoint.GetY() >= mBottom && inPoint.GetY() <= mTop // Within height + && Square(inPoint.GetX()) + Square(inPoint.GetZ()) <= Square(mBottomRadius + (inPoint.GetY() - mBottom) * (mTopRadius - mBottomRadius) / (mTop - mBottom))) // Within the radius + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); +} + +void TaperedCylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation(); + + // Get scaled tapered cylinder + float top, bottom, top_radius, bottom_radius, convex_radius; + GetScaled(inScale, top, bottom, top_radius, bottom_radius, convex_radius); + Vec3 top_3d(0, top, 0); + Vec3 bottom_3d(0, bottom, 0); + + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) + { + Vec3 local_pos = inverse_transform * v.GetPosition(); + + // Calculate penetration into side surface + Vec3 normal_xz = sCalculateSideNormalXZ(local_pos); + Vec3 side_normal = sCalculateSideNormal(normal_xz, top, bottom, top_radius, bottom_radius); + Vec3 side_support_top = normal_xz * top_radius + top_3d; + float side_penetration = (side_support_top - local_pos).Dot(side_normal); + + // Calculate penetration into top and bottom plane + float top_penetration = top - local_pos.GetY(); + float bottom_penetration = local_pos.GetY() - bottom; + float min_top_bottom_penetration = min(top_penetration, bottom_penetration); + + Vec3 point, normal; + if (side_penetration < 0.0f || min_top_bottom_penetration < 0.0f) + { + // We're outside the cylinder + // Calculate the closest point on the line segment from bottom to top support point: + // closest_point = bottom + fraction * (top - bottom) / |top - bottom|^2 + Vec3 side_support_bottom = normal_xz * bottom_radius + bottom_3d; + Vec3 bottom_to_top = side_support_top - side_support_bottom; + float fraction = (local_pos - side_support_bottom).Dot(bottom_to_top); + + // Calculate the distance to the axis of the cylinder + float distance_to_axis = normal_xz.Dot(local_pos); + bool inside_top_radius = distance_to_axis <= top_radius; + bool inside_bottom_radius = distance_to_axis <= bottom_radius; + + /* + Regions of tapered cylinder (side view): + + _ B | | + --_ | A | + t-------+ + C / \ + / tapered \ + _ / cylinder \ + --_ / \ + b-----------------+ + D | E | + | | + + t = side_support_top, b = side_support_bottom + Lines between B and C and C and D are at a 90 degree angle to the line between t and b + */ + if (fraction >= bottom_to_top.LengthSq() // Region B: Above the line segment + && !inside_top_radius) // Outside the top radius + { + // Top support point is closest + point = side_support_top; + normal = (local_pos - point).NormalizedOr(Vec3::sAxisY()); + } + else if (fraction < 0.0f // Region D: Below the line segment + && !inside_bottom_radius) // Outside the bottom radius + { + // Bottom support point is closest + point = side_support_bottom; + normal = (local_pos - point).NormalizedOr(Vec3::sAxisY()); + } + else if (top_penetration < 0.0f // Region A: Above the top plane + && inside_top_radius) // Inside the top radius + { + // Top plane is closest + point = top_3d; + normal = Vec3(0, 1, 0); + } + else if (bottom_penetration < 0.0f // Region E: Below the bottom plane + && inside_bottom_radius) // Inside the bottom radius + { + // Bottom plane is closest + point = bottom_3d; + normal = Vec3(0, -1, 0); + } + else // Region C + { + // Side surface is closest + point = side_support_top; + normal = side_normal; + } + } + else if (side_penetration < min_top_bottom_penetration) + { + // Side surface is closest + point = side_support_top; + normal = side_normal; + } + else if (top_penetration < bottom_penetration) + { + // Top plane is closest + point = top_3d; + normal = Vec3(0, 1, 0); + } + else + { + // Bottom plane is closest + point = bottom_3d; + normal = Vec3(0, -1, 0); + } + + // Calculate penetration + Plane plane = Plane::sFromPointAndNormal(point, normal); + float penetration = -plane.SignedDistance(local_pos); + if (v.UpdatePenetration(penetration)) + v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); + } +} + +class TaperedCylinderShape::TCSGetTrianglesContext +{ +public: + explicit TCSGetTrianglesContext(Mat44Arg inTransform) : mTransform(inTransform) { } + + Mat44 mTransform; + uint mProcessed = 0; // Which elements we processed, bit 0 = top, bit 1 = bottom, bit 2 = side +}; + +void TaperedCylinderShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + static_assert(sizeof(TCSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small"); + 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; + + // Mark top and bottom processed if their radius is too small + TCSGetTrianglesContext *context = new (&ioContext) TCSGetTrianglesContext(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(scale)); + constexpr float cMinRadius = 1.0e-3f; + if (mTopRadius < cMinRadius) + context->mProcessed |= 0b001; + if (mBottomRadius < cMinRadius) + context->mProcessed |= 0b010; +} + +int TaperedCylinderShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + constexpr int cNumVertices = int(std::size(cTaperedCylinderFace)); + + static_assert(cGetTrianglesMinTrianglesRequested >= 2 * cNumVertices); + JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested); + + TCSGetTrianglesContext &context = (TCSGetTrianglesContext &)ioContext; + + int total_num_triangles = 0; + + // Top cap + Vec3 top_3d(0, mTop, 0); + if ((context.mProcessed & 0b001) == 0) + { + Vec3 v0 = context.mTransform * (top_3d + mTopRadius * cTaperedCylinderFace[0]); + Vec3 v1 = context.mTransform * (top_3d + mTopRadius * cTaperedCylinderFace[1]); + + for (const Vec3 *v = cTaperedCylinderFace + 2, *v_end = cTaperedCylinderFace + cNumVertices; v < v_end; ++v) + { + Vec3 v2 = context.mTransform * (top_3d + mTopRadius * *v); + + v0.StoreFloat3(outTriangleVertices++); + v1.StoreFloat3(outTriangleVertices++); + v2.StoreFloat3(outTriangleVertices++); + + v1 = v2; + } + + total_num_triangles = cNumVertices - 2; + context.mProcessed |= 0b001; + } + + // Bottom cap + Vec3 bottom_3d(0, mBottom, 0); + if ((context.mProcessed & 0b010) == 0 + && total_num_triangles + cNumVertices - 2 < inMaxTrianglesRequested) + { + Vec3 v0 = context.mTransform * (bottom_3d + mBottomRadius * cTaperedCylinderFace[0]); + Vec3 v1 = context.mTransform * (bottom_3d + mBottomRadius * cTaperedCylinderFace[1]); + + for (const Vec3 *v = cTaperedCylinderFace + 2, *v_end = cTaperedCylinderFace + cNumVertices; v < v_end; ++v) + { + Vec3 v2 = context.mTransform * (bottom_3d + mBottomRadius * *v); + + v0.StoreFloat3(outTriangleVertices++); + v2.StoreFloat3(outTriangleVertices++); + v1.StoreFloat3(outTriangleVertices++); + + v1 = v2; + } + + total_num_triangles += cNumVertices - 2; + context.mProcessed |= 0b010; + } + + // Side + if ((context.mProcessed & 0b100) == 0 + && total_num_triangles + 2 * cNumVertices < inMaxTrianglesRequested) + { + Vec3 v0t = context.mTransform * (top_3d + mTopRadius * cTaperedCylinderFace[cNumVertices - 1]); + Vec3 v0b = context.mTransform * (bottom_3d + mBottomRadius * cTaperedCylinderFace[cNumVertices - 1]); + + for (const Vec3 *v = cTaperedCylinderFace, *v_end = cTaperedCylinderFace + cNumVertices; v < v_end; ++v) + { + Vec3 v1t = context.mTransform * (top_3d + mTopRadius * *v); + v0t.StoreFloat3(outTriangleVertices++); + v0b.StoreFloat3(outTriangleVertices++); + v1t.StoreFloat3(outTriangleVertices++); + + Vec3 v1b = context.mTransform * (bottom_3d + mBottomRadius * *v); + v1t.StoreFloat3(outTriangleVertices++); + v0b.StoreFloat3(outTriangleVertices++); + v1b.StoreFloat3(outTriangleVertices++); + + v0t = v1t; + v0b = v1b; + } + + total_num_triangles += 2 * cNumVertices; + context.mProcessed |= 0b100; + } + + // Store materials + if (outMaterials != nullptr) + { + const PhysicsMaterial *material = GetMaterial(); + for (const PhysicsMaterial **m = outMaterials, **m_end = outMaterials + total_num_triangles; m < m_end; ++m) + *m = material; + } + + return total_num_triangles; +} + +#ifdef JPH_DEBUG_RENDERER +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; + RMat44 world_transform = inCenterOfMassTransform * Mat44::sScale(scale); + + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + inRenderer->DrawTaperedCylinder(world_transform, mTop, mBottom, mTopRadius, mBottomRadius, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode); +} +#endif // JPH_DEBUG_RENDERER + +void TaperedCylinderShape::SaveBinaryState(StreamOut &inStream) const +{ + ConvexShape::SaveBinaryState(inStream); + + inStream.Write(mTop); + inStream.Write(mBottom); + inStream.Write(mTopRadius); + inStream.Write(mBottomRadius); + inStream.Write(mConvexRadius); +} + +void TaperedCylinderShape::RestoreBinaryState(StreamIn &inStream) +{ + ConvexShape::RestoreBinaryState(inStream); + + inStream.Read(mTop); + inStream.Read(mBottom); + inStream.Read(mTopRadius); + inStream.Read(mBottomRadius); + inStream.Read(mConvexRadius); +} + +float TaperedCylinderShape::GetVolume() const +{ + // Volume of a tapered cylinder is: integrate(%pi*(b+x*(t-b)/h)^2,x,0,h) where t is the top radius, b is the bottom radius and h is the height + return (JPH_PI / 3.0f) * (mTop - mBottom) * (Square(mTopRadius) + mTopRadius * mBottomRadius + Square(mBottomRadius)); +} + +bool TaperedCylinderShape::IsValidScale(Vec3Arg inScale) const +{ + return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScaleXZ(inScale.Abs()); +} + +Vec3 TaperedCylinderShape::MakeScaleValid(Vec3Arg inScale) const +{ + Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale); + + return scale.GetSign() * ScaleHelpers::MakeUniformScaleXZ(scale.Abs()); +} + +void TaperedCylinderShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::TaperedCylinder); + f.mConstruct = []() -> Shape * { return new TaperedCylinderShape; }; + f.mColor = Color::sGreen; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.h new file mode 100644 index 000000000000..e48ca93d0f8a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.h @@ -0,0 +1,132 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a TaperedCylinderShape +class JPH_EXPORT TaperedCylinderShapeSettings final : public ConvexShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, TaperedCylinderShapeSettings) + +public: + /// Default constructor for deserialization + TaperedCylinderShapeSettings() = default; + + /// Create a tapered cylinder centered around the origin with bottom at (0, -inHalfHeightOfTaperedCylinder, 0) with radius inBottomRadius and top at (0, inHalfHeightOfTaperedCylinder, 0) with radius inTopRadius + TaperedCylinderShapeSettings(float inHalfHeightOfTaperedCylinder, float inTopRadius, float inBottomRadius, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr); + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + float mHalfHeight = 0.0f; + float mTopRadius = 0.0f; + float mBottomRadius = 0.0f; + float mConvexRadius = 0.0f; +}; + +/// A cylinder with different top and bottom radii +class JPH_EXPORT TaperedCylinderShape final : public ConvexShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + TaperedCylinderShape() : ConvexShape(EShapeSubType::TaperedCylinder) { } + TaperedCylinderShape(const TaperedCylinderShapeSettings &inSettings, ShapeResult &outResult); + + /// Get top radius of the tapered cylinder + inline float GetTopRadius() const { return mTopRadius; } + + /// Get bottom radius of the tapered cylinder + inline float GetBottomRadius() const { return mBottomRadius; } + + /// Get convex radius of the tapered cylinder + inline float GetConvexRadius() const { return mConvexRadius; } + + /// Get half height of the tapered cylinder + inline float GetHalfHeight() const { return 0.5f * (mTop - mBottom); } + + // See Shape::GetCenterOfMass + virtual Vec3 GetCenterOfMass() const override { return Vec3(0, -0.5f * (mTop + mBottom), 0); } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return min(mTopRadius, mBottomRadius); } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See ConvexShape::GetSupportFunction + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override; + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // See Shape::MakeScaleValid + virtual Vec3 MakeScaleValid(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Class for GetSupportFunction + class TaperedCylinder; + + // Class for GetTrianglesTart + class TCSGetTrianglesContext; + + // Scale the cylinder + JPH_INLINE void GetScaled(Vec3Arg inScale, float &outTop, float &outBottom, float &outTopRadius, float &outBottomRadius, float &outConvexRadius) const; + + float mTop = 0.0f; + float mBottom = 0.0f; + float mTopRadius = 0.0f; + float mBottomRadius = 0.0f; + float mConvexRadius = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.cpp new file mode 100644 index 000000000000..5f2454bbb5bf --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.cpp @@ -0,0 +1,423 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TriangleShapeSettings) +{ + JPH_ADD_BASE_CLASS(TriangleShapeSettings, ConvexShapeSettings) + + JPH_ADD_ATTRIBUTE(TriangleShapeSettings, mV1) + JPH_ADD_ATTRIBUTE(TriangleShapeSettings, mV2) + JPH_ADD_ATTRIBUTE(TriangleShapeSettings, mV3) + JPH_ADD_ATTRIBUTE(TriangleShapeSettings, mConvexRadius) +} + +ShapeSettings::ShapeResult TriangleShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new TriangleShape(*this, mCachedResult); + return mCachedResult; +} + +TriangleShape::TriangleShape(const TriangleShapeSettings &inSettings, ShapeResult &outResult) : + ConvexShape(EShapeSubType::Triangle, inSettings, outResult), + mV1(inSettings.mV1), + mV2(inSettings.mV2), + mV3(inSettings.mV3), + mConvexRadius(inSettings.mConvexRadius) +{ + if (inSettings.mConvexRadius < 0.0f) + { + outResult.SetError("Invalid convex radius"); + return; + } + + outResult.Set(this); +} + +AABox TriangleShape::GetLocalBounds() const +{ + AABox bounds(mV1, mV1); + bounds.Encapsulate(mV2); + bounds.Encapsulate(mV3); + bounds.ExpandBy(Vec3::sReplicate(mConvexRadius)); + return bounds; +} + +AABox TriangleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Vec3 v1 = inCenterOfMassTransform * (inScale * mV1); + Vec3 v2 = inCenterOfMassTransform * (inScale * mV2); + Vec3 v3 = inCenterOfMassTransform * (inScale * mV3); + + AABox bounds(v1, v1); + bounds.Encapsulate(v2); + bounds.Encapsulate(v3); + bounds.ExpandBy(inScale * mConvexRadius); + return bounds; +} + +class TriangleShape::TriangleNoConvex final : public Support +{ +public: + TriangleNoConvex(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3) : + mTriangleSuport(inV1, inV2, inV3) + { + static_assert(sizeof(TriangleNoConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(TriangleNoConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + return mTriangleSuport.GetSupport(inDirection); + } + + virtual float GetConvexRadius() const override + { + return 0.0f; + } + +private: + TriangleConvexSupport mTriangleSuport; +}; + +class TriangleShape::TriangleWithConvex final : public Support +{ +public: + TriangleWithConvex(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, float inConvexRadius) : + mConvexRadius(inConvexRadius), + mTriangleSuport(inV1, inV2, inV3) + { + static_assert(sizeof(TriangleWithConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(TriangleWithConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + Vec3 support = mTriangleSuport.GetSupport(inDirection); + float len = inDirection.Length(); + if (len > 0.0f) + support += (mConvexRadius / len) * inDirection; + return support; + } + + virtual float GetConvexRadius() const override + { + return mConvexRadius; + } + +private: + float mConvexRadius; + TriangleConvexSupport mTriangleSuport; +}; + +const ConvexShape::Support *TriangleShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const +{ + switch (inMode) + { + case ESupportMode::IncludeConvexRadius: + case ESupportMode::Default: + if (mConvexRadius > 0.0f) + return new (&inBuffer) TriangleWithConvex(inScale * mV1, inScale * mV2, inScale * mV3, mConvexRadius); + [[fallthrough]]; + + case ESupportMode::ExcludeConvexRadius: + return new (&inBuffer) TriangleNoConvex(inScale * mV1, inScale * mV2, inScale * mV3); + } + + JPH_ASSERT(false); + return nullptr; +} + +void TriangleShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + // Calculate transform with scale + Mat44 transform = inCenterOfMassTransform.PreScaled(inScale); + + // Flip triangle if scaled inside out + if (ScaleHelpers::IsInsideOut(inScale)) + { + outVertices.push_back(transform * mV1); + outVertices.push_back(transform * mV3); + outVertices.push_back(transform * mV2); + } + else + { + outVertices.push_back(transform * mV1); + outVertices.push_back(transform * mV2); + outVertices.push_back(transform * mV3); + } +} + +MassProperties TriangleShape::GetMassProperties() const +{ + // We cannot calculate the volume for a triangle, so we return invalid mass properties. + // If you want your triangle to be dynamic, then you should provide the mass properties yourself when + // creating a Body: + // + // BodyCreationSettings::mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided; + // BodyCreationSettings::mMassPropertiesOverride.SetMassAndInertiaOfSolidBox(Vec3::sReplicate(1.0f), 1000.0f); + // + // Note that this makes the triangle shape behave the same as a mesh shape with a single triangle. + // In practice there is very little use for a dynamic triangle shape as back side collisions will be ignored + // so if the triangle falls the wrong way it will sink through the floor. + return MassProperties(); +} + +Vec3 TriangleShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + Vec3 cross = (mV2 - mV1).Cross(mV3 - mV1); + float len = cross.Length(); + return len != 0.0f? cross / len : Vec3::sAxisY(); +} + +void TriangleShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + // A triangle has no volume + outTotalVolume = outSubmergedVolume = 0.0f; + outCenterOfBuoyancy = Vec3::sZero(); +} + +#ifdef JPH_DEBUG_RENDERER +void TriangleShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + RVec3 v1 = inCenterOfMassTransform * (inScale * mV1); + RVec3 v2 = inCenterOfMassTransform * (inScale * mV2); + RVec3 v3 = inCenterOfMassTransform * (inScale * mV3); + + if (ScaleHelpers::IsInsideOut(inScale)) + std::swap(v1, v2); + + if (inDrawWireframe) + inRenderer->DrawWireTriangle(v1, v2, v3, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor); + else + inRenderer->DrawTriangle(v1, v2, v3, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor); +} +#endif // JPH_DEBUG_RENDERER + +bool TriangleShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + float fraction = RayTriangle(inRay.mOrigin, inRay.mDirection, mV1, mV2, mV3); + if (fraction < ioHit.mFraction) + { + ioHit.mFraction = fraction; + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + return false; +} + +void TriangleShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Back facing check + if (inRayCastSettings.mBackFaceModeTriangles == EBackFaceMode::IgnoreBackFaces && (mV2 - mV1).Cross(mV3 - mV1).Dot(inRay.mDirection) > 0.0f) + return; + + // Test ray against triangle + float fraction = RayTriangle(inRay.mOrigin, inRay.mDirection, mV1, mV2, mV3); + if (fraction < ioCollector.GetEarlyOutFraction()) + { + // Better hit than the current hit + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + hit.mFraction = fraction; + hit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + ioCollector.AddHit(hit); + } +} + +void TriangleShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Can't be inside a triangle +} + +void TriangleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + CollideSoftBodyVerticesVsTriangles collider(inCenterOfMassTransform, inScale); + + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) + { + collider.StartVertex(v); + collider.ProcessTriangle(mV1, mV2, mV3); + collider.FinishVertex(v, inCollidingShapeIndex); + } +} + +void TriangleShape::sCollideConvexVsTriangle(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, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape1->GetType() == EShapeType::Convex); + const ConvexShape *shape1 = static_cast(inShape1); + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::Triangle); + const TriangleShape *shape2 = static_cast(inShape2); + + CollideConvexVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + collider.Collide(shape2->mV1, shape2->mV2, shape2->mV3, 0b111, inSubShapeIDCreator2.GetID()); +} + +void TriangleShape::sCollideSphereVsTriangle(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, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Sphere); + const SphereShape *shape1 = static_cast(inShape1); + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::Triangle); + const TriangleShape *shape2 = static_cast(inShape2); + + CollideSphereVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + collider.Collide(shape2->mV1, shape2->mV2, shape2->mV3, 0b111, inSubShapeIDCreator2.GetID()); +} + +void TriangleShape::sCastConvexVsTriangle(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Triangle); + const TriangleShape *shape = static_cast(inShape); + + CastConvexVsTriangles caster(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + caster.Cast(shape->mV1, shape->mV2, shape->mV3, 0b111, inSubShapeIDCreator2.GetID()); +} + +void TriangleShape::sCastSphereVsTriangle(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Triangle); + const TriangleShape *shape = static_cast(inShape); + + CastSphereVsTriangles caster(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + caster.Cast(shape->mV1, shape->mV2, shape->mV3, 0b111, inSubShapeIDCreator2.GetID()); +} + +class TriangleShape::TSGetTrianglesContext +{ +public: + TSGetTrianglesContext(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3) : mV1(inV1), mV2(inV2), mV3(inV3) { } + + Vec3 mV1; + Vec3 mV2; + Vec3 mV3; + + bool mIsDone = false; +}; + +void TriangleShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + static_assert(sizeof(TSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(&ioContext, alignof(TSGetTrianglesContext))); + + Mat44 m = Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale); + + new (&ioContext) TSGetTrianglesContext(m * mV1, m * mV2, m * mV3); +} + +int TriangleShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + static_assert(cGetTrianglesMinTrianglesRequested >= 3, "cGetTrianglesMinTrianglesRequested is too small"); + JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested); + + TSGetTrianglesContext &context = (TSGetTrianglesContext &)ioContext; + + // Only return the triangle the 1st time + if (context.mIsDone) + return 0; + context.mIsDone = true; + + // Store triangle + context.mV1.StoreFloat3(outTriangleVertices); + context.mV2.StoreFloat3(outTriangleVertices + 1); + context.mV3.StoreFloat3(outTriangleVertices + 2); + + // Store material + if (outMaterials != nullptr) + *outMaterials = GetMaterial(); + + return 1; +} + +void TriangleShape::SaveBinaryState(StreamOut &inStream) const +{ + ConvexShape::SaveBinaryState(inStream); + + inStream.Write(mV1); + inStream.Write(mV2); + inStream.Write(mV3); + inStream.Write(mConvexRadius); +} + +void TriangleShape::RestoreBinaryState(StreamIn &inStream) +{ + ConvexShape::RestoreBinaryState(inStream); + + inStream.Read(mV1); + inStream.Read(mV2); + inStream.Read(mV3); + inStream.Read(mConvexRadius); +} + +bool TriangleShape::IsValidScale(Vec3Arg inScale) const +{ + return ConvexShape::IsValidScale(inScale) && (mConvexRadius == 0.0f || ScaleHelpers::IsUniformScale(inScale.Abs())); +} + +Vec3 TriangleShape::MakeScaleValid(Vec3Arg inScale) const +{ + Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale); + + if (mConvexRadius == 0.0f) + return scale; + + return scale.GetSign() * ScaleHelpers::MakeUniformScale(scale.Abs()); +} + +void TriangleShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Triangle); + f.mConstruct = []() -> Shape * { return new TriangleShape; }; + f.mColor = Color::sGreen; + + for (EShapeSubType s : sConvexSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Triangle, sCollideConvexVsTriangle); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Triangle, sCastConvexVsTriangle); + } + + // Specialized collision functions + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Sphere, EShapeSubType::Triangle, sCollideSphereVsTriangle); + CollisionDispatch::sRegisterCastShape(EShapeSubType::Sphere, EShapeSubType::Triangle, sCastSphereVsTriangle); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.h new file mode 100644 index 000000000000..b56ccc1f691b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.h @@ -0,0 +1,143 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a TriangleShape +class JPH_EXPORT TriangleShapeSettings final : public ConvexShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, TriangleShapeSettings) + +public: + /// Default constructor for deserialization + TriangleShapeSettings() = default; + + /// Create a triangle with points (inV1, inV2, inV3) (counter clockwise) and convex radius inConvexRadius. + /// Note that the convex radius is currently only used for shape vs shape collision, for all other purposes the triangle is infinitely thin. + TriangleShapeSettings(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, float inConvexRadius = 0.0f, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mV1(inV1), mV2(inV2), mV3(inV3), mConvexRadius(inConvexRadius) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + Vec3 mV1; + Vec3 mV2; + Vec3 mV3; + float mConvexRadius = 0.0f; +}; + +/// A single triangle, not the most efficient way of creating a world filled with triangles but can be used as a query shape for example. +class JPH_EXPORT TriangleShape final : public ConvexShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + TriangleShape() : ConvexShape(EShapeSubType::Triangle) { } + TriangleShape(const TriangleShapeSettings &inSettings, ShapeResult &outResult); + + /// Create a triangle with points (inV1, inV2, inV3) (counter clockwise) and convex radius inConvexRadius. + /// Note that the convex radius is currently only used for shape vs shape collision, for all other purposes the triangle is infinitely thin. + TriangleShape(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, float inConvexRadius = 0.0f, const PhysicsMaterial *inMaterial = nullptr) : ConvexShape(EShapeSubType::Triangle, inMaterial), mV1(inV1), mV2(inV2), mV3(inV3), mConvexRadius(inConvexRadius) { JPH_ASSERT(inConvexRadius >= 0.0f); } + + /// Get the vertices of the triangle + inline Vec3 GetVertex1() const { return mV1; } + inline Vec3 GetVertex2() const { return mV2; } + inline Vec3 GetVertex3() const { return mV3; } + + /// Convex radius + float GetConvexRadius() const { return mConvexRadius; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mConvexRadius; } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See ConvexShape::GetSupportFunction + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 1); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return 0; } + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // See Shape::MakeScaleValid + virtual Vec3 MakeScaleValid(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Helper functions called by CollisionDispatch + static void sCollideConvexVsTriangle(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 sCollideSphereVsTriangle(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 sCastConvexVsTriangle(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastSphereVsTriangle(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + // Context for GetTrianglesStart/Next + class TSGetTrianglesContext; + + // Classes for GetSupportFunction + class TriangleNoConvex; + class TriangleWithConvex; + + Vec3 mV1; + Vec3 mV2; + Vec3 mV3; + float mConvexRadius = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ShapeCast.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ShapeCast.h new file mode 100644 index 000000000000..4271ac06f7e0 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ShapeCast.h @@ -0,0 +1,173 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Structure that holds a single shape cast (a shape moving along a linear path in 3d space with no rotation) +template +struct ShapeCastT +{ + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + ShapeCastT(const Shape *inShape, Vec3Arg inScale, typename Mat::ArgType inCenterOfMassStart, Vec3Arg inDirection, const AABox &inWorldSpaceBounds) : + mShape(inShape), + mScale(inScale), + mCenterOfMassStart(inCenterOfMassStart), + mDirection(inDirection), + mShapeWorldBounds(inWorldSpaceBounds) + { + } + + /// Constructor + ShapeCastT(const Shape *inShape, Vec3Arg inScale, typename Mat::ArgType inCenterOfMassStart, Vec3Arg inDirection) : + ShapeCastT(inShape, inScale, inCenterOfMassStart, inDirection, inShape->GetWorldSpaceBounds(inCenterOfMassStart, inScale)) + { + } + + /// Construct a shape cast using a world transform for a shape instead of a center of mass transform + static inline ShapeCastType sFromWorldTransform(const Shape *inShape, Vec3Arg inScale, typename Mat::ArgType inWorldTransform, Vec3Arg inDirection) + { + return ShapeCastType(inShape, inScale, inWorldTransform.PreTranslated(inShape->GetCenterOfMass()), inDirection); + } + + /// Transform this shape cast using inTransform. Multiply transform on the left left hand side. + ShapeCastType PostTransformed(typename Mat::ArgType inTransform) const + { + Mat44 start = inTransform * mCenterOfMassStart; + Vec3 direction = inTransform.Multiply3x3(mDirection); + return { mShape, mScale, start, direction }; + } + + /// Translate this shape cast by inTranslation. + ShapeCastType PostTranslated(typename Vec::ArgType inTranslation) const + { + return { mShape, mScale, mCenterOfMassStart.PostTranslated(inTranslation), mDirection }; + } + + /// Get point with fraction inFraction on ray from mCenterOfMassStart to mCenterOfMassStart + mDirection (0 = start of ray, 1 = end of ray) + inline Vec GetPointOnRay(float inFraction) const + { + return mCenterOfMassStart.GetTranslation() + inFraction * mDirection; + } + + const Shape * mShape; ///< Shape that's being cast (cannot be mesh shape). Note that this structure does not assume ownership over the shape for performance reasons. + const Vec3 mScale; ///< Scale in local space of the shape being cast (scales relative to its center of mass) + const Mat mCenterOfMassStart; ///< Start position and orientation of the center of mass of the shape (construct using sFromWorldTransform if you have a world transform for your shape) + const Vec3 mDirection; ///< Direction and length of the cast (anything beyond this length will not be reported as a hit) + const AABox mShapeWorldBounds; ///< Cached shape's world bounds, calculated in constructor +}; + +struct ShapeCast : public ShapeCastT +{ + using ShapeCastT::ShapeCastT; +}; + +struct RShapeCast : public ShapeCastT +{ + using ShapeCastT::ShapeCastT; + + /// Convert from ShapeCast, converts single to double precision + explicit RShapeCast(const ShapeCast &inCast) : + RShapeCast(inCast.mShape, inCast.mScale, RMat44(inCast.mCenterOfMassStart), inCast.mDirection, inCast.mShapeWorldBounds) + { + } + + /// Convert to ShapeCast, which implies casting from double precision to single precision + explicit operator ShapeCast() const + { + return ShapeCast(mShape, mScale, mCenterOfMassStart.ToMat44(), mDirection, mShapeWorldBounds); + } +}; + +/// Settings to be passed with a shape cast +class ShapeCastSettings : public CollideSettingsBase +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Set the backfacing mode for all shapes + void SetBackFaceMode(EBackFaceMode inMode) { mBackFaceModeTriangles = mBackFaceModeConvex = inMode; } + + /// How backfacing triangles should be treated (should we report moving from back to front for triangle based shapes, e.g. for MeshShape/HeightFieldShape?) + EBackFaceMode mBackFaceModeTriangles = EBackFaceMode::IgnoreBackFaces; + + /// How backfacing convex objects should be treated (should we report starting inside an object and moving out?) + EBackFaceMode mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces; + + /// Indicates if we want to shrink the shape by the convex radius and then expand it again. This speeds up collision detection and gives a more accurate normal at the cost of a more 'rounded' shape. + bool mUseShrunkenShapeAndConvexRadius = false; + + /// When true, and the shape is intersecting at the beginning of the cast (fraction = 0) then this will calculate the deepest penetration point (costing additional CPU time) + bool mReturnDeepestPoint = false; +}; + +/// Result of a shape cast test +class ShapeCastResult : public CollideShapeResult +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Default constructor + ShapeCastResult() = default; + + /// Constructor + /// @param inFraction Fraction at which the cast hit + /// @param inContactPoint1 Contact point on shape 1 + /// @param inContactPoint2 Contact point on shape 2 + /// @param inContactNormalOrPenetrationDepth Contact normal pointing from shape 1 to 2 or penetration depth vector when the objects are penetrating (also from 1 to 2) + /// @param inBackFaceHit If this hit was a back face hit + /// @param inSubShapeID1 Sub shape id for shape 1 + /// @param inSubShapeID2 Sub shape id for shape 2 + /// @param inBodyID2 BodyID that was hit + ShapeCastResult(float inFraction, Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inContactNormalOrPenetrationDepth, bool inBackFaceHit, const SubShapeID &inSubShapeID1, const SubShapeID &inSubShapeID2, const BodyID &inBodyID2) : + CollideShapeResult(inContactPoint1, inContactPoint2, inContactNormalOrPenetrationDepth, (inContactPoint2 - inContactPoint1).Length(), inSubShapeID1, inSubShapeID2, inBodyID2), + mFraction(inFraction), + mIsBackFaceHit(inBackFaceHit) + { + } + + /// Function required by the CollisionCollector. A smaller fraction is considered to be a 'better hit'. For rays/cast shapes we can just use the collision fraction. The fraction and penetration depth are combined in such a way that deeper hits at fraction 0 go first. + inline float GetEarlyOutFraction() const { return mFraction > 0.0f? mFraction : -mPenetrationDepth; } + + /// Reverses the hit result, swapping contact point 1 with contact point 2 etc. + /// @param inWorldSpaceCastDirection Direction of the shape cast in world space + ShapeCastResult Reversed(Vec3Arg inWorldSpaceCastDirection) const + { + // Calculate by how much to shift the contact points + Vec3 delta = mFraction * inWorldSpaceCastDirection; + + ShapeCastResult result; + result.mContactPointOn2 = mContactPointOn1 - delta; + result.mContactPointOn1 = mContactPointOn2 - delta; + result.mPenetrationAxis = -mPenetrationAxis; + result.mPenetrationDepth = mPenetrationDepth; + result.mSubShapeID2 = mSubShapeID1; + result.mSubShapeID1 = mSubShapeID2; + result.mBodyID2 = mBodyID2; + result.mFraction = mFraction; + result.mIsBackFaceHit = mIsBackFaceHit; + + result.mShape2Face.resize(mShape1Face.size()); + for (Face::size_type i = 0; i < mShape1Face.size(); ++i) + result.mShape2Face[i] = mShape1Face[i] - delta; + + result.mShape1Face.resize(mShape2Face.size()); + for (Face::size_type i = 0; i < mShape2Face.size(); ++i) + result.mShape1Face[i] = mShape2Face[i] - delta; + + return result; + } + + float mFraction; ///< This is the fraction where the shape hit the other shape: CenterOfMassOnHit = Start + value * (End - Start) + bool mIsBackFaceHit; ///< True if the shape was hit from the back side +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ShapeFilter.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ShapeFilter.h new file mode 100644 index 000000000000..3e9f4ff71a8d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ShapeFilter.h @@ -0,0 +1,73 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class Shape; +class SubShapeID; + +/// Filter class +class ShapeFilter : public NonCopyable +{ +public: + /// Destructor + virtual ~ShapeFilter() = default; + + /// Filter function to determine if we should collide with a shape. Returns true if the filter passes. + /// This overload is called when the query doesn't have a source shape (e.g. ray cast / collide point) + /// @param inShape2 Shape we're colliding against + /// @param inSubShapeIDOfShape2 The sub shape ID that will lead from the root shape to inShape2 (i.e. the shape of mBodyID2) + virtual bool ShouldCollide([[maybe_unused]] const Shape *inShape2, [[maybe_unused]] const SubShapeID &inSubShapeIDOfShape2) const + { + return true; + } + + /// Filter function to determine if two shapes should collide. Returns true if the filter passes. + /// This overload is called when querying a shape vs a shape (e.g. collide object / cast object). + /// It is called at each level of the shape hierarchy, so if you have a compound shape with a box, this function will be called twice. + /// It will not be called on triangles that are part of another shape, i.e a mesh shape will not trigger a callback per triangle. You can filter out individual triangles in the CollisionCollector::AddHit function by their sub shape ID. + /// @param inShape1 1st shape that is colliding + /// @param inSubShapeIDOfShape1 The sub shape ID that will lead from the root shape to inShape1 (i.e. the shape that is used to collide or cast against shape 2) + /// @param inShape2 2nd shape that is colliding + /// @param inSubShapeIDOfShape2 The sub shape ID that will lead from the root shape to inShape2 (i.e. the shape of mBodyID2) + virtual bool ShouldCollide([[maybe_unused]] const Shape *inShape1, [[maybe_unused]] const SubShapeID &inSubShapeIDOfShape1, [[maybe_unused]] const Shape *inShape2, [[maybe_unused]] const SubShapeID &inSubShapeIDOfShape2) const + { + return true; + } + + /// Used during NarrowPhase queries and TransformedShape queries. Set to the body ID of inShape2 before calling ShouldCollide. + /// Provides context to the filter to indicate which body is colliding. + mutable BodyID mBodyID2; +}; + +/// Helper class to reverse the order of the shapes in the ShouldCollide function +class ReversedShapeFilter : public ShapeFilter +{ +public: + /// Constructor + explicit ReversedShapeFilter(const ShapeFilter &inFilter) : mFilter(inFilter) + { + mBodyID2 = inFilter.mBodyID2; + } + + virtual bool ShouldCollide(const Shape *inShape2, const SubShapeID &inSubShapeIDOfShape2) const override + { + return mFilter.ShouldCollide(inShape2, inSubShapeIDOfShape2); + } + + virtual bool ShouldCollide(const Shape *inShape1, const SubShapeID &inSubShapeIDOfShape1, const Shape *inShape2, const SubShapeID &inSubShapeIDOfShape2) const override + { + return mFilter.ShouldCollide(inShape2, inSubShapeIDOfShape2, inShape1, inSubShapeIDOfShape1); + } + +private: + const ShapeFilter & mFilter; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilter.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilter.h new file mode 100644 index 000000000000..8c0320a83d4d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilter.h @@ -0,0 +1,40 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class Body; +class Shape; +class SubShapeID; + +/// Filter class used during the simulation (PhysicsSystem::Update) to filter out collisions at shape level +class SimShapeFilter : public NonCopyable +{ +public: + /// Destructor + virtual ~SimShapeFilter() = default; + + /// Filter function to determine if two shapes should collide. Returns true if the filter passes. + /// This overload is called during the simulation (PhysicsSystem::Update) and must be registered with PhysicsSystem::SetSimShapeFilter. + /// It is called at each level of the shape hierarchy, so if you have a compound shape with a box, this function will be called twice. + /// It will not be called on triangles that are part of another shape, i.e a mesh shape will not trigger a callback per triangle. + /// Note that this function is called from multiple threads and must be thread safe. All properties are read only. + /// @param inBody1 1st body that is colliding + /// @param inShape1 1st shape that is colliding + /// @param inSubShapeIDOfShape1 The sub shape ID that will lead from inBody1.GetShape() to inShape1 + /// @param inBody2 2nd body that is colliding + /// @param inShape2 2nd shape that is colliding + /// @param inSubShapeIDOfShape2 The sub shape ID that will lead from inBody2.GetShape() to inShape2 + virtual bool ShouldCollide([[maybe_unused]] const Body &inBody1, [[maybe_unused]] const Shape *inShape1, [[maybe_unused]] const SubShapeID &inSubShapeIDOfShape1, + [[maybe_unused]] const Body &inBody2, [[maybe_unused]] const Shape *inShape2, [[maybe_unused]] const SubShapeID &inSubShapeIDOfShape2) const + { + return true; + } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilterWrapper.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilterWrapper.h new file mode 100644 index 000000000000..02e4f0dcfea3 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilterWrapper.h @@ -0,0 +1,81 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Helper class to forward ShapeFilter calls to a SimShapeFilter +/// INTERNAL CLASS DO NOT USE! +class SimShapeFilterWrapper : public ShapeFilter +{ +public: + /// Constructor + SimShapeFilterWrapper(const SimShapeFilter *inFilter, const Body *inBody1) : + mFilter(inFilter), + mBody1(inBody1) + { + } + + /// Forward to the simulation shape filter + virtual bool ShouldCollide(const Shape *inShape1, const SubShapeID &inSubShapeIDOfShape1, const Shape *inShape2, const SubShapeID &inSubShapeIDOfShape2) const override + { + return mFilter->ShouldCollide(*mBody1, inShape1, inSubShapeIDOfShape1, *mBody2, inShape2, inSubShapeIDOfShape2); + } + + /// Forward to the simulation shape filter + virtual bool ShouldCollide(const Shape *inShape2, const SubShapeID &inSubShapeIDOfShape2) const override + { + return mFilter->ShouldCollide(*mBody1, mBody1->GetShape(), SubShapeID(), *mBody2, inShape2, inSubShapeIDOfShape2); + } + + /// Set the body we're colliding against + void SetBody2(const Body *inBody2) + { + 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) + { + // 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; + } + +private: + SimShapeFilterWrapper mSimShapeFilterWrapper; + ShapeFilter mShapeFilter; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/SortReverseAndStore.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/SortReverseAndStore.h new file mode 100644 index 000000000000..4a073096c264 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/SortReverseAndStore.h @@ -0,0 +1,48 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// This function will sort values from high to low and only keep the ones that are less than inMaxValue +/// @param inValues Values to be sorted +/// @param inMaxValue Values need to be less than this to keep them +/// @param ioIdentifiers 4 identifiers that will be sorted in the same way as the values +/// @param outValues The values are stored here from high to low +/// @return The number of values that were kept +JPH_INLINE int SortReverseAndStore(Vec4Arg inValues, float inMaxValue, UVec4 &ioIdentifiers, float *outValues) +{ + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + Vec4 values = inValues; + Vec4::sSort4Reverse(values, ioIdentifiers); + + // Count how many results are less than the max value + UVec4 closer = Vec4::sLess(values, Vec4::sReplicate(inMaxValue)); + int num_results = closer.CountTrues(); + + // Shift the values so that only the ones that are less than max are kept + values = values.ReinterpretAsInt().ShiftComponents4Minus(num_results).ReinterpretAsFloat(); + ioIdentifiers = ioIdentifiers.ShiftComponents4Minus(num_results); + + // Store the values + values.StoreFloat4(reinterpret_cast(outValues)); + + return num_results; +} + +/// Shift the elements so that the identifiers that correspond with the trues in inValue come first +/// @param inValue Values to test for true or false +/// @param ioIdentifiers the identifiers that are shifted, on return they are shifted +/// @return The number of trues +JPH_INLINE int CountAndSortTrues(UVec4Arg inValue, UVec4 &ioIdentifiers) +{ + // Sort the hits + ioIdentifiers = UVec4::sSort4True(inValue, ioIdentifiers); + + // Return the amount of hits + return inValue.CountTrues(); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.cpp new file mode 100644 index 000000000000..470387b52a65 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.cpp @@ -0,0 +1,180 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +bool TransformedShape::CastRay(const RRayCast &inRay, RayCastResult &ioHit) const +{ + if (mShape != nullptr) + { + // Transform the ray to local space, note that this drops precision which is possible because we're in local space now + RayCast ray(inRay.Transformed(GetInverseCenterOfMassTransform())); + + // Scale the ray + Vec3 inv_scale = GetShapeScale().Reciprocal(); + ray.mOrigin *= inv_scale; + ray.mDirection *= inv_scale; + + // Cast the ray on the shape + SubShapeIDCreator sub_shape_id(mSubShapeIDCreator); + if (mShape->CastRay(ray, sub_shape_id, ioHit)) + { + // Set body ID on the hit result + ioHit.mBodyID = mBodyID; + + return true; + } + } + + return false; +} + +void TransformedShape::CastRay(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + if (mShape != nullptr) + { + // Set the context on the collector and filter + ioCollector.SetContext(this); + inShapeFilter.mBodyID2 = mBodyID; + + // Transform the ray to local space, note that this drops precision which is possible because we're in local space now + RayCast ray(inRay.Transformed(GetInverseCenterOfMassTransform())); + + // Scale the ray + Vec3 inv_scale = GetShapeScale().Reciprocal(); + ray.mOrigin *= inv_scale; + ray.mDirection *= inv_scale; + + // Cast the ray on the shape + SubShapeIDCreator sub_shape_id(mSubShapeIDCreator); + mShape->CastRay(ray, inRayCastSettings, sub_shape_id, ioCollector, inShapeFilter); + } +} + +void TransformedShape::CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + if (mShape != nullptr) + { + // Set the context on the collector and filter + ioCollector.SetContext(this); + inShapeFilter.mBodyID2 = mBodyID; + + // Transform and scale the point to local space + Vec3 point = Vec3(GetInverseCenterOfMassTransform() * inPoint) / GetShapeScale(); + + // Do point collide on the shape + SubShapeIDCreator sub_shape_id(mSubShapeIDCreator); + mShape->CollidePoint(point, sub_shape_id, ioCollector, inShapeFilter); + } +} + +void TransformedShape::CollideShape(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + if (mShape != nullptr) + { + // Set the context on the collector and filter + ioCollector.SetContext(this); + inShapeFilter.mBodyID2 = mBodyID; + + SubShapeIDCreator sub_shape_id1, sub_shape_id2(mSubShapeIDCreator); + Mat44 transform1 = inCenterOfMassTransform.PostTranslated(-inBaseOffset).ToMat44(); + Mat44 transform2 = GetCenterOfMassTransform().PostTranslated(-inBaseOffset).ToMat44(); + CollisionDispatch::sCollideShapeVsShape(inShape, mShape, inShapeScale, GetShapeScale(), transform1, transform2, sub_shape_id1, sub_shape_id2, inCollideShapeSettings, ioCollector, inShapeFilter); + } +} + +void TransformedShape::CastShape(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + if (mShape != nullptr) + { + // Set the context on the collector and filter + ioCollector.SetContext(this); + inShapeFilter.mBodyID2 = mBodyID; + + // Get the shape cast relative to the base offset and convert it to floats + ShapeCast shape_cast(inShapeCast.PostTranslated(-inBaseOffset)); + + // Get center of mass of object we're casting against relative to the base offset and convert it to floats + Mat44 center_of_mass_transform2 = GetCenterOfMassTransform().PostTranslated(-inBaseOffset).ToMat44(); + + SubShapeIDCreator sub_shape_id1, sub_shape_id2(mSubShapeIDCreator); + CollisionDispatch::sCastShapeVsShapeWorldSpace(shape_cast, inShapeCastSettings, mShape, GetShapeScale(), inShapeFilter, center_of_mass_transform2, sub_shape_id1, sub_shape_id2, ioCollector); + } +} + +void TransformedShape::CollectTransformedShapes(const AABox &inBox, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + if (mShape != nullptr) + { + struct MyCollector : public TransformedShapeCollector + { + MyCollector(TransformedShapeCollector &ioCollector, RVec3 inShapePositionCOM) : + TransformedShapeCollector(ioCollector), + mCollector(ioCollector), + mShapePositionCOM(inShapePositionCOM) + { + } + + virtual void AddHit(const TransformedShape &inResult) override + { + // Apply the center of mass offset + TransformedShape ts = inResult; + ts.mShapePositionCOM += mShapePositionCOM; + + // Pass hit on to child collector + mCollector.AddHit(ts); + + // Update early out fraction based on child collector + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + + TransformedShapeCollector & mCollector; + RVec3 mShapePositionCOM; + }; + + // Set the context on the collector + ioCollector.SetContext(this); + + // Wrap the collector so we can add the center of mass precision, we do this to avoid losing precision because CollectTransformedShapes uses single precision floats + MyCollector collector(ioCollector, mShapePositionCOM); + + // Take box to local space for the shape + AABox box = inBox; + box.Translate(-mShapePositionCOM); + + mShape->CollectTransformedShapes(box, Vec3::sZero(), mShapeRotation, GetShapeScale(), mSubShapeIDCreator, collector, inShapeFilter); + } +} + +void TransformedShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, RVec3Arg inBaseOffset) const +{ + if (mShape != nullptr) + { + // Take box to local space for the shape + AABox box = inBox; + box.Translate(-inBaseOffset); + + mShape->GetTrianglesStart(ioContext, box, Vec3(mShapePositionCOM - inBaseOffset), mShapeRotation, GetShapeScale()); + } +} + +int TransformedShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + if (mShape != nullptr) + return mShape->GetTrianglesNext(ioContext, inMaxTrianglesRequested, outTriangleVertices, outMaterials); + else + return 0; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.h new file mode 100644 index 000000000000..887a8ee44114 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.h @@ -0,0 +1,194 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +struct RRayCast; +struct RShapeCast; +class CollideShapeSettings; +class RayCastResult; + +/// Temporary data structure that contains a shape and a transform. +/// This structure can be obtained from a body (e.g. after a broad phase query) under lock protection. +/// The lock can then be released and collision detection operations can be safely performed since +/// the class takes a reference on the shape and does not use anything from the body anymore. +class JPH_EXPORT TransformedShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + TransformedShape() = default; + TransformedShape(RVec3Arg inPositionCOM, QuatArg inRotation, const Shape *inShape, const BodyID &inBodyID, const SubShapeIDCreator &inSubShapeIDCreator = SubShapeIDCreator()) : mShapePositionCOM(inPositionCOM), mShapeRotation(inRotation), mShape(inShape), mBodyID(inBodyID), mSubShapeIDCreator(inSubShapeIDCreator) { } + + /// Cast a ray and find the closest hit. Returns true if it finds a hit. Hits further than ioHit.mFraction will not be considered and in this case ioHit will remain unmodified (and the function will return false). + /// Convex objects will be treated as solid (meaning if the ray starts inside, you'll get a hit fraction of 0) and back face hits are returned. + /// If you want the surface normal of the hit use GetWorldSpaceSurfaceNormal(ioHit.mSubShapeID2, inRay.GetPointOnRay(ioHit.mFraction)) on this object. + bool CastRay(const RRayCast &inRay, RayCastResult &ioHit) const; + + /// Cast a ray, allows collecting multiple hits. Note that this version is more flexible but also slightly slower than the CastRay function that returns only a single hit. + /// If you want the surface normal of the hit use GetWorldSpaceSurfaceNormal(collected sub shape ID, inRay.GetPointOnRay(collected fraction)) on this object. + void CastRay(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const; + + /// Check if inPoint is inside any shapes. For this tests all shapes are treated as if they were solid. + /// For a mesh shape, this test will only provide sensible information if the mesh is a closed manifold. + /// For each shape that collides, ioCollector will receive a hit + void CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const; + + /// Collide a shape and report any hits to ioCollector + /// @param inShape Shape to test + /// @param inShapeScale Scale in local space of shape + /// @param inCenterOfMassTransform Center of mass transform for the shape + /// @param inCollideShapeSettings Settings + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. mShapePositionCOM since floats are most accurate near the origin + /// @param ioCollector Collector that receives the hits + /// @param inShapeFilter Filter that allows you to reject collisions + void CollideShape(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const; + + /// Cast a shape and report any hits to ioCollector + /// @param inShapeCast The shape cast and its position and direction + /// @param inShapeCastSettings Settings for the shape cast + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. mShapePositionCOM or inShapeCast.mCenterOfMassStart.GetTranslation() since floats are most accurate near the origin + /// @param ioCollector Collector that receives the hits + /// @param inShapeFilter Filter that allows you to reject collisions + void CastShape(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const; + + /// Collect the leaf transformed shapes of all leaf shapes of this shape + /// inBox is the world space axis aligned box which leaf shapes should collide with + void CollectTransformedShapes(const AABox &inBox, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const; + + /// Use the context from Shape + using GetTrianglesContext = Shape::GetTrianglesContext; + + /// To start iterating over triangles, call this function first. + /// To get the actual triangles call GetTrianglesNext. + /// @param ioContext A temporary buffer and should remain untouched until the last call to GetTrianglesNext. + /// @param inBox The world space bounding in which you want to get the triangles. + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. inBox.GetCenter() since floats are most accurate near the origin + void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, RVec3Arg inBaseOffset) const; + + /// Call this repeatedly to get all triangles in the box. + /// outTriangleVertices should be large enough to hold 3 * inMaxTriangleRequested entries + /// outMaterials (if it is not null) should contain inMaxTrianglesRequested entries + /// The function returns the amount of triangles that it found (which will be <= inMaxTrianglesRequested), or 0 if there are no more triangles. + /// Note that the function can return a value < inMaxTrianglesRequested and still have more triangles to process (triangles can be returned in blocks) + /// Note that the function may return triangles outside of the requested box, only coarse culling is performed on the returned triangles + int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const; + + /// Get/set the scale of the shape as a Vec3 + inline Vec3 GetShapeScale() const { return Vec3::sLoadFloat3Unsafe(mShapeScale); } + inline void SetShapeScale(Vec3Arg inScale) { inScale.StoreFloat3(&mShapeScale); } + + /// Calculates the transform for this shapes's center of mass (excluding scale) + inline RMat44 GetCenterOfMassTransform() const { return RMat44::sRotationTranslation(mShapeRotation, mShapePositionCOM); } + + /// Calculates the inverse of the transform for this shape's center of mass (excluding scale) + inline RMat44 GetInverseCenterOfMassTransform() const { return RMat44::sInverseRotationTranslation(mShapeRotation, mShapePositionCOM); } + + /// Sets the world transform (including scale) of this transformed shape (not from the center of mass but in the space the shape was created) + inline void SetWorldTransform(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inScale) + { + mShapePositionCOM = inPosition + inRotation * (inScale * mShape->GetCenterOfMass()); + mShapeRotation = inRotation; + SetShapeScale(inScale); + } + + /// Sets the world transform (including scale) of this transformed shape (not from the center of mass but in the space the shape was created) + inline void SetWorldTransform(RMat44Arg inTransform) + { + Vec3 scale; + RMat44 rot_trans = inTransform.Decompose(scale); + SetWorldTransform(rot_trans.GetTranslation(), rot_trans.GetQuaternion(), scale); + } + + /// Calculates the world transform including scale of this shape (not from the center of mass but in the space the shape was created) + inline RMat44 GetWorldTransform() const + { + RMat44 transform = RMat44::sRotation(mShapeRotation).PreScaled(GetShapeScale()); + transform.SetTranslation(mShapePositionCOM - transform.Multiply3x3(mShape->GetCenterOfMass())); + return transform; + } + + /// Get the world space bounding box for this transformed shape + AABox GetWorldSpaceBounds() const { return mShape != nullptr? mShape->GetWorldSpaceBounds(GetCenterOfMassTransform(), GetShapeScale()) : AABox(); } + + /// Make inSubShapeID relative to mShape. When mSubShapeIDCreator is not empty, this is needed in order to get the correct path to the sub shape. + inline SubShapeID MakeSubShapeIDRelativeToShape(const SubShapeID &inSubShapeID) const + { + // Take off the sub shape ID part that comes from mSubShapeIDCreator and validate that it is the same + SubShapeID sub_shape_id; + uint num_bits_written = mSubShapeIDCreator.GetNumBitsWritten(); + JPH_IF_ENABLE_ASSERTS(uint32 root_id =) inSubShapeID.PopID(num_bits_written, sub_shape_id); + JPH_ASSERT(root_id == (mSubShapeIDCreator.GetID().GetValue() & ((1 << num_bits_written) - 1))); + return sub_shape_id; + } + + /// Get surface normal of a particular sub shape and its world space surface position on this body. + /// Note: When you have a CollideShapeResult or ShapeCastResult you should use -mPenetrationAxis.Normalized() as contact normal as GetWorldSpaceSurfaceNormal will only return face normals (and not vertex or edge normals). + inline Vec3 GetWorldSpaceSurfaceNormal(const SubShapeID &inSubShapeID, RVec3Arg inPosition) const + { + RMat44 inv_com = GetInverseCenterOfMassTransform(); + Vec3 scale = GetShapeScale(); // See comment at ScaledShape::GetSurfaceNormal for the math behind the scaling of the normal + return inv_com.Multiply3x3Transposed(mShape->GetSurfaceNormal(MakeSubShapeIDRelativeToShape(inSubShapeID), Vec3(inv_com * inPosition) / scale) / scale).Normalized(); + } + + /// Get the vertices of the face that faces inDirection the most (includes any convex radius). Note that this function can only return faces of + /// convex shapes or triangles, which is why a sub shape ID to get to that leaf must be provided. + /// @param inSubShapeID Sub shape ID of target shape + /// @param inDirection Direction that the face should be facing (in world space) + /// @param inBaseOffset The vertices will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. mShapePositionCOM since floats are most accurate near the origin + /// @param outVertices Resulting face. Note the returned face can have a single point if the shape doesn't have polygons to return (e.g. because it's a sphere). The face will be returned in world space. + void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, RVec3Arg inBaseOffset, Shape::SupportingFace &outVertices) const + { + Mat44 com = GetCenterOfMassTransform().PostTranslated(-inBaseOffset).ToMat44(); + mShape->GetSupportingFace(MakeSubShapeIDRelativeToShape(inSubShapeID), com.Multiply3x3Transposed(inDirection), GetShapeScale(), com, outVertices); + } + + /// Get material of a particular sub shape + inline const PhysicsMaterial *GetMaterial(const SubShapeID &inSubShapeID) const + { + return mShape->GetMaterial(MakeSubShapeIDRelativeToShape(inSubShapeID)); + } + + /// Get the user data of a particular sub shape + inline uint64 GetSubShapeUserData(const SubShapeID &inSubShapeID) const + { + return mShape->GetSubShapeUserData(MakeSubShapeIDRelativeToShape(inSubShapeID)); + } + + /// Get the direct child sub shape and its transform for a sub shape ID. + /// @param inSubShapeID Sub shape ID that indicates the path to the leaf shape + /// @param outRemainder The remainder of the sub shape ID after removing the sub shape + /// @return Direct child sub shape and its transform, note that the body ID and sub shape ID will be invalid + TransformedShape GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const + { + TransformedShape ts = mShape->GetSubShapeTransformedShape(inSubShapeID, Vec3::sZero(), mShapeRotation, GetShapeScale(), outRemainder); + ts.mShapePositionCOM += mShapePositionCOM; + return ts; + } + + /// Helper function to return the body id from a transformed shape. If the transformed shape is null an invalid body ID will be returned. + inline static BodyID sGetBodyID(const TransformedShape *inTS) { return inTS != nullptr? inTS->mBodyID : BodyID(); } + + RVec3 mShapePositionCOM; ///< Center of mass world position of the shape + Quat mShapeRotation; ///< Rotation of the shape + RefConst mShape; ///< The shape itself + Float3 mShapeScale { 1, 1, 1 }; ///< Not stored as Vec3 to get a nicely packed structure + BodyID mBodyID; ///< Optional body ID from which this shape comes + 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"); + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/CalculateSolverSteps.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/CalculateSolverSteps.h new file mode 100644 index 000000000000..857eddfb8e17 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/CalculateSolverSteps.h @@ -0,0 +1,66 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class used to calculate the total number of velocity and position steps +class CalculateSolverSteps +{ +public: + /// Constructor + JPH_INLINE explicit CalculateSolverSteps(const PhysicsSettings &inSettings) : mSettings(inSettings) { } + + /// Combine the number of velocity and position steps for this body/constraint with the current values + template + JPH_INLINE void operator () (const Type *inObject) + { + uint num_velocity_steps = inObject->GetNumVelocityStepsOverride(); + mNumVelocitySteps = max(mNumVelocitySteps, num_velocity_steps); + mApplyDefaultVelocity |= num_velocity_steps == 0; + + uint num_position_steps = inObject->GetNumPositionStepsOverride(); + mNumPositionSteps = max(mNumPositionSteps, num_position_steps); + mApplyDefaultPosition |= num_position_steps == 0; + } + + /// Must be called after all bodies/constraints have been processed + JPH_INLINE void Finalize() + { + // If we have a default velocity/position step count, take the max of the default and the overrides + if (mApplyDefaultVelocity) + mNumVelocitySteps = max(mNumVelocitySteps, mSettings.mNumVelocitySteps); + if (mApplyDefaultPosition) + mNumPositionSteps = max(mNumPositionSteps, mSettings.mNumPositionSteps); + } + + /// Get the results of the calculation + JPH_INLINE uint GetNumPositionSteps() const { return mNumPositionSteps; } + JPH_INLINE uint GetNumVelocitySteps() const { return mNumVelocitySteps; } + +private: + const PhysicsSettings & mSettings; + + uint mNumVelocitySteps = 0; + uint mNumPositionSteps = 0; + + bool mApplyDefaultVelocity = false; + bool mApplyDefaultPosition = false; +}; + +/// Dummy class to replace the steps calculator when we don't need the result +class DummyCalculateSolverSteps +{ +public: + template + JPH_INLINE void operator () (const Type *) const + { + /* Nothing to do */ + } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConeConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConeConstraint.cpp new file mode 100644 index 000000000000..9889fa643de2 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConeConstraint.cpp @@ -0,0 +1,246 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ConeConstraintSettings) +{ + JPH_ADD_BASE_CLASS(ConeConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(ConeConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mPoint1) + JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mTwistAxis1) + JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mPoint2) + JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mTwistAxis2) + JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mHalfConeAngle) +} + +void ConeConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mPoint1); + inStream.Write(mTwistAxis1); + inStream.Write(mPoint2); + inStream.Write(mTwistAxis2); + inStream.Write(mHalfConeAngle); +} + +void ConeConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mPoint1); + inStream.Read(mTwistAxis1); + inStream.Read(mPoint2); + inStream.Read(mTwistAxis2); + inStream.Read(mHalfConeAngle); +} + +TwoBodyConstraint *ConeConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new ConeConstraint(inBody1, inBody2, *this); +} + +ConeConstraint::ConeConstraint(Body &inBody1, Body &inBody2, const ConeConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings) +{ + // Store limits + SetHalfConeAngle(inSettings.mHalfConeAngle); + + // Initialize rotation axis to perpendicular of twist axis in case the angle between the twist axis is 0 in the first frame + mWorldSpaceRotationAxis = inSettings.mTwistAxis1.GetNormalizedPerpendicular(); + + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + RMat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform(); + mLocalSpacePosition1 = Vec3(inv_transform1 * inSettings.mPoint1); + mLocalSpaceTwistAxis1 = inv_transform1.Multiply3x3(inSettings.mTwistAxis1); + + RMat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform(); + mLocalSpacePosition2 = Vec3(inv_transform2 * inSettings.mPoint2); + mLocalSpaceTwistAxis2 = inv_transform2.Multiply3x3(inSettings.mTwistAxis2); + } + else + { + // Properties already in local space + mLocalSpacePosition1 = Vec3(inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inSettings.mPoint2); + mLocalSpaceTwistAxis1 = inSettings.mTwistAxis1; + mLocalSpaceTwistAxis2 = inSettings.mTwistAxis2; + + // If they were in local space, we need to take the initial rotation axis to world space + mWorldSpaceRotationAxis = inBody1.GetRotation() * mWorldSpaceRotationAxis; + } +} + +void ConeConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +void ConeConstraint::CalculateRotationConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2) +{ + // Rotation is along the cross product of both twist axis + Vec3 twist1 = inRotation1.Multiply3x3(mLocalSpaceTwistAxis1); + Vec3 twist2 = inRotation2.Multiply3x3(mLocalSpaceTwistAxis2); + + // Calculate dot product between twist axis, if it's smaller than the cone angle we need to correct + mCosTheta = twist1.Dot(twist2); + if (mCosTheta < mCosHalfConeAngle) + { + // Rotation axis is defined by the two twist axis + Vec3 rot_axis = twist2.Cross(twist1); + + // If we can't find a rotation axis because the twist is too small, we'll use last frame's rotation axis + float len = rot_axis.Length(); + if (len > 0.0f) + mWorldSpaceRotationAxis = rot_axis / len; + + mAngleConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceRotationAxis); + } + else + mAngleConstraintPart.Deactivate(); +} + +void ConeConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2); + CalculateRotationConstraintProperties(rotation1, rotation2); +} + +void ConeConstraint::ResetWarmStart() +{ + mPointConstraintPart.Deactivate(); + mAngleConstraintPart.Deactivate(); +} + +void ConeConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mAngleConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); +} + +bool ConeConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + bool pos = mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + bool rot = false; + if (mAngleConstraintPart.IsActive()) + rot = mAngleConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceRotationAxis, 0, FLT_MAX); + + return pos || rot; +} + +bool ConeConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2); + bool pos = mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); + + bool rot = false; + CalculateRotationConstraintProperties(Mat44::sRotation(mBody1->GetRotation()), Mat44::sRotation(mBody2->GetRotation())); + if (mAngleConstraintPart.IsActive()) + rot = mAngleConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mCosTheta - mCosHalfConeAngle, inBaumgarte); + + return pos || rot; +} + +#ifdef JPH_DEBUG_RENDERER +void ConeConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform(); + + RVec3 p1 = transform1 * mLocalSpacePosition1; + RVec3 p2 = transform2 * mLocalSpacePosition2; + + // Draw constraint + inRenderer->DrawMarker(p1, Color::sRed, 0.1f); + inRenderer->DrawMarker(p2, Color::sGreen, 0.1f); + + // Draw twist axis + inRenderer->DrawLine(p1, p1 + mDrawConstraintSize * transform1.Multiply3x3(mLocalSpaceTwistAxis1), Color::sRed); + inRenderer->DrawLine(p2, p2 + mDrawConstraintSize * transform2.Multiply3x3(mLocalSpaceTwistAxis2), Color::sGreen); +} + +void ConeConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const +{ + // Get constraint properties in world space + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RVec3 position1 = transform1 * mLocalSpacePosition1; + Vec3 twist_axis1 = transform1.Multiply3x3(mLocalSpaceTwistAxis1); + Vec3 normal_axis1 = transform1.Multiply3x3(mLocalSpaceTwistAxis1.GetNormalizedPerpendicular()); + + inRenderer->DrawOpenCone(position1, twist_axis1, normal_axis1, ACos(mCosHalfConeAngle), mDrawConstraintSize * mCosHalfConeAngle, Color::sPurple, DebugRenderer::ECastShadow::Off); +} +#endif // JPH_DEBUG_RENDERER + +void ConeConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mPointConstraintPart.SaveState(inStream); + mAngleConstraintPart.SaveState(inStream); + inStream.Write(mWorldSpaceRotationAxis); // When twist is too small, the rotation is used from last frame so we need to store it +} + +void ConeConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mPointConstraintPart.RestoreState(inStream); + mAngleConstraintPart.RestoreState(inStream); + inStream.Read(mWorldSpaceRotationAxis); +} + +Ref ConeConstraint::GetConstraintSettings() const +{ + ConeConstraintSettings *settings = new ConeConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPoint1 = RVec3(mLocalSpacePosition1); + settings->mTwistAxis1 = mLocalSpaceTwistAxis1; + settings->mPoint2 = RVec3(mLocalSpacePosition2); + settings->mTwistAxis2 = mLocalSpaceTwistAxis2; + settings->mHalfConeAngle = ACos(mCosHalfConeAngle); + return settings; +} + +Mat44 ConeConstraint::GetConstraintToBody1Matrix() const +{ + Vec3 perp = mLocalSpaceTwistAxis1.GetNormalizedPerpendicular(); + Vec3 perp2 = mLocalSpaceTwistAxis1.Cross(perp); + return Mat44(Vec4(mLocalSpaceTwistAxis1, 0), Vec4(perp, 0), Vec4(perp2, 0), Vec4(mLocalSpacePosition1, 1)); +} + +Mat44 ConeConstraint::GetConstraintToBody2Matrix() const +{ + // Note: Incorrect in rotation around the twist axis (the perpendicular does not match that of body 1), + // this should not matter as we're not limiting rotation around the twist axis. + Vec3 perp = mLocalSpaceTwistAxis2.GetNormalizedPerpendicular(); + Vec3 perp2 = mLocalSpaceTwistAxis2.Cross(perp); + return Mat44(Vec4(mLocalSpaceTwistAxis2, 0), Vec4(perp, 0), Vec4(perp2, 0), Vec4(mLocalSpacePosition2, 1)); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConeConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConeConstraint.h new file mode 100644 index 000000000000..1e25ab208678 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConeConstraint.h @@ -0,0 +1,133 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Cone constraint settings, used to create a cone constraint +class JPH_EXPORT ConeConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, ConeConstraintSettings) + +public: + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 constraint reference frame (space determined by mSpace) + RVec3 mPoint1 = RVec3::sZero(); + Vec3 mTwistAxis1 = Vec3::sAxisX(); + + /// Body 2 constraint reference frame (space determined by mSpace) + RVec3 mPoint2 = RVec3::sZero(); + Vec3 mTwistAxis2 = Vec3::sAxisX(); + + /// Half of maximum angle between twist axis of body 1 and 2 + float mHalfConeAngle = 0.0f; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A cone constraint constraints 2 bodies to a single point and limits the swing between the twist axis within a cone: +/// +/// t1 . t2 <= cos(theta) +/// +/// Where: +/// +/// t1 = twist axis of body 1. +/// t2 = twist axis of body 2. +/// theta = half cone angle (angle from the principal axis of the cone to the edge). +/// +/// Calculating the Jacobian: +/// +/// Constraint equation: +/// +/// C = t1 . t2 - cos(theta) +/// +/// Derivative: +/// +/// d/dt C = d/dt (t1 . t2) = (d/dt t1) . t2 + t1 . (d/dt t2) = (w1 x t1) . t2 + t1 . (w2 x t2) = (t1 x t2) . w1 + (t2 x t1) . w2 +/// +/// d/dt C = J v = [0, -t2 x t1, 0, t2 x t1] [v1, w1, v2, w2] +/// +/// Where J is the Jacobian. +/// +/// Note that this is the exact same equation as used in AngleConstraintPart if we use t2 x t1 as the world space axis +class JPH_EXPORT ConeConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct cone constraint + ConeConstraint(Body &inBody1, Body &inBody2, const ConeConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Cone; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; + virtual void DrawConstraintLimits(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override; + virtual Mat44 GetConstraintToBody2Matrix() const override; + + /// Update maximum angle between body 1 and 2 (see ConeConstraintSettings) + void SetHalfConeAngle(float inHalfConeAngle) { JPH_ASSERT(inHalfConeAngle >= 0.0f && inHalfConeAngle <= JPH_PI); mCosHalfConeAngle = Cos(inHalfConeAngle); } + float GetCosHalfConeAngle() const { return mCosHalfConeAngle; } + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline Vec3 GetTotalLambdaPosition() const { return mPointConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaRotation() const { return mAngleConstraintPart.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateRotationConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2); + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // Local space constraint axis + Vec3 mLocalSpaceTwistAxis1; + Vec3 mLocalSpaceTwistAxis2; + + // Angular limits + float mCosHalfConeAngle; + + // RUN TIME PROPERTIES FOLLOW + + // Axis and angle of rotation between the two bodies + Vec3 mWorldSpaceRotationAxis; + float mCosTheta; + + // The constraint parts + PointConstraintPart mPointConstraintPart; + AngleConstraintPart mAngleConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.cpp new file mode 100644 index 000000000000..b81a8d81a71d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.cpp @@ -0,0 +1,73 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ConstraintSettings) +{ + JPH_ADD_BASE_CLASS(ConstraintSettings, SerializableObject) + + JPH_ADD_ATTRIBUTE(ConstraintSettings, mEnabled) + JPH_ADD_ATTRIBUTE(ConstraintSettings, mDrawConstraintSize) + JPH_ADD_ATTRIBUTE(ConstraintSettings, mConstraintPriority) + JPH_ADD_ATTRIBUTE(ConstraintSettings, mNumVelocityStepsOverride) + JPH_ADD_ATTRIBUTE(ConstraintSettings, mNumPositionStepsOverride) + JPH_ADD_ATTRIBUTE(ConstraintSettings, mUserData) +} + +void ConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(GetRTTI()->GetHash()); + inStream.Write(mEnabled); + inStream.Write(mDrawConstraintSize); + inStream.Write(mConstraintPriority); + inStream.Write(mNumVelocityStepsOverride); + inStream.Write(mNumPositionStepsOverride); +} + +void ConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + // Type hash read by sRestoreFromBinaryState + inStream.Read(mEnabled); + inStream.Read(mDrawConstraintSize); + inStream.Read(mConstraintPriority); + inStream.Read(mNumVelocityStepsOverride); + inStream.Read(mNumPositionStepsOverride); +} + +ConstraintSettings::ConstraintResult ConstraintSettings::sRestoreFromBinaryState(StreamIn &inStream) +{ + return StreamUtils::RestoreObject(inStream, &ConstraintSettings::RestoreBinaryState); +} + +void Constraint::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mEnabled); +} + +void Constraint::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mEnabled); +} + +void Constraint::ToConstraintSettings(ConstraintSettings &outSettings) const +{ + outSettings.mEnabled = mEnabled; + outSettings.mConstraintPriority = mConstraintPriority; + outSettings.mNumVelocityStepsOverride = mNumVelocityStepsOverride; + outSettings.mNumPositionStepsOverride = mNumPositionStepsOverride; + outSettings.mUserData = mUserData; +#ifdef JPH_DEBUG_RENDERER + outSettings.mDrawConstraintSize = mDrawConstraintSize; +#endif // JPH_DEBUG_RENDERER +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.h new file mode 100644 index 000000000000..5127f3989577 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.h @@ -0,0 +1,238 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class BodyID; +class IslandBuilder; +class LargeIslandSplitter; +class BodyManager; +class StateRecorder; +class StreamIn; +class StreamOut; +#ifdef JPH_DEBUG_RENDERER +class DebugRenderer; +#endif // JPH_DEBUG_RENDERER + +/// Enum to identify constraint type +enum class EConstraintType +{ + Constraint, + TwoBodyConstraint, +}; + +/// Enum to identify constraint sub type +enum class EConstraintSubType +{ + Fixed, + Point, + Hinge, + Slider, + Distance, + Cone, + SwingTwist, + SixDOF, + Path, + Vehicle, + RackAndPinion, + Gear, + Pulley, + + /// User defined constraint types start here + User1, + User2, + User3, + User4 +}; + +/// Certain constraints support setting them up in local or world space. This governs what is used. +enum class EConstraintSpace +{ + LocalToBodyCOM, ///< All constraint properties are specified in local space to center of mass of the bodies that are being constrained (so e.g. 'constraint position 1' will be local to body 1 COM, 'constraint position 2' will be local to body 2 COM). Note that this means you need to subtract Shape::GetCenterOfMass() from positions! + WorldSpace, ///< All constraint properties are specified in world space +}; + +/// Class used to store the configuration of a constraint. Allows run-time creation of constraints. +class JPH_EXPORT ConstraintSettings : public SerializableObject, public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, ConstraintSettings) + +public: + using ConstraintResult = Result>; + + /// Saves the contents of the constraint settings in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const; + + /// Creates a constraint of the correct type and restores its contents from the binary stream inStream. + static ConstraintResult sRestoreFromBinaryState(StreamIn &inStream); + + /// If this constraint is enabled initially. Use Constraint::SetEnabled to toggle after creation. + bool mEnabled = true; + + /// Priority of the constraint when solving. Higher numbers have are more likely to be solved correctly. + /// Note that if you want a deterministic simulation and you cannot guarantee the order in which constraints are added/removed, you can make the priority for all constraints unique to get a deterministic ordering. + uint32 mConstraintPriority = 0; + + /// Used only when the constraint is active. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island. + uint mNumVelocityStepsOverride = 0; + + /// Used only when the constraint is active. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island. + uint mNumPositionStepsOverride = 0; + + /// Size of constraint when drawing it through the debug renderer + float mDrawConstraintSize = 1.0f; + + /// User data value (can be used by application) + uint64 mUserData = 0; + +protected: + /// This function should not be called directly, it is used by sRestoreFromBinaryState. + virtual void RestoreBinaryState(StreamIn &inStream); +}; + +/// Base class for all physics constraints. A constraint removes one or more degrees of freedom for a rigid body. +class JPH_EXPORT Constraint : public RefTarget, public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit Constraint(const ConstraintSettings &inSettings) : +#ifdef JPH_DEBUG_RENDERER + mDrawConstraintSize(inSettings.mDrawConstraintSize), +#endif // JPH_DEBUG_RENDERER + mConstraintPriority(inSettings.mConstraintPriority), + mNumVelocityStepsOverride(uint8(inSettings.mNumVelocityStepsOverride)), + mNumPositionStepsOverride(uint8(inSettings.mNumPositionStepsOverride)), + mEnabled(inSettings.mEnabled), + mUserData(inSettings.mUserData) + { + JPH_ASSERT(inSettings.mNumVelocityStepsOverride < 256); + JPH_ASSERT(inSettings.mNumPositionStepsOverride < 256); + } + + /// Virtual destructor + virtual ~Constraint() = default; + + /// Get the type of a constraint + virtual EConstraintType GetType() const { return EConstraintType::Constraint; } + + /// Get the sub type of a constraint + virtual EConstraintSubType GetSubType() const = 0; + + /// Priority of the constraint when solving. Higher numbers have are more likely to be solved correctly. + /// Note that if you want a deterministic simulation and you cannot guarantee the order in which constraints are added/removed, you can make the priority for all constraints unique to get a deterministic ordering. + uint32 GetConstraintPriority() const { return mConstraintPriority; } + void SetConstraintPriority(uint32 inPriority) { mConstraintPriority = inPriority; } + + /// Used only when the constraint is active. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island. + void SetNumVelocityStepsOverride(uint inN) { JPH_ASSERT(inN < 256); mNumVelocityStepsOverride = uint8(inN); } + uint GetNumVelocityStepsOverride() const { return mNumVelocityStepsOverride; } + + /// Used only when the constraint is active. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island. + void SetNumPositionStepsOverride(uint inN) { JPH_ASSERT(inN < 256); mNumPositionStepsOverride = uint8(inN); } + uint GetNumPositionStepsOverride() const { return mNumPositionStepsOverride; } + + /// Enable / disable this constraint. This can e.g. be used to implement a breakable constraint by detecting that the constraint impulse + /// (see e.g. PointConstraint::GetTotalLambdaPosition) went over a certain limit and then disabling the constraint. + /// Note that although a disabled constraint will not affect the simulation in any way anymore, it does incur some processing overhead. + /// Alternatively you can remove a constraint from the constraint manager (which may be more costly if you want to disable the constraint for a short while). + void SetEnabled(bool inEnabled) { mEnabled = inEnabled; } + + /// Test if a constraint is enabled. + bool GetEnabled() const { return mEnabled; } + + /// Access to the user data, can be used for anything by the application + uint64 GetUserData() const { return mUserData; } + void SetUserData(uint64 inUserData) { mUserData = inUserData; } + + /// Notify the constraint that the shape of a body has changed and that its center of mass has moved by inDeltaCOM. + /// Bodies don't know which constraints are connected to them so the user is responsible for notifying the relevant constraints when a body changes. + /// @param inBodyID ID of the body that has changed + /// @param inDeltaCOM The delta of the center of mass of the body (shape->GetCenterOfMass() - shape_before_change->GetCenterOfMass()) + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) = 0; + + /// Notify the system that the configuration of the bodies and/or constraint has changed enough so that the warm start impulses should not be applied the next frame. + /// You can use this function for example when repositioning a ragdoll through Ragdoll::SetPose in such a way that the orientation of the bodies completely changes so that + /// the previous frame impulses are no longer a good approximation of what the impulses will be in the next frame. Calling this function when there are no big changes + /// will result in the constraints being much 'softer' than usual so they are more easily violated (e.g. a long chain of bodies might sag a bit if you call this every frame). + virtual void ResetWarmStart() = 0; + + ///@name Solver interface + ///@{ + virtual bool IsActive() const { return mEnabled; } + virtual void SetupVelocityConstraint(float inDeltaTime) = 0; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) = 0; + virtual bool SolveVelocityConstraint(float inDeltaTime) = 0; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) = 0; + ///@} + + /// Link bodies that are connected by this constraint in the island builder + virtual void BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager) = 0; + + /// Link bodies that are connected by this constraint in the same split. Returns the split index. + virtual uint BuildIslandSplits(LargeIslandSplitter &ioSplitter) const = 0; + +#ifdef JPH_DEBUG_RENDERER + // Drawing interface + virtual void DrawConstraint(DebugRenderer *inRenderer) const = 0; + virtual void DrawConstraintLimits([[maybe_unused]] DebugRenderer *inRenderer) const { } + virtual void DrawConstraintReferenceFrame([[maybe_unused]] DebugRenderer *inRenderer) const { } + + /// Size of constraint when drawing it through the debug renderer + float GetDrawConstraintSize() const { return mDrawConstraintSize; } + void SetDrawConstraintSize(float inSize) { mDrawConstraintSize = inSize; } +#endif // JPH_DEBUG_RENDERER + + /// Saving state for replay + virtual void SaveState(StateRecorder &inStream) const; + + /// Restoring state for replay + virtual void RestoreState(StateRecorder &inStream); + + /// Debug function to convert a constraint to its settings, note that this will not save to which bodies the constraint is connected to + virtual Ref GetConstraintSettings() const = 0; + +protected: + /// Helper function to copy settings back to constraint settings for this base class + void ToConstraintSettings(ConstraintSettings &outSettings) const; + +#ifdef JPH_DEBUG_RENDERER + /// Size of constraint when drawing it through the debug renderer + float mDrawConstraintSize; +#endif // JPH_DEBUG_RENDERER + +private: + friend class ConstraintManager; + + /// Index that indicates this constraint is not in the constraint manager + static constexpr uint32 cInvalidConstraintIndex = 0xffffffff; + + /// Index in the mConstraints list of the ConstraintManager for easy finding + uint32 mConstraintIndex = cInvalidConstraintIndex; + + /// Priority of the constraint when solving. Higher numbers have are more likely to be solved correctly. + uint32 mConstraintPriority = 0; + + /// Used only when the constraint is active. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island. + uint8 mNumVelocityStepsOverride = 0; + + /// Used only when the constraint is active. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island. + uint8 mNumPositionStepsOverride = 0; + + /// If this constraint is currently enabled + bool mEnabled = true; + + /// User data value (can be used by application) + uint64 mUserData; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.cpp new file mode 100644 index 000000000000..509bddbd9720 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.cpp @@ -0,0 +1,289 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +void ConstraintManager::Add(Constraint **inConstraints, int inNumber) +{ + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + mConstraints.reserve(mConstraints.size() + inNumber); + + for (Constraint **c = inConstraints, **c_end = inConstraints + inNumber; c < c_end; ++c) + { + Constraint *constraint = *c; + + // Assume this constraint has not been added yet + JPH_ASSERT(constraint->mConstraintIndex == Constraint::cInvalidConstraintIndex); + + // Add to the list + constraint->mConstraintIndex = uint32(mConstraints.size()); + mConstraints.push_back(constraint); + } +} + +void ConstraintManager::Remove(Constraint **inConstraints, int inNumber) +{ + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + for (Constraint **c = inConstraints, **c_end = inConstraints + inNumber; c < c_end; ++c) + { + Constraint *constraint = *c; + + // Reset constraint index for this constraint + uint32 this_constraint_idx = constraint->mConstraintIndex; + constraint->mConstraintIndex = Constraint::cInvalidConstraintIndex; + JPH_ASSERT(this_constraint_idx != Constraint::cInvalidConstraintIndex); + + // Check if this constraint is somewhere in the middle of the constraints, in this case we need to move the last constraint to this position + uint32 last_constraint_idx = uint32(mConstraints.size() - 1); + if (this_constraint_idx < last_constraint_idx) + { + Constraint *last_constraint = mConstraints[last_constraint_idx]; + last_constraint->mConstraintIndex = this_constraint_idx; + mConstraints[this_constraint_idx] = last_constraint; + } + + // Pop last constraint + mConstraints.pop_back(); + } +} + +Constraints ConstraintManager::GetConstraints() const +{ + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + Constraints copy = mConstraints; + return copy; +} + +void ConstraintManager::GetActiveConstraints(uint32 inStartConstraintIdx, uint32 inEndConstraintIdx, Constraint **outActiveConstraints, uint32 &outNumActiveConstraints) const +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inEndConstraintIdx <= mConstraints.size()); + + uint32 num_active_constraints = 0; + for (uint32 constraint_idx = inStartConstraintIdx; constraint_idx < inEndConstraintIdx; ++constraint_idx) + { + Constraint *c = mConstraints[constraint_idx]; + JPH_ASSERT(c->mConstraintIndex == constraint_idx); + if (c->IsActive()) + { + *(outActiveConstraints++) = c; + num_active_constraints++; + } + } + + outNumActiveConstraints = num_active_constraints; +} + +void ConstraintManager::sBuildIslands(Constraint **inActiveConstraints, uint32 inNumActiveConstraints, IslandBuilder &ioBuilder, BodyManager &inBodyManager) +{ + JPH_PROFILE_FUNCTION(); + + for (uint32 constraint_idx = 0; constraint_idx < inNumActiveConstraints; ++constraint_idx) + { + Constraint *c = inActiveConstraints[constraint_idx]; + c->BuildIslands(constraint_idx, ioBuilder, inBodyManager); + } +} + +void ConstraintManager::sSortConstraints(Constraint **inActiveConstraints, uint32 *inConstraintIdxBegin, uint32 *inConstraintIdxEnd) +{ + JPH_PROFILE_FUNCTION(); + + QuickSort(inConstraintIdxBegin, inConstraintIdxEnd, [inActiveConstraints](uint32 inLHS, uint32 inRHS) { + const Constraint *lhs = inActiveConstraints[inLHS]; + const Constraint *rhs = inActiveConstraints[inRHS]; + + if (lhs->GetConstraintPriority() != rhs->GetConstraintPriority()) + return lhs->GetConstraintPriority() < rhs->GetConstraintPriority(); + + return lhs->mConstraintIndex < rhs->mConstraintIndex; + }); +} + +void ConstraintManager::sSetupVelocityConstraints(Constraint **inActiveConstraints, uint32 inNumActiveConstraints, float inDeltaTime) +{ + JPH_PROFILE_FUNCTION(); + + for (Constraint **c = inActiveConstraints, **c_end = inActiveConstraints + inNumActiveConstraints; c < c_end; ++c) + (*c)->SetupVelocityConstraint(inDeltaTime); +} + +template +void ConstraintManager::sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, ConstraintCallback &ioCallback) +{ + JPH_PROFILE_FUNCTION(); + + for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx) + { + Constraint *c = inActiveConstraints[*constraint_idx]; + ioCallback(c); + c->WarmStartVelocityConstraint(inWarmStartImpulseRatio); + } +} + +// Specialize for the two constraint callback types +template void ConstraintManager::sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, CalculateSolverSteps &ioCallback); +template void ConstraintManager::sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, DummyCalculateSolverSteps &ioCallback); + +bool ConstraintManager::sSolveVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime) +{ + JPH_PROFILE_FUNCTION(); + + bool any_impulse_applied = false; + + for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx) + { + Constraint *c = inActiveConstraints[*constraint_idx]; + any_impulse_applied |= c->SolveVelocityConstraint(inDeltaTime); + } + + return any_impulse_applied; +} + +bool ConstraintManager::sSolvePositionConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime, float inBaumgarte) +{ + JPH_PROFILE_FUNCTION(); + + bool any_impulse_applied = false; + + for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx) + { + Constraint *c = inActiveConstraints[*constraint_idx]; + any_impulse_applied |= c->SolvePositionConstraint(inDeltaTime, inBaumgarte); + } + + return any_impulse_applied; +} + +#ifdef JPH_DEBUG_RENDERER +void ConstraintManager::DrawConstraints(DebugRenderer *inRenderer) const +{ + JPH_PROFILE_FUNCTION(); + + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + for (const Ref &c : mConstraints) + c->DrawConstraint(inRenderer); +} + +void ConstraintManager::DrawConstraintLimits(DebugRenderer *inRenderer) const +{ + JPH_PROFILE_FUNCTION(); + + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + for (const Ref &c : mConstraints) + c->DrawConstraintLimits(inRenderer); +} + +void ConstraintManager::DrawConstraintReferenceFrame(DebugRenderer *inRenderer) const +{ + JPH_PROFILE_FUNCTION(); + + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + for (const Ref &c : mConstraints) + c->DrawConstraintReferenceFrame(inRenderer); +} +#endif // JPH_DEBUG_RENDERER + +void ConstraintManager::SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const +{ + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + // Write state of constraints + if (inFilter != nullptr) + { + // Determine which constraints to save + Array constraints; + constraints.reserve(mConstraints.size()); + for (const Ref &c : mConstraints) + if (inFilter->ShouldSaveConstraint(*c)) + constraints.push_back(c); + + // Save them + uint32 num_constraints = (uint32)constraints.size(); + inStream.Write(num_constraints); + for (const Constraint *c : constraints) + { + inStream.Write(c->mConstraintIndex); + c->SaveState(inStream); + } + } + else + { + // Save all constraints + uint32 num_constraints = (uint32)mConstraints.size(); + inStream.Write(num_constraints); + for (const Ref &c : mConstraints) + { + inStream.Write(c->mConstraintIndex); + c->SaveState(inStream); + } + } +} + +bool ConstraintManager::RestoreState(StateRecorder &inStream) +{ + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + if (inStream.IsValidating()) + { + // Read state of constraints + uint32 num_constraints = (uint32)mConstraints.size(); // Initialize to current value for validation + inStream.Read(num_constraints); + if (num_constraints != mConstraints.size()) + { + JPH_ASSERT(false, "Cannot handle adding/removing constraints"); + return false; + } + for (const Ref &c : mConstraints) + { + uint32 constraint_index = c->mConstraintIndex; + inStream.Read(constraint_index); + if (constraint_index != c->mConstraintIndex) + { + JPH_ASSERT(false, "Unexpected constraint index"); + return false; + } + c->RestoreState(inStream); + } + } + else + { + // Not validating, use more flexible reading, read number of constraints + uint32 num_constraints = 0; + inStream.Read(num_constraints); + + for (uint32 idx = 0; idx < num_constraints; ++idx) + { + uint32 constraint_index; + inStream.Read(constraint_index); + if (mConstraints.size() <= constraint_index) + { + JPH_ASSERT(false, "Restoring state for non-existing constraint"); + return false; + } + mConstraints[constraint_index]->RestoreState(inStream); + } + } + + return true; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.h new file mode 100644 index 000000000000..e1bfb0f21419 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.h @@ -0,0 +1,97 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class IslandBuilder; +class BodyManager; +class StateRecorderFilter; +#ifdef JPH_DEBUG_RENDERER +class DebugRenderer; +#endif // JPH_DEBUG_RENDERER + +/// A list of constraints +using Constraints = Array>; + +/// A constraint manager manages all constraints of the same type +class JPH_EXPORT ConstraintManager : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + +#ifdef JPH_ENABLE_ASSERTS + /// Constructor + ConstraintManager(PhysicsLockContext inContext) : mLockContext(inContext) { } +#endif // JPH_ENABLE_ASSERTS + + /// Add a new constraint. This is thread safe. + void Add(Constraint **inConstraints, int inNumber); + + /// Remove a constraint. This is thread safe. + void Remove(Constraint **inConstraint, int inNumber); + + /// Get a list of all constraints + Constraints GetConstraints() const; + + /// Get total number of constraints + inline uint32 GetNumConstraints() const { return uint32(mConstraints.size()); } + + /// Determine the active constraints of a subset of the constraints + void GetActiveConstraints(uint32 inStartConstraintIdx, uint32 inEndConstraintIdx, Constraint **outActiveConstraints, uint32 &outNumActiveConstraints) const; + + /// Link bodies to form islands + static void sBuildIslands(Constraint **inActiveConstraints, uint32 inNumActiveConstraints, IslandBuilder &ioBuilder, BodyManager &inBodyManager); + + /// In order to have a deterministic simulation, we need to sort the constraints of an island before solving them + static void sSortConstraints(Constraint **inActiveConstraints, uint32 *inConstraintIdxBegin, uint32 *inConstraintIdxEnd); + + /// Prior to solving the velocity constraints, you must call SetupVelocityConstraints once to precalculate values that are independent of velocity + static void sSetupVelocityConstraints(Constraint **inActiveConstraints, uint32 inNumActiveConstraints, float inDeltaTime); + + /// Apply last frame's impulses, must be called prior to SolveVelocityConstraints + template + static void sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, ConstraintCallback &ioCallback); + + /// This function is called multiple times to iteratively come to a solution that meets all velocity constraints + static bool sSolveVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime); + + /// This function is called multiple times to iteratively come to a solution that meets all position constraints + static bool sSolvePositionConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime, float inBaumgarte); + +#ifdef JPH_DEBUG_RENDERER + /// Draw all constraints + void DrawConstraints(DebugRenderer *inRenderer) const; + + /// Draw all constraint limits + void DrawConstraintLimits(DebugRenderer *inRenderer) const; + + /// Draw all constraint reference frames + void DrawConstraintReferenceFrame(DebugRenderer *inRenderer) const; +#endif // JPH_DEBUG_RENDERER + + /// Save state of constraints + void SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const; + + /// Restore the state of constraints. Returns false if failed. + bool RestoreState(StateRecorder &inStream); + + /// Lock all constraints. This should only be done during PhysicsSystem::Update(). + void LockAllConstraints() { PhysicsLock::sLock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); } + void UnlockAllConstraints() { PhysicsLock::sUnlock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); } + +private: +#ifdef JPH_ENABLE_ASSERTS + PhysicsLockContext mLockContext; +#endif // JPH_ENABLE_ASSERTS + Constraints mConstraints; + mutable Mutex mConstraintsMutex; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h new file mode 100644 index 000000000000..f7493102e2a0 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h @@ -0,0 +1,257 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Constraint that constrains rotation along 1 axis +/// +/// Based on: "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, see section 2.4.5 +/// +/// Constraint equation (eq 108): +/// +/// \f[C = \theta(t) - \theta_{min}\f] +/// +/// Jacobian (eq 109): +/// +/// \f[J = \begin{bmatrix}0 & -a^T & 0 & a^T\end{bmatrix}\f] +/// +/// Used terms (here and below, everything in world space):\n +/// a = axis around which rotation is constrained (normalized).\n +/// x1, x2 = center of mass for the bodies.\n +/// v = [v1, w1, v2, w2].\n +/// v1, v2 = linear velocity of body 1 and 2.\n +/// w1, w2 = angular velocity of body 1 and 2.\n +/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n +/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n +/// b = velocity bias.\n +/// \f$\beta\f$ = baumgarte constant. +class AngleConstraintPart +{ + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, float inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != 0.0f) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + if (ioBody1.IsDynamic()) + ioBody1.GetMotionProperties()->SubAngularVelocityStep(inLambda * mInvI1_Axis); + if (ioBody2.IsDynamic()) + ioBody2.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI2_Axis); + return true; + } + + return false; + } + + /// Internal helper function to calculate the inverse effective mass + JPH_INLINE float CalculateInverseEffectiveMass(const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis) + { + JPH_ASSERT(inWorldSpaceAxis.IsNormalized(1.0e-4f)); + + // Calculate properties used below + mInvI1_Axis = inBody1.IsDynamic()? inBody1.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), inWorldSpaceAxis) : Vec3::sZero(); + mInvI2_Axis = inBody2.IsDynamic()? inBody2.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody2.GetRotation(), inWorldSpaceAxis) : Vec3::sZero(); + + // Calculate inverse effective mass: K = J M^-1 J^T + return inWorldSpaceAxis.Dot(mInvI1_Axis + mInvI2_Axis); + } + +public: + /// Calculate properties used during the functions below + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inWorldSpaceAxis The axis of rotation along which the constraint acts (normalized) + /// Set the following terms to zero if you don't want to drive the constraint to zero with a spring: + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + inline void CalculateConstraintProperties(const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + { + mEffectiveMass = 1.0f / inv_effective_mass; + mSpringPart.CalculateSpringPropertiesWithBias(inBias); + } + } + + /// Calculate properties used during the functions below + /// @param inDeltaTime Time step + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inWorldSpaceAxis The axis of rotation along which the constraint acts (normalized) + /// Set the following terms to zero if you don't want to drive the constraint to zero with a spring: + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + /// @param inC Value of the constraint equation (C) + /// @param inFrequency Oscillation frequency (Hz) + /// @param inDamping Damping factor (0 = no damping, 1 = critical damping) + inline void CalculateConstraintPropertiesWithFrequencyAndDamping(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inFrequency, float inDamping) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inFrequency, inDamping, mEffectiveMass); + } + + /// Calculate properties used during the functions below + /// @param inDeltaTime Time step + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inWorldSpaceAxis The axis of rotation along which the constraint acts (normalized) + /// Set the following terms to zero if you don't want to drive the constraint to zero with a spring: + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + /// @param inC Value of the constraint equation (C) + /// @param inStiffness Spring stiffness k. + /// @param inDamping Spring damping coefficient c. + inline void CalculateConstraintPropertiesWithStiffnessAndDamping(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inStiffness, float inDamping) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inStiffness, inDamping, mEffectiveMass); + } + + /// Selects one of the above functions based on the spring settings + inline void CalculateConstraintPropertiesWithSettings(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, const SpringSettings &inSpringSettings) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else if (inSpringSettings.mMode == ESpringMode::FrequencyAndDamping) + mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mFrequency, inSpringSettings.mDamping, mEffectiveMass); + else + mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mStiffness, inSpringSettings.mDamping, mEffectiveMass); + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = 0.0f; + mTotalLambda = 0.0f; + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass != 0.0f; + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWorldSpaceAxis The axis of rotation along which the constraint acts (normalized) + /// @param inMinLambda Minimum angular impulse to apply (N m s) + /// @param inMaxLambda Maximum angular impulse to apply (N m s) + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda) + { + // Lagrange multiplier is: + // + // lambda = -K^-1 (J v + b) + float lambda = mEffectiveMass * (inWorldSpaceAxis.Dot(ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity()) - mSpringPart.GetBias(mTotalLambda)); + float new_lambda = Clamp(mTotalLambda + lambda, inMinLambda, inMaxLambda); // Clamp impulse + lambda = new_lambda - mTotalLambda; // Lambda potentially got clamped, calculate the new impulse to apply + mTotalLambda = new_lambda; // Store accumulated impulse + + return ApplyVelocityStep(ioBody1, ioBody2, lambda); + } + + /// Return lagrange multiplier + float GetTotalLambda() const + { + return mTotalLambda; + } + + /// Iteratively update the position constraint. Makes sure C(...) == 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inC Value of the constraint equation (C) + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inC, float inBaumgarte) const + { + // Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint + if (inC != 0.0f && !mSpringPart.IsActive()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + float lambda = -mEffectiveMass * inBaumgarte * inC; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + ioBody1.SubRotationStep(lambda * mInvI1_Axis); + if (ioBody2.IsDynamic()) + ioBody2.AddRotationStep(lambda * mInvI2_Axis); + return true; + } + + return false; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Vec3 mInvI1_Axis; + Vec3 mInvI2_Axis; + float mEffectiveMass = 0.0f; + SpringPart mSpringPart; + float mTotalLambda = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h new file mode 100644 index 000000000000..a41ee3af3c85 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h @@ -0,0 +1,682 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Constraint that constrains motion along 1 axis +/// +/// @see "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.1.1 +/// (we're not using the approximation of eq 27 but instead add the U term as in eq 55) +/// +/// Constraint equation (eq 25): +/// +/// \f[C = (p_2 - p_1) \cdot n\f] +/// +/// Jacobian (eq 28): +/// +/// \f[J = \begin{bmatrix} -n^T & (-(r_1 + u) \times n)^T & n^T & (r_2 \times n)^T \end{bmatrix}\f] +/// +/// Used terms (here and below, everything in world space):\n +/// n = constraint axis (normalized).\n +/// p1, p2 = constraint points.\n +/// r1 = p1 - x1.\n +/// r2 = p2 - x2.\n +/// u = x2 + r2 - x1 - r1 = p2 - p1.\n +/// x1, x2 = center of mass for the bodies.\n +/// v = [v1, w1, v2, w2].\n +/// v1, v2 = linear velocity of body 1 and 2.\n +/// w1, w2 = angular velocity of body 1 and 2.\n +/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n +/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n +/// b = velocity bias.\n +/// \f$\beta\f$ = baumgarte constant. +class AxisConstraintPart +{ + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + template + JPH_INLINE bool ApplyVelocityStep(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != 0.0f) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + if constexpr (Type1 == EMotionType::Dynamic) + { + ioMotionProperties1->SubLinearVelocityStep((inLambda * inInvMass1) * inWorldSpaceAxis); + ioMotionProperties1->SubAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis)); + } + if constexpr (Type2 == EMotionType::Dynamic) + { + ioMotionProperties2->AddLinearVelocityStep((inLambda * inInvMass2) * inWorldSpaceAxis); + ioMotionProperties2->AddAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis)); + } + return true; + } + + return false; + } + + /// Internal helper function to calculate the inverse effective mass + template + JPH_INLINE float TemplatedCalculateInverseEffectiveMass(float inInvMass1, Mat44Arg inInvI1, Vec3Arg inR1PlusU, float inInvMass2, Mat44Arg inInvI2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis) + { + JPH_ASSERT(inWorldSpaceAxis.IsNormalized(1.0e-5f)); + + // Calculate properties used below + Vec3 r1_plus_u_x_axis; + if constexpr (Type1 != EMotionType::Static) + { + r1_plus_u_x_axis = inR1PlusU.Cross(inWorldSpaceAxis); + r1_plus_u_x_axis.StoreFloat3(&mR1PlusUxAxis); + } + else + { + #ifdef JPH_DEBUG + Vec3::sNaN().StoreFloat3(&mR1PlusUxAxis); + #endif + } + + Vec3 r2_x_axis; + if constexpr (Type2 != EMotionType::Static) + { + r2_x_axis = inR2.Cross(inWorldSpaceAxis); + r2_x_axis.StoreFloat3(&mR2xAxis); + } + else + { + #ifdef JPH_DEBUG + Vec3::sNaN().StoreFloat3(&mR2xAxis); + #endif + } + + // Calculate inverse effective mass: K = J M^-1 J^T + float inv_effective_mass; + + if constexpr (Type1 == EMotionType::Dynamic) + { + Vec3 invi1_r1_plus_u_x_axis = inInvI1.Multiply3x3(r1_plus_u_x_axis); + invi1_r1_plus_u_x_axis.StoreFloat3(&mInvI1_R1PlusUxAxis); + inv_effective_mass = inInvMass1 + invi1_r1_plus_u_x_axis.Dot(r1_plus_u_x_axis); + } + else + { + (void)r1_plus_u_x_axis; // Fix compiler warning: Not using this (it's not calculated either) + JPH_IF_DEBUG(Vec3::sNaN().StoreFloat3(&mInvI1_R1PlusUxAxis);) + inv_effective_mass = 0.0f; + } + + if constexpr (Type2 == EMotionType::Dynamic) + { + Vec3 invi2_r2_x_axis = inInvI2.Multiply3x3(r2_x_axis); + invi2_r2_x_axis.StoreFloat3(&mInvI2_R2xAxis); + inv_effective_mass += inInvMass2 + invi2_r2_x_axis.Dot(r2_x_axis); + } + else + { + (void)r2_x_axis; // Fix compiler warning: Not using this (it's not calculated either) + JPH_IF_DEBUG(Vec3::sNaN().StoreFloat3(&mInvI2_R2xAxis);) + } + + return inv_effective_mass; + } + + /// Internal helper function to calculate the inverse effective mass + JPH_INLINE float CalculateInverseEffectiveMass(const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis) + { + // Dispatch to the correct templated form + switch (inBody1.GetMotionType()) + { + case EMotionType::Dynamic: + { + const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked(); + float inv_m1 = mp1->GetInverseMass(); + Mat44 inv_i1 = inBody1.GetInverseInertia(); + switch (inBody2.GetMotionType()) + { + case EMotionType::Dynamic: + return TemplatedCalculateInverseEffectiveMass(inv_m1, inv_i1, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis); + + case EMotionType::Kinematic: + return TemplatedCalculateInverseEffectiveMass(inv_m1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis); + + case EMotionType::Static: + return TemplatedCalculateInverseEffectiveMass(inv_m1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis); + + default: + break; + } + break; + } + + case EMotionType::Kinematic: + JPH_ASSERT(inBody2.IsDynamic()); + return TemplatedCalculateInverseEffectiveMass(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis); + + case EMotionType::Static: + JPH_ASSERT(inBody2.IsDynamic()); + return TemplatedCalculateInverseEffectiveMass(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis); + + default: + break; + } + + JPH_ASSERT(false); + return 0.0f; + } + + /// Internal helper function to calculate the inverse effective mass, version that supports mass scaling + JPH_INLINE float CalculateInverseEffectiveMassWithMassOverride(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, Vec3Arg inR1PlusU, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis) + { + // Dispatch to the correct templated form + switch (inBody1.GetMotionType()) + { + case EMotionType::Dynamic: + { + Mat44 inv_i1 = inInvInertiaScale1 * inBody1.GetInverseInertia(); + switch (inBody2.GetMotionType()) + { + case EMotionType::Dynamic: + return TemplatedCalculateInverseEffectiveMass(inInvMass1, inv_i1, inR1PlusU, inInvMass2, inInvInertiaScale2 * inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis); + + case EMotionType::Kinematic: + return TemplatedCalculateInverseEffectiveMass(inInvMass1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis); + + case EMotionType::Static: + return TemplatedCalculateInverseEffectiveMass(inInvMass1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis); + + default: + break; + } + break; + } + + case EMotionType::Kinematic: + JPH_ASSERT(inBody2.IsDynamic()); + return TemplatedCalculateInverseEffectiveMass(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inInvMass2, inInvInertiaScale2 * inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis); + + case EMotionType::Static: + JPH_ASSERT(inBody2.IsDynamic()); + return TemplatedCalculateInverseEffectiveMass(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inInvMass2, inInvInertiaScale2 * inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis); + + default: + break; + } + + JPH_ASSERT(false); + return 0.0f; + } + +public: + /// Templated form of CalculateConstraintProperties with the motion types baked in + template + JPH_INLINE void TemplatedCalculateConstraintProperties(float inInvMass1, Mat44Arg inInvI1, Vec3Arg inR1PlusU, float inInvMass2, Mat44Arg inInvI2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f) + { + float inv_effective_mass = TemplatedCalculateInverseEffectiveMass(inInvMass1, inInvI1, inR1PlusU, inInvMass2, inInvI2, inR2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + { + mEffectiveMass = 1.0f / inv_effective_mass; + mSpringPart.CalculateSpringPropertiesWithBias(inBias); + } + + JPH_DET_LOG("TemplatedCalculateConstraintProperties: invM1: " << inInvMass1 << " invI1: " << inInvI1 << " r1PlusU: " << inR1PlusU << " invM2: " << inInvMass2 << " invI2: " << inInvI2 << " r2: " << inR2 << " bias: " << inBias << " r1PlusUxAxis: " << mR1PlusUxAxis << " r2xAxis: " << mR2xAxis << " invI1_R1PlusUxAxis: " << mInvI1_R1PlusUxAxis << " invI2_R2xAxis: " << mInvI2_R2xAxis << " effectiveMass: " << mEffectiveMass << " totalLambda: " << mTotalLambda); + } + + /// Calculate properties used during the functions below + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inR1PlusU See equations above (r1 + u) + /// @param inR2 See equations above (r2) + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2) + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + inline void CalculateConstraintProperties(const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + { + mEffectiveMass = 1.0f / inv_effective_mass; + mSpringPart.CalculateSpringPropertiesWithBias(inBias); + } + } + + /// Calculate properties used during the functions below, version that supports mass scaling + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inInvMass1 The inverse mass of body 1 (only used when body 1 is dynamic) + /// @param inInvMass2 The inverse mass of body 2 (only used when body 2 is dynamic) + /// @param inInvInertiaScale1 Scale factor for the inverse inertia of body 1 + /// @param inInvInertiaScale2 Scale factor for the inverse inertia of body 2 + /// @param inR1PlusU See equations above (r1 + u) + /// @param inR2 See equations above (r2) + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2) + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + inline void CalculateConstraintPropertiesWithMassOverride(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, Vec3Arg inR1PlusU, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f) + { + float inv_effective_mass = CalculateInverseEffectiveMassWithMassOverride(inBody1, inInvMass1, inInvInertiaScale1, inR1PlusU, inBody2, inInvMass2, inInvInertiaScale2, inR2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + { + mEffectiveMass = 1.0f / inv_effective_mass; + mSpringPart.CalculateSpringPropertiesWithBias(inBias); + } + } + + /// Calculate properties used during the functions below + /// @param inDeltaTime Time step + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inR1PlusU See equations above (r1 + u) + /// @param inR2 See equations above (r2) + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2) + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + /// @param inC Value of the constraint equation (C). + /// @param inFrequency Oscillation frequency (Hz). + /// @param inDamping Damping factor (0 = no damping, 1 = critical damping). + inline void CalculateConstraintPropertiesWithFrequencyAndDamping(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inFrequency, float inDamping) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inFrequency, inDamping, mEffectiveMass); + } + + /// Calculate properties used during the functions below + /// @param inDeltaTime Time step + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inR1PlusU See equations above (r1 + u) + /// @param inR2 See equations above (r2) + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2) + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + /// @param inC Value of the constraint equation (C). + /// @param inStiffness Spring stiffness k. + /// @param inDamping Spring damping coefficient c. + inline void CalculateConstraintPropertiesWithStiffnessAndDamping(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inStiffness, float inDamping) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inStiffness, inDamping, mEffectiveMass); + } + + /// Selects one of the above functions based on the spring settings + inline void CalculateConstraintPropertiesWithSettings(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, const SpringSettings &inSpringSettings) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else if (inSpringSettings.mMode == ESpringMode::FrequencyAndDamping) + mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mFrequency, inSpringSettings.mDamping, mEffectiveMass); + else + mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mStiffness, inSpringSettings.mDamping, mEffectiveMass); + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = 0.0f; + mTotalLambda = 0.0f; + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass != 0.0f; + } + + /// Templated form of WarmStart with the motion types baked in + template + inline void TemplatedWarmStart(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + + ApplyVelocityStep(ioMotionProperties1, inInvMass1, ioMotionProperties2, inInvMass2, inWorldSpaceAxis, mTotalLambda); + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized) + /// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame + inline void WarmStart(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inWarmStartImpulseRatio) + { + EMotionType motion_type1 = ioBody1.GetMotionType(); + MotionProperties *motion_properties1 = ioBody1.GetMotionPropertiesUnchecked(); + + EMotionType motion_type2 = ioBody2.GetMotionType(); + MotionProperties *motion_properties2 = ioBody2.GetMotionPropertiesUnchecked(); + + // Dispatch to the correct templated form + // Note: Warm starting doesn't differentiate between kinematic/static bodies so we handle both as static bodies + if (motion_type1 == EMotionType::Dynamic) + { + if (motion_type2 == EMotionType::Dynamic) + TemplatedWarmStart(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inWarmStartImpulseRatio); + else + TemplatedWarmStart(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inWarmStartImpulseRatio); + } + else + { + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + TemplatedWarmStart(motion_properties1, 0.0f /* Unused */, motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inWarmStartImpulseRatio); + } + } + + /// Templated form of SolveVelocityConstraint with the motion types baked in, part 1: get the total lambda + template + JPH_INLINE float TemplatedSolveVelocityConstraintGetTotalLambda(const MotionProperties *ioMotionProperties1, const MotionProperties *ioMotionProperties2, Vec3Arg inWorldSpaceAxis) const + { + // Calculate jacobian multiplied by linear velocity + float jv; + if constexpr (Type1 != EMotionType::Static && Type2 != EMotionType::Static) + jv = inWorldSpaceAxis.Dot(ioMotionProperties1->GetLinearVelocity() - ioMotionProperties2->GetLinearVelocity()); + else if constexpr (Type1 != EMotionType::Static) + jv = inWorldSpaceAxis.Dot(ioMotionProperties1->GetLinearVelocity()); + else if constexpr (Type2 != EMotionType::Static) + jv = inWorldSpaceAxis.Dot(-ioMotionProperties2->GetLinearVelocity()); + else + JPH_ASSERT(false); // Static vs static is nonsensical! + + // Calculate jacobian multiplied by angular velocity + if constexpr (Type1 != EMotionType::Static) + jv += Vec3::sLoadFloat3Unsafe(mR1PlusUxAxis).Dot(ioMotionProperties1->GetAngularVelocity()); + if constexpr (Type2 != EMotionType::Static) + jv -= Vec3::sLoadFloat3Unsafe(mR2xAxis).Dot(ioMotionProperties2->GetAngularVelocity()); + + // Lagrange multiplier is: + // + // lambda = -K^-1 (J v + b) + float lambda = mEffectiveMass * (jv - mSpringPart.GetBias(mTotalLambda)); + + // Return the total accumulated lambda + return mTotalLambda + lambda; + } + + /// Templated form of SolveVelocityConstraint with the motion types baked in, part 2: apply new lambda + template + JPH_INLINE bool TemplatedSolveVelocityConstraintApplyLambda(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inTotalLambda) + { + float delta_lambda = inTotalLambda - mTotalLambda; // Calculate change in lambda + mTotalLambda = inTotalLambda; // Store accumulated impulse + + return ApplyVelocityStep(ioMotionProperties1, inInvMass1, ioMotionProperties2, inInvMass2, inWorldSpaceAxis, delta_lambda); + } + + /// Templated form of SolveVelocityConstraint with the motion types baked in + template + inline bool TemplatedSolveVelocityConstraint(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda) + { + float total_lambda = TemplatedSolveVelocityConstraintGetTotalLambda(ioMotionProperties1, ioMotionProperties2, inWorldSpaceAxis); + + // Clamp impulse to specified range + total_lambda = Clamp(total_lambda, inMinLambda, inMaxLambda); + + return TemplatedSolveVelocityConstraintApplyLambda(ioMotionProperties1, inInvMass1, ioMotionProperties2, inInvMass2, inWorldSpaceAxis, total_lambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized) + /// @param inMinLambda Minimum value of constraint impulse to apply (N s) + /// @param inMaxLambda Maximum value of constraint impulse to apply (N s) + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda) + { + EMotionType motion_type1 = ioBody1.GetMotionType(); + MotionProperties *motion_properties1 = ioBody1.GetMotionPropertiesUnchecked(); + + EMotionType motion_type2 = ioBody2.GetMotionType(); + MotionProperties *motion_properties2 = ioBody2.GetMotionPropertiesUnchecked(); + + // Dispatch to the correct templated form + switch (motion_type1) + { + case EMotionType::Dynamic: + switch (motion_type2) + { + case EMotionType::Dynamic: + return TemplatedSolveVelocityConstraint(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inMinLambda, inMaxLambda); + + case EMotionType::Kinematic: + return TemplatedSolveVelocityConstraint(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda); + + case EMotionType::Static: + return TemplatedSolveVelocityConstraint(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda); + + default: + JPH_ASSERT(false); + break; + } + break; + + case EMotionType::Kinematic: + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + return TemplatedSolveVelocityConstraint(motion_properties1, 0.0f /* Unused */, motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inMinLambda, inMaxLambda); + + case EMotionType::Static: + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + return TemplatedSolveVelocityConstraint(motion_properties1, 0.0f /* Unused */, motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inMinLambda, inMaxLambda); + + default: + JPH_ASSERT(false); + break; + } + + return false; + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inInvMass1 The inverse mass of body 1 (only used when body 1 is dynamic) + /// @param inInvMass2 The inverse mass of body 2 (only used when body 2 is dynamic) + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized) + /// @param inMinLambda Minimum value of constraint impulse to apply (N s) + /// @param inMaxLambda Maximum value of constraint impulse to apply (N s) + inline bool SolveVelocityConstraintWithMassOverride(Body &ioBody1, float inInvMass1, Body &ioBody2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda) + { + EMotionType motion_type1 = ioBody1.GetMotionType(); + MotionProperties *motion_properties1 = ioBody1.GetMotionPropertiesUnchecked(); + + EMotionType motion_type2 = ioBody2.GetMotionType(); + MotionProperties *motion_properties2 = ioBody2.GetMotionPropertiesUnchecked(); + + // Dispatch to the correct templated form + switch (motion_type1) + { + case EMotionType::Dynamic: + switch (motion_type2) + { + case EMotionType::Dynamic: + return TemplatedSolveVelocityConstraint(motion_properties1, inInvMass1, motion_properties2, inInvMass2, inWorldSpaceAxis, inMinLambda, inMaxLambda); + + case EMotionType::Kinematic: + return TemplatedSolveVelocityConstraint(motion_properties1, inInvMass1, motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda); + + case EMotionType::Static: + return TemplatedSolveVelocityConstraint(motion_properties1, inInvMass1, motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda); + + default: + JPH_ASSERT(false); + break; + } + break; + + case EMotionType::Kinematic: + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + return TemplatedSolveVelocityConstraint(motion_properties1, 0.0f /* Unused */, motion_properties2, inInvMass2, inWorldSpaceAxis, inMinLambda, inMaxLambda); + + case EMotionType::Static: + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + return TemplatedSolveVelocityConstraint(motion_properties1, 0.0f /* Unused */, motion_properties2, inInvMass2, inWorldSpaceAxis, inMinLambda, inMaxLambda); + + default: + JPH_ASSERT(false); + break; + } + + return false; + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized) + /// @param inC Value of the constraint equation (C) + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inC, float inBaumgarte) const + { + // Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint + if (inC != 0.0f && !mSpringPart.IsActive()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + float lambda = -mEffectiveMass * inBaumgarte * inC; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + { + ioBody1.SubPositionStep((lambda * ioBody1.GetMotionProperties()->GetInverseMass()) * inWorldSpaceAxis); + ioBody1.SubRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis)); + } + if (ioBody2.IsDynamic()) + { + ioBody2.AddPositionStep((lambda * ioBody2.GetMotionProperties()->GetInverseMass()) * inWorldSpaceAxis); + ioBody2.AddRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis)); + } + return true; + } + + return false; + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inInvMass1 The inverse mass of body 1 (only used when body 1 is dynamic) + /// @param inInvMass2 The inverse mass of body 2 (only used when body 2 is dynamic) + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized) + /// @param inC Value of the constraint equation (C) + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraintWithMassOverride(Body &ioBody1, float inInvMass1, Body &ioBody2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inC, float inBaumgarte) const + { + // Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint + if (inC != 0.0f && !mSpringPart.IsActive()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + float lambda = -mEffectiveMass * inBaumgarte * inC; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + { + ioBody1.SubPositionStep((lambda * inInvMass1) * inWorldSpaceAxis); + ioBody1.SubRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis)); + } + if (ioBody2.IsDynamic()) + { + ioBody2.AddPositionStep((lambda * inInvMass2) * inWorldSpaceAxis); + ioBody2.AddRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis)); + } + return true; + } + + return false; + } + + /// Override total lagrange multiplier, can be used to set the initial value for warm starting + inline void SetTotalLambda(float inLambda) + { + mTotalLambda = inLambda; + } + + /// Return lagrange multiplier + inline float GetTotalLambda() const + { + return mTotalLambda; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Float3 mR1PlusUxAxis; + Float3 mR2xAxis; + Float3 mInvI1_R1PlusUxAxis; + Float3 mInvI2_R2xAxis; + float mEffectiveMass = 0.0f; + SpringPart mSpringPart; + float mTotalLambda = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h new file mode 100644 index 000000000000..c0ebe5ac42e9 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h @@ -0,0 +1,276 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/** + Constrains movement on 2 axis + + @see "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.3.1 + + Constraint equation (eq 51): + + \f[C = \begin{bmatrix} (p_2 - p_1) \cdot n_1 \\ (p_2 - p_1) \cdot n_2\end{bmatrix}\f] + + Jacobian (transposed) (eq 55): + + \f[J^T = \begin{bmatrix} + -n_1 & -n_2 \\ + -(r_1 + u) \times n_1 & -(r_1 + u) \times n_2 \\ + n_1 & n_2 \\ + r_2 \times n_1 & r_2 \times n_2 + \end{bmatrix}\f] + + Used terms (here and below, everything in world space):\n + n1, n2 = constraint axis (normalized).\n + p1, p2 = constraint points.\n + r1 = p1 - x1.\n + r2 = p2 - x2.\n + u = x2 + r2 - x1 - r1 = p2 - p1.\n + x1, x2 = center of mass for the bodies.\n + v = [v1, w1, v2, w2].\n + v1, v2 = linear velocity of body 1 and 2.\n + w1, w2 = angular velocity of body 1 and 2.\n + M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n + \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n + b = velocity bias.\n + \f$\beta\f$ = baumgarte constant. +**/ +class DualAxisConstraintPart +{ +public: + using Vec2 = Vector<2>; + using Mat22 = Matrix<2, 2>; + +private: + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, const Vec2 &inLambda) const + { + // Apply impulse if delta is not zero + if (!inLambda.IsZero()) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + Vec3 impulse = inN1 * inLambda[0] + inN2 * inLambda[1]; + if (ioBody1.IsDynamic()) + { + MotionProperties *mp1 = ioBody1.GetMotionProperties(); + mp1->SubLinearVelocityStep(mp1->GetInverseMass() * impulse); + mp1->SubAngularVelocityStep(mInvI1_R1PlusUxN1 * inLambda[0] + mInvI1_R1PlusUxN2 * inLambda[1]); + } + if (ioBody2.IsDynamic()) + { + MotionProperties *mp2 = ioBody2.GetMotionProperties(); + mp2->AddLinearVelocityStep(mp2->GetInverseMass() * impulse); + mp2->AddAngularVelocityStep(mInvI2_R2xN1 * inLambda[0] + mInvI2_R2xN2 * inLambda[1]); + } + return true; + } + + return false; + } + + /// Internal helper function to calculate the lagrange multiplier + inline void CalculateLagrangeMultiplier(const Body &inBody1, const Body &inBody2, Vec3Arg inN1, Vec3Arg inN2, Vec2 &outLambda) const + { + // Calculate lagrange multiplier: + // + // lambda = -K^-1 (J v + b) + Vec3 delta_lin = inBody1.GetLinearVelocity() - inBody2.GetLinearVelocity(); + Vec2 jv; + jv[0] = inN1.Dot(delta_lin) + mR1PlusUxN1.Dot(inBody1.GetAngularVelocity()) - mR2xN1.Dot(inBody2.GetAngularVelocity()); + jv[1] = inN2.Dot(delta_lin) + mR1PlusUxN2.Dot(inBody1.GetAngularVelocity()) - mR2xN2.Dot(inBody2.GetAngularVelocity()); + outLambda = mEffectiveMass * jv; + } + +public: + /// Calculate properties used during the functions below + /// All input vectors are in world space + inline void CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, Vec3Arg inR1PlusU, const Body &inBody2, Mat44Arg inRotation2, Vec3Arg inR2, Vec3Arg inN1, Vec3Arg inN2) + { + JPH_ASSERT(inN1.IsNormalized(1.0e-5f)); + JPH_ASSERT(inN2.IsNormalized(1.0e-5f)); + + // Calculate properties used during constraint solving + mR1PlusUxN1 = inR1PlusU.Cross(inN1); + mR1PlusUxN2 = inR1PlusU.Cross(inN2); + mR2xN1 = inR2.Cross(inN1); + mR2xN2 = inR2.Cross(inN2); + + // Calculate effective mass: K^-1 = (J M^-1 J^T)^-1, eq 59 + Mat22 inv_effective_mass; + if (inBody1.IsDynamic()) + { + const MotionProperties *mp1 = inBody1.GetMotionProperties(); + Mat44 inv_i1 = mp1->GetInverseInertiaForRotation(inRotation1); + mInvI1_R1PlusUxN1 = inv_i1.Multiply3x3(mR1PlusUxN1); + mInvI1_R1PlusUxN2 = inv_i1.Multiply3x3(mR1PlusUxN2); + + inv_effective_mass(0, 0) = mp1->GetInverseMass() + mR1PlusUxN1.Dot(mInvI1_R1PlusUxN1); + inv_effective_mass(0, 1) = mR1PlusUxN1.Dot(mInvI1_R1PlusUxN2); + inv_effective_mass(1, 0) = mR1PlusUxN2.Dot(mInvI1_R1PlusUxN1); + inv_effective_mass(1, 1) = mp1->GetInverseMass() + mR1PlusUxN2.Dot(mInvI1_R1PlusUxN2); + } + else + { + JPH_IF_DEBUG(mInvI1_R1PlusUxN1 = Vec3::sNaN();) + JPH_IF_DEBUG(mInvI1_R1PlusUxN2 = Vec3::sNaN();) + + inv_effective_mass = Mat22::sZero(); + } + + if (inBody2.IsDynamic()) + { + const MotionProperties *mp2 = inBody2.GetMotionProperties(); + Mat44 inv_i2 = mp2->GetInverseInertiaForRotation(inRotation2); + mInvI2_R2xN1 = inv_i2.Multiply3x3(mR2xN1); + mInvI2_R2xN2 = inv_i2.Multiply3x3(mR2xN2); + + inv_effective_mass(0, 0) += mp2->GetInverseMass() + mR2xN1.Dot(mInvI2_R2xN1); + inv_effective_mass(0, 1) += mR2xN1.Dot(mInvI2_R2xN2); + inv_effective_mass(1, 0) += mR2xN2.Dot(mInvI2_R2xN1); + inv_effective_mass(1, 1) += mp2->GetInverseMass() + mR2xN2.Dot(mInvI2_R2xN2); + } + else + { + JPH_IF_DEBUG(mInvI2_R2xN1 = Vec3::sNaN();) + JPH_IF_DEBUG(mInvI2_R2xN2 = Vec3::sNaN();) + } + + if (!mEffectiveMass.SetInversed(inv_effective_mass)) + Deactivate(); + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass.SetZero(); + mTotalLambda.SetZero(); + } + + /// Check if constraint is active + inline bool IsActive() const + { + return !mEffectiveMass.IsZero(); + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + /// All input vectors are in world space + inline void WarmStart(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// All input vectors are in world space + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2) + { + Vec2 lambda; + CalculateLagrangeMultiplier(ioBody1, ioBody2, inN1, inN2, lambda); + + // Store accumulated lambda + mTotalLambda += lambda; + + return ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, lambda); + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + /// All input vectors are in world space + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inU, Vec3Arg inN1, Vec3Arg inN2, float inBaumgarte) const + { + Vec2 c; + c[0] = inU.Dot(inN1); + c[1] = inU.Dot(inN2); + if (!c.IsZero()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + Vec2 lambda = -inBaumgarte * (mEffectiveMass * c); + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + Vec3 impulse = inN1 * lambda[0] + inN2 * lambda[1]; + if (ioBody1.IsDynamic()) + { + ioBody1.SubPositionStep(ioBody1.GetMotionProperties()->GetInverseMass() * impulse); + ioBody1.SubRotationStep(mInvI1_R1PlusUxN1 * lambda[0] + mInvI1_R1PlusUxN2 * lambda[1]); + } + if (ioBody2.IsDynamic()) + { + ioBody2.AddPositionStep(ioBody2.GetMotionProperties()->GetInverseMass() * impulse); + ioBody2.AddRotationStep(mInvI2_R2xN1 * lambda[0] + mInvI2_R2xN2 * lambda[1]); + } + return true; + } + + return false; + } + + /// Override total lagrange multiplier, can be used to set the initial value for warm starting + inline void SetTotalLambda(const Vec2 &inLambda) + { + mTotalLambda = inLambda; + } + + /// Return lagrange multiplier + inline const Vec2 & GetTotalLambda() const + { + return mTotalLambda; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Vec3 mR1PlusUxN1; + Vec3 mR1PlusUxN2; + Vec3 mR2xN1; + Vec3 mR2xN2; + Vec3 mInvI1_R1PlusUxN1; + Vec3 mInvI1_R1PlusUxN2; + Vec3 mInvI2_R2xN1; + Vec3 mInvI2_R2xN2; + Mat22 mEffectiveMass; + Vec2 mTotalLambda { Vec2::sZero() }; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/GearConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/GearConstraintPart.h new file mode 100644 index 000000000000..6e277a64b563 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/GearConstraintPart.h @@ -0,0 +1,195 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Constraint that constrains two rotations using a gear (rotating in opposite direction) +/// +/// Constraint equation: +/// +/// C = Rotation1(t) + r Rotation2(t) +/// +/// Derivative: +/// +/// d/dt C = 0 +/// <=> w1 . a + r w2 . b = 0 +/// +/// Jacobian: +/// +/// \f[J = \begin{bmatrix}0 & a^T & 0 & r b^T\end{bmatrix}\f] +/// +/// Used terms (here and below, everything in world space):\n +/// a = axis around which body 1 rotates (normalized).\n +/// b = axis along which body 2 slides (normalized).\n +/// Rotation1(t) = rotation around a of body 1.\n +/// Rotation2(t) = rotation around b of body 2.\n +/// r = ratio between rotation for body 1 and 2.\n +/// v = [v1, w1, v2, w2].\n +/// v1, v2 = linear velocity of body 1 and 2.\n +/// w1, w2 = angular velocity of body 1 and 2.\n +/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n +/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n +/// \f$\beta\f$ = baumgarte constant. +class GearConstraintPart +{ + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, float inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != 0.0f) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + ioBody1.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI1_A); + ioBody2.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI2_B); + return true; + } + + return false; + } + +public: + /// Calculate properties used during the functions below + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inWorldSpaceHingeAxis1 The axis around which body 1 rotates + /// @param inWorldSpaceHingeAxis2 The axis around which body 2 rotates + /// @param inRatio The ratio between rotation and translation + inline void CalculateConstraintProperties(const Body &inBody1, Vec3Arg inWorldSpaceHingeAxis1, const Body &inBody2, Vec3Arg inWorldSpaceHingeAxis2, float inRatio) + { + JPH_ASSERT(inWorldSpaceHingeAxis1.IsNormalized(1.0e-4f)); + JPH_ASSERT(inWorldSpaceHingeAxis2.IsNormalized(1.0e-4f)); + + // Calculate: I1^-1 a + mInvI1_A = inBody1.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), inWorldSpaceHingeAxis1); + + // Calculate: I2^-1 b + mInvI2_B = inBody2.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody2.GetRotation(), inWorldSpaceHingeAxis2); + + // K^-1 = 1 / (J M^-1 J^T) = 1 / (a^T I1^-1 a + r^2 * b^T I2^-1 b) + float inv_effective_mass = (inWorldSpaceHingeAxis1.Dot(mInvI1_A) + inWorldSpaceHingeAxis2.Dot(mInvI2_B) * Square(inRatio)); + if (inv_effective_mass == 0.0f) + Deactivate(); + else + mEffectiveMass = 1.0f / inv_effective_mass; + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = 0.0f; + mTotalLambda = 0.0f; + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass != 0.0f; + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWorldSpaceHingeAxis1 The axis around which body 1 rotates + /// @param inWorldSpaceHingeAxis2 The axis around which body 2 rotates + /// @param inRatio The ratio between rotation and translation + inline bool SolveVelocityConstraint(Body &ioBody1, Vec3Arg inWorldSpaceHingeAxis1, Body &ioBody2, Vec3Arg inWorldSpaceHingeAxis2, float inRatio) + { + // Lagrange multiplier is: + // + // lambda = -K^-1 (J v + b) + float lambda = -mEffectiveMass * (inWorldSpaceHingeAxis1.Dot(ioBody1.GetAngularVelocity()) + inRatio * inWorldSpaceHingeAxis2.Dot(ioBody2.GetAngularVelocity())); + mTotalLambda += lambda; // Store accumulated impulse + + return ApplyVelocityStep(ioBody1, ioBody2, lambda); + } + + /// Return lagrange multiplier + float GetTotalLambda() const + { + return mTotalLambda; + } + + /// Iteratively update the position constraint. Makes sure C(...) == 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inC Value of the constraint equation (C) + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inC, float inBaumgarte) const + { + // Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint + if (inC != 0.0f) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + float lambda = -mEffectiveMass * inBaumgarte * inC; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + ioBody1.AddRotationStep(lambda * mInvI1_A); + if (ioBody2.IsDynamic()) + ioBody2.AddRotationStep(lambda * mInvI2_B); + return true; + } + + return false; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Vec3 mInvI1_A; + Vec3 mInvI2_B; + float mEffectiveMass = 0.0f; + float mTotalLambda = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/HingeRotationConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/HingeRotationConstraintPart.h new file mode 100644 index 000000000000..5f566f701793 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/HingeRotationConstraintPart.h @@ -0,0 +1,222 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/** + Constrains rotation around 2 axis so that it only allows rotation around 1 axis + + Based on: "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.4.1 + + Constraint equation (eq 87): + + \f[C = \begin{bmatrix}a_1 \cdot b_2 \\ a_1 \cdot c_2\end{bmatrix}\f] + + Jacobian (eq 90): + + \f[J = \begin{bmatrix} + 0 & -b_2 \times a_1 & 0 & b_2 \times a_1 \\ + 0 & -c_2 \times a_1 & 0 & c2 \times a_1 + \end{bmatrix}\f] + + Used terms (here and below, everything in world space):\n + a1 = hinge axis on body 1.\n + b2, c2 = axis perpendicular to hinge axis on body 2.\n + x1, x2 = center of mass for the bodies.\n + v = [v1, w1, v2, w2].\n + v1, v2 = linear velocity of body 1 and 2.\n + w1, w2 = angular velocity of body 1 and 2.\n + M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n + \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n + b = velocity bias.\n + \f$\beta\f$ = baumgarte constant.\n + E = identity matrix. +**/ +class HingeRotationConstraintPart +{ +public: + using Vec2 = Vector<2>; + using Mat22 = Matrix<2, 2>; + +private: + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, const Vec2 &inLambda) const + { + // Apply impulse if delta is not zero + if (!inLambda.IsZero()) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + Vec3 impulse = mB2xA1 * inLambda[0] + mC2xA1 * inLambda[1]; + if (ioBody1.IsDynamic()) + ioBody1.GetMotionProperties()->SubAngularVelocityStep(mInvI1.Multiply3x3(impulse)); + if (ioBody2.IsDynamic()) + ioBody2.GetMotionProperties()->AddAngularVelocityStep(mInvI2.Multiply3x3(impulse)); + return true; + } + + return false; + } + +public: + /// Calculate properties used during the functions below + inline void CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, Vec3Arg inWorldSpaceHingeAxis1, const Body &inBody2, Mat44Arg inRotation2, Vec3Arg inWorldSpaceHingeAxis2) + { + JPH_ASSERT(inWorldSpaceHingeAxis1.IsNormalized(1.0e-5f)); + JPH_ASSERT(inWorldSpaceHingeAxis2.IsNormalized(1.0e-5f)); + + // Calculate hinge axis in world space + mA1 = inWorldSpaceHingeAxis1; + Vec3 a2 = inWorldSpaceHingeAxis2; + float dot = mA1.Dot(a2); + if (dot <= 1.0e-3f) + { + // World space axes are more than 90 degrees apart, get a perpendicular vector in the plane formed by mA1 and a2 as hinge axis until the rotation is less than 90 degrees + Vec3 perp = a2 - dot * mA1; + if (perp.LengthSq() < 1.0e-6f) + { + // mA1 ~ -a2, take random perpendicular + perp = mA1.GetNormalizedPerpendicular(); + } + + // Blend in a little bit from mA1 so we're less than 90 degrees apart + a2 = (0.99f * perp.Normalized() + 0.01f * mA1).Normalized(); + } + mB2 = a2.GetNormalizedPerpendicular(); + mC2 = a2.Cross(mB2); + + // Calculate properties used during constraint solving + mInvI1 = inBody1.IsDynamic()? inBody1.GetMotionProperties()->GetInverseInertiaForRotation(inRotation1) : Mat44::sZero(); + mInvI2 = inBody2.IsDynamic()? inBody2.GetMotionProperties()->GetInverseInertiaForRotation(inRotation2) : Mat44::sZero(); + mB2xA1 = mB2.Cross(mA1); + mC2xA1 = mC2.Cross(mA1); + + // Calculate effective mass: K^-1 = (J M^-1 J^T)^-1 + Mat44 summed_inv_inertia = mInvI1 + mInvI2; + Mat22 inv_effective_mass; + inv_effective_mass(0, 0) = mB2xA1.Dot(summed_inv_inertia.Multiply3x3(mB2xA1)); + inv_effective_mass(0, 1) = mB2xA1.Dot(summed_inv_inertia.Multiply3x3(mC2xA1)); + inv_effective_mass(1, 0) = mC2xA1.Dot(summed_inv_inertia.Multiply3x3(mB2xA1)); + inv_effective_mass(1, 1) = mC2xA1.Dot(summed_inv_inertia.Multiply3x3(mC2xA1)); + if (!mEffectiveMass.SetInversed(inv_effective_mass)) + Deactivate(); + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass.SetZero(); + mTotalLambda.SetZero(); + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2) + { + // Calculate lagrange multiplier: + // + // lambda = -K^-1 (J v + b) + Vec3 delta_ang = ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity(); + Vec2 jv; + jv[0] = mB2xA1.Dot(delta_ang); + jv[1] = mC2xA1.Dot(delta_ang); + Vec2 lambda = mEffectiveMass * jv; + + // Store accumulated lambda + mTotalLambda += lambda; + + return ApplyVelocityStep(ioBody1, ioBody2, lambda); + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inBaumgarte) const + { + // Constraint needs Axis of body 1 perpendicular to both B and C from body 2 (which are both perpendicular to the Axis of body 2) + Vec2 c; + c[0] = mA1.Dot(mB2); + c[1] = mA1.Dot(mC2); + if (!c.IsZero()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + Vec2 lambda = -inBaumgarte * (mEffectiveMass * c); + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + Vec3 impulse = mB2xA1 * lambda[0] + mC2xA1 * lambda[1]; + if (ioBody1.IsDynamic()) + ioBody1.SubRotationStep(mInvI1.Multiply3x3(impulse)); + if (ioBody2.IsDynamic()) + ioBody2.AddRotationStep(mInvI2.Multiply3x3(impulse)); + return true; + } + + return false; + } + + /// Return lagrange multiplier + const Vec2 & GetTotalLambda() const + { + return mTotalLambda; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Vec3 mA1; ///< World space hinge axis for body 1 + Vec3 mB2; ///< World space perpendiculars of hinge axis for body 2 + Vec3 mC2; + Mat44 mInvI1; + Mat44 mInvI2; + Vec3 mB2xA1; + Vec3 mC2xA1; + Mat22 mEffectiveMass; + Vec2 mTotalLambda { Vec2::sZero() }; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/IndependentAxisConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/IndependentAxisConstraintPart.h new file mode 100644 index 000000000000..5b752c6c6936 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/IndependentAxisConstraintPart.h @@ -0,0 +1,246 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Constraint part to an AxisConstraintPart but both bodies have an independent axis on which the force is applied. +/// +/// Constraint equation: +/// +/// \f[C = (x_1 + r_1 - f_1) . n_1 + r (x_2 + r_2 - f_2) \cdot n_2\f] +/// +/// Calculating the Jacobian: +/// +/// \f[dC/dt = (v_1 + w_1 \times r_1) \cdot n_1 + (x_1 + r_1 - f_1) \cdot d n_1/dt + r (v_2 + w_2 \times r_2) \cdot n_2 + r (x_2 + r_2 - f_2) \cdot d n_2/dt\f] +/// +/// Assuming that d n1/dt and d n2/dt are small this becomes: +/// +/// \f[(v_1 + w_1 \times r_1) \cdot n_1 + r (v_2 + w_2 \times r_2) \cdot n_2\f] +/// \f[= v_1 \cdot n_1 + r_1 \times n_1 \cdot w_1 + r v_2 \cdot n_2 + r r_2 \times n_2 \cdot w_2\f] +/// +/// Jacobian: +/// +/// \f[J = \begin{bmatrix}n_1 & r_1 \times n_1 & r n_2 & r r_2 \times n_2\end{bmatrix}\f] +/// +/// Effective mass: +/// +/// \f[K = m_1^{-1} + r_1 \times n_1 I_1^{-1} r_1 \times n_1 + r^2 m_2^{-1} + r^2 r_2 \times n_2 I_2^{-1} r_2 \times n_2\f] +/// +/// Used terms (here and below, everything in world space):\n +/// n1 = (x1 + r1 - f1) / |x1 + r1 - f1|, axis along which the force is applied for body 1\n +/// n2 = (x2 + r2 - f2) / |x2 + r2 - f2|, axis along which the force is applied for body 2\n +/// r = ratio how forces are applied between bodies.\n +/// x1, x2 = center of mass for the bodies.\n +/// v = [v1, w1, v2, w2].\n +/// v1, v2 = linear velocity of body 1 and 2.\n +/// w1, w2 = angular velocity of body 1 and 2.\n +/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n +/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n +/// b = velocity bias.\n +/// \f$\beta\f$ = baumgarte constant. +class IndependentAxisConstraintPart +{ + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != 0.0f) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + if (ioBody1.IsDynamic()) + { + MotionProperties *mp1 = ioBody1.GetMotionProperties(); + mp1->AddLinearVelocityStep((mp1->GetInverseMass() * inLambda) * inN1); + mp1->AddAngularVelocityStep(mInvI1_R1xN1 * inLambda); + } + if (ioBody2.IsDynamic()) + { + MotionProperties *mp2 = ioBody2.GetMotionProperties(); + mp2->AddLinearVelocityStep((inRatio * mp2->GetInverseMass() * inLambda) * inN2); + mp2->AddAngularVelocityStep(mInvI2_RatioR2xN2 * inLambda); + } + return true; + } + + return false; + } + +public: + /// Calculate properties used during the functions below + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inR1 The position on which the constraint operates on body 1 relative to COM + /// @param inN1 The world space normal in which the constraint operates for body 1 + /// @param inR2 The position on which the constraint operates on body 1 relative to COM + /// @param inN2 The world space normal in which the constraint operates for body 2 + /// @param inRatio The ratio how forces are applied between bodies + inline void CalculateConstraintProperties(const Body &inBody1, const Body &inBody2, Vec3Arg inR1, Vec3Arg inN1, Vec3Arg inR2, Vec3Arg inN2, float inRatio) + { + JPH_ASSERT(inN1.IsNormalized(1.0e-4f) && inN2.IsNormalized(1.0e-4f)); + + float inv_effective_mass = 0.0f; + + if (!inBody1.IsStatic()) + { + const MotionProperties *mp1 = inBody1.GetMotionProperties(); + + mR1xN1 = inR1.Cross(inN1); + mInvI1_R1xN1 = mp1->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), mR1xN1); + + inv_effective_mass += mp1->GetInverseMass() + mInvI1_R1xN1.Dot(mR1xN1); + } + + if (!inBody2.IsStatic()) + { + const MotionProperties *mp2 = inBody2.GetMotionProperties(); + + mRatioR2xN2 = inRatio * inR2.Cross(inN2); + mInvI2_RatioR2xN2 = mp2->MultiplyWorldSpaceInverseInertiaByVector(inBody2.GetRotation(), mRatioR2xN2); + + inv_effective_mass += Square(inRatio) * mp2->GetInverseMass() + mInvI2_RatioR2xN2.Dot(mRatioR2xN2); + } + + // Calculate inverse effective mass: K = J M^-1 J^T + if (inv_effective_mass == 0.0f) + Deactivate(); + else + mEffectiveMass = 1.0f / inv_effective_mass; + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = 0.0f; + mTotalLambda = 0.0f; + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass != 0.0f; + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inN1 The world space normal in which the constraint operates for body 1 + /// @param inN2 The world space normal in which the constraint operates for body 2 + /// @param inRatio The ratio how forces are applied between bodies + /// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame + inline void WarmStart(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, inRatio, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inN1 The world space normal in which the constraint operates for body 1 + /// @param inN2 The world space normal in which the constraint operates for body 2 + /// @param inRatio The ratio how forces are applied between bodies + /// @param inMinLambda Minimum angular impulse to apply (N m s) + /// @param inMaxLambda Maximum angular impulse to apply (N m s) + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inMinLambda, float inMaxLambda) + { + // Lagrange multiplier is: + // + // lambda = -K^-1 (J v + b) + float lambda = -mEffectiveMass * (inN1.Dot(ioBody1.GetLinearVelocity()) + mR1xN1.Dot(ioBody1.GetAngularVelocity()) + inRatio * inN2.Dot(ioBody2.GetLinearVelocity()) + mRatioR2xN2.Dot(ioBody2.GetAngularVelocity())); + float new_lambda = Clamp(mTotalLambda + lambda, inMinLambda, inMaxLambda); // Clamp impulse + lambda = new_lambda - mTotalLambda; // Lambda potentially got clamped, calculate the new impulse to apply + mTotalLambda = new_lambda; // Store accumulated impulse + + return ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, inRatio, lambda); + } + + /// Return lagrange multiplier + float GetTotalLambda() const + { + return mTotalLambda; + } + + /// Iteratively update the position constraint. Makes sure C(...) == 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inN1 The world space normal in which the constraint operates for body 1 + /// @param inN2 The world space normal in which the constraint operates for body 2 + /// @param inRatio The ratio how forces are applied between bodies + /// @param inC Value of the constraint equation (C) + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inC, float inBaumgarte) const + { + if (inC != 0.0f) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + float lambda = -mEffectiveMass * inBaumgarte * inC; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + { + ioBody1.AddPositionStep((lambda * ioBody1.GetMotionPropertiesUnchecked()->GetInverseMass()) * inN1); + ioBody1.AddRotationStep(lambda * mInvI1_R1xN1); + } + if (ioBody2.IsDynamic()) + { + ioBody2.AddPositionStep((lambda * inRatio * ioBody2.GetMotionPropertiesUnchecked()->GetInverseMass()) * inN2); + ioBody2.AddRotationStep(lambda * mInvI2_RatioR2xN2); + } + return true; + } + + return false; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Vec3 mR1xN1; + Vec3 mInvI1_R1xN1; + Vec3 mRatioR2xN2; + Vec3 mInvI2_RatioR2xN2; + float mEffectiveMass = 0.0f; + float mTotalLambda = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h new file mode 100644 index 000000000000..f9e86b86b2f0 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h @@ -0,0 +1,239 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Constrains movement along 3 axis +/// +/// @see "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.2.1 +/// +/// Constraint equation (eq 45): +/// +/// \f[C = p_2 - p_1\f] +/// +/// Jacobian (transposed) (eq 47): +/// +/// \f[J^T = \begin{bmatrix}-E & r1x & E & -r2x^T\end{bmatrix} +/// = \begin{bmatrix}-E^T \\ r1x^T \\ E^T \\ -r2x^T\end{bmatrix} +/// = \begin{bmatrix}-E \\ -r1x \\ E \\ r2x\end{bmatrix}\f] +/// +/// Used terms (here and below, everything in world space):\n +/// p1, p2 = constraint points.\n +/// r1 = p1 - x1.\n +/// r2 = p2 - x2.\n +/// r1x = 3x3 matrix for which r1x v = r1 x v (cross product).\n +/// x1, x2 = center of mass for the bodies.\n +/// v = [v1, w1, v2, w2].\n +/// v1, v2 = linear velocity of body 1 and 2.\n +/// w1, w2 = angular velocity of body 1 and 2.\n +/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n +/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n +/// b = velocity bias.\n +/// \f$\beta\f$ = baumgarte constant.\n +/// E = identity matrix. +class PointConstraintPart +{ + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != Vec3::sZero()) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + if (ioBody1.IsDynamic()) + { + MotionProperties *mp1 = ioBody1.GetMotionProperties(); + mp1->SubLinearVelocityStep(mp1->GetInverseMass() * inLambda); + mp1->SubAngularVelocityStep(mInvI1_R1X * inLambda); + } + if (ioBody2.IsDynamic()) + { + MotionProperties *mp2 = ioBody2.GetMotionProperties(); + mp2->AddLinearVelocityStep(mp2->GetInverseMass() * inLambda); + mp2->AddAngularVelocityStep(mInvI2_R2X * inLambda); + } + return true; + } + + return false; + } + +public: + /// Calculate properties used during the functions below + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inRotation1 The 3x3 rotation matrix for body 1 (translation part is ignored) + /// @param inRotation2 The 3x3 rotation matrix for body 2 (translation part is ignored) + /// @param inR1 Local space vector from center of mass to constraint point for body 1 + /// @param inR2 Local space vector from center of mass to constraint point for body 2 + inline void CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, Vec3Arg inR1, const Body &inBody2, Mat44Arg inRotation2, Vec3Arg inR2) + { + // Positions where the point constraint acts on (middle point between center of masses) in world space + mR1 = inRotation1.Multiply3x3(inR1); + mR2 = inRotation2.Multiply3x3(inR2); + + // Calculate effective mass: K^-1 = (J M^-1 J^T)^-1 + // Using: I^-1 = R * Ibody^-1 * R^T + float summed_inv_mass; + Mat44 inv_effective_mass; + if (inBody1.IsDynamic()) + { + const MotionProperties *mp1 = inBody1.GetMotionProperties(); + Mat44 inv_i1 = mp1->GetInverseInertiaForRotation(inRotation1); + summed_inv_mass = mp1->GetInverseMass(); + + Mat44 r1x = Mat44::sCrossProduct(mR1); + mInvI1_R1X = inv_i1.Multiply3x3(r1x); + inv_effective_mass = r1x.Multiply3x3(inv_i1).Multiply3x3RightTransposed(r1x); + } + else + { + JPH_IF_DEBUG(mInvI1_R1X = Mat44::sNaN();) + + summed_inv_mass = 0.0f; + inv_effective_mass = Mat44::sZero(); + } + + if (inBody2.IsDynamic()) + { + const MotionProperties *mp2 = inBody2.GetMotionProperties(); + Mat44 inv_i2 = mp2->GetInverseInertiaForRotation(inRotation2); + summed_inv_mass += mp2->GetInverseMass(); + + Mat44 r2x = Mat44::sCrossProduct(mR2); + mInvI2_R2X = inv_i2.Multiply3x3(r2x); + inv_effective_mass += r2x.Multiply3x3(inv_i2).Multiply3x3RightTransposed(r2x); + } + else + { + JPH_IF_DEBUG(mInvI2_R2X = Mat44::sNaN();) + } + + inv_effective_mass += Mat44::sScale(summed_inv_mass); + if (!mEffectiveMass.SetInversed3x3(inv_effective_mass)) + Deactivate(); + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = Mat44::sZero(); + mTotalLambda = Vec3::sZero(); + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass(3, 3) != 0.0f; + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2) + { + // Calculate lagrange multiplier: + // + // lambda = -K^-1 (J v + b) + Vec3 lambda = mEffectiveMass * (ioBody1.GetLinearVelocity() - mR1.Cross(ioBody1.GetAngularVelocity()) - ioBody2.GetLinearVelocity() + mR2.Cross(ioBody2.GetAngularVelocity())); + mTotalLambda += lambda; // Store accumulated lambda + return ApplyVelocityStep(ioBody1, ioBody2, lambda); + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inBaumgarte) const + { + Vec3 separation = (Vec3(ioBody2.GetCenterOfMassPosition() - ioBody1.GetCenterOfMassPosition()) + mR2 - mR1); + if (separation != Vec3::sZero()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + Vec3 lambda = mEffectiveMass * -inBaumgarte * separation; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + { + ioBody1.SubPositionStep(ioBody1.GetMotionProperties()->GetInverseMass() * lambda); + ioBody1.SubRotationStep(mInvI1_R1X * lambda); + } + if (ioBody2.IsDynamic()) + { + ioBody2.AddPositionStep(ioBody2.GetMotionProperties()->GetInverseMass() * lambda); + ioBody2.AddRotationStep(mInvI2_R2X * lambda); + } + + return true; + } + + return false; + } + + /// Return lagrange multiplier + Vec3 GetTotalLambda() const + { + return mTotalLambda; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Vec3 mR1; + Vec3 mR2; + Mat44 mInvI1_R1X; + Mat44 mInvI2_R2X; + Mat44 mEffectiveMass; + Vec3 mTotalLambda { Vec3::sZero() }; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RackAndPinionConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RackAndPinionConstraintPart.h new file mode 100644 index 000000000000..641f4867a115 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RackAndPinionConstraintPart.h @@ -0,0 +1,196 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Constraint that constrains a rotation to a translation +/// +/// Constraint equation: +/// +/// C = Theta(t) - r d(t) +/// +/// Derivative: +/// +/// d/dt C = 0 +/// <=> w1 . a - r v2 . b = 0 +/// +/// Jacobian: +/// +/// \f[J = \begin{bmatrix}0 & a^T & -r b^T & 0\end{bmatrix}\f] +/// +/// Used terms (here and below, everything in world space):\n +/// a = axis around which body 1 rotates (normalized).\n +/// b = axis along which body 2 slides (normalized).\n +/// Theta(t) = rotation around a of body 1.\n +/// d(t) = distance body 2 slides.\n +/// r = ratio between rotation and translation.\n +/// v = [v1, w1, v2, w2].\n +/// v1, v2 = linear velocity of body 1 and 2.\n +/// w1, w2 = angular velocity of body 1 and 2.\n +/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n +/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n +/// \f$\beta\f$ = baumgarte constant. +class RackAndPinionConstraintPart +{ + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, float inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != 0.0f) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + ioBody1.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI1_A); + ioBody2.GetMotionProperties()->SubLinearVelocityStep(inLambda * mRatio_InvM2_B); + return true; + } + + return false; + } + +public: + /// Calculate properties used during the functions below + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inWorldSpaceHingeAxis The axis around which body 1 rotates + /// @param inWorldSpaceSliderAxis The axis along which body 2 slides + /// @param inRatio The ratio between rotation and translation + inline void CalculateConstraintProperties(const Body &inBody1, Vec3Arg inWorldSpaceHingeAxis, const Body &inBody2, Vec3Arg inWorldSpaceSliderAxis, float inRatio) + { + JPH_ASSERT(inWorldSpaceHingeAxis.IsNormalized(1.0e-4f)); + JPH_ASSERT(inWorldSpaceSliderAxis.IsNormalized(1.0e-4f)); + + // Calculate: I1^-1 a + mInvI1_A = inBody1.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), inWorldSpaceHingeAxis); + + // Calculate: r/m2 b + float inv_m2 = inBody2.GetMotionProperties()->GetInverseMass(); + mRatio_InvM2_B = inRatio * inv_m2 * inWorldSpaceSliderAxis; + + // K^-1 = 1 / (J M^-1 J^T) = 1 / (a^T I1^-1 a + 1/m2 * r^2 * b . b) + float inv_effective_mass = (inWorldSpaceHingeAxis.Dot(mInvI1_A) + inv_m2 * Square(inRatio)); + if (inv_effective_mass == 0.0f) + Deactivate(); + else + mEffectiveMass = 1.0f / inv_effective_mass; + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = 0.0f; + mTotalLambda = 0.0f; + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass != 0.0f; + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWorldSpaceHingeAxis The axis around which body 1 rotates + /// @param inWorldSpaceSliderAxis The axis along which body 2 slides + /// @param inRatio The ratio between rotation and translation + inline bool SolveVelocityConstraint(Body &ioBody1, Vec3Arg inWorldSpaceHingeAxis, Body &ioBody2, Vec3Arg inWorldSpaceSliderAxis, float inRatio) + { + // Lagrange multiplier is: + // + // lambda = -K^-1 (J v + b) + float lambda = mEffectiveMass * (inRatio * inWorldSpaceSliderAxis.Dot(ioBody2.GetLinearVelocity()) - inWorldSpaceHingeAxis.Dot(ioBody1.GetAngularVelocity())); + mTotalLambda += lambda; // Store accumulated impulse + + return ApplyVelocityStep(ioBody1, ioBody2, lambda); + } + + /// Return lagrange multiplier + float GetTotalLambda() const + { + return mTotalLambda; + } + + /// Iteratively update the position constraint. Makes sure C(...) == 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inC Value of the constraint equation (C) + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inC, float inBaumgarte) const + { + // Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint + if (inC != 0.0f) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + float lambda = -mEffectiveMass * inBaumgarte * inC; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + ioBody1.AddRotationStep(lambda * mInvI1_A); + if (ioBody2.IsDynamic()) + ioBody2.SubPositionStep(lambda * mRatio_InvM2_B); + return true; + } + + return false; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Vec3 mInvI1_A; + Vec3 mRatio_InvM2_B; + float mEffectiveMass = 0.0f; + float mTotalLambda = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h new file mode 100644 index 000000000000..ec84776a46f0 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h @@ -0,0 +1,270 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Constrains rotation around all axis so that only translation is allowed +/// +/// Based on: "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.5.1 +/// +/// Constraint equation (eq 129): +/// +/// \f[C = \begin{bmatrix}\Delta\theta_x, \Delta\theta_y, \Delta\theta_z\end{bmatrix}\f] +/// +/// Jacobian (eq 131): +/// +/// \f[J = \begin{bmatrix}0 & -E & 0 & E\end{bmatrix}\f] +/// +/// Used terms (here and below, everything in world space):\n +/// delta_theta_* = difference in rotation between initial rotation of bodies 1 and 2.\n +/// x1, x2 = center of mass for the bodies.\n +/// v = [v1, w1, v2, w2].\n +/// v1, v2 = linear velocity of body 1 and 2.\n +/// w1, w2 = angular velocity of body 1 and 2.\n +/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n +/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n +/// b = velocity bias.\n +/// \f$\beta\f$ = baumgarte constant.\n +/// E = identity matrix.\n +class RotationEulerConstraintPart +{ +private: + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != Vec3::sZero()) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + if (ioBody1.IsDynamic()) + ioBody1.GetMotionProperties()->SubAngularVelocityStep(mInvI1.Multiply3x3(inLambda)); + if (ioBody2.IsDynamic()) + ioBody2.GetMotionProperties()->AddAngularVelocityStep(mInvI2.Multiply3x3(inLambda)); + return true; + } + + return false; + } + +public: + /// Return inverse of initial rotation from body 1 to body 2 in body 1 space + static Quat sGetInvInitialOrientation(const Body &inBody1, const Body &inBody2) + { + // q20 = q10 r0 + // <=> r0 = q10^-1 q20 + // <=> r0^-1 = q20^-1 q10 + // + // where: + // + // q20 = initial orientation of body 2 + // q10 = initial orientation of body 1 + // r0 = initial rotation from body 1 to body 2 + return inBody2.GetRotation().Conjugated() * inBody1.GetRotation(); + } + + /// @brief Return inverse of initial rotation from body 1 to body 2 in body 1 space + /// @param inAxisX1 Reference axis X for body 1 + /// @param inAxisY1 Reference axis Y for body 1 + /// @param inAxisX2 Reference axis X for body 2 + /// @param inAxisY2 Reference axis Y for body 2 + static Quat sGetInvInitialOrientationXY(Vec3Arg inAxisX1, Vec3Arg inAxisY1, Vec3Arg inAxisX2, Vec3Arg inAxisY2) + { + // Store inverse of initial rotation from body 1 to body 2 in body 1 space: + // + // q20 = q10 r0 + // <=> r0 = q10^-1 q20 + // <=> r0^-1 = q20^-1 q10 + // + // where: + // + // q10, q20 = world space initial orientation of body 1 and 2 + // r0 = initial rotation from body 1 to body 2 in local space of body 1 + // + // We can also write this in terms of the constraint matrices: + // + // q20 c2 = q10 c1 + // <=> q20 = q10 c1 c2^-1 + // => r0 = c1 c2^-1 + // <=> r0^-1 = c2 c1^-1 + // + // where: + // + // c1, c2 = matrix that takes us from body 1 and 2 COM to constraint space 1 and 2 + if (inAxisX1 == inAxisX2 && inAxisY1 == inAxisY2) + { + // Axis are the same -> identity transform + return Quat::sIdentity(); + } + else + { + Mat44 constraint1(Vec4(inAxisX1, 0), Vec4(inAxisY1, 0), Vec4(inAxisX1.Cross(inAxisY1), 0), Vec4(0, 0, 0, 1)); + Mat44 constraint2(Vec4(inAxisX2, 0), Vec4(inAxisY2, 0), Vec4(inAxisX2.Cross(inAxisY2), 0), Vec4(0, 0, 0, 1)); + return constraint2.GetQuaternion() * constraint1.GetQuaternion().Conjugated(); + } + } + + /// @brief Return inverse of initial rotation from body 1 to body 2 in body 1 space + /// @param inAxisX1 Reference axis X for body 1 + /// @param inAxisZ1 Reference axis Z for body 1 + /// @param inAxisX2 Reference axis X for body 2 + /// @param inAxisZ2 Reference axis Z for body 2 + static Quat sGetInvInitialOrientationXZ(Vec3Arg inAxisX1, Vec3Arg inAxisZ1, Vec3Arg inAxisX2, Vec3Arg inAxisZ2) + { + // See comment at sGetInvInitialOrientationXY + if (inAxisX1 == inAxisX2 && inAxisZ1 == inAxisZ2) + { + return Quat::sIdentity(); + } + else + { + Mat44 constraint1(Vec4(inAxisX1, 0), Vec4(inAxisZ1.Cross(inAxisX1), 0), Vec4(inAxisZ1, 0), Vec4(0, 0, 0, 1)); + Mat44 constraint2(Vec4(inAxisX2, 0), Vec4(inAxisZ2.Cross(inAxisX2), 0), Vec4(inAxisZ2, 0), Vec4(0, 0, 0, 1)); + return constraint2.GetQuaternion() * constraint1.GetQuaternion().Conjugated(); + } + } + + /// Calculate properties used during the functions below + inline void CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, const Body &inBody2, Mat44Arg inRotation2) + { + // Calculate properties used during constraint solving + mInvI1 = inBody1.IsDynamic()? inBody1.GetMotionProperties()->GetInverseInertiaForRotation(inRotation1) : Mat44::sZero(); + mInvI2 = inBody2.IsDynamic()? inBody2.GetMotionProperties()->GetInverseInertiaForRotation(inRotation2) : Mat44::sZero(); + + // Calculate effective mass: K^-1 = (J M^-1 J^T)^-1 + if (!mEffectiveMass.SetInversed3x3(mInvI1 + mInvI2)) + Deactivate(); + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = Mat44::sZero(); + mTotalLambda = Vec3::sZero(); + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass(3, 3) != 0.0f; + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2) + { + // Calculate lagrange multiplier: + // + // lambda = -K^-1 (J v + b) + Vec3 lambda = mEffectiveMass.Multiply3x3(ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity()); + mTotalLambda += lambda; + return ApplyVelocityStep(ioBody1, ioBody2, lambda); + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, QuatArg inInvInitialOrientation, float inBaumgarte) const + { + // Calculate difference in rotation + // + // The rotation should be: + // + // q2 = q1 r0 + // + // But because of drift the actual rotation is + // + // q2 = diff q1 r0 + // <=> diff = q2 r0^-1 q1^-1 + // + // Where: + // q1 = current rotation of body 1 + // q2 = current rotation of body 2 + // diff = error that needs to be reduced to zero + Quat diff = ioBody2.GetRotation() * inInvInitialOrientation * ioBody1.GetRotation().Conjugated(); + + // A quaternion can be seen as: + // + // q = [sin(theta / 2) * v, cos(theta/2)] + // + // Where: + // v = rotation vector + // theta = rotation angle + // + // If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is: + Vec3 error = 2.0f * diff.EnsureWPositive().GetXYZ(); + if (error != Vec3::sZero()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + Vec3 lambda = -inBaumgarte * mEffectiveMass * error; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + ioBody1.SubRotationStep(mInvI1.Multiply3x3(lambda)); + if (ioBody2.IsDynamic()) + ioBody2.AddRotationStep(mInvI2.Multiply3x3(lambda)); + return true; + } + + return false; + } + + /// Return lagrange multiplier + Vec3 GetTotalLambda() const + { + return mTotalLambda; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Mat44 mInvI1; + Mat44 mInvI2; + Mat44 mEffectiveMass; + Vec3 mTotalLambda { Vec3::sZero() }; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationQuatConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationQuatConstraintPart.h new file mode 100644 index 000000000000..a4675a51b9f1 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationQuatConstraintPart.h @@ -0,0 +1,246 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Quaternion based constraint that constrains rotation around all axis so that only translation is allowed. +/// +/// NOTE: This constraint part is more expensive than the RotationEulerConstraintPart and slightly more correct since +/// RotationEulerConstraintPart::SolvePositionConstraint contains an approximation. In practice the difference +/// is small, so the RotationEulerConstraintPart is probably the better choice. +/// +/// Rotation is fixed between bodies like this: +/// +/// q2 = q1 r0 +/// +/// Where: +/// q1, q2 = world space quaternions representing rotation of body 1 and 2. +/// r0 = initial rotation between bodies in local space of body 1, this can be calculated by: +/// +/// q20 = q10 r0 +/// <=> r0 = q10^* q20 +/// +/// Where: +/// q10, q20 = initial world space rotations of body 1 and 2. +/// q10^* = conjugate of quaternion q10 (which is the same as the inverse for a unit quaternion) +/// +/// We exclusively use the conjugate below: +/// +/// r0^* = q20^* q10 +/// +/// The error in the rotation is (in local space of body 1): +/// +/// q2 = q1 error r0 +/// <=> error = q1^* q2 r0^* +/// +/// The imaginary part of the quaternion represents the rotation axis * sin(angle / 2). The real part of the quaternion +/// does not add any additional information (we know the quaternion in normalized) and we're removing 3 degrees of freedom +/// so we want 3 parameters. Therefore we define the constraint equation like: +/// +/// C = A q1^* q2 r0^* = 0 +/// +/// Where (if you write a quaternion as [real-part, i-part, j-part, k-part]): +/// +/// [0, 1, 0, 0] +/// A = [0, 0, 1, 0] +/// [0, 0, 0, 1] +/// +/// or in our case since we store a quaternion like [i-part, j-part, k-part, real-part]: +/// +/// [1, 0, 0, 0] +/// A = [0, 1, 0, 0] +/// [0, 0, 1, 0] +/// +/// Time derivative: +/// +/// d/dt C = A (q1^* d/dt(q2) + d/dt(q1^*) q2) r0^* +/// = A (q1^* (1/2 W2 q2) + (1/2 W1 q1)^* q2) r0^* +/// = 1/2 A (q1^* W2 q2 + q1^* W1^* q2) r0^* +/// = 1/2 A (q1^* W2 q2 - q1^* W1 * q2) r0^* +/// = 1/2 A ML(q1^*) MR(q2 r0^*) (W2 - W1) +/// = 1/2 A ML(q1^*) MR(q2 r0^*) A^T (w2 - w1) +/// +/// Where: +/// W1 = [0, w1], W2 = [0, w2] (converting angular velocity to imaginary part of quaternion). +/// w1, w2 = angular velocity of body 1 and 2. +/// d/dt(q) = 1/2 W q (time derivative of a quaternion). +/// W^* = -W (conjugate negates angular velocity as quaternion). +/// ML(q): 4x4 matrix so that q * p = ML(q) * p, where q and p are quaternions. +/// MR(p): 4x4 matrix so that q * p = MR(p) * q, where q and p are quaternions. +/// A^T: Transpose of A. +/// +/// Jacobian: +/// +/// J = [0, -1/2 A ML(q1^*) MR(q2 r0^*) A^T, 0, 1/2 A ML(q1^*) MR(q2 r0^*) A^T] +/// = [0, -JP, 0, JP] +/// +/// Suggested reading: +/// - 3D Constraint Derivations for Impulse Solvers - Marijn Tamis +/// - Game Physics Pearls - Section 9 - Quaternion Based Constraints - Claude Lacoursiere +class RotationQuatConstraintPart +{ +private: + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != Vec3::sZero()) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + if (ioBody1.IsDynamic()) + ioBody1.GetMotionProperties()->SubAngularVelocityStep(mInvI1_JPT.Multiply3x3(inLambda)); + if (ioBody2.IsDynamic()) + ioBody2.GetMotionProperties()->AddAngularVelocityStep(mInvI2_JPT.Multiply3x3(inLambda)); + return true; + } + + return false; + } + +public: + /// Return inverse of initial rotation from body 1 to body 2 in body 1 space + static Quat sGetInvInitialOrientation(const Body &inBody1, const Body &inBody2) + { + // q20 = q10 r0 + // <=> r0 = q10^-1 q20 + // <=> r0^-1 = q20^-1 q10 + // + // where: + // + // q20 = initial orientation of body 2 + // q10 = initial orientation of body 1 + // r0 = initial rotation from body 1 to body 2 + return inBody2.GetRotation().Conjugated() * inBody1.GetRotation(); + } + + /// Calculate properties used during the functions below + inline void CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, const Body &inBody2, Mat44Arg inRotation2, QuatArg inInvInitialOrientation) + { + // Calculate: JP = 1/2 A ML(q1^*) MR(q2 r0^*) A^T + Mat44 jp = (Mat44::sQuatLeftMultiply(0.5f * inBody1.GetRotation().Conjugated()) * Mat44::sQuatRightMultiply(inBody2.GetRotation() * inInvInitialOrientation)).GetRotationSafe(); + + // Calculate properties used during constraint solving + Mat44 inv_i1 = inBody1.IsDynamic()? inBody1.GetMotionProperties()->GetInverseInertiaForRotation(inRotation1) : Mat44::sZero(); + Mat44 inv_i2 = inBody2.IsDynamic()? inBody2.GetMotionProperties()->GetInverseInertiaForRotation(inRotation2) : Mat44::sZero(); + mInvI1_JPT = inv_i1.Multiply3x3RightTransposed(jp); + mInvI2_JPT = inv_i2.Multiply3x3RightTransposed(jp); + + // Calculate effective mass: K^-1 = (J M^-1 J^T)^-1 + // = (JP * I1^-1 * JP^T + JP * I2^-1 * JP^T)^-1 + // = (JP * (I1^-1 + I2^-1) * JP^T)^-1 + if (!mEffectiveMass.SetInversed3x3(jp.Multiply3x3(inv_i1 + inv_i2).Multiply3x3RightTransposed(jp))) + Deactivate(); + else + mEffectiveMass_JP = mEffectiveMass.Multiply3x3(jp); + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = Mat44::sZero(); + mEffectiveMass_JP = Mat44::sZero(); + mTotalLambda = Vec3::sZero(); + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass(3, 3) != 0.0f; + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2) + { + // Calculate lagrange multiplier: + // + // lambda = -K^-1 (J v + b) + Vec3 lambda = mEffectiveMass_JP.Multiply3x3(ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity()); + mTotalLambda += lambda; + return ApplyVelocityStep(ioBody1, ioBody2, lambda); + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, QuatArg inInvInitialOrientation, float inBaumgarte) const + { + // Calculate constraint equation + Vec3 c = (ioBody1.GetRotation().Conjugated() * ioBody2.GetRotation() * inInvInitialOrientation).GetXYZ(); + if (c != Vec3::sZero()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + Vec3 lambda = -inBaumgarte * mEffectiveMass * c; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + ioBody1.SubRotationStep(mInvI1_JPT.Multiply3x3(lambda)); + if (ioBody2.IsDynamic()) + ioBody2.AddRotationStep(mInvI2_JPT.Multiply3x3(lambda)); + return true; + } + + return false; + } + + /// Return lagrange multiplier + Vec3 GetTotalLambda() const + { + return mTotalLambda; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Mat44 mInvI1_JPT; + Mat44 mInvI2_JPT; + Mat44 mEffectiveMass; + Mat44 mEffectiveMass_JP; + Vec3 mTotalLambda { Vec3::sZero() }; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/SpringPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/SpringPart.h new file mode 100644 index 000000000000..0a8a4a9730c8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/SpringPart.h @@ -0,0 +1,169 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN +#ifndef JPH_PLATFORM_DOXYGEN // Somehow Doxygen gets confused and thinks the parameters to CalculateSpringProperties belong to this macro +JPH_MSVC_SUPPRESS_WARNING(4723) // potential divide by 0 - caused by line: outEffectiveMass = 1.0f / inInvEffectiveMass, note that JPH_NAMESPACE_BEGIN already pushes the warning state +#endif // !JPH_PLATFORM_DOXYGEN + +/// Class used in other constraint parts to calculate the required bias factor in the lagrange multiplier for creating springs +class SpringPart +{ +private: + JPH_INLINE void CalculateSpringPropertiesHelper(float inDeltaTime, float inInvEffectiveMass, float inBias, float inC, float inStiffness, float inDamping, float &outEffectiveMass) + { + // Soft constraints as per: Soft Constraints: Reinventing The Spring - Erin Catto - GDC 2011 + + // Note that the calculation of beta and gamma below are based on the solution of an implicit Euler integration scheme + // This scheme is unconditionally stable but has built in damping, so even when you set the damping ratio to 0 there will still + // be damping. See page 16 and 32. + + // Calculate softness (gamma in the slides) + // See page 34 and note that the gamma needs to be divided by delta time since we're working with impulses rather than forces: + // softness = 1 / (dt * (c + dt * k)) + // Note that the spring stiffness is k and the spring damping is c + mSoftness = 1.0f / (inDeltaTime * (inDamping + inDeltaTime * inStiffness)); + + // Calculate bias factor (baumgarte stabilization): + // beta = dt * k / (c + dt * k) = dt * k^2 * softness + // b = beta / dt * C = dt * k * softness * C + mBias = inBias + inDeltaTime * inStiffness * mSoftness * inC; + + // Update the effective mass, see post by Erin Catto: http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=1354 + // + // Newton's Law: + // M * (v2 - v1) = J^T * lambda + // + // Velocity constraint with softness and Baumgarte: + // J * v2 + softness * lambda + b = 0 + // + // where b = beta * C / dt + // + // We know everything except v2 and lambda. + // + // First solve Newton's law for v2 in terms of lambda: + // + // v2 = v1 + M^-1 * J^T * lambda + // + // Substitute this expression into the velocity constraint: + // + // J * (v1 + M^-1 * J^T * lambda) + softness * lambda + b = 0 + // + // Now collect coefficients of lambda: + // + // (J * M^-1 * J^T + softness) * lambda = - J * v1 - b + // + // Now we define: + // + // K = J * M^-1 * J^T + softness + // + // So our new effective mass is K^-1 + outEffectiveMass = 1.0f / (inInvEffectiveMass + mSoftness); + } + +public: + /// Turn off the spring and set a bias only + /// + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + inline void CalculateSpringPropertiesWithBias(float inBias) + { + mSoftness = 0.0f; + mBias = inBias; + } + + /// Calculate spring properties based on frequency and damping ratio + /// + /// @param inDeltaTime Time step + /// @param inInvEffectiveMass Inverse effective mass K + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + /// @param inC Value of the constraint equation (C). Set to zero if you don't want to drive the constraint to zero with a spring. + /// @param inFrequency Oscillation frequency (Hz). Set to zero if you don't want to drive the constraint to zero with a spring. + /// @param inDamping Damping factor (0 = no damping, 1 = critical damping). Set to zero if you don't want to drive the constraint to zero with a spring. + /// @param outEffectiveMass On return, this contains the new effective mass K^-1 + inline void CalculateSpringPropertiesWithFrequencyAndDamping(float inDeltaTime, float inInvEffectiveMass, float inBias, float inC, float inFrequency, float inDamping, float &outEffectiveMass) + { + outEffectiveMass = 1.0f / inInvEffectiveMass; + + if (inFrequency > 0.0f) + { + // Calculate angular frequency + float omega = 2.0f * JPH_PI * inFrequency; + + // Calculate spring stiffness k and damping constant c (page 45) + float k = outEffectiveMass * Square(omega); + float c = 2.0f * outEffectiveMass * inDamping * omega; + + CalculateSpringPropertiesHelper(inDeltaTime, inInvEffectiveMass, inBias, inC, k, c, outEffectiveMass); + } + else + { + CalculateSpringPropertiesWithBias(inBias); + } + } + + /// Calculate spring properties with spring Stiffness (k) and damping (c), this is based on the spring equation: F = -k * x - c * v + /// + /// @param inDeltaTime Time step + /// @param inInvEffectiveMass Inverse effective mass K + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + /// @param inC Value of the constraint equation (C). Set to zero if you don't want to drive the constraint to zero with a spring. + /// @param inStiffness Spring stiffness k. Set to zero if you don't want to drive the constraint to zero with a spring. + /// @param inDamping Spring damping coefficient c. Set to zero if you don't want to drive the constraint to zero with a spring. + /// @param outEffectiveMass On return, this contains the new effective mass K^-1 + inline void CalculateSpringPropertiesWithStiffnessAndDamping(float inDeltaTime, float inInvEffectiveMass, float inBias, float inC, float inStiffness, float inDamping, float &outEffectiveMass) + { + if (inStiffness > 0.0f) + { + CalculateSpringPropertiesHelper(inDeltaTime, inInvEffectiveMass, inBias, inC, inStiffness, inDamping, outEffectiveMass); + } + else + { + outEffectiveMass = 1.0f / inInvEffectiveMass; + + CalculateSpringPropertiesWithBias(inBias); + } + } + + /// Returns if this spring is active + inline bool IsActive() const + { + return mSoftness != 0.0f; + } + + /// Get total bias b, including supplied bias and bias for spring: lambda = J v + b + inline float GetBias(float inTotalLambda) const + { + // Remainder of post by Erin Catto: http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=1354 + // + // Each iteration we are not computing the whole impulse, we are computing an increment to the impulse and we are updating the velocity. + // Also, as we solve each constraint we get a perfect v2, but then some other constraint will come along and mess it up. + // So we want to patch up the constraint while acknowledging the accumulated impulse and the damaged velocity. + // To help with that we use P for the accumulated impulse and lambda as the update. Mathematically we have: + // + // M * (v2new - v2damaged) = J^T * lambda + // J * v2new + softness * (total_lambda + lambda) + b = 0 + // + // If we solve this we get: + // + // v2new = v2damaged + M^-1 * J^T * lambda + // J * (v2damaged + M^-1 * J^T * lambda) + softness * total_lambda + softness * lambda + b = 0 + // + // (J * M^-1 * J^T + softness) * lambda = -(J * v2damaged + softness * total_lambda + b) + // + // So our lagrange multiplier becomes: + // + // lambda = -K^-1 (J v + softness * total_lambda + b) + // + // So we return the bias: softness * total_lambda + b + return mSoftness * inTotalLambda + mBias; + } + +private: + float mBias = 0.0f; + float mSoftness = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h new file mode 100644 index 000000000000..c2a47547f5e7 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h @@ -0,0 +1,597 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// How the swing limit behaves +enum class ESwingType : uint8 +{ + Cone, ///< Swing is limited by a cone shape, note that this cone starts to deform for larger swing angles. Cone limits only support limits that are symmetric around 0. + Pyramid, ///< Swing is limited by a pyramid shape, note that this pyramid starts to deform for larger swing angles. +}; + +/// Quaternion based constraint that decomposes the rotation in constraint space in swing and twist: q = q_swing * q_twist +/// where q_swing.x = 0 and where q_twist.y = q_twist.z = 0 +/// +/// - Rotation around the twist (x-axis) is within [inTwistMinAngle, inTwistMaxAngle]. +/// - Rotation around the swing axis (y and z axis) are limited to an ellipsoid in quaternion space formed by the equation: +/// +/// (q_swing.y / sin(inSwingYHalfAngle / 2))^2 + (q_swing.z / sin(inSwingZHalfAngle / 2))^2 <= 1 +/// +/// Which roughly corresponds to an elliptic cone shape with major axis (inSwingYHalfAngle, inSwingZHalfAngle). +/// +/// In case inSwingYHalfAngle = 0, the rotation around Y will be constrained to 0 and the rotation around Z +/// will be constrained between [-inSwingZHalfAngle, inSwingZHalfAngle]. Vice versa if inSwingZHalfAngle = 0. +class SwingTwistConstraintPart +{ +public: + /// Override the swing type + void SetSwingType(ESwingType inSwingType) + { + mSwingType = inSwingType; + } + + /// Get the swing type for this part + ESwingType GetSwingType() const + { + return mSwingType; + } + + /// Set limits for this constraint (see description above for parameters) + void SetLimits(float inTwistMinAngle, float inTwistMaxAngle, float inSwingYMinAngle, float inSwingYMaxAngle, float inSwingZMinAngle, float inSwingZMaxAngle) + { + constexpr float cLockedAngle = DegreesToRadians(0.5f); + constexpr float cFreeAngle = DegreesToRadians(179.5f); + + // Assume sane input + JPH_ASSERT(inTwistMinAngle <= inTwistMaxAngle); + JPH_ASSERT(inSwingYMinAngle <= inSwingYMaxAngle); + JPH_ASSERT(inSwingZMinAngle <= inSwingZMaxAngle); + JPH_ASSERT(inSwingYMinAngle >= -JPH_PI && inSwingYMaxAngle <= JPH_PI); + JPH_ASSERT(inSwingZMinAngle >= -JPH_PI && inSwingZMaxAngle <= JPH_PI); + + // Calculate the sine and cosine of the half angles + Vec4 half_twist = 0.5f * Vec4(inTwistMinAngle, inTwistMaxAngle, 0, 0); + Vec4 twist_s, twist_c; + half_twist.SinCos(twist_s, twist_c); + Vec4 half_swing = 0.5f * Vec4(inSwingYMinAngle, inSwingYMaxAngle, inSwingZMinAngle, inSwingZMaxAngle); + Vec4 swing_s, swing_c; + half_swing.SinCos(swing_s, swing_c); + + // Store half angles for pyramid limit + mSwingYHalfMinAngle = half_swing.GetX(); + mSwingYHalfMaxAngle = half_swing.GetY(); + mSwingZHalfMinAngle = half_swing.GetZ(); + mSwingZHalfMaxAngle = half_swing.GetW(); + + // Store axis flags which are used at runtime to quickly decided which constraints to apply + mRotationFlags = 0; + if (inTwistMinAngle > -cLockedAngle && inTwistMaxAngle < cLockedAngle) + { + mRotationFlags |= TwistXLocked; + mSinTwistHalfMinAngle = 0.0f; + mSinTwistHalfMaxAngle = 0.0f; + mCosTwistHalfMinAngle = 1.0f; + mCosTwistHalfMaxAngle = 1.0f; + } + else if (inTwistMinAngle < -cFreeAngle && inTwistMaxAngle > cFreeAngle) + { + mRotationFlags |= TwistXFree; + mSinTwistHalfMinAngle = -1.0f; + mSinTwistHalfMaxAngle = 1.0f; + mCosTwistHalfMinAngle = 0.0f; + mCosTwistHalfMaxAngle = 0.0f; + } + else + { + mSinTwistHalfMinAngle = twist_s.GetX(); + mSinTwistHalfMaxAngle = twist_s.GetY(); + mCosTwistHalfMinAngle = twist_c.GetX(); + mCosTwistHalfMaxAngle = twist_c.GetY(); + } + + if (inSwingYMinAngle > -cLockedAngle && inSwingYMaxAngle < cLockedAngle) + { + mRotationFlags |= SwingYLocked; + mSinSwingYHalfMinAngle = 0.0f; + mSinSwingYHalfMaxAngle = 0.0f; + mCosSwingYHalfMinAngle = 1.0f; + mCosSwingYHalfMaxAngle = 1.0f; + } + else if (inSwingYMinAngle < -cFreeAngle && inSwingYMaxAngle > cFreeAngle) + { + mRotationFlags |= SwingYFree; + mSinSwingYHalfMinAngle = -1.0f; + mSinSwingYHalfMaxAngle = 1.0f; + mCosSwingYHalfMinAngle = 0.0f; + mCosSwingYHalfMaxAngle = 0.0f; + } + else + { + mSinSwingYHalfMinAngle = swing_s.GetX(); + mSinSwingYHalfMaxAngle = swing_s.GetY(); + mCosSwingYHalfMinAngle = swing_c.GetX(); + mCosSwingYHalfMaxAngle = swing_c.GetY(); + JPH_ASSERT(mSinSwingYHalfMinAngle <= mSinSwingYHalfMaxAngle); + } + + if (inSwingZMinAngle > -cLockedAngle && inSwingZMaxAngle < cLockedAngle) + { + mRotationFlags |= SwingZLocked; + mSinSwingZHalfMinAngle = 0.0f; + mSinSwingZHalfMaxAngle = 0.0f; + mCosSwingZHalfMinAngle = 1.0f; + mCosSwingZHalfMaxAngle = 1.0f; + } + else if (inSwingZMinAngle < -cFreeAngle && inSwingZMaxAngle > cFreeAngle) + { + mRotationFlags |= SwingZFree; + mSinSwingZHalfMinAngle = -1.0f; + mSinSwingZHalfMaxAngle = 1.0f; + mCosSwingZHalfMinAngle = 0.0f; + mCosSwingZHalfMaxAngle = 0.0f; + } + else + { + mSinSwingZHalfMinAngle = swing_s.GetZ(); + mSinSwingZHalfMaxAngle = swing_s.GetW(); + mCosSwingZHalfMinAngle = swing_c.GetZ(); + mCosSwingZHalfMaxAngle = swing_c.GetW(); + JPH_ASSERT(mSinSwingZHalfMinAngle <= mSinSwingZHalfMaxAngle); + } + } + + /// Flags to indicate which axis got clamped by ClampSwingTwist + static constexpr uint cClampedTwistMin = 1 << 0; + static constexpr uint cClampedTwistMax = 1 << 1; + static constexpr uint cClampedSwingYMin = 1 << 2; + static constexpr uint cClampedSwingYMax = 1 << 3; + static constexpr uint cClampedSwingZMin = 1 << 4; + static constexpr uint cClampedSwingZMax = 1 << 5; + + /// Helper function to determine if we're clamped against the min or max limit + static JPH_INLINE bool sDistanceToMinShorter(float inDeltaMin, float inDeltaMax) + { + // We're outside of the limits, get actual delta to min/max range + // Note that a swing/twist of -1 and 1 represent the same angle, so if the difference is bigger than 1, the shortest angle is the other way around (2 - difference) + // We should actually be working with angles rather than sin(angle / 2). When the difference is small the approximation is accurate, but + // when working with extreme values the calculation is off and e.g. when the limit is between 0 and 180 a value of approx -60 will clamp + // to 180 rather than 0 (you'd expect anything > -90 to go to 0). + inDeltaMin = abs(inDeltaMin); + if (inDeltaMin > 1.0f) inDeltaMin = 2.0f - inDeltaMin; + inDeltaMax = abs(inDeltaMax); + if (inDeltaMax > 1.0f) inDeltaMax = 2.0f - inDeltaMax; + return inDeltaMin < inDeltaMax; + } + + /// Clamp twist and swing against the constraint limits, returns which parts were clamped (everything assumed in constraint space) + inline void ClampSwingTwist(Quat &ioSwing, Quat &ioTwist, uint &outClampedAxis) const + { + // Start with not clamped + outClampedAxis = 0; + + // Check that swing and twist quaternions don't contain rotations around the wrong axis + JPH_ASSERT(ioSwing.GetX() == 0.0f); + JPH_ASSERT(ioTwist.GetY() == 0.0f); + JPH_ASSERT(ioTwist.GetZ() == 0.0f); + + // Ensure quaternions have w > 0 + bool negate_swing = ioSwing.GetW() < 0.0f; + if (negate_swing) + ioSwing = -ioSwing; + bool negate_twist = ioTwist.GetW() < 0.0f; + if (negate_twist) + ioTwist = -ioTwist; + + if (mRotationFlags & TwistXLocked) + { + // Twist axis is locked, clamp whenever twist is not identity + outClampedAxis |= ioTwist.GetX() != 0.0f? (cClampedTwistMin | cClampedTwistMax) : 0; + ioTwist = Quat::sIdentity(); + } + else if ((mRotationFlags & TwistXFree) == 0) + { + // Twist axis has limit, clamp whenever out of range + float delta_min = mSinTwistHalfMinAngle - ioTwist.GetX(); + float delta_max = ioTwist.GetX() - mSinTwistHalfMaxAngle; + if (delta_min > 0.0f || delta_max > 0.0f) + { + // Pick the twist that corresponds to the smallest delta + if (sDistanceToMinShorter(delta_min, delta_max)) + { + ioTwist = Quat(mSinTwistHalfMinAngle, 0, 0, mCosTwistHalfMinAngle); + outClampedAxis |= cClampedTwistMin; + } + else + { + ioTwist = Quat(mSinTwistHalfMaxAngle, 0, 0, mCosTwistHalfMaxAngle); + outClampedAxis |= cClampedTwistMax; + } + } + } + + // Clamp swing + if (mRotationFlags & SwingYLocked) + { + if (mRotationFlags & SwingZLocked) + { + // Both swing Y and Z are disabled, no degrees of freedom in swing + outClampedAxis |= ioSwing.GetY() != 0.0f? (cClampedSwingYMin | cClampedSwingYMax) : 0; + outClampedAxis |= ioSwing.GetZ() != 0.0f? (cClampedSwingZMin | cClampedSwingZMax) : 0; + ioSwing = Quat::sIdentity(); + } + else + { + // Swing Y angle disabled, only 1 degree of freedom in swing + outClampedAxis |= ioSwing.GetY() != 0.0f? (cClampedSwingYMin | cClampedSwingYMax) : 0; + float delta_min = mSinSwingZHalfMinAngle - ioSwing.GetZ(); + float delta_max = ioSwing.GetZ() - mSinSwingZHalfMaxAngle; + if (delta_min > 0.0f || delta_max > 0.0f) + { + // Pick the swing that corresponds to the smallest delta + if (sDistanceToMinShorter(delta_min, delta_max)) + { + ioSwing = Quat(0, 0, mSinSwingZHalfMinAngle, mCosSwingZHalfMinAngle); + outClampedAxis |= cClampedSwingZMin; + } + else + { + ioSwing = Quat(0, 0, mSinSwingZHalfMaxAngle, mCosSwingZHalfMaxAngle); + outClampedAxis |= cClampedSwingZMax; + } + } + else if ((outClampedAxis & cClampedSwingYMin) != 0) + { + float z = ioSwing.GetZ(); + ioSwing = Quat(0, 0, z, sqrt(1.0f - Square(z))); + } + } + } + else if (mRotationFlags & SwingZLocked) + { + // Swing Z angle disabled, only 1 degree of freedom in swing + outClampedAxis |= ioSwing.GetZ() != 0.0f? (cClampedSwingZMin | cClampedSwingZMax) : 0; + float delta_min = mSinSwingYHalfMinAngle - ioSwing.GetY(); + float delta_max = ioSwing.GetY() - mSinSwingYHalfMaxAngle; + if (delta_min > 0.0f || delta_max > 0.0f) + { + // Pick the swing that corresponds to the smallest delta + if (sDistanceToMinShorter(delta_min, delta_max)) + { + ioSwing = Quat(0, mSinSwingYHalfMinAngle, 0, mCosSwingYHalfMinAngle); + outClampedAxis |= cClampedSwingYMin; + } + else + { + ioSwing = Quat(0, mSinSwingYHalfMaxAngle, 0, mCosSwingYHalfMaxAngle); + outClampedAxis |= cClampedSwingYMax; + } + } + else if ((outClampedAxis & cClampedSwingZMin) != 0) + { + float y = ioSwing.GetY(); + ioSwing = Quat(0, y, 0, sqrt(1.0f - Square(y))); + } + } + else + { + // Two degrees of freedom + if (mSwingType == ESwingType::Cone) + { + // Use ellipse to solve limits + Ellipse ellipse(mSinSwingYHalfMaxAngle, mSinSwingZHalfMaxAngle); + Float2 point(ioSwing.GetY(), ioSwing.GetZ()); + if (!ellipse.IsInside(point)) + { + Float2 closest = ellipse.GetClosestPoint(point); + ioSwing = Quat(0, closest.x, closest.y, sqrt(max(0.0f, 1.0f - Square(closest.x) - Square(closest.y)))); + outClampedAxis |= cClampedSwingYMin | cClampedSwingYMax | cClampedSwingZMin | cClampedSwingZMax; // We're not using the flags on which side we got clamped here + } + } + else + { + // Use pyramid to solve limits + // The quaternion rotating by angle y around the Y axis then rotating by angle z around the Z axis is: + // q = Quat::sRotation(Vec3::sAxisZ(), z) * Quat::sRotation(Vec3::sAxisY(), y) + // [q.x, q.y, q.z, q.w] = [-sin(y / 2) * sin(z / 2), sin(y / 2) * cos(z / 2), cos(y / 2) * sin(z / 2), cos(y / 2) * cos(z / 2)] + // So we can calculate y / 2 = atan2(q.y, q.w) and z / 2 = atan2(q.z, q.w) + Vec4 half_angle = Vec4::sATan2(ioSwing.GetXYZW().Swizzle(), ioSwing.GetXYZW().SplatW()); + Vec4 min_half_angle(mSwingYHalfMinAngle, mSwingYHalfMinAngle, mSwingZHalfMinAngle, mSwingZHalfMinAngle); + Vec4 max_half_angle(mSwingYHalfMaxAngle, mSwingYHalfMaxAngle, mSwingZHalfMaxAngle, mSwingZHalfMaxAngle); + Vec4 clamped_half_angle = Vec4::sMin(Vec4::sMax(half_angle, min_half_angle), max_half_angle); + UVec4 unclamped = Vec4::sEquals(half_angle, clamped_half_angle); + if (!unclamped.TestAllTrue()) + { + // We now calculate the quaternion again using the formula for q above, + // but we leave out the x component in order to not introduce twist + Vec4 s, c; + clamped_half_angle.SinCos(s, c); + ioSwing = Quat(0, s.GetY() * c.GetZ(), c.GetY() * s.GetZ(), c.GetY() * c.GetZ()).Normalized(); + outClampedAxis |= cClampedSwingYMin | cClampedSwingYMax | cClampedSwingZMin | cClampedSwingZMax; // We're not using the flags on which side we got clamped here + } + } + } + + // Flip sign back + if (negate_swing) + ioSwing = -ioSwing; + if (negate_twist) + ioTwist = -ioTwist; + + JPH_ASSERT(ioSwing.IsNormalized()); + JPH_ASSERT(ioTwist.IsNormalized()); + } + + /// Calculate properties used during the functions below + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inConstraintRotation The current rotation of the constraint in constraint space + /// @param inConstraintToWorld Rotates from constraint space into world space + inline void CalculateConstraintProperties(const Body &inBody1, const Body &inBody2, QuatArg inConstraintRotation, QuatArg inConstraintToWorld) + { + // Decompose into swing and twist + Quat q_swing, q_twist; + inConstraintRotation.GetSwingTwist(q_swing, q_twist); + + // Clamp against joint limits + Quat q_clamped_swing = q_swing, q_clamped_twist = q_twist; + uint clamped_axis; + ClampSwingTwist(q_clamped_swing, q_clamped_twist, clamped_axis); + + if (mRotationFlags & SwingYLocked) + { + Quat twist_to_world = inConstraintToWorld * q_swing; + mWorldSpaceSwingLimitYRotationAxis = twist_to_world.RotateAxisY(); + mWorldSpaceSwingLimitZRotationAxis = twist_to_world.RotateAxisZ(); + + if (mRotationFlags & SwingZLocked) + { + // Swing fully locked + mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis); + mSwingLimitZConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitZRotationAxis); + } + else + { + // Swing only locked around Y + mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis); + if ((clamped_axis & (cClampedSwingZMin | cClampedSwingZMax)) != 0) + { + if ((clamped_axis & cClampedSwingZMin) != 0) + mWorldSpaceSwingLimitZRotationAxis = -mWorldSpaceSwingLimitZRotationAxis; // Flip axis if hitting min limit because the impulse limit is going to be between [-FLT_MAX, 0] + mSwingLimitZConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitZRotationAxis); + } + else + mSwingLimitZConstraintPart.Deactivate(); + } + } + else if (mRotationFlags & SwingZLocked) + { + // Swing only locked around Z + Quat twist_to_world = inConstraintToWorld * q_swing; + mWorldSpaceSwingLimitYRotationAxis = twist_to_world.RotateAxisY(); + mWorldSpaceSwingLimitZRotationAxis = twist_to_world.RotateAxisZ(); + + if ((clamped_axis & (cClampedSwingYMin | cClampedSwingYMax)) != 0) + { + if ((clamped_axis & cClampedSwingYMin) != 0) + mWorldSpaceSwingLimitYRotationAxis = -mWorldSpaceSwingLimitYRotationAxis; // Flip axis if hitting min limit because the impulse limit is going to be between [-FLT_MAX, 0] + mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis); + } + else + mSwingLimitYConstraintPart.Deactivate(); + mSwingLimitZConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitZRotationAxis); + } + else if ((mRotationFlags & SwingYZFree) != SwingYZFree) + { + // Swing has limits around Y and Z + if ((clamped_axis & (cClampedSwingYMin | cClampedSwingYMax | cClampedSwingZMin | cClampedSwingZMax)) != 0) + { + // Calculate axis of rotation from clamped swing to swing + Vec3 current = (inConstraintToWorld * q_swing).RotateAxisX(); + Vec3 desired = (inConstraintToWorld * q_clamped_swing).RotateAxisX(); + mWorldSpaceSwingLimitYRotationAxis = desired.Cross(current); + float len = mWorldSpaceSwingLimitYRotationAxis.Length(); + if (len != 0.0f) + { + mWorldSpaceSwingLimitYRotationAxis /= len; + mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis); + } + else + mSwingLimitYConstraintPart.Deactivate(); + } + else + mSwingLimitYConstraintPart.Deactivate(); + mSwingLimitZConstraintPart.Deactivate(); + } + else + { + // No swing limits + mSwingLimitYConstraintPart.Deactivate(); + mSwingLimitZConstraintPart.Deactivate(); + } + + if (mRotationFlags & TwistXLocked) + { + // Twist locked, always activate constraint + mWorldSpaceTwistLimitRotationAxis = (inConstraintToWorld * q_swing).RotateAxisX(); + mTwistLimitConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceTwistLimitRotationAxis); + } + else if ((mRotationFlags & TwistXFree) == 0) + { + // Twist has limits + if ((clamped_axis & (cClampedTwistMin | cClampedTwistMax)) != 0) + { + mWorldSpaceTwistLimitRotationAxis = (inConstraintToWorld * q_swing).RotateAxisX(); + if ((clamped_axis & cClampedTwistMin) != 0) + mWorldSpaceTwistLimitRotationAxis = -mWorldSpaceTwistLimitRotationAxis; // Flip axis if hitting min limit because the impulse limit is going to be between [-FLT_MAX, 0] + mTwistLimitConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceTwistLimitRotationAxis); + } + else + mTwistLimitConstraintPart.Deactivate(); + } + else + { + // No twist limits + mTwistLimitConstraintPart.Deactivate(); + } + } + + /// Deactivate this constraint + void Deactivate() + { + mSwingLimitYConstraintPart.Deactivate(); + mSwingLimitZConstraintPart.Deactivate(); + mTwistLimitConstraintPart.Deactivate(); + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mSwingLimitYConstraintPart.IsActive() || mSwingLimitZConstraintPart.IsActive() || mTwistLimitConstraintPart.IsActive(); + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mSwingLimitYConstraintPart.WarmStart(ioBody1, ioBody2, inWarmStartImpulseRatio); + mSwingLimitZConstraintPart.WarmStart(ioBody1, ioBody2, inWarmStartImpulseRatio); + mTwistLimitConstraintPart.WarmStart(ioBody1, ioBody2, inWarmStartImpulseRatio); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2) + { + bool impulse = false; + + // Solve swing constraint + if (mSwingLimitYConstraintPart.IsActive()) + impulse |= mSwingLimitYConstraintPart.SolveVelocityConstraint(ioBody1, ioBody2, mWorldSpaceSwingLimitYRotationAxis, -FLT_MAX, mSinSwingYHalfMinAngle == mSinSwingYHalfMaxAngle? FLT_MAX : 0.0f); + + if (mSwingLimitZConstraintPart.IsActive()) + impulse |= mSwingLimitZConstraintPart.SolveVelocityConstraint(ioBody1, ioBody2, mWorldSpaceSwingLimitZRotationAxis, -FLT_MAX, mSinSwingZHalfMinAngle == mSinSwingZHalfMaxAngle? FLT_MAX : 0.0f); + + // Solve twist constraint + if (mTwistLimitConstraintPart.IsActive()) + impulse |= mTwistLimitConstraintPart.SolveVelocityConstraint(ioBody1, ioBody2, mWorldSpaceTwistLimitRotationAxis, -FLT_MAX, mSinTwistHalfMinAngle == mSinTwistHalfMaxAngle? FLT_MAX : 0.0f); + + return impulse; + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inConstraintRotation The current rotation of the constraint in constraint space + /// @param inConstraintToBody1 , inConstraintToBody2 Rotates from constraint space to body 1/2 space + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, QuatArg inConstraintRotation, QuatArg inConstraintToBody1, QuatArg inConstraintToBody2, float inBaumgarte) const + { + Quat q_swing, q_twist; + inConstraintRotation.GetSwingTwist(q_swing, q_twist); + + uint clamped_axis; + ClampSwingTwist(q_swing, q_twist, clamped_axis); + + // Solve rotation violations + if (clamped_axis != 0) + { + RotationEulerConstraintPart part; + Quat inv_initial_orientation = inConstraintToBody2 * (inConstraintToBody1 * q_swing * q_twist).Conjugated(); + part.CalculateConstraintProperties(ioBody1, Mat44::sRotation(ioBody1.GetRotation()), ioBody2, Mat44::sRotation(ioBody2.GetRotation())); + return part.SolvePositionConstraint(ioBody1, ioBody2, inv_initial_orientation, inBaumgarte); + } + + return false; + } + + /// Return lagrange multiplier for swing + inline float GetTotalSwingYLambda() const + { + return mSwingLimitYConstraintPart.GetTotalLambda(); + } + + inline float GetTotalSwingZLambda() const + { + return mSwingLimitZConstraintPart.GetTotalLambda(); + } + + /// Return lagrange multiplier for twist + inline float GetTotalTwistLambda() const + { + return mTwistLimitConstraintPart.GetTotalLambda(); + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + mSwingLimitYConstraintPart.SaveState(inStream); + mSwingLimitZConstraintPart.SaveState(inStream); + mTwistLimitConstraintPart.SaveState(inStream); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + mSwingLimitYConstraintPart.RestoreState(inStream); + mSwingLimitZConstraintPart.RestoreState(inStream); + mTwistLimitConstraintPart.RestoreState(inStream); + } + +private: + // CONFIGURATION PROPERTIES FOLLOW + + enum ERotationFlags + { + /// Indicates that axis is completely locked (cannot rotate around this axis) + TwistXLocked = 1 << 0, + SwingYLocked = 1 << 1, + SwingZLocked = 1 << 2, + + /// Indicates that axis is completely free (can rotate around without limits) + TwistXFree = 1 << 3, + SwingYFree = 1 << 4, + SwingZFree = 1 << 5, + SwingYZFree = SwingYFree | SwingZFree + }; + + uint8 mRotationFlags; + + // Constants + ESwingType mSwingType = ESwingType::Cone; + float mSinTwistHalfMinAngle; + float mSinTwistHalfMaxAngle; + float mCosTwistHalfMinAngle; + float mCosTwistHalfMaxAngle; + float mSwingYHalfMinAngle; + float mSwingYHalfMaxAngle; + float mSwingZHalfMinAngle; + float mSwingZHalfMaxAngle; + float mSinSwingYHalfMinAngle; + float mSinSwingYHalfMaxAngle; + float mSinSwingZHalfMinAngle; + float mSinSwingZHalfMaxAngle; + float mCosSwingYHalfMinAngle; + float mCosSwingYHalfMaxAngle; + float mCosSwingZHalfMinAngle; + float mCosSwingZHalfMaxAngle; + + // RUN TIME PROPERTIES FOLLOW + + /// Rotation axis for the angle constraint parts + Vec3 mWorldSpaceSwingLimitYRotationAxis; + Vec3 mWorldSpaceSwingLimitZRotationAxis; + Vec3 mWorldSpaceTwistLimitRotationAxis; + + /// The constraint parts + AngleConstraintPart mSwingLimitYConstraintPart; + AngleConstraintPart mSwingLimitZConstraintPart; + AngleConstraintPart mTwistLimitConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp new file mode 100644 index 000000000000..928acdaa4221 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp @@ -0,0 +1,1783 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +using namespace literals; + +#ifdef JPH_DEBUG_RENDERER +bool ContactConstraintManager::sDrawContactPoint = false; +bool ContactConstraintManager::sDrawSupportingFaces = false; +bool ContactConstraintManager::sDrawContactPointReduction = false; +bool ContactConstraintManager::sDrawContactManifolds = false; +#endif // JPH_DEBUG_RENDERER + +//#define JPH_MANIFOLD_CACHE_DEBUG + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// ContactConstraintManager::WorldContactPoint +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +void ContactConstraintManager::WorldContactPoint::CalculateNonPenetrationConstraintProperties(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal) +{ + // Calculate collision points relative to body + RVec3 p = 0.5_r * (inWorldSpacePosition1 + inWorldSpacePosition2); + Vec3 r1 = Vec3(p - inBody1.GetCenterOfMassPosition()); + Vec3 r2 = Vec3(p - inBody2.GetCenterOfMassPosition()); + + mNonPenetrationConstraint.CalculateConstraintPropertiesWithMassOverride(inBody1, inInvMass1, inInvInertiaScale1, r1, inBody2, inInvMass2, inInvInertiaScale2, r2, inWorldSpaceNormal); +} + +template +JPH_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, float inGravityDeltaTimeDotNormal, const Body &inBody1, const Body &inBody2, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, const ContactSettings &inSettings, float inMinVelocityForRestitution) +{ + JPH_DET_LOG("TemplatedCalculateFrictionAndNonPenetrationConstraintProperties: p1: " << inWorldSpacePosition1 << " p2: " << inWorldSpacePosition2 + << " normal: " << inWorldSpaceNormal << " tangent1: " << inWorldSpaceTangent1 << " tangent2: " << inWorldSpaceTangent2 + << " restitution: " << inSettings.mCombinedRestitution << " friction: " << inSettings.mCombinedFriction << " minv: " << inMinVelocityForRestitution + << " surface_vel: " << inSettings.mRelativeLinearSurfaceVelocity << " surface_ang: " << inSettings.mRelativeAngularSurfaceVelocity); + + // Calculate collision points relative to body + RVec3 p = 0.5_r * (inWorldSpacePosition1 + inWorldSpacePosition2); + Vec3 r1 = Vec3(p - inBody1.GetCenterOfMassPosition()); + Vec3 r2 = Vec3(p - inBody2.GetCenterOfMassPosition()); + + // The gravity is applied in the beginning of the time step. If we get here, there was a collision + // at the beginning of the time step, so we've applied too much gravity. This means that our + // calculated restitution can be too high, so when we apply restitution, we cancel the added + // velocity due to gravity. + float gravity_dt_dot_normal; + + // Calculate velocity of collision points + Vec3 relative_velocity; + if constexpr (Type1 != EMotionType::Static && Type2 != EMotionType::Static) + { + const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked(); + const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked(); + relative_velocity = mp2->GetPointVelocityCOM(r2) - mp1->GetPointVelocityCOM(r1); + gravity_dt_dot_normal = inGravityDeltaTimeDotNormal * (mp2->GetGravityFactor() - mp1->GetGravityFactor()); + } + else if constexpr (Type1 != EMotionType::Static) + { + const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked(); + relative_velocity = -mp1->GetPointVelocityCOM(r1); + gravity_dt_dot_normal = inGravityDeltaTimeDotNormal * mp1->GetGravityFactor(); + } + else if constexpr (Type2 != EMotionType::Static) + { + const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked(); + relative_velocity = mp2->GetPointVelocityCOM(r2); + gravity_dt_dot_normal = inGravityDeltaTimeDotNormal * mp2->GetGravityFactor(); + } + else + { + JPH_ASSERT(false); // Static vs static makes no sense + relative_velocity = Vec3::sZero(); + gravity_dt_dot_normal = 0.0f; + } + float normal_velocity = relative_velocity.Dot(inWorldSpaceNormal); + + // How much the shapes are penetrating (> 0 if penetrating, < 0 if separated) + float penetration = Vec3(inWorldSpacePosition1 - inWorldSpacePosition2).Dot(inWorldSpaceNormal); + + // If there is no penetration, this is a speculative contact and we will apply a bias to the contact constraint + // so that the constraint becomes relative_velocity . contact normal > -penetration / delta_time + // instead of relative_velocity . contact normal > 0 + // See: GDC 2013: "Physics for Game Programmers; Continuous Collision" - Erin Catto + float speculative_contact_velocity_bias = max(0.0f, -penetration / inDeltaTime); + + // Determine if the velocity is big enough for restitution + float normal_velocity_bias; + if (inSettings.mCombinedRestitution > 0.0f && normal_velocity < -inMinVelocityForRestitution) + { + // We have a velocity that is big enough for restitution. This is where speculative contacts don't work + // great as we have to decide now if we're going to apply the restitution or not. If the relative + // velocity is big enough for a hit, we apply the restitution (in the end, due to other constraints, + // the objects may actually not collide and we will have applied restitution incorrectly). Another + // artifact that occurs because of this approximation is that the object will bounce from its current + // position rather than from a position where it is touching the other object. This causes the object + // to appear to move faster for 1 frame (the opposite of time stealing). + if (normal_velocity < -speculative_contact_velocity_bias) + normal_velocity_bias = inSettings.mCombinedRestitution * (normal_velocity - gravity_dt_dot_normal); + else + // In this case we have predicted that we don't hit the other object, but if we do (due to other constraints changing velocities) + // the speculative contact will prevent penetration but will not apply restitution leading to another artifact. + normal_velocity_bias = speculative_contact_velocity_bias; + } + else + { + // No restitution. We can safely apply our contact velocity bias. + normal_velocity_bias = speculative_contact_velocity_bias; + } + + mNonPenetrationConstraint.TemplatedCalculateConstraintProperties(inInvM1, inInvI1, r1, inInvM2, inInvI2, r2, inWorldSpaceNormal, normal_velocity_bias); + + // Calculate friction part + if (inSettings.mCombinedFriction > 0.0f) + { + // Get surface velocity relative to tangents + Vec3 ws_surface_velocity = inSettings.mRelativeLinearSurfaceVelocity + inSettings.mRelativeAngularSurfaceVelocity.Cross(r1); + float surface_velocity1 = inWorldSpaceTangent1.Dot(ws_surface_velocity); + float surface_velocity2 = inWorldSpaceTangent2.Dot(ws_surface_velocity); + + // Implement friction as 2 AxisContraintParts + mFrictionConstraint1.TemplatedCalculateConstraintProperties(inInvM1, inInvI1, r1, inInvM2, inInvI2, r2, inWorldSpaceTangent1, surface_velocity1); + mFrictionConstraint2.TemplatedCalculateConstraintProperties(inInvM1, inInvI1, r1, inInvM2, inInvI2, r2, inWorldSpaceTangent2, surface_velocity2); + } + else + { + // Turn off friction constraint + mFrictionConstraint1.Deactivate(); + mFrictionConstraint2.Deactivate(); + } +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// ContactConstraintManager::ContactConstraint +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +#ifdef JPH_DEBUG_RENDERER +void ContactConstraintManager::ContactConstraint::Draw(DebugRenderer *inRenderer, ColorArg inManifoldColor) const +{ + if (mContactPoints.empty()) + return; + + // Get body transforms + RMat44 transform_body1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform_body2 = mBody2->GetCenterOfMassTransform(); + + RVec3 prev_point = transform_body1 * Vec3::sLoadFloat3Unsafe(mContactPoints.back().mContactPoint->mPosition1); + for (const WorldContactPoint &wcp : mContactPoints) + { + // Test if any lambda from the previous frame was transferred + float radius = wcp.mNonPenetrationConstraint.GetTotalLambda() == 0.0f + && wcp.mFrictionConstraint1.GetTotalLambda() == 0.0f + && wcp.mFrictionConstraint2.GetTotalLambda() == 0.0f? 0.1f : 0.2f; + + RVec3 next_point = transform_body1 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition1); + inRenderer->DrawMarker(next_point, Color::sCyan, radius); + inRenderer->DrawMarker(transform_body2 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition2), Color::sPurple, radius); + + // Draw edge + inRenderer->DrawArrow(prev_point, next_point, inManifoldColor, 0.05f); + prev_point = next_point; + } + + // Draw normal + RVec3 wp = transform_body1 * Vec3::sLoadFloat3Unsafe(mContactPoints[0].mContactPoint->mPosition1); + inRenderer->DrawArrow(wp, wp + GetWorldSpaceNormal(), Color::sRed, 0.05f); + + // Get tangents + Vec3 t1, t2; + GetTangents(t1, t2); + + // Draw tangents + inRenderer->DrawLine(wp, wp + t1, Color::sGreen); + inRenderer->DrawLine(wp, wp + t2, Color::sBlue); +} +#endif // JPH_DEBUG_RENDERER + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// ContactConstraintManager::CachedContactPoint +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +void ContactConstraintManager::CachedContactPoint::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mPosition1); + inStream.Write(mPosition2); + inStream.Write(mNonPenetrationLambda); + inStream.Write(mFrictionLambda); +} + +void ContactConstraintManager::CachedContactPoint::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mPosition1); + inStream.Read(mPosition2); + inStream.Read(mNonPenetrationLambda); + inStream.Read(mFrictionLambda); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// ContactConstraintManager::CachedManifold +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +void ContactConstraintManager::CachedManifold::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mContactNormal); +} + +void ContactConstraintManager::CachedManifold::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mContactNormal); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// ContactConstraintManager::CachedBodyPair +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +void ContactConstraintManager::CachedBodyPair::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mDeltaPosition); + inStream.Write(mDeltaRotation); +} + +void ContactConstraintManager::CachedBodyPair::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mDeltaPosition); + inStream.Read(mDeltaRotation); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// ContactConstraintManager::ManifoldCache +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +void ContactConstraintManager::ManifoldCache::Init(uint inMaxBodyPairs, uint inMaxContactConstraints, uint inCachedManifoldsSize) +{ + mAllocator.Init(inMaxBodyPairs * sizeof(BodyPairMap::KeyValue) + inCachedManifoldsSize); + mCachedManifolds.Init(GetNextPowerOf2(inMaxContactConstraints)); + mCachedBodyPairs.Init(GetNextPowerOf2(inMaxBodyPairs)); +} + +void ContactConstraintManager::ManifoldCache::Clear() +{ + JPH_PROFILE_FUNCTION(); + + mCachedManifolds.Clear(); + mCachedBodyPairs.Clear(); + mAllocator.Clear(); + +#ifdef JPH_ENABLE_ASSERTS + // Mark as incomplete + mIsFinalized = false; +#endif +} + +void ContactConstraintManager::ManifoldCache::Prepare(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds) +{ + // Minimum amount of buckets to use in the hash map + constexpr uint32 cMinBuckets = 1024; + + // Use the next higher power of 2 of amount of objects in the cache from last frame to determine the amount of buckets in this frame + mCachedManifolds.SetNumBuckets(min(max(cMinBuckets, GetNextPowerOf2(inExpectedNumManifolds)), mCachedManifolds.GetMaxBuckets())); + mCachedBodyPairs.SetNumBuckets(min(max(cMinBuckets, GetNextPowerOf2(inExpectedNumBodyPairs)), mCachedBodyPairs.GetMaxBuckets())); +} + +const ContactConstraintManager::MKeyValue *ContactConstraintManager::ManifoldCache::Find(const SubShapeIDPair &inKey, uint64 inKeyHash) const +{ + JPH_ASSERT(mIsFinalized); + return mCachedManifolds.Find(inKey, inKeyHash); +} + +ContactConstraintManager::MKeyValue *ContactConstraintManager::ManifoldCache::Create(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints) +{ + JPH_ASSERT(!mIsFinalized); + MKeyValue *kv = mCachedManifolds.Create(ioContactAllocator, inKey, inKeyHash, CachedManifold::sGetRequiredExtraSize(inNumContactPoints)); + if (kv == nullptr) + { + ioContactAllocator.mErrors |= EPhysicsUpdateError::ManifoldCacheFull; + return nullptr; + } + kv->GetValue().mNumContactPoints = uint16(inNumContactPoints); + ++ioContactAllocator.mNumManifolds; + return kv; +} + +ContactConstraintManager::MKVAndCreated ContactConstraintManager::ManifoldCache::FindOrCreate(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints) +{ + MKeyValue *kv = const_cast(mCachedManifolds.Find(inKey, inKeyHash)); + if (kv != nullptr) + return { kv, false }; + + return { Create(ioContactAllocator, inKey, inKeyHash, inNumContactPoints), true }; +} + +uint32 ContactConstraintManager::ManifoldCache::ToHandle(const MKeyValue *inKeyValue) const +{ + JPH_ASSERT(!mIsFinalized); + return mCachedManifolds.ToHandle(inKeyValue); +} + +const ContactConstraintManager::MKeyValue *ContactConstraintManager::ManifoldCache::FromHandle(uint32 inHandle) const +{ + JPH_ASSERT(mIsFinalized); + return mCachedManifolds.FromHandle(inHandle); +} + +const ContactConstraintManager::BPKeyValue *ContactConstraintManager::ManifoldCache::Find(const BodyPair &inKey, uint64 inKeyHash) const +{ + JPH_ASSERT(mIsFinalized); + return mCachedBodyPairs.Find(inKey, inKeyHash); +} + +ContactConstraintManager::BPKeyValue *ContactConstraintManager::ManifoldCache::Create(ContactAllocator &ioContactAllocator, const BodyPair &inKey, uint64 inKeyHash) +{ + JPH_ASSERT(!mIsFinalized); + BPKeyValue *kv = mCachedBodyPairs.Create(ioContactAllocator, inKey, inKeyHash, 0); + if (kv == nullptr) + { + ioContactAllocator.mErrors |= EPhysicsUpdateError::BodyPairCacheFull; + return nullptr; + } + ++ioContactAllocator.mNumBodyPairs; + return kv; +} + +void ContactConstraintManager::ManifoldCache::GetAllBodyPairsSorted(Array &outAll) const +{ + JPH_ASSERT(mIsFinalized); + mCachedBodyPairs.GetAllKeyValues(outAll); + + // Sort by key + QuickSort(outAll.begin(), outAll.end(), [](const BPKeyValue *inLHS, const BPKeyValue *inRHS) { + return inLHS->GetKey() < inRHS->GetKey(); + }); +} + +void ContactConstraintManager::ManifoldCache::GetAllManifoldsSorted(const CachedBodyPair &inBodyPair, Array &outAll) const +{ + JPH_ASSERT(mIsFinalized); + + // Iterate through the attached manifolds + for (uint32 handle = inBodyPair.mFirstCachedManifold; handle != ManifoldMap::cInvalidHandle; handle = FromHandle(handle)->GetValue().mNextWithSameBodyPair) + { + const MKeyValue *kv = mCachedManifolds.FromHandle(handle); + outAll.push_back(kv); + } + + // Sort by key + QuickSort(outAll.begin(), outAll.end(), [](const MKeyValue *inLHS, const MKeyValue *inRHS) { + return inLHS->GetKey() < inRHS->GetKey(); + }); +} + +void ContactConstraintManager::ManifoldCache::GetAllCCDManifoldsSorted(Array &outAll) const +{ + mCachedManifolds.GetAllKeyValues(outAll); + + for (int i = (int)outAll.size() - 1; i >= 0; --i) + if ((outAll[i]->GetValue().mFlags & (uint16)CachedManifold::EFlags::CCDContact) == 0) + { + outAll[i] = outAll.back(); + outAll.pop_back(); + } + + // Sort by key + QuickSort(outAll.begin(), outAll.end(), [](const MKeyValue *inLHS, const MKeyValue *inRHS) { + return inLHS->GetKey() < inRHS->GetKey(); + }); +} + +void ContactConstraintManager::ManifoldCache::ContactPointRemovedCallbacks(ContactListener *inListener) +{ + JPH_PROFILE_FUNCTION(); + + for (MKeyValue &kv : mCachedManifolds) + if ((kv.GetValue().mFlags & uint16(CachedManifold::EFlags::ContactPersisted)) == 0) + inListener->OnContactRemoved(kv.GetKey()); +} + +#ifdef JPH_ENABLE_ASSERTS + +void ContactConstraintManager::ManifoldCache::Finalize() +{ + mIsFinalized = true; + +#ifdef JPH_MANIFOLD_CACHE_DEBUG + Trace("ManifoldMap:"); + mCachedManifolds.TraceStats(); + Trace("BodyPairMap:"); + mCachedBodyPairs.TraceStats(); +#endif // JPH_MANIFOLD_CACHE_DEBUG +} + +#endif + +void ContactConstraintManager::ManifoldCache::SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const +{ + JPH_ASSERT(mIsFinalized); + + // Get contents of cache + Array all_bp; + GetAllBodyPairsSorted(all_bp); + + // Determine which ones to save + Array selected_bp; + if (inFilter == nullptr) + selected_bp = std::move(all_bp); + else + { + selected_bp.reserve(all_bp.size()); + for (const BPKeyValue *bp_kv : all_bp) + if (inFilter->ShouldSaveContact(bp_kv->GetKey().mBodyA, bp_kv->GetKey().mBodyB)) + selected_bp.push_back(bp_kv); + } + + // Write body pairs + uint32 num_body_pairs = uint32(selected_bp.size()); + inStream.Write(num_body_pairs); + for (const BPKeyValue *bp_kv : selected_bp) + { + // Write body pair key + inStream.Write(bp_kv->GetKey()); + + // Write body pair + const CachedBodyPair &bp = bp_kv->GetValue(); + bp.SaveState(inStream); + + // Get attached manifolds + Array all_m; + GetAllManifoldsSorted(bp, all_m); + + // Write num manifolds + uint32 num_manifolds = uint32(all_m.size()); + inStream.Write(num_manifolds); + + // Write all manifolds + for (const MKeyValue *m_kv : all_m) + { + // Write key + inStream.Write(m_kv->GetKey()); + const CachedManifold &cm = m_kv->GetValue(); + JPH_ASSERT((cm.mFlags & (uint16)CachedManifold::EFlags::CCDContact) == 0); + + // Write amount of contacts + inStream.Write(cm.mNumContactPoints); + + // Write manifold + cm.SaveState(inStream); + + // Write contact points + for (uint32 i = 0; i < cm.mNumContactPoints; ++i) + cm.mContactPoints[i].SaveState(inStream); + } + } + + // Get CCD manifolds + Array all_m; + GetAllCCDManifoldsSorted(all_m); + + // Determine which ones to save + Array selected_m; + if (inFilter == nullptr) + selected_m = std::move(all_m); + else + { + selected_m.reserve(all_m.size()); + for (const MKeyValue *m_kv : all_m) + if (inFilter->ShouldSaveContact(m_kv->GetKey().GetBody1ID(), m_kv->GetKey().GetBody2ID())) + selected_m.push_back(m_kv); + } + + // Write all CCD manifold keys + uint32 num_manifolds = uint32(selected_m.size()); + inStream.Write(num_manifolds); + for (const MKeyValue *m_kv : selected_m) + inStream.Write(m_kv->GetKey()); +} + +bool ContactConstraintManager::ManifoldCache::RestoreState(const ManifoldCache &inReadCache, StateRecorder &inStream, const StateRecorderFilter *inFilter) +{ + JPH_ASSERT(!mIsFinalized); + + bool success = true; + + // Create a contact allocator for restoring the contact cache + ContactAllocator contact_allocator(GetContactAllocator()); + + // When validating, get all existing body pairs + Array all_bp; + if (inStream.IsValidating()) + inReadCache.GetAllBodyPairsSorted(all_bp); + + // Read amount of body pairs + uint32 num_body_pairs; + if (inStream.IsValidating()) + num_body_pairs = uint32(all_bp.size()); + inStream.Read(num_body_pairs); + + // Read entire cache + for (uint32 i = 0; i < num_body_pairs; ++i) + { + // Read key + BodyPair body_pair_key; + if (inStream.IsValidating() && i < all_bp.size()) + body_pair_key = all_bp[i]->GetKey(); + inStream.Read(body_pair_key); + + // Check if we want to restore this contact + if (inFilter == nullptr || inFilter->ShouldRestoreContact(body_pair_key.mBodyA, body_pair_key.mBodyB)) + { + // Create new entry for this body pair + uint64 body_pair_hash = body_pair_key.GetHash(); + BPKeyValue *bp_kv = Create(contact_allocator, body_pair_key, body_pair_hash); + if (bp_kv == nullptr) + { + // Out of cache space + success = false; + break; + } + CachedBodyPair &bp = bp_kv->GetValue(); + + // Read body pair + if (inStream.IsValidating() && i < all_bp.size()) + memcpy(&bp, &all_bp[i]->GetValue(), sizeof(CachedBodyPair)); + bp.RestoreState(inStream); + + // When validating, get all existing manifolds + Array all_m; + if (inStream.IsValidating()) + inReadCache.GetAllManifoldsSorted(all_bp[i]->GetValue(), all_m); + + // Read amount of manifolds + uint32 num_manifolds = 0; + if (inStream.IsValidating()) + num_manifolds = uint32(all_m.size()); + inStream.Read(num_manifolds); + + uint32 handle = ManifoldMap::cInvalidHandle; + for (uint32 j = 0; j < num_manifolds; ++j) + { + // Read key + SubShapeIDPair sub_shape_key; + if (inStream.IsValidating() && j < all_m.size()) + sub_shape_key = all_m[j]->GetKey(); + inStream.Read(sub_shape_key); + uint64 sub_shape_key_hash = sub_shape_key.GetHash(); + + // Read amount of contact points + uint16 num_contact_points = 0; + if (inStream.IsValidating() && j < all_m.size()) + num_contact_points = all_m[j]->GetValue().mNumContactPoints; + inStream.Read(num_contact_points); + + // Read manifold + MKeyValue *m_kv = Create(contact_allocator, sub_shape_key, sub_shape_key_hash, num_contact_points); + if (m_kv == nullptr) + { + // Out of cache space + success = false; + break; + } + CachedManifold &cm = m_kv->GetValue(); + if (inStream.IsValidating() && j < all_m.size()) + { + memcpy(&cm, &all_m[j]->GetValue(), CachedManifold::sGetRequiredTotalSize(num_contact_points)); + cm.mNumContactPoints = uint16(num_contact_points); // Restore num contact points + } + cm.RestoreState(inStream); + cm.mNextWithSameBodyPair = handle; + handle = ToHandle(m_kv); + + // Read contact points + for (uint32 k = 0; k < num_contact_points; ++k) + cm.mContactPoints[k].RestoreState(inStream); + } + bp.mFirstCachedManifold = handle; + } + else + { + // Skip the contact + CachedBodyPair bp; + bp.RestoreState(inStream); + uint32 num_manifolds = 0; + inStream.Read(num_manifolds); + for (uint32 j = 0; j < num_manifolds; ++j) + { + SubShapeIDPair sub_shape_key; + inStream.Read(sub_shape_key); + uint16 num_contact_points; + inStream.Read(num_contact_points); + CachedManifold cm; + cm.RestoreState(inStream); + for (uint32 k = 0; k < num_contact_points; ++k) + cm.mContactPoints[0].RestoreState(inStream); + } + } + } + + // When validating, get all existing CCD manifolds + Array all_m; + if (inStream.IsValidating()) + inReadCache.GetAllCCDManifoldsSorted(all_m); + + // Read amount of CCD manifolds + uint32 num_manifolds; + if (inStream.IsValidating()) + num_manifolds = uint32(all_m.size()); + inStream.Read(num_manifolds); + + for (uint32 j = 0; j < num_manifolds; ++j) + { + // Read key + SubShapeIDPair sub_shape_key; + if (inStream.IsValidating() && j < all_m.size()) + sub_shape_key = all_m[j]->GetKey(); + inStream.Read(sub_shape_key); + + // Check if we want to restore this contact + if (inFilter == nullptr || inFilter->ShouldRestoreContact(sub_shape_key.GetBody1ID(), sub_shape_key.GetBody2ID())) + { + // Create CCD manifold + uint64 sub_shape_key_hash = sub_shape_key.GetHash(); + MKeyValue *m_kv = Create(contact_allocator, sub_shape_key, sub_shape_key_hash, 0); + if (m_kv == nullptr) + { + // Out of cache space + success = false; + break; + } + CachedManifold &cm = m_kv->GetValue(); + cm.mFlags |= (uint16)CachedManifold::EFlags::CCDContact; + } + } + +#ifdef JPH_ENABLE_ASSERTS + // We don't finalize until the last part is restored + if (inStream.IsLastPart()) + mIsFinalized = true; +#endif + + return success; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// ContactConstraintManager +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +ContactConstraintManager::ContactConstraintManager(const PhysicsSettings &inPhysicsSettings) : + mPhysicsSettings(inPhysicsSettings) +{ +#ifdef JPH_ENABLE_ASSERTS + // For the first frame mark this empty buffer as finalized + mCache[mCacheWriteIdx ^ 1].Finalize(); +#endif +} + +ContactConstraintManager::~ContactConstraintManager() +{ + JPH_ASSERT(mConstraints == nullptr); +} + +void ContactConstraintManager::Init(uint inMaxBodyPairs, uint inMaxContactConstraints) +{ + mMaxConstraints = inMaxContactConstraints; + + // Calculate worst case cache usage + uint cached_manifolds_size = inMaxContactConstraints * (sizeof(CachedManifold) + (MaxContactPoints - 1) * sizeof(CachedContactPoint)); + + // Init the caches + mCache[0].Init(inMaxBodyPairs, inMaxContactConstraints, cached_manifolds_size); + mCache[1].Init(inMaxBodyPairs, inMaxContactConstraints, cached_manifolds_size); +} + +void ContactConstraintManager::PrepareConstraintBuffer(PhysicsUpdateContext *inContext) +{ + // Store context + mUpdateContext = inContext; + + // Allocate temporary constraint buffer + JPH_ASSERT(mConstraints == nullptr); + mConstraints = (ContactConstraint *)inContext->mTempAllocator->Allocate(mMaxConstraints * sizeof(ContactConstraint)); +} + +template +JPH_INLINE void ContactConstraintManager::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2) +{ + // Calculate scaled mass and inertia + Mat44 inv_i1; + if constexpr (Type1 == EMotionType::Dynamic) + { + const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked(); + inv_i1 = inSettings.mInvInertiaScale1 * mp1->GetInverseInertiaForRotation(inTransformBody1.GetRotation()); + } + else + { + inv_i1 = Mat44::sZero(); + } + + Mat44 inv_i2; + if constexpr (Type2 == EMotionType::Dynamic) + { + const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked(); + inv_i2 = inSettings.mInvInertiaScale2 * mp2->GetInverseInertiaForRotation(inTransformBody2.GetRotation()); + } + else + { + inv_i2 = Mat44::sZero(); + } + + // Calculate tangents + Vec3 t1, t2; + ioConstraint.GetTangents(t1, t2); + + Vec3 ws_normal = ioConstraint.GetWorldSpaceNormal(); + + // Calculate value for restitution correction + float gravity_dt_dot_normal = inGravityDeltaTime.Dot(ws_normal); + + // Setup velocity constraint properties + float min_velocity_for_restitution = mPhysicsSettings.mMinVelocityForRestitution; + for (WorldContactPoint &wcp : ioConstraint.mContactPoints) + { + RVec3 p1 = inTransformBody1 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition1); + RVec3 p2 = inTransformBody2 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition2); + wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(inDeltaTime, gravity_dt_dot_normal, inBody1, inBody2, ioConstraint.mInvMass1, ioConstraint.mInvMass2, inv_i1, inv_i2, p1, p2, ws_normal, t1, t2, inSettings, min_velocity_for_restitution); + } +} + +inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2) +{ + // Dispatch to the correct templated form + switch (inBody1.GetMotionType()) + { + case EMotionType::Dynamic: + switch (inBody2.GetMotionType()) + { + case EMotionType::Dynamic: + TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); + break; + + case EMotionType::Kinematic: + TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); + break; + + case EMotionType::Static: + TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); + break; + + default: + JPH_ASSERT(false); + break; + } + break; + + case EMotionType::Kinematic: + JPH_ASSERT(inBody2.IsDynamic()); + TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); + break; + + case EMotionType::Static: + JPH_ASSERT(inBody2.IsDynamic()); + TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); + break; + + default: + JPH_ASSERT(false); + break; + } +} + +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; + + // Swap bodies so that body 1 id < body 2 id + Body *body1, *body2; + if (inBody1.GetID() < inBody2.GetID()) + { + body1 = &inBody1; + body2 = &inBody2; + } + else + { + body1 = &inBody2; + body2 = &inBody1; + } + + // Find the cached body pair + BodyPair body_pair_key(body1->GetID(), body2->GetID()); + uint64 body_pair_hash = body_pair_key.GetHash(); + const ManifoldCache &read_cache = mCache[mCacheWriteIdx ^ 1]; + const BPKeyValue *kv = read_cache.Find(body_pair_key, body_pair_hash); + if (kv == nullptr) + return; + const CachedBodyPair &input_cbp = kv->GetValue(); + + // Get relative translation + Quat inv_r1 = body1->GetRotation().Conjugated(); + Vec3 delta_position = inv_r1 * Vec3(body2->GetCenterOfMassPosition() - body1->GetCenterOfMassPosition()); + + // Get old position delta + Vec3 old_delta_position = Vec3::sLoadFloat3Unsafe(input_cbp.mDeltaPosition); + + // Check if bodies are still roughly in the same relative position + if ((delta_position - old_delta_position).LengthSq() > mPhysicsSettings.mBodyPairCacheMaxDeltaPositionSq) + return; + + // Determine relative orientation + Quat delta_rotation = inv_r1 * body2->GetRotation(); + + // Reconstruct old quaternion delta + Quat old_delta_rotation = Quat::sLoadFloat3Unsafe(input_cbp.mDeltaRotation); + + // Check if bodies are still roughly in the same relative orientation + // The delta between 2 quaternions p and q is: p q^* = [rotation_axis * sin(angle / 2), cos(angle / 2)] + // From the W component we can extract the angle: cos(angle / 2) = px * qx + py * qy + pz * qz + pw * qw = p . q + // Since we want to abort if the rotation is smaller than -angle or bigger than angle, we can write the comparison as |p . q| < cos(angle / 2) + if (abs(delta_rotation.Dot(old_delta_rotation)) < mPhysicsSettings.mBodyPairCacheCosMaxDeltaRotationDiv2) + return; + + // The cache is valid, return that we've handled this body pair + outPairHandled = true; + + // Copy the cached body pair to this frame + ManifoldCache &write_cache = mCache[mCacheWriteIdx]; + BPKeyValue *output_bp_kv = write_cache.Create(ioContactAllocator, body_pair_key, body_pair_hash); + if (output_bp_kv == nullptr) + return; // Out of cache space + CachedBodyPair *output_cbp = &output_bp_kv->GetValue(); + memcpy(output_cbp, &input_cbp, sizeof(CachedBodyPair)); + + // If there were no contacts, we have handled the contact + if (input_cbp.mFirstCachedManifold == ManifoldMap::cInvalidHandle) + return; + + // Get body transforms + RMat44 transform_body1 = body1->GetCenterOfMassTransform(); + RMat44 transform_body2 = body2->GetCenterOfMassTransform(); + + // Get time step + float delta_time = mUpdateContext->mStepDeltaTime; + + // Calculate value for restitution correction + Vec3 gravity_dt = mUpdateContext->mPhysicsSystem->GetGravity() * delta_time; + + // Copy manifolds + uint32 output_handle = ManifoldMap::cInvalidHandle; + uint32 input_handle = input_cbp.mFirstCachedManifold; + do + { + JPH_PROFILE("Add Constraint From Cached Manifold"); + + // Find the existing manifold + const MKeyValue *input_kv = read_cache.FromHandle(input_handle); + const SubShapeIDPair &input_key = input_kv->GetKey(); + const CachedManifold &input_cm = input_kv->GetValue(); + JPH_ASSERT(input_cm.mNumContactPoints > 0); // There should be contact points in this manifold! + + // Create room for manifold in write buffer and copy data + uint64 input_hash = input_key.GetHash(); + MKeyValue *output_kv = write_cache.Create(ioContactAllocator, input_key, input_hash, input_cm.mNumContactPoints); + if (output_kv == nullptr) + break; // Out of cache space + CachedManifold *output_cm = &output_kv->GetValue(); + memcpy(output_cm, &input_cm, CachedManifold::sGetRequiredTotalSize(input_cm.mNumContactPoints)); + + // Link the object under the body pairs + output_cm->mNextWithSameBodyPair = output_handle; + output_handle = write_cache.ToHandle(output_kv); + + // Calculate default contact settings + ContactSettings settings; + settings.mCombinedFriction = mCombineFriction(*body1, input_key.GetSubShapeID1(), *body2, input_key.GetSubShapeID2()); + settings.mCombinedRestitution = mCombineRestitution(*body1, input_key.GetSubShapeID1(), *body2, input_key.GetSubShapeID2()); + settings.mIsSensor = body1->IsSensor() || body2->IsSensor(); + + // Calculate world space contact normal + Vec3 world_space_normal = transform_body2.Multiply3x3(Vec3::sLoadFloat3Unsafe(output_cm->mContactNormal)).Normalized(); + + // Call contact listener to update settings + if (mContactListener != nullptr) + { + // Convert constraint to manifold structure for callback + ContactManifold manifold; + manifold.mWorldSpaceNormal = world_space_normal; + manifold.mSubShapeID1 = input_key.GetSubShapeID1(); + manifold.mSubShapeID2 = input_key.GetSubShapeID2(); + manifold.mBaseOffset = transform_body1.GetTranslation(); + manifold.mRelativeContactPointsOn1.resize(output_cm->mNumContactPoints); + manifold.mRelativeContactPointsOn2.resize(output_cm->mNumContactPoints); + Mat44 local_transform_body2 = transform_body2.PostTranslated(-manifold.mBaseOffset).ToMat44(); + float penetration_depth = -FLT_MAX; + for (uint32 i = 0; i < output_cm->mNumContactPoints; ++i) + { + 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)); + } + manifold.mPenetrationDepth = penetration_depth; // We don't have the penetration depth anymore, estimate it + + // Notify callback + mContactListener->OnContactPersisted(*body1, *body2, manifold, settings); + } + + JPH_ASSERT(settings.mIsSensor || !(body1->IsSensor() || body2->IsSensor()), "Sensors cannot be converted into regular bodies by a contact callback!"); + if (!settings.mIsSensor // If one of the bodies is a sensor, don't actually create the constraint + && ((body1->IsDynamic() && settings.mInvMassScale1 != 0.0f) // One of the bodies must have mass to be able to create a contact constraint + || (body2->IsDynamic() && settings.mInvMassScale2 != 0.0f))) + { + // Add contact constraint in world space for the solver + uint32 constraint_idx = mNumConstraints++; + if (constraint_idx >= mMaxConstraints) + { + ioContactAllocator.mErrors |= EPhysicsUpdateError::ContactConstraintsFull; + break; + } + + // A constraint will be created + outConstraintCreated = true; + + ContactConstraint &constraint = mConstraints[constraint_idx]; + new (&constraint) ContactConstraint(); + constraint.mBody1 = body1; + constraint.mBody2 = body2; + constraint.mSortKey = input_hash; + world_space_normal.StoreFloat3(&constraint.mWorldSpaceNormal); + constraint.mCombinedFriction = settings.mCombinedFriction; + constraint.mInvMass1 = body1->GetMotionPropertiesUnchecked() != nullptr? settings.mInvMassScale1 * body1->GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f; + constraint.mInvInertiaScale1 = settings.mInvInertiaScale1; + constraint.mInvMass2 = body2->GetMotionPropertiesUnchecked() != nullptr? settings.mInvMassScale2 * body2->GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f; + constraint.mInvInertiaScale2 = settings.mInvInertiaScale2; + constraint.mContactPoints.resize(output_cm->mNumContactPoints); + for (uint32 i = 0; i < output_cm->mNumContactPoints; ++i) + { + CachedContactPoint &ccp = output_cm->mContactPoints[i]; + WorldContactPoint &wcp = constraint.mContactPoints[i]; + wcp.mNonPenetrationConstraint.SetTotalLambda(ccp.mNonPenetrationLambda); + wcp.mFrictionConstraint1.SetTotalLambda(ccp.mFrictionLambda[0]); + wcp.mFrictionConstraint2.SetTotalLambda(ccp.mFrictionLambda[1]); + wcp.mContactPoint = &ccp; + } + + JPH_DET_LOG("GetContactsFromCache: id1: " << constraint.mBody1->GetID() << " id2: " << constraint.mBody2->GetID() << " key: " << constraint.mSortKey); + + // Calculate friction and non-penetration constraint properties for all contact points + CalculateFrictionAndNonPenetrationConstraintProperties(constraint, settings, delta_time, gravity_dt, transform_body1, transform_body2, *body1, *body2); + + // Notify island builder + mUpdateContext->mIslandBuilder->LinkContact(constraint_idx, body1->GetIndexInActiveBodiesInternal(), body2->GetIndexInActiveBodiesInternal()); + + #ifdef JPH_DEBUG_RENDERER + // Draw the manifold + if (sDrawContactManifolds) + constraint.Draw(DebugRenderer::sInstance, Color::sYellow); + #endif // JPH_DEBUG_RENDERER + } + + // Mark contact as persisted so that we won't fire OnContactRemoved callbacks + input_cm.mFlags |= (uint16)CachedManifold::EFlags::ContactPersisted; + + // Fetch the next manifold + input_handle = input_cm.mNextWithSameBodyPair; + } + while (input_handle != ManifoldMap::cInvalidHandle); + output_cbp->mFirstCachedManifold = output_handle; +} + +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()) + { + body1 = &inBody1; + body2 = &inBody2; + } + else + { + body1 = &inBody2; + body2 = &inBody1; + } + + // Add an entry + BodyPair body_pair_key(body1->GetID(), body2->GetID()); + uint64 body_pair_hash = body_pair_key.GetHash(); + BPKeyValue *body_pair_kv = mCache[mCacheWriteIdx].Create(ioContactAllocator, body_pair_key, body_pair_hash); + if (body_pair_kv == nullptr) + return nullptr; // Out of cache space + CachedBodyPair *cbp = &body_pair_kv->GetValue(); + cbp->mFirstCachedManifold = ManifoldMap::cInvalidHandle; + + // Get relative translation + Quat inv_r1 = body1->GetRotation().Conjugated(); + Vec3 delta_position = inv_r1 * Vec3(body2->GetCenterOfMassPosition() - body1->GetCenterOfMassPosition()); + + // Store it + delta_position.StoreFloat3(&cbp->mDeltaPosition); + + // Determine relative orientation + Quat delta_rotation = inv_r1 * body2->GetRotation(); + + // Store it + delta_rotation.StoreFloat3(&cbp->mDeltaRotation); + + return cbp; +} + +template +bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold) +{ + // Calculate hash + SubShapeIDPair key { inBody1.GetID(), inManifold.mSubShapeID1, inBody2.GetID(), inManifold.mSubShapeID2 }; + uint64 key_hash = key.GetHash(); + + // Determine number of contact points + int num_contact_points = (int)inManifold.mRelativeContactPointsOn1.size(); + JPH_ASSERT(num_contact_points <= MaxContactPoints); + JPH_ASSERT(num_contact_points == (int)inManifold.mRelativeContactPointsOn2.size()); + + // Reserve space for new contact cache entry + // Note that for dynamic vs dynamic we always require the first body to have a lower body id to get a consistent key + // under which to look up the contact + ManifoldCache &write_cache = mCache[mCacheWriteIdx]; + MKeyValue *new_manifold_kv = write_cache.Create(ioContactAllocator, key, key_hash, num_contact_points); + if (new_manifold_kv == nullptr) + return false; // Out of cache space + CachedManifold *new_manifold = &new_manifold_kv->GetValue(); + + // Transform the world space normal to the space of body 2 (this is usually the static body) + RMat44 inverse_transform_body2 = inBody2.GetInverseCenterOfMassTransform(); + inverse_transform_body2.Multiply3x3(inManifold.mWorldSpaceNormal).Normalized().StoreFloat3(&new_manifold->mContactNormal); + + // Settings object that gets passed to the callback + ContactSettings settings; + settings.mCombinedFriction = mCombineFriction(inBody1, inManifold.mSubShapeID1, inBody2, inManifold.mSubShapeID2); + settings.mCombinedRestitution = mCombineRestitution(inBody1, inManifold.mSubShapeID1, inBody2, inManifold.mSubShapeID2); + settings.mIsSensor = inBody1.IsSensor() || inBody2.IsSensor(); + + // Get the contact points for the old cache entry + const ManifoldCache &read_cache = mCache[mCacheWriteIdx ^ 1]; + const MKeyValue *old_manifold_kv = read_cache.Find(key, key_hash); + const CachedContactPoint *ccp_start; + const CachedContactPoint *ccp_end; + if (old_manifold_kv != nullptr) + { + // Call point persisted listener + if (mContactListener != nullptr) + mContactListener->OnContactPersisted(inBody1, inBody2, inManifold, settings); + + // Fetch the contact points from the old manifold + const CachedManifold *old_manifold = &old_manifold_kv->GetValue(); + ccp_start = old_manifold->mContactPoints; + ccp_end = ccp_start + old_manifold->mNumContactPoints; + + // Mark contact as persisted so that we won't fire OnContactRemoved callbacks + old_manifold->mFlags |= (uint16)CachedManifold::EFlags::ContactPersisted; + } + else + { + // Call point added listener + if (mContactListener != nullptr) + mContactListener->OnContactAdded(inBody1, inBody2, inManifold, settings); + + // No contact points available from old manifold + ccp_start = nullptr; + ccp_end = nullptr; + } + + // Get inverse transform for body 1 + RMat44 inverse_transform_body1 = inBody1.GetInverseCenterOfMassTransform(); + + bool contact_constraint_created = false; + + // If one of the bodies is a sensor, don't actually create the constraint + JPH_ASSERT(settings.mIsSensor || !(inBody1.IsSensor() || inBody2.IsSensor()), "Sensors cannot be converted into regular bodies by a contact callback!"); + if (!settings.mIsSensor + && ((inBody1.IsDynamic() && settings.mInvMassScale1 != 0.0f) // One of the bodies must have mass to be able to create a contact constraint + || (inBody2.IsDynamic() && settings.mInvMassScale2 != 0.0f))) + { + // Add contact constraint + uint32 constraint_idx = mNumConstraints++; + if (constraint_idx >= mMaxConstraints) + { + ioContactAllocator.mErrors |= EPhysicsUpdateError::ContactConstraintsFull; + + // Manifold has been created already, we're not filling it in, so we need to reset the contact number of points. + // Note that we don't hook it up to the body pair cache so that it won't be used as a cache during the next simulation. + new_manifold->mNumContactPoints = 0; + return false; + } + + // We will create a contact constraint + contact_constraint_created = true; + + ContactConstraint &constraint = mConstraints[constraint_idx]; + new (&constraint) ContactConstraint(); + constraint.mBody1 = &inBody1; + constraint.mBody2 = &inBody2; + constraint.mSortKey = key_hash; + inManifold.mWorldSpaceNormal.StoreFloat3(&constraint.mWorldSpaceNormal); + constraint.mCombinedFriction = settings.mCombinedFriction; + constraint.mInvMass1 = inBody1.GetMotionPropertiesUnchecked() != nullptr? settings.mInvMassScale1 * inBody1.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f; + constraint.mInvInertiaScale1 = settings.mInvInertiaScale1; + constraint.mInvMass2 = inBody2.GetMotionPropertiesUnchecked() != nullptr? settings.mInvMassScale2 * inBody2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f; + constraint.mInvInertiaScale2 = settings.mInvInertiaScale2; + + JPH_DET_LOG("TemplatedAddContactConstraint: id1: " << constraint.mBody1->GetID() << " id2: " << constraint.mBody2->GetID() << " key: " << constraint.mSortKey); + + // Notify island builder + mUpdateContext->mIslandBuilder->LinkContact(constraint_idx, inBody1.GetIndexInActiveBodiesInternal(), inBody2.GetIndexInActiveBodiesInternal()); + + // Get time step + float delta_time = mUpdateContext->mStepDeltaTime; + + // Calculate value for restitution correction + float gravity_dt_dot_normal = inManifold.mWorldSpaceNormal.Dot(mUpdateContext->mPhysicsSystem->GetGravity() * delta_time); + + // Calculate scaled mass and inertia + float inv_m1; + Mat44 inv_i1; + if constexpr (Type1 == EMotionType::Dynamic) + { + const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked(); + inv_m1 = settings.mInvMassScale1 * mp1->GetInverseMass(); + inv_i1 = settings.mInvInertiaScale1 * mp1->GetInverseInertiaForRotation(inverse_transform_body1.Transposed3x3()); + } + else + { + inv_m1 = 0.0f; + inv_i1 = Mat44::sZero(); + } + + float inv_m2; + Mat44 inv_i2; + if constexpr (Type2 == EMotionType::Dynamic) + { + const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked(); + inv_m2 = settings.mInvMassScale2 * mp2->GetInverseMass(); + inv_i2 = settings.mInvInertiaScale2 * mp2->GetInverseInertiaForRotation(inverse_transform_body2.Transposed3x3()); + } + else + { + inv_m2 = 0.0f; + inv_i2 = Mat44::sZero(); + } + + // Calculate tangents + Vec3 t1, t2; + constraint.GetTangents(t1, t2); + + constraint.mContactPoints.resize(num_contact_points); + for (int i = 0; i < num_contact_points; ++i) + { + // Convert to world space and set positions + WorldContactPoint &wcp = constraint.mContactPoints[i]; + RVec3 p1_ws = inManifold.mBaseOffset + inManifold.mRelativeContactPointsOn1[i]; + RVec3 p2_ws = inManifold.mBaseOffset + inManifold.mRelativeContactPointsOn2[i]; + + // Convert to local space to the body + Vec3 p1_ls = Vec3(inverse_transform_body1 * p1_ws); + Vec3 p2_ls = Vec3(inverse_transform_body2 * p2_ws); + + // Check if we have a close contact point from last update + bool lambda_set = false; + for (const CachedContactPoint *ccp = ccp_start; ccp < ccp_end; ccp++) + if (Vec3::sLoadFloat3Unsafe(ccp->mPosition1).IsClose(p1_ls, mPhysicsSettings.mContactPointPreserveLambdaMaxDistSq) + && Vec3::sLoadFloat3Unsafe(ccp->mPosition2).IsClose(p2_ls, mPhysicsSettings.mContactPointPreserveLambdaMaxDistSq)) + { + // Get lambdas from previous frame + wcp.mNonPenetrationConstraint.SetTotalLambda(ccp->mNonPenetrationLambda); + wcp.mFrictionConstraint1.SetTotalLambda(ccp->mFrictionLambda[0]); + wcp.mFrictionConstraint2.SetTotalLambda(ccp->mFrictionLambda[1]); + lambda_set = true; + break; + } + if (!lambda_set) + { + wcp.mNonPenetrationConstraint.SetTotalLambda(0.0f); + wcp.mFrictionConstraint1.SetTotalLambda(0.0f); + wcp.mFrictionConstraint2.SetTotalLambda(0.0f); + } + + // Create new contact point + CachedContactPoint &cp = new_manifold->mContactPoints[i]; + p1_ls.StoreFloat3(&cp.mPosition1); + p2_ls.StoreFloat3(&cp.mPosition2); + wcp.mContactPoint = &cp; + + // Setup velocity constraint + wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(delta_time, gravity_dt_dot_normal, inBody1, inBody2, inv_m1, inv_m2, inv_i1, inv_i2, p1_ws, p2_ws, inManifold.mWorldSpaceNormal, t1, t2, settings, mPhysicsSettings.mMinVelocityForRestitution); + } + + #ifdef JPH_DEBUG_RENDERER + // Draw the manifold + if (sDrawContactManifolds) + constraint.Draw(DebugRenderer::sInstance, Color::sOrange); + #endif // JPH_DEBUG_RENDERER + } + else + { + // Store the contact manifold in the cache + for (int i = 0; i < num_contact_points; ++i) + { + // Convert to local space to the body + Vec3 p1 = Vec3(inverse_transform_body1 * (inManifold.mBaseOffset + inManifold.mRelativeContactPointsOn1[i])); + Vec3 p2 = Vec3(inverse_transform_body2 * (inManifold.mBaseOffset + inManifold.mRelativeContactPointsOn2[i])); + + // Create new contact point + CachedContactPoint &cp = new_manifold->mContactPoints[i]; + p1.StoreFloat3(&cp.mPosition1); + p2.StoreFloat3(&cp.mPosition2); + + // Reset contact impulses, we haven't applied any + cp.mNonPenetrationLambda = 0.0f; + cp.mFrictionLambda[0] = 0.0f; + cp.mFrictionLambda[1] = 0.0f; + } + } + + // Store cached contact point in body pair cache + CachedBodyPair *cbp = reinterpret_cast(inBodyPairHandle); + new_manifold->mNextWithSameBodyPair = cbp->mFirstCachedManifold; + cbp->mFirstCachedManifold = write_cache.ToHandle(new_manifold_kv); + + // A contact constraint was added + return contact_constraint_created; +} + +bool ContactConstraintManager::AddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold) +{ + JPH_PROFILE_FUNCTION(); + + JPH_DET_LOG("AddContactConstraint: id1: " << inBody1.GetID() << " id2: " << inBody2.GetID() + << " subshape1: " << inManifold.mSubShapeID1 << " subshape2: " << inManifold.mSubShapeID2 + << " normal: " << inManifold.mWorldSpaceNormal << " pendepth: " << inManifold.mPenetrationDepth); + + JPH_ASSERT(inManifold.mWorldSpaceNormal.IsNormalized()); + + // Swap bodies so that body 1 id < body 2 id + const ContactManifold *manifold; + Body *body1, *body2; + ContactManifold temp; + if (inBody2.GetID() < inBody1.GetID()) + { + body1 = &inBody2; + body2 = &inBody1; + temp = inManifold.SwapShapes(); + manifold = &temp; + } + else + { + body1 = &inBody1; + body2 = &inBody2; + manifold = &inManifold; + } + + // Dispatch to the correct templated form + // Note: Non-dynamic vs non-dynamic can happen in this case due to one body being a sensor, so we need to have an extended switch case here + switch (body1->GetMotionType()) + { + case EMotionType::Dynamic: + { + switch (body2->GetMotionType()) + { + case EMotionType::Dynamic: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + case EMotionType::Kinematic: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + case EMotionType::Static: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + default: + JPH_ASSERT(false); + break; + } + break; + } + + case EMotionType::Kinematic: + switch (body2->GetMotionType()) + { + case EMotionType::Dynamic: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + case EMotionType::Kinematic: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + case EMotionType::Static: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + default: + JPH_ASSERT(false); + break; + } + break; + + case EMotionType::Static: + switch (body2->GetMotionType()) + { + case EMotionType::Dynamic: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + case EMotionType::Kinematic: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + case EMotionType::Static: // Static vs static not possible + default: + JPH_ASSERT(false); + break; + } + break; + + default: + JPH_ASSERT(false); + break; + } + + return false; +} + +void ContactConstraintManager::OnCCDContactAdded(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &outSettings) +{ + JPH_ASSERT(inManifold.mWorldSpaceNormal.IsNormalized()); + + // Calculate contact settings + outSettings.mCombinedFriction = mCombineFriction(inBody1, inManifold.mSubShapeID1, inBody2, inManifold.mSubShapeID2); + outSettings.mCombinedRestitution = mCombineRestitution(inBody1, inManifold.mSubShapeID1, inBody2, inManifold.mSubShapeID2); + outSettings.mIsSensor = false; // For now, no sensors are supported during CCD + + // The remainder of this function only deals with calling contact callbacks, if there's no contact callback we also don't need to do this work + if (mContactListener != nullptr) + { + // Swap bodies so that body 1 id < body 2 id + const ContactManifold *manifold; + const Body *body1, *body2; + ContactManifold temp; + if (inBody2.GetID() < inBody1.GetID()) + { + body1 = &inBody2; + body2 = &inBody1; + temp = inManifold.SwapShapes(); + manifold = &temp; + } + else + { + body1 = &inBody1; + body2 = &inBody2; + manifold = &inManifold; + } + + // Calculate hash + SubShapeIDPair key { body1->GetID(), manifold->mSubShapeID1, body2->GetID(), manifold->mSubShapeID2 }; + uint64 key_hash = key.GetHash(); + + // Check if we already created this contact this physics update + ManifoldCache &write_cache = mCache[mCacheWriteIdx]; + MKVAndCreated new_manifold_kv = write_cache.FindOrCreate(ioContactAllocator, key, key_hash, 0); + if (new_manifold_kv.second) + { + // This contact is new for this physics update, check if previous update we already had this contact. + const ManifoldCache &read_cache = mCache[mCacheWriteIdx ^ 1]; + const MKeyValue *old_manifold_kv = read_cache.Find(key, key_hash); + if (old_manifold_kv == nullptr) + { + // New contact + mContactListener->OnContactAdded(*body1, *body2, *manifold, outSettings); + } + else + { + // Existing contact + mContactListener->OnContactPersisted(*body1, *body2, *manifold, outSettings); + + // Mark contact as persisted so that we won't fire OnContactRemoved callbacks + old_manifold_kv->GetValue().mFlags |= (uint16)CachedManifold::EFlags::ContactPersisted; + } + + // Check if the cache is full + if (new_manifold_kv.first != nullptr) + { + // We don't store any contact points in this manifold as it is not for caching impulses, we only need to know that the contact was created + CachedManifold &new_manifold = new_manifold_kv.first->GetValue(); + new_manifold.mContactNormal = { 0, 0, 0 }; + new_manifold.mFlags |= (uint16)CachedManifold::EFlags::CCDContact; + } + } + else + { + // Already found this contact this physics update. + // Note that we can trigger OnContactPersisted multiple times per physics update, but otherwise we have no way of obtaining the settings + mContactListener->OnContactPersisted(*body1, *body2, *manifold, outSettings); + } + + // If we swapped body1 and body2 we need to swap the mass scales back + if (manifold == &temp) + { + std::swap(outSettings.mInvMassScale1, outSettings.mInvMassScale2); + std::swap(outSettings.mInvInertiaScale1, outSettings.mInvInertiaScale2); + // Note we do not need to negate the relative surface velocity as it is not applied by the CCD collision constraint + } + } + + JPH_ASSERT(outSettings.mIsSensor || !(inBody1.IsSensor() || inBody2.IsSensor()), "Sensors cannot be converted into regular bodies by a contact callback!"); +} + +void ContactConstraintManager::SortContacts(uint32 *inConstraintIdxBegin, uint32 *inConstraintIdxEnd) const +{ + JPH_PROFILE_FUNCTION(); + + QuickSort(inConstraintIdxBegin, inConstraintIdxEnd, [this](uint32 inLHS, uint32 inRHS) { + const ContactConstraint &lhs = mConstraints[inLHS]; + const ContactConstraint &rhs = mConstraints[inRHS]; + + // Most of the time the sort key will be different so we sort on that + if (lhs.mSortKey != rhs.mSortKey) + return lhs.mSortKey < rhs.mSortKey; + + // If they're equal we use the IDs of body 1 to order + if (lhs.mBody1 != rhs.mBody1) + return lhs.mBody1->GetID() < rhs.mBody1->GetID(); + + // If they're still equal we use the IDs of body 2 to order + if (lhs.mBody2 != rhs.mBody2) + return lhs.mBody2->GetID() < rhs.mBody2->GetID(); + + JPH_ASSERT(inLHS == inRHS, "Hash collision, ordering will be inconsistent"); + return false; + }); +} + +void ContactConstraintManager::FinalizeContactCacheAndCallContactPointRemovedCallbacks(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds) +{ + JPH_PROFILE_FUNCTION(); + +#ifdef JPH_ENABLE_ASSERTS + // Mark cache as finalized + ManifoldCache &old_write_cache = mCache[mCacheWriteIdx]; + old_write_cache.Finalize(); + + // Check that the count of body pairs and manifolds that we tracked outside of the cache (to avoid contention on an atomic) is correct + JPH_ASSERT(old_write_cache.GetNumBodyPairs() == inExpectedNumBodyPairs); + JPH_ASSERT(old_write_cache.GetNumManifolds() == inExpectedNumManifolds); +#endif + + // Buffers are now complete, make write buffer the read buffer + mCacheWriteIdx ^= 1; + + // Get the old read cache / new write cache + ManifoldCache &old_read_cache = mCache[mCacheWriteIdx]; + + // Call the contact point removal callbacks + if (mContactListener != nullptr) + old_read_cache.ContactPointRemovedCallbacks(mContactListener); + + // We're done with the old read cache now + old_read_cache.Clear(); + + // Use the amount of contacts from the last iteration to determine the amount of buckets to use in the hash map for the next iteration + old_read_cache.Prepare(inExpectedNumBodyPairs, inExpectedNumManifolds); +} + +bool ContactConstraintManager::WereBodiesInContact(const BodyID &inBody1ID, const BodyID &inBody2ID) const +{ + // The body pair needs to be in the cache and it needs to have a manifold (otherwise it's just a record indicating that there are no collisions) + const ManifoldCache &read_cache = mCache[mCacheWriteIdx ^ 1]; + BodyPair key; + if (inBody1ID < inBody2ID) + key = BodyPair(inBody1ID, inBody2ID); + else + key = BodyPair(inBody2ID, inBody1ID); + uint64 key_hash = key.GetHash(); + const BPKeyValue *kv = read_cache.Find(key, key_hash); + return kv != nullptr && kv->GetValue().mFirstCachedManifold != ManifoldMap::cInvalidHandle; +} + +template +JPH_INLINE void ContactConstraintManager::sWarmStartConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, float inWarmStartImpulseRatio) +{ + // Calculate tangents + Vec3 t1, t2; + ioConstraint.GetTangents(t1, t2); + + Vec3 ws_normal = ioConstraint.GetWorldSpaceNormal(); + + for (WorldContactPoint &wcp : ioConstraint.mContactPoints) + { + // Warm starting: Apply impulse from last frame + if (wcp.mFrictionConstraint1.IsActive() || wcp.mFrictionConstraint2.IsActive()) + { + wcp.mFrictionConstraint1.TemplatedWarmStart(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, t1, inWarmStartImpulseRatio); + wcp.mFrictionConstraint2.TemplatedWarmStart(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, t2, inWarmStartImpulseRatio); + } + wcp.mNonPenetrationConstraint.TemplatedWarmStart(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, ws_normal, inWarmStartImpulseRatio); + } +} + +template +void ContactConstraintManager::WarmStartVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, MotionPropertiesCallback &ioCallback) +{ + JPH_PROFILE_FUNCTION(); + + for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx) + { + ContactConstraint &constraint = mConstraints[*constraint_idx]; + + // Fetch bodies + Body &body1 = *constraint.mBody1; + EMotionType motion_type1 = body1.GetMotionType(); + MotionProperties *motion_properties1 = body1.GetMotionPropertiesUnchecked(); + + Body &body2 = *constraint.mBody2; + EMotionType motion_type2 = body2.GetMotionType(); + MotionProperties *motion_properties2 = body2.GetMotionPropertiesUnchecked(); + + // Dispatch to the correct templated form + // Note: Warm starting doesn't differentiate between kinematic/static bodies so we handle both as static bodies + if (motion_type1 == EMotionType::Dynamic) + { + if (motion_type2 == EMotionType::Dynamic) + { + sWarmStartConstraint(constraint, motion_properties1, motion_properties2, inWarmStartImpulseRatio); + + ioCallback(motion_properties2); + } + else + sWarmStartConstraint(constraint, motion_properties1, motion_properties2, inWarmStartImpulseRatio); + + ioCallback(motion_properties1); + } + else + { + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + + sWarmStartConstraint(constraint, motion_properties1, motion_properties2, inWarmStartImpulseRatio); + + ioCallback(motion_properties2); + } + } +} + +// Specialize for the two body callback types +template void ContactConstraintManager::WarmStartVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, CalculateSolverSteps &ioCallback); +template void ContactConstraintManager::WarmStartVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, DummyCalculateSolverSteps &ioCallback); + +template +JPH_INLINE bool ContactConstraintManager::sSolveVelocityConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2) +{ + bool any_impulse_applied = false; + + // Calculate tangents + Vec3 t1, t2; + ioConstraint.GetTangents(t1, t2); + + // First apply all friction constraints (non-penetration is more important than friction) + for (WorldContactPoint &wcp : ioConstraint.mContactPoints) + { + // Check if friction is enabled + if (wcp.mFrictionConstraint1.IsActive() || wcp.mFrictionConstraint2.IsActive()) + { + // Calculate impulse to stop motion in tangential direction + float lambda1 = wcp.mFrictionConstraint1.TemplatedSolveVelocityConstraintGetTotalLambda(ioMotionProperties1, ioMotionProperties2, t1); + float lambda2 = wcp.mFrictionConstraint2.TemplatedSolveVelocityConstraintGetTotalLambda(ioMotionProperties1, ioMotionProperties2, t2); + float total_lambda_sq = Square(lambda1) + Square(lambda2); + + // Calculate max impulse that can be applied. Note that we're using the non-penetration impulse from the previous iteration here. + // We do this because non-penetration is more important so is solved last (the last things that are solved in an iterative solver + // contribute the most). + float max_lambda_f = ioConstraint.mCombinedFriction * wcp.mNonPenetrationConstraint.GetTotalLambda(); + + // If the total lambda that we will apply is too large, scale it back + if (total_lambda_sq > Square(max_lambda_f)) + { + float scale = max_lambda_f / sqrt(total_lambda_sq); + lambda1 *= scale; + lambda2 *= scale; + } + + // Apply the friction impulse + if (wcp.mFrictionConstraint1.TemplatedSolveVelocityConstraintApplyLambda(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, t1, lambda1)) + any_impulse_applied = true; + if (wcp.mFrictionConstraint2.TemplatedSolveVelocityConstraintApplyLambda(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, t2, lambda2)) + any_impulse_applied = true; + } + } + + Vec3 ws_normal = ioConstraint.GetWorldSpaceNormal(); + + // Then apply all non-penetration constraints + for (WorldContactPoint &wcp : ioConstraint.mContactPoints) + { + // Solve non penetration velocities + if (wcp.mNonPenetrationConstraint.TemplatedSolveVelocityConstraint(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, ws_normal, 0.0f, FLT_MAX)) + any_impulse_applied = true; + } + + return any_impulse_applied; +} + +bool ContactConstraintManager::SolveVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd) +{ + JPH_PROFILE_FUNCTION(); + + bool any_impulse_applied = false; + + for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx) + { + ContactConstraint &constraint = mConstraints[*constraint_idx]; + + // Fetch bodies + Body &body1 = *constraint.mBody1; + EMotionType motion_type1 = body1.GetMotionType(); + MotionProperties *motion_properties1 = body1.GetMotionPropertiesUnchecked(); + + Body &body2 = *constraint.mBody2; + EMotionType motion_type2 = body2.GetMotionType(); + MotionProperties *motion_properties2 = body2.GetMotionPropertiesUnchecked(); + + // Dispatch to the correct templated form + switch (motion_type1) + { + case EMotionType::Dynamic: + switch (motion_type2) + { + case EMotionType::Dynamic: + any_impulse_applied |= sSolveVelocityConstraint(constraint, motion_properties1, motion_properties2); + break; + + case EMotionType::Kinematic: + any_impulse_applied |= sSolveVelocityConstraint(constraint, motion_properties1, motion_properties2); + break; + + case EMotionType::Static: + any_impulse_applied |= sSolveVelocityConstraint(constraint, motion_properties1, motion_properties2); + break; + + default: + JPH_ASSERT(false); + break; + } + break; + + case EMotionType::Kinematic: + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + any_impulse_applied |= sSolveVelocityConstraint(constraint, motion_properties1, motion_properties2); + break; + + case EMotionType::Static: + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + any_impulse_applied |= sSolveVelocityConstraint(constraint, motion_properties1, motion_properties2); + break; + + default: + JPH_ASSERT(false); + break; + } + } + + return any_impulse_applied; +} + +void ContactConstraintManager::StoreAppliedImpulses(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd) const +{ + // Copy back total applied impulse to cache for the next frame + for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx) + { + const ContactConstraint &constraint = mConstraints[*constraint_idx]; + + for (const WorldContactPoint &wcp : constraint.mContactPoints) + { + wcp.mContactPoint->mNonPenetrationLambda = wcp.mNonPenetrationConstraint.GetTotalLambda(); + wcp.mContactPoint->mFrictionLambda[0] = wcp.mFrictionConstraint1.GetTotalLambda(); + wcp.mContactPoint->mFrictionLambda[1] = wcp.mFrictionConstraint2.GetTotalLambda(); + } + } +} + +bool ContactConstraintManager::SolvePositionConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd) +{ + JPH_PROFILE_FUNCTION(); + + bool any_impulse_applied = false; + + for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx) + { + ContactConstraint &constraint = mConstraints[*constraint_idx]; + + // Fetch bodies + Body &body1 = *constraint.mBody1; + Body &body2 = *constraint.mBody2; + + // Get transforms + RMat44 transform1 = body1.GetCenterOfMassTransform(); + RMat44 transform2 = body2.GetCenterOfMassTransform(); + + Vec3 ws_normal = constraint.GetWorldSpaceNormal(); + + for (WorldContactPoint &wcp : constraint.mContactPoints) + { + // Calculate new contact point positions in world space (the bodies may have moved) + RVec3 p1 = transform1 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition1); + RVec3 p2 = transform2 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition2); + + // Calculate separation along the normal (negative if interpenetrating) + // Allow a little penetration by default (PhysicsSettings::mPenetrationSlop) to avoid jittering between contact/no-contact which wipes out the contact cache and warm start impulses + // Clamp penetration to a max PhysicsSettings::mMaxPenetrationDistance so that we don't apply a huge impulse if we're penetrating a lot + float separation = max(Vec3(p2 - p1).Dot(ws_normal) + mPhysicsSettings.mPenetrationSlop, -mPhysicsSettings.mMaxPenetrationDistance); + + // Only enforce constraint when separation < 0 (otherwise we're apart) + if (separation < 0.0f) + { + // Update constraint properties (bodies may have moved) + wcp.CalculateNonPenetrationConstraintProperties(body1, constraint.mInvMass1, constraint.mInvInertiaScale1, body2, constraint.mInvMass2, constraint.mInvInertiaScale2, p1, p2, ws_normal); + + // Solve position errors + if (wcp.mNonPenetrationConstraint.SolvePositionConstraintWithMassOverride(body1, constraint.mInvMass1, body2, constraint.mInvMass2, ws_normal, separation, mPhysicsSettings.mBaumgarte)) + any_impulse_applied = true; + } + } + } + + return any_impulse_applied; +} + +void ContactConstraintManager::RecycleConstraintBuffer() +{ + // Reset constraint array + mNumConstraints = 0; +} + +void ContactConstraintManager::FinishConstraintBuffer() +{ + // Free constraints buffer + mUpdateContext->mTempAllocator->Free(mConstraints, mMaxConstraints * sizeof(ContactConstraint)); + mConstraints = nullptr; + mNumConstraints = 0; + + // Reset update context + mUpdateContext = nullptr; +} + +void ContactConstraintManager::SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const +{ + mCache[mCacheWriteIdx ^ 1].SaveState(inStream, inFilter); +} + +bool ContactConstraintManager::RestoreState(StateRecorder &inStream, const StateRecorderFilter *inFilter) +{ + bool success = mCache[mCacheWriteIdx].RestoreState(mCache[mCacheWriteIdx ^ 1], inStream, inFilter); + + // If this is the last part, the cache is finalized + if (inStream.IsLastPart()) + { + mCacheWriteIdx ^= 1; + mCache[mCacheWriteIdx].Clear(); + } + + return success; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h new file mode 100644 index 000000000000..3cb848cb5802 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h @@ -0,0 +1,513 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +struct PhysicsSettings; +class PhysicsUpdateContext; + +class JPH_EXPORT ContactConstraintManager : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit ContactConstraintManager(const PhysicsSettings &inPhysicsSettings); + ~ContactConstraintManager(); + + /// Initialize the system. + /// @param inMaxBodyPairs Maximum amount of body pairs to process (anything else will fall through the world), this number should generally be much higher than the max amount of contact points as there will be lots of bodies close that are not actually touching + /// @param inMaxContactConstraints Maximum amount of contact constraints to process (anything else will fall through the world) + void Init(uint inMaxBodyPairs, uint inMaxContactConstraints); + + /// Listener that is notified whenever a contact point between two bodies is added/updated/removed + void SetContactListener(ContactListener *inListener) { mContactListener = inListener; } + ContactListener * GetContactListener() const { return mContactListener; } + + /// Callback function to combine the restitution or friction of two bodies + /// Note that when merging manifolds (when PhysicsSettings::mUseManifoldReduction is true) you will only get a callback for the merged manifold. + /// It is not possible in that case to get all sub shape ID pairs that were colliding, you'll get the first encountered pair. + using CombineFunction = float (*)(const Body &inBody1, const SubShapeID &inSubShapeID1, const Body &inBody2, const SubShapeID &inSubShapeID2); + + /// Set the function that combines the friction of two bodies and returns it + /// Default method is the geometric mean: sqrt(friction1 * friction2). + void SetCombineFriction(CombineFunction inCombineFriction) { mCombineFriction = inCombineFriction; } + CombineFunction GetCombineFriction() const { return mCombineFriction; } + + /// Set the function that combines the restitution of two bodies and returns it + /// Default method is max(restitution1, restitution1) + void SetCombineRestitution(CombineFunction inCombineRestitution) { mCombineRestitution = inCombineRestitution; } + CombineFunction GetCombineRestitution() const { return mCombineRestitution; } + + /// Get the max number of contact constraints that are allowed + uint32 GetMaxConstraints() const { return mMaxConstraints; } + + /// Check with the listener if inBody1 and inBody2 could collide, returns false if not + inline ValidateResult ValidateContactPoint(const Body &inBody1, const Body &inBody2, RVec3Arg inBaseOffset, const CollideShapeResult &inCollisionResult) const + { + if (mContactListener == nullptr) + return ValidateResult::AcceptAllContactsForThisBodyPair; + + return mContactListener->OnContactValidate(inBody1, inBody2, inBaseOffset, inCollisionResult); + } + + /// Sets up the constraint buffer. Should be called before starting collision detection. + void PrepareConstraintBuffer(PhysicsUpdateContext *inContext); + + /// Max 4 contact points are needed for a stable manifold + static const int MaxContactPoints = 4; + + /// Contacts are allocated in a lock free hash map + class ContactAllocator : public LFHMAllocatorContext + { + public: + using LFHMAllocatorContext::LFHMAllocatorContext; + + uint mNumBodyPairs = 0; ///< Total number of body pairs added using this allocator + uint mNumManifolds = 0; ///< Total number of manifolds added using this allocator + EPhysicsUpdateError mErrors = EPhysicsUpdateError::None; ///< Errors reported on this allocator + }; + + /// Get a new allocator context for storing contacts. Note that you should call this once and then add multiple contacts using the context. + ContactAllocator GetContactAllocator() { return mCache[mCacheWriteIdx].GetContactAllocator(); } + + /// Check if the contact points from the previous frame are reusable and if so copy them. + /// When the cache was usable and the pair has been handled: outPairHandled = true. + /// When a contact constraint was produced: outConstraintCreated = true. + void GetContactsFromCache(ContactAllocator &ioContactAllocator, Body &inBody1, Body &inBody2, bool &outPairHandled, bool &outConstraintCreated); + + /// Handle used to keep track of the current body pair + using BodyPairHandle = void *; + + /// Create a handle for a colliding body pair so that contact constraints can be added between them. + /// Needs to be called once per body pair per frame before calling AddContactConstraint. + BodyPairHandle AddBodyPair(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2); + + /// Add a contact constraint for this frame. + /// + /// @param ioContactAllocator The allocator that reserves memory for the contacts + /// @param inBodyPair The handle for the contact cache for this body pair + /// @param inBody1 The first body that is colliding + /// @param inBody2 The second body that is colliding + /// @param inManifold The manifold that describes the collision + /// @return true if a contact constraint was created (can be false in the case of a sensor) + /// + /// This is using the approach described in 'Modeling and Solving Constraints' by Erin Catto presented at GDC 2009 (and later years with slight modifications). + /// We're using the formulas from slide 50 - 53 combined. + /// + /// Euler velocity integration: + /// + /// v1' = v1 + M^-1 P + /// + /// Impulse: + /// + /// P = J^T lambda + /// + /// Constraint force: + /// + /// lambda = -K^-1 J v1 + /// + /// Inverse effective mass: + /// + /// K = J M^-1 J^T + /// + /// Constraint equation (limits movement in 1 axis): + /// + /// C = (p2 - p1) . n + /// + /// Jacobian (for position constraint) + /// + /// J = [-n, -r1 x n, n, r2 x n] + /// + /// n = contact normal (pointing away from body 1). + /// p1, p2 = positions of collision on body 1 and 2. + /// r1, r2 = contact point relative to center of mass of body 1 and body 2 (r1 = p1 - x1, r2 = p2 - x2). + /// v1, v2 = (linear velocity, angular velocity): 6 vectors containing linear and angular velocity for body 1 and 2. + /// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2]. + bool AddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPair, Body &inBody1, Body &inBody2, const ContactManifold &inManifold); + + /// Finalizes the contact cache, the contact cache that was generated during the calls to AddContactConstraint in this update + /// will be used from now on to read from. After finalizing the contact cache, the contact removed callbacks will be called. + /// inExpectedNumBodyPairs / inExpectedNumManifolds are the amount of body pairs / manifolds found in the previous step and is + /// used to determine the amount of buckets the contact cache hash map will use in the next update. + void FinalizeContactCacheAndCallContactPointRemovedCallbacks(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds); + + /// Check if 2 bodies were in contact during the last simulation step. Since contacts are only detected between active bodies, at least one of the bodies must be active. + /// Uses the read collision cache to determine if 2 bodies are in contact. + bool WereBodiesInContact(const BodyID &inBody1ID, const BodyID &inBody2ID) const; + + /// Get the number of contact constraints that were found + uint32 GetNumConstraints() const { return min(mNumConstraints, mMaxConstraints); } + + /// Sort contact constraints deterministically + void SortContacts(uint32 *inConstraintIdxBegin, uint32 *inConstraintIdxEnd) const; + + /// Get the affected bodies for a given constraint + inline void GetAffectedBodies(uint32 inConstraintIdx, const Body *&outBody1, const Body *&outBody2) const + { + const ContactConstraint &constraint = mConstraints[inConstraintIdx]; + outBody1 = constraint.mBody1; + outBody2 = constraint.mBody2; + } + + /// Apply last frame's impulses as an initial guess for this frame's impulses + template + void WarmStartVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, MotionPropertiesCallback &ioCallback); + + /// Solve velocity constraints, when almost nothing changes this should only apply very small impulses + /// since we're warm starting with the total impulse applied in the last frame above. + /// + /// Friction wise we're using the Coulomb friction model which says that: + /// + /// |F_T| <= mu |F_N| + /// + /// Where F_T is the tangential force, F_N is the normal force and mu is the friction coefficient + /// + /// In impulse terms this becomes: + /// + /// |lambda_T| <= mu |lambda_N| + /// + /// And the constraint that needs to be applied is exactly the same as a non penetration constraint + /// except that we use a tangent instead of a normal. The tangent should point in the direction of the + /// tangential velocity of the point: + /// + /// J = [-T, -r1 x T, T, r2 x T] + /// + /// Where T is the tangent. + /// + /// See slide 42 and 43. + /// + /// Restitution is implemented as a velocity bias (see slide 41): + /// + /// b = e v_n^- + /// + /// e = the restitution coefficient, v_n^- is the normal velocity prior to the collision + /// + /// Restitution is only applied when v_n^- is large enough and the points are moving towards collision + bool SolveVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd); + + /// Save back the lambdas to the contact cache for the next warm start + void StoreAppliedImpulses(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd) const; + + /// Solve position constraints. + /// This is using the approach described in 'Modeling and Solving Constraints' by Erin Catto presented at GDC 2007. + /// On slide 78 it is suggested to split up the Baumgarte stabilization for positional drift so that it does not + /// actually add to the momentum. We combine an Euler velocity integrate + a position integrate and then discard the velocity + /// change. + /// + /// Constraint force: + /// + /// lambda = -K^-1 b + /// + /// Baumgarte stabilization: + /// + /// b = beta / dt C + /// + /// beta = baumgarte stabilization factor. + /// dt = delta time. + bool SolvePositionConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd); + + /// Recycle the constraint buffer. Should be called between collision simulation steps. + void RecycleConstraintBuffer(); + + /// Terminate the constraint buffer. Should be called after simulation ends. + void FinishConstraintBuffer(); + + /// Called by continuous collision detection to notify the contact listener that a contact was added + /// @param ioContactAllocator The allocator that reserves memory for the contacts + /// @param inBody1 The first body that is colliding + /// @param inBody2 The second body that is colliding + /// @param inManifold The manifold that describes the collision + /// @param outSettings The calculated contact settings (may be overridden by the contact listener) + void OnCCDContactAdded(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &outSettings); + +#ifdef JPH_DEBUG_RENDERER + // Drawing properties + static bool sDrawContactPoint; + static bool sDrawSupportingFaces; + static bool sDrawContactPointReduction; + static bool sDrawContactManifolds; +#endif // JPH_DEBUG_RENDERER + + /// Saving state for replay + void SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const; + + /// Restoring state for replay. Returns false when failed. + bool RestoreState(StateRecorder &inStream, const StateRecorderFilter *inFilter); + +private: + /// Local space contact point, used for caching impulses + class CachedContactPoint + { + public: + /// Saving / restoring state for replay + void SaveState(StateRecorder &inStream) const; + void RestoreState(StateRecorder &inStream); + + /// Local space positions on body 1 and 2. + /// Note: these values are read through sLoadFloat3Unsafe. + Float3 mPosition1; + Float3 mPosition2; + + /// Total applied impulse during the last update that it was used + float mNonPenetrationLambda; + Vector<2> mFrictionLambda; + }; + + static_assert(sizeof(CachedContactPoint) == 36, "Unexpected size"); + static_assert(alignof(CachedContactPoint) == 4, "Assuming 4 byte aligned"); + + /// A single cached manifold + class CachedManifold + { + public: + /// Calculate size in bytes needed beyond the size of the class to store inNumContactPoints + static int sGetRequiredExtraSize(int inNumContactPoints) { return max(0, inNumContactPoints - 1) * sizeof(CachedContactPoint); } + + /// Calculate total class size needed for storing inNumContactPoints + static int sGetRequiredTotalSize(int inNumContactPoints) { return sizeof(CachedManifold) + sGetRequiredExtraSize(inNumContactPoints); } + + /// Saving / restoring state for replay + void SaveState(StateRecorder &inStream) const; + void RestoreState(StateRecorder &inStream); + + /// Handle to next cached contact points in ManifoldCache::mCachedManifolds for the same body pair + uint32 mNextWithSameBodyPair; + + /// Contact normal in the space of 2. + /// Note: this value is read through sLoadFloat3Unsafe. + Float3 mContactNormal; + + /// Flags for this cached manifold + enum class EFlags : uint16 + { + ContactPersisted = 1, ///< If this cache entry was reused in the next simulation update + CCDContact = 2 ///< This is a cached manifold reported by continuous collision detection and was only used to create a contact callback + }; + + /// @see EFlags + mutable atomic mFlags { 0 }; + + /// Number of contact points in the array below + uint16 mNumContactPoints; + + /// Contact points that this manifold consists of + CachedContactPoint mContactPoints[1]; + }; + + static_assert(sizeof(CachedManifold) == 56, "This structure is expect to not contain any waste due to alignment"); + static_assert(alignof(CachedManifold) == 4, "Assuming 4 byte aligned"); + + /// Define a map that maps SubShapeIDPair -> manifold + using ManifoldMap = LockFreeHashMap; + using MKeyValue = ManifoldMap::KeyValue; + using MKVAndCreated = std::pair; + + /// Start of list of contact points for a particular pair of bodies + class CachedBodyPair + { + public: + /// Saving / restoring state for replay + void SaveState(StateRecorder &inStream) const; + void RestoreState(StateRecorder &inStream); + + /// Local space position difference from Body A to Body B. + /// Note: this value is read through sLoadFloat3Unsafe + Float3 mDeltaPosition; + + /// Local space rotation difference from Body A to Body B, fourth component of quaternion is not stored but is guaranteed >= 0. + /// Note: this value is read through sLoadFloat3Unsafe + Float3 mDeltaRotation; + + /// Handle to first manifold in ManifoldCache::mCachedManifolds + uint32 mFirstCachedManifold; + }; + + static_assert(sizeof(CachedBodyPair) == 28, "Unexpected size"); + static_assert(alignof(CachedBodyPair) == 4, "Assuming 4 byte aligned"); + + /// Define a map that maps BodyPair -> CachedBodyPair + using BodyPairMap = LockFreeHashMap; + using BPKeyValue = BodyPairMap::KeyValue; + + /// Holds all caches that are needed to quickly find cached body pairs / manifolds + class ManifoldCache + { + public: + /// Initialize the cache + void Init(uint inMaxBodyPairs, uint inMaxContactConstraints, uint inCachedManifoldsSize); + + /// Reset all entries from the cache + void Clear(); + + /// Prepare cache before creating new contacts. + /// inExpectedNumBodyPairs / inExpectedNumManifolds are the amount of body pairs / manifolds found in the previous step and is used to determine the amount of buckets the contact cache hash map will use. + void Prepare(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds); + + /// Get a new allocator context for storing contacts. Note that you should call this once and then add multiple contacts using the context. + ContactAllocator GetContactAllocator() { return ContactAllocator(mAllocator, cAllocatorBlockSize); } + + /// Find / create cached entry for SubShapeIDPair -> CachedManifold + const MKeyValue * Find(const SubShapeIDPair &inKey, uint64 inKeyHash) const; + MKeyValue * Create(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints); + MKVAndCreated FindOrCreate(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints); + uint32 ToHandle(const MKeyValue *inKeyValue) const; + const MKeyValue * FromHandle(uint32 inHandle) const; + + /// Find / create entry for BodyPair -> CachedBodyPair + const BPKeyValue * Find(const BodyPair &inKey, uint64 inKeyHash) const; + BPKeyValue * Create(ContactAllocator &ioContactAllocator, const BodyPair &inKey, uint64 inKeyHash); + void GetAllBodyPairsSorted(Array &outAll) const; + void GetAllManifoldsSorted(const CachedBodyPair &inBodyPair, Array &outAll) const; + void GetAllCCDManifoldsSorted(Array &outAll) const; + void ContactPointRemovedCallbacks(ContactListener *inListener); + +#ifdef JPH_ENABLE_ASSERTS + /// Get the amount of manifolds in the cache + uint GetNumManifolds() const { return mCachedManifolds.GetNumKeyValues(); } + + /// Get the amount of body pairs in the cache + uint GetNumBodyPairs() const { return mCachedBodyPairs.GetNumKeyValues(); } + + /// Before a cache is finalized you can only do Create(), after only Find() or Clear() + void Finalize(); +#endif + + /// Saving / restoring state for replay + void SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const; + bool RestoreState(const ManifoldCache &inReadCache, StateRecorder &inStream, const StateRecorderFilter *inFilter); + + private: + /// Block size used when allocating new blocks in the contact cache + static constexpr uint32 cAllocatorBlockSize = 4096; + + /// Allocator used by both mCachedManifolds and mCachedBodyPairs, this makes it more likely that a body pair and its manifolds are close in memory + LFHMAllocator mAllocator; + + /// Simple hash map for SubShapeIDPair -> CachedManifold + ManifoldMap mCachedManifolds { mAllocator }; + + /// Simple hash map for BodyPair -> CachedBodyPair + BodyPairMap mCachedBodyPairs { mAllocator }; + +#ifdef JPH_ENABLE_ASSERTS + bool mIsFinalized = false; ///< Marks if this buffer is complete +#endif + }; + + ManifoldCache mCache[2]; ///< We have one cache to read from and one to write to + int mCacheWriteIdx = 0; ///< Which cache we're currently writing to + + /// World space contact point, used for solving penetrations + class WorldContactPoint + { + public: + /// Calculate constraint properties below + void CalculateNonPenetrationConstraintProperties(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal); + + template + JPH_INLINE void TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, float inGravityDeltaTimeDotNormal, const Body &inBody1, const Body &inBody2, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, const ContactSettings &inSettings, float inMinVelocityForRestitution); + + /// The constraint parts + AxisConstraintPart mNonPenetrationConstraint; + AxisConstraintPart mFrictionConstraint1; + AxisConstraintPart mFrictionConstraint2; + + /// Contact cache + CachedContactPoint * mContactPoint; + }; + + using WorldContactPoints = StaticArray; + + /// Contact constraint class, used for solving penetrations + class ContactConstraint + { + public: + #ifdef JPH_DEBUG_RENDERER + /// Draw the state of the contact constraint + void Draw(DebugRenderer *inRenderer, ColorArg inManifoldColor) const; + #endif // JPH_DEBUG_RENDERER + + /// Convert the world space normal to a Vec3 + JPH_INLINE Vec3 GetWorldSpaceNormal() const + { + return Vec3::sLoadFloat3Unsafe(mWorldSpaceNormal); + } + + /// Get the tangents for this contact constraint + JPH_INLINE void GetTangents(Vec3 &outTangent1, Vec3 &outTangent2) const + { + Vec3 ws_normal = GetWorldSpaceNormal(); + outTangent1 = ws_normal.GetNormalizedPerpendicular(); + outTangent2 = ws_normal.Cross(outTangent1); + } + + Body * mBody1; + Body * mBody2; + uint64 mSortKey; + Float3 mWorldSpaceNormal; + float mCombinedFriction; + float mInvMass1; + float mInvInertiaScale1; + float mInvMass2; + float mInvInertiaScale2; + WorldContactPoints mContactPoints; + }; + + /// Internal helper function to calculate the friction and non-penetration constraint properties. Templated to the motion type to reduce the amount of branches and calculations. + template + JPH_INLINE void TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2); + + /// Internal helper function to calculate the friction and non-penetration constraint properties. + inline void CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2); + + /// Internal helper function to add a contact constraint. Templated to the motion type to reduce the amount of branches and calculations. + template + bool TemplatedAddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold); + + /// Internal helper function to warm start contact constraint. Templated to the motion type to reduce the amount of branches and calculations. + template + JPH_INLINE static void sWarmStartConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, float inWarmStartImpulseRatio); + + /// Internal helper function to solve a single contact constraint. Templated to the motion type to reduce the amount of branches and calculations. + template + JPH_INLINE static bool sSolveVelocityConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2); + + /// The main physics settings instance + const PhysicsSettings & mPhysicsSettings; + + /// Listener that is notified whenever a contact point between two bodies is added/updated/removed + ContactListener * mContactListener = nullptr; + + /// Functions that are used to combine friction and restitution of 2 bodies + CombineFunction mCombineFriction = [](const Body &inBody1, const SubShapeID &, const Body &inBody2, const SubShapeID &) { return sqrt(inBody1.GetFriction() * inBody2.GetFriction()); }; + CombineFunction mCombineRestitution = [](const Body &inBody1, const SubShapeID &, const Body &inBody2, const SubShapeID &) { return max(inBody1.GetRestitution(), inBody2.GetRestitution()); }; + + /// The constraints that were added this frame + ContactConstraint * mConstraints = nullptr; + uint32 mMaxConstraints = 0; + atomic mNumConstraints { 0 }; + + /// Context used for this physics update + PhysicsUpdateContext * mUpdateContext; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/DistanceConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/DistanceConstraint.cpp new file mode 100644 index 000000000000..e70bfb24ced5 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/DistanceConstraint.cpp @@ -0,0 +1,266 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +using namespace literals; + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(DistanceConstraintSettings) +{ + JPH_ADD_BASE_CLASS(DistanceConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(DistanceConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mPoint1) + JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mPoint2) + JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mMinDistance) + JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mMaxDistance) + JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(DistanceConstraintSettings, mLimitsSpringSettings.mMode, "mSpringMode") + JPH_ADD_ATTRIBUTE_WITH_ALIAS(DistanceConstraintSettings, mLimitsSpringSettings.mFrequency, "mFrequency") // Renaming attributes to stay compatible with old versions of the library + JPH_ADD_ATTRIBUTE_WITH_ALIAS(DistanceConstraintSettings, mLimitsSpringSettings.mDamping, "mDamping") +} + +void DistanceConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mPoint1); + inStream.Write(mPoint2); + inStream.Write(mMinDistance); + inStream.Write(mMaxDistance); + mLimitsSpringSettings.SaveBinaryState(inStream); +} + +void DistanceConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mPoint1); + inStream.Read(mPoint2); + inStream.Read(mMinDistance); + inStream.Read(mMaxDistance); + mLimitsSpringSettings.RestoreBinaryState(inStream); +} + +TwoBodyConstraint *DistanceConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new DistanceConstraint(inBody1, inBody2, *this); +} + +DistanceConstraint::DistanceConstraint(Body &inBody1, Body &inBody2, const DistanceConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mMinDistance(inSettings.mMinDistance), + mMaxDistance(inSettings.mMaxDistance) +{ + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPoint2); + mWorldSpacePosition1 = inSettings.mPoint1; + mWorldSpacePosition2 = inSettings.mPoint2; + } + else + { + // If properties were specified in local space, we need to calculate world space positions + mLocalSpacePosition1 = Vec3(inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inSettings.mPoint2); + mWorldSpacePosition1 = inBody1.GetCenterOfMassTransform() * inSettings.mPoint1; + mWorldSpacePosition2 = inBody2.GetCenterOfMassTransform() * inSettings.mPoint2; + } + + // Store distance we want to keep between the world space points + float distance = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1).Length(); + float min_distance, max_distance; + if (mMinDistance < 0.0f && mMaxDistance < 0.0f) + { + min_distance = max_distance = distance; + } + else + { + min_distance = mMinDistance < 0.0f? min(distance, mMaxDistance) : mMinDistance; + max_distance = mMaxDistance < 0.0f? max(distance, mMinDistance) : mMaxDistance; + } + SetDistance(min_distance, max_distance); + + // Most likely gravity is going to tear us apart (this is only used when the distance between the points = 0) + mWorldSpaceNormal = Vec3::sAxisY(); + + // Store spring settings + SetLimitsSpringSettings(inSettings.mLimitsSpringSettings); +} + +void DistanceConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +void DistanceConstraint::CalculateConstraintProperties(float inDeltaTime) +{ + // Update world space positions (the bodies may have moved) + mWorldSpacePosition1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1; + mWorldSpacePosition2 = mBody2->GetCenterOfMassTransform() * mLocalSpacePosition2; + + // Calculate world space normal + Vec3 delta = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1); + float delta_len = delta.Length(); + if (delta_len > 0.0f) + mWorldSpaceNormal = delta / delta_len; + + // Calculate points relative to body + // r1 + u = (p1 - x1) + (p2 - p1) = p2 - x1 + Vec3 r1_plus_u = Vec3(mWorldSpacePosition2 - mBody1->GetCenterOfMassPosition()); + Vec3 r2 = Vec3(mWorldSpacePosition2 - mBody2->GetCenterOfMassPosition()); + + if (mMinDistance == mMaxDistance) + { + mAxisConstraint.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMinDistance, mLimitsSpringSettings); + + // Single distance, allow constraint forces in both directions + mMinLambda = -FLT_MAX; + mMaxLambda = FLT_MAX; + } + else if (delta_len <= mMinDistance) + { + mAxisConstraint.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMinDistance, mLimitsSpringSettings); + + // Allow constraint forces to make distance bigger only + mMinLambda = 0; + mMaxLambda = FLT_MAX; + } + else if (delta_len >= mMaxDistance) + { + mAxisConstraint.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMaxDistance, mLimitsSpringSettings); + + // Allow constraint forces to make distance smaller only + mMinLambda = -FLT_MAX; + mMaxLambda = 0; + } + else + mAxisConstraint.Deactivate(); +} + +void DistanceConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + CalculateConstraintProperties(inDeltaTime); +} + +void DistanceConstraint::ResetWarmStart() +{ + mAxisConstraint.Deactivate(); +} + +void DistanceConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + mAxisConstraint.WarmStart(*mBody1, *mBody2, mWorldSpaceNormal, inWarmStartImpulseRatio); +} + +bool DistanceConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + if (mAxisConstraint.IsActive()) + return mAxisConstraint.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceNormal, mMinLambda, mMaxLambda); + else + return false; +} + +bool DistanceConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + if (mLimitsSpringSettings.mFrequency <= 0.0f) // When the spring is active, we don't need to solve the position constraint + { + float distance = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1).Dot(mWorldSpaceNormal); + + // Calculate position error + float position_error = 0.0f; + if (distance < mMinDistance) + position_error = distance - mMinDistance; + else if (distance > mMaxDistance) + position_error = distance - mMaxDistance; + + if (position_error != 0.0f) + { + // Update constraint properties (bodies may have moved) + CalculateConstraintProperties(inDeltaTime); + + return mAxisConstraint.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceNormal, position_error, inBaumgarte); + } + } + + return false; +} + +#ifdef JPH_DEBUG_RENDERER +void DistanceConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + // Draw constraint + Vec3 delta = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1); + float len = delta.Length(); + if (len < mMinDistance) + { + RVec3 real_end_pos = mWorldSpacePosition1 + (len > 0.0f? delta * mMinDistance / len : Vec3(0, len, 0)); + inRenderer->DrawLine(mWorldSpacePosition1, mWorldSpacePosition2, Color::sGreen); + inRenderer->DrawLine(mWorldSpacePosition2, real_end_pos, Color::sYellow); + } + else if (len > mMaxDistance) + { + RVec3 real_end_pos = mWorldSpacePosition1 + (len > 0.0f? delta * mMaxDistance / len : Vec3(0, len, 0)); + inRenderer->DrawLine(mWorldSpacePosition1, real_end_pos, Color::sGreen); + inRenderer->DrawLine(real_end_pos, mWorldSpacePosition2, Color::sRed); + } + else + inRenderer->DrawLine(mWorldSpacePosition1, mWorldSpacePosition2, Color::sGreen); + + // Draw constraint end points + inRenderer->DrawMarker(mWorldSpacePosition1, Color::sWhite, 0.1f); + inRenderer->DrawMarker(mWorldSpacePosition2, Color::sWhite, 0.1f); + + // Draw current length + inRenderer->DrawText3D(0.5_r * (mWorldSpacePosition1 + mWorldSpacePosition2), StringFormat("%.2f", (double)len)); +} +#endif // JPH_DEBUG_RENDERER + +void DistanceConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mAxisConstraint.SaveState(inStream); + inStream.Write(mWorldSpaceNormal); // When distance = 0, the normal is used from last frame so we need to store it +} + +void DistanceConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mAxisConstraint.RestoreState(inStream); + inStream.Read(mWorldSpaceNormal); +} + +Ref DistanceConstraint::GetConstraintSettings() const +{ + DistanceConstraintSettings *settings = new DistanceConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPoint1 = RVec3(mLocalSpacePosition1); + settings->mPoint2 = RVec3(mLocalSpacePosition2); + settings->mMinDistance = mMinDistance; + settings->mMaxDistance = mMaxDistance; + settings->mLimitsSpringSettings = mLimitsSpringSettings; + return settings; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/DistanceConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/DistanceConstraint.h new file mode 100644 index 000000000000..d237b8fd2b50 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/DistanceConstraint.h @@ -0,0 +1,120 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Distance constraint settings, used to create a distance constraint +class JPH_EXPORT DistanceConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, DistanceConstraintSettings) + +public: + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 constraint reference frame (space determined by mSpace). + /// Constraint will keep mPoint1 (a point on body 1) and mPoint2 (a point on body 2) at the same distance. + /// Note that this constraint can be used as a cheap PointConstraint by setting mPoint1 = mPoint2 (but this removes only 1 degree of freedom instead of 3). + RVec3 mPoint1 = RVec3::sZero(); + + /// Body 2 constraint reference frame (space determined by mSpace) + RVec3 mPoint2 = RVec3::sZero(); + + /// Ability to override the distance range at which the two points are kept apart. If the value is negative, it will be replaced by the distance between mPoint1 and mPoint2 (works only if mSpace is world space). + float mMinDistance = -1.0f; + float mMaxDistance = -1.0f; + + /// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back. + SpringSettings mLimitsSpringSettings; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// This constraint is a stiff spring that holds 2 points at a fixed distance from each other +class JPH_EXPORT DistanceConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct distance constraint + DistanceConstraint(Body &inBody1, Body &inBody2, const DistanceConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Distance; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override { return Mat44::sTranslation(mLocalSpacePosition1); } + virtual Mat44 GetConstraintToBody2Matrix() const override { return Mat44::sTranslation(mLocalSpacePosition2); } // Note: Incorrect rotation as we don't track the original rotation difference, should not matter though as the constraint is not limiting rotation. + + /// Update the minimum and maximum distance for the constraint + void SetDistance(float inMinDistance, float inMaxDistance) { JPH_ASSERT(inMinDistance <= inMaxDistance); mMinDistance = inMinDistance; mMaxDistance = inMaxDistance; } + float GetMinDistance() const { return mMinDistance; } + float GetMaxDistance() const { return mMaxDistance; } + + /// Update the limits spring settings + const SpringSettings & GetLimitsSpringSettings() const { return mLimitsSpringSettings; } + SpringSettings & GetLimitsSpringSettings() { return mLimitsSpringSettings; } + void SetLimitsSpringSettings(const SpringSettings &inLimitsSpringSettings) { mLimitsSpringSettings = inLimitsSpringSettings; } + + ///@name Get Lagrange multiplier from last physics update (the linear impulse applied to satisfy the constraint) + inline float GetTotalLambdaPosition() const { return mAxisConstraint.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateConstraintProperties(float inDeltaTime); + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // Min/max distance that must be kept between the world space points + float mMinDistance; + float mMaxDistance; + + // Soft constraint limits + SpringSettings mLimitsSpringSettings; + + // RUN TIME PROPERTIES FOLLOW + + // World space positions and normal + RVec3 mWorldSpacePosition1; + RVec3 mWorldSpacePosition2; + Vec3 mWorldSpaceNormal; + + // Depending on if the distance < min or distance > max we can apply forces to prevent further violations + float mMinLambda; + float mMaxLambda; + + // The constraint part + AxisConstraintPart mAxisConstraint; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/FixedConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/FixedConstraint.cpp new file mode 100644 index 000000000000..b0639851672f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/FixedConstraint.cpp @@ -0,0 +1,215 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +using namespace literals; + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(FixedConstraintSettings) +{ + JPH_ADD_BASE_CLASS(FixedConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(FixedConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAutoDetectPoint) + JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mPoint1) + JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisX1) + JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisY1) + JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mPoint2) + JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisX2) + JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisY2) +} + +void FixedConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mAutoDetectPoint); + inStream.Write(mPoint1); + inStream.Write(mAxisX1); + inStream.Write(mAxisY1); + inStream.Write(mPoint2); + inStream.Write(mAxisX2); + inStream.Write(mAxisY2); +} + +void FixedConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mAutoDetectPoint); + inStream.Read(mPoint1); + inStream.Read(mAxisX1); + inStream.Read(mAxisY1); + inStream.Read(mPoint2); + inStream.Read(mAxisX2); + inStream.Read(mAxisY2); +} + +TwoBodyConstraint *FixedConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new FixedConstraint(inBody1, inBody2, *this); +} + +FixedConstraint::FixedConstraint(Body &inBody1, Body &inBody2, const FixedConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings) +{ + // Store inverse of initial rotation from body 1 to body 2 in body 1 space + mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientationXY(inSettings.mAxisX1, inSettings.mAxisY1, inSettings.mAxisX2, inSettings.mAxisY2); + + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + if (inSettings.mAutoDetectPoint) + { + // Determine anchor point: If any of the bodies can never be dynamic use the other body as anchor point + RVec3 anchor; + if (!inBody1.CanBeKinematicOrDynamic()) + anchor = inBody2.GetCenterOfMassPosition(); + else if (!inBody2.CanBeKinematicOrDynamic()) + anchor = inBody1.GetCenterOfMassPosition(); + else + { + // Otherwise use weighted anchor point towards the lightest body + Real inv_m1 = Real(inBody1.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked()); + Real inv_m2 = Real(inBody2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked()); + Real total_inv_mass = inv_m1 + inv_m2; + if (total_inv_mass != 0.0_r) + anchor = (inv_m1 * inBody1.GetCenterOfMassPosition() + inv_m2 * inBody2.GetCenterOfMassPosition()) / (inv_m1 + inv_m2); + else + anchor = inBody1.GetCenterOfMassPosition(); + } + + // Store local positions + mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * anchor); + mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * anchor); + } + else + { + // Store local positions + mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPoint2); + } + + // Constraints were specified in world space, so we should have replaced c1 with q10^-1 c1 and c2 with q20^-1 c2 + // => r0^-1 = (q20^-1 c2) (q10^-1 c1)^1 = q20^-1 (c2 c1^-1) q10 + mInvInitialOrientation = inBody2.GetRotation().Conjugated() * mInvInitialOrientation * inBody1.GetRotation(); + } + else + { + // Store local positions + mLocalSpacePosition1 = Vec3(inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inSettings.mPoint2); + } +} + +void FixedConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +void FixedConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Calculate constraint values that don't change when the bodies don't change position + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, *mBody2, rotation2); + mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2); +} + +void FixedConstraint::ResetWarmStart() +{ + mRotationConstraintPart.Deactivate(); + mPointConstraintPart.Deactivate(); +} + +void FixedConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); +} + +bool FixedConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + // Solve rotation constraint + bool rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + // Solve position constraint + bool pos = mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + return rot || pos; +} + +bool FixedConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + // Solve rotation constraint + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation())); + bool rot = mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mInvInitialOrientation, inBaumgarte); + + // Solve position constraint + mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2); + bool pos = mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); + + return rot || pos; +} + +#ifdef JPH_DEBUG_RENDERER +void FixedConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + RMat44 com1 = mBody1->GetCenterOfMassTransform(); + RMat44 com2 = mBody2->GetCenterOfMassTransform(); + + RVec3 anchor1 = com1 * mLocalSpacePosition1; + RVec3 anchor2 = com2 * mLocalSpacePosition2; + + // Draw constraint + inRenderer->DrawLine(com1.GetTranslation(), anchor1, Color::sGreen); + inRenderer->DrawLine(com2.GetTranslation(), anchor2, Color::sBlue); +} +#endif // JPH_DEBUG_RENDERER + +void FixedConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mRotationConstraintPart.SaveState(inStream); + mPointConstraintPart.SaveState(inStream); +} + +void FixedConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mRotationConstraintPart.RestoreState(inStream); + mPointConstraintPart.RestoreState(inStream); +} + +Ref FixedConstraint::GetConstraintSettings() const +{ + FixedConstraintSettings *settings = new FixedConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPoint1 = RVec3(mLocalSpacePosition1); + settings->mAxisX1 = Vec3::sAxisX(); + settings->mAxisY1 = Vec3::sAxisY(); + settings->mPoint2 = RVec3(mLocalSpacePosition2); + settings->mAxisX2 = mInvInitialOrientation.RotateAxisX(); + settings->mAxisY2 = mInvInitialOrientation.RotateAxisY(); + return settings; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/FixedConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/FixedConstraint.h new file mode 100644 index 000000000000..114cf646e154 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/FixedConstraint.h @@ -0,0 +1,96 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Fixed constraint settings, used to create a fixed constraint +class JPH_EXPORT FixedConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, FixedConstraintSettings) + +public: + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// When mSpace is WorldSpace mPoint1 and mPoint2 can be automatically calculated based on the positions of the bodies when the constraint is created (they will be fixated in their current relative position/orientation). Set this to false if you want to supply the attachment points yourself. + bool mAutoDetectPoint = false; + + /// Body 1 constraint reference frame (space determined by mSpace) + RVec3 mPoint1 = RVec3::sZero(); + Vec3 mAxisX1 = Vec3::sAxisX(); + Vec3 mAxisY1 = Vec3::sAxisY(); + + /// Body 2 constraint reference frame (space determined by mSpace) + RVec3 mPoint2 = RVec3::sZero(); + Vec3 mAxisX2 = Vec3::sAxisX(); + Vec3 mAxisY2 = Vec3::sAxisY(); + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A fixed constraint welds two bodies together removing all degrees of freedom between them. +/// This variant uses Euler angles for the rotation constraint. +class JPH_EXPORT FixedConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + FixedConstraint(Body &inBody1, Body &inBody2, const FixedConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Fixed; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override { return Mat44::sTranslation(mLocalSpacePosition1); } + virtual Mat44 GetConstraintToBody2Matrix() const override { return Mat44::sRotationTranslation(mInvInitialOrientation, mLocalSpacePosition2); } + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline Vec3 GetTotalLambdaPosition() const { return mPointConstraintPart.GetTotalLambda(); } + inline Vec3 GetTotalLambdaRotation() const { return mRotationConstraintPart.GetTotalLambda(); } + +private: + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // Inverse of initial rotation from body 1 to body 2 in body 1 space + Quat mInvInitialOrientation; + + // RUN TIME PROPERTIES FOLLOW + + // The constraint parts + RotationEulerConstraintPart mRotationConstraintPart; + PointConstraintPart mPointConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/GearConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/GearConstraint.cpp new file mode 100644 index 000000000000..b2a7284c9edf --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/GearConstraint.cpp @@ -0,0 +1,188 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(GearConstraintSettings) +{ + JPH_ADD_BASE_CLASS(GearConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(GearConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(GearConstraintSettings, mHingeAxis1) + JPH_ADD_ATTRIBUTE(GearConstraintSettings, mHingeAxis2) + JPH_ADD_ATTRIBUTE(GearConstraintSettings, mRatio) +} + +void GearConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mHingeAxis1); + inStream.Write(mHingeAxis2); + inStream.Write(mRatio); +} + +void GearConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mHingeAxis1); + inStream.Read(mHingeAxis2); + inStream.Read(mRatio); +} + +TwoBodyConstraint *GearConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new GearConstraint(inBody1, inBody2, *this); +} + +GearConstraint::GearConstraint(Body &inBody1, Body &inBody2, const GearConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mLocalSpaceHingeAxis1(inSettings.mHingeAxis1), + mLocalSpaceHingeAxis2(inSettings.mHingeAxis2), + mRatio(inSettings.mRatio) +{ + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + mLocalSpaceHingeAxis1 = inBody1.GetInverseCenterOfMassTransform().Multiply3x3(mLocalSpaceHingeAxis1).Normalized(); + mLocalSpaceHingeAxis2 = inBody2.GetInverseCenterOfMassTransform().Multiply3x3(mLocalSpaceHingeAxis2).Normalized(); + } +} + +void GearConstraint::CalculateConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2) +{ + // Calculate world space normals + mWorldSpaceHingeAxis1 = inRotation1 * mLocalSpaceHingeAxis1; + mWorldSpaceHingeAxis2 = inRotation2 * mLocalSpaceHingeAxis2; + + mGearConstraintPart.CalculateConstraintProperties(*mBody1, mWorldSpaceHingeAxis1, *mBody2, mWorldSpaceHingeAxis2, mRatio); +} + +void GearConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Calculate constraint properties that are constant while bodies don't move + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + CalculateConstraintProperties(rotation1, rotation2); +} + +void GearConstraint::ResetWarmStart() +{ + mGearConstraintPart.Deactivate(); +} + +void GearConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mGearConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); +} + +bool GearConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + return mGearConstraintPart.SolveVelocityConstraint(*mBody1, mWorldSpaceHingeAxis1, *mBody2, mWorldSpaceHingeAxis2, mRatio); +} + +bool GearConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + if (mGear1Constraint == nullptr || mGear2Constraint == nullptr) + return false; + + float gear1rot; + if (mGear1Constraint->GetSubType() == EConstraintSubType::Hinge) + { + gear1rot = StaticCast(mGear1Constraint)->GetCurrentAngle(); + } + else + { + JPH_ASSERT(false, "Unsupported"); + return false; + } + + float gear2rot; + if (mGear2Constraint->GetSubType() == EConstraintSubType::Hinge) + { + gear2rot = StaticCast(mGear2Constraint)->GetCurrentAngle(); + } + else + { + JPH_ASSERT(false, "Unsupported"); + return false; + } + + float error = CenterAngleAroundZero(fmod(gear1rot + mRatio * gear2rot, 2.0f * JPH_PI)); + if (error == 0.0f) + return false; + + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + CalculateConstraintProperties(rotation1, rotation2); + return mGearConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, error, inBaumgarte); +} + +#ifdef JPH_DEBUG_RENDERER +void GearConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform(); + + // Draw constraint axis + inRenderer->DrawArrow(transform1.GetTranslation(), transform1 * mLocalSpaceHingeAxis1, Color::sGreen, 0.01f); + inRenderer->DrawArrow(transform2.GetTranslation(), transform2 * mLocalSpaceHingeAxis2, Color::sBlue, 0.01f); +} + +#endif // JPH_DEBUG_RENDERER + +void GearConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mGearConstraintPart.SaveState(inStream); +} + +void GearConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mGearConstraintPart.RestoreState(inStream); +} + +Ref GearConstraint::GetConstraintSettings() const +{ + GearConstraintSettings *settings = new GearConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mHingeAxis1 = mLocalSpaceHingeAxis1; + settings->mHingeAxis2 = mLocalSpaceHingeAxis2; + settings->mRatio = mRatio; + return settings; +} + +Mat44 GearConstraint::GetConstraintToBody1Matrix() const +{ + Vec3 perp = mLocalSpaceHingeAxis1.GetNormalizedPerpendicular(); + return Mat44(Vec4(mLocalSpaceHingeAxis1, 0), Vec4(perp, 0), Vec4(mLocalSpaceHingeAxis1.Cross(perp), 0), Vec4(0, 0, 0, 1)); +} + +Mat44 GearConstraint::GetConstraintToBody2Matrix() const +{ + Vec3 perp = mLocalSpaceHingeAxis2.GetNormalizedPerpendicular(); + return Mat44(Vec4(mLocalSpaceHingeAxis2, 0), Vec4(perp, 0), Vec4(mLocalSpaceHingeAxis2.Cross(perp), 0), Vec4(0, 0, 0, 1)); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/GearConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/GearConstraint.h new file mode 100644 index 000000000000..134e74c7ccdf --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/GearConstraint.h @@ -0,0 +1,116 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Gear constraint settings +class JPH_EXPORT GearConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, GearConstraintSettings) + +public: + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint. + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// Defines the ratio between the rotation of both gears + /// The ratio is defined as: Gear1Rotation(t) = -ratio * Gear2Rotation(t) + /// @param inNumTeethGear1 Number of teeth that body 1 has + /// @param inNumTeethGear2 Number of teeth that body 2 has + void SetRatio(int inNumTeethGear1, int inNumTeethGear2) + { + mRatio = float(inNumTeethGear2) / float(inNumTeethGear1); + } + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 constraint reference frame (space determined by mSpace). + Vec3 mHingeAxis1 = Vec3::sAxisX(); + + /// Body 2 constraint reference frame (space determined by mSpace) + Vec3 mHingeAxis2 = Vec3::sAxisX(); + + /// Ratio between both gears, see SetRatio. + float mRatio = 1.0f; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A gear constraint constrains the rotation of body1 to the rotation of body 2 using a gear. +/// Note that this constraint needs to be used in conjunction with a two hinge constraints. +class JPH_EXPORT GearConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct gear constraint + GearConstraint(Body &inBody1, Body &inBody2, const GearConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Gear; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override { /* Do nothing */ } + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override; + virtual Mat44 GetConstraintToBody2Matrix() const override; + + /// The constraints that constrain both gears (2 hinges), optional and used to calculate the rotation error and fix numerical drift. + void SetConstraints(const Constraint *inGear1, const Constraint *inGear2) { mGear1Constraint = inGear1; mGear2Constraint = inGear2; } + + ///@name Get Lagrange multiplier from last physics update (the angular impulse applied to satisfy the constraint) + inline float GetTotalLambda() const { return mGearConstraintPart.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2); + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space hinge axis for body 1 + Vec3 mLocalSpaceHingeAxis1; + + // Local space hinge axis for body 2 + Vec3 mLocalSpaceHingeAxis2; + + // Ratio between gear 1 and 2 + float mRatio; + + // The constraints that constrain both gears (2 hinges), optional and used to calculate the rotation error and fix numerical drift. + RefConst mGear1Constraint; + RefConst mGear2Constraint; + + // RUN TIME PROPERTIES FOLLOW + + // World space hinge axis for body 1 + Vec3 mWorldSpaceHingeAxis1; + + // World space hinge axis for body 2 + Vec3 mWorldSpaceHingeAxis2; + + // The constraint parts + GearConstraintPart mGearConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.cpp new file mode 100644 index 000000000000..824657362959 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.cpp @@ -0,0 +1,424 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(HingeConstraintSettings) +{ + JPH_ADD_BASE_CLASS(HingeConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(HingeConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mPoint1) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mHingeAxis1) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mNormalAxis1) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mPoint2) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mHingeAxis2) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mNormalAxis2) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mLimitsMin) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mLimitsMax) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mLimitsSpringSettings) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mMaxFrictionTorque) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mMotorSettings) +} + +void HingeConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mPoint1); + inStream.Write(mHingeAxis1); + inStream.Write(mNormalAxis1); + inStream.Write(mPoint2); + inStream.Write(mHingeAxis2); + inStream.Write(mNormalAxis2); + inStream.Write(mLimitsMin); + inStream.Write(mLimitsMax); + inStream.Write(mMaxFrictionTorque); + mLimitsSpringSettings.SaveBinaryState(inStream); + mMotorSettings.SaveBinaryState(inStream); +} + +void HingeConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mPoint1); + inStream.Read(mHingeAxis1); + inStream.Read(mNormalAxis1); + inStream.Read(mPoint2); + inStream.Read(mHingeAxis2); + inStream.Read(mNormalAxis2); + inStream.Read(mLimitsMin); + inStream.Read(mLimitsMax); + inStream.Read(mMaxFrictionTorque); + mLimitsSpringSettings.RestoreBinaryState(inStream); + mMotorSettings.RestoreBinaryState(inStream);} + +TwoBodyConstraint *HingeConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new HingeConstraint(inBody1, inBody2, *this); +} + +HingeConstraint::HingeConstraint(Body &inBody1, Body &inBody2, const HingeConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mMaxFrictionTorque(inSettings.mMaxFrictionTorque), + mMotorSettings(inSettings.mMotorSettings) +{ + // Store limits + JPH_ASSERT(inSettings.mLimitsMin != inSettings.mLimitsMax || inSettings.mLimitsSpringSettings.mFrequency > 0.0f, "Better use a fixed constraint in this case"); + SetLimits(inSettings.mLimitsMin, inSettings.mLimitsMax); + + // Store inverse of initial rotation from body 1 to body 2 in body 1 space + mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientationXZ(inSettings.mNormalAxis1, inSettings.mHingeAxis1, inSettings.mNormalAxis2, inSettings.mHingeAxis2); + + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + RMat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform(); + mLocalSpacePosition1 = Vec3(inv_transform1 * inSettings.mPoint1); + mLocalSpaceHingeAxis1 = inv_transform1.Multiply3x3(inSettings.mHingeAxis1).Normalized(); + mLocalSpaceNormalAxis1 = inv_transform1.Multiply3x3(inSettings.mNormalAxis1).Normalized(); + + RMat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform(); + mLocalSpacePosition2 = Vec3(inv_transform2 * inSettings.mPoint2); + mLocalSpaceHingeAxis2 = inv_transform2.Multiply3x3(inSettings.mHingeAxis2).Normalized(); + mLocalSpaceNormalAxis2 = inv_transform2.Multiply3x3(inSettings.mNormalAxis2).Normalized(); + + // Constraints were specified in world space, so we should have replaced c1 with q10^-1 c1 and c2 with q20^-1 c2 + // => r0^-1 = (q20^-1 c2) (q10^-1 c1)^1 = q20^-1 (c2 c1^-1) q10 + mInvInitialOrientation = inBody2.GetRotation().Conjugated() * mInvInitialOrientation * inBody1.GetRotation(); + } + else + { + mLocalSpacePosition1 = Vec3(inSettings.mPoint1); + mLocalSpaceHingeAxis1 = inSettings.mHingeAxis1; + mLocalSpaceNormalAxis1 = inSettings.mNormalAxis1; + + mLocalSpacePosition2 = Vec3(inSettings.mPoint2); + mLocalSpaceHingeAxis2 = inSettings.mHingeAxis2; + mLocalSpaceNormalAxis2 = inSettings.mNormalAxis2; + } + + // Store spring settings + SetLimitsSpringSettings(inSettings.mLimitsSpringSettings); +} + +void HingeConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +float HingeConstraint::GetCurrentAngle() const +{ + // See: CalculateA1AndTheta + Quat rotation1 = mBody1->GetRotation(); + Quat diff = mBody2->GetRotation() * mInvInitialOrientation * rotation1.Conjugated(); + return diff.GetRotationAngle(rotation1 * mLocalSpaceHingeAxis1); +} + +void HingeConstraint::SetLimits(float inLimitsMin, float inLimitsMax) +{ + JPH_ASSERT(inLimitsMin <= 0.0f && inLimitsMin >= -JPH_PI); + JPH_ASSERT(inLimitsMax >= 0.0f && inLimitsMax <= JPH_PI); + mLimitsMin = inLimitsMin; + mLimitsMax = inLimitsMax; + mHasLimits = mLimitsMin > -JPH_PI && mLimitsMax < JPH_PI; +} + +void HingeConstraint::CalculateA1AndTheta() +{ + if (mHasLimits || mMotorState != EMotorState::Off || mMaxFrictionTorque > 0.0f) + { + Quat rotation1 = mBody1->GetRotation(); + + // Calculate relative rotation in world space + // + // The rest rotation is: + // + // q2 = q1 r0 + // + // But the actual rotation is + // + // q2 = diff q1 r0 + // <=> diff = q2 r0^-1 q1^-1 + // + // Where: + // q1 = current rotation of body 1 + // q2 = current rotation of body 2 + // diff = relative rotation in world space + Quat diff = mBody2->GetRotation() * mInvInitialOrientation * rotation1.Conjugated(); + + // Calculate hinge axis in world space + mA1 = rotation1 * mLocalSpaceHingeAxis1; + + // Get rotation angle around the hinge axis + mTheta = diff.GetRotationAngle(mA1); + } +} + +void HingeConstraint::CalculateRotationLimitsConstraintProperties(float inDeltaTime) +{ + // Apply constraint if outside of limits + if (mHasLimits && (mTheta <= mLimitsMin || mTheta >= mLimitsMax)) + mRotationLimitsConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mA1, 0.0f, GetSmallestAngleToLimit(), mLimitsSpringSettings); + else + mRotationLimitsConstraintPart.Deactivate(); +} + +void HingeConstraint::CalculateMotorConstraintProperties(float inDeltaTime) +{ + switch (mMotorState) + { + case EMotorState::Off: + if (mMaxFrictionTorque > 0.0f) + mMotorConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, mA1); + else + mMotorConstraintPart.Deactivate(); + break; + + case EMotorState::Velocity: + mMotorConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, mA1, -mTargetAngularVelocity); + break; + + case EMotorState::Position: + if (mMotorSettings.mSpringSettings.HasStiffness()) + mMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mA1, 0.0f, CenterAngleAroundZero(mTheta - mTargetAngle), mMotorSettings.mSpringSettings); + else + mMotorConstraintPart.Deactivate(); + break; + } +} + +void HingeConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Cache constraint values that are valid until the bodies move + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2); + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, rotation1.Multiply3x3(mLocalSpaceHingeAxis1), *mBody2, rotation2, rotation2.Multiply3x3(mLocalSpaceHingeAxis2)); + CalculateA1AndTheta(); + CalculateRotationLimitsConstraintProperties(inDeltaTime); + CalculateMotorConstraintProperties(inDeltaTime); +} + +void HingeConstraint::ResetWarmStart() +{ + mMotorConstraintPart.Deactivate(); + mPointConstraintPart.Deactivate(); + mRotationConstraintPart.Deactivate(); + mRotationLimitsConstraintPart.Deactivate(); +} + +void HingeConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mMotorConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mRotationLimitsConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); +} + +float HingeConstraint::GetSmallestAngleToLimit() const +{ + float dist_to_min = CenterAngleAroundZero(mTheta - mLimitsMin); + float dist_to_max = CenterAngleAroundZero(mTheta - mLimitsMax); + return abs(dist_to_min) < abs(dist_to_max)? dist_to_min : dist_to_max; +} + +bool HingeConstraint::IsMinLimitClosest() const +{ + float dist_to_min = CenterAngleAroundZero(mTheta - mLimitsMin); + float dist_to_max = CenterAngleAroundZero(mTheta - mLimitsMax); + return abs(dist_to_min) < abs(dist_to_max); +} + +bool HingeConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + // Solve motor + bool motor = false; + if (mMotorConstraintPart.IsActive()) + { + switch (mMotorState) + { + case EMotorState::Off: + { + float max_lambda = mMaxFrictionTorque * inDeltaTime; + motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mA1, -max_lambda, max_lambda); + break; + } + + case EMotorState::Velocity: + case EMotorState::Position: + motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mA1, inDeltaTime * mMotorSettings.mMinTorqueLimit, inDeltaTime * mMotorSettings.mMaxTorqueLimit); + break; + } + } + + // Solve point constraint + bool pos = mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + // Solve rotation constraint + bool rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + // Solve rotation limits + bool limit = false; + if (mRotationLimitsConstraintPart.IsActive()) + { + float min_lambda, max_lambda; + if (mLimitsMin == mLimitsMax) + { + min_lambda = -FLT_MAX; + max_lambda = FLT_MAX; + } + else if (IsMinLimitClosest()) + { + min_lambda = 0.0f; + max_lambda = FLT_MAX; + } + else + { + min_lambda = -FLT_MAX; + max_lambda = 0.0f; + } + limit = mRotationLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mA1, min_lambda, max_lambda); + } + + return motor || pos || rot || limit; +} + +bool HingeConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + // Motor operates on velocities only, don't call SolvePositionConstraint + + // Solve point constraint + mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2); + bool pos = mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); + + // Solve rotation constraint + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); // Note that previous call to GetRotation() is out of date since the rotation has changed + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, rotation1.Multiply3x3(mLocalSpaceHingeAxis1), *mBody2, rotation2, rotation2.Multiply3x3(mLocalSpaceHingeAxis2)); + bool rot = mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); + + // Solve rotation limits + bool limit = false; + if (mHasLimits && mLimitsSpringSettings.mFrequency <= 0.0f) + { + CalculateA1AndTheta(); + CalculateRotationLimitsConstraintProperties(inDeltaTime); + if (mRotationLimitsConstraintPart.IsActive()) + limit = mRotationLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, GetSmallestAngleToLimit(), inBaumgarte); + } + + return pos || rot || limit; +} + +#ifdef JPH_DEBUG_RENDERER +void HingeConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform(); + + // Draw constraint + RVec3 constraint_pos1 = transform1 * mLocalSpacePosition1; + inRenderer->DrawMarker(constraint_pos1, Color::sRed, 0.1f); + inRenderer->DrawLine(constraint_pos1, transform1 * (mLocalSpacePosition1 + mDrawConstraintSize * mLocalSpaceHingeAxis1), Color::sRed); + + RVec3 constraint_pos2 = transform2 * mLocalSpacePosition2; + inRenderer->DrawMarker(constraint_pos2, Color::sGreen, 0.1f); + inRenderer->DrawLine(constraint_pos2, transform2 * (mLocalSpacePosition2 + mDrawConstraintSize * mLocalSpaceHingeAxis2), Color::sGreen); + inRenderer->DrawLine(constraint_pos2, transform2 * (mLocalSpacePosition2 + mDrawConstraintSize * mLocalSpaceNormalAxis2), Color::sWhite); +} + +void HingeConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const +{ + if (mHasLimits && mLimitsMax > mLimitsMin) + { + // Get constraint properties in world space + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RVec3 position1 = transform1 * mLocalSpacePosition1; + Vec3 hinge_axis1 = transform1.Multiply3x3(mLocalSpaceHingeAxis1); + Vec3 normal_axis1 = transform1.Multiply3x3(mLocalSpaceNormalAxis1); + + inRenderer->DrawPie(position1, mDrawConstraintSize, hinge_axis1, normal_axis1, mLimitsMin, mLimitsMax, Color::sPurple, DebugRenderer::ECastShadow::Off); + } +} +#endif // JPH_DEBUG_RENDERER + +void HingeConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mMotorConstraintPart.SaveState(inStream); + mRotationConstraintPart.SaveState(inStream); + mPointConstraintPart.SaveState(inStream); + mRotationLimitsConstraintPart.SaveState(inStream); + + inStream.Write(mMotorState); + inStream.Write(mTargetAngularVelocity); + inStream.Write(mTargetAngle); +} + +void HingeConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mMotorConstraintPart.RestoreState(inStream); + mRotationConstraintPart.RestoreState(inStream); + mPointConstraintPart.RestoreState(inStream); + mRotationLimitsConstraintPart.RestoreState(inStream); + + inStream.Read(mMotorState); + inStream.Read(mTargetAngularVelocity); + inStream.Read(mTargetAngle); +} + + +Ref HingeConstraint::GetConstraintSettings() const +{ + HingeConstraintSettings *settings = new HingeConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPoint1 = RVec3(mLocalSpacePosition1); + settings->mHingeAxis1 = mLocalSpaceHingeAxis1; + settings->mNormalAxis1 = mLocalSpaceNormalAxis1; + settings->mPoint2 = RVec3(mLocalSpacePosition2); + settings->mHingeAxis2 = mLocalSpaceHingeAxis2; + settings->mNormalAxis2 = mLocalSpaceNormalAxis2; + settings->mLimitsMin = mLimitsMin; + settings->mLimitsMax = mLimitsMax; + settings->mLimitsSpringSettings = mLimitsSpringSettings; + settings->mMaxFrictionTorque = mMaxFrictionTorque; + settings->mMotorSettings = mMotorSettings; + return settings; +} + +Mat44 HingeConstraint::GetConstraintToBody1Matrix() const +{ + return Mat44(Vec4(mLocalSpaceHingeAxis1, 0), Vec4(mLocalSpaceNormalAxis1, 0), Vec4(mLocalSpaceHingeAxis1.Cross(mLocalSpaceNormalAxis1), 0), Vec4(mLocalSpacePosition1, 1)); +} + +Mat44 HingeConstraint::GetConstraintToBody2Matrix() const +{ + return Mat44(Vec4(mLocalSpaceHingeAxis2, 0), Vec4(mLocalSpaceNormalAxis2, 0), Vec4(mLocalSpaceHingeAxis2.Cross(mLocalSpaceNormalAxis2), 0), Vec4(mLocalSpacePosition2, 1)); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.h new file mode 100644 index 000000000000..691bb1f5f4b5 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.h @@ -0,0 +1,200 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Hinge constraint settings, used to create a hinge constraint +class JPH_EXPORT HingeConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, HingeConstraintSettings) + +public: + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 constraint reference frame (space determined by mSpace). + /// Hinge axis is the axis where rotation is allowed. + /// When the normal axis of both bodies align in world space, the hinge angle is defined to be 0. + /// mHingeAxis1 and mNormalAxis1 should be perpendicular. mHingeAxis2 and mNormalAxis2 should also be perpendicular. + /// If you configure the joint in world space and create both bodies with a relative rotation you want to be defined as zero, + /// you can simply set mHingeAxis1 = mHingeAxis2 and mNormalAxis1 = mNormalAxis2. + RVec3 mPoint1 = RVec3::sZero(); + Vec3 mHingeAxis1 = Vec3::sAxisY(); + Vec3 mNormalAxis1 = Vec3::sAxisX(); + + /// Body 2 constraint reference frame (space determined by mSpace) + RVec3 mPoint2 = RVec3::sZero(); + Vec3 mHingeAxis2 = Vec3::sAxisY(); + Vec3 mNormalAxis2 = Vec3::sAxisX(); + + /// Rotation around the hinge axis will be limited between [mLimitsMin, mLimitsMax] where mLimitsMin e [-pi, 0] and mLimitsMax e [0, pi]. + /// Both angles are in radians. + float mLimitsMin = -JPH_PI; + float mLimitsMax = JPH_PI; + + /// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back. + SpringSettings mLimitsSpringSettings; + + /// Maximum amount of torque (N m) to apply as friction when the constraint is not powered by a motor + float mMaxFrictionTorque = 0.0f; + + /// In case the constraint is powered, this determines the motor settings around the hinge axis + MotorSettings mMotorSettings; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A hinge constraint constrains 2 bodies on a single point and allows only a single axis of rotation +class JPH_EXPORT HingeConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct hinge constraint + HingeConstraint(Body &inBody1, Body &inBody2, const HingeConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Hinge; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; + virtual void DrawConstraintLimits(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override; + virtual Mat44 GetConstraintToBody2Matrix() const override; + + /// Get the attachment point for body 1 relative to body 1 COM (transform by Body::GetCenterOfMassTransform to take to world space) + inline Vec3 GetLocalSpacePoint1() const { return mLocalSpacePosition1; } + + /// Get the attachment point for body 2 relative to body 2 COM (transform by Body::GetCenterOfMassTransform to take to world space) + inline Vec3 GetLocalSpacePoint2() const { return mLocalSpacePosition2; } + + // Local space hinge directions (transform direction by Body::GetCenterOfMassTransform to take to world space) + Vec3 GetLocalSpaceHingeAxis1() const { return mLocalSpaceHingeAxis1; } + Vec3 GetLocalSpaceHingeAxis2() const { return mLocalSpaceHingeAxis2; } + + // Local space normal directions (transform direction by Body::GetCenterOfMassTransform to take to world space) + Vec3 GetLocalSpaceNormalAxis1() const { return mLocalSpaceNormalAxis1; } + Vec3 GetLocalSpaceNormalAxis2() const { return mLocalSpaceNormalAxis2; } + + /// Get the current rotation angle from the rest position + float GetCurrentAngle() const; + + // Friction control + void SetMaxFrictionTorque(float inFrictionTorque) { mMaxFrictionTorque = inFrictionTorque; } + float GetMaxFrictionTorque() const { return mMaxFrictionTorque; } + + // Motor settings + MotorSettings & GetMotorSettings() { return mMotorSettings; } + const MotorSettings & GetMotorSettings() const { return mMotorSettings; } + + // Motor controls + void SetMotorState(EMotorState inState) { JPH_ASSERT(inState == EMotorState::Off || mMotorSettings.IsValid()); mMotorState = inState; } + EMotorState GetMotorState() const { return mMotorState; } + void SetTargetAngularVelocity(float inAngularVelocity) { mTargetAngularVelocity = inAngularVelocity; } ///< rad/s + float GetTargetAngularVelocity() const { return mTargetAngularVelocity; } + void SetTargetAngle(float inAngle) { mTargetAngle = mHasLimits? Clamp(inAngle, mLimitsMin, mLimitsMax) : inAngle; } ///< rad + float GetTargetAngle() const { return mTargetAngle; } + + /// Update the rotation limits of the hinge, value in radians (see HingeConstraintSettings) + void SetLimits(float inLimitsMin, float inLimitsMax); + float GetLimitsMin() const { return mLimitsMin; } + float GetLimitsMax() const { return mLimitsMax; } + bool HasLimits() const { return mHasLimits; } + + /// Update the limits spring settings + const SpringSettings & GetLimitsSpringSettings() const { return mLimitsSpringSettings; } + SpringSettings & GetLimitsSpringSettings() { return mLimitsSpringSettings; } + void SetLimitsSpringSettings(const SpringSettings &inLimitsSpringSettings) { mLimitsSpringSettings = inLimitsSpringSettings; } + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline Vec3 GetTotalLambdaPosition() const { return mPointConstraintPart.GetTotalLambda(); } + inline Vector<2> GetTotalLambdaRotation() const { return mRotationConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaRotationLimits() const { return mRotationLimitsConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaMotor() const { return mMotorConstraintPart.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateA1AndTheta(); + void CalculateRotationLimitsConstraintProperties(float inDeltaTime); + void CalculateMotorConstraintProperties(float inDeltaTime); + inline float GetSmallestAngleToLimit() const; + inline bool IsMinLimitClosest() const; + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // Local space hinge directions + Vec3 mLocalSpaceHingeAxis1; + Vec3 mLocalSpaceHingeAxis2; + + // Local space normal direction (direction relative to which to draw constraint limits) + Vec3 mLocalSpaceNormalAxis1; + Vec3 mLocalSpaceNormalAxis2; + + // Inverse of initial relative orientation between bodies (which defines hinge angle = 0) + Quat mInvInitialOrientation; + + // Hinge limits + bool mHasLimits; + float mLimitsMin; + float mLimitsMax; + + // Soft constraint limits + SpringSettings mLimitsSpringSettings; + + // Friction + float mMaxFrictionTorque; + + // Motor controls + MotorSettings mMotorSettings; + EMotorState mMotorState = EMotorState::Off; + float mTargetAngularVelocity = 0.0f; + float mTargetAngle = 0.0f; + + // RUN TIME PROPERTIES FOLLOW + + // Current rotation around the hinge axis + float mTheta = 0.0f; + + // World space hinge axis for body 1 + Vec3 mA1; + + // The constraint parts + PointConstraintPart mPointConstraintPart; + HingeRotationConstraintPart mRotationConstraintPart; + AngleConstraintPart mRotationLimitsConstraintPart; + AngleConstraintPart mMotorConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/MotorSettings.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/MotorSettings.cpp new file mode 100644 index 000000000000..e4daecdd7d52 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/MotorSettings.cpp @@ -0,0 +1,43 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(MotorSettings) +{ + JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(MotorSettings, mSpringSettings.mMode, "mSpringMode") + JPH_ADD_ATTRIBUTE_WITH_ALIAS(MotorSettings, mSpringSettings.mFrequency, "mFrequency") // Renaming attributes to stay compatible with old versions of the library + JPH_ADD_ATTRIBUTE_WITH_ALIAS(MotorSettings, mSpringSettings.mDamping, "mDamping") + JPH_ADD_ATTRIBUTE(MotorSettings, mMinForceLimit) + JPH_ADD_ATTRIBUTE(MotorSettings, mMaxForceLimit) + JPH_ADD_ATTRIBUTE(MotorSettings, mMinTorqueLimit) + JPH_ADD_ATTRIBUTE(MotorSettings, mMaxTorqueLimit) +} + +void MotorSettings::SaveBinaryState(StreamOut &inStream) const +{ + mSpringSettings.SaveBinaryState(inStream); + inStream.Write(mMinForceLimit); + inStream.Write(mMaxForceLimit); + inStream.Write(mMinTorqueLimit); + inStream.Write(mMaxTorqueLimit); +} + +void MotorSettings::RestoreBinaryState(StreamIn &inStream) +{ + mSpringSettings.RestoreBinaryState(inStream); + inStream.Read(mMinForceLimit); + inStream.Read(mMaxForceLimit); + inStream.Read(mMinTorqueLimit); + inStream.Read(mMaxTorqueLimit); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/MotorSettings.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/MotorSettings.h new file mode 100644 index 000000000000..601fa3dc3f9b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/MotorSettings.h @@ -0,0 +1,66 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; + +enum class EMotorState +{ + Off, ///< Motor is off + Velocity, ///< Motor will drive to target velocity + Position ///< Motor will drive to target position +}; + +/// Class that contains the settings for a constraint motor. +/// See the main page of the API documentation for more information on how to configure a motor. +class JPH_EXPORT MotorSettings +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, MotorSettings) + +public: + /// Constructor + MotorSettings() = default; + MotorSettings(const MotorSettings &) = default; + MotorSettings & operator = (const MotorSettings &) = default; + MotorSettings(float inFrequency, float inDamping) : mSpringSettings(ESpringMode::FrequencyAndDamping, inFrequency, inDamping) { JPH_ASSERT(IsValid()); } + MotorSettings(float inFrequency, float inDamping, float inForceLimit, float inTorqueLimit) : mSpringSettings(ESpringMode::FrequencyAndDamping, inFrequency, inDamping), mMinForceLimit(-inForceLimit), mMaxForceLimit(inForceLimit), mMinTorqueLimit(-inTorqueLimit), mMaxTorqueLimit(inTorqueLimit) { JPH_ASSERT(IsValid()); } + + /// Set asymmetric force limits + void SetForceLimits(float inMin, float inMax) { JPH_ASSERT(inMin <= inMax); mMinForceLimit = inMin; mMaxForceLimit = inMax; } + + /// Set asymmetric torque limits + void SetTorqueLimits(float inMin, float inMax) { JPH_ASSERT(inMin <= inMax); mMinTorqueLimit = inMin; mMaxTorqueLimit = inMax; } + + /// Set symmetric force limits + void SetForceLimit(float inLimit) { mMinForceLimit = -inLimit; mMaxForceLimit = inLimit; } + + /// Set symmetric torque limits + void SetTorqueLimit(float inLimit) { mMinTorqueLimit = -inLimit; mMaxTorqueLimit = inLimit; } + + /// Check if settings are valid + bool IsValid() const { return mSpringSettings.mFrequency >= 0.0f && mSpringSettings.mDamping >= 0.0f && mMinForceLimit <= mMaxForceLimit && mMinTorqueLimit <= mMaxTorqueLimit; } + + /// Saves the contents of the motor settings in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restores contents from the binary stream inStream. + void RestoreBinaryState(StreamIn &inStream); + + // Settings + SpringSettings mSpringSettings { ESpringMode::FrequencyAndDamping, 2.0f, 1.0f }; ///< Settings for the spring that is used to drive to the position target (not used when motor is a velocity motor). + float mMinForceLimit = -FLT_MAX; ///< Minimum force to apply in case of a linear constraint (N). Usually this is -mMaxForceLimit unless you want a motor that can e.g. push but not pull. Not used when motor is an angular motor. + float mMaxForceLimit = FLT_MAX; ///< Maximum force to apply in case of a linear constraint (N). Not used when motor is an angular motor. + float mMinTorqueLimit = -FLT_MAX; ///< Minimum torque to apply in case of a angular constraint (N m). Usually this is -mMaxTorqueLimit unless you want a motor that can e.g. push but not pull. Not used when motor is a position motor. + float mMaxTorqueLimit = FLT_MAX; ///< Maximum torque to apply in case of a angular constraint (N m). Not used when motor is a position motor. +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraint.cpp new file mode 100644 index 000000000000..e5b6eb7da037 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraint.cpp @@ -0,0 +1,458 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PathConstraintSettings) +{ + JPH_ADD_BASE_CLASS(PathConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPath) + JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPathPosition) + JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPathRotation) + JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPathFraction) + JPH_ADD_ATTRIBUTE(PathConstraintSettings, mMaxFrictionForce) + JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPositionMotorSettings) + JPH_ADD_ENUM_ATTRIBUTE(PathConstraintSettings, mRotationConstraintType) +} + +void PathConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + mPath->SaveBinaryState(inStream); + inStream.Write(mPathPosition); + inStream.Write(mPathRotation); + inStream.Write(mPathFraction); + inStream.Write(mMaxFrictionForce); + inStream.Write(mRotationConstraintType); + mPositionMotorSettings.SaveBinaryState(inStream); +} + +void PathConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + PathConstraintPath::PathResult result = PathConstraintPath::sRestoreFromBinaryState(inStream); + if (!result.HasError()) + mPath = result.Get(); + inStream.Read(mPathPosition); + inStream.Read(mPathRotation); + inStream.Read(mPathFraction); + inStream.Read(mMaxFrictionForce); + inStream.Read(mRotationConstraintType); + mPositionMotorSettings.RestoreBinaryState(inStream); +} + +TwoBodyConstraint *PathConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new PathConstraint(inBody1, inBody2, *this); +} + +PathConstraint::PathConstraint(Body &inBody1, Body &inBody2, const PathConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mRotationConstraintType(inSettings.mRotationConstraintType), + mMaxFrictionForce(inSettings.mMaxFrictionForce), + mPositionMotorSettings(inSettings.mPositionMotorSettings) +{ + // Calculate transform that takes us from the path start to center of mass space of body 1 + mPathToBody1 = Mat44::sRotationTranslation(inSettings.mPathRotation, inSettings.mPathPosition - inBody1.GetShape()->GetCenterOfMass()); + + SetPath(inSettings.mPath, inSettings.mPathFraction); +} + +void PathConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mPathToBody1.SetTranslation(mPathToBody1.GetTranslation() - inDeltaCOM); + else if (mBody2->GetID() == inBodyID) + mPathToBody2.SetTranslation(mPathToBody2.GetTranslation() - inDeltaCOM); +} + +void PathConstraint::SetPath(const PathConstraintPath *inPath, float inPathFraction) +{ + mPath = inPath; + mPathFraction = inPathFraction; + + if (mPath != nullptr) + { + // Get the point on the path for this fraction + Vec3 path_point, path_tangent, path_normal, path_binormal; + mPath->GetPointOnPath(mPathFraction, path_point, path_tangent, path_normal, path_binormal); + + // Construct the matrix that takes us from the closest point on the path to body 2 center of mass space + Mat44 closest_point_to_path(Vec4(path_tangent, 0), Vec4(path_binormal, 0), Vec4(path_normal, 0), Vec4(path_point, 1)); + Mat44 cp_to_body1 = mPathToBody1 * closest_point_to_path; + mPathToBody2 = (mBody2->GetInverseCenterOfMassTransform() * mBody1->GetCenterOfMassTransform()).ToMat44() * cp_to_body1; + + // Calculate initial orientation + if (mRotationConstraintType == EPathRotationConstraintType::FullyConstrained) + mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientation(*mBody1, *mBody2); + } +} + +void PathConstraint::CalculateConstraintProperties(float inDeltaTime) +{ + // Get transforms of body 1 and 2 + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform(); + + // Get the transform of the path transform as seen from body 1 in world space + RMat44 path_to_world_1 = transform1 * mPathToBody1; + + // Get the transform of from the point on path that body 2 is attached to in world space + RMat44 path_to_world_2 = transform2 * mPathToBody2; + + // Calculate new closest point on path + RVec3 position2 = path_to_world_2.GetTranslation(); + Vec3 position2_local_to_path = Vec3(path_to_world_1.InversedRotationTranslation() * position2); + mPathFraction = mPath->GetClosestPoint(position2_local_to_path, mPathFraction); + + // Get the point on the path for this fraction + Vec3 path_point, path_tangent, path_normal, path_binormal; + mPath->GetPointOnPath(mPathFraction, path_point, path_tangent, path_normal, path_binormal); + + // Calculate R1 and R2 + RVec3 path_point_ws = path_to_world_1 * path_point; + mR1 = Vec3(path_point_ws - mBody1->GetCenterOfMassPosition()); + mR2 = Vec3(position2 - mBody2->GetCenterOfMassPosition()); + + // Calculate U = X2 + R2 - X1 - R1 + mU = Vec3(position2 - path_point_ws); + + // Calculate world space normals + mPathNormal = path_to_world_1.Multiply3x3(path_normal); + mPathBinormal = path_to_world_1.Multiply3x3(path_binormal); + + // Calculate slide axis + mPathTangent = path_to_world_1.Multiply3x3(path_tangent); + + // Prepare constraint part for position constraint to slide along the path + mPositionConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mR1 + mU, *mBody2, transform2.GetRotation(), mR2, mPathNormal, mPathBinormal); + + // Check if closest point is on the boundary of the path and if so apply limit + if (!mPath->IsLooping() && (mPathFraction <= 0.0f || mPathFraction >= mPath->GetPathMaxFraction())) + mPositionLimitsConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mPathTangent); + else + mPositionLimitsConstraintPart.Deactivate(); + + // Prepare rotation constraint part + switch (mRotationConstraintType) + { + case EPathRotationConstraintType::Free: + // No rotational limits + break; + + case EPathRotationConstraintType::ConstrainAroundTangent: + mHingeConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mPathTangent, *mBody2, transform2.GetRotation(), path_to_world_2.GetAxisX()); + break; + + case EPathRotationConstraintType::ConstrainAroundNormal: + mHingeConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mPathNormal, *mBody2, transform2.GetRotation(), path_to_world_2.GetAxisZ()); + break; + + case EPathRotationConstraintType::ConstrainAroundBinormal: + mHingeConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mPathBinormal, *mBody2, transform2.GetRotation(), path_to_world_2.GetAxisY()); + break; + + case EPathRotationConstraintType::ConstrainToPath: + // We need to calculate the inverse of the rotation from body 1 to body 2 for the current path position (see: RotationEulerConstraintPart::sGetInvInitialOrientation) + // RotationBody2 = RotationBody1 * InitialOrientation <=> InitialOrientation^-1 = RotationBody2^-1 * RotationBody1 + // We can express RotationBody2 in terms of RotationBody1: RotationBody2 = RotationBody1 * PathToBody1 * RotationClosestPointOnPath * PathToBody2^-1 + // Combining these two: InitialOrientation^-1 = PathToBody2 * (PathToBody1 * RotationClosestPointOnPath)^-1 + mInvInitialOrientation = mPathToBody2.Multiply3x3RightTransposed(mPathToBody1.Multiply3x3(Mat44(Vec4(path_tangent, 0), Vec4(path_binormal, 0), Vec4(path_normal, 0), Vec4::sZero()))).GetQuaternion(); + [[fallthrough]]; + + case EPathRotationConstraintType::FullyConstrained: + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), *mBody2, transform2.GetRotation()); + break; + } + + // Motor properties + switch (mPositionMotorState) + { + case EMotorState::Off: + if (mMaxFrictionForce > 0.0f) + mPositionMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mPathTangent); + else + mPositionMotorConstraintPart.Deactivate(); + break; + + case EMotorState::Velocity: + mPositionMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mPathTangent, -mTargetVelocity); + break; + + case EMotorState::Position: + if (mPositionMotorSettings.mSpringSettings.HasStiffness()) + { + // Calculate constraint value to drive to + float c; + if (mPath->IsLooping()) + { + float max_fraction = mPath->GetPathMaxFraction(); + c = fmod(mPathFraction - mTargetPathFraction, max_fraction); + float half_max_fraction = 0.5f * max_fraction; + if (c > half_max_fraction) + c -= max_fraction; + else if (c < -half_max_fraction) + c += max_fraction; + } + else + c = mPathFraction - mTargetPathFraction; + mPositionMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mPathTangent, 0.0f, c, mPositionMotorSettings.mSpringSettings); + } + else + mPositionMotorConstraintPart.Deactivate(); + break; + } +} + +void PathConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + CalculateConstraintProperties(inDeltaTime); +} + +void PathConstraint::ResetWarmStart() +{ + mPositionMotorConstraintPart.Deactivate(); + mPositionConstraintPart.Deactivate(); + mPositionLimitsConstraintPart.Deactivate(); + mHingeConstraintPart.Deactivate(); + mRotationConstraintPart.Deactivate(); +} + +void PathConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mPositionMotorConstraintPart.WarmStart(*mBody1, *mBody2, mPathTangent, inWarmStartImpulseRatio); + mPositionConstraintPart.WarmStart(*mBody1, *mBody2, mPathNormal, mPathBinormal, inWarmStartImpulseRatio); + mPositionLimitsConstraintPart.WarmStart(*mBody1, *mBody2, mPathTangent, inWarmStartImpulseRatio); + + switch (mRotationConstraintType) + { + case EPathRotationConstraintType::Free: + // No rotational limits + break; + + case EPathRotationConstraintType::ConstrainAroundTangent: + case EPathRotationConstraintType::ConstrainAroundNormal: + case EPathRotationConstraintType::ConstrainAroundBinormal: + mHingeConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + break; + + case EPathRotationConstraintType::ConstrainToPath: + case EPathRotationConstraintType::FullyConstrained: + mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + break; + } +} + +bool PathConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + // Solve motor + bool motor = false; + if (mPositionMotorConstraintPart.IsActive()) + { + switch (mPositionMotorState) + { + case EMotorState::Off: + { + float max_lambda = mMaxFrictionForce * inDeltaTime; + motor = mPositionMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, -max_lambda, max_lambda); + break; + } + + case EMotorState::Velocity: + case EMotorState::Position: + motor = mPositionMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, inDeltaTime * mPositionMotorSettings.mMinForceLimit, inDeltaTime * mPositionMotorSettings.mMaxForceLimit); + break; + } + } + + // Solve position constraint along 2 axis + bool pos = mPositionConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathNormal, mPathBinormal); + + // Solve limits along path axis + bool limit = false; + if (mPositionLimitsConstraintPart.IsActive()) + { + if (mPathFraction <= 0.0f) + limit = mPositionLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, 0, FLT_MAX); + else + { + JPH_ASSERT(mPathFraction >= mPath->GetPathMaxFraction()); + limit = mPositionLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, -FLT_MAX, 0); + } + } + + // Solve rotational constraint + // Note, this is not entirely correct, we should apply a velocity constraint so that the body will actually follow the path + // by looking at the derivative of the tangent, normal or binormal but we don't. This means the position constraint solver + // will need to correct the orientation error that builds up, which in turn means that the simulation is not physically correct. + bool rot = false; + switch (mRotationConstraintType) + { + case EPathRotationConstraintType::Free: + // No rotational limits + break; + + case EPathRotationConstraintType::ConstrainAroundTangent: + case EPathRotationConstraintType::ConstrainAroundNormal: + case EPathRotationConstraintType::ConstrainAroundBinormal: + rot = mHingeConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + break; + + case EPathRotationConstraintType::ConstrainToPath: + case EPathRotationConstraintType::FullyConstrained: + rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + break; + } + + return motor || pos || limit || rot; +} + +bool PathConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + // Update constraint properties (bodies may have moved) + CalculateConstraintProperties(inDeltaTime); + + // Solve position constraint along 2 axis + bool pos = mPositionConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mU, mPathNormal, mPathBinormal, inBaumgarte); + + // Solve limits along path axis + bool limit = false; + if (mPositionLimitsConstraintPart.IsActive()) + { + if (mPathFraction <= 0.0f) + limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mPathTangent, mU.Dot(mPathTangent), inBaumgarte); + else + { + JPH_ASSERT(mPathFraction >= mPath->GetPathMaxFraction()); + limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mPathTangent, mU.Dot(mPathTangent), inBaumgarte); + } + } + + // Solve rotational constraint + bool rot = false; + switch (mRotationConstraintType) + { + case EPathRotationConstraintType::Free: + // No rotational limits + break; + + case EPathRotationConstraintType::ConstrainAroundTangent: + case EPathRotationConstraintType::ConstrainAroundNormal: + case EPathRotationConstraintType::ConstrainAroundBinormal: + rot = mHingeConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); + break; + + case EPathRotationConstraintType::ConstrainToPath: + case EPathRotationConstraintType::FullyConstrained: + rot = mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mInvInitialOrientation, inBaumgarte); + break; + } + + return pos || limit || rot; +} + +#ifdef JPH_DEBUG_RENDERER +void PathConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + if (mPath != nullptr) + { + // Draw the path in world space + RMat44 path_to_world = mBody1->GetCenterOfMassTransform() * mPathToBody1; + mPath->DrawPath(inRenderer, path_to_world); + + // Draw anchor point of both bodies in world space + RVec3 x1 = mBody1->GetCenterOfMassPosition() + mR1; + RVec3 x2 = mBody2->GetCenterOfMassPosition() + mR2; + inRenderer->DrawMarker(x1, Color::sYellow, 0.1f); + inRenderer->DrawMarker(x2, Color::sYellow, 0.1f); + inRenderer->DrawArrow(x1, x1 + mPathTangent, Color::sBlue, 0.1f); + inRenderer->DrawArrow(x1, x1 + mPathNormal, Color::sRed, 0.1f); + inRenderer->DrawArrow(x1, x1 + mPathBinormal, Color::sGreen, 0.1f); + inRenderer->DrawText3D(x1, StringFormat("%.1f", (double)mPathFraction)); + + // Draw motor + switch (mPositionMotorState) + { + case EMotorState::Position: + { + // Draw target marker + Vec3 position, tangent, normal, binormal; + mPath->GetPointOnPath(mTargetPathFraction, position, tangent, normal, binormal); + inRenderer->DrawMarker(path_to_world * position, Color::sYellow, 1.0f); + break; + } + + case EMotorState::Velocity: + { + RVec3 position = mBody2->GetCenterOfMassPosition() + mR2; + inRenderer->DrawArrow(position, position + mPathTangent * mTargetVelocity, Color::sRed, 0.1f); + break; + } + + case EMotorState::Off: + break; + } + } +} +#endif // JPH_DEBUG_RENDERER + +void PathConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mPositionConstraintPart.SaveState(inStream); + mPositionLimitsConstraintPart.SaveState(inStream); + mPositionMotorConstraintPart.SaveState(inStream); + mHingeConstraintPart.SaveState(inStream); + mRotationConstraintPart.SaveState(inStream); + + inStream.Write(mMaxFrictionForce); + inStream.Write(mPositionMotorSettings); + inStream.Write(mPositionMotorState); + inStream.Write(mTargetVelocity); + inStream.Write(mTargetPathFraction); + inStream.Write(mPathFraction); +} + +void PathConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mPositionConstraintPart.RestoreState(inStream); + mPositionLimitsConstraintPart.RestoreState(inStream); + mPositionMotorConstraintPart.RestoreState(inStream); + mHingeConstraintPart.RestoreState(inStream); + mRotationConstraintPart.RestoreState(inStream); + + inStream.Read(mMaxFrictionForce); + inStream.Read(mPositionMotorSettings); + inStream.Read(mPositionMotorState); + inStream.Read(mTargetVelocity); + inStream.Read(mTargetPathFraction); + inStream.Read(mPathFraction); +} + +Ref PathConstraint::GetConstraintSettings() const +{ + JPH_ASSERT(false); // Not implemented yet + return nullptr; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraint.h new file mode 100644 index 000000000000..3d15438bc9f8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraint.h @@ -0,0 +1,191 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_SUPPRESS_WARNING_PUSH +JPH_GCC_SUPPRESS_WARNING("-Wshadow") // GCC complains about the 'Free' value conflicting with the 'Free' method + +/// How to constrain the rotation of the body to a PathConstraint +enum class EPathRotationConstraintType +{ + Free, ///< Do not constrain the rotation of the body at all + ConstrainAroundTangent, ///< Only allow rotation around the tangent vector (following the path) + ConstrainAroundNormal, ///< Only allow rotation around the normal vector (perpendicular to the path) + ConstrainAroundBinormal, ///< Only allow rotation around the binormal vector (perpendicular to the path) + ConstrainToPath, ///< Fully constrain the rotation of body 2 to the path (following the tangent and normal of the path) + FullyConstrained, ///< Fully constrain the rotation of the body 2 to the rotation of body 1 +}; + +JPH_SUPPRESS_WARNING_POP + +/// Path constraint settings, used to constrain the degrees of freedom between two bodies to a path +/// +/// The requirements of the path are that: +/// * Tangent, normal and bi-normal form an orthonormal basis with: tangent cross bi-normal = normal +/// * The path points along the tangent vector +/// * The path is continuous so doesn't contain any sharp corners +/// +/// The reason for all this is that the constraint acts like a slider constraint with the sliding axis being the tangent vector (the assumption here is that delta time will be small enough so that the path is linear for that delta time). +class JPH_EXPORT PathConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PathConstraintSettings) + +public: + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// The path that constrains the two bodies + RefConst mPath; + + /// The position of the path start relative to world transform of body 1 + Vec3 mPathPosition = Vec3::sZero(); + + /// The rotation of the path start relative to world transform of body 1 + Quat mPathRotation = Quat::sIdentity(); + + /// The fraction along the path that corresponds to the initial position of body 2. Usually this is 0, the beginning of the path. But if you want to start an object halfway the path you can calculate this with mPath->GetClosestPoint(point on path to attach body to). + float mPathFraction = 0.0f; + + /// Maximum amount of friction force to apply (N) when not driven by a motor. + float mMaxFrictionForce = 0.0f; + + /// In case the constraint is powered, this determines the motor settings along the path + MotorSettings mPositionMotorSettings; + + /// How to constrain the rotation of the body to the path + EPathRotationConstraintType mRotationConstraintType = EPathRotationConstraintType::Free; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// Path constraint, used to constrain the degrees of freedom between two bodies to a path +class JPH_EXPORT PathConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct point constraint + PathConstraint(Body &inBody1, Body &inBody2, const PathConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Path; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual bool IsActive() const override { return TwoBodyConstraint::IsActive() && mPath != nullptr; } + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override { return mPathToBody1; } + virtual Mat44 GetConstraintToBody2Matrix() const override { return mPathToBody2; } + + /// Update the path for this constraint + void SetPath(const PathConstraintPath *inPath, float inPathFraction); + + /// Access to the current path + const PathConstraintPath * GetPath() const { return mPath; } + + /// Access to the current fraction along the path e [0, GetPath()->GetMaxPathFraction()] + float GetPathFraction() const { return mPathFraction; } + + /// Friction control + void SetMaxFrictionForce(float inFrictionForce) { mMaxFrictionForce = inFrictionForce; } + float GetMaxFrictionForce() const { return mMaxFrictionForce; } + + /// Position motor settings + MotorSettings & GetPositionMotorSettings() { return mPositionMotorSettings; } + const MotorSettings & GetPositionMotorSettings() const { return mPositionMotorSettings; } + + // Position motor controls (drives body 2 along the path) + void SetPositionMotorState(EMotorState inState) { JPH_ASSERT(inState == EMotorState::Off || mPositionMotorSettings.IsValid()); mPositionMotorState = inState; } + EMotorState GetPositionMotorState() const { return mPositionMotorState; } + void SetTargetVelocity(float inVelocity) { mTargetVelocity = inVelocity; } + float GetTargetVelocity() const { return mTargetVelocity; } + void SetTargetPathFraction(float inFraction) { JPH_ASSERT(mPath->IsLooping() || (inFraction >= 0.0f && inFraction <= mPath->GetPathMaxFraction())); mTargetPathFraction = inFraction; } + float GetTargetPathFraction() const { return mTargetPathFraction; } + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline Vector<2> GetTotalLambdaPosition() const { return mPositionConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaPositionLimits() const { return mPositionLimitsConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaMotor() const { return mPositionMotorConstraintPart.GetTotalLambda(); } + inline Vector<2> GetTotalLambdaRotationHinge() const { return mHingeConstraintPart.GetTotalLambda(); } + inline Vec3 GetTotalLambdaRotation() const { return mRotationConstraintPart.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateConstraintProperties(float inDeltaTime); + + // CONFIGURATION PROPERTIES FOLLOW + + RefConst mPath; ///< The path that attaches the two bodies + Mat44 mPathToBody1; ///< Transform that takes a quantity from path space to body 1 center of mass space + Mat44 mPathToBody2; ///< Transform that takes a quantity from path space to body 2 center of mass space + EPathRotationConstraintType mRotationConstraintType; ///< How to constrain the rotation of the path + + // Friction + float mMaxFrictionForce; + + // Motor controls + MotorSettings mPositionMotorSettings; + EMotorState mPositionMotorState = EMotorState::Off; + float mTargetVelocity = 0.0f; + float mTargetPathFraction = 0.0f; + + // RUN TIME PROPERTIES FOLLOW + + // Positions where the point constraint acts on in world space + Vec3 mR1; + Vec3 mR2; + + // X2 + R2 - X1 - R1 + Vec3 mU; + + // World space path tangent + Vec3 mPathTangent; + + // Normals to the path tangent + Vec3 mPathNormal; + Vec3 mPathBinormal; + + // Inverse of initial rotation from body 1 to body 2 in body 1 space (only used when rotation constraint type is FullyConstrained) + Quat mInvInitialOrientation; + + // Current fraction along the path where body 2 is attached + float mPathFraction = 0.0f; + + // Translation constraint parts + DualAxisConstraintPart mPositionConstraintPart; ///< Constraint part that keeps the movement along the tangent of the path + AxisConstraintPart mPositionLimitsConstraintPart; ///< Constraint part that prevents movement beyond the beginning and end of the path + AxisConstraintPart mPositionMotorConstraintPart; ///< Constraint to drive the object along the path or to apply friction + + // Rotation constraint parts + HingeRotationConstraintPart mHingeConstraintPart; ///< Constraint part that removes 2 degrees of rotation freedom + RotationEulerConstraintPart mRotationConstraintPart; ///< Constraint part that removes all rotational freedom +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.cpp new file mode 100644 index 000000000000..69c0a82f3bd8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.cpp @@ -0,0 +1,85 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(PathConstraintPath) +{ + JPH_ADD_BASE_CLASS(PathConstraintPath, SerializableObject) +} + +#ifdef JPH_DEBUG_RENDERER +// Helper function to transform the results of GetPointOnPath to world space +static inline void sTransformPathPoint(RMat44Arg inTransform, Vec3Arg inPosition, RVec3 &outPosition, Vec3 &ioNormal, Vec3 &ioBinormal) +{ + outPosition = inTransform * inPosition; + ioNormal = inTransform.Multiply3x3(ioNormal); + ioBinormal = inTransform.Multiply3x3(ioBinormal); +} + +// Helper function to draw a path segment +static inline void sDrawPathSegment(DebugRenderer *inRenderer, RVec3Arg inPrevPosition, RVec3Arg inPosition, Vec3Arg inNormal, Vec3Arg inBinormal) +{ + inRenderer->DrawLine(inPrevPosition, inPosition, Color::sWhite); + inRenderer->DrawArrow(inPosition, inPosition + 0.1f * inNormal, Color::sRed, 0.02f); + inRenderer->DrawArrow(inPosition, inPosition + 0.1f * inBinormal, Color::sGreen, 0.02f); +} + +void PathConstraintPath::DrawPath(DebugRenderer *inRenderer, RMat44Arg inBaseTransform) const +{ + // Calculate first point + Vec3 lfirst_pos, first_tangent, first_normal, first_binormal; + GetPointOnPath(0.0f, lfirst_pos, first_tangent, first_normal, first_binormal); + RVec3 first_pos; + sTransformPathPoint(inBaseTransform, lfirst_pos, first_pos, first_normal, first_binormal); + + float t_max = GetPathMaxFraction(); + + // Draw the segments + RVec3 prev_pos = first_pos; + for (float t = 0.1f; t < t_max; t += 0.1f) + { + Vec3 lpos, tangent, normal, binormal; + GetPointOnPath(t, lpos, tangent, normal, binormal); + RVec3 pos; + sTransformPathPoint(inBaseTransform, lpos, pos, normal, binormal); + sDrawPathSegment(inRenderer, prev_pos, pos, normal, binormal); + prev_pos = pos; + } + + // Draw last point + Vec3 lpos, tangent, normal, binormal; + GetPointOnPath(t_max, lpos, tangent, normal, binormal); + RVec3 pos; + sTransformPathPoint(inBaseTransform, lpos, pos, normal, binormal); + sDrawPathSegment(inRenderer, prev_pos, pos, normal, binormal); +} +#endif // JPH_DEBUG_RENDERER + +void PathConstraintPath::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(GetRTTI()->GetHash()); + inStream.Write(mIsLooping); +} + +void PathConstraintPath::RestoreBinaryState(StreamIn &inStream) +{ + // Type hash read by sRestoreFromBinaryState + inStream.Read(mIsLooping); +} + +PathConstraintPath::PathResult PathConstraintPath::sRestoreFromBinaryState(StreamIn &inStream) +{ + return StreamUtils::RestoreObject(inStream, &PathConstraintPath::RestoreBinaryState); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.h new file mode 100644 index 000000000000..2e05c2ce6f6f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.h @@ -0,0 +1,71 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; +#ifdef JPH_DEBUG_RENDERER +class DebugRenderer; +#endif // JPH_DEBUG_RENDERER + +/// The path for a path constraint. It allows attaching two bodies to each other while giving the second body the freedom to move along a path relative to the first. +class JPH_EXPORT PathConstraintPath : public SerializableObject, public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, PathConstraintPath) + +public: + using PathResult = Result>; + + /// Virtual destructor to ensure that derived types get their destructors called + virtual ~PathConstraintPath() override = default; + + /// Gets the max fraction along the path. I.e. sort of the length of the path. + virtual float GetPathMaxFraction() const = 0; + + /// Get the globally closest point on the curve (Could be slow!) + /// @param inPosition Position to find closest point for + /// @param inFractionHint Last known fraction along the path (can be used to speed up the search) + /// @return Fraction of closest point along the path + virtual float GetClosestPoint(Vec3Arg inPosition, float inFractionHint) const = 0; + + /// Given the fraction along the path, get the point, tangent and normal. + /// @param inFraction Fraction along the path [0, GetPathMaxFraction()]. + /// @param outPathPosition Returns the closest position to inSearchPosition on the path. + /// @param outPathTangent Returns the tangent to the path at outPathPosition (the vector that follows the direction of the path) + /// @param outPathNormal Return the normal to the path at outPathPosition (a vector that's perpendicular to outPathTangent) + /// @param outPathBinormal Returns the binormal to the path at outPathPosition (a vector so that normal cross tangent = binormal) + virtual void GetPointOnPath(float inFraction, Vec3 &outPathPosition, Vec3 &outPathTangent, Vec3 &outPathNormal, Vec3 &outPathBinormal) const = 0; + + /// If the path is looping or not. If a path is looping, the first and last point are automatically connected to each other. They should not be the same points. + void SetIsLooping(bool inIsLooping) { mIsLooping = inIsLooping; } + bool IsLooping() const { return mIsLooping; } + +#ifdef JPH_DEBUG_RENDERER + /// Draw the path relative to inBaseTransform. Used for debug purposes. + void DrawPath(DebugRenderer *inRenderer, RMat44Arg inBaseTransform) const; +#endif // JPH_DEBUG_RENDERER + + /// Saves the contents of the path in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const; + + /// Creates a Shape of the correct type and restores its contents from the binary stream inStream. + static PathResult sRestoreFromBinaryState(StreamIn &inStream); + +protected: + /// This function should not be called directly, it is used by sRestoreFromBinaryState. + virtual void RestoreBinaryState(StreamIn &inStream); + +private: + /// If the path is looping or not. If a path is looping, the first and last point are automatically connected to each other. They should not be the same points. + bool mIsLooping = false; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPathHermite.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPathHermite.cpp new file mode 100644 index 000000000000..132d3b25fcca --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPathHermite.cpp @@ -0,0 +1,308 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(PathConstraintPathHermite::Point) +{ + JPH_ADD_ATTRIBUTE(PathConstraintPathHermite::Point, mPosition) + JPH_ADD_ATTRIBUTE(PathConstraintPathHermite::Point, mTangent) + JPH_ADD_ATTRIBUTE(PathConstraintPathHermite::Point, mNormal) +} + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PathConstraintPathHermite) +{ + JPH_ADD_BASE_CLASS(PathConstraintPathHermite, PathConstraintPath) + + JPH_ADD_ATTRIBUTE(PathConstraintPathHermite, mPoints) +} + +// Calculate position and tangent for a Cubic Hermite Spline segment +static inline void sCalculatePositionAndTangent(Vec3Arg inP1, Vec3Arg inM1, Vec3Arg inP2, Vec3Arg inM2, float inT, Vec3 &outPosition, Vec3 &outTangent) +{ + // Calculate factors for Cubic Hermite Spline + // See: https://en.wikipedia.org/wiki/Cubic_Hermite_spline + float t2 = inT * inT; + float t3 = inT * t2; + float h00 = 2.0f * t3 - 3.0f * t2 + 1.0f; + float h10 = t3 - 2.0f * t2 + inT; + float h01 = -2.0f * t3 + 3.0f * t2; + float h11 = t3 - t2; + + // Calculate d/dt for factors to calculate the tangent + float ddt_h00 = 6.0f * (t2 - inT); + float ddt_h10 = 3.0f * t2 - 4.0f * inT + 1.0f; + float ddt_h01 = -ddt_h00; + float ddt_h11 = 3.0f * t2 - 2.0f * inT; + + outPosition = h00 * inP1 + h10 * inM1 + h01 * inP2 + h11 * inM2; + outTangent = ddt_h00 * inP1 + ddt_h10 * inM1 + ddt_h01 * inP2 + ddt_h11 * inM2; +} + +// Calculate the closest point to the origin for a Cubic Hermite Spline segment +// This is used to get an estimate for the interval in which the closest point can be found, +// the interval [0, 1] is too big for Newton Raphson to work on because it is solving a 5th degree polynomial which may +// have multiple local minima that are not the root. This happens especially when the path is straight (tangents aligned with inP2 - inP1). +// Based on the bisection method: https://en.wikipedia.org/wiki/Bisection_method +static inline void sCalculateClosestPointThroughBisection(Vec3Arg inP1, Vec3Arg inM1, Vec3Arg inP2, Vec3Arg inM2, float &outTMin, float &outTMax) +{ + outTMin = 0.0f; + outTMax = 1.0f; + + // To get the closest point of the curve to the origin we need to solve: + // d/dt P(t) . P(t) = 0 for t, where P(t) is the point on the curve segment + // Using d/dt (a(t) . b(t)) = d/dt a(t) . b(t) + a(t) . d/dt b(t) + // See: https://proofwiki.org/wiki/Derivative_of_Dot_Product_of_Vector-Valued_Functions + // d/dt P(t) . P(t) = 2 P(t) d/dt P(t) = 2 P(t) . Tangent(t) + + // Calculate the derivative at t = 0, we know P(0) = inP1 and Tangent(0) = inM1 + float ddt_min = inP1.Dot(inM1); // Leaving out factor 2, we're only interested in the root + if (abs(ddt_min) < 1.0e-6f) + { + // Derivative is near zero, we found our root + outTMax = 0.0f; + return; + } + bool ddt_min_negative = ddt_min < 0.0f; + + // Calculate derivative at t = 1, we know P(1) = inP2 and Tangent(1) = inM2 + float ddt_max = inP2.Dot(inM2); + if (abs(ddt_max) < 1.0e-6f) + { + // Derivative is near zero, we found our root + outTMin = 1.0f; + return; + } + bool ddt_max_negative = ddt_max < 0.0f; + + // If the signs of the derivative are not different, this algorithm can't find the root + if (ddt_min_negative == ddt_max_negative) + return; + + // With 4 iterations we'll get a result accurate to 1 / 2^4 = 0.0625 + for (int iteration = 0; iteration < 4; ++iteration) + { + float t_mid = 0.5f * (outTMin + outTMax); + Vec3 position, tangent; + sCalculatePositionAndTangent(inP1, inM1, inP2, inM2, t_mid, position, tangent); + float ddt_mid = position.Dot(tangent); + if (abs(ddt_mid) < 1.0e-6f) + { + // Derivative is near zero, we found our root + outTMin = outTMax = t_mid; + return; + } + bool ddt_mid_negative = ddt_mid < 0.0f; + + // Update the search interval so that the signs of the derivative at both ends of the interval are still different + if (ddt_mid_negative == ddt_min_negative) + outTMin = t_mid; + else + outTMax = t_mid; + } +} + +// Calculate the closest point to the origin for a Cubic Hermite Spline segment +// Only considers the range t e [inTMin, inTMax] and will stop as soon as the closest point falls outside of that range +static inline float sCalculateClosestPointThroughNewtonRaphson(Vec3Arg inP1, Vec3Arg inM1, Vec3Arg inP2, Vec3Arg inM2, float inTMin, float inTMax, float &outDistanceSq) +{ + // This is the closest position on the curve to the origin that we found + Vec3 position; + + // Calculate the size of the interval + float interval = inTMax - inTMin; + + // Start in the middle of the interval + float t = 0.5f * (inTMin + inTMax); + + // Do max 10 iterations to prevent taking too much CPU time + for (int iteration = 0; iteration < 10; ++iteration) + { + // Calculate derivative at t, see comment at sCalculateClosestPointThroughBisection for derivation of the equations + Vec3 tangent; + sCalculatePositionAndTangent(inP1, inM1, inP2, inM2, t, position, tangent); + float ddt = position.Dot(tangent); // Leaving out factor 2, we're only interested in the root + + // Calculate derivative of ddt: d^2/dt P(t) . P(t) = d/dt (2 P(t) . Tangent(t)) + // = 2 (d/dt P(t)) . Tangent(t) + P(t) . d/dt Tangent(t)) = 2 (Tangent(t) . Tangent(t) + P(t) . d/dt Tangent(t)) + float d2dt_h00 = 12.0f * t - 6.0f; + float d2dt_h10 = 6.0f * t - 4.0f; + float d2dt_h01 = -d2dt_h00; + float d2dt_h11 = 6.0f * t - 2.0f; + Vec3 ddt_tangent = d2dt_h00 * inP1 + d2dt_h10 * inM1 + d2dt_h01 * inP2 + d2dt_h11 * inM2; + float d2dt = tangent.Dot(tangent) + position.Dot(ddt_tangent); // Leaving out factor 2, because we left it out above too + + // If d2dt is zero, the curve is flat and there are multiple t's for which we are closest to the origin, stop now + if (d2dt == 0.0f) + break; + + // Do a Newton Raphson step + // See: https://en.wikipedia.org/wiki/Newton%27s_method + // Clamp against [-interval, interval] to avoid overshooting too much, we're not interested outside the interval + float delta = Clamp(-ddt / d2dt, -interval, interval); + + // If we're stepping away further from t e [inTMin, inTMax] stop now + if ((t > inTMax && delta > 0.0f) || (t < inTMin && delta < 0.0f)) + break; + + // If we've converged, stop now + t += delta; + if (abs(delta) < 1.0e-4f) + break; + } + + // Calculate the distance squared for the origin to the curve + outDistanceSq = position.LengthSq(); + return t; +} + +void PathConstraintPathHermite::GetIndexAndT(float inFraction, int &outIndex, float &outT) const +{ + int num_points = int(mPoints.size()); + + // Start by truncating the fraction to get the index and storing the remainder in t + int index = int(trunc(inFraction)); + float t = inFraction - float(index); + + if (IsLooping()) + { + JPH_ASSERT(!mPoints.front().mPosition.IsClose(mPoints.back().mPosition), "A looping path should have a different first and last point!"); + + // Make sure index is positive by adding a multiple of num_points + if (index < 0) + index += (-index / num_points + 1) * num_points; + + // Index needs to be modulo num_points + index = index % num_points; + } + else + { + // Clamp against range of points + if (index < 0) + { + index = 0; + t = 0.0f; + } + else if (index >= num_points - 1) + { + index = num_points - 2; + t = 1.0f; + } + } + + outIndex = index; + outT = t; +} + +float PathConstraintPathHermite::GetClosestPoint(Vec3Arg inPosition, float inFractionHint) const +{ + JPH_PROFILE_FUNCTION(); + + int num_points = int(mPoints.size()); + + // Start with last point on the path, in the non-looping case we won't be visiting this point + float best_dist_sq = (mPoints[num_points - 1].mPosition - inPosition).LengthSq(); + float best_t = float(num_points - 1); + + // Loop over all points + for (int i = 0, max_i = IsLooping()? num_points : num_points - 1; i < max_i; ++i) + { + const Point &p1 = mPoints[i]; + const Point &p2 = mPoints[(i + 1) % num_points]; + + // Make the curve relative to inPosition + Vec3 p1_pos = p1.mPosition - inPosition; + Vec3 p2_pos = p2.mPosition - inPosition; + + // Get distance to p1 + float dist_sq = p1_pos.LengthSq(); + if (dist_sq < best_dist_sq) + { + best_t = float(i); + best_dist_sq = dist_sq; + } + + // First find an interval for the closest point so that we can start doing Newton Raphson steps + float t_min, t_max; + sCalculateClosestPointThroughBisection(p1_pos, p1.mTangent, p2_pos, p2.mTangent, t_min, t_max); + + if (t_min == t_max) + { + // If the function above returned no interval then it found the root already and we can just calculate the distance + Vec3 position, tangent; + sCalculatePositionAndTangent(p1_pos, p1.mTangent, p2_pos, p2.mTangent, t_min, position, tangent); + dist_sq = position.LengthSq(); + if (dist_sq < best_dist_sq) + { + best_t = float(i) + t_min; + best_dist_sq = dist_sq; + } + } + else + { + // Get closest distance along curve segment + float t = sCalculateClosestPointThroughNewtonRaphson(p1_pos, p1.mTangent, p2_pos, p2.mTangent, t_min, t_max, dist_sq); + if (t >= 0.0f && t <= 1.0f && dist_sq < best_dist_sq) + { + best_t = float(i) + t; + best_dist_sq = dist_sq; + } + } + } + + return best_t; +} + +void PathConstraintPathHermite::GetPointOnPath(float inFraction, Vec3 &outPathPosition, Vec3 &outPathTangent, Vec3 &outPathNormal, Vec3 &outPathBinormal) const +{ + JPH_PROFILE_FUNCTION(); + + // Determine which hermite spline segment we need + int index; + float t; + GetIndexAndT(inFraction, index, t); + + // Get the points on the segment + const Point &p1 = mPoints[index]; + const Point &p2 = mPoints[(index + 1) % int(mPoints.size())]; + + // Calculate the position and tangent on the path + Vec3 tangent; + sCalculatePositionAndTangent(p1.mPosition, p1.mTangent, p2.mPosition, p2.mTangent, t, outPathPosition, tangent); + outPathTangent = tangent.Normalized(); + + // Just linearly interpolate the normal + Vec3 normal = (1.0f - t) * p1.mNormal + t * p2.mNormal; + + // Calculate binormal + outPathBinormal = normal.Cross(outPathTangent).Normalized(); + + // Recalculate normal so it is perpendicular to both (linear interpolation will cause it not to be) + outPathNormal = outPathTangent.Cross(outPathBinormal); + JPH_ASSERT(outPathNormal.IsNormalized()); +} + +void PathConstraintPathHermite::SaveBinaryState(StreamOut &inStream) const +{ + PathConstraintPath::SaveBinaryState(inStream); + + inStream.Write(mPoints); +} + +void PathConstraintPathHermite::RestoreBinaryState(StreamIn &inStream) +{ + PathConstraintPath::RestoreBinaryState(inStream); + + inStream.Read(mPoints); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPathHermite.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPathHermite.h new file mode 100644 index 000000000000..839909be8b90 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPathHermite.h @@ -0,0 +1,54 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// A path that follows a Hermite spline +class JPH_EXPORT PathConstraintPathHermite final : public PathConstraintPath +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PathConstraintPathHermite) + +public: + // See PathConstraintPath::GetPathMaxFraction + virtual float GetPathMaxFraction() const override { return float(IsLooping()? mPoints.size() : mPoints.size() - 1); } + + // See PathConstraintPath::GetClosestPoint + virtual float GetClosestPoint(Vec3Arg inPosition, float inFractionHint) const override; + + // See PathConstraintPath::GetPointOnPath + virtual void GetPointOnPath(float inFraction, Vec3 &outPathPosition, Vec3 &outPathTangent, Vec3 &outPathNormal, Vec3 &outPathBinormal) const override; + + /// Adds a point to the path + void AddPoint(Vec3Arg inPosition, Vec3Arg inTangent, Vec3Arg inNormal) { mPoints.push_back({ inPosition, inTangent, inNormal}); } + + // See: PathConstraintPath::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + struct Point + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Point) + + Vec3 mPosition; ///< Position on the path + Vec3 mTangent; ///< Tangent of the path, does not need to be normalized (in the direction of the path) + Vec3 mNormal; ///< Normal of the path (together with the tangent along the curve this forms a basis for the constraint) + }; + +protected: + // See: PathConstraintPath::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + /// Helper function that returns the index of the path segment and the fraction t on the path segment based on the full path fraction + inline void GetIndexAndT(float inFraction, int &outIndex, float &outT) const; + + using Points = Array; + + Points mPoints; ///< Points on the Hermite spline +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PointConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PointConstraint.cpp new file mode 100644 index 000000000000..74d0ecd7c74f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PointConstraint.cpp @@ -0,0 +1,157 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PointConstraintSettings) +{ + JPH_ADD_BASE_CLASS(PointConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(PointConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(PointConstraintSettings, mPoint1) + JPH_ADD_ATTRIBUTE(PointConstraintSettings, mPoint2) +} + +void PointConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mPoint1); + inStream.Write(mPoint2); +} + +void PointConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mPoint1); + inStream.Read(mPoint2); +} + +TwoBodyConstraint *PointConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new PointConstraint(inBody1, inBody2, *this); +} + +PointConstraint::PointConstraint(Body &inBody1, Body &inBody2, const PointConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings) +{ + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPoint2); + } + else + { + mLocalSpacePosition1 = Vec3(inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inSettings.mPoint2); + } +} + +void PointConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +void PointConstraint::SetPoint1(EConstraintSpace inSpace, RVec3Arg inPoint1) +{ + if (inSpace == EConstraintSpace::WorldSpace) + mLocalSpacePosition1 = Vec3(mBody1->GetInverseCenterOfMassTransform() * inPoint1); + else + mLocalSpacePosition1 = Vec3(inPoint1); +} + +void PointConstraint::SetPoint2(EConstraintSpace inSpace, RVec3Arg inPoint2) +{ + if (inSpace == EConstraintSpace::WorldSpace) + mLocalSpacePosition2 = Vec3(mBody2->GetInverseCenterOfMassTransform() * inPoint2); + else + mLocalSpacePosition2 = Vec3(inPoint2); +} + +void PointConstraint::CalculateConstraintProperties() +{ + mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2); +} + +void PointConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + CalculateConstraintProperties(); +} + +void PointConstraint::ResetWarmStart() +{ + mPointConstraintPart.Deactivate(); +} + +void PointConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); +} + +bool PointConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + return mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); +} + +bool PointConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + // Update constraint properties (bodies may have moved) + CalculateConstraintProperties(); + + return mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); +} + +#ifdef JPH_DEBUG_RENDERER +void PointConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + // Draw constraint + inRenderer->DrawMarker(mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1, Color::sRed, 0.1f); + inRenderer->DrawMarker(mBody2->GetCenterOfMassTransform() * mLocalSpacePosition2, Color::sGreen, 0.1f); +} +#endif // JPH_DEBUG_RENDERER + +void PointConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mPointConstraintPart.SaveState(inStream); +} + +void PointConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mPointConstraintPart.RestoreState(inStream); +} + +Ref PointConstraint::GetConstraintSettings() const +{ + PointConstraintSettings *settings = new PointConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPoint1 = RVec3(mLocalSpacePosition1); + settings->mPoint2 = RVec3(mLocalSpacePosition2); + return settings; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PointConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PointConstraint.h new file mode 100644 index 000000000000..8ab943eec81f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PointConstraint.h @@ -0,0 +1,94 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Point constraint settings, used to create a point constraint +class JPH_EXPORT PointConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PointConstraintSettings) + +public: + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 constraint position (space determined by mSpace). + RVec3 mPoint1 = RVec3::sZero(); + + /// Body 2 constraint position (space determined by mSpace). + /// Note: Normally you would set mPoint1 = mPoint2 if the bodies are already placed how you want to constrain them (if mSpace = world space). + RVec3 mPoint2 = RVec3::sZero(); + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A point constraint constrains 2 bodies on a single point (removing 3 degrees of freedom) +class JPH_EXPORT PointConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct point constraint + PointConstraint(Body &inBody1, Body &inBody2, const PointConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Point; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + /// Update the attachment point for body 1 + void SetPoint1(EConstraintSpace inSpace, RVec3Arg inPoint1); + + /// Update the attachment point for body 2 + void SetPoint2(EConstraintSpace inSpace, RVec3Arg inPoint2); + + /// Get the attachment point for body 1 relative to body 1 COM (transform by Body::GetCenterOfMassTransform to take to world space) + inline Vec3 GetLocalSpacePoint1() const { return mLocalSpacePosition1; } + + /// Get the attachment point for body 2 relative to body 2 COM (transform by Body::GetCenterOfMassTransform to take to world space) + inline Vec3 GetLocalSpacePoint2() const { return mLocalSpacePosition2; } + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override { return Mat44::sTranslation(mLocalSpacePosition1); } + virtual Mat44 GetConstraintToBody2Matrix() const override { return Mat44::sTranslation(mLocalSpacePosition2); } // Note: Incorrect rotation as we don't track the original rotation difference, should not matter though as the constraint is not limiting rotation. + + ///@name Get Lagrange multiplier from last physics update (the linear impulse applied to satisfy the constraint) + inline Vec3 GetTotalLambdaPosition() const { return mPointConstraintPart.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateConstraintProperties(); + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // The constraint part + PointConstraintPart mPointConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.cpp new file mode 100644 index 000000000000..9d15c1d4dc1e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.cpp @@ -0,0 +1,253 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +using namespace literals; + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PulleyConstraintSettings) +{ + JPH_ADD_BASE_CLASS(PulleyConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(PulleyConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mBodyPoint1) + JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mFixedPoint1) + JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mBodyPoint2) + JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mFixedPoint2) + JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mRatio) + JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mMinLength) + JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mMaxLength) +} + +void PulleyConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mBodyPoint1); + inStream.Write(mFixedPoint1); + inStream.Write(mBodyPoint2); + inStream.Write(mFixedPoint2); + inStream.Write(mRatio); + inStream.Write(mMinLength); + inStream.Write(mMaxLength); +} + +void PulleyConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mBodyPoint1); + inStream.Read(mFixedPoint1); + inStream.Read(mBodyPoint2); + inStream.Read(mFixedPoint2); + inStream.Read(mRatio); + inStream.Read(mMinLength); + inStream.Read(mMaxLength); +} + +TwoBodyConstraint *PulleyConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new PulleyConstraint(inBody1, inBody2, *this); +} + +PulleyConstraint::PulleyConstraint(Body &inBody1, Body &inBody2, const PulleyConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mFixedPosition1(inSettings.mFixedPoint1), + mFixedPosition2(inSettings.mFixedPoint2), + mRatio(inSettings.mRatio), + mMinLength(inSettings.mMinLength), + mMaxLength(inSettings.mMaxLength) +{ + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mBodyPoint1); + mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mBodyPoint2); + mWorldSpacePosition1 = inSettings.mBodyPoint1; + mWorldSpacePosition2 = inSettings.mBodyPoint2; + } + else + { + // If properties were specified in local space, we need to calculate world space positions + mLocalSpacePosition1 = Vec3(inSettings.mBodyPoint1); + mLocalSpacePosition2 = Vec3(inSettings.mBodyPoint2); + mWorldSpacePosition1 = inBody1.GetCenterOfMassTransform() * inSettings.mBodyPoint1; + mWorldSpacePosition2 = inBody2.GetCenterOfMassTransform() * inSettings.mBodyPoint2; + } + + // Calculate min/max length if it was not provided + float current_length = GetCurrentLength(); + if (mMinLength < 0.0f) + mMinLength = current_length; + if (mMaxLength < 0.0f) + mMaxLength = current_length; + + // Initialize the normals to a likely valid axis in case the fixed points overlap with the attachment points (most likely the fixed points are above both bodies) + mWorldSpaceNormal1 = mWorldSpaceNormal2 = -Vec3::sAxisY(); +} + +void PulleyConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +float PulleyConstraint::CalculatePositionsNormalsAndLength() +{ + // Update world space positions (the bodies may have moved) + mWorldSpacePosition1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1; + mWorldSpacePosition2 = mBody2->GetCenterOfMassTransform() * mLocalSpacePosition2; + + // Calculate world space normals + Vec3 delta1 = Vec3(mWorldSpacePosition1 - mFixedPosition1); + float delta1_len = delta1.Length(); + if (delta1_len > 0.0f) + mWorldSpaceNormal1 = delta1 / delta1_len; + + Vec3 delta2 = Vec3(mWorldSpacePosition2 - mFixedPosition2); + float delta2_len = delta2.Length(); + if (delta2_len > 0.0f) + mWorldSpaceNormal2 = delta2 / delta2_len; + + // Calculate length + return delta1_len + mRatio * delta2_len; +} + +void PulleyConstraint::CalculateConstraintProperties() +{ + // Calculate attachment points relative to COM + Vec3 r1 = Vec3(mWorldSpacePosition1 - mBody1->GetCenterOfMassPosition()); + Vec3 r2 = Vec3(mWorldSpacePosition2 - mBody2->GetCenterOfMassPosition()); + + mIndependentAxisConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, r1, mWorldSpaceNormal1, r2, mWorldSpaceNormal2, mRatio); +} + +void PulleyConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Determine if the constraint is active + float current_length = CalculatePositionsNormalsAndLength(); + bool min_length_violation = current_length <= mMinLength; + bool max_length_violation = current_length >= mMaxLength; + if (min_length_violation || max_length_violation) + { + // Determine max lambda based on if the length is too big or small + mMinLambda = max_length_violation? -FLT_MAX : 0.0f; + mMaxLambda = min_length_violation? FLT_MAX : 0.0f; + + CalculateConstraintProperties(); + } + else + mIndependentAxisConstraintPart.Deactivate(); +} + +void PulleyConstraint::ResetWarmStart() +{ + mIndependentAxisConstraintPart.Deactivate(); +} + +void PulleyConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + mIndependentAxisConstraintPart.WarmStart(*mBody1, *mBody2, mWorldSpaceNormal1, mWorldSpaceNormal2, mRatio, inWarmStartImpulseRatio); +} + +bool PulleyConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + if (mIndependentAxisConstraintPart.IsActive()) + return mIndependentAxisConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceNormal1, mWorldSpaceNormal2, mRatio, mMinLambda, mMaxLambda); + else + return false; +} + +bool PulleyConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + // Calculate new length (bodies may have changed) + float current_length = CalculatePositionsNormalsAndLength(); + + float position_error = 0.0f; + if (current_length < mMinLength) + position_error = current_length - mMinLength; + else if (current_length > mMaxLength) + position_error = current_length - mMaxLength; + + if (position_error != 0.0f) + { + // Update constraint properties (bodies may have moved) + CalculateConstraintProperties(); + + return mIndependentAxisConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceNormal1, mWorldSpaceNormal2, mRatio, position_error, inBaumgarte); + } + + return false; +} + +#ifdef JPH_DEBUG_RENDERER +void PulleyConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + // Color according to length vs min/max length + float current_length = GetCurrentLength(); + Color color = Color::sGreen; + if (current_length < mMinLength) + color = Color::sYellow; + else if (current_length > mMaxLength) + color = Color::sRed; + + // Draw constraint + inRenderer->DrawLine(mWorldSpacePosition1, mFixedPosition1, color); + inRenderer->DrawLine(mFixedPosition1, mFixedPosition2, color); + inRenderer->DrawLine(mFixedPosition2, mWorldSpacePosition2, color); + + // Draw current length + inRenderer->DrawText3D(0.5_r * (mFixedPosition1 + mFixedPosition2), StringFormat("%.2f", (double)current_length)); +} +#endif // JPH_DEBUG_RENDERER + +void PulleyConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mIndependentAxisConstraintPart.SaveState(inStream); + inStream.Write(mWorldSpaceNormal1); // When distance to fixed point = 0, the normal is used from last frame so we need to store it + inStream.Write(mWorldSpaceNormal2); +} + +void PulleyConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mIndependentAxisConstraintPart.RestoreState(inStream); + inStream.Read(mWorldSpaceNormal1); + inStream.Read(mWorldSpaceNormal2); +} + +Ref PulleyConstraint::GetConstraintSettings() const +{ + PulleyConstraintSettings *settings = new PulleyConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mBodyPoint1 = RVec3(mLocalSpacePosition1); + settings->mFixedPoint1 = mFixedPosition1; + settings->mBodyPoint2 = RVec3(mLocalSpacePosition2); + settings->mFixedPoint2 = mFixedPosition2; + settings->mRatio = mRatio; + settings->mMinLength = mMinLength; + settings->mMaxLength = mMaxLength; + return settings; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.h new file mode 100644 index 000000000000..092be68c1b46 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.h @@ -0,0 +1,137 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Pulley constraint settings, used to create a pulley constraint. +/// A pulley connects two bodies via two fixed world points to each other similar to a distance constraint. +/// We define Length1 = |BodyPoint1 - FixedPoint1| where Body1 is a point on body 1 in world space and FixedPoint1 a fixed point in world space +/// Length2 = |BodyPoint2 - FixedPoint2| +/// The constraint keeps the two line segments constrained so that +/// MinDistance <= Length1 + Ratio * Length2 <= MaxDistance +class JPH_EXPORT PulleyConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PulleyConstraintSettings) + +public: + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, specified properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 constraint attachment point (space determined by mSpace). + RVec3 mBodyPoint1 = RVec3::sZero(); + + /// Fixed world point to which body 1 is connected (always world space) + RVec3 mFixedPoint1 = RVec3::sZero(); + + /// Body 2 constraint attachment point (space determined by mSpace) + RVec3 mBodyPoint2 = RVec3::sZero(); + + /// Fixed world point to which body 2 is connected (always world space) + RVec3 mFixedPoint2 = RVec3::sZero(); + + /// Ratio between the two line segments (see formula above), can be used to create a block and tackle + float mRatio = 1.0f; + + /// The minimum length of the line segments (see formula above), use -1 to calculate the length based on the positions of the objects when the constraint is created. + float mMinLength = 0.0f; + + /// The maximum length of the line segments (see formula above), use -1 to calculate the length based on the positions of the objects when the constraint is created. + float mMaxLength = -1.0f; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A pulley constraint. +class JPH_EXPORT PulleyConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct distance constraint + PulleyConstraint(Body &inBody1, Body &inBody2, const PulleyConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Pulley; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override { return Mat44::sTranslation(mLocalSpacePosition1); } + virtual Mat44 GetConstraintToBody2Matrix() const override { return Mat44::sTranslation(mLocalSpacePosition2); } // Note: Incorrect rotation as we don't track the original rotation difference, should not matter though as the constraint is not limiting rotation. + + /// Update the minimum and maximum length for the constraint + void SetLength(float inMinLength, float inMaxLength) { JPH_ASSERT(inMinLength >= 0.0f && inMinLength <= inMaxLength); mMinLength = inMinLength; mMaxLength = inMaxLength; } + float GetMinLength() const { return mMinLength; } + float GetMaxLength() const { return mMaxLength; } + + /// Get the current length of both segments (multiplied by the ratio for segment 2) + float GetCurrentLength() const { return Vec3(mWorldSpacePosition1 - mFixedPosition1).Length() + mRatio * Vec3(mWorldSpacePosition2 - mFixedPosition2).Length(); } + + ///@name Get Lagrange multiplier from last physics update (the linear impulse applied to satisfy the constraint) + inline float GetTotalLambdaPosition() const { return mIndependentAxisConstraintPart.GetTotalLambda(); } + +private: + // Calculates world positions and normals and returns current length + float CalculatePositionsNormalsAndLength(); + + // Internal helper function to calculate the values below + void CalculateConstraintProperties(); + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions on the bodies + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // World space fixed positions + RVec3 mFixedPosition1; + RVec3 mFixedPosition2; + + /// Ratio between the two line segments + float mRatio; + + // The minimum/maximum length of the line segments + float mMinLength; + float mMaxLength; + + // RUN TIME PROPERTIES FOLLOW + + // World space positions and normal + RVec3 mWorldSpacePosition1; + RVec3 mWorldSpacePosition2; + Vec3 mWorldSpaceNormal1; + Vec3 mWorldSpaceNormal2; + + // Depending on if the length < min or length > max we can apply forces to prevent further violations + float mMinLambda; + float mMaxLambda; + + // The constraint part + IndependentAxisConstraintPart mIndependentAxisConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/RackAndPinionConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/RackAndPinionConstraint.cpp new file mode 100644 index 000000000000..d0bcb62febd7 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/RackAndPinionConstraint.cpp @@ -0,0 +1,189 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(RackAndPinionConstraintSettings) +{ + JPH_ADD_BASE_CLASS(RackAndPinionConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(RackAndPinionConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(RackAndPinionConstraintSettings, mHingeAxis) + JPH_ADD_ATTRIBUTE(RackAndPinionConstraintSettings, mSliderAxis) + JPH_ADD_ATTRIBUTE(RackAndPinionConstraintSettings, mRatio) +} + +void RackAndPinionConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mHingeAxis); + inStream.Write(mSliderAxis); + inStream.Write(mRatio); +} + +void RackAndPinionConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mHingeAxis); + inStream.Read(mSliderAxis); + inStream.Read(mRatio); +} + +TwoBodyConstraint *RackAndPinionConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new RackAndPinionConstraint(inBody1, inBody2, *this); +} + +RackAndPinionConstraint::RackAndPinionConstraint(Body &inBody1, Body &inBody2, const RackAndPinionConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mLocalSpaceHingeAxis(inSettings.mHingeAxis), + mLocalSpaceSliderAxis(inSettings.mSliderAxis), + mRatio(inSettings.mRatio) +{ + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + mLocalSpaceHingeAxis = inBody1.GetInverseCenterOfMassTransform().Multiply3x3(mLocalSpaceHingeAxis).Normalized(); + mLocalSpaceSliderAxis = inBody2.GetInverseCenterOfMassTransform().Multiply3x3(mLocalSpaceSliderAxis).Normalized(); + } +} + +void RackAndPinionConstraint::CalculateConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2) +{ + // Calculate world space normals + mWorldSpaceHingeAxis = inRotation1 * mLocalSpaceHingeAxis; + mWorldSpaceSliderAxis = inRotation2 * mLocalSpaceSliderAxis; + + mRackAndPinionConstraintPart.CalculateConstraintProperties(*mBody1, mWorldSpaceHingeAxis, *mBody2, mWorldSpaceSliderAxis, mRatio); +} + +void RackAndPinionConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Calculate constraint properties that are constant while bodies don't move + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + CalculateConstraintProperties(rotation1, rotation2); +} + +void RackAndPinionConstraint::ResetWarmStart() +{ + mRackAndPinionConstraintPart.Deactivate(); +} + +void RackAndPinionConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mRackAndPinionConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); +} + +bool RackAndPinionConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + return mRackAndPinionConstraintPart.SolveVelocityConstraint(*mBody1, mWorldSpaceHingeAxis, *mBody2, mWorldSpaceSliderAxis, mRatio); +} + +bool RackAndPinionConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + if (mRackConstraint == nullptr || mPinionConstraint == nullptr) + return false; + + float rotation; + if (mPinionConstraint->GetSubType() == EConstraintSubType::Hinge) + { + rotation = StaticCast(mPinionConstraint)->GetCurrentAngle(); + } + else + { + JPH_ASSERT(false, "Unsupported"); + return false; + } + + float translation; + if (mRackConstraint->GetSubType() == EConstraintSubType::Slider) + { + translation = StaticCast(mRackConstraint)->GetCurrentPosition(); + } + else + { + JPH_ASSERT(false, "Unsupported"); + return false; + } + + float error = CenterAngleAroundZero(fmod(rotation - mRatio * translation, 2.0f * JPH_PI)); + if (error == 0.0f) + return false; + + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + CalculateConstraintProperties(rotation1, rotation2); + return mRackAndPinionConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, error, inBaumgarte); +} + +#ifdef JPH_DEBUG_RENDERER +void RackAndPinionConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform(); + + // Draw constraint axis + inRenderer->DrawArrow(transform1.GetTranslation(), transform1 * mLocalSpaceHingeAxis, Color::sGreen, 0.01f); + inRenderer->DrawArrow(transform2.GetTranslation(), transform2 * mLocalSpaceSliderAxis, Color::sBlue, 0.01f); +} + +#endif // JPH_DEBUG_RENDERER + +void RackAndPinionConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mRackAndPinionConstraintPart.SaveState(inStream); +} + +void RackAndPinionConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mRackAndPinionConstraintPart.RestoreState(inStream); +} + +Ref RackAndPinionConstraint::GetConstraintSettings() const +{ + RackAndPinionConstraintSettings *settings = new RackAndPinionConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mHingeAxis = mLocalSpaceHingeAxis; + settings->mSliderAxis = mLocalSpaceSliderAxis; + settings->mRatio = mRatio; + return settings; +} + +Mat44 RackAndPinionConstraint::GetConstraintToBody1Matrix() const +{ + Vec3 perp = mLocalSpaceHingeAxis.GetNormalizedPerpendicular(); + return Mat44(Vec4(mLocalSpaceHingeAxis, 0), Vec4(perp, 0), Vec4(mLocalSpaceHingeAxis.Cross(perp), 0), Vec4(0, 0, 0, 1)); +} + +Mat44 RackAndPinionConstraint::GetConstraintToBody2Matrix() const +{ + Vec3 perp = mLocalSpaceSliderAxis.GetNormalizedPerpendicular(); + return Mat44(Vec4(mLocalSpaceSliderAxis, 0), Vec4(perp, 0), Vec4(mLocalSpaceSliderAxis.Cross(perp), 0), Vec4(0, 0, 0, 1)); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/RackAndPinionConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/RackAndPinionConstraint.h new file mode 100644 index 000000000000..f26af31f1b79 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/RackAndPinionConstraint.h @@ -0,0 +1,118 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Rack and pinion constraint (slider & gear) settings +class JPH_EXPORT RackAndPinionConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, RackAndPinionConstraintSettings) + +public: + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint. + /// Body1 should be the pinion (gear) and body 2 the rack (slider). + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// Defines the ratio between the rotation of the pinion and the translation of the rack. + /// The ratio is defined as: PinionRotation(t) = ratio * RackTranslation(t) + /// @param inNumTeethRack Number of teeth that the rack has + /// @param inRackLength Length of the rack + /// @param inNumTeethPinion Number of teeth the pinion has + void SetRatio(int inNumTeethRack, float inRackLength, int inNumTeethPinion) + { + mRatio = 2.0f * JPH_PI * inNumTeethRack / (inRackLength * inNumTeethPinion); + } + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 (pinion) constraint reference frame (space determined by mSpace). + Vec3 mHingeAxis = Vec3::sAxisX(); + + /// Body 2 (rack) constraint reference frame (space determined by mSpace) + Vec3 mSliderAxis = Vec3::sAxisX(); + + /// Ratio between the rack and pinion, see SetRatio. + float mRatio = 1.0f; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A rack and pinion constraint constrains the rotation of body1 to the translation of body 2. +/// Note that this constraint needs to be used in conjunction with a hinge constraint for body 1 and a slider constraint for body 2. +class JPH_EXPORT RackAndPinionConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct gear constraint + RackAndPinionConstraint(Body &inBody1, Body &inBody2, const RackAndPinionConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::RackAndPinion; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override { /* Nothing */ } + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override; + virtual Mat44 GetConstraintToBody2Matrix() const override; + + /// The constraints that constrain the rack and pinion (a slider and a hinge), optional and used to calculate the position error and fix numerical drift. + void SetConstraints(const Constraint *inPinion, const Constraint *inRack) { mPinionConstraint = inPinion; mRackConstraint = inRack; } + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline float GetTotalLambda() const { return mRackAndPinionConstraintPart.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2); + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space hinge axis + Vec3 mLocalSpaceHingeAxis; + + // Local space sliding direction + Vec3 mLocalSpaceSliderAxis; + + // Ratio between rack and pinion + float mRatio; + + // The constraints that constrain the rack and pinion (a slider and a hinge), optional and used to calculate the position error and fix numerical drift. + RefConst mPinionConstraint; + RefConst mRackConstraint; + + // RUN TIME PROPERTIES FOLLOW + + // World space hinge axis + Vec3 mWorldSpaceHingeAxis; + + // World space sliding direction + Vec3 mWorldSpaceSliderAxis; + + // The constraint parts + RackAndPinionConstraintPart mRackAndPinionConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.cpp new file mode 100644 index 000000000000..070a45e213d4 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.cpp @@ -0,0 +1,900 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SixDOFConstraintSettings) +{ + JPH_ADD_BASE_CLASS(SixDOFConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(SixDOFConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mPosition1) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisX1) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisY1) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mPosition2) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisX2) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisY2) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mMaxFriction) + JPH_ADD_ENUM_ATTRIBUTE(SixDOFConstraintSettings, mSwingType) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitMin) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitMax) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitsSpringSettings) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mMotorSettings) +} + +void SixDOFConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mPosition1); + inStream.Write(mAxisX1); + inStream.Write(mAxisY1); + inStream.Write(mPosition2); + inStream.Write(mAxisX2); + inStream.Write(mAxisY2); + inStream.Write(mMaxFriction); + inStream.Write(mSwingType); + inStream.Write(mLimitMin); + inStream.Write(mLimitMax); + for (const SpringSettings &s : mLimitsSpringSettings) + s.SaveBinaryState(inStream); + for (const MotorSettings &m : mMotorSettings) + m.SaveBinaryState(inStream); +} + +void SixDOFConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mPosition1); + inStream.Read(mAxisX1); + inStream.Read(mAxisY1); + inStream.Read(mPosition2); + inStream.Read(mAxisX2); + inStream.Read(mAxisY2); + inStream.Read(mMaxFriction); + inStream.Read(mSwingType); + inStream.Read(mLimitMin); + inStream.Read(mLimitMax); + for (SpringSettings &s : mLimitsSpringSettings) + s.RestoreBinaryState(inStream); + for (MotorSettings &m : mMotorSettings) + m.RestoreBinaryState(inStream); +} + +TwoBodyConstraint *SixDOFConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new SixDOFConstraint(inBody1, inBody2, *this); +} + +void SixDOFConstraint::UpdateTranslationLimits() +{ + // Set to zero if the limits are inversed + for (int i = EAxis::TranslationX; i <= EAxis::TranslationZ; ++i) + if (mLimitMin[i] > mLimitMax[i]) + mLimitMin[i] = mLimitMax[i] = 0.0f; +} + +void SixDOFConstraint::UpdateRotationLimits() +{ + if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Cone) + { + // Cone swing upper limit needs to be positive + mLimitMax[EAxis::RotationY] = max(0.0f, mLimitMax[EAxis::RotationY]); + mLimitMax[EAxis::RotationZ] = max(0.0f, mLimitMax[EAxis::RotationZ]); + + // Cone swing limits only support symmetric ranges + mLimitMin[EAxis::RotationY] = -mLimitMax[EAxis::RotationY]; + mLimitMin[EAxis::RotationZ] = -mLimitMax[EAxis::RotationZ]; + } + + for (int i = EAxis::RotationX; i <= EAxis::RotationZ; ++i) + { + // Clamp to [-PI, PI] range + mLimitMin[i] = Clamp(mLimitMin[i], -JPH_PI, JPH_PI); + mLimitMax[i] = Clamp(mLimitMax[i], -JPH_PI, JPH_PI); + + // Set to zero if the limits are inversed + if (mLimitMin[i] > mLimitMax[i]) + mLimitMin[i] = mLimitMax[i] = 0.0f; + } + + // Pass limits on to constraint part + mSwingTwistConstraintPart.SetLimits(mLimitMin[EAxis::RotationX], mLimitMax[EAxis::RotationX], mLimitMin[EAxis::RotationY], mLimitMax[EAxis::RotationY], mLimitMin[EAxis::RotationZ], mLimitMax[EAxis::RotationZ]); +} + +void SixDOFConstraint::UpdateFixedFreeAxis() +{ + uint8 old_free_axis = mFreeAxis; + uint8 old_fixed_axis = mFixedAxis; + + // Cache which axis are fixed and which ones are free + mFreeAxis = 0; + mFixedAxis = 0; + for (int a = 0; a < EAxis::Num; ++a) + { + float limit = a >= EAxis::RotationX? JPH_PI : FLT_MAX; + + if (mLimitMin[a] >= mLimitMax[a]) + mFixedAxis |= 1 << a; + else if (mLimitMin[a] <= -limit && mLimitMax[a] >= limit) + mFreeAxis |= 1 << a; + } + + // On change we deactivate all constraints to reset warm starting + if (old_free_axis != mFreeAxis || old_fixed_axis != mFixedAxis) + { + for (AxisConstraintPart &c : mTranslationConstraintPart) + c.Deactivate(); + mPointConstraintPart.Deactivate(); + mSwingTwistConstraintPart.Deactivate(); + mRotationConstraintPart.Deactivate(); + for (AxisConstraintPart &c : mMotorTranslationConstraintPart) + c.Deactivate(); + for (AngleConstraintPart &c : mMotorRotationConstraintPart) + c.Deactivate(); + } +} + +SixDOFConstraint::SixDOFConstraint(Body &inBody1, Body &inBody2, const SixDOFConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings) +{ + // Override swing type + mSwingTwistConstraintPart.SetSwingType(inSettings.mSwingType); + + // Calculate rotation needed to go from constraint space to body1 local space + Vec3 axis_z1 = inSettings.mAxisX1.Cross(inSettings.mAxisY1); + Mat44 c_to_b1(Vec4(inSettings.mAxisX1, 0), Vec4(inSettings.mAxisY1, 0), Vec4(axis_z1, 0), Vec4(0, 0, 0, 1)); + mConstraintToBody1 = c_to_b1.GetQuaternion(); + + // Calculate rotation needed to go from constraint space to body2 local space + Vec3 axis_z2 = inSettings.mAxisX2.Cross(inSettings.mAxisY2); + Mat44 c_to_b2(Vec4(inSettings.mAxisX2, 0), Vec4(inSettings.mAxisY2, 0), Vec4(axis_z2, 0), Vec4(0, 0, 0, 1)); + mConstraintToBody2 = c_to_b2.GetQuaternion(); + + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPosition1); + mConstraintToBody1 = inBody1.GetRotation().Conjugated() * mConstraintToBody1; + + mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPosition2); + mConstraintToBody2 = inBody2.GetRotation().Conjugated() * mConstraintToBody2; + } + else + { + mLocalSpacePosition1 = Vec3(inSettings.mPosition1); + mLocalSpacePosition2 = Vec3(inSettings.mPosition2); + } + + // Copy translation and rotation limits + memcpy(mLimitMin, inSettings.mLimitMin, sizeof(mLimitMin)); + memcpy(mLimitMax, inSettings.mLimitMax, sizeof(mLimitMax)); + memcpy(mLimitsSpringSettings, inSettings.mLimitsSpringSettings, sizeof(mLimitsSpringSettings)); + UpdateTranslationLimits(); + UpdateRotationLimits(); + UpdateFixedFreeAxis(); + CacheHasSpringLimits(); + + // Store friction settings + memcpy(mMaxFriction, inSettings.mMaxFriction, sizeof(mMaxFriction)); + + // Store motor settings + for (int i = 0; i < EAxis::Num; ++i) + mMotorSettings[i] = inSettings.mMotorSettings[i]; + + // Cache if motors are active (motors are off initially, but we may have friction) + CacheTranslationMotorActive(); + CacheRotationMotorActive(); +} + +void SixDOFConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +void SixDOFConstraint::SetTranslationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax) +{ + mLimitMin[EAxis::TranslationX] = inLimitMin.GetX(); + mLimitMin[EAxis::TranslationY] = inLimitMin.GetY(); + mLimitMin[EAxis::TranslationZ] = inLimitMin.GetZ(); + mLimitMax[EAxis::TranslationX] = inLimitMax.GetX(); + mLimitMax[EAxis::TranslationY] = inLimitMax.GetY(); + mLimitMax[EAxis::TranslationZ] = inLimitMax.GetZ(); + + UpdateTranslationLimits(); + UpdateFixedFreeAxis(); +} + +void SixDOFConstraint::SetRotationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax) +{ + mLimitMin[EAxis::RotationX] = inLimitMin.GetX(); + mLimitMin[EAxis::RotationY] = inLimitMin.GetY(); + mLimitMin[EAxis::RotationZ] = inLimitMin.GetZ(); + mLimitMax[EAxis::RotationX] = inLimitMax.GetX(); + mLimitMax[EAxis::RotationY] = inLimitMax.GetY(); + mLimitMax[EAxis::RotationZ] = inLimitMax.GetZ(); + + UpdateRotationLimits(); + UpdateFixedFreeAxis(); +} + +void SixDOFConstraint::SetMaxFriction(EAxis inAxis, float inFriction) +{ + mMaxFriction[inAxis] = inFriction; + + if (inAxis >= EAxis::TranslationX && inAxis <= EAxis::TranslationZ) + CacheTranslationMotorActive(); + else + CacheRotationMotorActive(); +} + +void SixDOFConstraint::GetPositionConstraintProperties(Vec3 &outR1PlusU, Vec3 &outR2, Vec3 &outU) const +{ + RVec3 p1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1; + RVec3 p2 = mBody2->GetCenterOfMassTransform() * mLocalSpacePosition2; + outR1PlusU = Vec3(p2 - mBody1->GetCenterOfMassPosition()); // r1 + u = (p1 - x1) + (p2 - p1) = p2 - x1 + outR2 = Vec3(p2 - mBody2->GetCenterOfMassPosition()); + outU = Vec3(p2 - p1); +} + +Quat SixDOFConstraint::GetRotationInConstraintSpace() const +{ + // Let b1, b2 be the center of mass transform of body1 and body2 (For body1 this is mBody1->GetCenterOfMassTransform()) + // Let c1, c2 be the transform that takes a vector from constraint space to local space of body1 and body2 (For body1 this is Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1)) + // Let q be the rotation of the constraint in constraint space + // b2 takes a vector from the local space of body2 to world space + // To express this in terms of b1: b2 = b1 * c1 * q * c2^-1 + // c2^-1 goes from local body 2 space to constraint space + // q rotates the constraint + // c1 goes from constraint space to body 1 local space + // b1 goes from body 1 local space to world space + // So when the body rotations are given, q = (b1 * c1)^-1 * b2 c2 + // Or: q = (q1 * c1)^-1 * (q2 * c2) if we're only interested in rotations + return (mBody1->GetRotation() * mConstraintToBody1).Conjugated() * mBody2->GetRotation() * mConstraintToBody2; +} + +void SixDOFConstraint::CacheTranslationMotorActive() +{ + mTranslationMotorActive = mMotorState[EAxis::TranslationX] != EMotorState::Off + || mMotorState[EAxis::TranslationY] != EMotorState::Off + || mMotorState[EAxis::TranslationZ] != EMotorState::Off + || HasFriction(EAxis::TranslationX) + || HasFriction(EAxis::TranslationY) + || HasFriction(EAxis::TranslationZ); +} + +void SixDOFConstraint::CacheRotationMotorActive() +{ + mRotationMotorActive = mMotorState[EAxis::RotationX] != EMotorState::Off + || mMotorState[EAxis::RotationY] != EMotorState::Off + || mMotorState[EAxis::RotationZ] != EMotorState::Off + || HasFriction(EAxis::RotationX) + || HasFriction(EAxis::RotationY) + || HasFriction(EAxis::RotationZ); +} + +void SixDOFConstraint::CacheRotationPositionMotorActive() +{ + mRotationPositionMotorActive = 0; + for (int i = 0; i < 3; ++i) + if (mMotorState[EAxis::RotationX + i] == EMotorState::Position) + mRotationPositionMotorActive |= 1 << i; +} + +void SixDOFConstraint::CacheHasSpringLimits() +{ + mHasSpringLimits = mLimitsSpringSettings[EAxis::TranslationX].mFrequency > 0.0f + || mLimitsSpringSettings[EAxis::TranslationY].mFrequency > 0.0f + || mLimitsSpringSettings[EAxis::TranslationZ].mFrequency > 0.0f; +} + +void SixDOFConstraint::SetMotorState(EAxis inAxis, EMotorState inState) +{ + JPH_ASSERT(inState == EMotorState::Off || mMotorSettings[inAxis].IsValid()); + + if (mMotorState[inAxis] != inState) + { + mMotorState[inAxis] = inState; + + // Ensure that warm starting next frame doesn't apply any impulses (motor parts are repurposed for different modes) + if (inAxis >= EAxis::TranslationX && inAxis <= EAxis::TranslationZ) + { + mMotorTranslationConstraintPart[inAxis - EAxis::TranslationX].Deactivate(); + + CacheTranslationMotorActive(); + } + else + { + JPH_ASSERT(inAxis >= EAxis::RotationX && inAxis <= EAxis::RotationZ); + + mMotorRotationConstraintPart[inAxis - EAxis::RotationX].Deactivate(); + + CacheRotationMotorActive(); + CacheRotationPositionMotorActive(); + } + } +} + +void SixDOFConstraint::SetTargetOrientationCS(QuatArg inOrientation) +{ + Quat q_swing, q_twist; + inOrientation.GetSwingTwist(q_swing, q_twist); + + uint clamped_axis; + mSwingTwistConstraintPart.ClampSwingTwist(q_swing, q_twist, clamped_axis); + + if (clamped_axis != 0) + mTargetOrientation = q_swing * q_twist; + else + mTargetOrientation = inOrientation; +} + +void SixDOFConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Get body rotations + Quat rotation1 = mBody1->GetRotation(); + Quat rotation2 = mBody2->GetRotation(); + + // Quaternion that rotates from body1's constraint space to world space + Quat constraint_body1_to_world = rotation1 * mConstraintToBody1; + + // Store world space axis of constraint space + Mat44 translation_axis_mat = Mat44::sRotation(constraint_body1_to_world); + for (int i = 0; i < 3; ++i) + mTranslationAxis[i] = translation_axis_mat.GetColumn3(i); + + if (IsTranslationFullyConstrained()) + { + // All translation locked: Setup point constraint + mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(rotation1), mLocalSpacePosition1, *mBody2, Mat44::sRotation(rotation2), mLocalSpacePosition2); + } + else if (IsTranslationConstrained() || mTranslationMotorActive) + { + // Update world space positions (the bodies may have moved) + Vec3 r1_plus_u, r2, u; + GetPositionConstraintProperties(r1_plus_u, r2, u); + + // Setup axis constraint parts + for (int i = 0; i < 3; ++i) + { + EAxis axis = EAxis(EAxis::TranslationX + i); + + Vec3 translation_axis = mTranslationAxis[i]; + + // Calculate displacement along this axis + float d = translation_axis.Dot(u); + mDisplacement[i] = d; // Store for SolveVelocityConstraint + + // Setup limit constraint + bool constraint_active = false; + float constraint_value = 0.0f; + if (IsFixedAxis(axis)) + { + // When constraint is fixed it is always active + constraint_value = d - mLimitMin[i]; + constraint_active = true; + } + else if (!IsFreeAxis(axis)) + { + // When constraint is limited, it is only active when outside of the allowed range + if (d <= mLimitMin[i]) + { + constraint_value = d - mLimitMin[i]; + constraint_active = true; + } + else if (d >= mLimitMax[i]) + { + constraint_value = d - mLimitMax[i]; + constraint_active = true; + } + } + + if (constraint_active) + mTranslationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, translation_axis, 0.0f, constraint_value, mLimitsSpringSettings[i]); + else + mTranslationConstraintPart[i].Deactivate(); + + // Setup motor constraint + switch (mMotorState[i]) + { + case EMotorState::Off: + if (HasFriction(axis)) + mMotorTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis); + else + mMotorTranslationConstraintPart[i].Deactivate(); + break; + + case EMotorState::Velocity: + mMotorTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis, -mTargetVelocity[i]); + break; + + case EMotorState::Position: + { + const SpringSettings &spring_settings = mMotorSettings[i].mSpringSettings; + if (spring_settings.HasStiffness()) + mMotorTranslationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, translation_axis, 0.0f, translation_axis.Dot(u) - mTargetPosition[i], spring_settings); + else + mMotorTranslationConstraintPart[i].Deactivate(); + break; + } + } + } + } + + // Setup rotation constraints + if (IsRotationFullyConstrained()) + { + // All rotation locked: Setup rotation constraint + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation())); + } + else if (IsRotationConstrained() || mRotationMotorActive) + { + // GetRotationInConstraintSpace without redoing the calculation of constraint_body1_to_world + Quat constraint_body2_to_world = mBody2->GetRotation() * mConstraintToBody2; + Quat q = constraint_body1_to_world.Conjugated() * constraint_body2_to_world; + + // Use swing twist constraint part + if (IsRotationConstrained()) + mSwingTwistConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, q, constraint_body1_to_world); + else + mSwingTwistConstraintPart.Deactivate(); + + if (mRotationMotorActive) + { + // Calculate rotation motor axis + Mat44 ws_axis = Mat44::sRotation(constraint_body2_to_world); + for (int i = 0; i < 3; ++i) + mRotationAxis[i] = ws_axis.GetColumn3(i); + + // Get target orientation along the shortest path from q + Quat target_orientation = q.Dot(mTargetOrientation) > 0.0f? mTargetOrientation : -mTargetOrientation; + + // The definition of the constraint rotation q: + // R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q (1) + // + // R2' is the rotation of body 2 when reaching the target_orientation: + // R2' * ConstraintToBody2 = R1 * ConstraintToBody1 * target_orientation (2) + // + // The difference in body 2 space: + // R2' = R2 * diff_body2 (3) + // + // We want to specify the difference in the constraint space of body 2: + // diff_body2 = ConstraintToBody2 * diff * ConstraintToBody2^* (4) + // + // Extracting R2' from 2: R2' = R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* (5) + // Combining 3 & 4: R2' = R2 * ConstraintToBody2 * diff * ConstraintToBody2^* (6) + // Combining 1 & 6: R2' = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^* (7) + // Combining 5 & 7: R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^* + // <=> target_orientation = q * diff + // <=> diff = q^* * target_orientation + Quat diff = q.Conjugated() * target_orientation; + + // Project diff so that only rotation around axis that have a position motor are remaining + Quat projected_diff; + switch (mRotationPositionMotorActive) + { + case 0b001: + // Keep only rotation around X + projected_diff = diff.GetTwist(Vec3::sAxisX()); + break; + + case 0b010: + // Keep only rotation around Y + projected_diff = diff.GetTwist(Vec3::sAxisY()); + break; + + case 0b100: + // Keep only rotation around Z + projected_diff = diff.GetTwist(Vec3::sAxisZ()); + break; + + case 0b011: + // Remove rotation around Z + // q = swing_xy * twist_z <=> swing_xy = q * twist_z^* + projected_diff = diff * diff.GetTwist(Vec3::sAxisZ()).Conjugated(); + break; + + case 0b101: + // Remove rotation around Y + // q = swing_xz * twist_y <=> swing_xz = q * twist_y^* + projected_diff = diff * diff.GetTwist(Vec3::sAxisY()).Conjugated(); + break; + + case 0b110: + // Remove rotation around X + // q = swing_yz * twist_x <=> swing_yz = q * twist_x^* + projected_diff = diff * diff.GetTwist(Vec3::sAxisX()).Conjugated(); + break; + + case 0b111: + default: // All motors off is handled here but the results are unused + // Keep entire rotation + projected_diff = diff; + break; + } + + // Approximate error angles + // The imaginary part of a quaternion is rotation_axis * sin(angle / 2) + // If angle is small, sin(x) = x so angle[i] ~ 2.0f * rotation_axis[i] + // We'll be making small time steps, so if the angle is not small at least the sign will be correct and we'll move in the right direction + Vec3 rotation_error = -2.0f * projected_diff.GetXYZ(); + + // Setup motors + for (int i = 0; i < 3; ++i) + { + EAxis axis = EAxis(EAxis::RotationX + i); + + Vec3 rotation_axis = mRotationAxis[i]; + + switch (mMotorState[axis]) + { + case EMotorState::Off: + if (HasFriction(axis)) + mMotorRotationConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, rotation_axis); + else + mMotorRotationConstraintPart[i].Deactivate(); + break; + + case EMotorState::Velocity: + mMotorRotationConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, rotation_axis, -mTargetAngularVelocity[i]); + break; + + case EMotorState::Position: + { + const SpringSettings &spring_settings = mMotorSettings[axis].mSpringSettings; + if (spring_settings.HasStiffness()) + mMotorRotationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, rotation_axis, 0.0f, rotation_error[i], spring_settings); + else + mMotorRotationConstraintPart[i].Deactivate(); + break; + } + } + } + } + } +} + +void SixDOFConstraint::ResetWarmStart() +{ + for (AxisConstraintPart &c : mMotorTranslationConstraintPart) + c.Deactivate(); + for (AngleConstraintPart &c : mMotorRotationConstraintPart) + c.Deactivate(); + mRotationConstraintPart.Deactivate(); + mSwingTwistConstraintPart.Deactivate(); + mPointConstraintPart.Deactivate(); + for (AxisConstraintPart &c : mTranslationConstraintPart) + c.Deactivate(); +} + +void SixDOFConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm start translation motors + if (mTranslationMotorActive) + for (int i = 0; i < 3; ++i) + if (mMotorTranslationConstraintPart[i].IsActive()) + mMotorTranslationConstraintPart[i].WarmStart(*mBody1, *mBody2, mTranslationAxis[i], inWarmStartImpulseRatio); + + // Warm start rotation motors + if (mRotationMotorActive) + for (AngleConstraintPart &c : mMotorRotationConstraintPart) + if (c.IsActive()) + c.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + + // Warm start rotation constraints + if (IsRotationFullyConstrained()) + mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + else if (IsRotationConstrained()) + mSwingTwistConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + + // Warm start translation constraints + if (IsTranslationFullyConstrained()) + mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + else if (IsTranslationConstrained()) + for (int i = 0; i < 3; ++i) + if (mTranslationConstraintPart[i].IsActive()) + mTranslationConstraintPart[i].WarmStart(*mBody1, *mBody2, mTranslationAxis[i], inWarmStartImpulseRatio); +} + +bool SixDOFConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + bool impulse = false; + + // Solve translation motor + if (mTranslationMotorActive) + for (int i = 0; i < 3; ++i) + if (mMotorTranslationConstraintPart[i].IsActive()) + switch (mMotorState[i]) + { + case EMotorState::Off: + { + // Apply friction only + float max_lambda = mMaxFriction[i] * inDeltaTime; + impulse |= mMotorTranslationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mTranslationAxis[i], -max_lambda, max_lambda); + break; + } + + case EMotorState::Velocity: + case EMotorState::Position: + // Drive motor + impulse |= mMotorTranslationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mTranslationAxis[i], inDeltaTime * mMotorSettings[i].mMinForceLimit, inDeltaTime * mMotorSettings[i].mMaxForceLimit); + break; + } + + // Solve rotation motor + if (mRotationMotorActive) + for (int i = 0; i < 3; ++i) + { + EAxis axis = EAxis(EAxis::RotationX + i); + if (mMotorRotationConstraintPart[i].IsActive()) + switch (mMotorState[axis]) + { + case EMotorState::Off: + { + // Apply friction only + float max_lambda = mMaxFriction[axis] * inDeltaTime; + impulse |= mMotorRotationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mRotationAxis[i], -max_lambda, max_lambda); + break; + } + + case EMotorState::Velocity: + case EMotorState::Position: + // Drive motor + impulse |= mMotorRotationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mRotationAxis[i], inDeltaTime * mMotorSettings[axis].mMinTorqueLimit, inDeltaTime * mMotorSettings[axis].mMaxTorqueLimit); + break; + } + } + + // Solve rotation constraint + if (IsRotationFullyConstrained()) + impulse |= mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + else if (IsRotationConstrained()) + impulse |= mSwingTwistConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + // Solve position constraint + if (IsTranslationFullyConstrained()) + impulse |= mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + else if (IsTranslationConstrained()) + for (int i = 0; i < 3; ++i) + if (mTranslationConstraintPart[i].IsActive()) + { + // If the axis is not fixed it must be limited (or else the constraint would not be active) + // Calculate the min and max constraint force based on on which side we're limited + float limit_min = -FLT_MAX, limit_max = FLT_MAX; + if (!IsFixedAxis(EAxis(EAxis::TranslationX + i))) + { + JPH_ASSERT(!IsFreeAxis(EAxis(EAxis::TranslationX + i))); + if (mDisplacement[i] <= mLimitMin[i]) + limit_min = 0; + else if (mDisplacement[i] >= mLimitMax[i]) + limit_max = 0; + } + + impulse |= mTranslationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mTranslationAxis[i], limit_min, limit_max); + } + + return impulse; +} + +bool SixDOFConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + bool impulse = false; + + if (IsRotationFullyConstrained()) + { + // Rotation locked: Solve rotation constraint + + // Inverse of initial rotation from body 1 to body 2 in body 1 space + // Definition of initial orientation r0: q2 = q1 r0 + // Initial rotation (see: GetRotationInConstraintSpace): q2 = q1 c1 c2^-1 + // So: r0^-1 = (c1 c2^-1)^-1 = c2 * c1^-1 + Quat constraint_to_body1 = mConstraintToBody1 * Quat::sEulerAngles(GetRotationLimitsMin()); + Quat inv_initial_orientation = mConstraintToBody2 * constraint_to_body1.Conjugated(); + + // Solve rotation violations + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation())); + impulse |= mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inv_initial_orientation, inBaumgarte); + } + else if (IsRotationConstrained()) + { + // Rotation partially constraint + + // Solve rotation violations + Quat q = GetRotationInConstraintSpace(); + impulse |= mSwingTwistConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, q, mConstraintToBody1, mConstraintToBody2, inBaumgarte); + } + + // Solve position violations + if (IsTranslationFullyConstrained()) + { + // Translation locked: Solve point constraint + Vec3 local_space_position1 = mLocalSpacePosition1 + mConstraintToBody1 * GetTranslationLimitsMin(); + mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), local_space_position1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2); + impulse |= mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); + } + else if (IsTranslationConstrained()) + { + // Translation partially locked: Solve per axis + for (int i = 0; i < 3; ++i) + if (mLimitsSpringSettings[i].mFrequency <= 0.0f) // If not soft limit + { + // Update world space positions (the bodies may have moved) + Vec3 r1_plus_u, r2, u; + GetPositionConstraintProperties(r1_plus_u, r2, u); + + // Quaternion that rotates from body1's constraint space to world space + Quat constraint_body1_to_world = mBody1->GetRotation() * mConstraintToBody1; + + // Calculate axis + Vec3 translation_axis; + switch (i) + { + case 0: translation_axis = constraint_body1_to_world.RotateAxisX(); break; + case 1: translation_axis = constraint_body1_to_world.RotateAxisY(); break; + default: JPH_ASSERT(i == 2); translation_axis = constraint_body1_to_world.RotateAxisZ(); break; + } + + // Determine position error + float error = 0.0f; + EAxis axis(EAxis(EAxis::TranslationX + i)); + if (IsFixedAxis(axis)) + error = u.Dot(translation_axis) - mLimitMin[axis]; + else if (!IsFreeAxis(axis)) + { + float displacement = u.Dot(translation_axis); + if (displacement <= mLimitMin[axis]) + error = displacement - mLimitMin[axis]; + else if (displacement >= mLimitMax[axis]) + error = displacement - mLimitMax[axis]; + } + + if (error != 0.0f) + { + // Setup axis constraint part and solve it + mTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis); + impulse |= mTranslationConstraintPart[i].SolvePositionConstraint(*mBody1, *mBody2, translation_axis, error, inBaumgarte); + } + } + } + + return impulse; +} + +#ifdef JPH_DEBUG_RENDERER +void SixDOFConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + // Get constraint properties in world space + RVec3 position1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1; + Quat rotation1 = mBody1->GetRotation() * mConstraintToBody1; + Quat rotation2 = mBody2->GetRotation() * mConstraintToBody2; + + // Draw constraint orientation + inRenderer->DrawCoordinateSystem(RMat44::sRotationTranslation(rotation1, position1), mDrawConstraintSize); + + if ((IsRotationConstrained() || mRotationPositionMotorActive != 0) && !IsRotationFullyConstrained()) + { + // Draw current swing and twist + Quat q = GetRotationInConstraintSpace(); + Quat q_swing, q_twist; + q.GetSwingTwist(q_swing, q_twist); + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_twist).RotateAxisY(), Color::sWhite); + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_swing).RotateAxisX(), Color::sWhite); + } + + // Draw target rotation + Quat m_swing, m_twist; + mTargetOrientation.GetSwingTwist(m_swing, m_twist); + if (mMotorState[EAxis::RotationX] == EMotorState::Position) + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * m_twist).RotateAxisY(), Color::sYellow); + if (mMotorState[EAxis::RotationY] == EMotorState::Position || mMotorState[EAxis::RotationZ] == EMotorState::Position) + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * m_swing).RotateAxisX(), Color::sYellow); + + // Draw target angular velocity + Vec3 target_angular_velocity = Vec3::sZero(); + for (int i = 0; i < 3; ++i) + if (mMotorState[EAxis::RotationX + i] == EMotorState::Velocity) + target_angular_velocity.SetComponent(i, mTargetAngularVelocity[i]); + if (target_angular_velocity != Vec3::sZero()) + inRenderer->DrawArrow(position1, position1 + rotation2 * target_angular_velocity, Color::sRed, 0.1f); +} + +void SixDOFConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const +{ + // Get matrix that transforms from constraint space to world space + RMat44 constraint_body1_to_world = RMat44::sRotationTranslation(mBody1->GetRotation() * mConstraintToBody1, mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1); + + // Draw limits + if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Pyramid) + inRenderer->DrawSwingPyramidLimits(constraint_body1_to_world, mLimitMin[EAxis::RotationY], mLimitMax[EAxis::RotationY], mLimitMin[EAxis::RotationZ], mLimitMax[EAxis::RotationZ], mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off); + else + inRenderer->DrawSwingConeLimits(constraint_body1_to_world, mLimitMax[EAxis::RotationY], mLimitMax[EAxis::RotationZ], mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off); + inRenderer->DrawPie(constraint_body1_to_world.GetTranslation(), mDrawConstraintSize, constraint_body1_to_world.GetAxisX(), constraint_body1_to_world.GetAxisY(), mLimitMin[EAxis::RotationX], mLimitMax[EAxis::RotationX], Color::sPurple, DebugRenderer::ECastShadow::Off); +} +#endif // JPH_DEBUG_RENDERER + +void SixDOFConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + for (const AxisConstraintPart &c : mTranslationConstraintPart) + c.SaveState(inStream); + mPointConstraintPart.SaveState(inStream); + mSwingTwistConstraintPart.SaveState(inStream); + mRotationConstraintPart.SaveState(inStream); + for (const AxisConstraintPart &c : mMotorTranslationConstraintPart) + c.SaveState(inStream); + for (const AngleConstraintPart &c : mMotorRotationConstraintPart) + c.SaveState(inStream); + + inStream.Write(mMotorState); + inStream.Write(mTargetVelocity); + inStream.Write(mTargetAngularVelocity); + inStream.Write(mTargetPosition); + inStream.Write(mTargetOrientation); +} + +void SixDOFConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + for (AxisConstraintPart &c : mTranslationConstraintPart) + c.RestoreState(inStream); + mPointConstraintPart.RestoreState(inStream); + mSwingTwistConstraintPart.RestoreState(inStream); + mRotationConstraintPart.RestoreState(inStream); + for (AxisConstraintPart &c : mMotorTranslationConstraintPart) + c.RestoreState(inStream); + for (AngleConstraintPart &c : mMotorRotationConstraintPart) + c.RestoreState(inStream); + + inStream.Read(mMotorState); + inStream.Read(mTargetVelocity); + inStream.Read(mTargetAngularVelocity); + inStream.Read(mTargetPosition); + inStream.Read(mTargetOrientation); + + CacheTranslationMotorActive(); + CacheRotationMotorActive(); + CacheRotationPositionMotorActive(); +} + +Ref SixDOFConstraint::GetConstraintSettings() const +{ + SixDOFConstraintSettings *settings = new SixDOFConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPosition1 = RVec3(mLocalSpacePosition1); + settings->mAxisX1 = mConstraintToBody1.RotateAxisX(); + settings->mAxisY1 = mConstraintToBody1.RotateAxisY(); + settings->mPosition2 = RVec3(mLocalSpacePosition2); + settings->mAxisX2 = mConstraintToBody2.RotateAxisX(); + settings->mAxisY2 = mConstraintToBody2.RotateAxisY(); + settings->mSwingType = mSwingTwistConstraintPart.GetSwingType(); + memcpy(settings->mLimitMin, mLimitMin, sizeof(mLimitMin)); + memcpy(settings->mLimitMax, mLimitMax, sizeof(mLimitMax)); + memcpy(settings->mMaxFriction, mMaxFriction, sizeof(mMaxFriction)); + for (int i = 0; i < EAxis::Num; ++i) + settings->mMotorSettings[i] = mMotorSettings[i]; + return settings; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.h new file mode 100644 index 000000000000..2ebb9856bda7 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.h @@ -0,0 +1,289 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// 6 Degree Of Freedom Constraint setup structure. Allows control over each of the 6 degrees of freedom. +class JPH_EXPORT SixDOFConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, SixDOFConstraintSettings) + +public: + /// Constraint is split up into translation/rotation around X, Y and Z axis. + enum EAxis + { + TranslationX, + TranslationY, + TranslationZ, + + RotationX, + RotationY, + RotationZ, + + Num, + NumTranslation = TranslationZ + 1, + }; + + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 constraint reference frame (space determined by mSpace) + RVec3 mPosition1 = RVec3::sZero(); + Vec3 mAxisX1 = Vec3::sAxisX(); + Vec3 mAxisY1 = Vec3::sAxisY(); + + /// Body 2 constraint reference frame (space determined by mSpace) + RVec3 mPosition2 = RVec3::sZero(); + Vec3 mAxisX2 = Vec3::sAxisX(); + Vec3 mAxisY2 = Vec3::sAxisY(); + + /// Friction settings. + /// For translation: Max friction force in N. 0 = no friction. + /// For rotation: Max friction torque in Nm. 0 = no friction. + float mMaxFriction[EAxis::Num] = { 0, 0, 0, 0, 0, 0 }; + + /// The type of swing constraint that we want to use. + ESwingType mSwingType = ESwingType::Cone; + + /// Limits. + /// For translation: Min and max linear limits in m (0 is frame of body 1 and 2 coincide). + /// For rotation: Min and max angular limits in rad (0 is frame of body 1 and 2 coincide). See comments at Axis enum for limit ranges. + /// + /// Remove degree of freedom by setting min = FLT_MAX and max = -FLT_MAX. The constraint will be driven to 0 for this axis. + /// + /// Free movement over an axis is allowed when min = -FLT_MAX and max = FLT_MAX. + /// + /// Rotation limit around X-Axis: When limited, should be \f$\in [-\pi, \pi]\f$. Can be asymmetric around zero. + /// + /// Rotation limit around Y-Z Axis: Forms a pyramid or cone shaped limit: + /// * For pyramid, should be \f$\in [-\pi, \pi]\f$ and does not need to be symmetrical around zero. + /// * For cone should be \f$\in [0, \pi]\f$ and needs to be symmetrical around zero (min limit is assumed to be -max limit). + float mLimitMin[EAxis::Num] = { -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX }; + float mLimitMax[EAxis::Num] = { FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX }; + + /// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back. + /// Only soft translation limits are supported, soft rotation limits are not currently supported. + SpringSettings mLimitsSpringSettings[EAxis::NumTranslation]; + + /// Make axis free (unconstrained) + void MakeFreeAxis(EAxis inAxis) { mLimitMin[inAxis] = -FLT_MAX; mLimitMax[inAxis] = FLT_MAX; } + bool IsFreeAxis(EAxis inAxis) const { return mLimitMin[inAxis] == -FLT_MAX && mLimitMax[inAxis] == FLT_MAX; } + + /// Make axis fixed (fixed at value 0) + void MakeFixedAxis(EAxis inAxis) { mLimitMin[inAxis] = FLT_MAX; mLimitMax[inAxis] = -FLT_MAX; } + bool IsFixedAxis(EAxis inAxis) const { return mLimitMin[inAxis] >= mLimitMax[inAxis]; } + + /// Set a valid range for the constraint (if inMax < inMin, the axis will become fixed) + void SetLimitedAxis(EAxis inAxis, float inMin, float inMax) { mLimitMin[inAxis] = inMin; mLimitMax[inAxis] = inMax; } + + /// Motor settings for each axis + MotorSettings mMotorSettings[EAxis::Num]; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// 6 Degree Of Freedom Constraint. Allows control over each of the 6 degrees of freedom. +class JPH_EXPORT SixDOFConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Get Axis from settings class + using EAxis = SixDOFConstraintSettings::EAxis; + + /// Construct six DOF constraint + SixDOFConstraint(Body &inBody1, Body &inBody2, const SixDOFConstraintSettings &inSettings); + + /// Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::SixDOF; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; + virtual void DrawConstraintLimits(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override { return Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1); } + virtual Mat44 GetConstraintToBody2Matrix() const override { return Mat44::sRotationTranslation(mConstraintToBody2, mLocalSpacePosition2); } + + /// Update the translation limits for this constraint + void SetTranslationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax); + + /// Update the rotational limits for this constraint + void SetRotationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax); + + /// Get constraint Limits + float GetLimitsMin(EAxis inAxis) const { return mLimitMin[inAxis]; } + float GetLimitsMax(EAxis inAxis) const { return mLimitMax[inAxis]; } + Vec3 GetTranslationLimitsMin() const { return Vec3::sLoadFloat3Unsafe(*reinterpret_cast(&mLimitMin[EAxis::TranslationX])); } + Vec3 GetTranslationLimitsMax() const { return Vec3::sLoadFloat3Unsafe(*reinterpret_cast(&mLimitMax[EAxis::TranslationX])); } + Vec3 GetRotationLimitsMin() const { return Vec3::sLoadFloat3Unsafe(*reinterpret_cast(&mLimitMin[EAxis::RotationX])); } + Vec3 GetRotationLimitsMax() const { return Vec3::sLoadFloat3Unsafe(*reinterpret_cast(&mLimitMax[EAxis::RotationX])); } + + /// Check which axis are fixed/free + inline bool IsFixedAxis(EAxis inAxis) const { return (mFixedAxis & (1 << inAxis)) != 0; } + inline bool IsFreeAxis(EAxis inAxis) const { return (mFreeAxis & (1 << inAxis)) != 0; } + + /// Update the limits spring settings + const SpringSettings & GetLimitsSpringSettings(EAxis inAxis) const { JPH_ASSERT(inAxis < EAxis::NumTranslation); return mLimitsSpringSettings[inAxis]; } + void SetLimitsSpringSettings(EAxis inAxis, const SpringSettings& inLimitsSpringSettings) { JPH_ASSERT(inAxis < EAxis::NumTranslation); mLimitsSpringSettings[inAxis] = inLimitsSpringSettings; CacheHasSpringLimits(); } + + /// Set the max friction for each axis + void SetMaxFriction(EAxis inAxis, float inFriction); + float GetMaxFriction(EAxis inAxis) const { return mMaxFriction[inAxis]; } + + /// Get rotation of constraint in constraint space + Quat GetRotationInConstraintSpace() const; + + /// Motor settings + MotorSettings & GetMotorSettings(EAxis inAxis) { return mMotorSettings[inAxis]; } + const MotorSettings & GetMotorSettings(EAxis inAxis) const { return mMotorSettings[inAxis]; } + + /// Motor controls. + /// Translation motors work in constraint space of body 1. + /// Rotation motors work in constraint space of body 2 (!). + void SetMotorState(EAxis inAxis, EMotorState inState); + EMotorState GetMotorState(EAxis inAxis) const { return mMotorState[inAxis]; } + + /// Set the target velocity in body 1 constraint space + Vec3 GetTargetVelocityCS() const { return mTargetVelocity; } + void SetTargetVelocityCS(Vec3Arg inVelocity) { mTargetVelocity = inVelocity; } + + /// Set the target angular velocity in body 2 constraint space (!) + void SetTargetAngularVelocityCS(Vec3Arg inAngularVelocity) { mTargetAngularVelocity = inAngularVelocity; } + Vec3 GetTargetAngularVelocityCS() const { return mTargetAngularVelocity; } + + /// Set the target position in body 1 constraint space + Vec3 GetTargetPositionCS() const { return mTargetPosition; } + void SetTargetPositionCS(Vec3Arg inPosition) { mTargetPosition = inPosition; } + + /// Set the target orientation in body 1 constraint space + void SetTargetOrientationCS(QuatArg inOrientation); + Quat GetTargetOrientationCS() const { return mTargetOrientation; } + + /// Set the target orientation in body space (R2 = R1 * inOrientation, where R1 and R2 are the world space rotations for body 1 and 2). + /// Solve: R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q (see SwingTwistConstraint::GetSwingTwist) and R2 = R1 * inOrientation for q. + void SetTargetOrientationBS(QuatArg inOrientation) { SetTargetOrientationCS(mConstraintToBody1.Conjugated() * inOrientation * mConstraintToBody2); } + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline Vec3 GetTotalLambdaPosition() const { return IsTranslationFullyConstrained()? mPointConstraintPart.GetTotalLambda() : Vec3(mTranslationConstraintPart[0].GetTotalLambda(), mTranslationConstraintPart[1].GetTotalLambda(), mTranslationConstraintPart[2].GetTotalLambda()); } + inline Vec3 GetTotalLambdaRotation() const { return IsRotationFullyConstrained()? mRotationConstraintPart.GetTotalLambda() : Vec3(mSwingTwistConstraintPart.GetTotalTwistLambda(), mSwingTwistConstraintPart.GetTotalSwingYLambda(), mSwingTwistConstraintPart.GetTotalSwingZLambda()); } + inline Vec3 GetTotalLambdaMotorTranslation() const { return Vec3(mMotorTranslationConstraintPart[0].GetTotalLambda(), mMotorTranslationConstraintPart[1].GetTotalLambda(), mMotorTranslationConstraintPart[2].GetTotalLambda()); } + inline Vec3 GetTotalLambdaMotorRotation() const { return Vec3(mMotorRotationConstraintPart[0].GetTotalLambda(), mMotorRotationConstraintPart[1].GetTotalLambda(), mMotorRotationConstraintPart[2].GetTotalLambda()); } + +private: + // Calculate properties needed for the position constraint + inline void GetPositionConstraintProperties(Vec3 &outR1PlusU, Vec3 &outR2, Vec3 &outU) const; + + // Sanitize the translation limits + inline void UpdateTranslationLimits(); + + // Propagate the rotation limits to the constraint part + inline void UpdateRotationLimits(); + + // Update the cached state of which axis are free and which ones are fixed + inline void UpdateFixedFreeAxis(); + + // Cache the state of mTranslationMotorActive + void CacheTranslationMotorActive(); + + // Cache the state of mRotationMotorActive + void CacheRotationMotorActive(); + + // Cache the state of mRotationPositionMotorActive + void CacheRotationPositionMotorActive(); + + /// Cache the state of mHasSpringLimits + void CacheHasSpringLimits(); + + // Constraint settings helper functions + inline bool IsTranslationConstrained() const { return (mFreeAxis & 0b111) != 0b111; } + inline bool IsTranslationFullyConstrained() const { return (mFixedAxis & 0b111) == 0b111 && !mHasSpringLimits; } + inline bool IsRotationConstrained() const { return (mFreeAxis & 0b111000) != 0b111000; } + inline bool IsRotationFullyConstrained() const { return (mFixedAxis & 0b111000) == 0b111000; } + inline bool HasFriction(EAxis inAxis) const { return !IsFixedAxis(inAxis) && mMaxFriction[inAxis] > 0.0f; } + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // Transforms from constraint space to body space + Quat mConstraintToBody1; + Quat mConstraintToBody2; + + // Limits + uint8 mFreeAxis = 0; // Bitmask of free axis (bit 0 = TranslationX) + uint8 mFixedAxis = 0; // Bitmask of fixed axis (bit 0 = TranslationX) + bool mTranslationMotorActive = false; // If any of the translational frictions / motors are active + bool mRotationMotorActive = false; // If any of the rotational frictions / motors are active + uint8 mRotationPositionMotorActive = 0; // Bitmask of axis that have position motor active (bit 0 = RotationX) + bool mHasSpringLimits = false; // If any of the limit springs have a non-zero frequency/stiffness + float mLimitMin[EAxis::Num]; + float mLimitMax[EAxis::Num]; + SpringSettings mLimitsSpringSettings[EAxis::NumTranslation]; + + // Motor settings for each axis + MotorSettings mMotorSettings[EAxis::Num]; + + // Friction settings for each axis + float mMaxFriction[EAxis::Num]; + + // Motor controls + EMotorState mMotorState[EAxis::Num] = { EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off }; + Vec3 mTargetVelocity = Vec3::sZero(); + Vec3 mTargetAngularVelocity = Vec3::sZero(); + Vec3 mTargetPosition = Vec3::sZero(); + Quat mTargetOrientation = Quat::sIdentity(); + + // RUN TIME PROPERTIES FOLLOW + + // Constraint space axis in world space + Vec3 mTranslationAxis[3]; + Vec3 mRotationAxis[3]; + + // Translation displacement (valid when translation axis has a range limit) + float mDisplacement[3]; + + // Individual constraint parts for translation, or a combined point constraint part if all axis are fixed + AxisConstraintPart mTranslationConstraintPart[3]; + PointConstraintPart mPointConstraintPart; + + // Individual constraint parts for rotation or a combined constraint part if rotation is fixed + SwingTwistConstraintPart mSwingTwistConstraintPart; + RotationEulerConstraintPart mRotationConstraintPart; + + // Motor or friction constraints + AxisConstraintPart mMotorTranslationConstraintPart[3]; + AngleConstraintPart mMotorRotationConstraintPart[3]; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SliderConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SliderConstraint.cpp new file mode 100644 index 000000000000..75335c32cba3 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SliderConstraint.cpp @@ -0,0 +1,501 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +using namespace literals; + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SliderConstraintSettings) +{ + JPH_ADD_BASE_CLASS(SliderConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(SliderConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mAutoDetectPoint) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mPoint1) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mSliderAxis1) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mNormalAxis1) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mPoint2) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mSliderAxis2) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mNormalAxis2) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mLimitsMin) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mLimitsMax) + JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mMode, "mSpringMode") + JPH_ADD_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mFrequency, "mFrequency") // Renaming attributes to stay compatible with old versions of the library + JPH_ADD_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mDamping, "mDamping") + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mMaxFrictionForce) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mMotorSettings) +} + +void SliderConstraintSettings::SetSliderAxis(Vec3Arg inSliderAxis) +{ + JPH_ASSERT(mSpace == EConstraintSpace::WorldSpace); + + mSliderAxis1 = mSliderAxis2 = inSliderAxis; + mNormalAxis1 = mNormalAxis2 = inSliderAxis.GetNormalizedPerpendicular(); +} + +void SliderConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mAutoDetectPoint); + inStream.Write(mPoint1); + inStream.Write(mSliderAxis1); + inStream.Write(mNormalAxis1); + inStream.Write(mPoint2); + inStream.Write(mSliderAxis2); + inStream.Write(mNormalAxis2); + inStream.Write(mLimitsMin); + inStream.Write(mLimitsMax); + inStream.Write(mMaxFrictionForce); + mLimitsSpringSettings.SaveBinaryState(inStream); + mMotorSettings.SaveBinaryState(inStream); +} + +void SliderConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mAutoDetectPoint); + inStream.Read(mPoint1); + inStream.Read(mSliderAxis1); + inStream.Read(mNormalAxis1); + inStream.Read(mPoint2); + inStream.Read(mSliderAxis2); + inStream.Read(mNormalAxis2); + inStream.Read(mLimitsMin); + inStream.Read(mLimitsMax); + inStream.Read(mMaxFrictionForce); + mLimitsSpringSettings.RestoreBinaryState(inStream); + mMotorSettings.RestoreBinaryState(inStream); +} + +TwoBodyConstraint *SliderConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new SliderConstraint(inBody1, inBody2, *this); +} + +SliderConstraint::SliderConstraint(Body &inBody1, Body &inBody2, const SliderConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mMaxFrictionForce(inSettings.mMaxFrictionForce), + mMotorSettings(inSettings.mMotorSettings) +{ + // Store inverse of initial rotation from body 1 to body 2 in body 1 space + mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientationXY(inSettings.mSliderAxis1, inSettings.mNormalAxis1, inSettings.mSliderAxis2, inSettings.mNormalAxis2); + + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + RMat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform(); + RMat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform(); + + if (inSettings.mAutoDetectPoint) + { + // Determine anchor point: If any of the bodies can never be dynamic use the other body as anchor point + RVec3 anchor; + if (!inBody1.CanBeKinematicOrDynamic()) + anchor = inBody2.GetCenterOfMassPosition(); + else if (!inBody2.CanBeKinematicOrDynamic()) + anchor = inBody1.GetCenterOfMassPosition(); + else + { + // Otherwise use weighted anchor point towards the lightest body + Real inv_m1 = Real(inBody1.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked()); + Real inv_m2 = Real(inBody2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked()); + Real total_inv_mass = inv_m1 + inv_m2; + if (total_inv_mass != 0.0_r) + anchor = (inv_m1 * inBody1.GetCenterOfMassPosition() + inv_m2 * inBody2.GetCenterOfMassPosition()) / total_inv_mass; + else + anchor = inBody1.GetCenterOfMassPosition(); + } + + // Store local positions + mLocalSpacePosition1 = Vec3(inv_transform1 * anchor); + mLocalSpacePosition2 = Vec3(inv_transform2 * anchor); + } + else + { + // Store local positions + mLocalSpacePosition1 = Vec3(inv_transform1 * inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inv_transform2 * inSettings.mPoint2); + } + + // If all properties were specified in world space, take them to local space now + mLocalSpaceSliderAxis1 = inv_transform1.Multiply3x3(inSettings.mSliderAxis1).Normalized(); + mLocalSpaceNormal1 = inv_transform1.Multiply3x3(inSettings.mNormalAxis1).Normalized(); + + // Constraints were specified in world space, so we should have replaced c1 with q10^-1 c1 and c2 with q20^-1 c2 + // => r0^-1 = (q20^-1 c2) (q10^-1 c1)^1 = q20^-1 (c2 c1^-1) q10 + mInvInitialOrientation = inBody2.GetRotation().Conjugated() * mInvInitialOrientation * inBody1.GetRotation(); + } + else + { + // Store local positions + mLocalSpacePosition1 = Vec3(inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inSettings.mPoint2); + + // Store local space axis + mLocalSpaceSliderAxis1 = inSettings.mSliderAxis1; + mLocalSpaceNormal1 = inSettings.mNormalAxis1; + } + + // Calculate 2nd local space normal + mLocalSpaceNormal2 = mLocalSpaceSliderAxis1.Cross(mLocalSpaceNormal1); + + // Store limits + JPH_ASSERT(inSettings.mLimitsMin != inSettings.mLimitsMax || inSettings.mLimitsSpringSettings.mFrequency > 0.0f, "Better use a fixed constraint"); + SetLimits(inSettings.mLimitsMin, inSettings.mLimitsMax); + + // Store spring settings + SetLimitsSpringSettings(inSettings.mLimitsSpringSettings); +} + +void SliderConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +float SliderConstraint::GetCurrentPosition() const +{ + // See: CalculateR1R2U and CalculateSlidingAxisAndPosition + Vec3 r1 = mBody1->GetRotation() * mLocalSpacePosition1; + Vec3 r2 = mBody2->GetRotation() * mLocalSpacePosition2; + Vec3 u = Vec3(mBody2->GetCenterOfMassPosition() - mBody1->GetCenterOfMassPosition()) + r2 - r1; + return u.Dot(mBody1->GetRotation() * mLocalSpaceSliderAxis1); +} + +void SliderConstraint::SetLimits(float inLimitsMin, float inLimitsMax) +{ + JPH_ASSERT(inLimitsMin <= 0.0f); + JPH_ASSERT(inLimitsMax >= 0.0f); + mLimitsMin = inLimitsMin; + mLimitsMax = inLimitsMax; + mHasLimits = mLimitsMin != -FLT_MAX || mLimitsMax != FLT_MAX; +} + +void SliderConstraint::CalculateR1R2U(Mat44Arg inRotation1, Mat44Arg inRotation2) +{ + // Calculate points relative to body + mR1 = inRotation1 * mLocalSpacePosition1; + mR2 = inRotation2 * mLocalSpacePosition2; + + // Calculate X2 + R2 - X1 - R1 + mU = Vec3(mBody2->GetCenterOfMassPosition() - mBody1->GetCenterOfMassPosition()) + mR2 - mR1; +} + +void SliderConstraint::CalculatePositionConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2) +{ + // Calculate world space normals + mN1 = inRotation1 * mLocalSpaceNormal1; + mN2 = inRotation1 * mLocalSpaceNormal2; + + mPositionConstraintPart.CalculateConstraintProperties(*mBody1, inRotation1, mR1 + mU, *mBody2, inRotation2, mR2, mN1, mN2); +} + +void SliderConstraint::CalculateSlidingAxisAndPosition(Mat44Arg inRotation1) +{ + if (mHasLimits || mMotorState != EMotorState::Off || mMaxFrictionForce > 0.0f) + { + // Calculate world space slider axis + mWorldSpaceSliderAxis = inRotation1 * mLocalSpaceSliderAxis1; + + // Calculate slide distance along axis + mD = mU.Dot(mWorldSpaceSliderAxis); + } +} + +void SliderConstraint::CalculatePositionLimitsConstraintProperties(float inDeltaTime) +{ + // Check if distance is within limits + bool below_min = mD <= mLimitsMin; + if (mHasLimits && (below_min || mD >= mLimitsMax)) + mPositionLimitsConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, 0.0f, mD - (below_min? mLimitsMin : mLimitsMax), mLimitsSpringSettings); + else + mPositionLimitsConstraintPart.Deactivate(); +} + +void SliderConstraint::CalculateMotorConstraintProperties(float inDeltaTime) +{ + switch (mMotorState) + { + case EMotorState::Off: + if (mMaxFrictionForce > 0.0f) + mMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis); + else + mMotorConstraintPart.Deactivate(); + break; + + case EMotorState::Velocity: + mMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, -mTargetVelocity); + break; + + case EMotorState::Position: + if (mMotorSettings.mSpringSettings.HasStiffness()) + mMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, 0.0f, mD - mTargetPosition, mMotorSettings.mSpringSettings); + else + mMotorConstraintPart.Deactivate(); + break; + } +} + +void SliderConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Calculate constraint properties that are constant while bodies don't move + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + CalculateR1R2U(rotation1, rotation2); + CalculatePositionConstraintProperties(rotation1, rotation2); + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, *mBody2, rotation2); + CalculateSlidingAxisAndPosition(rotation1); + CalculatePositionLimitsConstraintProperties(inDeltaTime); + CalculateMotorConstraintProperties(inDeltaTime); +} + +void SliderConstraint::ResetWarmStart() +{ + mMotorConstraintPart.Deactivate(); + mPositionConstraintPart.Deactivate(); + mRotationConstraintPart.Deactivate(); + mPositionLimitsConstraintPart.Deactivate(); +} + +void SliderConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mMotorConstraintPart.WarmStart(*mBody1, *mBody2, mWorldSpaceSliderAxis, inWarmStartImpulseRatio); + mPositionConstraintPart.WarmStart(*mBody1, *mBody2, mN1, mN2, inWarmStartImpulseRatio); + mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mPositionLimitsConstraintPart.WarmStart(*mBody1, *mBody2, mWorldSpaceSliderAxis, inWarmStartImpulseRatio); +} + +bool SliderConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + // Solve motor + bool motor = false; + if (mMotorConstraintPart.IsActive()) + { + switch (mMotorState) + { + case EMotorState::Off: + { + float max_lambda = mMaxFrictionForce * inDeltaTime; + motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, -max_lambda, max_lambda); + break; + } + + case EMotorState::Velocity: + case EMotorState::Position: + motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, inDeltaTime * mMotorSettings.mMinForceLimit, inDeltaTime * mMotorSettings.mMaxForceLimit); + break; + } + } + + // Solve position constraint along 2 axis + bool pos = mPositionConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mN1, mN2); + + // Solve rotation constraint + bool rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + // Solve limits along slider axis + bool limit = false; + if (mPositionLimitsConstraintPart.IsActive()) + { + float min_lambda, max_lambda; + if (mLimitsMin == mLimitsMax) + { + min_lambda = -FLT_MAX; + max_lambda = FLT_MAX; + } + else if (mD <= mLimitsMin) + { + min_lambda = 0.0f; + max_lambda = FLT_MAX; + } + else + { + min_lambda = -FLT_MAX; + max_lambda = 0.0f; + } + limit = mPositionLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, min_lambda, max_lambda); + } + + return motor || pos || rot || limit; +} + +bool SliderConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + // Motor operates on velocities only, don't call SolvePositionConstraint + + // Solve position constraint along 2 axis + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + CalculateR1R2U(rotation1, rotation2); + CalculatePositionConstraintProperties(rotation1, rotation2); + bool pos = mPositionConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mU, mN1, mN2, inBaumgarte); + + // Solve rotation constraint + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation())); + bool rot = mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mInvInitialOrientation, inBaumgarte); + + // Solve limits along slider axis + bool limit = false; + if (mHasLimits && mLimitsSpringSettings.mFrequency <= 0.0f) + { + rotation1 = Mat44::sRotation(mBody1->GetRotation()); + rotation2 = Mat44::sRotation(mBody2->GetRotation()); + CalculateR1R2U(rotation1, rotation2); + CalculateSlidingAxisAndPosition(rotation1); + CalculatePositionLimitsConstraintProperties(inDeltaTime); + if (mPositionLimitsConstraintPart.IsActive()) + { + if (mD <= mLimitsMin) + limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, mD - mLimitsMin, inBaumgarte); + else + { + JPH_ASSERT(mD >= mLimitsMax); + limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, mD - mLimitsMax, inBaumgarte); + } + } + } + + return pos || rot || limit; +} + +#ifdef JPH_DEBUG_RENDERER +void SliderConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform(); + + // Transform the local positions into world space + Vec3 slider_axis = transform1.Multiply3x3(mLocalSpaceSliderAxis1); + RVec3 position1 = transform1 * mLocalSpacePosition1; + RVec3 position2 = transform2 * mLocalSpacePosition2; + + // Draw constraint + inRenderer->DrawMarker(position1, Color::sRed, 0.1f); + inRenderer->DrawMarker(position2, Color::sGreen, 0.1f); + inRenderer->DrawLine(position1, position2, Color::sGreen); + + // Draw motor + switch (mMotorState) + { + case EMotorState::Position: + inRenderer->DrawMarker(position1 + mTargetPosition * slider_axis, Color::sYellow, 1.0f); + break; + + case EMotorState::Velocity: + { + Vec3 cur_vel = (mBody2->GetLinearVelocity() - mBody1->GetLinearVelocity()).Dot(slider_axis) * slider_axis; + inRenderer->DrawLine(position2, position2 + cur_vel, Color::sBlue); + inRenderer->DrawArrow(position2 + cur_vel, position2 + mTargetVelocity * slider_axis, Color::sRed, 0.1f); + break; + } + + case EMotorState::Off: + break; + } +} + +void SliderConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const +{ + if (mHasLimits) + { + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform(); + + // Transform the local positions into world space + Vec3 slider_axis = transform1.Multiply3x3(mLocalSpaceSliderAxis1); + RVec3 position1 = transform1 * mLocalSpacePosition1; + RVec3 position2 = transform2 * mLocalSpacePosition2; + + // Calculate the limits in world space + RVec3 limits_min = position1 + mLimitsMin * slider_axis; + RVec3 limits_max = position1 + mLimitsMax * slider_axis; + + inRenderer->DrawLine(limits_min, position1, Color::sWhite); + inRenderer->DrawLine(position2, limits_max, Color::sWhite); + + inRenderer->DrawMarker(limits_min, Color::sWhite, 0.1f); + inRenderer->DrawMarker(limits_max, Color::sWhite, 0.1f); + } +} +#endif // JPH_DEBUG_RENDERER + +void SliderConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mMotorConstraintPart.SaveState(inStream); + mPositionConstraintPart.SaveState(inStream); + mRotationConstraintPart.SaveState(inStream); + mPositionLimitsConstraintPart.SaveState(inStream); + + inStream.Write(mMotorState); + inStream.Write(mTargetVelocity); + inStream.Write(mTargetPosition); +} + +void SliderConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mMotorConstraintPart.RestoreState(inStream); + mPositionConstraintPart.RestoreState(inStream); + mRotationConstraintPart.RestoreState(inStream); + mPositionLimitsConstraintPart.RestoreState(inStream); + + inStream.Read(mMotorState); + inStream.Read(mTargetVelocity); + inStream.Read(mTargetPosition); +} + +Ref SliderConstraint::GetConstraintSettings() const +{ + SliderConstraintSettings *settings = new SliderConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPoint1 = RVec3(mLocalSpacePosition1); + settings->mSliderAxis1 = mLocalSpaceSliderAxis1; + settings->mNormalAxis1 = mLocalSpaceNormal1; + settings->mPoint2 = RVec3(mLocalSpacePosition2); + Mat44 inv_initial_rotation = Mat44::sRotation(mInvInitialOrientation); + settings->mSliderAxis2 = inv_initial_rotation.Multiply3x3(mLocalSpaceSliderAxis1); + settings->mNormalAxis2 = inv_initial_rotation.Multiply3x3(mLocalSpaceNormal1); + settings->mLimitsMin = mLimitsMin; + settings->mLimitsMax = mLimitsMax; + settings->mLimitsSpringSettings = mLimitsSpringSettings; + settings->mMaxFrictionForce = mMaxFrictionForce; + settings->mMotorSettings = mMotorSettings; + return settings; +} + +Mat44 SliderConstraint::GetConstraintToBody1Matrix() const +{ + return Mat44(Vec4(mLocalSpaceSliderAxis1, 0), Vec4(mLocalSpaceNormal1, 0), Vec4(mLocalSpaceNormal2, 0), Vec4(mLocalSpacePosition1, 1)); +} + +Mat44 SliderConstraint::GetConstraintToBody2Matrix() const +{ + Mat44 mat = Mat44::sRotation(mInvInitialOrientation).Multiply3x3(Mat44(Vec4(mLocalSpaceSliderAxis1, 0), Vec4(mLocalSpaceNormal1, 0), Vec4(mLocalSpaceNormal2, 0), Vec4(0, 0, 0, 1))); + mat.SetTranslation(mLocalSpacePosition2); + return mat; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SliderConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SliderConstraint.h new file mode 100644 index 000000000000..1e126e57df5f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SliderConstraint.h @@ -0,0 +1,198 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Slider constraint settings, used to create a slider constraint +class JPH_EXPORT SliderConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, SliderConstraintSettings) + +public: + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint. + /// Note that the rotation constraint will be solved from body 1. This means that if body 1 and body 2 have different masses / inertias (kinematic body = infinite mass / inertia), body 1 should be the heaviest body. + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// Simple way of setting the slider and normal axis in world space (assumes the bodies are already oriented correctly when the constraint is created) + void SetSliderAxis(Vec3Arg inSliderAxis); + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// When mSpace is WorldSpace mPoint1 and mPoint2 can be automatically calculated based on the positions of the bodies when the constraint is created (the current relative position/orientation is chosen as the '0' position). Set this to false if you want to supply the attachment points yourself. + bool mAutoDetectPoint = false; + + /// Body 1 constraint reference frame (space determined by mSpace). + /// Slider axis is the axis along which movement is possible (direction), normal axis is a perpendicular vector to define the frame. + RVec3 mPoint1 = RVec3::sZero(); + Vec3 mSliderAxis1 = Vec3::sAxisX(); + Vec3 mNormalAxis1 = Vec3::sAxisY(); + + /// Body 2 constraint reference frame (space determined by mSpace) + RVec3 mPoint2 = RVec3::sZero(); + Vec3 mSliderAxis2 = Vec3::sAxisX(); + Vec3 mNormalAxis2 = Vec3::sAxisY(); + + /// When the bodies move so that mPoint1 coincides with mPoint2 the slider position is defined to be 0, movement will be limited between [mLimitsMin, mLimitsMax] where mLimitsMin e [-inf, 0] and mLimitsMax e [0, inf] + float mLimitsMin = -FLT_MAX; + float mLimitsMax = FLT_MAX; + + /// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back. + SpringSettings mLimitsSpringSettings; + + /// Maximum amount of friction force to apply (N) when not driven by a motor. + float mMaxFrictionForce = 0.0f; + + /// In case the constraint is powered, this determines the motor settings around the sliding axis + MotorSettings mMotorSettings; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A slider constraint allows movement in only 1 axis (and no rotation). Also known as a prismatic constraint. +class JPH_EXPORT SliderConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct slider constraint + SliderConstraint(Body &inBody1, Body &inBody2, const SliderConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Slider; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; + virtual void DrawConstraintLimits(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override; + virtual Mat44 GetConstraintToBody2Matrix() const override; + + /// Get the current distance from the rest position + float GetCurrentPosition() const; + + /// Friction control + void SetMaxFrictionForce(float inFrictionForce) { mMaxFrictionForce = inFrictionForce; } + float GetMaxFrictionForce() const { return mMaxFrictionForce; } + + /// Motor settings + MotorSettings & GetMotorSettings() { return mMotorSettings; } + const MotorSettings & GetMotorSettings() const { return mMotorSettings; } + + // Motor controls + void SetMotorState(EMotorState inState) { JPH_ASSERT(inState == EMotorState::Off || mMotorSettings.IsValid()); mMotorState = inState; } + EMotorState GetMotorState() const { return mMotorState; } + void SetTargetVelocity(float inVelocity) { mTargetVelocity = inVelocity; } + float GetTargetVelocity() const { return mTargetVelocity; } + void SetTargetPosition(float inPosition) { mTargetPosition = mHasLimits? Clamp(inPosition, mLimitsMin, mLimitsMax) : inPosition; } + float GetTargetPosition() const { return mTargetPosition; } + + /// Update the limits of the slider constraint (see SliderConstraintSettings) + void SetLimits(float inLimitsMin, float inLimitsMax); + float GetLimitsMin() const { return mLimitsMin; } + float GetLimitsMax() const { return mLimitsMax; } + bool HasLimits() const { return mHasLimits; } + + /// Update the limits spring settings + const SpringSettings & GetLimitsSpringSettings() const { return mLimitsSpringSettings; } + SpringSettings & GetLimitsSpringSettings() { return mLimitsSpringSettings; } + void SetLimitsSpringSettings(const SpringSettings &inLimitsSpringSettings) { mLimitsSpringSettings = inLimitsSpringSettings; } + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline Vector<2> GetTotalLambdaPosition() const { return mPositionConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaPositionLimits() const { return mPositionLimitsConstraintPart.GetTotalLambda(); } + inline Vec3 GetTotalLambdaRotation() const { return mRotationConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaMotor() const { return mMotorConstraintPart.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateR1R2U(Mat44Arg inRotation1, Mat44Arg inRotation2); + void CalculateSlidingAxisAndPosition(Mat44Arg inRotation1); + void CalculatePositionConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2); + void CalculatePositionLimitsConstraintProperties(float inDeltaTime); + void CalculateMotorConstraintProperties(float inDeltaTime); + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // Local space sliding direction + Vec3 mLocalSpaceSliderAxis1; + + // Local space normals to the sliding direction (in body 1 space) + Vec3 mLocalSpaceNormal1; + Vec3 mLocalSpaceNormal2; + + // Inverse of initial rotation from body 1 to body 2 in body 1 space + Quat mInvInitialOrientation; + + // Slider limits + bool mHasLimits; + float mLimitsMin; + float mLimitsMax; + + // Soft constraint limits + SpringSettings mLimitsSpringSettings; + + // Friction + float mMaxFrictionForce; + + // Motor controls + MotorSettings mMotorSettings; + EMotorState mMotorState = EMotorState::Off; + float mTargetVelocity = 0.0f; + float mTargetPosition = 0.0f; + + // RUN TIME PROPERTIES FOLLOW + + // Positions where the point constraint acts on (middle point between center of masses) + Vec3 mR1; + Vec3 mR2; + + // X2 + R2 - X1 - R1 + Vec3 mU; + + // World space sliding direction + Vec3 mWorldSpaceSliderAxis; + + // Normals to the slider axis + Vec3 mN1; + Vec3 mN2; + + // Distance along the slide axis + float mD = 0.0f; + + // The constraint parts + DualAxisConstraintPart mPositionConstraintPart; + RotationEulerConstraintPart mRotationConstraintPart; + AxisConstraintPart mPositionLimitsConstraintPart; + AxisConstraintPart mMotorConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SpringSettings.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SpringSettings.cpp new file mode 100644 index 000000000000..c2c32400fed1 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SpringSettings.cpp @@ -0,0 +1,35 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SpringSettings) +{ + JPH_ADD_ENUM_ATTRIBUTE(SpringSettings, mMode) + JPH_ADD_ATTRIBUTE(SpringSettings, mFrequency) + JPH_ADD_ATTRIBUTE(SpringSettings, mDamping) +} + +void SpringSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mMode); + inStream.Write(mFrequency); + inStream.Write(mDamping); +} + +void SpringSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mMode); + inStream.Read(mFrequency); + inStream.Read(mDamping); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SpringSettings.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SpringSettings.h new file mode 100644 index 000000000000..bfa49caac08d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SpringSettings.h @@ -0,0 +1,70 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; + +/// Enum used by constraints to specify how the spring is defined +enum class ESpringMode : uint8 +{ + FrequencyAndDamping, ///< Frequency and damping are specified + StiffnessAndDamping, ///< Stiffness and damping are specified +}; + +/// Settings for a linear or angular spring +class JPH_EXPORT SpringSettings +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SpringSettings) + +public: + /// Constructor + SpringSettings() = default; + SpringSettings(const SpringSettings &) = default; + SpringSettings & operator = (const SpringSettings &) = default; + SpringSettings(ESpringMode inMode, float inFrequencyOrStiffness, float inDamping) : mMode(inMode), mFrequency(inFrequencyOrStiffness), mDamping(inDamping) { } + + /// Saves the contents of the spring settings in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restores contents from the binary stream inStream. + void RestoreBinaryState(StreamIn &inStream); + + /// Check if the spring has a valid frequency / stiffness, if not the spring will be hard + inline bool HasStiffness() const { return mFrequency > 0.0f; } + + /// Selects the way in which the spring is defined + /// If the mode is StiffnessAndDamping then mFrequency becomes the stiffness (k) and mDamping becomes the damping ratio (c) in the spring equation F = -k * x - c * v. Otherwise the properties are as documented. + ESpringMode mMode = ESpringMode::FrequencyAndDamping; + + union + { + /// Valid when mSpringMode = ESpringMode::FrequencyAndDamping. + /// If mFrequency > 0 the constraint will be soft and mFrequency specifies the oscillation frequency in Hz. + /// If mFrequency <= 0, mDamping is ignored and the constraint will have hard limits (as hard as the time step / the number of velocity / position solver steps allows). + float mFrequency = 0.0f; + + /// Valid when mSpringMode = ESpringMode::StiffnessAndDamping. + /// If mStiffness > 0 the constraint will be soft and mStiffness specifies the stiffness (k) in the spring equation F = -k * x - c * v for a linear or T = -k * theta - c * w for an angular spring. + /// If mStiffness <= 0, mDamping is ignored and the constraint will have hard limits (as hard as the time step / the number of velocity / position solver steps allows). + /// + /// Note that stiffness values are large numbers. To calculate a ballpark value for the needed stiffness you can use: + /// force = stiffness * delta_spring_length = mass * gravity <=> stiffness = mass * gravity / delta_spring_length. + /// So if your object weighs 1500 kg and the spring compresses by 2 meters, you need a stiffness in the order of 1500 * 9.81 / 2 ~ 7500 N/m. + float mStiffness; + }; + + /// When mSpringMode = ESpringMode::FrequencyAndDamping mDamping is the damping ratio (0 = no damping, 1 = critical damping). + /// When mSpringMode = ESpringMode::StiffnessAndDamping mDamping is the damping (c) in the spring equation F = -k * x - c * v for a linear or T = -k * theta - c * w for an angular spring. + /// Note that if you set mDamping = 0, you will not get an infinite oscillation. Because we integrate physics using an explicit Euler scheme, there is always energy loss. + /// This is done to keep the simulation from exploding, because with a damping of 0 and even the slightest rounding error, the oscillation could become bigger and bigger until the simulation explodes. + float mDamping = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SwingTwistConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SwingTwistConstraint.cpp new file mode 100644 index 000000000000..bcd74ff305a2 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SwingTwistConstraint.cpp @@ -0,0 +1,524 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SwingTwistConstraintSettings) +{ + JPH_ADD_BASE_CLASS(SwingTwistConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(SwingTwistConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPosition1) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistAxis1) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPlaneAxis1) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPosition2) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistAxis2) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPlaneAxis2) + JPH_ADD_ENUM_ATTRIBUTE(SwingTwistConstraintSettings, mSwingType) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mNormalHalfConeAngle) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPlaneHalfConeAngle) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistMinAngle) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistMaxAngle) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mMaxFrictionTorque) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mSwingMotorSettings) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistMotorSettings) +} + +void SwingTwistConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mPosition1); + inStream.Write(mTwistAxis1); + inStream.Write(mPlaneAxis1); + inStream.Write(mPosition2); + inStream.Write(mTwistAxis2); + inStream.Write(mPlaneAxis2); + inStream.Write(mSwingType); + inStream.Write(mNormalHalfConeAngle); + inStream.Write(mPlaneHalfConeAngle); + inStream.Write(mTwistMinAngle); + inStream.Write(mTwistMaxAngle); + inStream.Write(mMaxFrictionTorque); + mSwingMotorSettings.SaveBinaryState(inStream); + mTwistMotorSettings.SaveBinaryState(inStream); +} + +void SwingTwistConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mPosition1); + inStream.Read(mTwistAxis1); + inStream.Read(mPlaneAxis1); + inStream.Read(mPosition2); + inStream.Read(mTwistAxis2); + inStream.Read(mPlaneAxis2); + inStream.Read(mSwingType); + inStream.Read(mNormalHalfConeAngle); + inStream.Read(mPlaneHalfConeAngle); + inStream.Read(mTwistMinAngle); + inStream.Read(mTwistMaxAngle); + inStream.Read(mMaxFrictionTorque); + mSwingMotorSettings.RestoreBinaryState(inStream); + mTwistMotorSettings.RestoreBinaryState(inStream); +} + +TwoBodyConstraint *SwingTwistConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new SwingTwistConstraint(inBody1, inBody2, *this); +} + +void SwingTwistConstraint::UpdateLimits() +{ + // Pass limits on to swing twist constraint part + mSwingTwistConstraintPart.SetLimits(mTwistMinAngle, mTwistMaxAngle, -mPlaneHalfConeAngle, mPlaneHalfConeAngle, -mNormalHalfConeAngle, mNormalHalfConeAngle); +} + +SwingTwistConstraint::SwingTwistConstraint(Body &inBody1, Body &inBody2, const SwingTwistConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mNormalHalfConeAngle(inSettings.mNormalHalfConeAngle), + mPlaneHalfConeAngle(inSettings.mPlaneHalfConeAngle), + mTwistMinAngle(inSettings.mTwistMinAngle), + mTwistMaxAngle(inSettings.mTwistMaxAngle), + mMaxFrictionTorque(inSettings.mMaxFrictionTorque), + mSwingMotorSettings(inSettings.mSwingMotorSettings), + mTwistMotorSettings(inSettings.mTwistMotorSettings) +{ + // Override swing type + mSwingTwistConstraintPart.SetSwingType(inSettings.mSwingType); + + // Calculate rotation needed to go from constraint space to body1 local space + Vec3 normal_axis1 = inSettings.mPlaneAxis1.Cross(inSettings.mTwistAxis1); + Mat44 c_to_b1(Vec4(inSettings.mTwistAxis1, 0), Vec4(normal_axis1, 0), Vec4(inSettings.mPlaneAxis1, 0), Vec4(0, 0, 0, 1)); + mConstraintToBody1 = c_to_b1.GetQuaternion(); + + // Calculate rotation needed to go from constraint space to body2 local space + Vec3 normal_axis2 = inSettings.mPlaneAxis2.Cross(inSettings.mTwistAxis2); + Mat44 c_to_b2(Vec4(inSettings.mTwistAxis2, 0), Vec4(normal_axis2, 0), Vec4(inSettings.mPlaneAxis2, 0), Vec4(0, 0, 0, 1)); + mConstraintToBody2 = c_to_b2.GetQuaternion(); + + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPosition1); + mConstraintToBody1 = inBody1.GetRotation().Conjugated() * mConstraintToBody1; + + mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPosition2); + mConstraintToBody2 = inBody2.GetRotation().Conjugated() * mConstraintToBody2; + } + else + { + mLocalSpacePosition1 = Vec3(inSettings.mPosition1); + mLocalSpacePosition2 = Vec3(inSettings.mPosition2); + } + + UpdateLimits(); +} + +void SwingTwistConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +Quat SwingTwistConstraint::GetRotationInConstraintSpace() const +{ + // Let b1, b2 be the center of mass transform of body1 and body2 (For body1 this is mBody1->GetCenterOfMassTransform()) + // Let c1, c2 be the transform that takes a vector from constraint space to local space of body1 and body2 (For body1 this is Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1)) + // Let q be the rotation of the constraint in constraint space + // b2 takes a vector from the local space of body2 to world space + // To express this in terms of b1: b2 = b1 * c1 * q * c2^-1 + // c2^-1 goes from local body 2 space to constraint space + // q rotates the constraint + // c1 goes from constraint space to body 1 local space + // b1 goes from body 1 local space to world space + // So when the body rotations are given, q = (b1 * c1)^-1 * b2 c2 + // Or: q = (q1 * c1)^-1 * (q2 * c2) if we're only interested in rotations + Quat constraint_body1_to_world = mBody1->GetRotation() * mConstraintToBody1; + Quat constraint_body2_to_world = mBody2->GetRotation() * mConstraintToBody2; + return constraint_body1_to_world.Conjugated() * constraint_body2_to_world; +} + +void SwingTwistConstraint::SetSwingMotorState(EMotorState inState) +{ + JPH_ASSERT(inState == EMotorState::Off || mSwingMotorSettings.IsValid()); + + if (mSwingMotorState != inState) + { + mSwingMotorState = inState; + + // Ensure that warm starting next frame doesn't apply any impulses (motor parts are repurposed for different modes) + for (AngleConstraintPart &c : mMotorConstraintPart) + c.Deactivate(); + } +} + +void SwingTwistConstraint::SetTwistMotorState(EMotorState inState) +{ + JPH_ASSERT(inState == EMotorState::Off || mTwistMotorSettings.IsValid()); + + if (mTwistMotorState != inState) + { + mTwistMotorState = inState; + + // Ensure that warm starting next frame doesn't apply any impulses (motor parts are repurposed for different modes) + mMotorConstraintPart[0].Deactivate(); + } +} + +void SwingTwistConstraint::SetTargetOrientationCS(QuatArg inOrientation) +{ + Quat q_swing, q_twist; + inOrientation.GetSwingTwist(q_swing, q_twist); + + uint clamped_axis; + mSwingTwistConstraintPart.ClampSwingTwist(q_swing, q_twist, clamped_axis); + + if (clamped_axis != 0) + mTargetOrientation = q_swing * q_twist; + else + mTargetOrientation = inOrientation; +} + +void SwingTwistConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Setup point constraint + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2); + + // GetRotationInConstraintSpace written out since we reuse the sub expressions + Quat constraint_body1_to_world = mBody1->GetRotation() * mConstraintToBody1; + Quat constraint_body2_to_world = mBody2->GetRotation() * mConstraintToBody2; + Quat q = constraint_body1_to_world.Conjugated() * constraint_body2_to_world; + + // Calculate constraint properties for the swing twist limit + mSwingTwistConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, q, constraint_body1_to_world); + + if (mSwingMotorState != EMotorState::Off || mTwistMotorState != EMotorState::Off || mMaxFrictionTorque > 0.0f) + { + // Calculate rotation motor axis + Mat44 ws_axis = Mat44::sRotation(constraint_body2_to_world); + for (int i = 0; i < 3; ++i) + mWorldSpaceMotorAxis[i] = ws_axis.GetColumn3(i); + + Vec3 rotation_error; + if (mSwingMotorState == EMotorState::Position || mTwistMotorState == EMotorState::Position) + { + // Get target orientation along the shortest path from q + Quat target_orientation = q.Dot(mTargetOrientation) > 0.0f? mTargetOrientation : -mTargetOrientation; + + // The definition of the constraint rotation q: + // R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q (1) + // + // R2' is the rotation of body 2 when reaching the target_orientation: + // R2' * ConstraintToBody2 = R1 * ConstraintToBody1 * target_orientation (2) + // + // The difference in body 2 space: + // R2' = R2 * diff_body2 (3) + // + // We want to specify the difference in the constraint space of body 2: + // diff_body2 = ConstraintToBody2 * diff * ConstraintToBody2^* (4) + // + // Extracting R2' from 2: R2' = R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* (5) + // Combining 3 & 4: R2' = R2 * ConstraintToBody2 * diff * ConstraintToBody2^* (6) + // Combining 1 & 6: R2' = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^* (7) + // Combining 5 & 7: R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^* + // <=> target_orientation = q * diff + // <=> diff = q^* * target_orientation + Quat diff = q.Conjugated() * target_orientation; + + // Approximate error angles + // The imaginary part of a quaternion is rotation_axis * sin(angle / 2) + // If angle is small, sin(x) = x so angle[i] ~ 2.0f * rotation_axis[i] + // We'll be making small time steps, so if the angle is not small at least the sign will be correct and we'll move in the right direction + rotation_error = -2.0f * diff.GetXYZ(); + } + + // Swing motor + switch (mSwingMotorState) + { + case EMotorState::Off: + if (mMaxFrictionTorque > 0.0f) + { + // Enable friction + for (int i = 1; i < 3; ++i) + mMotorConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[i], 0.0f); + } + else + { + // Disable friction + for (AngleConstraintPart &c : mMotorConstraintPart) + c.Deactivate(); + } + break; + + case EMotorState::Velocity: + // Use motor to create angular velocity around desired axis + for (int i = 1; i < 3; ++i) + mMotorConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[i], -mTargetAngularVelocity[i]); + break; + + case EMotorState::Position: + // Use motor to drive rotation error to zero + if (mSwingMotorSettings.mSpringSettings.HasStiffness()) + { + for (int i = 1; i < 3; ++i) + mMotorConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[i], 0.0f, rotation_error[i], mSwingMotorSettings.mSpringSettings); + } + else + { + for (int i = 1; i < 3; ++i) + mMotorConstraintPart[i].Deactivate(); + } + break; + } + + // Twist motor + switch (mTwistMotorState) + { + case EMotorState::Off: + if (mMaxFrictionTorque > 0.0f) + { + // Enable friction + mMotorConstraintPart[0].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[0], 0.0f); + } + else + { + // Disable friction + mMotorConstraintPart[0].Deactivate(); + } + break; + + case EMotorState::Velocity: + // Use motor to create angular velocity around desired axis + mMotorConstraintPart[0].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[0], -mTargetAngularVelocity[0]); + break; + + case EMotorState::Position: + // Use motor to drive rotation error to zero + if (mTwistMotorSettings.mSpringSettings.HasStiffness()) + mMotorConstraintPart[0].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[0], 0.0f, rotation_error[0], mTwistMotorSettings.mSpringSettings); + else + mMotorConstraintPart[0].Deactivate(); + break; + } + } + else + { + // Disable rotation motor + for (AngleConstraintPart &c : mMotorConstraintPart) + c.Deactivate(); + } +} + +void SwingTwistConstraint::ResetWarmStart() +{ + for (AngleConstraintPart &c : mMotorConstraintPart) + c.Deactivate(); + mSwingTwistConstraintPart.Deactivate(); + mPointConstraintPart.Deactivate(); +} + +void SwingTwistConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + for (AngleConstraintPart &c : mMotorConstraintPart) + c.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mSwingTwistConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); +} + +bool SwingTwistConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + bool impulse = false; + + // Solve twist rotation motor + if (mMotorConstraintPart[0].IsActive()) + { + // Twist limits + float min_twist_limit, max_twist_limit; + if (mTwistMotorState == EMotorState::Off) + { + max_twist_limit = inDeltaTime * mMaxFrictionTorque; + min_twist_limit = -max_twist_limit; + } + else + { + min_twist_limit = inDeltaTime * mTwistMotorSettings.mMinTorqueLimit; + max_twist_limit = inDeltaTime * mTwistMotorSettings.mMaxTorqueLimit; + } + + impulse |= mMotorConstraintPart[0].SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceMotorAxis[0], min_twist_limit, max_twist_limit); + } + + // Solve swing rotation motor + if (mMotorConstraintPart[1].IsActive()) + { + // Swing parts should turn on / off together + JPH_ASSERT(mMotorConstraintPart[2].IsActive()); + + // Swing limits + float min_swing_limit, max_swing_limit; + if (mSwingMotorState == EMotorState::Off) + { + max_swing_limit = inDeltaTime * mMaxFrictionTorque; + min_swing_limit = -max_swing_limit; + } + else + { + min_swing_limit = inDeltaTime * mSwingMotorSettings.mMinTorqueLimit; + max_swing_limit = inDeltaTime * mSwingMotorSettings.mMaxTorqueLimit; + } + + for (int i = 1; i < 3; ++i) + impulse |= mMotorConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceMotorAxis[i], min_swing_limit, max_swing_limit); + } + else + { + // Swing parts should turn on / off together + JPH_ASSERT(!mMotorConstraintPart[2].IsActive()); + } + + // Solve rotation limits + impulse |= mSwingTwistConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + // Solve position constraint + impulse |= mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + return impulse; +} + +bool SwingTwistConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + bool impulse = false; + + // Solve rotation violations + Quat q = GetRotationInConstraintSpace(); + impulse |= mSwingTwistConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, q, mConstraintToBody1, mConstraintToBody2, inBaumgarte); + + // Solve position violations + mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2); + impulse |= mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); + + return impulse; +} + +#ifdef JPH_DEBUG_RENDERER +void SwingTwistConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + // Get constraint properties in world space + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RVec3 position1 = transform1 * mLocalSpacePosition1; + Quat rotation1 = mBody1->GetRotation() * mConstraintToBody1; + Quat rotation2 = mBody2->GetRotation() * mConstraintToBody2; + + // Draw constraint orientation + inRenderer->DrawCoordinateSystem(RMat44::sRotationTranslation(rotation1, position1), mDrawConstraintSize); + + // Draw current swing and twist + Quat q = GetRotationInConstraintSpace(); + Quat q_swing, q_twist; + q.GetSwingTwist(q_swing, q_twist); + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_twist).RotateAxisY(), Color::sWhite); + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_swing).RotateAxisX(), Color::sWhite); + + if (mSwingMotorState == EMotorState::Velocity || mTwistMotorState == EMotorState::Velocity) + { + // Draw target angular velocity + inRenderer->DrawArrow(position1, position1 + rotation2 * mTargetAngularVelocity, Color::sRed, 0.1f); + } + if (mSwingMotorState == EMotorState::Position || mTwistMotorState == EMotorState::Position) + { + // Draw motor swing and twist + Quat swing, twist; + mTargetOrientation.GetSwingTwist(swing, twist); + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * twist).RotateAxisY(), Color::sYellow); + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * swing).RotateAxisX(), Color::sCyan); + } +} + +void SwingTwistConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const +{ + // Get matrix that transforms from constraint space to world space + RMat44 constraint_to_world = RMat44::sRotationTranslation(mBody1->GetRotation() * mConstraintToBody1, mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1); + + // Draw limits + if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Pyramid) + inRenderer->DrawSwingPyramidLimits(constraint_to_world, -mPlaneHalfConeAngle, mPlaneHalfConeAngle, -mNormalHalfConeAngle, mNormalHalfConeAngle, mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off); + else + inRenderer->DrawSwingConeLimits(constraint_to_world, mPlaneHalfConeAngle, mNormalHalfConeAngle, mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off); + inRenderer->DrawPie(constraint_to_world.GetTranslation(), mDrawConstraintSize, constraint_to_world.GetAxisX(), constraint_to_world.GetAxisY(), mTwistMinAngle, mTwistMaxAngle, Color::sPurple, DebugRenderer::ECastShadow::Off); +} +#endif // JPH_DEBUG_RENDERER + +void SwingTwistConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mPointConstraintPart.SaveState(inStream); + mSwingTwistConstraintPart.SaveState(inStream); + for (const AngleConstraintPart &c : mMotorConstraintPart) + c.SaveState(inStream); + + inStream.Write(mSwingMotorState); + inStream.Write(mTwistMotorState); + inStream.Write(mTargetAngularVelocity); + inStream.Write(mTargetOrientation); +} + +void SwingTwistConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mPointConstraintPart.RestoreState(inStream); + mSwingTwistConstraintPart.RestoreState(inStream); + for (AngleConstraintPart &c : mMotorConstraintPart) + c.RestoreState(inStream); + + inStream.Read(mSwingMotorState); + inStream.Read(mTwistMotorState); + inStream.Read(mTargetAngularVelocity); + inStream.Read(mTargetOrientation); +} + +Ref SwingTwistConstraint::GetConstraintSettings() const +{ + SwingTwistConstraintSettings *settings = new SwingTwistConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPosition1 = RVec3(mLocalSpacePosition1); + settings->mTwistAxis1 = mConstraintToBody1.RotateAxisX(); + settings->mPlaneAxis1 = mConstraintToBody1.RotateAxisZ(); + settings->mPosition2 = RVec3(mLocalSpacePosition2); + settings->mTwistAxis2 = mConstraintToBody2.RotateAxisX(); + settings->mPlaneAxis2 = mConstraintToBody2.RotateAxisZ(); + settings->mSwingType = mSwingTwistConstraintPart.GetSwingType(); + settings->mNormalHalfConeAngle = mNormalHalfConeAngle; + settings->mPlaneHalfConeAngle = mPlaneHalfConeAngle; + settings->mTwistMinAngle = mTwistMinAngle; + settings->mTwistMaxAngle = mTwistMaxAngle; + settings->mMaxFrictionTorque = mMaxFrictionTorque; + settings->mSwingMotorSettings = mSwingMotorSettings; + settings->mTwistMotorSettings = mTwistMotorSettings; + return settings; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SwingTwistConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SwingTwistConstraint.h new file mode 100644 index 000000000000..5e3e896f44cd --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SwingTwistConstraint.h @@ -0,0 +1,197 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Swing twist constraint settings, used to create a swing twist constraint +/// All values in this structure are copied to the swing twist constraint and the settings object is no longer needed afterwards. +/// +/// This image describes the limit settings: +/// @image html Docs/SwingTwistConstraint.png +class JPH_EXPORT SwingTwistConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, SwingTwistConstraintSettings) + +public: + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + ///@name Body 1 constraint reference frame (space determined by mSpace) + RVec3 mPosition1 = RVec3::sZero(); + Vec3 mTwistAxis1 = Vec3::sAxisX(); + Vec3 mPlaneAxis1 = Vec3::sAxisY(); + + ///@name Body 2 constraint reference frame (space determined by mSpace) + RVec3 mPosition2 = RVec3::sZero(); + Vec3 mTwistAxis2 = Vec3::sAxisX(); + Vec3 mPlaneAxis2 = Vec3::sAxisY(); + + /// The type of swing constraint that we want to use. + ESwingType mSwingType = ESwingType::Cone; + + ///@name Swing rotation limits + float mNormalHalfConeAngle = 0.0f; ///< See image at Detailed Description. Angle in radians. + float mPlaneHalfConeAngle = 0.0f; ///< See image at Detailed Description. Angle in radians. + + ///@name Twist rotation limits + float mTwistMinAngle = 0.0f; ///< See image at Detailed Description. Angle in radians. Should be \f$\in [-\pi, \pi]\f$. + float mTwistMaxAngle = 0.0f; ///< See image at Detailed Description. Angle in radians. Should be \f$\in [-\pi, \pi]\f$. + + ///@name Friction + float mMaxFrictionTorque = 0.0f; ///< Maximum amount of torque (N m) to apply as friction when the constraint is not powered by a motor + + ///@name In case the constraint is powered, this determines the motor settings around the swing and twist axis + MotorSettings mSwingMotorSettings; + MotorSettings mTwistMotorSettings; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A swing twist constraint is a specialized constraint for humanoid ragdolls that allows limited rotation only +/// +/// @see SwingTwistConstraintSettings for a description of the limits +class JPH_EXPORT SwingTwistConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct swing twist constraint + SwingTwistConstraint(Body &inBody1, Body &inBody2, const SwingTwistConstraintSettings &inSettings); + + ///@name Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::SwingTwist; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; + virtual void DrawConstraintLimits(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override { return Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1); } + virtual Mat44 GetConstraintToBody2Matrix() const override { return Mat44::sRotationTranslation(mConstraintToBody2, mLocalSpacePosition2); } + + ///@name Constraint reference frame + inline Vec3 GetLocalSpacePosition1() const { return mLocalSpacePosition1; } + inline Vec3 GetLocalSpacePosition2() const { return mLocalSpacePosition2; } + inline Quat GetConstraintToBody1() const { return mConstraintToBody1; } + inline Quat GetConstraintToBody2() const { return mConstraintToBody2; } + + ///@name Constraint limits + inline float GetNormalHalfConeAngle() const { return mNormalHalfConeAngle; } + inline void SetNormalHalfConeAngle(float inAngle) { mNormalHalfConeAngle = inAngle; UpdateLimits(); } + inline float GetPlaneHalfConeAngle() const { return mPlaneHalfConeAngle; } + inline void SetPlaneHalfConeAngle(float inAngle) { mPlaneHalfConeAngle = inAngle; UpdateLimits(); } + inline float GetTwistMinAngle() const { return mTwistMinAngle; } + inline void SetTwistMinAngle(float inAngle) { mTwistMinAngle = inAngle; UpdateLimits(); } + inline float GetTwistMaxAngle() const { return mTwistMaxAngle; } + inline void SetTwistMaxAngle(float inAngle) { mTwistMaxAngle = inAngle; UpdateLimits(); } + + ///@name Motor settings + const MotorSettings & GetSwingMotorSettings() const { return mSwingMotorSettings; } + MotorSettings & GetSwingMotorSettings() { return mSwingMotorSettings; } + const MotorSettings & GetTwistMotorSettings() const { return mTwistMotorSettings; } + MotorSettings & GetTwistMotorSettings() { return mTwistMotorSettings; } + + ///@name Friction control + void SetMaxFrictionTorque(float inFrictionTorque) { mMaxFrictionTorque = inFrictionTorque; } + float GetMaxFrictionTorque() const { return mMaxFrictionTorque; } + + ///@name Motor controls + + /// Controls if the motors are on or off + void SetSwingMotorState(EMotorState inState); + EMotorState GetSwingMotorState() const { return mSwingMotorState; } + void SetTwistMotorState(EMotorState inState); + EMotorState GetTwistMotorState() const { return mTwistMotorState; } + + /// Set the target angular velocity of body 2 in constraint space of body 2 + void SetTargetAngularVelocityCS(Vec3Arg inAngularVelocity) { mTargetAngularVelocity = inAngularVelocity; } + Vec3 GetTargetAngularVelocityCS() const { return mTargetAngularVelocity; } + + /// Set the target orientation in constraint space (drives constraint to: GetRotationInConstraintSpace() == inOrientation) + void SetTargetOrientationCS(QuatArg inOrientation); + Quat GetTargetOrientationCS() const { return mTargetOrientation; } + + /// Set the target orientation in body space (R2 = R1 * inOrientation, where R1 and R2 are the world space rotations for body 1 and 2). + /// Solve: R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q (see SwingTwistConstraint::GetSwingTwist) and R2 = R1 * inOrientation for q. + void SetTargetOrientationBS(QuatArg inOrientation) { SetTargetOrientationCS(mConstraintToBody1.Conjugated() * inOrientation * mConstraintToBody2); } + + /// Get current rotation of constraint in constraint space. + /// Solve: R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q for q. + Quat GetRotationInConstraintSpace() const; + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline Vec3 GetTotalLambdaPosition() const { return mPointConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaTwist() const { return mSwingTwistConstraintPart.GetTotalTwistLambda(); } + inline float GetTotalLambdaSwingY() const { return mSwingTwistConstraintPart.GetTotalSwingYLambda(); } + inline float GetTotalLambdaSwingZ() const { return mSwingTwistConstraintPart.GetTotalSwingZLambda(); } + inline Vec3 GetTotalLambdaMotor() const { return Vec3(mMotorConstraintPart[0].GetTotalLambda(), mMotorConstraintPart[1].GetTotalLambda(), mMotorConstraintPart[2].GetTotalLambda()); } + +private: + // Update the limits in the swing twist constraint part + void UpdateLimits(); + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // Transforms from constraint space to body space + Quat mConstraintToBody1; + Quat mConstraintToBody2; + + // Limits + float mNormalHalfConeAngle; + float mPlaneHalfConeAngle; + float mTwistMinAngle; + float mTwistMaxAngle; + + // Friction + float mMaxFrictionTorque; + + // Motor controls + MotorSettings mSwingMotorSettings; + MotorSettings mTwistMotorSettings; + EMotorState mSwingMotorState = EMotorState::Off; + EMotorState mTwistMotorState = EMotorState::Off; + Vec3 mTargetAngularVelocity = Vec3::sZero(); + Quat mTargetOrientation = Quat::sIdentity(); + + // RUN TIME PROPERTIES FOLLOW + + // Rotation axis for motor constraint parts + Vec3 mWorldSpaceMotorAxis[3]; + + // The constraint parts + PointConstraintPart mPointConstraintPart; + SwingTwistConstraintPart mSwingTwistConstraintPart; + AngleConstraintPart mMotorConstraintPart[3]; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/TwoBodyConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/TwoBodyConstraint.cpp new file mode 100644 index 000000000000..9ee7cc5d892d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/TwoBodyConstraint.cpp @@ -0,0 +1,56 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(TwoBodyConstraintSettings) +{ + JPH_ADD_BASE_CLASS(TwoBodyConstraintSettings, ConstraintSettings) +} + +void TwoBodyConstraint::BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager) +{ + // Activate bodies + BodyID body_ids[2]; + int num_bodies = 0; + if (mBody1->IsDynamic() && !mBody1->IsActive()) + body_ids[num_bodies++] = mBody1->GetID(); + if (mBody2->IsDynamic() && !mBody2->IsActive()) + body_ids[num_bodies++] = mBody2->GetID(); + if (num_bodies > 0) + inBodyManager.ActivateBodies(body_ids, num_bodies); + + // Link the bodies into the same island + ioBuilder.LinkConstraint(inConstraintIndex, mBody1->GetIndexInActiveBodiesInternal(), mBody2->GetIndexInActiveBodiesInternal()); +} + +uint TwoBodyConstraint::BuildIslandSplits(LargeIslandSplitter &ioSplitter) const +{ + return ioSplitter.AssignSplit(mBody1, mBody2); +} + +#ifdef JPH_DEBUG_RENDERER + +void TwoBodyConstraint::DrawConstraintReferenceFrame(DebugRenderer *inRenderer) const +{ + RMat44 transform1 = mBody1->GetCenterOfMassTransform() * GetConstraintToBody1Matrix(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform() * GetConstraintToBody2Matrix(); + inRenderer->DrawCoordinateSystem(transform1, 1.1f * mDrawConstraintSize); + inRenderer->DrawCoordinateSystem(transform2, mDrawConstraintSize); +} + +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/TwoBodyConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/TwoBodyConstraint.h new file mode 100644 index 000000000000..24630eb6de3c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/TwoBodyConstraint.h @@ -0,0 +1,65 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class TwoBodyConstraint; + +/// Base class for settings for all constraints that involve 2 bodies +class JPH_EXPORT TwoBodyConstraintSettings : public ConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, TwoBodyConstraintSettings) + +public: + /// Create an instance of this constraint + /// You can use Body::sFixedToWorld for inBody1 if you want to attach inBody2 to the world + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const = 0; +}; + +/// Base class for all constraints that involve 2 bodies. Body1 is usually considered the parent, Body2 the child. +class JPH_EXPORT TwoBodyConstraint : public Constraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + TwoBodyConstraint(Body &inBody1, Body &inBody2, const TwoBodyConstraintSettings &inSettings) : Constraint(inSettings), mBody1(&inBody1), mBody2(&inBody2) { } + + /// Get the type of a constraint + virtual EConstraintType GetType() const override { return EConstraintType::TwoBodyConstraint; } + + /// Solver interface + virtual bool IsActive() const override { return Constraint::IsActive() && (mBody1->IsActive() || mBody2->IsActive()) && (mBody2->IsDynamic() || mBody1->IsDynamic()); } +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraintReferenceFrame(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + + /// Access to the connected bodies + Body * GetBody1() const { return mBody1; } + Body * GetBody2() const { return mBody2; } + + /// Calculates the transform that transforms from constraint space to body 1 space. The first column of the matrix is the primary constraint axis (e.g. the hinge axis / slider direction), second column the secondary etc. + virtual Mat44 GetConstraintToBody1Matrix() const = 0; + + /// Calculates the transform that transforms from constraint space to body 2 space. The first column of the matrix is the primary constraint axis (e.g. the hinge axis / slider direction), second column the secondary etc. + virtual Mat44 GetConstraintToBody2Matrix() const = 0; + + /// Link bodies that are connected by this constraint in the island builder + virtual void BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager) override; + + /// Link bodies that are connected by this constraint in the same split. Returns the split index. + virtual uint BuildIslandSplits(LargeIslandSplitter &ioSplitter) const override; + +protected: + /// The two bodies involved + Body * mBody1; + Body * mBody2; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/DeterminismLog.cpp b/thirdparty/jolt_physics/Jolt/Physics/DeterminismLog.cpp new file mode 100644 index 000000000000..7985a36bf971 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/DeterminismLog.cpp @@ -0,0 +1,17 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +#ifdef JPH_ENABLE_DETERMINISM_LOG + +JPH_NAMESPACE_BEGIN + +DeterminismLog DeterminismLog::sLog; + +JPH_NAMESPACE_END + +#endif // JPH_ENABLE_DETERMINISM_LOG diff --git a/thirdparty/jolt_physics/Jolt/Physics/DeterminismLog.h b/thirdparty/jolt_physics/Jolt/Physics/DeterminismLog.h new file mode 100644 index 000000000000..e2930ff3ceaa --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/DeterminismLog.h @@ -0,0 +1,159 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +//#define JPH_ENABLE_DETERMINISM_LOG +#ifdef JPH_ENABLE_DETERMINISM_LOG + +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +/// A simple class that logs the state of the simulation. The resulting text file can be used to diff between platforms and find issues in determinism. +class DeterminismLog +{ +private: + JPH_INLINE uint32 Convert(float inValue) const + { + return *(uint32 *)&inValue; + } + + JPH_INLINE uint64 Convert(double inValue) const + { + return *(uint64 *)&inValue; + } + +public: + DeterminismLog() + { + mLog.open("detlog.txt", std::ios::out | std::ios::trunc | std::ios::binary); // Binary because we don't want a difference between Unix and Windows line endings. + mLog.fill('0'); + } + + DeterminismLog & operator << (char inValue) + { + mLog << inValue; + return *this; + } + + DeterminismLog & operator << (const char *inValue) + { + mLog << std::dec << inValue; + return *this; + } + + DeterminismLog & operator << (const string &inValue) + { + mLog << std::dec << inValue; + return *this; + } + + DeterminismLog & operator << (const BodyID &inValue) + { + mLog << std::hex << std::setw(8) << inValue.GetIndexAndSequenceNumber(); + return *this; + } + + DeterminismLog & operator << (const SubShapeID &inValue) + { + mLog << std::hex << std::setw(8) << inValue.GetValue(); + return *this; + } + + DeterminismLog & operator << (float inValue) + { + mLog << std::hex << std::setw(8) << Convert(inValue); + return *this; + } + + DeterminismLog & operator << (int inValue) + { + mLog << inValue; + return *this; + } + + DeterminismLog & operator << (uint32 inValue) + { + mLog << std::hex << std::setw(8) << inValue; + return *this; + } + + DeterminismLog & operator << (uint64 inValue) + { + mLog << std::hex << std::setw(16) << inValue; + return *this; + } + + DeterminismLog & operator << (Vec3Arg inValue) + { + mLog << std::hex << std::setw(8) << Convert(inValue.GetX()) << " " << std::setw(8) << Convert(inValue.GetY()) << " " << std::setw(8) << Convert(inValue.GetZ()); + return *this; + } + + DeterminismLog & operator << (DVec3Arg inValue) + { + mLog << std::hex << std::setw(16) << Convert(inValue.GetX()) << " " << std::setw(16) << Convert(inValue.GetY()) << " " << std::setw(16) << Convert(inValue.GetZ()); + return *this; + } + + DeterminismLog & operator << (Vec4Arg inValue) + { + mLog << std::hex << std::setw(8) << Convert(inValue.GetX()) << " " << std::setw(8) << Convert(inValue.GetY()) << " " << std::setw(8) << Convert(inValue.GetZ()) << " " << std::setw(8) << Convert(inValue.GetW()); + return *this; + } + + DeterminismLog & operator << (const Float3 &inValue) + { + mLog << std::hex << std::setw(8) << Convert(inValue.x) << " " << std::setw(8) << Convert(inValue.y) << " " << std::setw(8) << Convert(inValue.z); + return *this; + } + + DeterminismLog & operator << (Mat44Arg inValue) + { + *this << inValue.GetColumn4(0) << " " << inValue.GetColumn4(1) << " " << inValue.GetColumn4(2) << " " << inValue.GetColumn4(3); + return *this; + } + + DeterminismLog & operator << (DMat44Arg inValue) + { + *this << inValue.GetColumn4(0) << " " << inValue.GetColumn4(1) << " " << inValue.GetColumn4(2) << " " << inValue.GetTranslation(); + return *this; + } + + DeterminismLog & operator << (QuatArg inValue) + { + *this << inValue.GetXYZW(); + return *this; + } + + // Singleton instance + static DeterminismLog sLog; + +private: + std::ofstream mLog; +}; + +/// Will log something to the determinism log, usage: JPH_DET_LOG("label " << value); +#define JPH_DET_LOG(...) DeterminismLog::sLog << __VA_ARGS__ << '\n' + +JPH_NAMESPACE_END + +#else + +JPH_SUPPRESS_WARNING_PUSH +JPH_SUPPRESS_WARNINGS + +/// By default we log nothing +#define JPH_DET_LOG(...) + +JPH_SUPPRESS_WARNING_POP + +#endif // JPH_ENABLE_DETERMINISM_LOG diff --git a/thirdparty/jolt_physics/Jolt/Physics/EActivation.h b/thirdparty/jolt_physics/Jolt/Physics/EActivation.h new file mode 100644 index 000000000000..08c10c20dd42 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/EActivation.h @@ -0,0 +1,16 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Enum used by AddBody to determine if the body needs to be initially active +enum class EActivation +{ + Activate, ///< Activate the body, making it part of the simulation + DontActivate ///< Leave activation state as it is (will not deactivate an active body) +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/EPhysicsUpdateError.h b/thirdparty/jolt_physics/Jolt/Physics/EPhysicsUpdateError.h new file mode 100644 index 000000000000..c9edd6deda08 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/EPhysicsUpdateError.h @@ -0,0 +1,37 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Enum used by PhysicsSystem to report error conditions during the PhysicsSystem::Update call. This is a bit field, multiple errors can trigger in the same update. +enum class EPhysicsUpdateError : uint32 +{ + None = 0, ///< No errors + ManifoldCacheFull = 1 << 0, ///< The manifold cache is full, this means that the total number of contacts between bodies is too high. Some contacts were ignored. Increase inMaxContactConstraints in PhysicsSystem::Init. + BodyPairCacheFull = 1 << 1, ///< The body pair cache is full, this means that too many bodies contacted. Some contacts were ignored. Increase inMaxBodyPairs in PhysicsSystem::Init. + ContactConstraintsFull = 1 << 2, ///< The contact constraints buffer is full. Some contacts were ignored. Increase inMaxContactConstraints in PhysicsSystem::Init. +}; + +/// OR operator for EPhysicsUpdateError +inline EPhysicsUpdateError operator | (EPhysicsUpdateError inA, EPhysicsUpdateError inB) +{ + return static_cast(static_cast(inA) | static_cast(inB)); +} + +/// OR operator for EPhysicsUpdateError +inline EPhysicsUpdateError operator |= (EPhysicsUpdateError &ioA, EPhysicsUpdateError inB) +{ + ioA = ioA | inB; + return ioA; +} + +/// AND operator for EPhysicsUpdateError +inline EPhysicsUpdateError operator & (EPhysicsUpdateError inA, EPhysicsUpdateError inB) +{ + return static_cast(static_cast(inA) & static_cast(inB)); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.cpp b/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.cpp new file mode 100644 index 000000000000..ed1064df9965 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.cpp @@ -0,0 +1,484 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +IslandBuilder::~IslandBuilder() +{ + JPH_ASSERT(mConstraintLinks == nullptr); + JPH_ASSERT(mContactLinks == nullptr); + JPH_ASSERT(mBodyIslands == nullptr); + JPH_ASSERT(mBodyIslandEnds == nullptr); + JPH_ASSERT(mConstraintIslands == nullptr); + JPH_ASSERT(mConstraintIslandEnds == nullptr); + JPH_ASSERT(mContactIslands == nullptr); + JPH_ASSERT(mContactIslandEnds == nullptr); + JPH_ASSERT(mIslandsSorted == nullptr); + + delete [] mBodyLinks; +} + +void IslandBuilder::Init(uint32 inMaxActiveBodies) +{ + mMaxActiveBodies = inMaxActiveBodies; + + // Link each body to itself, BuildBodyIslands() will restore this so that we don't need to do this each step + JPH_ASSERT(mBodyLinks == nullptr); + mBodyLinks = new BodyLink [mMaxActiveBodies]; + for (uint32 i = 0; i < mMaxActiveBodies; ++i) + mBodyLinks[i].mLinkedTo.store(i, memory_order_relaxed); +} + +void IslandBuilder::PrepareContactConstraints(uint32 inMaxContacts, TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + // Need to call Init first + JPH_ASSERT(mBodyLinks != nullptr); + + // Check that the builder has been reset + JPH_ASSERT(mNumContacts == 0); + JPH_ASSERT(mNumIslands == 0); + + // Create contact link buffer, not initialized so each contact needs to be explicitly set + JPH_ASSERT(mContactLinks == nullptr); + mContactLinks = (uint32 *)inTempAllocator->Allocate(inMaxContacts * sizeof(uint32)); + mMaxContacts = inMaxContacts; + +#ifdef JPH_VALIDATE_ISLAND_BUILDER + // Create validation structures + JPH_ASSERT(mLinkValidation == nullptr); + mLinkValidation = (LinkValidation *)inTempAllocator->Allocate(inMaxContacts * sizeof(LinkValidation)); + mNumLinkValidation = 0; +#endif +} + +void IslandBuilder::PrepareNonContactConstraints(uint32 inNumConstraints, TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + // Need to call Init first + JPH_ASSERT(mBodyLinks != nullptr); + + // Check that the builder has been reset + JPH_ASSERT(mNumIslands == 0); + + // Store number of constraints + mNumConstraints = inNumConstraints; + + // Create constraint link buffer, not initialized so each constraint needs to be explicitly set + JPH_ASSERT(mConstraintLinks == nullptr); + mConstraintLinks = (uint32 *)inTempAllocator->Allocate(inNumConstraints * sizeof(uint32)); +} + +uint32 IslandBuilder::GetLowestBodyIndex(uint32 inActiveBodyIndex) const +{ + uint32 index = inActiveBodyIndex; + for (;;) + { + uint32 link_to = mBodyLinks[index].mLinkedTo.load(memory_order_relaxed); + if (link_to == index) + break; + index = link_to; + } + return index; +} + +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; + +#ifdef JPH_VALIDATE_ISLAND_BUILDER + // Add link to the validation list + if (mNumLinkValidation < uint32(mMaxContacts)) + mLinkValidation[mNumLinkValidation++] = { inFirst, inSecond }; + else + JPH_ASSERT(false, "Out of links"); +#endif + + // Start the algorithm with the two bodies + uint32 first_link_to = inFirst; + uint32 second_link_to = inSecond; + + for (;;) + { + // Follow the chain until we get to the body with lowest index + // If the swap compare below fails, we'll keep searching from the lowest index for the new lowest index + first_link_to = GetLowestBodyIndex(first_link_to); + second_link_to = GetLowestBodyIndex(second_link_to); + + // If the targets are the same, the bodies are already connected + if (first_link_to != second_link_to) + { + // We always link the highest to the lowest + if (first_link_to < second_link_to) + { + // Attempt to link the second to the first + // Since we found this body to be at the end of the chain it must point to itself, and if it + // doesn't it has been reparented and we need to retry the algorithm + if (!mBodyLinks[second_link_to].mLinkedTo.compare_exchange_weak(second_link_to, first_link_to, memory_order_relaxed)) + continue; + } + else + { + // Attempt to link the first to the second + // Since we found this body to be at the end of the chain it must point to itself, and if it + // doesn't it has been reparented and we need to retry the algorithm + if (!mBodyLinks[first_link_to].mLinkedTo.compare_exchange_weak(first_link_to, second_link_to, memory_order_relaxed)) + continue; + } + } + + // Linking succeeded! + // Chains of bodies can become really long, resulting in an O(N) loop to find the lowest body index + // to prevent this we attempt to update the link of the bodies that were passed in to directly point + // to the lowest index that we found. If the value became lower than our lowest link, some other + // thread must have relinked these bodies in the mean time so we won't update the value. + uint32 lowest_link_to = min(first_link_to, second_link_to); + AtomicMin(mBodyLinks[inFirst].mLinkedTo, lowest_link_to, memory_order_relaxed); + AtomicMin(mBodyLinks[inSecond].mLinkedTo, lowest_link_to, memory_order_relaxed); + break; + } +} + +void IslandBuilder::LinkConstraint(uint32 inConstraintIndex, uint32 inFirst, uint32 inSecond) +{ + LinkBodies(inFirst, inSecond); + + JPH_ASSERT(inConstraintIndex < mNumConstraints); + uint32 min_value = min(inFirst, inSecond); // Use fact that invalid index is 0xffffffff, we want the active body of two + JPH_ASSERT(min_value != Body::cInactiveIndex); // At least one of the bodies must be active + mConstraintLinks[inConstraintIndex] = min_value; +} + +void IslandBuilder::LinkContact(uint32 inContactIndex, uint32 inFirst, uint32 inSecond) +{ + JPH_ASSERT(inContactIndex < mMaxContacts); + mContactLinks[inContactIndex] = min(inFirst, inSecond); // Use fact that invalid index is 0xffffffff, we want the active body of two +} + +#ifdef JPH_VALIDATE_ISLAND_BUILDER + +void IslandBuilder::ValidateIslands(uint32 inNumActiveBodies) const +{ + JPH_PROFILE_FUNCTION(); + + // Go through all links so far + for (uint32 i = 0; i < mNumLinkValidation; ++i) + { + // If the bodies in this link ended up in different groups we have a problem + if (mBodyLinks[mLinkValidation[i].mFirst].mIslandIndex != mBodyLinks[mLinkValidation[i].mSecond].mIslandIndex) + { + Trace("Fail: %u, %u", mLinkValidation[i].mFirst, mLinkValidation[i].mSecond); + Trace("Num Active: %u", inNumActiveBodies); + + for (uint32 j = 0; j < mNumLinkValidation; ++j) + Trace("builder.Link(%u, %u);", mLinkValidation[j].mFirst, mLinkValidation[j].mSecond); + + IslandBuilder tmp; + tmp.Init(inNumActiveBodies); + for (uint32 j = 0; j < mNumLinkValidation; ++j) + { + Trace("Link %u -> %u", mLinkValidation[j].mFirst, mLinkValidation[j].mSecond); + tmp.LinkBodies(mLinkValidation[j].mFirst, mLinkValidation[j].mSecond); + for (uint32 t = 0; t < inNumActiveBodies; ++t) + Trace("%u -> %u", t, (uint32)tmp.mBodyLinks[t].mLinkedTo); + } + + JPH_ASSERT(false, "IslandBuilder validation failed"); + } + } +} + +#endif + +void IslandBuilder::BuildBodyIslands(const BodyID *inActiveBodies, uint32 inNumActiveBodies, TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + // Store the amount of active bodies + mNumActiveBodies = inNumActiveBodies; + + // Create output arrays for body ID's, don't call constructors + JPH_ASSERT(mBodyIslands == nullptr); + mBodyIslands = (BodyID *)inTempAllocator->Allocate(inNumActiveBodies * sizeof(BodyID)); + + // Create output array for start index of each island. At this point we don't know how many islands there will be, but we know it cannot be more than inNumActiveBodies. + // Note: We allocate 1 extra entry because we always increment the count of the next island. + uint32 *body_island_starts = (uint32 *)inTempAllocator->Allocate((inNumActiveBodies + 1) * sizeof(uint32)); + + // First island always starts at 0 + body_island_starts[0] = 0; + + // Calculate island index for all bodies + JPH_ASSERT(mNumIslands == 0); + for (uint32 i = 0; i < inNumActiveBodies; ++i) + { + BodyLink &link = mBodyLinks[i]; + uint32 s = link.mLinkedTo.load(memory_order_relaxed); + if (s != i) + { + // Links to another body, take island index from other body (this must have been filled in already since we're looping from low to high) + JPH_ASSERT(s < uint32(i)); + uint32 island_index = mBodyLinks[s].mIslandIndex; + link.mIslandIndex = island_index; + + // Increment the start of the next island + body_island_starts[island_index + 1]++; + } + else + { + // Does not link to other body, this is the start of a new island + link.mIslandIndex = mNumIslands; + ++mNumIslands; + + // Set the start of the next island to 1 + body_island_starts[mNumIslands] = 1; + } + } + +#ifdef JPH_VALIDATE_ISLAND_BUILDER + ValidateIslands(inNumActiveBodies); +#endif + + // Make the start array absolute (so far we only counted) + for (uint32 island = 1; island < mNumIslands; ++island) + body_island_starts[island] += body_island_starts[island - 1]; + + // Convert the to a linear list grouped by island + for (uint32 i = 0; i < inNumActiveBodies; ++i) + { + BodyLink &link = mBodyLinks[i]; + + // Copy the body to the correct location in the array and increment it + uint32 &start = body_island_starts[link.mIslandIndex]; + mBodyIslands[start] = inActiveBodies[i]; + start++; + + // Reset linked to field for the next update + link.mLinkedTo.store(i, memory_order_relaxed); + } + + // We should now have a full array + JPH_ASSERT(mNumIslands == 0 || body_island_starts[mNumIslands - 1] == inNumActiveBodies); + + // We've incremented all body indices so that they now point at the end instead of the starts + JPH_ASSERT(mBodyIslandEnds == nullptr); + mBodyIslandEnds = body_island_starts; +} + +void IslandBuilder::BuildConstraintIslands(const uint32 *inConstraintToBody, uint32 inNumConstraints, uint32 *&outConstraints, uint32 *&outConstraintsEnd, TempAllocator *inTempAllocator) const +{ + JPH_PROFILE_FUNCTION(); + + // Check if there's anything to do + if (inNumConstraints == 0) + return; + + // Create output arrays for constraints + // Note: For the end indices we allocate 1 extra entry so we don't have to do an if in the inner loop + uint32 *constraints = (uint32 *)inTempAllocator->Allocate(inNumConstraints * sizeof(uint32)); + uint32 *constraint_ends = (uint32 *)inTempAllocator->Allocate((mNumIslands + 1) * sizeof(uint32)); + + // Reset sizes + for (uint32 island = 0; island < mNumIslands; ++island) + constraint_ends[island] = 0; + + // Loop over array and increment start relative position for the next island + for (uint32 constraint = 0; constraint < inNumConstraints; ++constraint) + { + uint32 body_idx = inConstraintToBody[constraint]; + uint32 next_island_idx = mBodyLinks[body_idx].mIslandIndex + 1; + JPH_ASSERT(next_island_idx <= mNumIslands); + constraint_ends[next_island_idx]++; + } + + // Make start positions absolute + for (uint32 island = 1; island < mNumIslands; ++island) + constraint_ends[island] += constraint_ends[island - 1]; + + // Loop over array and collect constraints + for (uint32 constraint = 0; constraint < inNumConstraints; ++constraint) + { + uint32 body_idx = inConstraintToBody[constraint]; + uint32 island_idx = mBodyLinks[body_idx].mIslandIndex; + constraints[constraint_ends[island_idx]++] = constraint; + } + + JPH_ASSERT(outConstraints == nullptr); + outConstraints = constraints; + JPH_ASSERT(outConstraintsEnd == nullptr); + outConstraintsEnd = constraint_ends; +} + +void IslandBuilder::SortIslands(TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + if (mNumContacts > 0 || mNumConstraints > 0) + { + // Allocate mapping table + JPH_ASSERT(mIslandsSorted == nullptr); + mIslandsSorted = (uint32 *)inTempAllocator->Allocate(mNumIslands * sizeof(uint32)); + + // Initialize index + for (uint32 island = 0; island < mNumIslands; ++island) + mIslandsSorted[island] = island; + + // Determine the sum of contact constraints / constraints per island + uint32 *num_constraints = (uint32 *)inTempAllocator->Allocate(mNumIslands * sizeof(uint32)); + if (mNumContacts > 0 && mNumConstraints > 0) + { + num_constraints[0] = mConstraintIslandEnds[0] + mContactIslandEnds[0]; + for (uint32 island = 1; island < mNumIslands; ++island) + num_constraints[island] = mConstraintIslandEnds[island] - mConstraintIslandEnds[island - 1] + + mContactIslandEnds[island] - mContactIslandEnds[island - 1]; + } + else if (mNumContacts > 0) + { + num_constraints[0] = mContactIslandEnds[0]; + for (uint32 island = 1; island < mNumIslands; ++island) + num_constraints[island] = mContactIslandEnds[island] - mContactIslandEnds[island - 1]; + } + else + { + num_constraints[0] = mConstraintIslandEnds[0]; + for (uint32 island = 1; island < mNumIslands; ++island) + num_constraints[island] = mConstraintIslandEnds[island] - mConstraintIslandEnds[island - 1]; + } + + // Sort so the biggest islands go first, this means that the jobs that take longest will be running + // first which improves the chance that all jobs finish at the same time. + QuickSort(mIslandsSorted, mIslandsSorted + mNumIslands, [num_constraints](uint32 inLHS, uint32 inRHS) { + return num_constraints[inLHS] > num_constraints[inRHS]; + }); + + inTempAllocator->Free(num_constraints, mNumIslands * sizeof(uint32)); + } +} + +void IslandBuilder::Finalize(const BodyID *inActiveBodies, uint32 inNumActiveBodies, uint32 inNumContacts, TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + mNumContacts = inNumContacts; + + BuildBodyIslands(inActiveBodies, inNumActiveBodies, inTempAllocator); + BuildConstraintIslands(mConstraintLinks, mNumConstraints, mConstraintIslands, mConstraintIslandEnds, inTempAllocator); + BuildConstraintIslands(mContactLinks, mNumContacts, mContactIslands, mContactIslandEnds, inTempAllocator); + SortIslands(inTempAllocator); + + mNumPositionSteps = (uint8 *)inTempAllocator->Allocate(mNumIslands * sizeof(uint8)); +} + +void IslandBuilder::GetBodiesInIsland(uint32 inIslandIndex, BodyID *&outBodiesBegin, BodyID *&outBodiesEnd) const +{ + JPH_ASSERT(inIslandIndex < mNumIslands); + uint32 sorted_index = mIslandsSorted != nullptr? mIslandsSorted[inIslandIndex] : inIslandIndex; + outBodiesBegin = sorted_index > 0? mBodyIslands + mBodyIslandEnds[sorted_index - 1] : mBodyIslands; + outBodiesEnd = mBodyIslands + mBodyIslandEnds[sorted_index]; +} + +bool IslandBuilder::GetConstraintsInIsland(uint32 inIslandIndex, uint32 *&outConstraintsBegin, uint32 *&outConstraintsEnd) const +{ + JPH_ASSERT(inIslandIndex < mNumIslands); + if (mNumConstraints == 0) + { + outConstraintsBegin = nullptr; + outConstraintsEnd = nullptr; + return false; + } + else + { + uint32 sorted_index = mIslandsSorted[inIslandIndex]; + outConstraintsBegin = sorted_index > 0? mConstraintIslands + mConstraintIslandEnds[sorted_index - 1] : mConstraintIslands; + outConstraintsEnd = mConstraintIslands + mConstraintIslandEnds[sorted_index]; + return outConstraintsBegin != outConstraintsEnd; + } +} + +bool IslandBuilder::GetContactsInIsland(uint32 inIslandIndex, uint32 *&outContactsBegin, uint32 *&outContactsEnd) const +{ + JPH_ASSERT(inIslandIndex < mNumIslands); + if (mNumContacts == 0) + { + outContactsBegin = nullptr; + outContactsEnd = nullptr; + return false; + } + else + { + uint32 sorted_index = mIslandsSorted[inIslandIndex]; + outContactsBegin = sorted_index > 0? mContactIslands + mContactIslandEnds[sorted_index - 1] : mContactIslands; + outContactsEnd = mContactIslands + mContactIslandEnds[sorted_index]; + return outContactsBegin != outContactsEnd; + } +} + +void IslandBuilder::ResetIslands(TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + inTempAllocator->Free(mNumPositionSteps, mNumIslands * sizeof(uint8)); + + if (mIslandsSorted != nullptr) + { + inTempAllocator->Free(mIslandsSorted, mNumIslands * sizeof(uint32)); + mIslandsSorted = nullptr; + } + + if (mContactIslands != nullptr) + { + inTempAllocator->Free(mContactIslandEnds, (mNumIslands + 1) * sizeof(uint32)); + mContactIslandEnds = nullptr; + inTempAllocator->Free(mContactIslands, mNumContacts * sizeof(uint32)); + mContactIslands = nullptr; + } + + if (mConstraintIslands != nullptr) + { + inTempAllocator->Free(mConstraintIslandEnds, (mNumIslands + 1) * sizeof(uint32)); + mConstraintIslandEnds = nullptr; + inTempAllocator->Free(mConstraintIslands, mNumConstraints * sizeof(uint32)); + mConstraintIslands = nullptr; + } + + inTempAllocator->Free(mBodyIslandEnds, (mNumActiveBodies + 1) * sizeof(uint32)); + mBodyIslandEnds = nullptr; + inTempAllocator->Free(mBodyIslands, mNumActiveBodies * sizeof(uint32)); + mBodyIslands = nullptr; + + inTempAllocator->Free(mConstraintLinks, mNumConstraints * sizeof(uint32)); + mConstraintLinks = nullptr; + +#ifdef JPH_VALIDATE_ISLAND_BUILDER + inTempAllocator->Free(mLinkValidation, mMaxContacts * sizeof(LinkValidation)); + mLinkValidation = nullptr; +#endif + + inTempAllocator->Free(mContactLinks, mMaxContacts * sizeof(uint32)); + mContactLinks = nullptr; + + mNumActiveBodies = 0; + mNumConstraints = 0; + mMaxContacts = 0; + mNumContacts = 0; + mNumIslands = 0; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.h b/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.h new file mode 100644 index 000000000000..4c2f097d60c3 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.h @@ -0,0 +1,125 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class TempAllocator; + +//#define JPH_VALIDATE_ISLAND_BUILDER + +/// Keeps track of connected bodies and builds islands for multithreaded velocity/position update +class IslandBuilder : public NonCopyable +{ +public: + /// Destructor + ~IslandBuilder(); + + /// Initialize the island builder with the maximum amount of bodies that could be active + void Init(uint32 inMaxActiveBodies); + + /// Prepare for simulation step by allocating space for the contact constraints + void PrepareContactConstraints(uint32 inMaxContactConstraints, TempAllocator *inTempAllocator); + + /// Prepare for simulation step by allocating space for the non-contact constraints + void PrepareNonContactConstraints(uint32 inNumConstraints, TempAllocator *inTempAllocator); + + /// Link two bodies by their index in the BodyManager::mActiveBodies list to form islands + void LinkBodies(uint32 inFirst, uint32 inSecond); + + /// Link a constraint to a body by their index in the BodyManager::mActiveBodies + void LinkConstraint(uint32 inConstraintIndex, uint32 inFirst, uint32 inSecond); + + /// Link a contact to a body by their index in the BodyManager::mActiveBodies + void LinkContact(uint32 inContactIndex, uint32 inFirst, uint32 inSecond); + + /// Finalize the islands after all bodies have been Link()-ed + void Finalize(const BodyID *inActiveBodies, uint32 inNumActiveBodies, uint32 inNumContacts, TempAllocator *inTempAllocator); + + /// Get the amount of islands formed + uint32 GetNumIslands() const { return mNumIslands; } + + /// Get iterator for a particular island, return false if there are no constraints + void GetBodiesInIsland(uint32 inIslandIndex, BodyID *&outBodiesBegin, BodyID *&outBodiesEnd) const; + bool GetConstraintsInIsland(uint32 inIslandIndex, uint32 *&outConstraintsBegin, uint32 *&outConstraintsEnd) const; + bool GetContactsInIsland(uint32 inIslandIndex, uint32 *&outContactsBegin, uint32 *&outContactsEnd) const; + + /// The number of position iterations for each island + void SetNumPositionSteps(uint32 inIslandIndex, uint inNumPositionSteps) { JPH_ASSERT(inIslandIndex < mNumIslands); JPH_ASSERT(inNumPositionSteps < 256); mNumPositionSteps[inIslandIndex] = uint8(inNumPositionSteps); } + uint GetNumPositionSteps(uint32 inIslandIndex) const { JPH_ASSERT(inIslandIndex < mNumIslands); return mNumPositionSteps[inIslandIndex]; } + + /// After you're done calling the three functions above, call this function to free associated data + void ResetIslands(TempAllocator *inTempAllocator); + +private: + /// Returns the index of the lowest body in the group + uint32 GetLowestBodyIndex(uint32 inActiveBodyIndex) const; + +#ifdef JPH_VALIDATE_ISLAND_BUILDER + /// Helper function to validate all islands so far generated + void ValidateIslands(uint32 inNumActiveBodies) const; +#endif + + // Helper functions to build various islands + void BuildBodyIslands(const BodyID *inActiveBodies, uint32 inNumActiveBodies, TempAllocator *inTempAllocator); + void BuildConstraintIslands(const uint32 *inConstraintToBody, uint32 inNumConstraints, uint32 *&outConstraints, uint32 *&outConstraintsEnd, TempAllocator *inTempAllocator) const; + + /// Sorts the islands so that the islands with most constraints go first + void SortIslands(TempAllocator *inTempAllocator); + + /// Intermediate data structure that for each body keeps track what the lowest index of the body is that it is connected to + struct BodyLink + { + JPH_OVERRIDE_NEW_DELETE + + atomic mLinkedTo; ///< An index in mBodyLinks pointing to another body in this island with a lower index than this body + uint32 mIslandIndex; ///< The island index of this body (filled in during Finalize) + }; + + // Intermediate data + BodyLink * mBodyLinks = nullptr; ///< Maps bodies to the first body in the island + uint32 * mConstraintLinks = nullptr; ///< Maps constraint index to body index (which maps to island index) + uint32 * mContactLinks = nullptr; ///< Maps contact constraint index to body index (which maps to island index) + + // Final data + BodyID * mBodyIslands = nullptr; ///< Bodies ordered by island + uint32 * mBodyIslandEnds = nullptr; ///< End index of each body island + + uint32 * mConstraintIslands = nullptr; ///< Constraints ordered by island + uint32 * mConstraintIslandEnds = nullptr; ///< End index of each constraint island + + uint32 * mContactIslands = nullptr; ///< Contacts ordered by island + uint32 * mContactIslandEnds = nullptr; ///< End index of each contact island + + uint32 * mIslandsSorted = nullptr; ///< A list of island indices in order of most constraints first + + uint8 * mNumPositionSteps = nullptr; ///< Number of position steps for each island + + // Counters + uint32 mMaxActiveBodies; ///< Maximum size of the active bodies list (see BodyManager::mActiveBodies) + uint32 mNumActiveBodies = 0; ///< Number of active bodies passed to + uint32 mNumConstraints = 0; ///< Size of the constraint list (see ConstraintManager::mConstraints) + uint32 mMaxContacts = 0; ///< Maximum amount of contacts supported + uint32 mNumContacts = 0; ///< Size of the contacts list (see ContactConstraintManager::mNumConstraints) + uint32 mNumIslands = 0; ///< Final number of islands + +#ifdef JPH_VALIDATE_ISLAND_BUILDER + /// Structure to keep track of all added links to validate that islands were generated correctly + struct LinkValidation + { + uint32 mFirst; + uint32 mSecond; + }; + + LinkValidation * mLinkValidation = nullptr; + atomic mNumLinkValidation; +#endif +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.cpp b/thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.cpp new file mode 100644 index 000000000000..50ab1c583fad --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.cpp @@ -0,0 +1,582 @@ +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +//#define JPH_LARGE_ISLAND_SPLITTER_DEBUG + +JPH_NAMESPACE_BEGIN + +LargeIslandSplitter::EStatus LargeIslandSplitter::Splits::FetchNextBatch(uint32 &outConstraintsBegin, uint32 &outConstraintsEnd, uint32 &outContactsBegin, uint32 &outContactsEnd, bool &outFirstIteration) +{ + { + // First check if we can get a new batch (doing a read to avoid hammering an atomic with an atomic subtract) + // Note this also avoids overflowing the status counter if we're done but there's still one thread processing items + uint64 status = mStatus.load(memory_order_acquire); + + // Check for special value that indicates that the splits are still being built + // (note we do not check for this condition again below as we reset all splits before kicking off jobs that fetch batches of work) + if (status == StatusItemMask) + return EStatus::WaitingForBatch; + + // Next check if all items have been processed. Note that we do this after checking if the job can be started + // as mNumIterations is not initialized until the split is started. + if (sGetIteration(status) >= mNumIterations) + return EStatus::AllBatchesDone; + + uint item = sGetItem(status); + uint split_index = sGetSplit(status); + if (split_index == cNonParallelSplitIdx) + { + // Non parallel split needs to be taken as a single batch, only the thread that takes element 0 will do it + if (item != 0) + return EStatus::WaitingForBatch; + } + else + { + // Parallel split is split into batches + JPH_ASSERT(split_index < mNumSplits); + const Split &split = mSplits[split_index]; + if (item >= split.GetNumItems()) + return EStatus::WaitingForBatch; + } + } + + // Then try to actually get the batch + uint64 status = mStatus.fetch_add(cBatchSize, memory_order_acquire); + int iteration = sGetIteration(status); + if (iteration >= mNumIterations) + return EStatus::AllBatchesDone; + + uint split_index = sGetSplit(status); + JPH_ASSERT(split_index < mNumSplits || split_index == cNonParallelSplitIdx); + const Split &split = mSplits[split_index]; + uint item_begin = sGetItem(status); + if (split_index == cNonParallelSplitIdx) + { + if (item_begin == 0) + { + // Non-parallel split always goes as a single batch + outConstraintsBegin = split.mConstraintBufferBegin; + outConstraintsEnd = split.mConstraintBufferEnd; + outContactsBegin = split.mContactBufferBegin; + outContactsEnd = split.mContactBufferEnd; + outFirstIteration = iteration == 0; + return EStatus::BatchRetrieved; + } + else + { + // Otherwise we're done with this split + return EStatus::WaitingForBatch; + } + } + + // Parallel split is split into batches + uint num_constraints = split.GetNumConstraints(); + uint num_contacts = split.GetNumContacts(); + uint num_items = num_constraints + num_contacts; + if (item_begin >= num_items) + return EStatus::WaitingForBatch; + + uint item_end = min(item_begin + cBatchSize, num_items); + if (item_end >= num_constraints) + { + if (item_begin < num_constraints) + { + // Partially from constraints and partially from contacts + outConstraintsBegin = split.mConstraintBufferBegin + item_begin; + outConstraintsEnd = split.mConstraintBufferEnd; + } + else + { + // Only contacts + outConstraintsBegin = 0; + outConstraintsEnd = 0; + } + + outContactsBegin = split.mContactBufferBegin + (max(item_begin, num_constraints) - num_constraints); + outContactsEnd = split.mContactBufferBegin + (item_end - num_constraints); + } + else + { + // Only constraints + outConstraintsBegin = split.mConstraintBufferBegin + item_begin; + outConstraintsEnd = split.mConstraintBufferBegin + item_end; + + outContactsBegin = 0; + outContactsEnd = 0; + } + + outFirstIteration = iteration == 0; + return EStatus::BatchRetrieved; +} + +void LargeIslandSplitter::Splits::MarkBatchProcessed(uint inNumProcessed, bool &outLastIteration, bool &outFinalBatch) +{ + // We fetched this batch, nobody should change the split and or iteration until we mark the last batch as processed so we can safely get the current status + uint64 status = mStatus.load(memory_order_relaxed); + uint split_index = sGetSplit(status); + JPH_ASSERT(split_index < mNumSplits || split_index == cNonParallelSplitIdx); + const Split &split = mSplits[split_index]; + uint num_items_in_split = split.GetNumItems(); + + // Determine if this is the last iteration before possibly incrementing it + int iteration = sGetIteration(status); + outLastIteration = iteration == mNumIterations - 1; + + // Add the number of items we processed to the total number of items processed + // Note: This needs to happen after we read the status as other threads may update the status after we mark items as processed + JPH_ASSERT(inNumProcessed > 0); // Logic will break if we mark a block of 0 items as processed + uint total_items_processed = mItemsProcessed.fetch_add(inNumProcessed, memory_order_acq_rel) + inNumProcessed; + + // Check if we're at the end of the split + if (total_items_processed >= num_items_in_split) + { + JPH_ASSERT(total_items_processed == num_items_in_split); // Should not overflow, that means we're retiring more items than we should process + + // Set items processed back to 0 for the next split/iteration + mItemsProcessed.store(0, memory_order_release); + + // Determine next split + do + { + if (split_index == cNonParallelSplitIdx) + { + // At start of next iteration + split_index = 0; + ++iteration; + } + else + { + // At start of next split + ++split_index; + } + + // If we're beyond the end of splits, go to the non-parallel split + if (split_index >= mNumSplits) + split_index = cNonParallelSplitIdx; + } + while (iteration < mNumIterations + && mSplits[split_index].GetNumItems() == 0); // We don't support processing empty splits, skip to the next split in this case + + mStatus.store((uint64(iteration) << StatusIterationShift) | (uint64(split_index) << StatusSplitShift), memory_order_release); + } + + // Track if this is the final batch + outFinalBatch = iteration >= mNumIterations; +} + +LargeIslandSplitter::~LargeIslandSplitter() +{ + JPH_ASSERT(mSplitMasks == nullptr); + JPH_ASSERT(mContactAndConstraintsSplitIdx == nullptr); + JPH_ASSERT(mContactAndConstraintIndices == nullptr); + JPH_ASSERT(mSplitIslands == nullptr); +} + +void LargeIslandSplitter::Prepare(const IslandBuilder &inIslandBuilder, uint32 inNumActiveBodies, TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + // Count the total number of constraints and contacts that we will be putting in splits + mContactAndConstraintsSize = 0; + for (uint32 island = 0; island < inIslandBuilder.GetNumIslands(); ++island) + { + // Get the contacts in this island + uint32 *contacts_start, *contacts_end; + inIslandBuilder.GetContactsInIsland(island, contacts_start, contacts_end); + uint num_contacts_in_island = uint(contacts_end - contacts_start); + + // Get the constraints in this island + uint32 *constraints_start, *constraints_end; + inIslandBuilder.GetConstraintsInIsland(island, constraints_start, constraints_end); + uint num_constraints_in_island = uint(constraints_end - constraints_start); + + uint island_size = num_contacts_in_island + num_constraints_in_island; + if (island_size >= cLargeIslandTreshold) + { + mNumSplitIslands++; + mContactAndConstraintsSize += island_size; + } + else + break; // If this island doesn't have enough constraints, the next islands won't either since they're sorted from big to small + } + + if (mContactAndConstraintsSize > 0) + { + mNumActiveBodies = inNumActiveBodies; + + // Allocate split mask buffer + mSplitMasks = (SplitMask *)inTempAllocator->Allocate(mNumActiveBodies * sizeof(SplitMask)); + + // Allocate contact and constraint buffer + uint contact_and_constraint_indices_size = mContactAndConstraintsSize * sizeof(uint32); + mContactAndConstraintsSplitIdx = (uint32 *)inTempAllocator->Allocate(contact_and_constraint_indices_size); + mContactAndConstraintIndices = (uint32 *)inTempAllocator->Allocate(contact_and_constraint_indices_size); + + // Allocate island split buffer + mSplitIslands = (Splits *)inTempAllocator->Allocate(mNumSplitIslands * sizeof(Splits)); + + // Prevent any of the splits from being picked up as work + for (uint i = 0; i < mNumSplitIslands; ++i) + mSplitIslands[i].ResetStatus(); + } +} + +uint LargeIslandSplitter::AssignSplit(const Body *inBody1, const Body *inBody2) +{ + uint32 idx1 = inBody1->GetIndexInActiveBodiesInternal(); + uint32 idx2 = inBody2->GetIndexInActiveBodiesInternal(); + + // Test if either index is negative + if (idx1 == Body::cInactiveIndex || !inBody1->IsDynamic()) + { + // Body 1 is not active or a kinematic body, so we only need to set 1 body + JPH_ASSERT(idx2 < mNumActiveBodies); + SplitMask &mask = mSplitMasks[idx2]; + uint split = min(CountTrailingZeros(~uint32(mask)), cNonParallelSplitIdx); + mask |= SplitMask(1U << split); + return split; + } + else if (idx2 == Body::cInactiveIndex || !inBody2->IsDynamic()) + { + // Body 2 is not active or a kinematic body, so we only need to set 1 body + JPH_ASSERT(idx1 < mNumActiveBodies); + SplitMask &mask = mSplitMasks[idx1]; + uint split = min(CountTrailingZeros(~uint32(mask)), cNonParallelSplitIdx); + mask |= SplitMask(1U << split); + return split; + } + else + { + // If both bodies are active, we need to set 2 bodies + JPH_ASSERT(idx1 < mNumActiveBodies); + JPH_ASSERT(idx2 < mNumActiveBodies); + SplitMask &mask1 = mSplitMasks[idx1]; + SplitMask &mask2 = mSplitMasks[idx2]; + uint split = min(CountTrailingZeros((~uint32(mask1)) & (~uint32(mask2))), cNonParallelSplitIdx); + SplitMask mask = SplitMask(1U << split); + mask1 |= mask; + mask2 |= mask; + return split; + } +} + +uint LargeIslandSplitter::AssignToNonParallelSplit(const Body *inBody) +{ + uint32 idx = inBody->GetIndexInActiveBodiesInternal(); + if (idx != Body::cInactiveIndex) + { + JPH_ASSERT(idx < mNumActiveBodies); + mSplitMasks[idx] |= 1U << cNonParallelSplitIdx; + } + + return cNonParallelSplitIdx; +} + +bool LargeIslandSplitter::SplitIsland(uint32 inIslandIndex, const IslandBuilder &inIslandBuilder, const BodyManager &inBodyManager, const ContactConstraintManager &inContactManager, Constraint **inActiveConstraints, CalculateSolverSteps &ioStepsCalculator) +{ + JPH_PROFILE_FUNCTION(); + + // Get the contacts in this island + uint32 *contacts_start, *contacts_end; + inIslandBuilder.GetContactsInIsland(inIslandIndex, contacts_start, contacts_end); + uint num_contacts_in_island = uint(contacts_end - contacts_start); + + // Get the constraints in this island + uint32 *constraints_start, *constraints_end; + inIslandBuilder.GetConstraintsInIsland(inIslandIndex, constraints_start, constraints_end); + uint num_constraints_in_island = uint(constraints_end - constraints_start); + + // Check if it exceeds the threshold + uint island_size = num_contacts_in_island + num_constraints_in_island; + if (island_size < cLargeIslandTreshold) + return false; + + // Get bodies in this island + BodyID *bodies_start, *bodies_end; + inIslandBuilder.GetBodiesInIsland(inIslandIndex, bodies_start, bodies_end); + + // Reset the split mask for all bodies in this island + Body const * const *bodies = inBodyManager.GetBodies().data(); + for (const BodyID *b = bodies_start; b < bodies_end; ++b) + mSplitMasks[bodies[b->GetIndex()]->GetIndexInActiveBodiesInternal()] = 0; + + // Count the number of contacts and constraints per split + uint num_contacts_in_split[cNumSplits] = { }; + uint num_constraints_in_split[cNumSplits] = { }; + + // Get space to store split indices + uint offset = mContactAndConstraintsNextFree.fetch_add(island_size, memory_order_relaxed); + uint32 *contact_split_idx = mContactAndConstraintsSplitIdx + offset; + uint32 *constraint_split_idx = contact_split_idx + num_contacts_in_island; + + // Assign the contacts to a split + uint32 *cur_contact_split_idx = contact_split_idx; + for (const uint32 *c = contacts_start; c < contacts_end; ++c) + { + const Body *body1, *body2; + inContactManager.GetAffectedBodies(*c, body1, body2); + uint split = AssignSplit(body1, body2); + num_contacts_in_split[split]++; + *cur_contact_split_idx++ = split; + + if (body1->IsDynamic()) + ioStepsCalculator(body1->GetMotionPropertiesUnchecked()); + if (body2->IsDynamic()) + ioStepsCalculator(body2->GetMotionPropertiesUnchecked()); + } + + // Assign the constraints to a split + uint32 *cur_constraint_split_idx = constraint_split_idx; + for (const uint32 *c = constraints_start; c < constraints_end; ++c) + { + const Constraint *constraint = inActiveConstraints[*c]; + uint split = constraint->BuildIslandSplits(*this); + num_constraints_in_split[split]++; + *cur_constraint_split_idx++ = split; + + ioStepsCalculator(constraint); + } + + ioStepsCalculator.Finalize(); + + // Start with 0 splits + uint split_remap_table[cNumSplits]; + uint new_split_idx = mNextSplitIsland.fetch_add(1, memory_order_relaxed); + JPH_ASSERT(new_split_idx < mNumSplitIslands); + Splits &splits = mSplitIslands[new_split_idx]; + splits.mIslandIndex = inIslandIndex; + splits.mNumSplits = 0; + splits.mNumIterations = ioStepsCalculator.GetNumVelocitySteps() + 1; // Iteration 0 is used for warm starting + splits.mNumVelocitySteps = ioStepsCalculator.GetNumVelocitySteps(); + splits.mNumPositionSteps = ioStepsCalculator.GetNumPositionSteps(); + splits.mItemsProcessed.store(0, memory_order_release); + + // Allocate space to store the sorted constraint and contact indices per split + uint32 *constraint_buffer_cur[cNumSplits], *contact_buffer_cur[cNumSplits]; + for (uint s = 0; s < cNumSplits; ++s) + { + // If this split doesn't contain enough constraints and contacts, we will combine it with the non parallel split + if (num_constraints_in_split[s] + num_contacts_in_split[s] < cSplitCombineTreshold + && s < cNonParallelSplitIdx) // The non-parallel split cannot merge into itself + { + // Remap it + split_remap_table[s] = cNonParallelSplitIdx; + + // Add the counts to the non parallel split + num_contacts_in_split[cNonParallelSplitIdx] += num_contacts_in_split[s]; + num_constraints_in_split[cNonParallelSplitIdx] += num_constraints_in_split[s]; + } + else + { + // This split is valid, map it to the next empty slot + uint target_split; + if (s < cNonParallelSplitIdx) + target_split = splits.mNumSplits++; + else + target_split = cNonParallelSplitIdx; + Split &split = splits.mSplits[target_split]; + split_remap_table[s] = target_split; + + // Allocate space for contacts + split.mContactBufferBegin = offset; + split.mContactBufferEnd = split.mContactBufferBegin + num_contacts_in_split[s]; + + // Allocate space for constraints + split.mConstraintBufferBegin = split.mContactBufferEnd; + split.mConstraintBufferEnd = split.mConstraintBufferBegin + num_constraints_in_split[s]; + + // Store start for each split + contact_buffer_cur[target_split] = mContactAndConstraintIndices + split.mContactBufferBegin; + constraint_buffer_cur[target_split] = mContactAndConstraintIndices + split.mConstraintBufferBegin; + + // Update offset + offset = split.mConstraintBufferEnd; + } + } + + // Split the contacts + for (uint c = 0; c < num_contacts_in_island; ++c) + { + uint split = split_remap_table[contact_split_idx[c]]; + *contact_buffer_cur[split]++ = contacts_start[c]; + } + + // Split the constraints + for (uint c = 0; c < num_constraints_in_island; ++c) + { + uint split = split_remap_table[constraint_split_idx[c]]; + *constraint_buffer_cur[split]++ = constraints_start[c]; + } + +#ifdef JPH_LARGE_ISLAND_SPLITTER_DEBUG + // Trace the size of all splits + uint sum = 0; + String stats; + for (uint s = 0; s < cNumSplits; ++s) + { + // If we've processed all splits, jump to the non-parallel split + if (s >= splits.GetNumSplits()) + s = cNonParallelSplitIdx; + + const Split &split = splits.mSplits[s]; + stats += StringFormat("g:%d:%d:%d, ", s, split.GetNumContacts(), split.GetNumConstraints()); + sum += split.GetNumItems(); + } + stats += StringFormat("sum: %d", sum); + Trace(stats.c_str()); +#endif // JPH_LARGE_ISLAND_SPLITTER_DEBUG + +#ifdef JPH_ENABLE_ASSERTS + for (uint s = 0; s < cNumSplits; ++s) + { + // If there are no more splits, process the non-parallel split + if (s >= splits.mNumSplits) + s = cNonParallelSplitIdx; + + // Check that we wrote all elements + Split &split = splits.mSplits[s]; + JPH_ASSERT(contact_buffer_cur[s] == mContactAndConstraintIndices + split.mContactBufferEnd); + JPH_ASSERT(constraint_buffer_cur[s] == mContactAndConstraintIndices + split.mConstraintBufferEnd); + } + +#ifdef JPH_DEBUG + // Validate that the splits are indeed not touching the same body + for (uint s = 0; s < splits.mNumSplits; ++s) + { + Array body_used(mNumActiveBodies, false); + + // Validate contacts + uint32 split_contacts_begin, split_contacts_end; + splits.GetContactsInSplit(s, split_contacts_begin, split_contacts_end); + for (uint32 *c = mContactAndConstraintIndices + split_contacts_begin; c < mContactAndConstraintIndices + split_contacts_end; ++c) + { + const Body *body1, *body2; + inContactManager.GetAffectedBodies(*c, body1, body2); + + uint32 idx1 = body1->GetIndexInActiveBodiesInternal(); + if (idx1 != Body::cInactiveIndex && body1->IsDynamic()) + { + JPH_ASSERT(!body_used[idx1]); + body_used[idx1] = true; + } + + uint32 idx2 = body2->GetIndexInActiveBodiesInternal(); + if (idx2 != Body::cInactiveIndex && body2->IsDynamic()) + { + JPH_ASSERT(!body_used[idx2]); + body_used[idx2] = true; + } + } + } +#endif // JPH_DEBUG +#endif // JPH_ENABLE_ASSERTS + + // Allow other threads to pick up this split island now + splits.StartFirstBatch(); + return true; +} + +LargeIslandSplitter::EStatus LargeIslandSplitter::FetchNextBatch(uint &outSplitIslandIndex, uint32 *&outConstraintsBegin, uint32 *&outConstraintsEnd, uint32 *&outContactsBegin, uint32 *&outContactsEnd, bool &outFirstIteration) +{ + // We can't be done when all islands haven't been submitted yet + uint num_splits_created = mNextSplitIsland.load(memory_order_acquire); + bool all_done = num_splits_created == mNumSplitIslands; + + // Loop over all split islands to find work + uint32 constraints_begin, constraints_end, contacts_begin, contacts_end; + for (Splits *s = mSplitIslands; s < mSplitIslands + num_splits_created; ++s) + switch (s->FetchNextBatch(constraints_begin, constraints_end, contacts_begin, contacts_end, outFirstIteration)) + { + case EStatus::AllBatchesDone: + break; + + case EStatus::WaitingForBatch: + all_done = false; + break; + + case EStatus::BatchRetrieved: + outSplitIslandIndex = uint(s - mSplitIslands); + outConstraintsBegin = mContactAndConstraintIndices + constraints_begin; + outConstraintsEnd = mContactAndConstraintIndices + constraints_end; + outContactsBegin = mContactAndConstraintIndices + contacts_begin; + outContactsEnd = mContactAndConstraintIndices + contacts_end; + return EStatus::BatchRetrieved; + } + + return all_done? EStatus::AllBatchesDone : EStatus::WaitingForBatch; +} + +void LargeIslandSplitter::MarkBatchProcessed(uint inSplitIslandIndex, const uint32 *inConstraintsBegin, const uint32 *inConstraintsEnd, const uint32 *inContactsBegin, const uint32 *inContactsEnd, bool &outLastIteration, bool &outFinalBatch) +{ + uint num_items_processed = uint(inConstraintsEnd - inConstraintsBegin) + uint(inContactsEnd - inContactsBegin); + + JPH_ASSERT(inSplitIslandIndex < mNextSplitIsland.load(memory_order_relaxed)); + Splits &splits = mSplitIslands[inSplitIslandIndex]; + splits.MarkBatchProcessed(num_items_processed, outLastIteration, outFinalBatch); +} + +void LargeIslandSplitter::PrepareForSolvePositions() +{ + for (Splits *s = mSplitIslands, *s_end = mSplitIslands + mNumSplitIslands; s < s_end; ++s) + { + // Set the number of iterations to the number of position steps + s->mNumIterations = s->mNumPositionSteps; + + // We can start again from the first batch + s->StartFirstBatch(); + } +} + +void LargeIslandSplitter::Reset(TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + // Everything should have been used + JPH_ASSERT(mContactAndConstraintsNextFree.load(memory_order_relaxed) == mContactAndConstraintsSize); + JPH_ASSERT(mNextSplitIsland.load(memory_order_relaxed) == mNumSplitIslands); + + // Free split islands + if (mNumSplitIslands > 0) + { + inTempAllocator->Free(mSplitIslands, mNumSplitIslands * sizeof(Splits)); + mSplitIslands = nullptr; + + mNumSplitIslands = 0; + mNextSplitIsland.store(0, memory_order_relaxed); + } + + // Free contact and constraint buffers + if (mContactAndConstraintsSize > 0) + { + inTempAllocator->Free(mContactAndConstraintIndices, mContactAndConstraintsSize * sizeof(uint32)); + mContactAndConstraintIndices = nullptr; + + inTempAllocator->Free(mContactAndConstraintsSplitIdx, mContactAndConstraintsSize * sizeof(uint32)); + mContactAndConstraintsSplitIdx = nullptr; + + mContactAndConstraintsSize = 0; + mContactAndConstraintsNextFree.store(0, memory_order_relaxed); + } + + // Free split masks + if (mSplitMasks != nullptr) + { + inTempAllocator->Free(mSplitMasks, mNumActiveBodies * sizeof(SplitMask)); + mSplitMasks = nullptr; + + mNumActiveBodies = 0; + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.h b/thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.h new file mode 100644 index 000000000000..36f0d6140126 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.h @@ -0,0 +1,185 @@ +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class Body; +class BodyID; +class IslandBuilder; +class TempAllocator; +class Constraint; +class BodyManager; +class ContactConstraintManager; +class CalculateSolverSteps; + +/// Assigns bodies in large islands to multiple groups that can run in parallel +/// +/// This basically implements what is described in: High-Performance Physical Simulations on Next-Generation Architecture with Many Cores by Chen et al. +/// See: http://web.eecs.umich.edu/~msmelyan/papers/physsim_onmanycore_itj.pdf section "PARALLELIZATION METHODOLOGY" +class LargeIslandSplitter : public NonCopyable +{ +private: + using SplitMask = uint32; + +public: + static constexpr uint cNumSplits = sizeof(SplitMask) * 8; + static constexpr uint cNonParallelSplitIdx = cNumSplits - 1; + static constexpr uint cLargeIslandTreshold = 128; ///< If the number of constraints + contacts in an island is larger than this, we will try to split the island + + /// Status code for retrieving a batch + enum class EStatus + { + WaitingForBatch, ///< Work is expected to be available later + BatchRetrieved, ///< Work is being returned + AllBatchesDone, ///< No further work is expected from this + }; + + /// Describes a split of constraints and contacts + struct Split + { + inline uint GetNumContacts() const { return mContactBufferEnd - mContactBufferBegin; } + inline uint GetNumConstraints() const { return mConstraintBufferEnd - mConstraintBufferBegin; } + inline uint GetNumItems() const { return GetNumContacts() + GetNumConstraints(); } + + uint32 mContactBufferBegin; ///< Begin of the contact buffer (offset relative to mContactAndConstraintIndices) + uint32 mContactBufferEnd; ///< End of the contact buffer + + uint32 mConstraintBufferBegin; ///< Begin of the constraint buffer (offset relative to mContactAndConstraintIndices) + uint32 mConstraintBufferEnd; ///< End of the constraint buffer + }; + + /// Structure that describes the resulting splits from the large island splitter + class Splits + { + public: + inline uint GetNumSplits() const + { + return mNumSplits; + } + + inline void GetConstraintsInSplit(uint inSplitIndex, uint32 &outConstraintsBegin, uint32 &outConstraintsEnd) const + { + const Split &split = mSplits[inSplitIndex]; + outConstraintsBegin = split.mConstraintBufferBegin; + outConstraintsEnd = split.mConstraintBufferEnd; + } + + inline void GetContactsInSplit(uint inSplitIndex, uint32 &outContactsBegin, uint32 &outContactsEnd) const + { + const Split &split = mSplits[inSplitIndex]; + outContactsBegin = split.mContactBufferBegin; + outContactsEnd = split.mContactBufferEnd; + } + + /// Reset current status so that no work can be picked up from this split + inline void ResetStatus() + { + mStatus.store(StatusItemMask, memory_order_relaxed); + } + + /// Make the first batch available to other threads + inline void StartFirstBatch() + { + uint split_index = mNumSplits > 0? 0 : cNonParallelSplitIdx; + mStatus.store(uint64(split_index) << StatusSplitShift, memory_order_release); + } + + /// Fetch the next batch to process + EStatus FetchNextBatch(uint32 &outConstraintsBegin, uint32 &outConstraintsEnd, uint32 &outContactsBegin, uint32 &outContactsEnd, bool &outFirstIteration); + + /// Mark a batch as processed + void MarkBatchProcessed(uint inNumProcessed, bool &outLastIteration, bool &outFinalBatch); + + enum EIterationStatus : uint64 + { + StatusIterationMask = 0xffff000000000000, + StatusIterationShift = 48, + StatusSplitMask = 0x0000ffff00000000, + StatusSplitShift = 32, + StatusItemMask = 0x00000000ffffffff, + }; + + static inline int sGetIteration(uint64 inStatus) + { + return int((inStatus & StatusIterationMask) >> StatusIterationShift); + } + + static inline uint sGetSplit(uint64 inStatus) + { + return uint((inStatus & StatusSplitMask) >> StatusSplitShift); + } + + static inline uint sGetItem(uint64 inStatus) + { + return uint(inStatus & StatusItemMask); + } + + Split mSplits[cNumSplits]; ///< Data per split + uint32 mIslandIndex; ///< Index of the island that was split + uint mNumSplits; ///< Number of splits that were created (excluding the non-parallel split) + int mNumIterations; ///< Number of iterations to do + int mNumVelocitySteps; ///< Number of velocity steps to do (cached for 2nd sub step) + int mNumPositionSteps; ///< Number of position steps to do + atomic mStatus; ///< Status of the split, see EIterationStatus + atomic mItemsProcessed; ///< Number of items that have been marked as processed + }; + +public: + /// Destructor + ~LargeIslandSplitter(); + + /// Prepare the island splitter by allocating memory + void Prepare(const IslandBuilder &inIslandBuilder, uint32 inNumActiveBodies, TempAllocator *inTempAllocator); + + /// Assign two bodies to a split. Returns the split index. + uint AssignSplit(const Body *inBody1, const Body *inBody2); + + /// Force a body to be in a non parallel split. Returns the split index. + uint AssignToNonParallelSplit(const Body *inBody); + + /// Splits up an island, the created splits will be added to the list of batches and can be fetched with FetchNextBatch. Returns false if the island did not need splitting. + bool SplitIsland(uint32 inIslandIndex, const IslandBuilder &inIslandBuilder, const BodyManager &inBodyManager, const ContactConstraintManager &inContactManager, Constraint **inActiveConstraints, CalculateSolverSteps &ioStepsCalculator); + + /// Fetch the next batch to process, returns a handle in outSplitIslandIndex that must be provided to MarkBatchProcessed when complete + EStatus FetchNextBatch(uint &outSplitIslandIndex, uint32 *&outConstraintsBegin, uint32 *&outConstraintsEnd, uint32 *&outContactsBegin, uint32 *&outContactsEnd, bool &outFirstIteration); + + /// Mark a batch as processed + void MarkBatchProcessed(uint inSplitIslandIndex, const uint32 *inConstraintsBegin, const uint32 *inConstraintsEnd, const uint32 *inContactsBegin, const uint32 *inContactsEnd, bool &outLastIteration, bool &outFinalBatch); + + /// Get the island index of the island that was split for a particular split island index + inline uint32 GetIslandIndex(uint inSplitIslandIndex) const + { + JPH_ASSERT(inSplitIslandIndex < mNumSplitIslands); + return mSplitIslands[inSplitIslandIndex].mIslandIndex; + } + + /// Prepare the island splitter for iterating over the split islands again for position solving. Marks all batches as startable. + void PrepareForSolvePositions(); + + /// Reset the island splitter + void Reset(TempAllocator *inTempAllocator); + +private: + static constexpr uint cSplitCombineTreshold = 32; ///< If the number of constraints + contacts in a split is lower than this, we will merge this split into the 'non-parallel split' + static constexpr uint cBatchSize = 16; ///< Number of items to process in a constraint batch + + uint32 mNumActiveBodies = 0; ///< Cached number of active bodies + + SplitMask * mSplitMasks = nullptr; ///< Bits that indicate for each body in the BodyManager::mActiveBodies list which split they already belong to + + uint32 * mContactAndConstraintsSplitIdx = nullptr; ///< Buffer to store the split index per constraint or contact + uint32 * mContactAndConstraintIndices = nullptr; ///< Buffer to store the ordered constraint indices per split + uint mContactAndConstraintsSize = 0; ///< Total size of mContactAndConstraintsSplitIdx and mContactAndConstraintIndices + atomic mContactAndConstraintsNextFree { 0 }; ///< Next element that is free in both buffers + + uint mNumSplitIslands = 0; ///< Total number of islands that required splitting + Splits * mSplitIslands = nullptr; ///< List of islands that required splitting + atomic mNextSplitIsland = 0; ///< Next split island to pick from mSplitIslands +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsLock.h b/thirdparty/jolt_physics/Jolt/Physics/PhysicsLock.h new file mode 100644 index 000000000000..1e83d1881011 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsLock.h @@ -0,0 +1,169 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_ENABLE_ASSERTS + +/// This is the list of locks used by the physics engine, they need to be locked in a particular order (from top of the list to bottom of the list) in order to prevent deadlocks +enum class EPhysicsLockTypes +{ + BroadPhaseQuery = 1 << 0, + PerBody = 1 << 1, + BodiesList = 1 << 2, + BroadPhaseUpdate = 1 << 3, + ConstraintsList = 1 << 4, + ActiveBodiesList = 1 << 5, +}; + +/// A token that indicates the context of a lock (we use 1 per physics system and we use the body manager pointer because it's convenient) +class BodyManager; +using PhysicsLockContext = const BodyManager *; + +#endif // !JPH_ENABLE_ASSERTS + +/// Helpers to safely lock the different mutexes that are part of the physics system while preventing deadlock +/// Class that keeps track per thread which lock are taken and if the order of locking is correct +class JPH_EXPORT PhysicsLock +{ +public: +#ifdef JPH_ENABLE_ASSERTS + /// Call before taking the lock + static inline void sCheckLock(PhysicsLockContext inContext, EPhysicsLockTypes inType) + { + uint32 &mutexes = sGetLockedMutexes(inContext); + JPH_ASSERT(uint32(inType) > mutexes, "A lock of same or higher priority was already taken, this can create a deadlock!"); + mutexes = mutexes | uint32(inType); + } + + /// Call after releasing the lock + static inline void sCheckUnlock(PhysicsLockContext inContext, EPhysicsLockTypes inType) + { + uint32 &mutexes = sGetLockedMutexes(inContext); + JPH_ASSERT((mutexes & uint32(inType)) != 0, "Mutex was not locked!"); + mutexes = mutexes & ~uint32(inType); + } +#endif // !JPH_ENABLE_ASSERTS + + template + static inline void sLock(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) + { + JPH_IF_ENABLE_ASSERTS(sCheckLock(inContext, inType);) + inMutex.lock(); + } + + template + static inline void sUnlock(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) + { + JPH_IF_ENABLE_ASSERTS(sCheckUnlock(inContext, inType);) + inMutex.unlock(); + } + + template + static inline void sLockShared(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) + { + JPH_IF_ENABLE_ASSERTS(sCheckLock(inContext, inType);) + inMutex.lock_shared(); + } + + template + static inline void sUnlockShared(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) + { + JPH_IF_ENABLE_ASSERTS(sCheckUnlock(inContext, inType);) + inMutex.unlock_shared(); + } + +#ifdef JPH_ENABLE_ASSERTS +private: + struct LockData + { + uint32 mLockedMutexes = 0; + PhysicsLockContext mContext = nullptr; + }; + + // Helper function to find the locked mutexes for a particular context + static uint32 & sGetLockedMutexes(PhysicsLockContext inContext) + { + static thread_local LockData sLocks[4]; + + // If we find a matching context we can use it + for (LockData &l : sLocks) + if (l.mContext == inContext) + return l.mLockedMutexes; + + // Otherwise we look for an entry that is not in use + for (LockData &l : sLocks) + if (l.mLockedMutexes == 0) + { + l.mContext = inContext; + return l.mLockedMutexes; + } + + JPH_ASSERT(false, "Too many physics systems locked at the same time!"); + return sLocks[0].mLockedMutexes; + } +#endif // !JPH_ENABLE_ASSERTS +}; + +/// Helper class that is similar to std::unique_lock +template +class UniqueLock : public NonCopyable +{ +public: + explicit UniqueLock(LockType &inLock JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) : + mLock(inLock) +#ifdef JPH_ENABLE_ASSERTS + , mContext(inContext), + mType(inType) +#endif // JPH_ENABLE_ASSERTS + { + PhysicsLock::sLock(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType)); + } + + ~UniqueLock() + { + PhysicsLock::sUnlock(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType)); + } + +private: + LockType & mLock; +#ifdef JPH_ENABLE_ASSERTS + PhysicsLockContext mContext; + EPhysicsLockTypes mType; +#endif // JPH_ENABLE_ASSERTS +}; + +/// Helper class that is similar to std::shared_lock +template +class SharedLock : public NonCopyable +{ +public: + explicit SharedLock(LockType &inLock JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) : + mLock(inLock) +#ifdef JPH_ENABLE_ASSERTS + , mContext(inContext) + , mType(inType) +#endif // JPH_ENABLE_ASSERTS + { + PhysicsLock::sLockShared(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType)); + } + + ~SharedLock() + { + PhysicsLock::sUnlockShared(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType)); + } + +private: + LockType & mLock; +#ifdef JPH_ENABLE_ASSERTS + PhysicsLockContext mContext; + EPhysicsLockTypes mType; +#endif // JPH_ENABLE_ASSERTS +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsScene.cpp b/thirdparty/jolt_physics/Jolt/Physics/PhysicsScene.cpp new file mode 100644 index 000000000000..1e9c60d230ef --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsScene.cpp @@ -0,0 +1,261 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(PhysicsScene) +{ + JPH_ADD_ATTRIBUTE(PhysicsScene, mBodies) + JPH_ADD_ATTRIBUTE(PhysicsScene, mConstraints) + JPH_ADD_ATTRIBUTE(PhysicsScene, mSoftBodies) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(PhysicsScene::ConnectedConstraint) +{ + JPH_ADD_ATTRIBUTE(PhysicsScene::ConnectedConstraint, mSettings) + JPH_ADD_ATTRIBUTE(PhysicsScene::ConnectedConstraint, mBody1) + JPH_ADD_ATTRIBUTE(PhysicsScene::ConnectedConstraint, mBody2) +} + +void PhysicsScene::AddBody(const BodyCreationSettings &inBody) +{ + mBodies.push_back(inBody); +} + +void PhysicsScene::AddConstraint(const TwoBodyConstraintSettings *inConstraint, uint32 inBody1, uint32 inBody2) +{ + mConstraints.emplace_back(inConstraint, inBody1, inBody2); +} + +void PhysicsScene::AddSoftBody(const SoftBodyCreationSettings &inSoftBody) +{ + mSoftBodies.push_back(inSoftBody); +} + +bool PhysicsScene::FixInvalidScales() +{ + const Vec3 unit_scale = Vec3::sReplicate(1.0f); + + bool success = true; + for (BodyCreationSettings &b : mBodies) + { + // Test if there is an invalid scale in the shape hierarchy + const Shape *shape = b.GetShape(); + if (!shape->IsValidScale(unit_scale)) + { + // Fix it up + Shape::ShapeResult result = shape->ScaleShape(unit_scale); + if (result.IsValid()) + b.SetShape(result.Get()); + else + success = false; + } + } + return success; +} + +bool PhysicsScene::CreateBodies(PhysicsSystem *inSystem) const +{ + BodyInterface &bi = inSystem->GetBodyInterface(); + + BodyIDVector body_ids; + body_ids.reserve(mBodies.size() + mSoftBodies.size()); + + // Create bodies + for (const BodyCreationSettings &b : mBodies) + { + const Body *body = bi.CreateBody(b); + if (body == nullptr) + break; + body_ids.push_back(body->GetID()); + } + + // Create soft bodies + for (const SoftBodyCreationSettings &b : mSoftBodies) + { + const Body *body = bi.CreateSoftBody(b); + if (body == nullptr) + break; + body_ids.push_back(body->GetID()); + } + + // Batch add bodies + BodyIDVector temp_body_ids = body_ids; // Body ID's get shuffled by AddBodiesPrepare + BodyInterface::AddState add_state = bi.AddBodiesPrepare(temp_body_ids.data(), (int)temp_body_ids.size()); + bi.AddBodiesFinalize(temp_body_ids.data(), (int)temp_body_ids.size(), add_state, EActivation::Activate); + + // If not all bodies are created, creating constraints will be unreliable + if (body_ids.size() != mBodies.size() + mSoftBodies.size()) + return false; + + // Create constraints + for (const ConnectedConstraint &cc : mConstraints) + { + BodyID body1_id = cc.mBody1 == cFixedToWorld? BodyID() : body_ids[cc.mBody1]; + BodyID body2_id = cc.mBody2 == cFixedToWorld? BodyID() : body_ids[cc.mBody2]; + Constraint *c = bi.CreateConstraint(cc.mSettings, body1_id, body2_id); + inSystem->AddConstraint(c); + } + + // Everything was created + return true; +} + +void PhysicsScene::SaveBinaryState(StreamOut &inStream, bool inSaveShapes, bool inSaveGroupFilter) const +{ + BodyCreationSettings::ShapeToIDMap shape_to_id; + BodyCreationSettings::MaterialToIDMap material_to_id; + BodyCreationSettings::GroupFilterToIDMap group_filter_to_id; + SoftBodyCreationSettings::SharedSettingsToIDMap settings_to_id; + + // Save bodies + inStream.Write((uint32)mBodies.size()); + for (const BodyCreationSettings &b : mBodies) + b.SaveWithChildren(inStream, inSaveShapes? &shape_to_id : nullptr, inSaveShapes? &material_to_id : nullptr, inSaveGroupFilter? &group_filter_to_id : nullptr); + + // Save constraints + inStream.Write((uint32)mConstraints.size()); + for (const ConnectedConstraint &cc : mConstraints) + { + cc.mSettings->SaveBinaryState(inStream); + inStream.Write(cc.mBody1); + inStream.Write(cc.mBody2); + } + + // Save soft bodies + inStream.Write((uint32)mSoftBodies.size()); + for (const SoftBodyCreationSettings &b : mSoftBodies) + b.SaveWithChildren(inStream, &settings_to_id, &material_to_id, inSaveGroupFilter? &group_filter_to_id : nullptr); +} + +PhysicsScene::PhysicsSceneResult PhysicsScene::sRestoreFromBinaryState(StreamIn &inStream) +{ + PhysicsSceneResult result; + + // Create scene + Ref scene = new PhysicsScene(); + + BodyCreationSettings::IDToShapeMap id_to_shape; + BodyCreationSettings::IDToMaterialMap id_to_material; + BodyCreationSettings::IDToGroupFilterMap id_to_group_filter; + SoftBodyCreationSettings::IDToSharedSettingsMap id_to_settings; + + // Reserve some memory to avoid frequent reallocations + id_to_shape.reserve(1024); + id_to_material.reserve(128); + id_to_group_filter.reserve(128); + + // Read bodies + uint32 len = 0; + inStream.Read(len); + scene->mBodies.resize(len); + for (BodyCreationSettings &b : scene->mBodies) + { + // Read creation settings + BodyCreationSettings::BCSResult bcs_result = BodyCreationSettings::sRestoreWithChildren(inStream, id_to_shape, id_to_material, id_to_group_filter); + if (bcs_result.HasError()) + { + result.SetError(bcs_result.GetError()); + return result; + } + b = bcs_result.Get(); + } + + // Read constraints + len = 0; + inStream.Read(len); + scene->mConstraints.resize(len); + for (ConnectedConstraint &cc : scene->mConstraints) + { + ConstraintSettings::ConstraintResult c_result = ConstraintSettings::sRestoreFromBinaryState(inStream); + if (c_result.HasError()) + { + result.SetError(c_result.GetError()); + return result; + } + cc.mSettings = StaticCast(c_result.Get()); + inStream.Read(cc.mBody1); + inStream.Read(cc.mBody2); + } + + // Read soft bodies + len = 0; + inStream.Read(len); + scene->mSoftBodies.resize(len); + for (SoftBodyCreationSettings &b : scene->mSoftBodies) + { + // Read creation settings + SoftBodyCreationSettings::SBCSResult sbcs_result = SoftBodyCreationSettings::sRestoreWithChildren(inStream, id_to_settings, id_to_material, id_to_group_filter); + if (sbcs_result.HasError()) + { + result.SetError(sbcs_result.GetError()); + return result; + } + b = sbcs_result.Get(); + } + + result.Set(scene); + return result; +} + +void PhysicsScene::FromPhysicsSystem(const PhysicsSystem *inSystem) +{ + // This map will track where each body went in mBodies + using BodyIDToIdxMap = UnorderedMap; + BodyIDToIdxMap body_id_to_idx; + + // Map invalid ID + body_id_to_idx[BodyID()] = cFixedToWorld; + + // Get all bodies + BodyIDVector body_ids; + inSystem->GetBodies(body_ids); + + // Loop over all bodies + const BodyLockInterface &bli = inSystem->GetBodyLockInterface(); + for (const BodyID &id : body_ids) + { + BodyLockRead lock(bli, id); + if (lock.Succeeded()) + { + // Store location of body + body_id_to_idx[id] = (uint32)mBodies.size(); + + const Body &body = lock.GetBody(); + + // Convert to body creation settings + if (body.IsRigidBody()) + AddBody(body.GetBodyCreationSettings()); + else + AddSoftBody(body.GetSoftBodyCreationSettings()); + } + } + + // Loop over all constraints + Constraints constraints = inSystem->GetConstraints(); + for (const Constraint *c : constraints) + if (c->GetType() == EConstraintType::TwoBodyConstraint) + { + // Cast to two body constraint + const TwoBodyConstraint *tbc = static_cast(c); + + // Find the body indices + BodyIDToIdxMap::const_iterator b1 = body_id_to_idx.find(tbc->GetBody1()->GetID()); + BodyIDToIdxMap::const_iterator b2 = body_id_to_idx.find(tbc->GetBody2()->GetID()); + JPH_ASSERT(b1 != body_id_to_idx.end() && b2 != body_id_to_idx.end()); + + // Create constraint settings and add the constraint + Ref settings = c->GetConstraintSettings(); + AddConstraint(StaticCast(settings), b1->second, b2->second); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsScene.h b/thirdparty/jolt_physics/Jolt/Physics/PhysicsScene.h new file mode 100644 index 000000000000..f974f83bd585 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsScene.h @@ -0,0 +1,104 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; + +/// Contains the creation settings of a set of bodies +class JPH_EXPORT PhysicsScene : public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, PhysicsScene) + +public: + /// Add a body to the scene + void AddBody(const BodyCreationSettings &inBody); + + /// Body constant to use to indicate that the constraint is attached to the fixed world + static constexpr uint32 cFixedToWorld = 0xffffffff; + + /// Add a constraint to the scene + /// @param inConstraint Constraint settings + /// @param inBody1 Index in the bodies list of first body to attach constraint to + /// @param inBody2 Index in the bodies list of the second body to attach constraint to + void AddConstraint(const TwoBodyConstraintSettings *inConstraint, uint32 inBody1, uint32 inBody2); + + /// Add a soft body to the scene + void AddSoftBody(const SoftBodyCreationSettings &inSoftBody); + + /// Get number of bodies in this scene + size_t GetNumBodies() const { return mBodies.size(); } + + /// Access to the body settings for this scene + const Array & GetBodies() const { return mBodies; } + Array & GetBodies() { return mBodies; } + + /// A constraint and how it is connected to the bodies in the scene + class ConnectedConstraint + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, ConnectedConstraint) + + public: + ConnectedConstraint() = default; + ConnectedConstraint(const TwoBodyConstraintSettings *inSettings, uint inBody1, uint inBody2) : mSettings(inSettings), mBody1(inBody1), mBody2(inBody2) { } + + RefConst mSettings; ///< Constraint settings + uint32 mBody1; ///< Index of first body (in mBodies) + uint32 mBody2; ///< Index of second body (in mBodies) + }; + + /// Get number of constraints in this scene + size_t GetNumConstraints() const { return mConstraints.size(); } + + /// Access to the constraints for this scene + const Array & GetConstraints() const { return mConstraints; } + Array & GetConstraints() { return mConstraints; } + + /// Get number of bodies in this scene + size_t GetNumSoftBodies() const { return mSoftBodies.size(); } + + /// Access to the soft body settings for this scene + const Array & GetSoftBodies() const { return mSoftBodies; } + Array & GetSoftBodies() { return mSoftBodies; } + + /// Instantiate all bodies, returns false if not all bodies could be created + bool CreateBodies(PhysicsSystem *inSystem) const; + + /// Go through all body creation settings and fix shapes that are scaled incorrectly (note this will change the scene a bit). + /// @return False when not all scales could be fixed. + bool FixInvalidScales(); + + /// Saves the state of this object in binary form to inStream. + /// @param inStream The stream to save the state to + /// @param inSaveShapes If the shapes should be saved as well (these could be shared between physics scenes, in which case the calling application may want to write custom code to restore them) + /// @param inSaveGroupFilter If the group filter should be saved as well (these could be shared) + void SaveBinaryState(StreamOut &inStream, bool inSaveShapes, bool inSaveGroupFilter) const; + + using PhysicsSceneResult = Result>; + + /// Restore a saved scene from inStream + static PhysicsSceneResult sRestoreFromBinaryState(StreamIn &inStream); + + /// For debugging purposes: Construct a scene from the current state of the physics system + void FromPhysicsSystem(const PhysicsSystem *inSystem); + +private: + /// The bodies that are part of this scene + Array mBodies; + + /// Constraints that are part of this scene + Array mConstraints; + + /// Soft bodies that are part of this scene + Array mSoftBodies; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsSettings.h b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSettings.h new file mode 100644 index 000000000000..eb8ecbbeaf81 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSettings.h @@ -0,0 +1,119 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// If objects are closer than this distance, they are considered to be colliding (used for GJK) (unit: meter) +constexpr float cDefaultCollisionTolerance = 1.0e-4f; + +/// A factor that determines the accuracy of the penetration depth calculation. If the change of the squared distance is less than tolerance * current_penetration_depth^2 the algorithm will terminate. (unit: dimensionless) +constexpr float cDefaultPenetrationTolerance = 1.0e-4f; ///< Stop when there's less than 1% change + +/// How much padding to add around objects +constexpr float cDefaultConvexRadius = 0.05f; + +/// Used by (Tapered)CapsuleShape to determine when supporting face is an edge rather than a point (unit: meter) +static constexpr float cCapsuleProjectionSlop = 0.02f; + +/// Maximum amount of jobs to allow +constexpr int cMaxPhysicsJobs = 2048; + +/// Maximum amount of barriers to allow +constexpr int cMaxPhysicsBarriers = 8; + +struct PhysicsSettings +{ + JPH_OVERRIDE_NEW_DELETE + + /// Size of body pairs array, corresponds to the maximum amount of potential body pairs that can be in flight at any time. + /// Setting this to a low value will use less memory but slow down simulation as threads may run out of narrow phase work. + int mMaxInFlightBodyPairs = 16384; + + /// How many PhysicsStepListeners to notify in 1 batch + int mStepListenersBatchSize = 8; + + /// How many step listener batches are needed before spawning another job (set to INT_MAX if no parallelism is desired) + int mStepListenerBatchesPerJob = 1; + + /// Baumgarte stabilization factor (how much of the position error to 'fix' in 1 update) (unit: dimensionless, 0 = nothing, 1 = 100%) + float mBaumgarte = 0.2f; + + /// Radius around objects inside which speculative contact points will be detected. Note that if this is too big + /// you will get ghost collisions as speculative contacts are based on the closest points during the collision detection + /// step which may not be the actual closest points by the time the two objects hit (unit: meters) + float mSpeculativeContactDistance = 0.02f; + + /// How much bodies are allowed to sink into each other (unit: meters) + float mPenetrationSlop = 0.02f; + + /// Fraction of its inner radius a body must move per step to enable casting for the LinearCast motion quality + float mLinearCastThreshold = 0.75f; + + /// Fraction of its inner radius a body may penetrate another body for the LinearCast motion quality + float mLinearCastMaxPenetration = 0.25f; + + /// Max squared distance to use to determine if two points are on the same plane for determining the contact manifold between two shape faces (unit: meter^2) + float mManifoldToleranceSq = 1.0e-6f; + + /// Maximum distance to correct in a single iteration when solving position constraints (unit: meters) + float mMaxPenetrationDistance = 0.2f; + + /// Maximum relative delta position for body pairs to be able to reuse collision results from last frame (units: meter^2) + float mBodyPairCacheMaxDeltaPositionSq = Square(0.001f); ///< 1 mm + + /// Maximum relative delta orientation for body pairs to be able to reuse collision results from last frame, stored as cos(max angle / 2) + float mBodyPairCacheCosMaxDeltaRotationDiv2 = 0.99984769515639123915701155881391f; ///< cos(2 degrees / 2) + + /// Maximum angle between normals that allows manifolds between different sub shapes of the same body pair to be combined + float mContactNormalCosMaxDeltaRotation = 0.99619469809174553229501040247389f; ///< cos(5 degree) + + /// Maximum allowed distance between old and new contact point to preserve contact forces for warm start (units: meter^2) + float mContactPointPreserveLambdaMaxDistSq = Square(0.01f); ///< 1 cm + + /// Number of solver velocity iterations to run + /// Note that this needs to be >= 2 in order for friction to work (friction is applied using the non-penetration impulse from the previous iteration) + uint mNumVelocitySteps = 10; + + /// Number of solver position iterations to run + uint mNumPositionSteps = 2; + + /// Minimal velocity needed before a collision can be elastic (unit: m) + float mMinVelocityForRestitution = 1.0f; + + /// Time before object is allowed to go to sleep (unit: seconds) + float mTimeBeforeSleep = 0.5f; + + /// Velocity of points on bounding box of object below which an object can be considered sleeping (unit: m/s) + float mPointVelocitySleepThreshold = 0.03f; + + /// By default the simulation is deterministic, it is possible to turn this off by setting this setting to false. This will make the simulation run faster but it will no longer be deterministic. + bool mDeterministicSimulation = true; + + ///@name These variables are mainly for debugging purposes, they allow turning on/off certain subsystems. You probably want to leave them alone. + ///@{ + + /// Whether or not to use warm starting for constraints (initially applying previous frames impulses) + bool mConstraintWarmStart = true; + + /// Whether or not to use the body pair cache, which removes the need for narrow phase collision detection when orientation between two bodies didn't change + bool mUseBodyPairContactCache = true; + + /// Whether or not to reduce manifolds with similar contact normals into one contact manifold (see description at Body::SetUseManifoldReduction) + bool mUseManifoldReduction = true; + + /// If we split up large islands into smaller parallel batches of work (to improve performance) + bool mUseLargeIslandSplitter = true; + + /// If objects can go to sleep or not + bool mAllowSleeping = true; + + /// When false, we prevent collision against non-active (shared) edges. Mainly for debugging the algorithm. + bool mCheckActiveEdges = true; + + ///@} +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsStepListener.h b/thirdparty/jolt_physics/Jolt/Physics/PhysicsStepListener.h new file mode 100644 index 000000000000..26c8eec34aa4 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsStepListener.h @@ -0,0 +1,37 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; + +/// Context information for the step listener +class JPH_EXPORT PhysicsStepListenerContext +{ +public: + float mDeltaTime; ///< Delta time of the current step + bool mIsFirstStep; ///< True if this is the first step + bool mIsLastStep; ///< True if this is the last step + PhysicsSystem * mPhysicsSystem; ///< The physics system that is being stepped +}; + +/// A listener class that receives a callback before every physics simulation step +class JPH_EXPORT PhysicsStepListener +{ +public: + /// Ensure virtual destructor + virtual ~PhysicsStepListener() = default; + + /// Called before every simulation step (received inCollisionSteps times for every PhysicsSystem::Update(...) call) + /// This is called while all body and constraint mutexes are locked. You can read/write bodies and constraints but not add/remove them. + /// Multiple listeners can be executed in parallel and it is the responsibility of the listener to avoid race conditions. + /// The best way to do this is to have each step listener operate on a subset of the bodies and constraints + /// and making sure that these bodies and constraints are not touched by any other step listener. + /// Note that this function is not called if there aren't any active bodies or when the physics system is updated with 0 delta time. + virtual void OnStep(const PhysicsStepListenerContext &inContext) = 0; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.cpp b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.cpp new file mode 100644 index 000000000000..3ea6beabfab6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.cpp @@ -0,0 +1,2754 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_DEBUG_RENDERER +bool PhysicsSystem::sDrawMotionQualityLinearCast = false; +#endif // JPH_DEBUG_RENDERER + +//#define BROAD_PHASE BroadPhaseBruteForce +#define BROAD_PHASE BroadPhaseQuadTree + +static const Color cColorUpdateBroadPhaseFinalize = Color::sGetDistinctColor(1); +static const Color cColorUpdateBroadPhasePrepare = Color::sGetDistinctColor(2); +static const Color cColorFindCollisions = Color::sGetDistinctColor(3); +static const Color cColorApplyGravity = Color::sGetDistinctColor(4); +static const Color cColorSetupVelocityConstraints = Color::sGetDistinctColor(5); +static const Color cColorBuildIslandsFromConstraints = Color::sGetDistinctColor(6); +static const Color cColorDetermineActiveConstraints = Color::sGetDistinctColor(7); +static const Color cColorFinalizeIslands = Color::sGetDistinctColor(8); +static const Color cColorContactRemovedCallbacks = Color::sGetDistinctColor(9); +static const Color cColorBodySetIslandIndex = Color::sGetDistinctColor(10); +static const Color cColorStartNextStep = Color::sGetDistinctColor(11); +static const Color cColorSolveVelocityConstraints = Color::sGetDistinctColor(12); +static const Color cColorPreIntegrateVelocity = Color::sGetDistinctColor(13); +static const Color cColorIntegrateVelocity = Color::sGetDistinctColor(14); +static const Color cColorPostIntegrateVelocity = Color::sGetDistinctColor(15); +static const Color cColorResolveCCDContacts = Color::sGetDistinctColor(16); +static const Color cColorSolvePositionConstraints = Color::sGetDistinctColor(17); +static const Color cColorFindCCDContacts = Color::sGetDistinctColor(18); +static const Color cColorStepListeners = Color::sGetDistinctColor(19); +static const Color cColorSoftBodyPrepare = Color::sGetDistinctColor(20); +static const Color cColorSoftBodyCollide = Color::sGetDistinctColor(21); +static const Color cColorSoftBodySimulate = Color::sGetDistinctColor(22); +static const Color cColorSoftBodyFinalize = Color::sGetDistinctColor(23); + +PhysicsSystem::~PhysicsSystem() +{ + // Remove broadphase + delete mBroadPhase; +} + +void PhysicsSystem::Init(uint inMaxBodies, uint inNumBodyMutexes, uint inMaxBodyPairs, uint inMaxContactConstraints, const BroadPhaseLayerInterface &inBroadPhaseLayerInterface, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter) +{ + JPH_ASSERT(inMaxBodies <= BodyID::cMaxBodyIndex, "Cannot support this many bodies"); + + mObjectVsBroadPhaseLayerFilter = &inObjectVsBroadPhaseLayerFilter; + mObjectLayerPairFilter = &inObjectLayerPairFilter; + + // Initialize body manager + mBodyManager.Init(inMaxBodies, inNumBodyMutexes, inBroadPhaseLayerInterface); + + // Create broadphase + mBroadPhase = new BROAD_PHASE(); + mBroadPhase->Init(&mBodyManager, inBroadPhaseLayerInterface); + + // Init contact constraint manager + mContactManager.Init(inMaxBodyPairs, inMaxContactConstraints); + + // Init islands builder + mIslandBuilder.Init(inMaxBodies); + + // Initialize body interface + mBodyInterfaceLocking.Init(mBodyLockInterfaceLocking, mBodyManager, *mBroadPhase); + mBodyInterfaceNoLock.Init(mBodyLockInterfaceNoLock, mBodyManager, *mBroadPhase); + + // Initialize narrow phase query + mNarrowPhaseQueryLocking.Init(mBodyLockInterfaceLocking, *mBroadPhase); + mNarrowPhaseQueryNoLock.Init(mBodyLockInterfaceNoLock, *mBroadPhase); +} + +void PhysicsSystem::OptimizeBroadPhase() +{ + mBroadPhase->Optimize(); +} + +void PhysicsSystem::AddStepListener(PhysicsStepListener *inListener) +{ + lock_guard lock(mStepListenersMutex); + + JPH_ASSERT(std::find(mStepListeners.begin(), mStepListeners.end(), inListener) == mStepListeners.end()); + mStepListeners.push_back(inListener); +} + +void PhysicsSystem::RemoveStepListener(PhysicsStepListener *inListener) +{ + lock_guard lock(mStepListenersMutex); + + StepListeners::iterator i = std::find(mStepListeners.begin(), mStepListeners.end(), inListener); + JPH_ASSERT(i != mStepListeners.end()); + *i = mStepListeners.back(); + mStepListeners.pop_back(); +} + +EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionSteps, TempAllocator *inTempAllocator, JobSystem *inJobSystem) +{ + JPH_PROFILE_FUNCTION(); + + JPH_DET_LOG("PhysicsSystem::Update: dt: " << inDeltaTime << " steps: " << inCollisionSteps); + + JPH_ASSERT(inCollisionSteps > 0); + JPH_ASSERT(inDeltaTime >= 0.0f); + + // Sync point for the broadphase. This will allow it to do clean up operations without having any mutexes locked yet. + mBroadPhase->FrameSync(); + + // If there are no active bodies or there's no time delta + uint32 num_active_rigid_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody); + uint32 num_active_soft_bodies = mBodyManager.GetNumActiveBodies(EBodyType::SoftBody); + if ((num_active_rigid_bodies == 0 && num_active_soft_bodies == 0) || inDeltaTime <= 0.0f) + { + mBodyManager.LockAllBodies(); + + // Update broadphase + mBroadPhase->LockModifications(); + BroadPhase::UpdateState update_state = mBroadPhase->UpdatePrepare(); + mBroadPhase->UpdateFinalize(update_state); + mBroadPhase->UnlockModifications(); + + // Call contact removal callbacks from contacts that existed in the previous update + mContactManager.FinalizeContactCacheAndCallContactPointRemovedCallbacks(0, 0); + + mBodyManager.UnlockAllBodies(); + return EPhysicsUpdateError::None; + } + + // Calculate ratio between current and previous frame delta time to scale initial constraint forces + float step_delta_time = inDeltaTime / inCollisionSteps; + float warm_start_impulse_ratio = mPhysicsSettings.mConstraintWarmStart && mPreviousStepDeltaTime > 0.0f? step_delta_time / mPreviousStepDeltaTime : 0.0f; + mPreviousStepDeltaTime = step_delta_time; + + // Create the context used for passing information between jobs + PhysicsUpdateContext context(*inTempAllocator); + context.mPhysicsSystem = this; + context.mJobSystem = inJobSystem; + context.mBarrier = inJobSystem->CreateBarrier(); + context.mIslandBuilder = &mIslandBuilder; + context.mStepDeltaTime = step_delta_time; + context.mWarmStartImpulseRatio = warm_start_impulse_ratio; + context.mSteps.resize(inCollisionSteps); + + // Allocate space for body pairs + JPH_ASSERT(context.mBodyPairs == nullptr); + context.mBodyPairs = static_cast(inTempAllocator->Allocate(sizeof(BodyPair) * mPhysicsSettings.mMaxInFlightBodyPairs)); + + // Lock all bodies for write so that we can freely touch them + mStepListenersMutex.lock(); + mBodyManager.LockAllBodies(); + mBroadPhase->LockModifications(); + + // Get max number of concurrent jobs + int max_concurrency = context.GetMaxConcurrency(); + + // Calculate how many step listener jobs we spawn + int num_step_listener_jobs = mStepListeners.empty()? 0 : max(1, min((int)mStepListeners.size() / mPhysicsSettings.mStepListenersBatchSize / mPhysicsSettings.mStepListenerBatchesPerJob, max_concurrency)); + + // Number of gravity jobs depends on the amount of active bodies. + // Launch max 1 job per batch of active bodies + // Leave 1 thread for update broadphase prepare and 1 for determine active constraints + int num_apply_gravity_jobs = max(1, min(((int)num_active_rigid_bodies + cApplyGravityBatchSize - 1) / cApplyGravityBatchSize, max_concurrency - 2)); + + // Number of determine active constraints jobs to run depends on number of constraints. + // Leave 1 thread for update broadphase prepare and 1 for apply gravity + int num_determine_active_constraints_jobs = max(1, min(((int)mConstraintManager.GetNumConstraints() + cDetermineActiveConstraintsBatchSize - 1) / cDetermineActiveConstraintsBatchSize, max_concurrency - 2)); + + // Number of setup velocity constraints jobs to run depends on number of constraints. + int num_setup_velocity_constraints_jobs = max(1, min(((int)mConstraintManager.GetNumConstraints() + cSetupVelocityConstraintsBatchSize - 1) / cSetupVelocityConstraintsBatchSize, max_concurrency)); + + // Number of find collisions jobs to run depends on number of active bodies. + // Note that when we have more than 1 thread, we always spawn at least 2 find collisions jobs so that the first job can wait for build islands from constraints + // (which may activate additional bodies that need to be processed) while the second job can start processing collision work. + int num_find_collisions_jobs = max(max_concurrency == 1? 1 : 2, min(((int)num_active_rigid_bodies + cActiveBodiesBatchSize - 1) / cActiveBodiesBatchSize, max_concurrency)); + + // Number of integrate velocity jobs depends on number of active bodies. + int num_integrate_velocity_jobs = max(1, min(((int)num_active_rigid_bodies + cIntegrateVelocityBatchSize - 1) / cIntegrateVelocityBatchSize, max_concurrency)); + + { + JPH_PROFILE("Build Jobs"); + + // Iterate over collision steps + for (int step_idx = 0; step_idx < inCollisionSteps; ++step_idx) + { + bool is_first_step = step_idx == 0; + bool is_last_step = step_idx == inCollisionSteps - 1; + + PhysicsUpdateContext::Step &step = context.mSteps[step_idx]; + step.mContext = &context; + step.mIsFirst = is_first_step; + step.mIsLast = is_last_step; + + // Create job to do broadphase finalization + // This job must finish before integrating velocities. Until then the positions will not be updated neither will bodies be added / removed. + step.mUpdateBroadphaseFinalize = inJobSystem->CreateJob("UpdateBroadPhaseFinalize", cColorUpdateBroadPhaseFinalize, [&context, &step]() + { + // Validate that all find collision jobs have stopped + JPH_ASSERT(step.mActiveFindCollisionJobs.load(memory_order_relaxed) == 0); + + // Finalize the broadphase update + context.mPhysicsSystem->mBroadPhase->UpdateFinalize(step.mBroadPhaseUpdateState); + + // Signal that it is done + step.mPreIntegrateVelocity.RemoveDependency(); + }, num_find_collisions_jobs + 2); // depends on: find collisions, broadphase prepare update, finish building jobs + + // The immediate jobs below are only immediate for the first step, the all finished job will kick them for the next step + int previous_step_dependency_count = is_first_step? 0 : 1; + + // Start job immediately: Start the prepare broadphase + // Must be done under body lock protection since the order is body locks then broadphase mutex + // If this is turned around the RemoveBody call will hang since it locks in that order + step.mBroadPhasePrepare = inJobSystem->CreateJob("UpdateBroadPhasePrepare", cColorUpdateBroadPhasePrepare, [&context, &step]() + { + // Prepare the broadphase update + step.mBroadPhaseUpdateState = context.mPhysicsSystem->mBroadPhase->UpdatePrepare(); + + // Now the finalize can run (if other dependencies are met too) + step.mUpdateBroadphaseFinalize.RemoveDependency(); + }, previous_step_dependency_count); + + // This job will find all collisions + step.mBodyPairQueues.resize(max_concurrency); + step.mMaxBodyPairsPerQueue = mPhysicsSettings.mMaxInFlightBodyPairs / max_concurrency; + step.mActiveFindCollisionJobs.store(~PhysicsUpdateContext::JobMask(0) >> (sizeof(PhysicsUpdateContext::JobMask) * 8 - num_find_collisions_jobs), memory_order_release); + step.mFindCollisions.resize(num_find_collisions_jobs); + for (int i = 0; i < num_find_collisions_jobs; ++i) + { + // Build islands from constraints may activate additional bodies, so the first job will wait for this to finish in order to not miss any active bodies + int num_dep_build_islands_from_constraints = i == 0? 1 : 0; + step.mFindCollisions[i] = inJobSystem->CreateJob("FindCollisions", cColorFindCollisions, [&step, i]() + { + step.mContext->mPhysicsSystem->JobFindCollisions(&step, i); + }, num_apply_gravity_jobs + num_determine_active_constraints_jobs + 1 + num_dep_build_islands_from_constraints); // depends on: apply gravity, determine active constraints, finish building jobs, build islands from constraints + } + + if (is_first_step) + { + #ifdef JPH_ENABLE_ASSERTS + // Don't allow write operations to the active bodies list + mBodyManager.SetActiveBodiesLocked(true); + #endif + + // Store the number of active bodies at the start of the step + step.mNumActiveBodiesAtStepStart = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody); + + // Lock all constraints + mConstraintManager.LockAllConstraints(); + + // Allocate memory for storing the active constraints + JPH_ASSERT(context.mActiveConstraints == nullptr); + context.mActiveConstraints = static_cast(inTempAllocator->Allocate(mConstraintManager.GetNumConstraints() * sizeof(Constraint *))); + + // Prepare contact buffer + mContactManager.PrepareConstraintBuffer(&context); + + // Setup island builder + mIslandBuilder.PrepareContactConstraints(mContactManager.GetMaxConstraints(), context.mTempAllocator); + } + + // This job applies gravity to all active bodies + step.mApplyGravity.resize(num_apply_gravity_jobs); + for (int i = 0; i < num_apply_gravity_jobs; ++i) + step.mApplyGravity[i] = inJobSystem->CreateJob("ApplyGravity", cColorApplyGravity, [&context, &step]() + { + context.mPhysicsSystem->JobApplyGravity(&context, &step); + + JobHandle::sRemoveDependencies(step.mFindCollisions); + }, num_step_listener_jobs > 0? num_step_listener_jobs : previous_step_dependency_count); // depends on: step listeners (or previous step if no step listeners) + + // This job will setup velocity constraints for non-collision constraints + step.mSetupVelocityConstraints.resize(num_setup_velocity_constraints_jobs); + for (int i = 0; i < num_setup_velocity_constraints_jobs; ++i) + step.mSetupVelocityConstraints[i] = inJobSystem->CreateJob("SetupVelocityConstraints", cColorSetupVelocityConstraints, [&context, &step]() + { + context.mPhysicsSystem->JobSetupVelocityConstraints(context.mStepDeltaTime, &step); + + JobHandle::sRemoveDependencies(step.mSolveVelocityConstraints); + }, num_determine_active_constraints_jobs + 1); // depends on: determine active constraints, finish building jobs + + // This job will build islands from constraints + step.mBuildIslandsFromConstraints = inJobSystem->CreateJob("BuildIslandsFromConstraints", cColorBuildIslandsFromConstraints, [&context, &step]() + { + context.mPhysicsSystem->JobBuildIslandsFromConstraints(&context, &step); + + step.mFindCollisions[0].RemoveDependency(); // The first collisions job cannot start running until we've finished building islands and activated all bodies + step.mFinalizeIslands.RemoveDependency(); + }, num_determine_active_constraints_jobs + 1); // depends on: determine active constraints, finish building jobs + + // This job determines active constraints + step.mDetermineActiveConstraints.resize(num_determine_active_constraints_jobs); + for (int i = 0; i < num_determine_active_constraints_jobs; ++i) + step.mDetermineActiveConstraints[i] = inJobSystem->CreateJob("DetermineActiveConstraints", cColorDetermineActiveConstraints, [&context, &step]() + { + context.mPhysicsSystem->JobDetermineActiveConstraints(&step); + + step.mBuildIslandsFromConstraints.RemoveDependency(); + + // Kick these jobs last as they will use up all CPU cores leaving no space for the previous job, we prefer setup velocity constraints to finish first so we kick it first + JobHandle::sRemoveDependencies(step.mSetupVelocityConstraints); + JobHandle::sRemoveDependencies(step.mFindCollisions); + }, num_step_listener_jobs > 0? num_step_listener_jobs : previous_step_dependency_count); // depends on: step listeners (or previous step if no step listeners) + + // This job calls the step listeners + step.mStepListeners.resize(num_step_listener_jobs); + for (int i = 0; i < num_step_listener_jobs; ++i) + step.mStepListeners[i] = inJobSystem->CreateJob("StepListeners", cColorStepListeners, [&context, &step]() + { + // Call the step listeners + context.mPhysicsSystem->JobStepListeners(&step); + + // Kick apply gravity and determine active constraint jobs + JobHandle::sRemoveDependencies(step.mApplyGravity); + JobHandle::sRemoveDependencies(step.mDetermineActiveConstraints); + }, previous_step_dependency_count); + + // Unblock the previous step + if (!is_first_step) + context.mSteps[step_idx - 1].mStartNextStep.RemoveDependency(); + + // This job will finalize the simulation islands + step.mFinalizeIslands = inJobSystem->CreateJob("FinalizeIslands", cColorFinalizeIslands, [&context, &step]() + { + // Validate that all find collision jobs have stopped + JPH_ASSERT(step.mActiveFindCollisionJobs.load(memory_order_relaxed) == 0); + + context.mPhysicsSystem->JobFinalizeIslands(&context); + + JobHandle::sRemoveDependencies(step.mSolveVelocityConstraints); + step.mBodySetIslandIndex.RemoveDependency(); + }, num_find_collisions_jobs + 2); // depends on: find collisions, build islands from constraints, finish building jobs + + // Unblock previous job + // Note: technically we could release find collisions here but we don't want to because that could make them run before 'setup velocity constraints' which means that job won't have a thread left + step.mBuildIslandsFromConstraints.RemoveDependency(); + + // This job will call the contact removed callbacks + step.mContactRemovedCallbacks = inJobSystem->CreateJob("ContactRemovedCallbacks", cColorContactRemovedCallbacks, [&context, &step]() + { + context.mPhysicsSystem->JobContactRemovedCallbacks(&step); + + if (step.mStartNextStep.IsValid()) + step.mStartNextStep.RemoveDependency(); + }, 1); // depends on the find ccd contacts + + // This job will set the island index on each body (only used for debug drawing purposes) + // It will also delete any bodies that have been destroyed in the last frame + step.mBodySetIslandIndex = inJobSystem->CreateJob("BodySetIslandIndex", cColorBodySetIslandIndex, [&context, &step]() + { + context.mPhysicsSystem->JobBodySetIslandIndex(); + + JobHandle::sRemoveDependencies(step.mSolvePositionConstraints); + }, 2); // depends on: finalize islands, finish building jobs + + // Job to start the next collision step + if (!is_last_step) + { + PhysicsUpdateContext::Step *next_step = &context.mSteps[step_idx + 1]; + step.mStartNextStep = inJobSystem->CreateJob("StartNextStep", cColorStartNextStep, [this, next_step]() + { + #ifdef JPH_DEBUG + // Validate that the cached bounds are correct + mBodyManager.ValidateActiveBodyBounds(); + #endif // JPH_DEBUG + + // Store the number of active bodies at the start of the step + next_step->mNumActiveBodiesAtStepStart = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody); + + // Clear the large island splitter + TempAllocator *temp_allocator = next_step->mContext->mTempAllocator; + mLargeIslandSplitter.Reset(temp_allocator); + + // Clear the island builder + mIslandBuilder.ResetIslands(temp_allocator); + + // Setup island builder + mIslandBuilder.PrepareContactConstraints(mContactManager.GetMaxConstraints(), temp_allocator); + + // Restart the contact manager + mContactManager.RecycleConstraintBuffer(); + + // Kick the jobs of the next step (in the same order as the first step) + next_step->mBroadPhasePrepare.RemoveDependency(); + if (next_step->mStepListeners.empty()) + { + // Kick the gravity and active constraints jobs immediately + JobHandle::sRemoveDependencies(next_step->mApplyGravity); + JobHandle::sRemoveDependencies(next_step->mDetermineActiveConstraints); + } + else + { + // Kick the step listeners job first + JobHandle::sRemoveDependencies(next_step->mStepListeners); + } + }, 3); // depends on: update soft bodies, contact removed callbacks, finish building the previous step + } + + // This job will solve the velocity constraints + step.mSolveVelocityConstraints.resize(max_concurrency); + for (int i = 0; i < max_concurrency; ++i) + step.mSolveVelocityConstraints[i] = inJobSystem->CreateJob("SolveVelocityConstraints", cColorSolveVelocityConstraints, [&context, &step]() + { + context.mPhysicsSystem->JobSolveVelocityConstraints(&context, &step); + + step.mPreIntegrateVelocity.RemoveDependency(); + }, num_setup_velocity_constraints_jobs + 2); // depends on: finalize islands, setup velocity constraints, finish building jobs. + + // We prefer setup velocity constraints to finish first so we kick it first + JobHandle::sRemoveDependencies(step.mSetupVelocityConstraints); + JobHandle::sRemoveDependencies(step.mFindCollisions); + + // Finalize islands is a dependency on find collisions so it can go last + step.mFinalizeIslands.RemoveDependency(); + + // This job will prepare the position update of all active bodies + step.mPreIntegrateVelocity = inJobSystem->CreateJob("PreIntegrateVelocity", cColorPreIntegrateVelocity, [&context, &step]() + { + context.mPhysicsSystem->JobPreIntegrateVelocity(&context, &step); + + JobHandle::sRemoveDependencies(step.mIntegrateVelocity); + }, 2 + max_concurrency); // depends on: broadphase update finalize, solve velocity constraints, finish building jobs. + + // Unblock previous jobs + step.mUpdateBroadphaseFinalize.RemoveDependency(); + JobHandle::sRemoveDependencies(step.mSolveVelocityConstraints); + + // This job will update the positions of all active bodies + step.mIntegrateVelocity.resize(num_integrate_velocity_jobs); + for (int i = 0; i < num_integrate_velocity_jobs; ++i) + step.mIntegrateVelocity[i] = inJobSystem->CreateJob("IntegrateVelocity", cColorIntegrateVelocity, [&context, &step]() + { + context.mPhysicsSystem->JobIntegrateVelocity(&context, &step); + + step.mPostIntegrateVelocity.RemoveDependency(); + }, 2); // depends on: pre integrate velocity, finish building jobs. + + // Unblock previous job + step.mPreIntegrateVelocity.RemoveDependency(); + + // This job will finish the position update of all active bodies + step.mPostIntegrateVelocity = inJobSystem->CreateJob("PostIntegrateVelocity", cColorPostIntegrateVelocity, [&context, &step]() + { + context.mPhysicsSystem->JobPostIntegrateVelocity(&context, &step); + + step.mResolveCCDContacts.RemoveDependency(); + }, num_integrate_velocity_jobs + 1); // depends on: integrate velocity, finish building jobs + + // Unblock previous jobs + JobHandle::sRemoveDependencies(step.mIntegrateVelocity); + + // This job will update the positions and velocities for all bodies that need continuous collision detection + step.mResolveCCDContacts = inJobSystem->CreateJob("ResolveCCDContacts", cColorResolveCCDContacts, [&context, &step]() + { + context.mPhysicsSystem->JobResolveCCDContacts(&context, &step); + + JobHandle::sRemoveDependencies(step.mSolvePositionConstraints); + }, 2); // depends on: integrate velocities, detect ccd contacts (added dynamically), finish building jobs. + + // Unblock previous job + step.mPostIntegrateVelocity.RemoveDependency(); + + // Fixes up drift in positions and updates the broadphase with new body positions + step.mSolvePositionConstraints.resize(max_concurrency); + for (int i = 0; i < max_concurrency; ++i) + step.mSolvePositionConstraints[i] = inJobSystem->CreateJob("SolvePositionConstraints", cColorSolvePositionConstraints, [&context, &step]() + { + context.mPhysicsSystem->JobSolvePositionConstraints(&context, &step); + + // Kick the next step + if (step.mSoftBodyPrepare.IsValid()) + step.mSoftBodyPrepare.RemoveDependency(); + }, 3); // depends on: resolve ccd contacts, body set island index, finish building jobs. + + // Unblock previous jobs. + step.mResolveCCDContacts.RemoveDependency(); + step.mBodySetIslandIndex.RemoveDependency(); + + // The soft body prepare job will create other jobs if needed + step.mSoftBodyPrepare = inJobSystem->CreateJob("SoftBodyPrepare", cColorSoftBodyPrepare, [&context, &step]() + { + context.mPhysicsSystem->JobSoftBodyPrepare(&context, &step); + }, max_concurrency); // depends on: solve position constraints. + + // Unblock previous jobs + JobHandle::sRemoveDependencies(step.mSolvePositionConstraints); + } + } + + // Build the list of jobs to wait for + JobSystem::Barrier *barrier = context.mBarrier; + { + JPH_PROFILE("Build job barrier"); + + StaticArray handles; + for (const PhysicsUpdateContext::Step &step : context.mSteps) + { + if (step.mBroadPhasePrepare.IsValid()) + handles.push_back(step.mBroadPhasePrepare); + for (const JobHandle &h : step.mStepListeners) + handles.push_back(h); + for (const JobHandle &h : step.mDetermineActiveConstraints) + handles.push_back(h); + for (const JobHandle &h : step.mApplyGravity) + handles.push_back(h); + for (const JobHandle &h : step.mFindCollisions) + handles.push_back(h); + if (step.mUpdateBroadphaseFinalize.IsValid()) + handles.push_back(step.mUpdateBroadphaseFinalize); + for (const JobHandle &h : step.mSetupVelocityConstraints) + handles.push_back(h); + handles.push_back(step.mBuildIslandsFromConstraints); + handles.push_back(step.mFinalizeIslands); + handles.push_back(step.mBodySetIslandIndex); + for (const JobHandle &h : step.mSolveVelocityConstraints) + handles.push_back(h); + handles.push_back(step.mPreIntegrateVelocity); + for (const JobHandle &h : step.mIntegrateVelocity) + handles.push_back(h); + handles.push_back(step.mPostIntegrateVelocity); + handles.push_back(step.mResolveCCDContacts); + for (const JobHandle &h : step.mSolvePositionConstraints) + handles.push_back(h); + handles.push_back(step.mContactRemovedCallbacks); + if (step.mSoftBodyPrepare.IsValid()) + handles.push_back(step.mSoftBodyPrepare); + if (step.mStartNextStep.IsValid()) + handles.push_back(step.mStartNextStep); + } + barrier->AddJobs(handles.data(), handles.size()); + } + + // Wait until all jobs finish + // Note we don't just wait for the last job. If we would and another job + // would be scheduled in between there is the possibility of a deadlock. + // The other job could try to e.g. add/remove a body which would try to + // lock a body mutex while this thread has already locked the mutex + inJobSystem->WaitForJobs(barrier); + + // We're done with the barrier for this update + inJobSystem->DestroyBarrier(barrier); + +#ifdef JPH_DEBUG + // Validate that the cached bounds are correct + mBodyManager.ValidateActiveBodyBounds(); +#endif // JPH_DEBUG + + // Clear the large island splitter + mLargeIslandSplitter.Reset(inTempAllocator); + + // Clear the island builder + mIslandBuilder.ResetIslands(inTempAllocator); + + // Clear the contact manager + mContactManager.FinishConstraintBuffer(); + + // Free active constraints + inTempAllocator->Free(context.mActiveConstraints, mConstraintManager.GetNumConstraints() * sizeof(Constraint *)); + context.mActiveConstraints = nullptr; + + // Free body pairs + inTempAllocator->Free(context.mBodyPairs, sizeof(BodyPair) * mPhysicsSettings.mMaxInFlightBodyPairs); + context.mBodyPairs = nullptr; + + // Unlock the broadphase + mBroadPhase->UnlockModifications(); + + // Unlock all constraints + mConstraintManager.UnlockAllConstraints(); + +#ifdef JPH_ENABLE_ASSERTS + // Allow write operations to the active bodies list + mBodyManager.SetActiveBodiesLocked(false); +#endif + + // Unlock all bodies + mBodyManager.UnlockAllBodies(); + + // Unlock step listeners + mStepListenersMutex.unlock(); + + // Return any errors + EPhysicsUpdateError errors = static_cast(context.mErrors.load(memory_order_acquire)); + JPH_ASSERT(errors == EPhysicsUpdateError::None, "An error occurred during the physics update, see EPhysicsUpdateError for more information"); + return errors; +} + +void PhysicsSystem::JobStepListeners(PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // Read positions (broadphase updates concurrently so we can't write), read/write velocities + BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::Read); + + // Can activate bodies only (we cache the amount of active bodies at the beginning of the step in mNumActiveBodiesAtStepStart so we cannot deactivate here) + BodyManager::GrantActiveBodiesAccess grant_active(true, false); +#endif + + PhysicsStepListenerContext context; + context.mDeltaTime = ioStep->mContext->mStepDeltaTime; + context.mIsFirstStep = ioStep->mIsFirst; + context.mIsLastStep = ioStep->mIsLast; + context.mPhysicsSystem = this; + + uint32 batch_size = mPhysicsSettings.mStepListenersBatchSize; + for (;;) + { + // Get the start of a new batch + uint32 batch = ioStep->mStepListenerReadIdx.fetch_add(batch_size); + if (batch >= mStepListeners.size()) + break; + + // Call the listeners + for (uint32 i = batch, i_end = min((uint32)mStepListeners.size(), batch + batch_size); i < i_end; ++i) + mStepListeners[i]->OnStep(context); + } +} + +void PhysicsSystem::JobDetermineActiveConstraints(PhysicsUpdateContext::Step *ioStep) const +{ +#ifdef JPH_ENABLE_ASSERTS + // No body access + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::None); +#endif + + uint32 num_constraints = mConstraintManager.GetNumConstraints(); + uint32 num_active_constraints; + Constraint **active_constraints = (Constraint **)JPH_STACK_ALLOC(cDetermineActiveConstraintsBatchSize * sizeof(Constraint *)); + + for (;;) + { + // Atomically fetch a batch of constraints + uint32 constraint_idx = ioStep->mDetermineActiveConstraintReadIdx.fetch_add(cDetermineActiveConstraintsBatchSize); + if (constraint_idx >= num_constraints) + break; + + // Calculate the end of the batch + uint32 constraint_idx_end = min(num_constraints, constraint_idx + cDetermineActiveConstraintsBatchSize); + + // Store the active constraints at the start of the step (bodies get activated during the step which in turn may activate constraints leading to an inconsistent shapshot) + mConstraintManager.GetActiveConstraints(constraint_idx, constraint_idx_end, active_constraints, num_active_constraints); + + // Copy the block of active constraints to the global list of active constraints + if (num_active_constraints > 0) + { + uint32 active_constraint_idx = ioStep->mNumActiveConstraints.fetch_add(num_active_constraints); + memcpy(ioStep->mContext->mActiveConstraints + active_constraint_idx, active_constraints, num_active_constraints * sizeof(Constraint *)); + } + } +} + +void PhysicsSystem::JobApplyGravity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // We update velocities and need the rotation to do so + BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::Read); +#endif + + // Get list of active bodies that we had at the start of the physics update. + // Any body that is activated as part of the simulation step does not receive gravity this frame. + // Note that bodies may be activated during this job but not deactivated, this means that only elements + // will be added to the array. Since the array is made to not reallocate, this is a safe operation. + const BodyID *active_bodies = mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody); + uint32 num_active_bodies_at_step_start = ioStep->mNumActiveBodiesAtStepStart; + + // Fetch delta time once outside the loop + float delta_time = ioContext->mStepDeltaTime; + + // Update velocities from forces + for (;;) + { + // Atomically fetch a batch of bodies + uint32 active_body_idx = ioStep->mApplyGravityReadIdx.fetch_add(cApplyGravityBatchSize); + if (active_body_idx >= num_active_bodies_at_step_start) + break; + + // Calculate the end of the batch + uint32 active_body_idx_end = min(num_active_bodies_at_step_start, active_body_idx + cApplyGravityBatchSize); + + // Process the batch + while (active_body_idx < active_body_idx_end) + { + Body &body = mBodyManager.GetBody(active_bodies[active_body_idx]); + if (body.IsDynamic()) + { + MotionProperties *mp = body.GetMotionProperties(); + Quat rotation = body.GetRotation(); + + if (body.GetApplyGyroscopicForce()) + mp->ApplyGyroscopicForceInternal(rotation, delta_time); + + mp->ApplyForceTorqueAndDragInternal(rotation, mGravity, delta_time); + } + active_body_idx++; + } + } +} + +void PhysicsSystem::JobSetupVelocityConstraints(float inDeltaTime, PhysicsUpdateContext::Step *ioStep) const +{ +#ifdef JPH_ENABLE_ASSERTS + // We only read positions + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::Read); +#endif + + uint32 num_constraints = ioStep->mNumActiveConstraints; + + for (;;) + { + // Atomically fetch a batch of constraints + uint32 constraint_idx = ioStep->mSetupVelocityConstraintsReadIdx.fetch_add(cSetupVelocityConstraintsBatchSize); + if (constraint_idx >= num_constraints) + break; + + ConstraintManager::sSetupVelocityConstraints(ioStep->mContext->mActiveConstraints + constraint_idx, min(cSetupVelocityConstraintsBatchSize, num_constraints - constraint_idx), inDeltaTime); + } +} + +void PhysicsSystem::JobBuildIslandsFromConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // We read constraints and positions + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::Read); + + // Can only activate bodies + BodyManager::GrantActiveBodiesAccess grant_active(true, false); +#endif + + // Prepare the island builder + mIslandBuilder.PrepareNonContactConstraints(ioStep->mNumActiveConstraints, ioContext->mTempAllocator); + + // Build the islands + ConstraintManager::sBuildIslands(ioStep->mContext->mActiveConstraints, ioStep->mNumActiveConstraints, mIslandBuilder, mBodyManager); +} + +void PhysicsSystem::TrySpawnJobFindCollisions(PhysicsUpdateContext::Step *ioStep) const +{ + // Get how many jobs we can spawn and check if we can spawn more + uint max_jobs = ioStep->mBodyPairQueues.size(); + if (CountBits(ioStep->mActiveFindCollisionJobs.load(memory_order_relaxed)) >= max_jobs) + return; + + // Count how many body pairs we have waiting + uint32 num_body_pairs = 0; + for (const PhysicsUpdateContext::BodyPairQueue &queue : ioStep->mBodyPairQueues) + num_body_pairs += queue.mWriteIdx - queue.mReadIdx; + + // Count how many active bodies we have waiting + uint32 num_active_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody) - ioStep->mActiveBodyReadIdx; + + // Calculate how many jobs we would like + uint desired_num_jobs = min((num_body_pairs + cNarrowPhaseBatchSize - 1) / cNarrowPhaseBatchSize + (num_active_bodies + cActiveBodiesBatchSize - 1) / cActiveBodiesBatchSize, max_jobs); + + for (;;) + { + // Get the bit mask of active jobs and see if we can spawn more + PhysicsUpdateContext::JobMask current_active_jobs = ioStep->mActiveFindCollisionJobs.load(memory_order_relaxed); + uint job_index = CountTrailingZeros(~current_active_jobs); + if (job_index >= desired_num_jobs) + break; + + // Try to claim the job index + PhysicsUpdateContext::JobMask job_mask = PhysicsUpdateContext::JobMask(1) << job_index; + PhysicsUpdateContext::JobMask prev_value = ioStep->mActiveFindCollisionJobs.fetch_or(job_mask, memory_order_acquire); + if ((prev_value & job_mask) == 0) + { + // Add dependencies from the find collisions job to the next jobs + ioStep->mUpdateBroadphaseFinalize.AddDependency(); + ioStep->mFinalizeIslands.AddDependency(); + + // Start the job + JobHandle job = ioStep->mContext->mJobSystem->CreateJob("FindCollisions", cColorFindCollisions, [step = ioStep, job_index]() + { + step->mContext->mPhysicsSystem->JobFindCollisions(step, job_index); + }); + + // Add the job to the job barrier so the main updating thread can execute the job too + ioStep->mContext->mBarrier->AddJob(job); + + // Spawn only 1 extra job at a time + return; + } + } +} + +static void sFinalizeContactAllocator(PhysicsUpdateContext::Step &ioStep, const ContactConstraintManager::ContactAllocator &inAllocator) +{ + // Atomically accumulate the number of found manifolds and body pairs + ioStep.mNumBodyPairs.fetch_add(inAllocator.mNumBodyPairs, memory_order_relaxed); + ioStep.mNumManifolds.fetch_add(inAllocator.mNumManifolds, memory_order_relaxed); + + // Combine update errors + ioStep.mContext->mErrors.fetch_or((uint32)inAllocator.mErrors, memory_order_relaxed); +} + +// Disable TSAN for this function. It detects a false positive race condition on mBodyPairs. +// We have written mBodyPairs before doing mWriteIdx++ and we check mWriteIdx before reading mBodyPairs, so this should be safe. +JPH_TSAN_NO_SANITIZE +void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int inJobIndex) +{ +#ifdef JPH_ENABLE_ASSERTS + // We read positions and read velocities (for elastic collisions) + BodyAccess::Grant grant(BodyAccess::EAccess::Read, BodyAccess::EAccess::Read); + + // Can only activate bodies + BodyManager::GrantActiveBodiesAccess grant_active(true, false); +#endif + + // Allocation context for allocating new contact points + ContactAllocator contact_allocator(mContactManager.GetContactAllocator()); + + // Determine initial queue to read pairs from if no broadphase work can be done + // (always start looking at results from the next job) + int read_queue_idx = (inJobIndex + 1) % ioStep->mBodyPairQueues.size(); + + for (;;) + { + // Check if there are active bodies to be processed + uint32 active_bodies_read_idx = ioStep->mActiveBodyReadIdx; + uint32 num_active_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody); + if (active_bodies_read_idx < num_active_bodies) + { + // Take a batch of active bodies + uint32 active_bodies_read_idx_end = min(num_active_bodies, active_bodies_read_idx + cActiveBodiesBatchSize); + if (ioStep->mActiveBodyReadIdx.compare_exchange_strong(active_bodies_read_idx, active_bodies_read_idx_end)) + { + // Callback when a new body pair is found + class MyBodyPairCallback : public BodyPairCollector + { + public: + // Constructor + MyBodyPairCallback(PhysicsUpdateContext::Step *inStep, ContactAllocator &ioContactAllocator, int inJobIndex) : + mStep(inStep), + mContactAllocator(ioContactAllocator), + mJobIndex(inJobIndex) + { + } + + // Callback function when a body pair is found + virtual void AddHit(const BodyPair &inPair) override + { + // Check if we have space in our write queue + PhysicsUpdateContext::BodyPairQueue &queue = mStep->mBodyPairQueues[mJobIndex]; + uint32 body_pairs_in_queue = queue.mWriteIdx - queue.mReadIdx; + if (body_pairs_in_queue >= mStep->mMaxBodyPairsPerQueue) + { + // Buffer full, process the pair now + mStep->mContext->mPhysicsSystem->ProcessBodyPair(mContactAllocator, inPair); + } + else + { + // Store the pair in our own queue + mStep->mContext->mBodyPairs[mJobIndex * mStep->mMaxBodyPairsPerQueue + queue.mWriteIdx % mStep->mMaxBodyPairsPerQueue] = inPair; + ++queue.mWriteIdx; + } + } + + private: + PhysicsUpdateContext::Step * mStep; + ContactAllocator & mContactAllocator; + int mJobIndex; + }; + MyBodyPairCallback add_pair(ioStep, contact_allocator, inJobIndex); + + // Copy active bodies to temporary array, broadphase will reorder them + uint32 batch_size = active_bodies_read_idx_end - active_bodies_read_idx; + BodyID *active_bodies = (BodyID *)JPH_STACK_ALLOC(batch_size * sizeof(BodyID)); + memcpy(active_bodies, mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody) + active_bodies_read_idx, batch_size * sizeof(BodyID)); + + // Find pairs in the broadphase + mBroadPhase->FindCollidingPairs(active_bodies, batch_size, mPhysicsSettings.mSpeculativeContactDistance, *mObjectVsBroadPhaseLayerFilter, *mObjectLayerPairFilter, add_pair); + + // Check if we have enough pairs in the buffer to start a new job + const PhysicsUpdateContext::BodyPairQueue &queue = ioStep->mBodyPairQueues[inJobIndex]; + uint32 body_pairs_in_queue = queue.mWriteIdx - queue.mReadIdx; + if (body_pairs_in_queue >= cNarrowPhaseBatchSize) + TrySpawnJobFindCollisions(ioStep); + } + } + else + { + // Lockless loop to get the next body pair from the pairs buffer + const PhysicsUpdateContext *context = ioStep->mContext; + int first_read_queue_idx = read_queue_idx; + for (;;) + { + PhysicsUpdateContext::BodyPairQueue &queue = ioStep->mBodyPairQueues[read_queue_idx]; + + // Get the next pair to process + uint32 pair_idx = queue.mReadIdx; + + // If the pair hasn't been written yet + if (pair_idx >= queue.mWriteIdx) + { + // Go to the next queue + read_queue_idx = (read_queue_idx + 1) % ioStep->mBodyPairQueues.size(); + + // If we're back at the first queue, we've looked at all of them and found nothing + if (read_queue_idx == first_read_queue_idx) + { + // Collect information from the contact allocator and accumulate it in the step. + sFinalizeContactAllocator(*ioStep, contact_allocator); + + // Mark this job as inactive + ioStep->mActiveFindCollisionJobs.fetch_and(~PhysicsUpdateContext::JobMask(1 << inJobIndex), memory_order_release); + + // Trigger the next jobs + ioStep->mUpdateBroadphaseFinalize.RemoveDependency(); + ioStep->mFinalizeIslands.RemoveDependency(); + return; + } + + // Try again reading from the next queue + continue; + } + + // Copy the body pair out of the buffer + const BodyPair bp = context->mBodyPairs[read_queue_idx * ioStep->mMaxBodyPairsPerQueue + pair_idx % ioStep->mMaxBodyPairsPerQueue]; + + // Mark this pair as taken + if (queue.mReadIdx.compare_exchange_strong(pair_idx, pair_idx + 1)) + { + // Process the actual body pair + ProcessBodyPair(contact_allocator, bp); + break; + } + } + } + } +} + +void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const BodyPair &inBodyPair) +{ + JPH_PROFILE_FUNCTION(); + + // Fetch body pair + Body *body1 = &mBodyManager.GetBody(inBodyPair.mBodyA); + Body *body2 = &mBodyManager.GetBody(inBodyPair.mBodyB); + JPH_ASSERT(body1->IsActive()); + + JPH_DET_LOG("ProcessBodyPair: id1: " << inBodyPair.mBodyA << " id2: " << inBodyPair.mBodyB << " p1: " << body1->GetCenterOfMassPosition() << " p2: " << body2->GetCenterOfMassPosition() << " r1: " << body1->GetRotation() << " r2: " << body2->GetRotation()); + + // Check for soft bodies + if (body2->IsSoftBody()) + { + // If the 2nd body is a soft body and not active, we activate it now + if (!body2->IsActive()) + mBodyManager.ActivateBodies(&inBodyPair.mBodyB, 1); + + // Soft body processing is done later in the pipeline + return; + } + + // Ensure that body1 has the higher motion type (i.e. dynamic trumps kinematic), this ensures that we do the collision detection in the space of a moving body, + // which avoids accuracy problems when testing a very large static object against a small dynamic object + // Ensure that body1 id < body2 id when motion types are the same. + if (body1->GetMotionType() < body2->GetMotionType() + || (body1->GetMotionType() == body2->GetMotionType() && inBodyPair.mBodyB < inBodyPair.mBodyA)) + std::swap(body1, body2); + + // Check if the contact points from the previous frame are reusable and if so copy them + bool pair_handled = false, constraint_created = false; + if (mPhysicsSettings.mUseBodyPairContactCache && !(body1->IsCollisionCacheInvalid() || body2->IsCollisionCacheInvalid())) + mContactManager.GetContactsFromCache(ioContactAllocator, *body1, *body2, pair_handled, constraint_created); + + // If the cache hasn't handled this body pair do actual collision detection + if (!pair_handled) + { + // Create entry in the cache for this body pair + // Needs to happen irrespective if we found a collision or not (we want to remember that no collision was found too) + ContactConstraintManager::BodyPairHandle body_pair_handle = mContactManager.AddBodyPair(ioContactAllocator, *body1, *body2); + if (body_pair_handle == nullptr) + return; // Out of cache space + + // If we want enhanced active edge detection for this body pair + bool enhanced_active_edges = body1->GetEnhancedInternalEdgeRemovalWithBody(*body2); + + // Create the query settings + CollideShapeSettings settings; + settings.mCollectFacesMode = ECollectFacesMode::CollectFaces; + settings.mActiveEdgeMode = mPhysicsSettings.mCheckActiveEdges && !enhanced_active_edges? EActiveEdgeMode::CollideOnlyWithActive : EActiveEdgeMode::CollideWithAll; + settings.mMaxSeparationDistance = body1->IsSensor() || body2->IsSensor()? 0.0f : mPhysicsSettings.mSpeculativeContactDistance; + settings.mActiveEdgeMovementDirection = body1->GetLinearVelocity() - body2->GetLinearVelocity(); + + // Create shape filter + SimShapeFilterWrapperUnion shape_filter_union(mSimShapeFilter, body1); + SimShapeFilterWrapper &shape_filter = shape_filter_union.GetSimShapeFilterWrapper(); + shape_filter.SetBody2(body2); + + // Get transforms relative to body1 + RVec3 offset = body1->GetCenterOfMassPosition(); + Mat44 transform1 = Mat44::sRotation(body1->GetRotation()); + Mat44 transform2 = body2->GetCenterOfMassTransform().PostTranslated(-offset).ToMat44(); + + if (mPhysicsSettings.mUseManifoldReduction // Check global flag + && body1->GetUseManifoldReductionWithBody(*body2)) // Check body flag + { + // Version WITH contact manifold reduction + + class MyManifold : public ContactManifold + { + public: + Vec3 mFirstWorldSpaceNormal; + }; + + // A temporary structure that allows us to keep track of the all manifolds between this body pair + using Manifolds = StaticArray; + + // Create collector + class ReductionCollideShapeCollector : public CollideShapeCollector + { + public: + ReductionCollideShapeCollector(PhysicsSystem *inSystem, const Body *inBody1, const Body *inBody2) : + mSystem(inSystem), + mBody1(inBody1), + mBody2(inBody2) + { + } + + virtual void AddHit(const CollideShapeResult &inResult) override + { + // The first body should be the one with the highest motion type + JPH_ASSERT(mBody1->GetMotionType() >= mBody2->GetMotionType()); + JPH_ASSERT(!ShouldEarlyOut()); + + // Test if we want to accept this hit + if (mValidateBodyPair) + { + switch (mSystem->mContactManager.ValidateContactPoint(*mBody1, *mBody2, mBody1->GetCenterOfMassPosition(), inResult)) + { + case ValidateResult::AcceptContact: + // We're just accepting this one, nothing to do + break; + + case ValidateResult::AcceptAllContactsForThisBodyPair: + // Accept and stop calling the validate callback + mValidateBodyPair = false; + break; + + case ValidateResult::RejectContact: + // Skip this contact + return; + + case ValidateResult::RejectAllContactsForThisBodyPair: + // Skip this and early out + ForceEarlyOut(); + return; + } + } + + // Calculate normal + Vec3 world_space_normal = inResult.mPenetrationAxis.Normalized(); + + // Check if we can add it to an existing manifold + Manifolds::iterator manifold; + float contact_normal_cos_max_delta_rot = mSystem->mPhysicsSettings.mContactNormalCosMaxDeltaRotation; + for (manifold = mManifolds.begin(); manifold != mManifolds.end(); ++manifold) + if (world_space_normal.Dot(manifold->mFirstWorldSpaceNormal) >= contact_normal_cos_max_delta_rot) + { + // Update average normal + manifold->mWorldSpaceNormal += world_space_normal; + manifold->mPenetrationDepth = max(manifold->mPenetrationDepth, inResult.mPenetrationDepth); + break; + } + if (manifold == mManifolds.end()) + { + // Check if array is full + if (mManifolds.size() == mManifolds.capacity()) + { + // Full, find manifold with least amount of penetration + manifold = mManifolds.begin(); + for (Manifolds::iterator m = mManifolds.begin() + 1; m < mManifolds.end(); ++m) + if (m->mPenetrationDepth < manifold->mPenetrationDepth) + manifold = m; + + // If this contacts penetration is smaller than the smallest manifold, we skip this contact + if (inResult.mPenetrationDepth < manifold->mPenetrationDepth) + return; + + // Replace the manifold + *manifold = { { mBody1->GetCenterOfMassPosition(), world_space_normal, inResult.mPenetrationDepth, inResult.mSubShapeID1, inResult.mSubShapeID2, { }, { } }, world_space_normal }; + } + else + { + // Not full, create new manifold + mManifolds.push_back({ { mBody1->GetCenterOfMassPosition(), world_space_normal, inResult.mPenetrationDepth, inResult.mSubShapeID1, inResult.mSubShapeID2, { }, { } }, world_space_normal }); + manifold = mManifolds.end() - 1; + } + } + + // Determine contact points + const PhysicsSettings &settings = mSystem->mPhysicsSettings; + ManifoldBetweenTwoFaces(inResult.mContactPointOn1, inResult.mContactPointOn2, inResult.mPenetrationAxis, Square(settings.mSpeculativeContactDistance) + settings.mManifoldToleranceSq, inResult.mShape1Face, inResult.mShape2Face, manifold->mRelativeContactPointsOn1, manifold->mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, mBody1->GetCenterOfMassPosition())); + + // Prune if we have more than 32 points (this means we could run out of space in the next iteration) + if (manifold->mRelativeContactPointsOn1.size() > 32) + PruneContactPoints(manifold->mFirstWorldSpaceNormal, manifold->mRelativeContactPointsOn1, manifold->mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold->mBaseOffset)); + } + + PhysicsSystem * mSystem; + const Body * mBody1; + const Body * mBody2; + bool mValidateBodyPair = true; + Manifolds mManifolds; + }; + ReductionCollideShapeCollector collector(this, body1, body2); + + // Perform collision detection between the two shapes + SubShapeIDCreator part1, part2; + auto f = enhanced_active_edges? InternalEdgeRemovingCollector::sCollideShapeVsShape : CollisionDispatch::sCollideShapeVsShape; + f(body1->GetShape(), body2->GetShape(), Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), transform1, transform2, part1, part2, settings, collector, shape_filter); + + // Add the contacts + for (ContactManifold &manifold : collector.mManifolds) + { + // Normalize the normal (is a sum of all normals from merged manifolds) + manifold.mWorldSpaceNormal = manifold.mWorldSpaceNormal.Normalized(); + + // If we still have too many points, prune them now + if (manifold.mRelativeContactPointsOn1.size() > 4) + PruneContactPoints(manifold.mWorldSpaceNormal, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset)); + + // Actually add the contact points to the manager + constraint_created |= mContactManager.AddContactConstraint(ioContactAllocator, body_pair_handle, *body1, *body2, manifold); + } + } + else + { + // Version WITHOUT contact manifold reduction + + // Create collector + class NonReductionCollideShapeCollector : public CollideShapeCollector + { + public: + NonReductionCollideShapeCollector(PhysicsSystem *inSystem, ContactAllocator &ioContactAllocator, Body *inBody1, Body *inBody2, const ContactConstraintManager::BodyPairHandle &inPairHandle) : + mSystem(inSystem), + mContactAllocator(ioContactAllocator), + mBody1(inBody1), + mBody2(inBody2), + mBodyPairHandle(inPairHandle) + { + } + + virtual void AddHit(const CollideShapeResult &inResult) override + { + // The first body should be the one with the highest motion type + JPH_ASSERT(mBody1->GetMotionType() >= mBody2->GetMotionType()); + JPH_ASSERT(!ShouldEarlyOut()); + + // Test if we want to accept this hit + if (mValidateBodyPair) + { + switch (mSystem->mContactManager.ValidateContactPoint(*mBody1, *mBody2, mBody1->GetCenterOfMassPosition(), inResult)) + { + case ValidateResult::AcceptContact: + // We're just accepting this one, nothing to do + break; + + case ValidateResult::AcceptAllContactsForThisBodyPair: + // Accept and stop calling the validate callback + mValidateBodyPair = false; + break; + + case ValidateResult::RejectContact: + // Skip this contact + return; + + case ValidateResult::RejectAllContactsForThisBodyPair: + // Skip this and early out + ForceEarlyOut(); + return; + } + } + + // Determine contact points + ContactManifold manifold; + manifold.mBaseOffset = mBody1->GetCenterOfMassPosition(); + const PhysicsSettings &settings = mSystem->mPhysicsSettings; + ManifoldBetweenTwoFaces(inResult.mContactPointOn1, inResult.mContactPointOn2, inResult.mPenetrationAxis, Square(settings.mSpeculativeContactDistance) + settings.mManifoldToleranceSq, inResult.mShape1Face, inResult.mShape2Face, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset)); + + // Calculate normal + manifold.mWorldSpaceNormal = inResult.mPenetrationAxis.Normalized(); + + // Store penetration depth + manifold.mPenetrationDepth = inResult.mPenetrationDepth; + + // Prune if we have more than 4 points + if (manifold.mRelativeContactPointsOn1.size() > 4) + PruneContactPoints(manifold.mWorldSpaceNormal, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset)); + + // Set other properties + manifold.mSubShapeID1 = inResult.mSubShapeID1; + manifold.mSubShapeID2 = inResult.mSubShapeID2; + + // Actually add the contact points to the manager + mConstraintCreated |= mSystem->mContactManager.AddContactConstraint(mContactAllocator, mBodyPairHandle, *mBody1, *mBody2, manifold); + } + + PhysicsSystem * mSystem; + ContactAllocator & mContactAllocator; + Body * mBody1; + Body * mBody2; + ContactConstraintManager::BodyPairHandle mBodyPairHandle; + bool mValidateBodyPair = true; + bool mConstraintCreated = false; + }; + NonReductionCollideShapeCollector collector(this, ioContactAllocator, body1, body2, body_pair_handle); + + // Perform collision detection between the two shapes + SubShapeIDCreator part1, part2; + auto f = enhanced_active_edges? InternalEdgeRemovingCollector::sCollideShapeVsShape : CollisionDispatch::sCollideShapeVsShape; + f(body1->GetShape(), body2->GetShape(), Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), transform1, transform2, part1, part2, settings, collector, shape_filter); + + constraint_created = collector.mConstraintCreated; + } + } + + // If a contact constraint was created, we need to do some extra work + if (constraint_created) + { + // Wake up sleeping bodies + BodyID body_ids[2]; + int num_bodies = 0; + if (body1->IsDynamic() && !body1->IsActive()) + body_ids[num_bodies++] = body1->GetID(); + if (body2->IsDynamic() && !body2->IsActive()) + body_ids[num_bodies++] = body2->GetID(); + if (num_bodies > 0) + mBodyManager.ActivateBodies(body_ids, num_bodies); + + // Link the two bodies + mIslandBuilder.LinkBodies(body1->GetIndexInActiveBodiesInternal(), body2->GetIndexInActiveBodiesInternal()); + } +} + +void PhysicsSystem::JobFinalizeIslands(PhysicsUpdateContext *ioContext) +{ +#ifdef JPH_ENABLE_ASSERTS + // We only touch island data + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::None); +#endif + + // Finish collecting the islands, at this point the active body list doesn't change so it's safe to access + mIslandBuilder.Finalize(mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody), mBodyManager.GetNumActiveBodies(EBodyType::RigidBody), mContactManager.GetNumConstraints(), ioContext->mTempAllocator); + + // Prepare the large island splitter + if (mPhysicsSettings.mUseLargeIslandSplitter) + mLargeIslandSplitter.Prepare(mIslandBuilder, mBodyManager.GetNumActiveBodies(EBodyType::RigidBody), ioContext->mTempAllocator); +} + +void PhysicsSystem::JobBodySetIslandIndex() +{ +#ifdef JPH_ENABLE_ASSERTS + // We only touch island data + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::None); +#endif + + // Loop through the result and tag all bodies with an island index + for (uint32 island_idx = 0, n = mIslandBuilder.GetNumIslands(); island_idx < n; ++island_idx) + { + BodyID *body_start, *body_end; + mIslandBuilder.GetBodiesInIsland(island_idx, body_start, body_end); + for (const BodyID *body = body_start; body < body_end; ++body) + mBodyManager.GetBody(*body).GetMotionProperties()->SetIslandIndexInternal(island_idx); + } +} + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wundefined-func-template") // ConstraintManager::sWarmStartVelocityConstraints / ContactConstraintManager::WarmStartVelocityConstraints is instantiated in the cpp file + +void PhysicsSystem::JobSolveVelocityConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // We update velocities and need to read positions to do so + BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::Read); +#endif + + float delta_time = ioContext->mStepDeltaTime; + Constraint **active_constraints = ioContext->mActiveConstraints; + + // Only the first step to correct for the delta time difference in the previous update + float warm_start_impulse_ratio = ioStep->mIsFirst? ioContext->mWarmStartImpulseRatio : 1.0f; + + bool check_islands = true, check_split_islands = mPhysicsSettings.mUseLargeIslandSplitter; + for (;;) + { + // First try to get work from large islands + if (check_split_islands) + { + bool first_iteration; + uint split_island_index; + uint32 *constraints_begin, *constraints_end, *contacts_begin, *contacts_end; + switch (mLargeIslandSplitter.FetchNextBatch(split_island_index, constraints_begin, constraints_end, contacts_begin, contacts_end, first_iteration)) + { + case LargeIslandSplitter::EStatus::BatchRetrieved: + { + if (first_iteration) + { + // Iteration 0 is used to warm start the batch (we added 1 to the number of iterations in LargeIslandSplitter::SplitIsland) + DummyCalculateSolverSteps dummy; + ConstraintManager::sWarmStartVelocityConstraints(active_constraints, constraints_begin, constraints_end, warm_start_impulse_ratio, dummy); + mContactManager.WarmStartVelocityConstraints(contacts_begin, contacts_end, warm_start_impulse_ratio, dummy); + } + else + { + // Solve velocity constraints + ConstraintManager::sSolveVelocityConstraints(active_constraints, constraints_begin, constraints_end, delta_time); + mContactManager.SolveVelocityConstraints(contacts_begin, contacts_end); + } + + // Mark the batch as processed + bool last_iteration, final_batch; + mLargeIslandSplitter.MarkBatchProcessed(split_island_index, constraints_begin, constraints_end, contacts_begin, contacts_end, last_iteration, final_batch); + + // Save back the lambdas in the contact cache for the warm start of the next physics update + if (last_iteration) + mContactManager.StoreAppliedImpulses(contacts_begin, contacts_end); + + // We processed work, loop again + continue; + } + + case LargeIslandSplitter::EStatus::WaitingForBatch: + break; + + case LargeIslandSplitter::EStatus::AllBatchesDone: + check_split_islands = false; + break; + } + } + + // If that didn't succeed try to process an island + if (check_islands) + { + // Next island + uint32 island_idx = ioStep->mSolveVelocityConstraintsNextIsland++; + if (island_idx >= mIslandBuilder.GetNumIslands()) + { + // We processed all islands, stop checking islands + check_islands = false; + continue; + } + + JPH_PROFILE("Island"); + + // Get iterators for this island + uint32 *constraints_begin, *constraints_end, *contacts_begin, *contacts_end; + bool has_constraints = mIslandBuilder.GetConstraintsInIsland(island_idx, constraints_begin, constraints_end); + bool has_contacts = mIslandBuilder.GetContactsInIsland(island_idx, contacts_begin, contacts_end); + + // If we don't have any contacts or constraints, we know that none of the following islands have any contacts or constraints + // (because they're sorted by most constraints first). This means we're done. + if (!has_contacts && !has_constraints) + { + #ifdef JPH_ENABLE_ASSERTS + // Validate our assumption that the next islands don't have any constraints or contacts + for (; island_idx < mIslandBuilder.GetNumIslands(); ++island_idx) + { + JPH_ASSERT(!mIslandBuilder.GetConstraintsInIsland(island_idx, constraints_begin, constraints_end)); + JPH_ASSERT(!mIslandBuilder.GetContactsInIsland(island_idx, contacts_begin, contacts_end)); + } + #endif // JPH_ENABLE_ASSERTS + + check_islands = false; + continue; + } + + // Sorting is costly but needed for a deterministic simulation, allow the user to turn this off + if (mPhysicsSettings.mDeterministicSimulation) + { + // Sort constraints to give a deterministic simulation + ConstraintManager::sSortConstraints(active_constraints, constraints_begin, constraints_end); + + // Sort contacts to give a deterministic simulation + mContactManager.SortContacts(contacts_begin, contacts_end); + } + + // Split up large islands + CalculateSolverSteps steps_calculator(mPhysicsSettings); + if (mPhysicsSettings.mUseLargeIslandSplitter + && mLargeIslandSplitter.SplitIsland(island_idx, mIslandBuilder, mBodyManager, mContactManager, active_constraints, steps_calculator)) + continue; // Loop again to try to fetch the newly split island + + // We didn't create a split, just run the solver now for this entire island. Begin by warm starting. + ConstraintManager::sWarmStartVelocityConstraints(active_constraints, constraints_begin, constraints_end, warm_start_impulse_ratio, steps_calculator); + mContactManager.WarmStartVelocityConstraints(contacts_begin, contacts_end, warm_start_impulse_ratio, steps_calculator); + steps_calculator.Finalize(); + + // Store the number of position steps for later + mIslandBuilder.SetNumPositionSteps(island_idx, steps_calculator.GetNumPositionSteps()); + + // Solve velocity constraints + for (uint velocity_step = 0; velocity_step < steps_calculator.GetNumVelocitySteps(); ++velocity_step) + { + bool applied_impulse = ConstraintManager::sSolveVelocityConstraints(active_constraints, constraints_begin, constraints_end, delta_time); + applied_impulse |= mContactManager.SolveVelocityConstraints(contacts_begin, contacts_end); + if (!applied_impulse) + break; + } + + // Save back the lambdas in the contact cache for the warm start of the next physics update + mContactManager.StoreAppliedImpulses(contacts_begin, contacts_end); + + // We processed work, loop again + continue; + } + + if (check_islands) + { + // If there are islands, we don't need to wait and can pick up new work + continue; + } + else if (check_split_islands) + { + // If there are split islands, but we did't do any work, give up a time slice + std::this_thread::yield(); + } + else + { + // No more work + break; + } + } +} + +JPH_SUPPRESS_WARNING_POP + +void PhysicsSystem::JobPreIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ + // Reserve enough space for all bodies that may need a cast + TempAllocator *temp_allocator = ioContext->mTempAllocator; + JPH_ASSERT(ioStep->mCCDBodies == nullptr); + ioStep->mCCDBodiesCapacity = mBodyManager.GetNumActiveCCDBodies(); + ioStep->mCCDBodies = (CCDBody *)temp_allocator->Allocate(ioStep->mCCDBodiesCapacity * sizeof(CCDBody)); + + // Initialize the mapping table between active body and CCD body + JPH_ASSERT(ioStep->mActiveBodyToCCDBody == nullptr); + ioStep->mNumActiveBodyToCCDBody = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody); + ioStep->mActiveBodyToCCDBody = (int *)temp_allocator->Allocate(ioStep->mNumActiveBodyToCCDBody * sizeof(int)); + + // Prepare the split island builder for solving the position constraints + mLargeIslandSplitter.PrepareForSolvePositions(); +} + +void PhysicsSystem::JobIntegrateVelocity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // We update positions and need velocity to do so, we also clamp velocities so need to write to them + BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::ReadWrite); +#endif + + float delta_time = ioContext->mStepDeltaTime; + const BodyID *active_bodies = mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody); + uint32 num_active_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody); + uint32 num_active_bodies_after_find_collisions = ioStep->mActiveBodyReadIdx; + + // We can move bodies that are not part of an island. In this case we need to notify the broadphase of the movement. + static constexpr int cBodiesBatch = 64; + BodyID *bodies_to_update_bounds = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID)); + int num_bodies_to_update_bounds = 0; + + for (;;) + { + // Atomically fetch a batch of bodies + uint32 active_body_idx = ioStep->mIntegrateVelocityReadIdx.fetch_add(cIntegrateVelocityBatchSize); + if (active_body_idx >= num_active_bodies) + break; + + // Calculate the end of the batch + uint32 active_body_idx_end = min(num_active_bodies, active_body_idx + cIntegrateVelocityBatchSize); + + // Process the batch + while (active_body_idx < active_body_idx_end) + { + // Update the positions using an Symplectic Euler step (which integrates using the updated velocity v1' rather + // than the original velocity v1): + // x1' = x1 + h * v1' + // At this point the active bodies list does not change, so it is safe to access the array. + BodyID body_id = active_bodies[active_body_idx]; + Body &body = mBodyManager.GetBody(body_id); + MotionProperties *mp = body.GetMotionProperties(); + + JPH_DET_LOG("JobIntegrateVelocity: id: " << body_id << " v: " << body.GetLinearVelocity() << " w: " << body.GetAngularVelocity()); + + // Clamp velocities (not for kinematic bodies) + if (body.IsDynamic()) + { + mp->ClampLinearVelocity(); + mp->ClampAngularVelocity(); + } + + // Update the rotation of the body according to the angular velocity + // For motion type discrete we need to do this anyway, for motion type linear cast we have multiple choices + // 1. Rotate the body first and then sweep + // 2. First sweep and then rotate the body at the end + // 3. Pick some in between rotation (e.g. half way), then sweep and finally rotate the remainder + // (1) has some clear advantages as when a long thin body hits a surface away from the center of mass, this will result in a large angular velocity and a limited reduction in linear velocity. + // When simulation the rotation first before doing the translation, the body will be able to rotate away from the contact point allowing the center of mass to approach the surface. When using + // approach (2) in this case what will happen is that we will immediately detect the same collision again (the body has not rotated and the body was already colliding at the end of the previous + // time step) resulting in a lot of stolen time and the body appearing to be frozen in an unnatural pose (like it is glued at an angle to the surface). (2) obviously has some negative side effects + // too as simulating the rotation first may cause it to tunnel through a small object that the linear cast might have otherwise detected. In any case a linear cast is not good for detecting + // tunneling due to angular rotation, so we don't care about that too much (you'd need a full cast to take angular effects into account). + body.AddRotationStep(body.GetAngularVelocity() * delta_time); + + // Get delta position + Vec3 delta_pos = body.GetLinearVelocity() * delta_time; + + // If the position should be updated (or if it is delayed because of CCD) + bool update_position = true; + + switch (mp->GetMotionQuality()) + { + case EMotionQuality::Discrete: + // No additional collision checking to be done + break; + + case EMotionQuality::LinearCast: + if (body.IsDynamic() // Kinematic bodies cannot be stopped + && !body.IsSensor()) // We don't support CCD sensors + { + // Determine inner radius (the smallest sphere that fits into the shape) + float inner_radius = body.GetShape()->GetInnerRadius(); + JPH_ASSERT(inner_radius > 0.0f, "The shape has no inner radius, this makes the shape unsuitable for the linear cast motion quality as we cannot move it without risking tunneling."); + + // Measure translation in this step and check if it above the threshold to perform a linear cast + float linear_cast_threshold_sq = Square(mPhysicsSettings.mLinearCastThreshold * inner_radius); + if (delta_pos.LengthSq() > linear_cast_threshold_sq) + { + // This body needs a cast + uint32 ccd_body_idx = ioStep->mNumCCDBodies++; + JPH_ASSERT(active_body_idx < ioStep->mNumActiveBodyToCCDBody); + ioStep->mActiveBodyToCCDBody[active_body_idx] = ccd_body_idx; + new (&ioStep->mCCDBodies[ccd_body_idx]) CCDBody(body_id, delta_pos, linear_cast_threshold_sq, min(mPhysicsSettings.mPenetrationSlop, mPhysicsSettings.mLinearCastMaxPenetration * inner_radius)); + + update_position = false; + } + } + break; + } + + if (update_position) + { + // Move the body now + body.AddPositionStep(delta_pos); + + // If the body was activated due to an earlier CCD step it will have an index in the active + // body list that it higher than the highest one we processed during FindCollisions + // which means it hasn't been assigned an island and will not be updated by an island + // this means that we need to update its bounds manually + if (mp->GetIndexInActiveBodiesInternal() >= num_active_bodies_after_find_collisions) + { + body.CalculateWorldSpaceBoundsInternal(); + bodies_to_update_bounds[num_bodies_to_update_bounds++] = body.GetID(); + if (num_bodies_to_update_bounds == cBodiesBatch) + { + // Buffer full, flush now + mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false); + num_bodies_to_update_bounds = 0; + } + } + + // We did not create a CCD body + ioStep->mActiveBodyToCCDBody[active_body_idx] = -1; + } + + active_body_idx++; + } + } + + // Notify change bounds on requested bodies + if (num_bodies_to_update_bounds > 0) + mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false); +} + +void PhysicsSystem::JobPostIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) const +{ + // Validate that our reservations were correct + JPH_ASSERT(ioStep->mNumCCDBodies <= mBodyManager.GetNumActiveCCDBodies()); + + if (ioStep->mNumCCDBodies == 0) + { + // No continuous collision detection jobs -> kick the next job ourselves + ioStep->mContactRemovedCallbacks.RemoveDependency(); + } + else + { + // Run the continuous collision detection jobs + int num_continuous_collision_jobs = min(int(ioStep->mNumCCDBodies + cNumCCDBodiesPerJob - 1) / cNumCCDBodiesPerJob, ioContext->GetMaxConcurrency()); + ioStep->mResolveCCDContacts.AddDependency(num_continuous_collision_jobs); + ioStep->mContactRemovedCallbacks.AddDependency(num_continuous_collision_jobs - 1); // Already had 1 dependency + for (int i = 0; i < num_continuous_collision_jobs; ++i) + { + JobHandle job = ioContext->mJobSystem->CreateJob("FindCCDContacts", cColorFindCCDContacts, [ioContext, ioStep]() + { + ioContext->mPhysicsSystem->JobFindCCDContacts(ioContext, ioStep); + + ioStep->mResolveCCDContacts.RemoveDependency(); + ioStep->mContactRemovedCallbacks.RemoveDependency(); + }); + ioContext->mBarrier->AddJob(job); + } + } +} + +// Helper function to calculate the motion of a body during this CCD step +inline static Vec3 sCalculateBodyMotion(const Body &inBody, float inDeltaTime) +{ + // If the body is linear casting, the body has not yet moved so we need to calculate its motion + if (inBody.IsDynamic() && inBody.GetMotionProperties()->GetMotionQuality() == EMotionQuality::LinearCast) + return inDeltaTime * inBody.GetLinearVelocity(); + + // Body has already moved, so we don't need to correct for anything + return Vec3::sZero(); +} + +// Helper function that finds the CCD body corresponding to a body (if it exists) +inline static PhysicsUpdateContext::Step::CCDBody *sGetCCDBody(const Body &inBody, PhysicsUpdateContext::Step *inStep) +{ + // Only rigid bodies can have a CCD body + if (!inBody.IsRigidBody()) + return nullptr; + + // If the body has no motion properties it cannot have a CCD body + const MotionProperties *motion_properties = inBody.GetMotionPropertiesUnchecked(); + if (motion_properties == nullptr) + return nullptr; + + // If it is not active it cannot have a CCD body + uint32 active_index = motion_properties->GetIndexInActiveBodiesInternal(); + if (active_index == Body::cInactiveIndex) + return nullptr; + + // Check if the active body has a corresponding CCD body + JPH_ASSERT(active_index < inStep->mNumActiveBodyToCCDBody); // Ensure that the body has a mapping to CCD body + int ccd_index = inStep->mActiveBodyToCCDBody[active_index]; + if (ccd_index < 0) + return nullptr; + + PhysicsUpdateContext::Step::CCDBody *ccd_body = &inStep->mCCDBodies[ccd_index]; + JPH_ASSERT(ccd_body->mBodyID1 == inBody.GetID(), "We found the wrong CCD body!"); + return ccd_body; +} + +void PhysicsSystem::JobFindCCDContacts(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // We only read positions, but the validate callback may read body positions and velocities + BodyAccess::Grant grant(BodyAccess::EAccess::Read, BodyAccess::EAccess::Read); +#endif + + // Allocation context for allocating new contact points + ContactAllocator contact_allocator(mContactManager.GetContactAllocator()); + + // Settings + ShapeCastSettings settings; + settings.mUseShrunkenShapeAndConvexRadius = true; + settings.mBackFaceModeTriangles = EBackFaceMode::IgnoreBackFaces; + settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces; + settings.mReturnDeepestPoint = true; + settings.mCollectFacesMode = ECollectFacesMode::CollectFaces; + settings.mActiveEdgeMode = mPhysicsSettings.mCheckActiveEdges? EActiveEdgeMode::CollideOnlyWithActive : EActiveEdgeMode::CollideWithAll; + + for (;;) + { + // Fetch the next body to cast + uint32 idx = ioStep->mNextCCDBody++; + if (idx >= ioStep->mNumCCDBodies) + break; + CCDBody &ccd_body = ioStep->mCCDBodies[idx]; + const Body &body = mBodyManager.GetBody(ccd_body.mBodyID1); + + // Filter out layers + DefaultBroadPhaseLayerFilter broadphase_layer_filter = GetDefaultBroadPhaseLayerFilter(body.GetObjectLayer()); + DefaultObjectLayerFilter object_layer_filter = GetDefaultLayerFilter(body.GetObjectLayer()); + + #ifdef JPH_DEBUG_RENDERER + // Draw start and end shape of cast + if (sDrawMotionQualityLinearCast) + { + RMat44 com = body.GetCenterOfMassTransform(); + body.GetShape()->Draw(DebugRenderer::sInstance, com, Vec3::sReplicate(1.0f), Color::sGreen, false, true); + DebugRenderer::sInstance->DrawArrow(com.GetTranslation(), com.GetTranslation() + ccd_body.mDeltaPosition, Color::sGreen, 0.1f); + body.GetShape()->Draw(DebugRenderer::sInstance, com.PostTranslated(ccd_body.mDeltaPosition), Vec3::sReplicate(1.0f), Color::sRed, false, true); + } + #endif // JPH_DEBUG_RENDERER + + // Create a collector that will find the maximum distance allowed to travel while not penetrating more than 'max penetration' + class CCDNarrowPhaseCollector : public CastShapeCollector + { + public: + CCDNarrowPhaseCollector(const BodyManager &inBodyManager, ContactConstraintManager &inContactConstraintManager, CCDBody &inCCDBody, ShapeCastResult &inResult, float inDeltaTime) : + mBodyManager(inBodyManager), + mContactConstraintManager(inContactConstraintManager), + mCCDBody(inCCDBody), + mResult(inResult), + mDeltaTime(inDeltaTime) + { + } + + virtual void AddHit(const ShapeCastResult &inResult) override + { + JPH_PROFILE_FUNCTION(); + + // Check if this is a possible earlier hit than the one before + float fraction = inResult.mFraction; + if (fraction < mCCDBody.mFractionPlusSlop) + { + // Normalize normal + Vec3 normal = inResult.mPenetrationAxis.Normalized(); + + // Calculate how much we can add to the fraction to penetrate the collision point by mMaxPenetration. + // Note that the normal is pointing towards body 2! + // Let the extra distance that we can travel along delta_pos be 'dist': mMaxPenetration / dist = cos(angle between normal and delta_pos) = normal . delta_pos / |delta_pos| + // <=> dist = mMaxPenetration * |delta_pos| / normal . delta_pos + // Converting to a faction: delta_fraction = dist / |delta_pos| = mLinearCastTreshold / normal . delta_pos + float denominator = normal.Dot(mCCDBody.mDeltaPosition); + if (denominator > mCCDBody.mMaxPenetration) // Avoid dividing by zero, if extra hit fraction > 1 there's also no point in continuing + { + float fraction_plus_slop = fraction + mCCDBody.mMaxPenetration / denominator; + if (fraction_plus_slop < mCCDBody.mFractionPlusSlop) + { + const Body &body2 = mBodyManager.GetBody(inResult.mBodyID2); + + // Check if we've already accepted all hits from this body + if (mValidateBodyPair) + { + // Validate the contact result + const Body &body1 = mBodyManager.GetBody(mCCDBody.mBodyID1); + ValidateResult validate_result = mContactConstraintManager.ValidateContactPoint(body1, body2, body1.GetCenterOfMassPosition(), inResult); // Note that the center of mass of body 1 is the start of the sweep and is used as base offset below + switch (validate_result) + { + case ValidateResult::AcceptContact: + // Just continue + break; + + case ValidateResult::AcceptAllContactsForThisBodyPair: + // Accept this and all following contacts from this body + mValidateBodyPair = false; + break; + + case ValidateResult::RejectContact: + return; + + case ValidateResult::RejectAllContactsForThisBodyPair: + // Reject this and all following contacts from this body + mRejectAll = true; + ForceEarlyOut(); + return; + } + } + + // This is the earliest hit so far, store it + mCCDBody.mContactNormal = normal; + mCCDBody.mBodyID2 = inResult.mBodyID2; + mCCDBody.mSubShapeID2 = inResult.mSubShapeID2; + mCCDBody.mFraction = fraction; + mCCDBody.mFractionPlusSlop = fraction_plus_slop; + mResult = inResult; + + // Result was assuming body 2 is not moving, but it is, so we need to correct for it + Vec3 movement2 = fraction * sCalculateBodyMotion(body2, mDeltaTime); + if (!movement2.IsNearZero()) + { + mResult.mContactPointOn1 += movement2; + mResult.mContactPointOn2 += movement2; + for (Vec3 &v : mResult.mShape1Face) + v += movement2; + for (Vec3 &v : mResult.mShape2Face) + v += movement2; + } + + // Update early out fraction + UpdateEarlyOutFraction(fraction_plus_slop); + } + } + } + } + + bool mValidateBodyPair; ///< If we still have to call the ValidateContactPoint for this body pair + bool mRejectAll; ///< Reject all further contacts between this body pair + + private: + const BodyManager & mBodyManager; + ContactConstraintManager & mContactConstraintManager; + CCDBody & mCCDBody; + ShapeCastResult & mResult; + float mDeltaTime; + BodyID mAcceptedBodyID; + }; + + // Narrowphase collector + ShapeCastResult cast_shape_result; + CCDNarrowPhaseCollector np_collector(mBodyManager, mContactManager, ccd_body, cast_shape_result, ioContext->mStepDeltaTime); + + // This collector wraps the narrowphase collector and collects the closest hit + class CCDBroadPhaseCollector : public CastShapeBodyCollector + { + public: + CCDBroadPhaseCollector(const CCDBody &inCCDBody, const Body &inBody1, const RShapeCast &inShapeCast, ShapeCastSettings &inShapeCastSettings, SimShapeFilterWrapper &inShapeFilter, CCDNarrowPhaseCollector &ioCollector, const BodyManager &inBodyManager, PhysicsUpdateContext::Step *inStep, float inDeltaTime) : + mCCDBody(inCCDBody), + mBody1(inBody1), + mBody1Extent(inShapeCast.mShapeWorldBounds.GetExtent()), + mShapeCast(inShapeCast), + mShapeCastSettings(inShapeCastSettings), + mShapeFilter(inShapeFilter), + mCollector(ioCollector), + mBodyManager(inBodyManager), + mStep(inStep), + mDeltaTime(inDeltaTime) + { + } + + virtual void AddHit(const BroadPhaseCastResult &inResult) override + { + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inResult.mFraction <= GetEarlyOutFraction(), "This hit should not have been passed on to the collector"); + + // Test if we're colliding with ourselves + if (mBody1.GetID() == inResult.mBodyID) + return; + + // Avoid treating duplicates, if both bodies are doing CCD then only consider collision if body ID < other body ID + const Body &body2 = mBodyManager.GetBody(inResult.mBodyID); + const CCDBody *ccd_body2 = sGetCCDBody(body2, mStep); + if (ccd_body2 != nullptr && mCCDBody.mBodyID1 > ccd_body2->mBodyID1) + return; + + // Test group filter + if (!mBody1.GetCollisionGroup().CanCollide(body2.GetCollisionGroup())) + return; + + // TODO: For now we ignore sensors + if (body2.IsSensor()) + return; + + // Get relative movement of these two bodies + Vec3 direction = mShapeCast.mDirection - sCalculateBodyMotion(body2, mDeltaTime); + + // Test if the remaining movement is less than our movement threshold + if (direction.LengthSq() < mCCDBody.mLinearCastThresholdSq) + return; + + // Get the bounds of 2, widen it by the extent of 1 and test a ray to see if it hits earlier than the current early out fraction + AABox bounds = body2.GetWorldSpaceBounds(); + bounds.mMin -= mBody1Extent; + bounds.mMax += mBody1Extent; + float hit_fraction = RayAABox(Vec3(mShapeCast.mCenterOfMassStart.GetTranslation()), RayInvDirection(direction), bounds.mMin, bounds.mMax); + if (hit_fraction > GetPositiveEarlyOutFraction()) // If early out fraction <= 0, we have the possibility of finding a deeper hit so we need to clamp the early out fraction + return; + + // Reset collector (this is a new body pair) + mCollector.ResetEarlyOutFraction(GetEarlyOutFraction()); + mCollector.mValidateBodyPair = true; + mCollector.mRejectAll = false; + + // Set body ID on shape filter + mShapeFilter.SetBody2(&body2); + + // Provide direction as hint for the active edges algorithm + mShapeCastSettings.mActiveEdgeMovementDirection = direction; + + // 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); + + // Update early out fraction based on narrow phase collector + if (!mCollector.mRejectAll) + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + + const CCDBody & mCCDBody; + const Body & mBody1; + Vec3 mBody1Extent; + RShapeCast mShapeCast; + ShapeCastSettings & mShapeCastSettings; + SimShapeFilterWrapper & mShapeFilter; + CCDNarrowPhaseCollector & mCollector; + const BodyManager & mBodyManager; + PhysicsUpdateContext::Step *mStep; + float mDeltaTime; + }; + + // Create shape filter + SimShapeFilterWrapperUnion shape_filter_union(mSimShapeFilter, &body); + SimShapeFilterWrapper &shape_filter = shape_filter_union.GetSimShapeFilterWrapper(); + + // 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::sReplicate(1.0f), body.GetCenterOfMassTransform(), ccd_body.mDeltaPosition); + CCDBroadPhaseCollector bp_collector(ccd_body, body, shape_cast, settings, shape_filter, np_collector, mBodyManager, ioStep, ioContext->mStepDeltaTime); + mBroadPhase->CastAABoxNoLock({ shape_cast.mShapeWorldBounds, shape_cast.mDirection }, bp_collector, broadphase_layer_filter, object_layer_filter); + + // Check if there was a hit + if (ccd_body.mFractionPlusSlop < 1.0f) + { + const Body &body2 = mBodyManager.GetBody(ccd_body.mBodyID2); + + // Determine contact manifold + ContactManifold manifold; + manifold.mBaseOffset = shape_cast.mCenterOfMassStart.GetTranslation(); + ManifoldBetweenTwoFaces(cast_shape_result.mContactPointOn1, cast_shape_result.mContactPointOn2, cast_shape_result.mPenetrationAxis, mPhysicsSettings.mManifoldToleranceSq, cast_shape_result.mShape1Face, cast_shape_result.mShape2Face, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset)); + manifold.mSubShapeID1 = cast_shape_result.mSubShapeID1; + manifold.mSubShapeID2 = cast_shape_result.mSubShapeID2; + manifold.mPenetrationDepth = cast_shape_result.mPenetrationDepth; + manifold.mWorldSpaceNormal = ccd_body.mContactNormal; + + // Call contact point callbacks + mContactManager.OnCCDContactAdded(contact_allocator, body, body2, manifold, ccd_body.mContactSettings); + + if (ccd_body.mContactSettings.mIsSensor) + { + // If this is a sensor, we don't want to solve the contact + ccd_body.mFractionPlusSlop = 1.0f; + ccd_body.mBodyID2 = BodyID(); + } + else + { + // Calculate the average position from the manifold (this will result in the same impulse applied as when we apply impulses to all contact points) + if (manifold.mRelativeContactPointsOn2.size() > 1) + { + Vec3 average_contact_point = Vec3::sZero(); + for (const Vec3 &v : manifold.mRelativeContactPointsOn2) + average_contact_point += v; + average_contact_point /= (float)manifold.mRelativeContactPointsOn2.size(); + ccd_body.mContactPointOn2 = manifold.mBaseOffset + average_contact_point; + } + else + ccd_body.mContactPointOn2 = manifold.mBaseOffset + cast_shape_result.mContactPointOn2; + } + } + } + + // Collect information from the contact allocator and accumulate it in the step. + sFinalizeContactAllocator(*ioStep, contact_allocator); +} + +void PhysicsSystem::JobResolveCCDContacts(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // Read/write body access + BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::ReadWrite); + + // We activate bodies that we collide with + BodyManager::GrantActiveBodiesAccess grant_active(true, false); +#endif + + uint32 num_active_bodies_after_find_collisions = ioStep->mActiveBodyReadIdx; + TempAllocator *temp_allocator = ioContext->mTempAllocator; + + // Check if there's anything to do + uint num_ccd_bodies = ioStep->mNumCCDBodies; + if (num_ccd_bodies > 0) + { + // Sort on fraction so that we process earliest collisions first + // This is needed to make the simulation deterministic and also to be able to stop contact processing + // between body pairs if an earlier hit was found involving the body by another CCD body + // (if it's body ID < this CCD body's body ID - see filtering logic in CCDBroadPhaseCollector) + CCDBody **sorted_ccd_bodies = (CCDBody **)temp_allocator->Allocate(num_ccd_bodies * sizeof(CCDBody *)); + JPH_SCOPE_EXIT([temp_allocator, sorted_ccd_bodies, num_ccd_bodies]{ temp_allocator->Free(sorted_ccd_bodies, num_ccd_bodies * sizeof(CCDBody *)); }); + { + JPH_PROFILE("Sort"); + + // We don't want to copy the entire struct (it's quite big), so we create a pointer array first + CCDBody *src_ccd_bodies = ioStep->mCCDBodies; + CCDBody **dst_ccd_bodies = sorted_ccd_bodies; + CCDBody **dst_ccd_bodies_end = dst_ccd_bodies + num_ccd_bodies; + while (dst_ccd_bodies < dst_ccd_bodies_end) + *(dst_ccd_bodies++) = src_ccd_bodies++; + + // Which we then sort + QuickSort(sorted_ccd_bodies, sorted_ccd_bodies + num_ccd_bodies, [](const CCDBody *inBody1, const CCDBody *inBody2) + { + if (inBody1->mFractionPlusSlop != inBody2->mFractionPlusSlop) + return inBody1->mFractionPlusSlop < inBody2->mFractionPlusSlop; + + return inBody1->mBodyID1 < inBody2->mBodyID1; + }); + } + + // We can collide with bodies that are not active, we track them here so we can activate them in one go at the end. + // This is also needed because we can't modify the active body array while we iterate it. + static constexpr int cBodiesBatch = 64; + BodyID *bodies_to_activate = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID)); + int num_bodies_to_activate = 0; + + // We can move bodies that are not part of an island. In this case we need to notify the broadphase of the movement. + BodyID *bodies_to_update_bounds = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID)); + int num_bodies_to_update_bounds = 0; + + for (uint i = 0; i < num_ccd_bodies; ++i) + { + const CCDBody *ccd_body = sorted_ccd_bodies[i]; + Body &body1 = mBodyManager.GetBody(ccd_body->mBodyID1); + MotionProperties *body_mp = body1.GetMotionProperties(); + + // If there was a hit + if (!ccd_body->mBodyID2.IsInvalid()) + { + Body &body2 = mBodyManager.GetBody(ccd_body->mBodyID2); + + // Determine if the other body has a CCD body + CCDBody *ccd_body2 = sGetCCDBody(body2, ioStep); + if (ccd_body2 != nullptr) + { + JPH_ASSERT(ccd_body2->mBodyID2 != ccd_body->mBodyID1, "If we collided with another body, that other body should have ignored collisions with us!"); + + // Check if the other body found a hit that is further away + if (ccd_body2->mFraction > ccd_body->mFraction) + { + // Reset the colliding body of the other CCD body. The other body will shorten its distance traveled and will not do any collision response (we'll do that). + // This means that at this point we have triggered a contact point add/persist for our further hit by accident for the other body. + // We accept this as calling the contact point callbacks here would require persisting the manifolds up to this point and doing the callbacks single threaded. + ccd_body2->mBodyID2 = BodyID(); + ccd_body2->mFractionPlusSlop = ccd_body->mFraction; + } + } + + // If the other body moved less than us before hitting something, we're not colliding with it so we again have triggered contact point add/persist callbacks by accident. + // We'll just move to the collision position anyway (as that's the last position we know is good), but we won't do any collision response. + if (ccd_body2 == nullptr || ccd_body2->mFraction >= ccd_body->mFraction) + { + const ContactSettings &contact_settings = ccd_body->mContactSettings; + + // Calculate contact point velocity for body 1 + Vec3 r1_plus_u = Vec3(ccd_body->mContactPointOn2 - (body1.GetCenterOfMassPosition() + ccd_body->mFraction * ccd_body->mDeltaPosition)); + Vec3 v1 = body1.GetPointVelocityCOM(r1_plus_u); + + // Calculate inverse mass for body 1 + float inv_m1 = contact_settings.mInvMassScale1 * body_mp->GetInverseMass(); + + if (body2.IsRigidBody()) + { + // Calculate contact point velocity for body 2 + Vec3 r2 = Vec3(ccd_body->mContactPointOn2 - body2.GetCenterOfMassPosition()); + Vec3 v2 = body2.GetPointVelocityCOM(r2); + + // Calculate relative contact velocity + Vec3 relative_velocity = v2 - v1; + float normal_velocity = relative_velocity.Dot(ccd_body->mContactNormal); + + // Calculate velocity bias due to restitution + float normal_velocity_bias; + if (contact_settings.mCombinedRestitution > 0.0f && normal_velocity < -mPhysicsSettings.mMinVelocityForRestitution) + normal_velocity_bias = contact_settings.mCombinedRestitution * normal_velocity; + else + normal_velocity_bias = 0.0f; + + // Get inverse mass of body 2 + float inv_m2 = body2.GetMotionPropertiesUnchecked() != nullptr? contact_settings.mInvMassScale2 * body2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f; + + // Solve contact constraint + AxisConstraintPart contact_constraint; + contact_constraint.CalculateConstraintPropertiesWithMassOverride(body1, inv_m1, contact_settings.mInvInertiaScale1, r1_plus_u, body2, inv_m2, contact_settings.mInvInertiaScale2, r2, ccd_body->mContactNormal, normal_velocity_bias); + contact_constraint.SolveVelocityConstraintWithMassOverride(body1, inv_m1, body2, inv_m2, ccd_body->mContactNormal, -FLT_MAX, FLT_MAX); + + // Apply friction + if (contact_settings.mCombinedFriction > 0.0f) + { + // Calculate friction direction by removing normal velocity from the relative velocity + Vec3 friction_direction = relative_velocity - normal_velocity * ccd_body->mContactNormal; + float friction_direction_len_sq = friction_direction.LengthSq(); + if (friction_direction_len_sq > 1.0e-12f) + { + // Normalize friction direction + friction_direction /= sqrt(friction_direction_len_sq); + + // Calculate max friction impulse + float max_lambda_f = contact_settings.mCombinedFriction * contact_constraint.GetTotalLambda(); + + AxisConstraintPart friction; + friction.CalculateConstraintPropertiesWithMassOverride(body1, inv_m1, contact_settings.mInvInertiaScale1, r1_plus_u, body2, inv_m2, contact_settings.mInvInertiaScale2, r2, friction_direction); + friction.SolveVelocityConstraintWithMassOverride(body1, inv_m1, body2, inv_m2, friction_direction, -max_lambda_f, max_lambda_f); + } + } + + // Clamp velocity of body 2 + if (body2.IsDynamic()) + { + MotionProperties *body2_mp = body2.GetMotionProperties(); + body2_mp->ClampLinearVelocity(); + body2_mp->ClampAngularVelocity(); + } + } + else + { + SoftBodyMotionProperties *soft_mp = static_cast(body2.GetMotionProperties()); + const SoftBodyShape *soft_shape = static_cast(body2.GetShape()); + + // Convert the sub shape ID of the soft body to a face + uint32 face_idx = soft_shape->GetFaceIndex(ccd_body->mSubShapeID2); + const SoftBodyMotionProperties::Face &face = soft_mp->GetFace(face_idx); + + // Get vertices of the face + SoftBodyMotionProperties::Vertex &vtx0 = soft_mp->GetVertex(face.mVertex[0]); + SoftBodyMotionProperties::Vertex &vtx1 = soft_mp->GetVertex(face.mVertex[1]); + SoftBodyMotionProperties::Vertex &vtx2 = soft_mp->GetVertex(face.mVertex[2]); + + // Inverse mass of the face + float vtx0_mass = vtx0.mInvMass > 0.0f? 1.0f / vtx0.mInvMass : 1.0e10f; + float vtx1_mass = vtx1.mInvMass > 0.0f? 1.0f / vtx1.mInvMass : 1.0e10f; + float vtx2_mass = vtx2.mInvMass > 0.0f? 1.0f / vtx2.mInvMass : 1.0e10f; + float inv_m2 = 1.0f / (vtx0_mass + vtx1_mass + vtx2_mass); + + // Calculate barycentric coordinates of the contact point on the soft body's face + float u, v, w; + RMat44 inv_body2_transform = body2.GetInverseCenterOfMassTransform(); + Vec3 local_contact = Vec3(inv_body2_transform * ccd_body->mContactPointOn2); + ClosestPoint::GetBaryCentricCoordinates(vtx0.mPosition - local_contact, vtx1.mPosition - local_contact, vtx2.mPosition - local_contact, u, v, w); + + // Calculate contact point velocity for the face + Vec3 v2 = inv_body2_transform.Multiply3x3Transposed(u * vtx0.mVelocity + v * vtx1.mVelocity + w * vtx2.mVelocity); + float normal_velocity = (v2 - v1).Dot(ccd_body->mContactNormal); + + // Calculate velocity bias due to restitution + float normal_velocity_bias; + if (contact_settings.mCombinedRestitution > 0.0f && normal_velocity < -mPhysicsSettings.mMinVelocityForRestitution) + normal_velocity_bias = contact_settings.mCombinedRestitution * normal_velocity; + else + normal_velocity_bias = 0.0f; + + // Calculate resulting velocity change (the math here is similar to AxisConstraintPart but without an inertia term for body 2 as we treat it as a point mass) + Vec3 r1_plus_u_x_n = r1_plus_u.Cross(ccd_body->mContactNormal); + Vec3 invi1_r1_plus_u_x_n = contact_settings.mInvInertiaScale1 * body1.GetInverseInertia().Multiply3x3(r1_plus_u_x_n); + float jv = r1_plus_u_x_n.Dot(body_mp->GetAngularVelocity()) - normal_velocity - normal_velocity_bias; + float inv_effective_mass = inv_m1 + inv_m2 + invi1_r1_plus_u_x_n.Dot(r1_plus_u_x_n); + float lambda = jv / inv_effective_mass; + body_mp->SubLinearVelocityStep((lambda * inv_m1) * ccd_body->mContactNormal); + body_mp->SubAngularVelocityStep(lambda * invi1_r1_plus_u_x_n); + Vec3 delta_v2 = inv_body2_transform.Multiply3x3(lambda * ccd_body->mContactNormal); + vtx0.mVelocity += delta_v2 * vtx0.mInvMass; + vtx1.mVelocity += delta_v2 * vtx1.mInvMass; + vtx2.mVelocity += delta_v2 * vtx2.mInvMass; + } + + // Clamp velocity of body 1 + body_mp->ClampLinearVelocity(); + body_mp->ClampAngularVelocity(); + + // Activate the 2nd body if it is not already active + if (body2.IsDynamic() && !body2.IsActive()) + { + bodies_to_activate[num_bodies_to_activate++] = ccd_body->mBodyID2; + if (num_bodies_to_activate == cBodiesBatch) + { + // Batch is full, activate now + mBodyManager.ActivateBodies(bodies_to_activate, num_bodies_to_activate); + num_bodies_to_activate = 0; + } + } + + #ifdef JPH_DEBUG_RENDERER + if (sDrawMotionQualityLinearCast) + { + // Draw the collision location + RMat44 collision_transform = body1.GetCenterOfMassTransform().PostTranslated(ccd_body->mFraction * ccd_body->mDeltaPosition); + body1.GetShape()->Draw(DebugRenderer::sInstance, collision_transform, Vec3::sReplicate(1.0f), Color::sYellow, false, true); + + // Draw the collision location + slop + RMat44 collision_transform_plus_slop = body1.GetCenterOfMassTransform().PostTranslated(ccd_body->mFractionPlusSlop * ccd_body->mDeltaPosition); + body1.GetShape()->Draw(DebugRenderer::sInstance, collision_transform_plus_slop, Vec3::sReplicate(1.0f), Color::sOrange, false, true); + + // Draw contact normal + DebugRenderer::sInstance->DrawArrow(ccd_body->mContactPointOn2, ccd_body->mContactPointOn2 - ccd_body->mContactNormal, Color::sYellow, 0.1f); + + // Draw post contact velocity + DebugRenderer::sInstance->DrawArrow(collision_transform.GetTranslation(), collision_transform.GetTranslation() + body1.GetLinearVelocity(), Color::sOrange, 0.1f); + DebugRenderer::sInstance->DrawArrow(collision_transform.GetTranslation(), collision_transform.GetTranslation() + body1.GetAngularVelocity(), Color::sPurple, 0.1f); + } + #endif // JPH_DEBUG_RENDERER + } + } + + // Update body position + body1.AddPositionStep(ccd_body->mDeltaPosition * ccd_body->mFractionPlusSlop); + + // If the body was activated due to an earlier CCD step it will have an index in the active + // body list that it higher than the highest one we processed during FindCollisions + // which means it hasn't been assigned an island and will not be updated by an island + // this means that we need to update its bounds manually + if (body_mp->GetIndexInActiveBodiesInternal() >= num_active_bodies_after_find_collisions) + { + body1.CalculateWorldSpaceBoundsInternal(); + bodies_to_update_bounds[num_bodies_to_update_bounds++] = body1.GetID(); + if (num_bodies_to_update_bounds == cBodiesBatch) + { + // Buffer full, flush now + mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false); + num_bodies_to_update_bounds = 0; + } + } + } + + // Activate the requested bodies + if (num_bodies_to_activate > 0) + mBodyManager.ActivateBodies(bodies_to_activate, num_bodies_to_activate); + + // Notify change bounds on requested bodies + if (num_bodies_to_update_bounds > 0) + mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false); + } + + // Ensure we free the CCD bodies array now, will not call the destructor! + temp_allocator->Free(ioStep->mActiveBodyToCCDBody, ioStep->mNumActiveBodyToCCDBody * sizeof(int)); + ioStep->mActiveBodyToCCDBody = nullptr; + ioStep->mNumActiveBodyToCCDBody = 0; + temp_allocator->Free(ioStep->mCCDBodies, ioStep->mCCDBodiesCapacity * sizeof(CCDBody)); + ioStep->mCCDBodies = nullptr; + ioStep->mCCDBodiesCapacity = 0; +} + +void PhysicsSystem::JobContactRemovedCallbacks(const PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // We don't touch any bodies + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::None); +#endif + + // Reset the Body::EFlags::InvalidateContactCache flag for all bodies + mBodyManager.ValidateContactCacheForAllBodies(); + + // Finalize the contact cache (this swaps the read and write versions of the contact cache) + // Trigger all contact removed callbacks by looking at last step contact points that have not been flagged as reused + mContactManager.FinalizeContactCacheAndCallContactPointRemovedCallbacks(ioStep->mNumBodyPairs, ioStep->mNumManifolds); +} + +class PhysicsSystem::BodiesToSleep : public NonCopyable +{ +public: + static constexpr int cBodiesToSleepSize = 512; + static constexpr int cMaxBodiesToPutInBuffer = 128; + + inline BodiesToSleep(BodyManager &inBodyManager, BodyID *inBodiesToSleepBuffer) : mBodyManager(inBodyManager), mBodiesToSleepBuffer(inBodiesToSleepBuffer), mBodiesToSleepCur(inBodiesToSleepBuffer) { } + + inline ~BodiesToSleep() + { + // Flush the bodies to sleep buffer + int num_bodies_in_buffer = int(mBodiesToSleepCur - mBodiesToSleepBuffer); + if (num_bodies_in_buffer > 0) + mBodyManager.DeactivateBodies(mBodiesToSleepBuffer, num_bodies_in_buffer); + } + + inline void PutToSleep(const BodyID *inBegin, const BodyID *inEnd) + { + int num_bodies_to_sleep = int(inEnd - inBegin); + if (num_bodies_to_sleep > cMaxBodiesToPutInBuffer) + { + // Too many bodies, deactivate immediately + mBodyManager.DeactivateBodies(inBegin, num_bodies_to_sleep); + } + else + { + // Check if there's enough space in the bodies to sleep buffer + int num_bodies_in_buffer = int(mBodiesToSleepCur - mBodiesToSleepBuffer); + if (num_bodies_in_buffer + num_bodies_to_sleep > cBodiesToSleepSize) + { + // Flush the bodies to sleep buffer + mBodyManager.DeactivateBodies(mBodiesToSleepBuffer, num_bodies_in_buffer); + mBodiesToSleepCur = mBodiesToSleepBuffer; + } + + // Copy the bodies in the buffer + memcpy(mBodiesToSleepCur, inBegin, num_bodies_to_sleep * sizeof(BodyID)); + mBodiesToSleepCur += num_bodies_to_sleep; + } + } + +private: + BodyManager & mBodyManager; + BodyID * mBodiesToSleepBuffer; + BodyID * mBodiesToSleepCur; +}; + +void PhysicsSystem::CheckSleepAndUpdateBounds(uint32 inIslandIndex, const PhysicsUpdateContext *ioContext, const PhysicsUpdateContext::Step *ioStep, BodiesToSleep &ioBodiesToSleep) +{ + // Get the bodies that belong to this island + BodyID *bodies_begin, *bodies_end; + mIslandBuilder.GetBodiesInIsland(inIslandIndex, bodies_begin, bodies_end); + + // Only check sleeping in the last step + // Also resets force and torque used during the apply gravity phase + if (ioStep->mIsLast) + { + JPH_PROFILE("Check Sleeping"); + + static_assert(int(ECanSleep::CannotSleep) == 0 && int(ECanSleep::CanSleep) == 1, "Loop below makes this assumption"); + int all_can_sleep = mPhysicsSettings.mAllowSleeping? int(ECanSleep::CanSleep) : int(ECanSleep::CannotSleep); + + float time_before_sleep = mPhysicsSettings.mTimeBeforeSleep; + float max_movement = mPhysicsSettings.mPointVelocitySleepThreshold * time_before_sleep; + + for (const BodyID *body_id = bodies_begin; body_id < bodies_end; ++body_id) + { + Body &body = mBodyManager.GetBody(*body_id); + + // Update bounding box + body.CalculateWorldSpaceBoundsInternal(); + + // Update sleeping + all_can_sleep &= int(body.UpdateSleepStateInternal(ioContext->mStepDeltaTime, max_movement, time_before_sleep)); + + // Reset force and torque + MotionProperties *mp = body.GetMotionProperties(); + mp->ResetForce(); + mp->ResetTorque(); + } + + // If all bodies indicate they can sleep we can deactivate them + if (all_can_sleep == int(ECanSleep::CanSleep)) + ioBodiesToSleep.PutToSleep(bodies_begin, bodies_end); + } + else + { + JPH_PROFILE("Update Bounds"); + + // Update bounding box only for all other steps + for (const BodyID *body_id = bodies_begin; body_id < bodies_end; ++body_id) + { + Body &body = mBodyManager.GetBody(*body_id); + body.CalculateWorldSpaceBoundsInternal(); + } + } + + // Notify broadphase of changed objects (find ccd contacts can do linear casts in the next step, so we need to do this every step) + // Note: Shuffles the BodyID's around!!! + mBroadPhase->NotifyBodiesAABBChanged(bodies_begin, int(bodies_end - bodies_begin), false); +} + +void PhysicsSystem::JobSolvePositionConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // We fix up position errors + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::ReadWrite); + + // Can only deactivate bodies + BodyManager::GrantActiveBodiesAccess grant_active(false, true); +#endif + + float delta_time = ioContext->mStepDeltaTime; + float baumgarte = mPhysicsSettings.mBaumgarte; + Constraint **active_constraints = ioContext->mActiveConstraints; + + // Keep a buffer of bodies that need to go to sleep in order to not constantly lock the active bodies mutex and create contention between all solving threads + BodiesToSleep bodies_to_sleep(mBodyManager, (BodyID *)JPH_STACK_ALLOC(BodiesToSleep::cBodiesToSleepSize * sizeof(BodyID))); + + bool check_islands = true, check_split_islands = mPhysicsSettings.mUseLargeIslandSplitter; + for (;;) + { + // First try to get work from large islands + if (check_split_islands) + { + bool first_iteration; + uint split_island_index; + uint32 *constraints_begin, *constraints_end, *contacts_begin, *contacts_end; + switch (mLargeIslandSplitter.FetchNextBatch(split_island_index, constraints_begin, constraints_end, contacts_begin, contacts_end, first_iteration)) + { + case LargeIslandSplitter::EStatus::BatchRetrieved: + // Solve the batch + ConstraintManager::sSolvePositionConstraints(active_constraints, constraints_begin, constraints_end, delta_time, baumgarte); + mContactManager.SolvePositionConstraints(contacts_begin, contacts_end); + + // Mark the batch as processed + bool last_iteration, final_batch; + mLargeIslandSplitter.MarkBatchProcessed(split_island_index, constraints_begin, constraints_end, contacts_begin, contacts_end, last_iteration, final_batch); + + // The final batch will update all bounds and check sleeping + if (final_batch) + CheckSleepAndUpdateBounds(mLargeIslandSplitter.GetIslandIndex(split_island_index), ioContext, ioStep, bodies_to_sleep); + + // We processed work, loop again + continue; + + case LargeIslandSplitter::EStatus::WaitingForBatch: + break; + + case LargeIslandSplitter::EStatus::AllBatchesDone: + check_split_islands = false; + break; + } + } + + // If that didn't succeed try to process an island + if (check_islands) + { + // Next island + uint32 island_idx = ioStep->mSolvePositionConstraintsNextIsland++; + if (island_idx >= mIslandBuilder.GetNumIslands()) + { + // We processed all islands, stop checking islands + check_islands = false; + continue; + } + + JPH_PROFILE("Island"); + + // Get iterators for this island + uint32 *constraints_begin, *constraints_end, *contacts_begin, *contacts_end; + mIslandBuilder.GetConstraintsInIsland(island_idx, constraints_begin, constraints_end); + mIslandBuilder.GetContactsInIsland(island_idx, contacts_begin, contacts_end); + + // If this island is a large island, it will be picked up as a batch and we don't need to do anything here + uint num_items = uint(constraints_end - constraints_begin) + uint(contacts_end - contacts_begin); + if (mPhysicsSettings.mUseLargeIslandSplitter + && num_items >= LargeIslandSplitter::cLargeIslandTreshold) + continue; + + // Check if this island needs solving + if (num_items > 0) + { + // Iterate + uint num_position_steps = mIslandBuilder.GetNumPositionSteps(island_idx); + for (uint position_step = 0; position_step < num_position_steps; ++position_step) + { + bool applied_impulse = ConstraintManager::sSolvePositionConstraints(active_constraints, constraints_begin, constraints_end, delta_time, baumgarte); + applied_impulse |= mContactManager.SolvePositionConstraints(contacts_begin, contacts_end); + if (!applied_impulse) + break; + } + } + + // After solving we will update all bounds and check sleeping + CheckSleepAndUpdateBounds(island_idx, ioContext, ioStep, bodies_to_sleep); + + // We processed work, loop again + continue; + } + + if (check_islands) + { + // If there are islands, we don't need to wait and can pick up new work + continue; + } + else if (check_split_islands) + { + // If there are split islands, but we did't do any work, give up a time slice + std::this_thread::yield(); + } + else + { + // No more work + break; + } + } +} + +void PhysicsSystem::JobSoftBodyPrepare(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ + JPH_PROFILE_FUNCTION(); + + { + #ifdef JPH_ENABLE_ASSERTS + // Reading soft body positions + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::Read); + #endif + + // Get the active soft bodies + BodyIDVector active_bodies; + mBodyManager.GetActiveBodies(EBodyType::SoftBody, active_bodies); + + // Quit if there are no active soft bodies + if (active_bodies.empty()) + { + // Kick the next step + if (ioStep->mStartNextStep.IsValid()) + ioStep->mStartNextStep.RemoveDependency(); + return; + } + + // Sort to get a deterministic update order + QuickSort(active_bodies.begin(), active_bodies.end()); + + // Allocate soft body contexts + ioContext->mNumSoftBodies = (uint)active_bodies.size(); + ioContext->mSoftBodyUpdateContexts = (SoftBodyUpdateContext *)ioContext->mTempAllocator->Allocate(ioContext->mNumSoftBodies * sizeof(SoftBodyUpdateContext)); + + // Initialize soft body contexts + for (SoftBodyUpdateContext *sb_ctx = ioContext->mSoftBodyUpdateContexts, *sb_ctx_end = ioContext->mSoftBodyUpdateContexts + ioContext->mNumSoftBodies; sb_ctx < sb_ctx_end; ++sb_ctx) + { + new (sb_ctx) SoftBodyUpdateContext; + Body &body = mBodyManager.GetBody(active_bodies[sb_ctx - ioContext->mSoftBodyUpdateContexts]); + SoftBodyMotionProperties *mp = static_cast(body.GetMotionProperties()); + mp->InitializeUpdateContext(ioContext->mStepDeltaTime, body, *this, *sb_ctx); + } + } + + // We're ready to collide the first soft body + ioContext->mSoftBodyToCollide.store(0, memory_order_release); + + // Determine number of jobs to spawn + int num_soft_body_jobs = ioContext->GetMaxConcurrency(); + + // Create finalize job + ioStep->mSoftBodyFinalize = ioContext->mJobSystem->CreateJob("SoftBodyFinalize", cColorSoftBodyFinalize, [ioContext, ioStep]() + { + ioContext->mPhysicsSystem->JobSoftBodyFinalize(ioContext); + + // Kick the next step + if (ioStep->mStartNextStep.IsValid()) + ioStep->mStartNextStep.RemoveDependency(); + }, num_soft_body_jobs); // depends on: soft body simulate + ioContext->mBarrier->AddJob(ioStep->mSoftBodyFinalize); + + // Create simulate jobs + ioStep->mSoftBodySimulate.resize(num_soft_body_jobs); + for (int i = 0; i < num_soft_body_jobs; ++i) + ioStep->mSoftBodySimulate[i] = ioContext->mJobSystem->CreateJob("SoftBodySimulate", cColorSoftBodySimulate, [ioStep, i]() + { + ioStep->mContext->mPhysicsSystem->JobSoftBodySimulate(ioStep->mContext, i); + + ioStep->mSoftBodyFinalize.RemoveDependency(); + }, num_soft_body_jobs); // depends on: soft body collide + ioContext->mBarrier->AddJobs(ioStep->mSoftBodySimulate.data(), ioStep->mSoftBodySimulate.size()); + + // Create collision jobs + ioStep->mSoftBodyCollide.resize(num_soft_body_jobs); + for (int i = 0; i < num_soft_body_jobs; ++i) + ioStep->mSoftBodyCollide[i] = ioContext->mJobSystem->CreateJob("SoftBodyCollide", cColorSoftBodyCollide, [ioContext, ioStep]() + { + ioContext->mPhysicsSystem->JobSoftBodyCollide(ioContext); + + for (const JobHandle &h : ioStep->mSoftBodySimulate) + h.RemoveDependency(); + }); // depends on: nothing + ioContext->mBarrier->AddJobs(ioStep->mSoftBodyCollide.data(), ioStep->mSoftBodyCollide.size()); +} + +void PhysicsSystem::JobSoftBodyCollide(PhysicsUpdateContext *ioContext) const +{ +#ifdef JPH_ENABLE_ASSERTS + // Reading rigid body positions and velocities + BodyAccess::Grant grant(BodyAccess::EAccess::Read, BodyAccess::EAccess::Read); +#endif + + for (;;) + { + // Fetch the next soft body + uint sb_idx = ioContext->mSoftBodyToCollide.fetch_add(1, std::memory_order_acquire); + if (sb_idx >= ioContext->mNumSoftBodies) + break; + + // Do a broadphase check + SoftBodyUpdateContext &sb_ctx = ioContext->mSoftBodyUpdateContexts[sb_idx]; + sb_ctx.mMotionProperties->DetermineCollidingShapes(sb_ctx, *this, GetBodyLockInterfaceNoLock()); + } +} + +void PhysicsSystem::JobSoftBodySimulate(PhysicsUpdateContext *ioContext, uint inThreadIndex) const +{ +#ifdef JPH_ENABLE_ASSERTS + // Updating velocities of soft bodies, allow the contact listener to read the soft body state + BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::Read); +#endif + + // Calculate at which body we start to distribute the workload across the threads + uint num_soft_bodies = ioContext->mNumSoftBodies; + uint start_idx = inThreadIndex * num_soft_bodies / ioContext->GetMaxConcurrency(); + + // Keep running partial updates until everything has been updated + uint status; + do + { + // Reset status + status = 0; + + // Update all soft bodies + for (uint i = 0; i < num_soft_bodies; ++i) + { + // Fetch the soft body context + SoftBodyUpdateContext &sb_ctx = ioContext->mSoftBodyUpdateContexts[(start_idx + i) % num_soft_bodies]; + + // To avoid trashing the cache too much, we prefer to stick to one soft body until we cannot progress it any further + uint sb_status; + do + { + sb_status = (uint)sb_ctx.mMotionProperties->ParallelUpdate(sb_ctx, mPhysicsSettings); + status |= sb_status; + } while (sb_status == (uint)SoftBodyMotionProperties::EStatus::DidWork); + } + + // If we didn't perform any work, yield the thread so that something else can run + if (!(status & (uint)SoftBodyMotionProperties::EStatus::DidWork)) + std::this_thread::yield(); + } + while (status != (uint)SoftBodyMotionProperties::EStatus::Done); +} + +void PhysicsSystem::JobSoftBodyFinalize(PhysicsUpdateContext *ioContext) +{ +#ifdef JPH_ENABLE_ASSERTS + // Updating rigid body velocities and soft body positions / velocities + BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::ReadWrite); + + // Can activate and deactivate bodies + BodyManager::GrantActiveBodiesAccess grant_active(true, true); +#endif + + static constexpr int cBodiesBatch = 64; + BodyID *bodies_to_update_bounds = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID)); + int num_bodies_to_update_bounds = 0; + BodyID *bodies_to_put_to_sleep = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID)); + int num_bodies_to_put_to_sleep = 0; + + for (SoftBodyUpdateContext *sb_ctx = ioContext->mSoftBodyUpdateContexts, *sb_ctx_end = ioContext->mSoftBodyUpdateContexts + ioContext->mNumSoftBodies; sb_ctx < sb_ctx_end; ++sb_ctx) + { + // Apply the rigid body velocity deltas + sb_ctx->mMotionProperties->UpdateRigidBodyVelocities(*sb_ctx, GetBodyInterfaceNoLock()); + + // Update the position + sb_ctx->mBody->SetPositionAndRotationInternal(sb_ctx->mBody->GetPosition() + sb_ctx->mDeltaPosition, sb_ctx->mBody->GetRotation(), false); + + BodyID id = sb_ctx->mBody->GetID(); + bodies_to_update_bounds[num_bodies_to_update_bounds++] = id; + if (num_bodies_to_update_bounds == cBodiesBatch) + { + // Buffer full, flush now + mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false); + num_bodies_to_update_bounds = 0; + } + + if (sb_ctx->mCanSleep == ECanSleep::CanSleep) + { + // This body should go to sleep + bodies_to_put_to_sleep[num_bodies_to_put_to_sleep++] = id; + if (num_bodies_to_put_to_sleep == cBodiesBatch) + { + mBodyManager.DeactivateBodies(bodies_to_put_to_sleep, num_bodies_to_put_to_sleep); + num_bodies_to_put_to_sleep = 0; + } + } + } + + // Notify change bounds on requested bodies + if (num_bodies_to_update_bounds > 0) + mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false); + + // Notify bodies to go to sleep + if (num_bodies_to_put_to_sleep > 0) + mBodyManager.DeactivateBodies(bodies_to_put_to_sleep, num_bodies_to_put_to_sleep); + + // Free soft body contexts + ioContext->mTempAllocator->Free(ioContext->mSoftBodyUpdateContexts, ioContext->mNumSoftBodies * sizeof(SoftBodyUpdateContext)); +} + +void PhysicsSystem::SaveState(StateRecorder &inStream, EStateRecorderState inState, const StateRecorderFilter *inFilter) const +{ + JPH_PROFILE_FUNCTION(); + + inStream.Write(inState); + + if (uint8(inState) & uint8(EStateRecorderState::Global)) + { + inStream.Write(mPreviousStepDeltaTime); + inStream.Write(mGravity); + } + + if (uint8(inState) & uint8(EStateRecorderState::Bodies)) + mBodyManager.SaveState(inStream, inFilter); + + if (uint8(inState) & uint8(EStateRecorderState::Contacts)) + mContactManager.SaveState(inStream, inFilter); + + if (uint8(inState) & uint8(EStateRecorderState::Constraints)) + mConstraintManager.SaveState(inStream, inFilter); +} + +bool PhysicsSystem::RestoreState(StateRecorder &inStream, const StateRecorderFilter *inFilter) +{ + JPH_PROFILE_FUNCTION(); + + EStateRecorderState state = EStateRecorderState::All; // Set this value for validation. If a partial state is saved, validation will not work anyway. + inStream.Read(state); + + if (uint8(state) & uint8(EStateRecorderState::Global)) + { + inStream.Read(mPreviousStepDeltaTime); + inStream.Read(mGravity); + } + + if (uint8(state) & uint8(EStateRecorderState::Bodies)) + { + if (!mBodyManager.RestoreState(inStream)) + return false; + + // Update bounding boxes for all bodies in the broadphase + if (inStream.IsLastPart()) + { + Array bodies; + for (const Body *b : mBodyManager.GetBodies()) + if (BodyManager::sIsValidBodyPointer(b) && b->IsInBroadPhase()) + bodies.push_back(b->GetID()); + if (!bodies.empty()) + mBroadPhase->NotifyBodiesAABBChanged(&bodies[0], (int)bodies.size()); + } + } + + if (uint8(state) & uint8(EStateRecorderState::Contacts)) + { + if (!mContactManager.RestoreState(inStream, inFilter)) + return false; + } + + if (uint8(state) & uint8(EStateRecorderState::Constraints)) + { + if (!mConstraintManager.RestoreState(inStream)) + return false; + } + + return true; +} + +void PhysicsSystem::SaveBodyState(const Body &inBody, StateRecorder &inStream) const +{ + mBodyManager.SaveBodyState(inBody, inStream); +} + +void PhysicsSystem::RestoreBodyState(Body &ioBody, StateRecorder &inStream) +{ + mBodyManager.RestoreBodyState(ioBody, inStream); + + BodyID id = ioBody.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.h b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.h new file mode 100644 index 000000000000..5b1d1acf54ca --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.h @@ -0,0 +1,338 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class JobSystem; +class StateRecorder; +class TempAllocator; +class PhysicsStepListener; +class SoftBodyContactListener; +class SimShapeFilter; + +/// The main class for the physics system. It contains all rigid bodies and simulates them. +/// +/// The main simulation is performed by the Update() call on multiple threads (if the JobSystem is configured to use them). Please refer to the general architecture overview in the Docs folder for more information. +class JPH_EXPORT PhysicsSystem : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor / Destructor + PhysicsSystem() : mContactManager(mPhysicsSettings) JPH_IF_ENABLE_ASSERTS(, mConstraintManager(&mBodyManager)) { } + ~PhysicsSystem(); + + /// Initialize the system. + /// @param inMaxBodies Maximum number of bodies to support. + /// @param inNumBodyMutexes Number of body mutexes to use. Should be a power of 2 in the range [1, 64], use 0 to auto detect. + /// @param inMaxBodyPairs Maximum amount of body pairs to process (anything else will fall through the world), this number should generally be much higher than the max amount of contact points as there will be lots of bodies close that are not actually touching. + /// @param inMaxContactConstraints Maximum amount of contact constraints to process (anything else will fall through the world). + /// @param inBroadPhaseLayerInterface Information on the mapping of object layers to broad phase layers. Since this is a virtual interface, the instance needs to stay alive during the lifetime of the PhysicsSystem. + /// @param inObjectVsBroadPhaseLayerFilter Filter callback function that is used to determine if an object layer collides with a broad phase layer. Since this is a virtual interface, the instance needs to stay alive during the lifetime of the PhysicsSystem. + /// @param inObjectLayerPairFilter Filter callback function that is used to determine if two object layers collide. Since this is a virtual interface, the instance needs to stay alive during the lifetime of the PhysicsSystem. + void Init(uint inMaxBodies, uint inNumBodyMutexes, uint inMaxBodyPairs, uint inMaxContactConstraints, const BroadPhaseLayerInterface &inBroadPhaseLayerInterface, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter); + + /// Listener that is notified whenever a body is activated/deactivated + void SetBodyActivationListener(BodyActivationListener *inListener) { mBodyManager.SetBodyActivationListener(inListener); } + BodyActivationListener * GetBodyActivationListener() const { return mBodyManager.GetBodyActivationListener(); } + + /// Listener that is notified whenever a contact point between two bodies is added/updated/removed. + /// You can't change contact listener during PhysicsSystem::Update but it can be changed at any other time. + void SetContactListener(ContactListener *inListener) { mContactManager.SetContactListener(inListener); } + ContactListener * GetContactListener() const { return mContactManager.GetContactListener(); } + + /// Listener that is notified whenever a contact point between a soft body and another body + void SetSoftBodyContactListener(SoftBodyContactListener *inListener) { mSoftBodyContactListener = inListener; } + SoftBodyContactListener * GetSoftBodyContactListener() const { return mSoftBodyContactListener; } + + /// Set the function that combines the friction of two bodies and returns it + /// Default method is the geometric mean: sqrt(friction1 * friction2). + void SetCombineFriction(ContactConstraintManager::CombineFunction inCombineFriction) { mContactManager.SetCombineFriction(inCombineFriction); } + ContactConstraintManager::CombineFunction GetCombineFriction() const { return mContactManager.GetCombineFriction(); } + + /// Set the function that combines the restitution of two bodies and returns it + /// Default method is max(restitution1, restitution1) + void SetCombineRestitution(ContactConstraintManager::CombineFunction inCombineRestition) { mContactManager.SetCombineRestitution(inCombineRestition); } + ContactConstraintManager::CombineFunction GetCombineRestitution() const { return mContactManager.GetCombineRestitution(); } + + /// Set/get the shape filter that will be used during simulation. This can be used to exclude shapes within a body from colliding with each other. + /// E.g. if you have a high detail and a low detail collision model, you can attach them to the same body in a StaticCompoundShape and use the ShapeFilter + /// to exclude the high detail collision model when simulating and exclude the low detail collision model when casting rays. Note that in this case + /// you would need to pass the inverse of inShapeFilter to the CastRay function. Pass a nullptr to disable the shape filter. + /// The PhysicsSystem does not own the ShapeFilter, make sure it stays alive during the lifetime of the PhysicsSystem. + void SetSimShapeFilter(const SimShapeFilter *inShapeFilter) { mSimShapeFilter = inShapeFilter; } + const SimShapeFilter * GetSimShapeFilter() const { return mSimShapeFilter; } + + /// Control the main constants of the physics simulation + void SetPhysicsSettings(const PhysicsSettings &inSettings) { mPhysicsSettings = inSettings; } + const PhysicsSettings & GetPhysicsSettings() const { return mPhysicsSettings; } + + /// Access to the body interface. This interface allows to to create / remove bodies and to change their properties. + const BodyInterface & GetBodyInterface() const { return mBodyInterfaceLocking; } + BodyInterface & GetBodyInterface() { return mBodyInterfaceLocking; } + const BodyInterface & GetBodyInterfaceNoLock() const { return mBodyInterfaceNoLock; } ///< Version that does not lock the bodies, use with great care! + BodyInterface & GetBodyInterfaceNoLock() { return mBodyInterfaceNoLock; } ///< Version that does not lock the bodies, use with great care! + + /// Access to the broadphase interface that allows coarse collision queries + const BroadPhaseQuery & GetBroadPhaseQuery() const { return *mBroadPhase; } + + /// Interface that allows fine collision queries against first the broad phase and then the narrow phase. + const NarrowPhaseQuery & GetNarrowPhaseQuery() const { return mNarrowPhaseQueryLocking; } + const NarrowPhaseQuery & GetNarrowPhaseQueryNoLock() const { return mNarrowPhaseQueryNoLock; } ///< Version that does not lock the bodies, use with great care! + + /// Add constraint to the world + void AddConstraint(Constraint *inConstraint) { mConstraintManager.Add(&inConstraint, 1); } + + /// Remove constraint from the world + void RemoveConstraint(Constraint *inConstraint) { mConstraintManager.Remove(&inConstraint, 1); } + + /// Batch add constraints. + void AddConstraints(Constraint **inConstraints, int inNumber) { mConstraintManager.Add(inConstraints, inNumber); } + + /// Batch remove constraints. + void RemoveConstraints(Constraint **inConstraints, int inNumber) { mConstraintManager.Remove(inConstraints, inNumber); } + + /// Get a list of all constraints + Constraints GetConstraints() const { return mConstraintManager.GetConstraints(); } + + /// Optimize the broadphase, needed only if you've added many bodies prior to calling Update() for the first time. + /// Don't call this every frame as PhysicsSystem::Update spreads out the same work over multiple frames. + /// If you add many bodies through BodyInterface::AddBodiesPrepare/AddBodiesFinalize and if the bodies in a batch are + /// in a roughly unoccupied space (e.g. a new level section) then a call to OptimizeBroadPhase is also not needed + /// as batch adding creates an efficient bounding volume hierarchy. + /// Don't call this function while bodies are being modified from another thread or use the locking BodyInterface to modify bodies. + void OptimizeBroadPhase(); + + /// Adds a new step listener + void AddStepListener(PhysicsStepListener *inListener); + + /// Removes a step listener + void RemoveStepListener(PhysicsStepListener *inListener); + + /// Simulate the system. + /// The world steps for a total of inDeltaTime seconds. This is divided in inCollisionSteps iterations. + /// Each iteration consists of collision detection followed by an integration step. + /// This function internally spawns jobs using inJobSystem and waits for them to complete, so no jobs will be running when this function returns. + EPhysicsUpdateError Update(float inDeltaTime, int inCollisionSteps, TempAllocator *inTempAllocator, JobSystem *inJobSystem); + + /// Saving state for replay + void SaveState(StateRecorder &inStream, EStateRecorderState inState = EStateRecorderState::All, const StateRecorderFilter *inFilter = nullptr) const; + + /// Restoring state for replay. Returns false if failed. + bool RestoreState(StateRecorder &inStream, const StateRecorderFilter *inFilter = nullptr); + + /// Saving state of a single body. + void SaveBodyState(const Body &inBody, StateRecorder &inStream) const; + + /// Restoring state of a single body. + void RestoreBodyState(Body &ioBody, StateRecorder &inStream); + +#ifdef JPH_DEBUG_RENDERER + // Drawing properties + static bool sDrawMotionQualityLinearCast; ///< Draw debug info for objects that perform continuous collision detection through the linear cast motion quality + + /// Draw the state of the bodies (debugging purposes) + void DrawBodies(const BodyManager::DrawSettings &inSettings, DebugRenderer *inRenderer, const BodyDrawFilter *inBodyFilter = nullptr) { mBodyManager.Draw(inSettings, mPhysicsSettings, inRenderer, inBodyFilter); } + + /// Draw the constraints only (debugging purposes) + void DrawConstraints(DebugRenderer *inRenderer) { mConstraintManager.DrawConstraints(inRenderer); } + + /// Draw the constraint limits only (debugging purposes) + void DrawConstraintLimits(DebugRenderer *inRenderer) { mConstraintManager.DrawConstraintLimits(inRenderer); } + + /// Draw the constraint reference frames only (debugging purposes) + void DrawConstraintReferenceFrame(DebugRenderer *inRenderer) { mConstraintManager.DrawConstraintReferenceFrame(inRenderer); } +#endif // JPH_DEBUG_RENDERER + + /// Set gravity value + void SetGravity(Vec3Arg inGravity) { mGravity = inGravity; } + Vec3 GetGravity() const { return mGravity; } + + /// Returns a locking interface that won't actually lock the body. Use with great care! + inline const BodyLockInterfaceNoLock & GetBodyLockInterfaceNoLock() const { return mBodyLockInterfaceNoLock; } + + /// Returns a locking interface that locks the body so other threads cannot modify it. + inline const BodyLockInterfaceLocking & GetBodyLockInterface() const { return mBodyLockInterfaceLocking; } + + /// 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); } + + /// Get an object layer filter that uses the default pair filter and a specified layer to determine if layers collide + DefaultObjectLayerFilter GetDefaultLayerFilter(ObjectLayer inLayer) const { return DefaultObjectLayerFilter(*mObjectLayerPairFilter, inLayer); } + + /// Gets the current amount of bodies that are in the body manager + uint GetNumBodies() const { return mBodyManager.GetNumBodies(); } + + /// Gets the current amount of active bodies that are in the body manager + uint32 GetNumActiveBodies(EBodyType inType) const { return mBodyManager.GetNumActiveBodies(inType); } + + /// Get the maximum amount of bodies that this physics system supports + uint GetMaxBodies() const { return mBodyManager.GetMaxBodies(); } + + /// Helper struct that counts the number of bodies of each type + using BodyStats = BodyManager::BodyStats; + + /// Get stats about the bodies in the body manager (slow, iterates through all bodies) + BodyStats GetBodyStats() const { return mBodyManager.GetBodyStats(); } + + /// Get copy of the list of all bodies under protection of a lock. + /// @param outBodyIDs On return, this will contain the list of BodyIDs + void GetBodies(BodyIDVector &outBodyIDs) const { return mBodyManager.GetBodyIDs(outBodyIDs); } + + /// Get copy of the list of active bodies under protection of a lock. + /// @param inType The type of bodies to get + /// @param outBodyIDs On return, this will contain the list of BodyIDs + void GetActiveBodies(EBodyType inType, BodyIDVector &outBodyIDs) const { return mBodyManager.GetActiveBodies(inType, outBodyIDs); } + + /// Get the list of active bodies, use GetNumActiveBodies() to find out how long the list is. + /// Note: Not thread safe. The active bodies list can change at any moment when other threads are doing work. Use GetActiveBodies() if you need a thread safe version. + const BodyID * GetActiveBodiesUnsafe(EBodyType inType) const { return mBodyManager.GetActiveBodiesUnsafe(inType); } + + /// Check if 2 bodies were in contact during the last simulation step. Since contacts are only detected between active bodies, so at least one of the bodies must be active in order for this function to work. + /// It queries the state at the time of the last PhysicsSystem::Update and will return true if the bodies were in contact, even if one of the bodies was moved / removed afterwards. + /// This function can be called from any thread when the PhysicsSystem::Update is not running. During PhysicsSystem::Update this function is only valid during contact callbacks: + /// - During the ContactListener::OnContactAdded callback this function can be used to determine if a different contact pair between the bodies was active in the previous simulation step (function returns true) or if this is the first step that the bodies are touching (function returns false). + /// - During the ContactListener::OnContactRemoved callback this function can be used to determine if this is the last contact pair between the bodies (function returns false) or if there are other contacts still present (function returns true). + bool WereBodiesInContact(const BodyID &inBody1ID, const BodyID &inBody2ID) const { return mContactManager.WereBodiesInContact(inBody1ID, inBody2ID); } + + /// Get the bounding box of all bodies in the physics system + AABox GetBounds() const { return mBroadPhase->GetBounds(); } + +#ifdef JPH_TRACK_BROADPHASE_STATS + /// Trace the accumulated broadphase stats to the TTY + void ReportBroadphaseStats() { mBroadPhase->ReportStats(); } +#endif // JPH_TRACK_BROADPHASE_STATS + +private: + using CCDBody = PhysicsUpdateContext::Step::CCDBody; + + // Various job entry points + void JobStepListeners(PhysicsUpdateContext::Step *ioStep); + void JobDetermineActiveConstraints(PhysicsUpdateContext::Step *ioStep) const; + void JobApplyGravity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobSetupVelocityConstraints(float inDeltaTime, PhysicsUpdateContext::Step *ioStep) const; + void JobBuildIslandsFromConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int inJobIndex); + void JobFinalizeIslands(PhysicsUpdateContext *ioContext); + void JobBodySetIslandIndex(); + void JobSolveVelocityConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobPreIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobIntegrateVelocity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobPostIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) const; + void JobFindCCDContacts(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobResolveCCDContacts(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobContactRemovedCallbacks(const PhysicsUpdateContext::Step *ioStep); + void JobSolvePositionConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobSoftBodyPrepare(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobSoftBodyCollide(PhysicsUpdateContext *ioContext) const; + void JobSoftBodySimulate(PhysicsUpdateContext *ioContext, uint inThreadIndex) const; + void JobSoftBodyFinalize(PhysicsUpdateContext *ioContext); + + /// Tries to spawn a new FindCollisions job if max concurrency hasn't been reached yet + void TrySpawnJobFindCollisions(PhysicsUpdateContext::Step *ioStep) const; + + using ContactAllocator = ContactConstraintManager::ContactAllocator; + + /// Process narrow phase for a single body pair + void ProcessBodyPair(ContactAllocator &ioContactAllocator, const BodyPair &inBodyPair); + + /// This helper batches up bodies that need to put to sleep to avoid contention on the activation mutex + class BodiesToSleep; + + /// Called at the end of JobSolveVelocityConstraints to check if bodies need to go to sleep and to update their bounding box in the broadphase + void CheckSleepAndUpdateBounds(uint32 inIslandIndex, const PhysicsUpdateContext *ioContext, const PhysicsUpdateContext::Step *ioStep, BodiesToSleep &ioBodiesToSleep); + + /// Number of constraints to process at once in JobDetermineActiveConstraints + static constexpr int cDetermineActiveConstraintsBatchSize = 64; + + /// Number of constraints to process at once in JobSetupVelocityConstraints, we want a low number of threads working on this so we take fairly large batches + static constexpr int cSetupVelocityConstraintsBatchSize = 256; + + /// Number of bodies to process at once in JobApplyGravity + static constexpr int cApplyGravityBatchSize = 64; + + /// Number of active bodies to test for collisions per batch + static constexpr int cActiveBodiesBatchSize = 16; + + /// Number of active bodies to integrate velocities for + static constexpr int cIntegrateVelocityBatchSize = 64; + + /// Number of contacts that need to be queued before another narrow phase job is started + static constexpr int cNarrowPhaseBatchSize = 16; + + /// Number of continuous collision shape casts that need to be queued before another job is started + static constexpr int cNumCCDBodiesPerJob = 4; + + /// Broadphase layer filter that decides if two objects can collide + const ObjectVsBroadPhaseLayerFilter *mObjectVsBroadPhaseLayerFilter = nullptr; + + /// Object layer filter that decides if two objects can collide + const ObjectLayerPairFilter *mObjectLayerPairFilter = nullptr; + + /// The body manager keeps track which bodies are in the simulation + BodyManager mBodyManager; + + /// Body locking interfaces + BodyLockInterfaceNoLock mBodyLockInterfaceNoLock { mBodyManager }; + BodyLockInterfaceLocking mBodyLockInterfaceLocking { mBodyManager }; + + /// Body interfaces + BodyInterface mBodyInterfaceNoLock; + BodyInterface mBodyInterfaceLocking; + + /// Narrow phase query interface + NarrowPhaseQuery mNarrowPhaseQueryNoLock; + NarrowPhaseQuery mNarrowPhaseQueryLocking; + + /// The broadphase does quick collision detection between body pairs + BroadPhase * mBroadPhase = nullptr; + + /// The soft body contact listener + SoftBodyContactListener * mSoftBodyContactListener = nullptr; + + /// The shape filter that is used to filter out sub shapes during simulation + const SimShapeFilter * mSimShapeFilter = nullptr; + + /// Simulation settings + PhysicsSettings mPhysicsSettings; + + /// The contact manager resolves all contacts during a simulation step + ContactConstraintManager mContactManager; + + /// All non-contact constraints + ConstraintManager mConstraintManager; + + /// Keeps track of connected bodies and builds islands for multithreaded velocity/position update + IslandBuilder mIslandBuilder; + + /// Will split large islands into smaller groups of bodies that can be processed in parallel + LargeIslandSplitter mLargeIslandSplitter; + + /// Mutex protecting mStepListeners + Mutex mStepListenersMutex; + + /// List of physics step listeners + using StepListeners = Array; + StepListeners mStepListeners; + + /// This is the global gravity vector + Vec3 mGravity = Vec3(0, -9.81f, 0); + + /// Previous frame's delta time of one sub step to allow scaling previous frame's constraint impulses + float mPreviousStepDeltaTime = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.cpp b/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.cpp new file mode 100644 index 000000000000..7c60ae6be66c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.cpp @@ -0,0 +1,23 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +PhysicsUpdateContext::PhysicsUpdateContext(TempAllocator &inTempAllocator) : + mTempAllocator(&inTempAllocator), + mSteps(inTempAllocator) +{ +} + +PhysicsUpdateContext::~PhysicsUpdateContext() +{ + JPH_ASSERT(mBodyPairs == nullptr); + JPH_ASSERT(mActiveConstraints == nullptr); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.h b/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.h new file mode 100644 index 000000000000..fe99c46cccf6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.h @@ -0,0 +1,172 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; +class IslandBuilder; +class Constraint; +class TempAllocator; +class SoftBodyUpdateContext; + +/// Information used during the Update call +class PhysicsUpdateContext : public NonCopyable +{ +public: + /// Destructor + explicit PhysicsUpdateContext(TempAllocator &inTempAllocator); + ~PhysicsUpdateContext(); + + static constexpr int cMaxConcurrency = 32; ///< Maximum supported amount of concurrent jobs + + using JobHandleArray = StaticArray; + + struct Step; + + struct BodyPairQueue + { + atomic mWriteIdx { 0 }; ///< Next index to write in mBodyPair array (need to add thread index * mMaxBodyPairsPerQueue and modulo mMaxBodyPairsPerQueue) + uint8 mPadding1[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Moved to own cache line to avoid conflicts with consumer jobs + + atomic mReadIdx { 0 }; ///< Next index to read in mBodyPair array (need to add thread index * mMaxBodyPairsPerQueue and modulo mMaxBodyPairsPerQueue) + uint8 mPadding2[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Moved to own cache line to avoid conflicts with producer/consumer jobs + }; + + using BodyPairQueues = StaticArray; + + using JobMask = uint32; ///< A mask that has as many bits as we can have concurrent jobs + static_assert(sizeof(JobMask) * 8 >= cMaxConcurrency); + + /// Structure that contains data needed for each collision step. + struct Step + { + Step() = default; + Step(const Step &) { JPH_ASSERT(false); } // vector needs a copy constructor, but we're never going to call it + + PhysicsUpdateContext *mContext; ///< The physics update context + + bool mIsFirst; ///< If this is the first step + bool mIsLast; ///< If this is the last step + + BroadPhase::UpdateState mBroadPhaseUpdateState; ///< Handle returned by Broadphase::UpdatePrepare + + uint32 mNumActiveBodiesAtStepStart; ///< Number of bodies that were active at the start of the physics update step. Only these bodies will receive gravity (they are the first N in the active body list). + + atomic mDetermineActiveConstraintReadIdx { 0 }; ///< Next constraint for determine active constraints + uint8 mPadding1[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Padding to avoid sharing cache line with the next atomic + + atomic mNumActiveConstraints { 0 }; ///< Number of constraints in the mActiveConstraints array + uint8 mPadding2[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Padding to avoid sharing cache line with the next atomic + + atomic mSetupVelocityConstraintsReadIdx { 0 }; ///< Next constraint for setting up velocity constraints + uint8 mPadding3[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Padding to avoid sharing cache line with the next atomic + + atomic mStepListenerReadIdx { 0 }; ///< Next step listener to call + uint8 mPadding4[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Padding to avoid sharing cache line with the next atomic + + atomic mApplyGravityReadIdx { 0 }; ///< Next body to apply gravity to + uint8 mPadding5[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Padding to avoid sharing cache line with the next atomic + + atomic mActiveBodyReadIdx { 0 }; ///< Index of fist active body that has not yet been processed by the broadphase + uint8 mPadding6[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Padding to avoid sharing cache line with the next atomic + + BodyPairQueues mBodyPairQueues; ///< Queues in which to put body pairs that need to be tested by the narrowphase + + uint32 mMaxBodyPairsPerQueue; ///< Amount of body pairs that we can queue per queue + + atomic mActiveFindCollisionJobs; ///< A bitmask that indicates which jobs are still active + + atomic mNumBodyPairs { 0 }; ///< The number of body pairs found in this step (used to size the contact cache in the next step) + atomic mNumManifolds { 0 }; ///< The number of manifolds found in this step (used to size the contact cache in the next step) + + atomic mSolveVelocityConstraintsNextIsland { 0 }; ///< Next island that needs to be processed for the solve velocity constraints step (doesn't need own cache line since position jobs don't run at same time) + atomic mSolvePositionConstraintsNextIsland { 0 }; ///< Next island that needs to be processed for the solve position constraints step (doesn't need own cache line since velocity jobs don't run at same time) + + /// Contains the information needed to cast a body through the scene to do continuous collision detection + struct CCDBody + { + CCDBody(BodyID inBodyID1, Vec3Arg inDeltaPosition, float inLinearCastThresholdSq, float inMaxPenetration) : mDeltaPosition(inDeltaPosition), mBodyID1(inBodyID1), mLinearCastThresholdSq(inLinearCastThresholdSq), mMaxPenetration(inMaxPenetration) { } + + Vec3 mDeltaPosition; ///< Desired rotation step + Vec3 mContactNormal; ///< World space normal of closest hit (only valid if mFractionPlusSlop < 1) + RVec3 mContactPointOn2; ///< World space contact point on body 2 of closest hit (only valid if mFractionPlusSlop < 1) + BodyID mBodyID1; ///< Body 1 (the body that is performing collision detection) + BodyID mBodyID2; ///< Body 2 (the body of the closest hit, only valid if mFractionPlusSlop < 1) + SubShapeID mSubShapeID2; ///< Sub shape of body 2 that was hit (only valid if mFractionPlusSlop < 1) + float mFraction = 1.0f; ///< Fraction at which the hit occurred + float mFractionPlusSlop = 1.0f; ///< Fraction at which the hit occurred + extra delta to allow body to penetrate by mMaxPenetration + float mLinearCastThresholdSq; ///< Maximum allowed squared movement before doing a linear cast (determined by inner radius of shape) + float mMaxPenetration; ///< Maximum allowed penetration (determined by inner radius of shape) + ContactSettings mContactSettings; ///< The contact settings for this contact + }; + atomic mIntegrateVelocityReadIdx { 0 }; ///< Next active body index to take when integrating velocities + CCDBody * mCCDBodies = nullptr; ///< List of bodies that need to do continuous collision detection + uint32 mCCDBodiesCapacity = 0; ///< Capacity of the mCCDBodies list + atomic mNumCCDBodies = 0; ///< Number of CCD bodies in mCCDBodies + atomic mNextCCDBody { 0 }; ///< Next unprocessed body index in mCCDBodies + int * mActiveBodyToCCDBody = nullptr; ///< A mapping between an index in BodyManager::mActiveBodies and the index in mCCDBodies + uint32 mNumActiveBodyToCCDBody = 0; ///< Number of indices in mActiveBodyToCCDBody + + // Jobs in order of execution (some run in parallel) + JobHandle mBroadPhasePrepare; ///< Prepares the new tree in the background + JobHandleArray mStepListeners; ///< Listeners to notify of the beginning of a physics step + JobHandleArray mDetermineActiveConstraints; ///< Determine which constraints will be active during this step + JobHandleArray mApplyGravity; ///< Update velocities of bodies with gravity + JobHandleArray mFindCollisions; ///< Find all collisions between active bodies an the world + JobHandle mUpdateBroadphaseFinalize; ///< Swap the newly built tree with the current tree + JobHandleArray mSetupVelocityConstraints; ///< Calculate properties for all constraints in the constraint manager + JobHandle mBuildIslandsFromConstraints; ///< Go over all constraints and assign the bodies they're attached to to an island + JobHandle mFinalizeIslands; ///< Finalize calculation simulation islands + JobHandle mBodySetIslandIndex; ///< Set the current island index on each body (not used by the simulation, only for drawing purposes) + JobHandleArray mSolveVelocityConstraints; ///< Solve the constraints in the velocity domain + JobHandle mPreIntegrateVelocity; ///< Setup integration of all body positions + JobHandleArray mIntegrateVelocity; ///< Integrate all body positions + JobHandle mPostIntegrateVelocity; ///< Finalize integration of all body positions + JobHandle mResolveCCDContacts; ///< Updates the positions and velocities for all bodies that need continuous collision detection + JobHandleArray mSolvePositionConstraints; ///< Solve all constraints in the position domain + JobHandle mContactRemovedCallbacks; ///< Calls the contact removed callbacks + JobHandle mSoftBodyPrepare; ///< Prepares updating the soft bodies + JobHandleArray mSoftBodyCollide; ///< Finds all colliding shapes for soft bodies + JobHandleArray mSoftBodySimulate; ///< Simulates all particles + JobHandle mSoftBodyFinalize; ///< Finalizes the soft body update + JobHandle mStartNextStep; ///< Job that kicks the next step (empty for the last step) + }; + + using Steps = Array>; + + /// Maximum amount of concurrent jobs on this machine + int GetMaxConcurrency() const { const int max_concurrency = PhysicsUpdateContext::cMaxConcurrency; return min(max_concurrency, mJobSystem->GetMaxConcurrency()); } ///< Need to put max concurrency in temp var as min requires a reference + + PhysicsSystem * mPhysicsSystem; ///< The physics system we belong to + TempAllocator * mTempAllocator; ///< Temporary allocator used during the update + JobSystem * mJobSystem; ///< Job system that processes jobs + JobSystem::Barrier * mBarrier; ///< Barrier used to wait for all physics jobs to complete + + float mStepDeltaTime; ///< Delta time for a simulation step (collision step) + float mWarmStartImpulseRatio; ///< Ratio of this step delta time vs last step + atomic mErrors { 0 }; ///< Errors that occurred during the update, actual type is EPhysicsUpdateError + + Constraint ** mActiveConstraints = nullptr; ///< Constraints that were active at the start of the physics update step (activating bodies can activate constraints and we need a consistent snapshot). Only these constraints will be resolved. + + BodyPair * mBodyPairs = nullptr; ///< A list of body pairs found by the broadphase + + IslandBuilder * mIslandBuilder; ///< Keeps track of connected bodies and builds islands for multithreaded velocity/position update + + Steps mSteps; + + uint mNumSoftBodies; ///< Number of active soft bodies in the simulation + SoftBodyUpdateContext * mSoftBodyUpdateContexts = nullptr; ///< Contexts for updating soft bodies + atomic mSoftBodyToCollide { 0 }; ///< Next soft body to take when running SoftBodyCollide jobs +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.cpp b/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.cpp new file mode 100644 index 000000000000..9d7e5b675d42 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.cpp @@ -0,0 +1,705 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(RagdollSettings::Part) +{ + JPH_ADD_BASE_CLASS(RagdollSettings::Part, BodyCreationSettings) + + JPH_ADD_ATTRIBUTE(RagdollSettings::Part, mToParent) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(RagdollSettings::AdditionalConstraint) +{ + JPH_ADD_ATTRIBUTE(RagdollSettings::AdditionalConstraint, mBodyIdx) + JPH_ADD_ATTRIBUTE(RagdollSettings::AdditionalConstraint, mConstraint) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(RagdollSettings) +{ + JPH_ADD_ATTRIBUTE(RagdollSettings, mSkeleton) + JPH_ADD_ATTRIBUTE(RagdollSettings, mParts) + JPH_ADD_ATTRIBUTE(RagdollSettings, mAdditionalConstraints) +} + +static inline BodyInterface &sGetBodyInterface(PhysicsSystem *inSystem, bool inLockBodies) +{ + return inLockBodies? inSystem->GetBodyInterface() : inSystem->GetBodyInterfaceNoLock(); +} + +static inline const BodyLockInterface &sGetBodyLockInterface(const PhysicsSystem *inSystem, bool inLockBodies) +{ + return inLockBodies? static_cast(inSystem->GetBodyLockInterface()) : static_cast(inSystem->GetBodyLockInterfaceNoLock()); +} + +bool RagdollSettings::Stabilize() +{ + // Based on: Stop my Constraints from Blowing Up! - Oliver Strunk (Havok) + // Do 2 things: + // 1. Limit the mass ratios between parents and children (slide 16) + // 2. Increase the inertia of parents so that they're bigger or equal to the sum of their children (slide 34) + + // If we don't have any joints there's nothing to stabilize + if (mSkeleton->GetJointCount() == 0) + return true; + + // The skeleton can contain one or more static bodies. We can't modify the mass for those so we start a new stabilization chain for each joint under a static body until we reach the next static body. + // This array keeps track of which joints have been processed. + Array visited; + visited.resize(mSkeleton->GetJointCount()); + for (size_t v = 0; v < visited.size(); ++v) + { + // Mark static bodies as visited so we won't process these + Part &p = mParts[v]; + bool has_mass_properties = p.HasMassProperties(); + visited[v] = !has_mass_properties; + + if (has_mass_properties && p.mOverrideMassProperties != EOverrideMassProperties::MassAndInertiaProvided) + { + // Mass properties not yet calculated, do it now + p.mMassPropertiesOverride = p.GetMassProperties(); + p.mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided; + } + } + + // Find first unvisited part that either has no parent or that has a parent that was visited + for (int first_idx = 0; first_idx < mSkeleton->GetJointCount(); ++first_idx) + { + int first_idx_parent = mSkeleton->GetJoint(first_idx).mParentJointIndex; + if (!visited[first_idx] && (first_idx_parent == -1 || visited[first_idx_parent])) + { + // Find all children of first_idx and their children up to the next static part + int next_to_process = 0; + Array indices; + indices.reserve(mSkeleton->GetJointCount()); + visited[first_idx] = true; + indices.push_back(first_idx); + do + { + int parent_idx = indices[next_to_process++]; + for (int child_idx = 0; child_idx < mSkeleton->GetJointCount(); ++child_idx) + if (!visited[child_idx] && mSkeleton->GetJoint(child_idx).mParentJointIndex == parent_idx) + { + visited[child_idx] = true; + indices.push_back(child_idx); + } + } while (next_to_process < (int)indices.size()); + + // If there's only 1 body, we can't redistribute mass + if (indices.size() == 1) + continue; + + const float cMinMassRatio = 0.8f; + const float cMaxMassRatio = 1.2f; + + // Ensure that the mass ratio from parent to child is within a range + float total_mass_ratio = 1.0f; + Array mass_ratios; + mass_ratios.resize(mSkeleton->GetJointCount()); + mass_ratios[indices[0]] = 1.0f; + for (int i = 1; i < (int)indices.size(); ++i) + { + int child_idx = indices[i]; + int parent_idx = mSkeleton->GetJoint(child_idx).mParentJointIndex; + float ratio = mParts[child_idx].mMassPropertiesOverride.mMass / mParts[parent_idx].mMassPropertiesOverride.mMass; + mass_ratios[child_idx] = mass_ratios[parent_idx] * Clamp(ratio, cMinMassRatio, cMaxMassRatio); + total_mass_ratio += mass_ratios[child_idx]; + } + + // Calculate total mass of this chain + float total_mass = 0.0f; + for (int idx : indices) + total_mass += mParts[idx].mMassPropertiesOverride.mMass; + + // Calculate how much mass belongs to a ratio of 1 + float ratio_to_mass = total_mass / total_mass_ratio; + + // Adjust all masses and inertia tensors for the new mass + for (int i : indices) + { + Part &p = mParts[i]; + float old_mass = p.mMassPropertiesOverride.mMass; + float new_mass = mass_ratios[i] * ratio_to_mass; + p.mMassPropertiesOverride.mMass = new_mass; + p.mMassPropertiesOverride.mInertia *= new_mass / old_mass; + p.mMassPropertiesOverride.mInertia.SetColumn4(3, Vec4(0, 0, 0, 1)); + } + + const float cMaxInertiaIncrease = 2.0f; + + // Get the principal moments of inertia for all parts + struct Principal + { + Mat44 mRotation; + Vec3 mDiagonal; + float mChildSum = 0.0f; + }; + Array principals; + principals.resize(mParts.size()); + for (int i : indices) + if (!mParts[i].mMassPropertiesOverride.DecomposePrincipalMomentsOfInertia(principals[i].mRotation, principals[i].mDiagonal)) + { + JPH_ASSERT(false, "Failed to decompose the inertia tensor!"); + return false; + } + + // Calculate sum of child inertias + // Walk backwards so we sum the leaves first + for (int i = (int)indices.size() - 1; i > 0; --i) + { + int child_idx = indices[i]; + int parent_idx = mSkeleton->GetJoint(child_idx).mParentJointIndex; + principals[parent_idx].mChildSum += principals[child_idx].mDiagonal[0] + principals[child_idx].mChildSum; + } + + // Adjust inertia tensors for all parts + for (int i : indices) + { + Part &p = mParts[i]; + Principal &principal = principals[i]; + if (principal.mChildSum != 0.0f) + { + // Calculate minimum inertia this object should have based on it children + float minimum = min(cMaxInertiaIncrease * principal.mDiagonal[0], principal.mChildSum); + principal.mDiagonal = Vec3::sMax(principal.mDiagonal, Vec3::sReplicate(minimum)); + + // Recalculate moment of inertia in body space + p.mMassPropertiesOverride.mInertia = principal.mRotation * Mat44::sScale(principal.mDiagonal) * principal.mRotation.Inversed3x3(); + } + } + } + } + + return true; +} + +void RagdollSettings::DisableParentChildCollisions(const Mat44 *inJointMatrices, float inMinSeparationDistance) +{ + int joint_count = mSkeleton->GetJointCount(); + JPH_ASSERT(joint_count == (int)mParts.size()); + + // Create a group filter table that disables collisions between parent and child + Ref group_filter = new GroupFilterTable(joint_count); + for (int joint_idx = 0; joint_idx < joint_count; ++joint_idx) + { + int parent_joint = mSkeleton->GetJoint(joint_idx).mParentJointIndex; + if (parent_joint >= 0) + group_filter->DisableCollision(joint_idx, parent_joint); + } + + // If joint matrices are provided + if (inJointMatrices != nullptr) + { + // Loop over all joints + for (int j1 = 0; j1 < joint_count; ++j1) + { + // Shape and transform for joint 1 + const Part &part1 = mParts[j1]; + const Shape *shape1 = part1.GetShape(); + Vec3 scale1; + Mat44 com1 = (inJointMatrices[j1].PreTranslated(shape1->GetCenterOfMass())).Decompose(scale1); + + // Loop over all other joints + for (int j2 = j1 + 1; j2 < joint_count; ++j2) + if (group_filter->IsCollisionEnabled(j1, j2)) // Only if collision is still enabled we need to test + { + // Shape and transform for joint 2 + const Part &part2 = mParts[j2]; + const Shape *shape2 = part2.GetShape(); + Vec3 scale2; + Mat44 com2 = (inJointMatrices[j2].PreTranslated(shape2->GetCenterOfMass())).Decompose(scale2); + + // Collision settings + CollideShapeSettings settings; + settings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll; + settings.mBackFaceMode = EBackFaceMode::CollideWithBackFaces; + settings.mMaxSeparationDistance = inMinSeparationDistance; + + // Only check if one of the two bodies can become dynamic + if (part1.HasMassProperties() || part2.HasMassProperties()) + { + // If there is a collision, disable the collision between the joints + AnyHitCollisionCollector collector; + if (part1.HasMassProperties()) // Ensure that the first shape is always a dynamic one (we can't check mesh vs convex but we can check convex vs mesh) + CollisionDispatch::sCollideShapeVsShape(shape1, shape2, scale1, scale2, com1, com2, SubShapeIDCreator(), SubShapeIDCreator(), settings, collector); + else + CollisionDispatch::sCollideShapeVsShape(shape2, shape1, scale2, scale1, com2, com1, SubShapeIDCreator(), SubShapeIDCreator(), settings, collector); + if (collector.HadHit()) + group_filter->DisableCollision(j1, j2); + } + } + } + } + + // Loop over the body parts and assign them a sub group ID and the group filter + for (int joint_idx = 0; joint_idx < joint_count; ++joint_idx) + { + Part &part = mParts[joint_idx]; + part.mCollisionGroup.SetSubGroupID(joint_idx); + part.mCollisionGroup.SetGroupFilter(group_filter); + } +} + +void RagdollSettings::SaveBinaryState(StreamOut &inStream, bool inSaveShapes, bool inSaveGroupFilter) const +{ + BodyCreationSettings::ShapeToIDMap shape_to_id; + BodyCreationSettings::MaterialToIDMap material_to_id; + BodyCreationSettings::GroupFilterToIDMap group_filter_to_id; + + // Save skeleton + mSkeleton->SaveBinaryState(inStream); + + // Save parts + inStream.Write((uint32)mParts.size()); + for (const Part &p : mParts) + { + // Write body creation settings + p.SaveWithChildren(inStream, inSaveShapes? &shape_to_id : nullptr, inSaveShapes? &material_to_id : nullptr, inSaveGroupFilter? &group_filter_to_id : nullptr); + + // Save constraint + inStream.Write(p.mToParent != nullptr); + if (p.mToParent != nullptr) + p.mToParent->SaveBinaryState(inStream); + } + + // Save additional constraints + inStream.Write((uint32)mAdditionalConstraints.size()); + for (const AdditionalConstraint &c : mAdditionalConstraints) + { + // Save bodies indices + inStream.Write(c.mBodyIdx); + + // Save constraint + c.mConstraint->SaveBinaryState(inStream); + } +} + +RagdollSettings::RagdollResult RagdollSettings::sRestoreFromBinaryState(StreamIn &inStream) +{ + RagdollResult result; + + // Restore skeleton + Skeleton::SkeletonResult skeleton_result = Skeleton::sRestoreFromBinaryState(inStream); + if (skeleton_result.HasError()) + { + result.SetError(skeleton_result.GetError()); + return result; + } + + // Create ragdoll + Ref ragdoll = new RagdollSettings(); + ragdoll->mSkeleton = skeleton_result.Get(); + + BodyCreationSettings::IDToShapeMap id_to_shape; + BodyCreationSettings::IDToMaterialMap id_to_material; + BodyCreationSettings::IDToGroupFilterMap id_to_group_filter; + + // Reserve some memory to avoid frequent reallocations + id_to_shape.reserve(1024); + id_to_material.reserve(128); + id_to_group_filter.reserve(128); + + // Read parts + uint32 len = 0; + inStream.Read(len); + ragdoll->mParts.resize(len); + for (Part &p : ragdoll->mParts) + { + // Read creation settings + BodyCreationSettings::BCSResult bcs_result = BodyCreationSettings::sRestoreWithChildren(inStream, id_to_shape, id_to_material, id_to_group_filter); + if (bcs_result.HasError()) + { + result.SetError(bcs_result.GetError()); + return result; + } + static_cast(p) = bcs_result.Get(); + + // Read constraint + bool has_constraint = false; + inStream.Read(has_constraint); + if (has_constraint) + { + ConstraintSettings::ConstraintResult constraint_result = ConstraintSettings::sRestoreFromBinaryState(inStream); + if (constraint_result.HasError()) + { + result.SetError(constraint_result.GetError()); + return result; + } + p.mToParent = DynamicCast(constraint_result.Get()); + } + } + + // Read additional constraints + len = 0; + inStream.Read(len); + ragdoll->mAdditionalConstraints.resize(len); + for (AdditionalConstraint &c : ragdoll->mAdditionalConstraints) + { + // Read body indices + inStream.Read(c.mBodyIdx); + + // Read constraint + ConstraintSettings::ConstraintResult constraint_result = ConstraintSettings::sRestoreFromBinaryState(inStream); + if (constraint_result.HasError()) + { + result.SetError(constraint_result.GetError()); + return result; + } + c.mConstraint = DynamicCast(constraint_result.Get()); + } + + // Create mapping tables + ragdoll->CalculateBodyIndexToConstraintIndex(); + ragdoll->CalculateConstraintIndexToBodyIdxPair(); + + result.Set(ragdoll); + return result; +} + +Ragdoll *RagdollSettings::CreateRagdoll(CollisionGroup::GroupID inCollisionGroup, uint64 inUserData, PhysicsSystem *inSystem) const +{ + Ragdoll *r = new Ragdoll(inSystem); + r->mRagdollSettings = this; + r->mBodyIDs.reserve(mParts.size()); + r->mConstraints.reserve(mParts.size() + mAdditionalConstraints.size()); + + // Create bodies and constraints + BodyInterface &bi = inSystem->GetBodyInterface(); + Body **bodies = (Body **)JPH_STACK_ALLOC(mParts.size() * sizeof(Body *)); + int joint_idx = 0; + for (const Part &p : mParts) + { + Body *body2 = bi.CreateBody(p); + if (body2 == nullptr) + { + // Out of bodies, failed to create ragdoll + delete r; + return nullptr; + } + body2->GetCollisionGroup().SetGroupID(inCollisionGroup); + body2->SetUserData(inUserData); + + // Temporarily store body pointer for hooking up constraints + bodies[joint_idx] = body2; + + // Create constraint + if (p.mToParent != nullptr) + { + Body *body1 = bodies[mSkeleton->GetJoint(joint_idx).mParentJointIndex]; + r->mConstraints.push_back(p.mToParent->Create(*body1, *body2)); + } + + // Store body ID and constraint in parallel arrays + r->mBodyIDs.push_back(body2->GetID()); + + ++joint_idx; + } + + // Add additional constraints + for (const AdditionalConstraint &c : mAdditionalConstraints) + { + Body *body1 = bodies[c.mBodyIdx[0]]; + Body *body2 = bodies[c.mBodyIdx[1]]; + r->mConstraints.push_back(c.mConstraint->Create(*body1, *body2)); + } + + return r; +} + +void RagdollSettings::CalculateBodyIndexToConstraintIndex() +{ + mBodyIndexToConstraintIndex.clear(); + mBodyIndexToConstraintIndex.reserve(mParts.size()); + + int constraint_index = 0; + for (const Part &p : mParts) + { + if (p.mToParent != nullptr) + mBodyIndexToConstraintIndex.push_back(constraint_index++); + else + mBodyIndexToConstraintIndex.push_back(-1); + } +} + +void RagdollSettings::CalculateConstraintIndexToBodyIdxPair() +{ + mConstraintIndexToBodyIdxPair.clear(); + mConstraintIndexToBodyIdxPair.reserve(mParts.size() + mAdditionalConstraints.size()); + + // Add constraints between parts + int joint_idx = 0; + for (const Part &p : mParts) + { + if (p.mToParent != nullptr) + { + int parent_joint_idx = mSkeleton->GetJoint(joint_idx).mParentJointIndex; + mConstraintIndexToBodyIdxPair.emplace_back(parent_joint_idx, joint_idx); + } + + ++joint_idx; + } + + // Add additional constraints + for (const AdditionalConstraint &c : mAdditionalConstraints) + mConstraintIndexToBodyIdxPair.emplace_back(c.mBodyIdx[0], c.mBodyIdx[1]); +} + +Ragdoll::~Ragdoll() +{ + // Destroy all bodies + mSystem->GetBodyInterface().DestroyBodies(mBodyIDs.data(), (int)mBodyIDs.size()); +} + +void Ragdoll::AddToPhysicsSystem(EActivation inActivationMode, bool inLockBodies) +{ + // Scope for JPH_STACK_ALLOC + { + // Create copy of body ids since they will be shuffled + int num_bodies = (int)mBodyIDs.size(); + BodyID *bodies = (BodyID *)JPH_STACK_ALLOC(num_bodies * sizeof(BodyID)); + memcpy(bodies, mBodyIDs.data(), num_bodies * sizeof(BodyID)); + + // Insert bodies as a batch + BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies); + BodyInterface::AddState add_state = bi.AddBodiesPrepare(bodies, num_bodies); + bi.AddBodiesFinalize(bodies, num_bodies, add_state, inActivationMode); + } + + // Add all constraints + mSystem->AddConstraints((Constraint **)mConstraints.data(), (int)mConstraints.size()); +} + +void Ragdoll::RemoveFromPhysicsSystem(bool inLockBodies) +{ + // Remove all constraints before removing the bodies + mSystem->RemoveConstraints((Constraint **)mConstraints.data(), (int)mConstraints.size()); + + // Scope for JPH_STACK_ALLOC + { + // Create copy of body ids since they will be shuffled + int num_bodies = (int)mBodyIDs.size(); + BodyID *bodies = (BodyID *)JPH_STACK_ALLOC(num_bodies * sizeof(BodyID)); + memcpy(bodies, mBodyIDs.data(), num_bodies * sizeof(BodyID)); + + // Remove all bodies as a batch + sGetBodyInterface(mSystem, inLockBodies).RemoveBodies(bodies, num_bodies); + } +} + +void Ragdoll::Activate(bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).ActivateBodies(mBodyIDs.data(), (int)mBodyIDs.size()); +} + +bool Ragdoll::IsActive(bool inLockBodies) const +{ + // Lock the bodies + int body_count = (int)mBodyIDs.size(); + BodyLockMultiRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs.data(), body_count); + + // Test if any body is active + for (int b = 0; b < body_count; ++b) + { + const Body *body = lock.GetBody(b); + if (body->IsActive()) + return true; + } + + return false; +} + +void Ragdoll::SetGroupID(CollisionGroup::GroupID inGroupID, bool inLockBodies) +{ + // Lock the bodies + int body_count = (int)mBodyIDs.size(); + BodyLockMultiWrite lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs.data(), body_count); + + // Update group ID + for (int b = 0; b < body_count; ++b) + { + Body *body = lock.GetBody(b); + body->GetCollisionGroup().SetGroupID(inGroupID); + } +} + +void Ragdoll::SetPose(const SkeletonPose &inPose, bool inLockBodies) +{ + JPH_ASSERT(inPose.GetSkeleton() == mRagdollSettings->mSkeleton); + + SetPose(inPose.GetRootOffset(), inPose.GetJointMatrices().data(), inLockBodies); +} + +void Ragdoll::SetPose(RVec3Arg inRootOffset, const Mat44 *inJointMatrices, bool inLockBodies) +{ + // Move bodies instantly into the correct position + BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies); + for (int i = 0; i < (int)mBodyIDs.size(); ++i) + { + const Mat44 &joint = inJointMatrices[i]; + bi.SetPositionAndRotation(mBodyIDs[i], inRootOffset + joint.GetTranslation(), joint.GetQuaternion(), EActivation::DontActivate); + } +} + +void Ragdoll::GetPose(SkeletonPose &outPose, bool inLockBodies) +{ + JPH_ASSERT(outPose.GetSkeleton() == mRagdollSettings->mSkeleton); + + RVec3 root_offset; + GetPose(root_offset, outPose.GetJointMatrices().data(), inLockBodies); + outPose.SetRootOffset(root_offset); +} + +void Ragdoll::GetPose(RVec3 &outRootOffset, Mat44 *outJointMatrices, bool inLockBodies) +{ + // Lock the bodies + int body_count = (int)mBodyIDs.size(); + if (body_count == 0) + return; + BodyLockMultiRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs.data(), body_count); + + // Get root matrix + const Body *root = lock.GetBody(0); + RMat44 root_transform = root->GetWorldTransform(); + outRootOffset = root_transform.GetTranslation(); + outJointMatrices[0] = Mat44(root_transform.GetColumn4(0), root_transform.GetColumn4(1), root_transform.GetColumn4(2), Vec4(0, 0, 0, 1)); + + // Get other matrices + for (int b = 1; b < body_count; ++b) + { + const Body *body = lock.GetBody(b); + RMat44 transform = body->GetWorldTransform(); + outJointMatrices[b] = Mat44(transform.GetColumn4(0), transform.GetColumn4(1), transform.GetColumn4(2), Vec4(Vec3(transform.GetTranslation() - outRootOffset), 1)); + } +} + +void Ragdoll::ResetWarmStart() +{ + for (TwoBodyConstraint *c : mConstraints) + c->ResetWarmStart(); +} + +void Ragdoll::DriveToPoseUsingKinematics(const SkeletonPose &inPose, float inDeltaTime, bool inLockBodies) +{ + JPH_ASSERT(inPose.GetSkeleton() == mRagdollSettings->mSkeleton); + + DriveToPoseUsingKinematics(inPose.GetRootOffset(), inPose.GetJointMatrices().data(), inDeltaTime, inLockBodies); +} + +void Ragdoll::DriveToPoseUsingKinematics(RVec3Arg inRootOffset, const Mat44 *inJointMatrices, float inDeltaTime, bool inLockBodies) +{ + // Move bodies into the correct position using kinematics + BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies); + for (int i = 0; i < (int)mBodyIDs.size(); ++i) + { + const Mat44 &joint = inJointMatrices[i]; + bi.MoveKinematic(mBodyIDs[i], inRootOffset + joint.GetTranslation(), joint.GetQuaternion(), inDeltaTime); + } +} + +void Ragdoll::DriveToPoseUsingMotors(const SkeletonPose &inPose) +{ + JPH_ASSERT(inPose.GetSkeleton() == mRagdollSettings->mSkeleton); + + // Move bodies into the correct position using constraints + for (int i = 0; i < (int)inPose.GetJointMatrices().size(); ++i) + { + int constraint_idx = mRagdollSettings->GetConstraintIndexForBodyIndex(i); + if (constraint_idx >= 0) + { + // Get desired rotation of this body relative to its parent + const SkeletalAnimation::JointState &joint_state = inPose.GetJoint(i); + + // Drive constraint to target + TwoBodyConstraint *constraint = mConstraints[constraint_idx]; + EConstraintSubType sub_type = constraint->GetSubType(); + if (sub_type == EConstraintSubType::SwingTwist) + { + SwingTwistConstraint *st_constraint = static_cast(constraint); + st_constraint->SetSwingMotorState(EMotorState::Position); + st_constraint->SetTwistMotorState(EMotorState::Position); + st_constraint->SetTargetOrientationBS(joint_state.mRotation); + } + else + JPH_ASSERT(false, "Constraint type not implemented!"); + } + } +} + +void Ragdoll::SetLinearAndAngularVelocity(Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, bool inLockBodies) +{ + BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies); + for (BodyID body_id : mBodyIDs) + bi.SetLinearAndAngularVelocity(body_id, inLinearVelocity, inAngularVelocity); +} + +void Ragdoll::SetLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies) +{ + BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies); + for (BodyID body_id : mBodyIDs) + bi.SetLinearVelocity(body_id, inLinearVelocity); +} + +void Ragdoll::AddLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies) +{ + BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies); + for (BodyID body_id : mBodyIDs) + bi.AddLinearVelocity(body_id, inLinearVelocity); +} + +void Ragdoll::AddImpulse(Vec3Arg inImpulse, bool inLockBodies) +{ + BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies); + for (BodyID body_id : mBodyIDs) + bi.AddImpulse(body_id, inImpulse); +} + +void Ragdoll::GetRootTransform(RVec3 &outPosition, Quat &outRotation, bool inLockBodies) const +{ + BodyLockRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs[0]); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + outPosition = body.GetPosition(); + outRotation = body.GetRotation(); + } + else + { + outPosition = RVec3::sZero(); + outRotation = Quat::sIdentity(); + } +} + +AABox Ragdoll::GetWorldSpaceBounds(bool inLockBodies) const +{ + // Lock the bodies + int body_count = (int)mBodyIDs.size(); + BodyLockMultiRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs.data(), body_count); + + // Encapsulate all bodies + AABox bounds; + for (int b = 0; b < body_count; ++b) + { + const Body *body = lock.GetBody(b); + if (body != nullptr) + bounds.Encapsulate(body->GetWorldSpaceBounds()); + } + return bounds; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.h b/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.h new file mode 100644 index 000000000000..8fe401080f77 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.h @@ -0,0 +1,240 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class Ragdoll; +class PhysicsSystem; + +/// Contains the structure of a ragdoll +class JPH_EXPORT RagdollSettings : public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, RagdollSettings) + +public: + /// Stabilize the constraints of the ragdoll + /// @return True on success, false on failure. + bool Stabilize(); + + /// After the ragdoll has been fully configured, call this function to automatically create and add a GroupFilterTable collision filter to all bodies + /// and configure them so that parent and children don't collide. + /// + /// This will: + /// - Create a GroupFilterTable and assign it to all of the bodies in a ragdoll. + /// - Each body in your ragdoll will get a SubGroupID that is equal to the joint index in the Skeleton that it is attached to. + /// - Loop over all joints in the Skeleton and call GroupFilterTable::DisableCollision(joint index, parent joint index). + /// - When a pose is provided through inJointMatrices the function will detect collisions between joints + /// (they must be separated by more than inMinSeparationDistance to be treated as not colliding) and automatically disable collisions. + /// + /// When you create an instance using Ragdoll::CreateRagdoll pass in a unique GroupID for each ragdoll (e.g. a simple counter), note that this number + /// should be unique throughout the PhysicsSystem, so if you have different types of ragdolls they should not share the same GroupID. + void DisableParentChildCollisions(const Mat44 *inJointMatrices = nullptr, float inMinSeparationDistance = 0.0f); + + /// Saves the state of this object in binary form to inStream. + /// @param inStream The stream to save the state to + /// @param inSaveShapes If the shapes should be saved as well (these could be shared between ragdolls, in which case the calling application may want to write custom code to restore them) + /// @param inSaveGroupFilter If the group filter should be saved as well (these could be shared) + void SaveBinaryState(StreamOut &inStream, bool inSaveShapes, bool inSaveGroupFilter) const; + + using RagdollResult = Result>; + + /// Restore a saved ragdoll from inStream + static RagdollResult sRestoreFromBinaryState(StreamIn &inStream); + + /// Create ragdoll instance from these settings + /// @return Newly created ragdoll or null when out of bodies + Ragdoll * CreateRagdoll(CollisionGroup::GroupID inCollisionGroup, uint64 inUserData, PhysicsSystem *inSystem) const; + + /// Access to the skeleton of this ragdoll + const Skeleton * GetSkeleton() const { return mSkeleton; } + Skeleton * GetSkeleton() { return mSkeleton; } + + /// Calculate the map needed for GetBodyIndexToConstraintIndex() + void CalculateBodyIndexToConstraintIndex(); + + /// Get table that maps a body index to the constraint index with which it is connected to its parent. -1 if there is no constraint associated with the body. + /// Note that this will only tell you which constraint connects the body to its parent, it will not look in the additional constraint list. + const Array & GetBodyIndexToConstraintIndex() const { return mBodyIndexToConstraintIndex; } + + /// Map a single body index to a constraint index + int GetConstraintIndexForBodyIndex(int inBodyIndex) const { return mBodyIndexToConstraintIndex[inBodyIndex]; } + + /// Calculate the map needed for GetConstraintIndexToBodyIdxPair() + void CalculateConstraintIndexToBodyIdxPair(); + + using BodyIdxPair = std::pair; + + /// Table that maps a constraint index (index in mConstraints) to the indices of the bodies that the constraint is connected to (index in mBodyIDs) + const Array & GetConstraintIndexToBodyIdxPair() const { return mConstraintIndexToBodyIdxPair; } + + /// Map a single constraint index (index in mConstraints) to the indices of the bodies that the constraint is connected to (index in mBodyIDs) + BodyIdxPair GetBodyIndicesForConstraintIndex(int inConstraintIndex) const { return mConstraintIndexToBodyIdxPair[inConstraintIndex]; } + + /// A single rigid body sub part of the ragdoll + class Part : public BodyCreationSettings + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Part) + + public: + Ref mToParent; + }; + + /// List of ragdoll parts + using PartVector = Array; ///< The constraint that connects this part to its parent part (should be null for the root) + + /// A constraint that connects two bodies in a ragdoll (for non parent child related constraints) + class AdditionalConstraint + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, AdditionalConstraint) + + public: + /// Constructors + AdditionalConstraint() = default; + AdditionalConstraint(int inBodyIdx1, int inBodyIdx2, TwoBodyConstraintSettings *inConstraint) : mBodyIdx { inBodyIdx1, inBodyIdx2 }, mConstraint(inConstraint) { } + + int mBodyIdx[2]; ///< Indices of the bodies that this constraint connects + Ref mConstraint; ///< The constraint that connects these bodies + }; + + /// List of additional constraints + using AdditionalConstraintVector = Array; + + /// The skeleton for this ragdoll + Ref mSkeleton; + + /// For each of the joints, the body and constraint attaching it to its parent body (1-on-1 with mSkeleton.GetJoints()) + PartVector mParts; + + /// A list of constraints that connects two bodies in a ragdoll (for non parent child related constraints) + AdditionalConstraintVector mAdditionalConstraints; + +private: + /// Table that maps a body index (index in mBodyIDs) to the constraint index with which it is connected to its parent. -1 if there is no constraint associated with the body. + Array mBodyIndexToConstraintIndex; + + /// Table that maps a constraint index (index in mConstraints) to the indices of the bodies that the constraint is connected to (index in mBodyIDs) + Array mConstraintIndexToBodyIdxPair; +}; + +/// Runtime ragdoll information +class JPH_EXPORT Ragdoll : public RefTarget, public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit Ragdoll(PhysicsSystem *inSystem) : mSystem(inSystem) { } + + /// Destructor + ~Ragdoll(); + + /// Add bodies and constraints to the system and optionally activate the bodies + void AddToPhysicsSystem(EActivation inActivationMode, bool inLockBodies = true); + + /// Remove bodies and constraints from the system + void RemoveFromPhysicsSystem(bool inLockBodies = true); + + /// Wake up all bodies in the ragdoll + void Activate(bool inLockBodies = true); + + /// Check if one or more of the bodies in the ragdoll are active. + /// Note that this involves locking the bodies (if inLockBodies is true) and looping over them. An alternative and possibly faster + /// way could be to install a BodyActivationListener and count the number of active bodies of a ragdoll as they're activated / deactivated + /// (basically check if the body that activates / deactivates is in GetBodyIDs() and increment / decrement a counter). + bool IsActive(bool inLockBodies = true) const; + + /// Set the group ID on all bodies in the ragdoll + void SetGroupID(CollisionGroup::GroupID inGroupID, bool inLockBodies = true); + + /// Set the ragdoll to a pose (calls BodyInterface::SetPositionAndRotation to instantly move the ragdoll) + void SetPose(const SkeletonPose &inPose, bool inLockBodies = true); + + /// Lower level version of SetPose that directly takes the world space joint matrices + void SetPose(RVec3Arg inRootOffset, const Mat44 *inJointMatrices, bool inLockBodies = true); + + /// Get the ragdoll pose (uses the world transform of the bodies to calculate the pose) + void GetPose(SkeletonPose &outPose, bool inLockBodies = true); + + /// Lower level version of GetPose that directly returns the world space joint matrices + void GetPose(RVec3 &outRootOffset, Mat44 *outJointMatrices, bool inLockBodies = true); + + /// This function calls ResetWarmStart on all constraints. It can be used after calling SetPose to reset previous frames impulses. See: Constraint::ResetWarmStart. + void ResetWarmStart(); + + /// Drive the ragdoll to a specific pose by setting velocities on each of the bodies so that it will reach inPose in inDeltaTime + void DriveToPoseUsingKinematics(const SkeletonPose &inPose, float inDeltaTime, bool inLockBodies = true); + + /// Lower level version of DriveToPoseUsingKinematics that directly takes the world space joint matrices + void DriveToPoseUsingKinematics(RVec3Arg inRootOffset, const Mat44 *inJointMatrices, float inDeltaTime, bool inLockBodies = true); + + /// Drive the ragdoll to a specific pose by activating the motors on each constraint + void DriveToPoseUsingMotors(const SkeletonPose &inPose); + + /// Control the linear and velocity of all bodies in the ragdoll + void SetLinearAndAngularVelocity(Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, bool inLockBodies = true); + + /// Set the world space linear velocity of all bodies in the ragdoll. + void SetLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies = true); + + /// Add a world space velocity (in m/s) to all bodies in the ragdoll. + void AddLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies = true); + + /// Add impulse to all bodies of the ragdoll (center of mass of each of them) + void AddImpulse(Vec3Arg inImpulse, bool inLockBodies = true); + + /// Get the position and orientation of the root of the ragdoll + void GetRootTransform(RVec3 &outPosition, Quat &outRotation, bool inLockBodies = true) const; + + /// Get number of bodies in the ragdoll + size_t GetBodyCount() const { return mBodyIDs.size(); } + + /// Access a body ID + BodyID GetBodyID(int inBodyIndex) const { return mBodyIDs[inBodyIndex]; } + + /// Access to the array of body IDs + const Array & GetBodyIDs() const { return mBodyIDs; } + + /// Get number of constraints in the ragdoll + size_t GetConstraintCount() const { return mConstraints.size(); } + + /// Access a constraint by index + TwoBodyConstraint * GetConstraint(int inConstraintIndex) { return mConstraints[inConstraintIndex]; } + + /// Access a constraint by index + const TwoBodyConstraint * GetConstraint(int inConstraintIndex) const { return mConstraints[inConstraintIndex]; } + + /// Get world space bounding box for all bodies of the ragdoll + AABox GetWorldSpaceBounds(bool inLockBodies = true) const; + + /// Get the settings object that created this ragdoll + const RagdollSettings * GetRagdollSettings() const { return mRagdollSettings; } + +private: + /// For RagdollSettings::CreateRagdoll function + friend class RagdollSettings; + + /// The settings that created this ragdoll + RefConst mRagdollSettings; + + /// The bodies and constraints that this ragdoll consists of (1-on-1 with mRagdollSettings->mParts) + Array mBodyIDs; + + /// Array of constraints that connect the bodies together + Array> mConstraints; + + /// Cached physics system + PhysicsSystem * mSystem; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyContactListener.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyContactListener.h new file mode 100644 index 000000000000..27e185375f34 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyContactListener.h @@ -0,0 +1,55 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +class Body; +class SoftBodyManifold; + +/// Return value for the OnSoftBodyContactValidate callback. Determines if the contact will be processed or not. +enum class SoftBodyValidateResult +{ + AcceptContact, ///< Accept this contact + RejectContact, ///< Reject this contact +}; + +/// Contact settings for a soft body contact. +/// The values are filled in with their defaults by the system so the callback doesn't need to modify anything, but it can if it wants to. +class SoftBodyContactSettings +{ +public: + float mInvMassScale1 = 1.0f; ///< Scale factor for the inverse mass of the soft body (0 = infinite mass, 1 = use original mass, 2 = body has half the mass). For the same contact pair, you should strive to keep the value the same over time. + float mInvMassScale2 = 1.0f; ///< Scale factor for the inverse mass of the other body (0 = infinite mass, 1 = use original mass, 2 = body has half the mass). For the same contact pair, you should strive to keep the value the same over time. + float mInvInertiaScale2 = 1.0f; ///< Scale factor for the inverse inertia of the other body (usually same as mInvMassScale2) + bool mIsSensor; ///< If the contact should be treated as a sensor vs body contact (no collision response) +}; + +/// A listener class that receives collision contact events for soft bodies against rigid bodies. +/// It can be registered with the PhysicsSystem. +class SoftBodyContactListener +{ +public: + /// Ensure virtual destructor + virtual ~SoftBodyContactListener() = default; + + /// Called whenever the soft body's aabox overlaps with another body's aabox (so receiving this callback doesn't tell if any of the vertices will collide). + /// This callback can be used to change the behavior of the collision response for all vertices in the soft body or to completely reject the contact. + /// Note that this callback is called when all bodies are locked, so don't use any locking functions! + /// @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 inOtherBody The other body that collided. Note that accessing the position/orientation/velocity of inOtherBody may result in a race condition as other threads may be modifying the body at the same time. + /// @param ioSettings The settings for all contact points that are generated by this collision. + /// @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. + /// 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. + virtual void OnSoftBodyContactAdded([[maybe_unused]] const Body &inSoftBody, const SoftBodyManifold &inManifold) { /* Do nothing */ } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.cpp b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.cpp new file mode 100644 index 000000000000..3ae026df64a0 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.cpp @@ -0,0 +1,122 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodyCreationSettings) +{ + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mSettings) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mPosition) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mRotation) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mUserData) + JPH_ADD_ENUM_ATTRIBUTE(SoftBodyCreationSettings, mObjectLayer) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mCollisionGroup) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mNumIterations) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mLinearDamping) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mMaxLinearVelocity) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mRestitution) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mFriction) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mPressure) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mGravityFactor) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mUpdatePosition) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mMakeRotationIdentity) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mAllowSleeping) +} + +void SoftBodyCreationSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mPosition); + inStream.Write(mRotation); + inStream.Write(mUserData); + inStream.Write(mObjectLayer); + mCollisionGroup.SaveBinaryState(inStream); + inStream.Write(mNumIterations); + inStream.Write(mLinearDamping); + inStream.Write(mMaxLinearVelocity); + inStream.Write(mRestitution); + inStream.Write(mFriction); + inStream.Write(mPressure); + inStream.Write(mGravityFactor); + inStream.Write(mUpdatePosition); + inStream.Write(mMakeRotationIdentity); + inStream.Write(mAllowSleeping); +} + +void SoftBodyCreationSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mPosition); + inStream.Read(mRotation); + inStream.Read(mUserData); + inStream.Read(mObjectLayer); + mCollisionGroup.RestoreBinaryState(inStream); + inStream.Read(mNumIterations); + inStream.Read(mLinearDamping); + inStream.Read(mMaxLinearVelocity); + inStream.Read(mRestitution); + inStream.Read(mFriction); + inStream.Read(mPressure); + inStream.Read(mGravityFactor); + inStream.Read(mUpdatePosition); + inStream.Read(mMakeRotationIdentity); + inStream.Read(mAllowSleeping); +} + +void SoftBodyCreationSettings::SaveWithChildren(StreamOut &inStream, SharedSettingsToIDMap *ioSharedSettingsMap, MaterialToIDMap *ioMaterialMap, GroupFilterToIDMap *ioGroupFilterMap) const +{ + // Save creation settings + SaveBinaryState(inStream); + + // Save shared settings + if (ioSharedSettingsMap != nullptr && ioMaterialMap != nullptr) + mSettings->SaveWithMaterials(inStream, *ioSharedSettingsMap, *ioMaterialMap); + else + inStream.Write(~uint32(0)); + + // Save group filter + StreamUtils::SaveObjectReference(inStream, mCollisionGroup.GetGroupFilter(), ioGroupFilterMap); +} + +SoftBodyCreationSettings::SBCSResult SoftBodyCreationSettings::sRestoreWithChildren(StreamIn &inStream, IDToSharedSettingsMap &ioSharedSettingsMap, IDToMaterialMap &ioMaterialMap, IDToGroupFilterMap &ioGroupFilterMap) +{ + SBCSResult result; + + // Read creation settings + SoftBodyCreationSettings settings; + settings.RestoreBinaryState(inStream); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Error reading body creation settings"); + return result; + } + + // Read shared settings + SoftBodySharedSettings::SettingsResult settings_result = SoftBodySharedSettings::sRestoreWithMaterials(inStream, ioSharedSettingsMap, ioMaterialMap); + if (settings_result.HasError()) + { + result.SetError(settings_result.GetError()); + return result; + } + settings.mSettings = settings_result.Get(); + + // Read group filter + Result gfresult = StreamUtils::RestoreObjectReference(inStream, ioGroupFilterMap); + if (gfresult.HasError()) + { + result.SetError(gfresult.GetError()); + return result; + } + settings.mCollisionGroup.SetGroupFilter(gfresult.Get()); + + result.Set(settings); + return result; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.h new file mode 100644 index 000000000000..b5dc163767ec --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.h @@ -0,0 +1,73 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// This class contains the information needed to create a soft body object +/// Note: Soft bodies are still in development and come with several caveats. Read the Architecture and API documentation for more information! +class JPH_EXPORT SoftBodyCreationSettings +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SoftBodyCreationSettings) + +public: + /// Constructor + SoftBodyCreationSettings() = default; + SoftBodyCreationSettings(const SoftBodySharedSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, ObjectLayer inObjectLayer) : mSettings(inSettings), mPosition(inPosition), mRotation(inRotation), mObjectLayer(inObjectLayer) { } + + /// Saves the state of this object in binary form to inStream. Doesn't store the shared settings nor the group filter. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restore the state of this object from inStream. Doesn't restore the shared settings nor the group filter. + void RestoreBinaryState(StreamIn &inStream); + + using GroupFilterToIDMap = StreamUtils::ObjectToIDMap; + using IDToGroupFilterMap = StreamUtils::IDToObjectMap; + using SharedSettingsToIDMap = SoftBodySharedSettings::SharedSettingsToIDMap; + using IDToSharedSettingsMap = SoftBodySharedSettings::IDToSharedSettingsMap; + using MaterialToIDMap = StreamUtils::ObjectToIDMap; + using IDToMaterialMap = StreamUtils::IDToObjectMap; + + /// Save this body creation settings, its shared settings and group filter. Pass in an empty map in ioSharedSettingsMap / ioMaterialMap / ioGroupFilterMap or reuse the same map while saving multiple shapes to the same stream in order to avoid writing duplicates. + /// Pass nullptr to ioSharedSettingsMap and ioMaterial map to skip saving shared settings and materials + /// Pass nullptr to ioGroupFilterMap to skip saving group filters + void SaveWithChildren(StreamOut &inStream, SharedSettingsToIDMap *ioSharedSettingsMap, MaterialToIDMap *ioMaterialMap, GroupFilterToIDMap *ioGroupFilterMap) const; + + using SBCSResult = Result; + + /// Restore a shape, all its children and materials. Pass in an empty map in ioSharedSettingsMap / ioMaterialMap / ioGroupFilterMap or reuse the same map while reading multiple shapes from the same stream in order to restore duplicates. + static SBCSResult sRestoreWithChildren(StreamIn &inStream, IDToSharedSettingsMap &ioSharedSettingsMap, IDToMaterialMap &ioMaterialMap, IDToGroupFilterMap &ioGroupFilterMap); + + RefConst mSettings; ///< Defines the configuration of this soft body + + RVec3 mPosition { RVec3::sZero() }; ///< Initial position of the soft body + Quat mRotation { Quat::sIdentity() }; ///< Initial rotation of the soft body + + /// User data value (can be used by application) + uint64 mUserData = 0; + + ///@name Collision settings + ObjectLayer mObjectLayer = 0; ///< The collision layer this body belongs to (determines if two objects can collide) + CollisionGroup mCollisionGroup; ///< The collision group this body belongs to (determines if two objects can collide) + + uint32 mNumIterations = 5; ///< Number of solver iterations + float mLinearDamping = 0.1f; ///< Linear damping: dv/dt = -mLinearDamping * v + float mMaxLinearVelocity = 500.0f; ///< Maximum linear velocity that a vertex can reach (m/s) + float mRestitution = 0.0f; ///< Restitution when colliding + 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 + 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 +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyManifold.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyManifold.h new file mode 100644 index 000000000000..de21ec50de0d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyManifold.h @@ -0,0 +1,74 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// An interface to query which vertices of a soft body are colliding with other bodies +class SoftBodyManifold +{ +public: + /// Get the vertices of the soft body for iterating + const Array & GetVertices() const { return mVertices; } + + /// Check if a vertex has collided with something in this update + JPH_INLINE bool HasContact(const SoftBodyVertex &inVertex) const + { + return inVertex.mHasContact; + } + + /// Get the local space contact point (multiply by GetCenterOfMassTransform() of the soft body to get world space) + JPH_INLINE Vec3 GetLocalContactPoint(const SoftBodyVertex &inVertex) const + { + return inVertex.mPosition - inVertex.mCollisionPlane.SignedDistance(inVertex.mPosition) * inVertex.mCollisionPlane.GetNormal(); + } + + /// Get the contact normal for the vertex (assumes there is a contact). + JPH_INLINE Vec3 GetContactNormal(const SoftBodyVertex &inVertex) const + { + return -inVertex.mCollisionPlane.GetNormal(); + } + + /// Get the body with which the vertex has collided in this update + JPH_INLINE BodyID GetContactBodyID(const SoftBodyVertex &inVertex) const + { + return inVertex.mHasContact? mCollidingShapes[inVertex.mCollidingShapeIndex].mBodyID : BodyID(); + } + + /// Get the number of sensors that are in contact with the soft body + JPH_INLINE uint GetNumSensorContacts() const + { + return (uint)mCollidingSensors.size(); + } + + /// Get the i-th sensor that is in contact with the soft body + JPH_INLINE BodyID GetSensorContactBodyID(uint inIndex) const + { + return mCollidingSensors[inIndex].mBodyID; + } + +private: + /// Allow SoftBodyMotionProperties to construct us + friend class SoftBodyMotionProperties; + + /// Constructor + explicit SoftBodyManifold(const SoftBodyMotionProperties *inMotionProperties) : + mVertices(inMotionProperties->mVertices), + mCollidingShapes(inMotionProperties->mCollidingShapes), + mCollidingSensors(inMotionProperties->mCollidingSensors) + { + } + + using CollidingShape = SoftBodyMotionProperties::CollidingShape; + using CollidingSensor = SoftBodyMotionProperties::CollidingSensor; + + const Array & mVertices; + const Array & mCollidingShapes; + const Array & mCollidingSensors; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp new file mode 100644 index 000000000000..0aad94de88c8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp @@ -0,0 +1,1321 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +using namespace JPH::literals; + +void SoftBodyMotionProperties::CalculateMassAndInertia() +{ + MassProperties mp; + + for (const Vertex &v : mVertices) + if (v.mInvMass > 0.0f) + { + Vec3 pos = v.mPosition; + + // Accumulate mass + float mass = 1.0f / v.mInvMass; + mp.mMass += mass; + + // Inertia tensor, diagonal + // See equations https://en.wikipedia.org/wiki/Moment_of_inertia section 'Inertia Tensor' + for (int i = 0; i < 3; ++i) + mp.mInertia(i, i) += mass * (Square(pos[(i + 1) % 3]) + Square(pos[(i + 2) % 3])); + + // Inertia tensor off diagonal + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + if (i != j) + mp.mInertia(i, j) -= mass * pos[i] * pos[j]; + } + else + { + // If one vertex is kinematic, the entire body will have infinite mass and inertia + SetInverseMass(0.0f); + SetInverseInertia(Vec3::sZero(), Quat::sIdentity()); + return; + } + + SetMassProperties(EAllowedDOFs::All, mp); +} + +void SoftBodyMotionProperties::Initialize(const SoftBodyCreationSettings &inSettings) +{ + // Store settings + mSettings = inSettings.mSettings; + mNumIterations = inSettings.mNumIterations; + mPressure = inSettings.mPressure; + mUpdatePosition = inSettings.mUpdatePosition; + + // Initialize vertices + mVertices.resize(inSettings.mSettings->mVertices.size()); + Mat44 rotation = inSettings.mMakeRotationIdentity? Mat44::sRotation(inSettings.mRotation) : Mat44::sIdentity(); + for (Array::size_type v = 0, s = mVertices.size(); v < s; ++v) + { + const SoftBodySharedSettings::Vertex &in_vertex = inSettings.mSettings->mVertices[v]; + Vertex &out_vertex = mVertices[v]; + out_vertex.mPreviousPosition = out_vertex.mPosition = rotation * Vec3(in_vertex.mPosition); + out_vertex.mVelocity = rotation.Multiply3x3(Vec3(in_vertex.mVelocity)); + out_vertex.ResetCollision(); + out_vertex.mInvMass = in_vertex.mInvMass; + mLocalBounds.Encapsulate(out_vertex.mPosition); + } + + // Allocate space for skinned vertices + if (!inSettings.mSettings->mSkinnedConstraints.empty()) + mSkinState.resize(mVertices.size()); + + // We don't know delta time yet, so we can't predict the bounds and use the local bounds as the predicted bounds + mLocalPredictedBounds = mLocalBounds; + + CalculateMassAndInertia(); +} + +float SoftBodyMotionProperties::GetVolumeTimesSix() const +{ + float six_volume = 0.0f; + for (const Face &f : mSettings->mFaces) + { + Vec3 x1 = mVertices[f.mVertex[0]].mPosition; + Vec3 x2 = mVertices[f.mVertex[1]].mPosition; + Vec3 x3 = mVertices[f.mVertex[2]].mPosition; + six_volume += x1.Cross(x2).Dot(x3); // We pick zero as the origin as this is the center of the bounding box so should give good accuracy + } + return six_volume; +} + +void SoftBodyMotionProperties::DetermineCollidingShapes(const SoftBodyUpdateContext &inContext, const PhysicsSystem &inSystem, const BodyLockInterface &inBodyLockInterface) +{ + JPH_PROFILE_FUNCTION(); + + // Reset flag prior to collision detection + mNeedContactCallback = false; + + struct Collector : public CollideShapeBodyCollector + { + Collector(const SoftBodyUpdateContext &inContext, const PhysicsSystem &inSystem, const BodyLockInterface &inBodyLockInterface, const AABox &inLocalBounds, SimShapeFilterWrapper &inShapeFilter, Array &ioHits, Array &ioSensors) : + mContext(inContext), + mInverseTransform(inContext.mCenterOfMassTransform.InversedRotationTranslation()), + mLocalBounds(inLocalBounds), + mBodyLockInterface(inBodyLockInterface), + mCombineFriction(inSystem.GetCombineFriction()), + mCombineRestitution(inSystem.GetCombineRestitution()), + mShapeFilter(inShapeFilter), + mHits(ioHits), + mSensors(ioSensors) + { + } + + virtual void AddHit(const BodyID &inResult) override + { + BodyLockRead lock(mBodyLockInterface, inResult); + if (lock.Succeeded()) + { + const Body &soft_body = *mContext.mBody; + const Body &body = lock.GetBody(); + if (body.IsRigidBody() // TODO: We should support soft body vs soft body + && soft_body.GetCollisionGroup().CanCollide(body.GetCollisionGroup())) + { + SoftBodyContactSettings settings; + settings.mIsSensor = body.IsSensor(); + + if (mContext.mContactListener == nullptr) + { + // If we have no contact listener, we can ignore sensors + if (settings.mIsSensor) + return; + } + else + { + // Call the contact listener to see if we should accept this contact + if (mContext.mContactListener->OnSoftBodyContactValidate(soft_body, body, settings) != SoftBodyValidateResult::AcceptContact) + return; + + // Check if there will be any interaction + if (!settings.mIsSensor + && settings.mInvMassScale1 == 0.0f + && (body.GetMotionType() != EMotionType::Dynamic || settings.mInvMassScale2 == 0.0f)) + return; + } + + // Calculate transform of this body relative to the soft body + Mat44 com = (mInverseTransform * body.GetCenterOfMassTransform()).ToMat44(); + + // Collect leaf shapes + mShapeFilter.SetBody2(&body); + struct LeafShapeCollector : public TransformedShapeCollector + { + virtual void AddHit(const TransformedShape &inResult) override + { + mHits.emplace_back(Mat44::sRotationTranslation(inResult.mShapeRotation, Vec3(inResult.mShapePositionCOM)), inResult.GetShapeScale(), inResult.mShape); + } + + Array mHits; + }; + LeafShapeCollector collector; + body.GetShape()->CollectTransformedShapes(mLocalBounds, com.GetTranslation(), com.GetQuaternion(), Vec3::sReplicate(1.0f), SubShapeIDCreator(), collector, mShapeFilter); + if (collector.mHits.empty()) + return; + + if (settings.mIsSensor) + { + CollidingSensor cs; + cs.mCenterOfMassTransform = com; + cs.mShapes = std::move(collector.mHits); + cs.mBodyID = inResult; + mSensors.push_back(cs); + } + else + { + CollidingShape cs; + cs.mCenterOfMassTransform = com; + cs.mShapes = std::move(collector.mHits); + cs.mBodyID = inResult; + cs.mMotionType = body.GetMotionType(); + cs.mUpdateVelocities = false; + cs.mFriction = mCombineFriction(soft_body, SubShapeID(), body, SubShapeID()); + cs.mRestitution = mCombineRestitution(soft_body, SubShapeID(), body, SubShapeID()); + cs.mSoftBodyInvMassScale = settings.mInvMassScale1; + if (cs.mMotionType == EMotionType::Dynamic) + { + const MotionProperties *mp = body.GetMotionProperties(); + cs.mInvMass = settings.mInvMassScale2 * mp->GetInverseMass(); + cs.mInvInertia = settings.mInvInertiaScale2 * mp->GetInverseInertiaForRotation(cs.mCenterOfMassTransform.GetRotation()); + cs.mOriginalLinearVelocity = cs.mLinearVelocity = mInverseTransform.Multiply3x3(mp->GetLinearVelocity()); + cs.mOriginalAngularVelocity = cs.mAngularVelocity = mInverseTransform.Multiply3x3(mp->GetAngularVelocity()); + } + mHits.push_back(cs); + } + } + } + } + + private: + const SoftBodyUpdateContext &mContext; + RMat44 mInverseTransform; + AABox mLocalBounds; + const BodyLockInterface & mBodyLockInterface; + ContactConstraintManager::CombineFunction mCombineFriction; + ContactConstraintManager::CombineFunction mCombineRestitution; + SimShapeFilterWrapper & mShapeFilter; + Array & mHits; + Array & mSensors; + }; + + // Calculate local bounding box + AABox local_bounds = mLocalBounds; + local_bounds.Encapsulate(mLocalPredictedBounds); + local_bounds.ExpandBy(Vec3::sReplicate(mSettings->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(); + + Collector collector(inContext, inSystem, inBodyLockInterface, local_bounds, shape_filter, mCollidingShapes, mCollidingSensors); + ObjectLayer layer = inContext.mBody->GetObjectLayer(); + DefaultBroadPhaseLayerFilter broadphase_layer_filter = inSystem.GetDefaultBroadPhaseLayerFilter(layer); + DefaultObjectLayerFilter object_layer_filter = inSystem.GetDefaultLayerFilter(layer); + inSystem.GetBroadPhaseQuery().CollideAABox(world_bounds, collector, broadphase_layer_filter, object_layer_filter); +} + +void SoftBodyMotionProperties::DetermineCollisionPlanes(uint inVertexStart, uint inNumVertices) +{ + JPH_PROFILE_FUNCTION(); + + // Generate collision planes + for (const CollidingShape &cs : mCollidingShapes) + for (const LeafShape &shape : cs.mShapes) + shape.mShape->CollideSoftBodyVertices(shape.mTransform, shape.mScale, CollideSoftBodyVertexIterator(mVertices.data() + inVertexStart), inNumVertices, int(&cs - mCollidingShapes.data())); +} + +void SoftBodyMotionProperties::DetermineSensorCollisions(CollidingSensor &ioSensor) +{ + JPH_PROFILE_FUNCTION(); + + Plane collision_plane; + float largest_penetration = -FLT_MAX; + int colliding_shape_idx = -1; + + // Collide sensor against all vertices + CollideSoftBodyVertexIterator vertex_iterator( + StridedPtr(&mVertices[0].mPosition, sizeof(SoftBodyVertex)), // The position and mass come from the soft body vertex + StridedPtr(&mVertices[0].mInvMass, sizeof(SoftBodyVertex)), + StridedPtr(&collision_plane, 0), // We want all vertices to result in a single collision so we pass stride 0 + StridedPtr(&largest_penetration, 0), + StridedPtr(&colliding_shape_idx, 0)); + for (const LeafShape &shape : ioSensor.mShapes) + shape.mShape->CollideSoftBodyVertices(shape.mTransform, shape.mScale, vertex_iterator, uint(mVertices.size()), 0); + ioSensor.mHasContact = largest_penetration > 0.0f; + + // We need a contact callback if one of the sensors collided + if (ioSensor.mHasContact) + mNeedContactCallback = true; +} + +void SoftBodyMotionProperties::ApplyPressure(const SoftBodyUpdateContext &inContext) +{ + JPH_PROFILE_FUNCTION(); + + float dt = inContext.mSubStepDeltaTime; + float pressure_coefficient = mPressure; + if (pressure_coefficient > 0.0f) + { + // Calculate total volume + float six_volume = GetVolumeTimesSix(); + if (six_volume > 0.0f) + { + // Apply pressure + // p = F / A = n R T / V (see https://en.wikipedia.org/wiki/Pressure) + // Our pressure coefficient is n R T so the impulse is: + // P = F dt = pressure_coefficient / V * A * dt + float coefficient = pressure_coefficient * dt / six_volume; // Need to still multiply by 6 for the volume + for (const Face &f : mSettings->mFaces) + { + Vec3 x1 = mVertices[f.mVertex[0]].mPosition; + Vec3 x2 = mVertices[f.mVertex[1]].mPosition; + Vec3 x3 = mVertices[f.mVertex[2]].mPosition; + + Vec3 impulse = coefficient * (x2 - x1).Cross(x3 - x1); // Area is half the cross product so need to still divide by 2 + for (uint32 i : f.mVertex) + { + Vertex &v = mVertices[i]; + v.mVelocity += v.mInvMass * impulse; // Want to divide by 3 because we spread over 3 vertices + } + } + } + } +} + +void SoftBodyMotionProperties::IntegratePositions(const SoftBodyUpdateContext &inContext) +{ + JPH_PROFILE_FUNCTION(); + + float dt = inContext.mSubStepDeltaTime; + float linear_damping = max(0.0f, 1.0f - GetLinearDamping() * dt); // See: MotionProperties::ApplyForceTorqueAndDragInternal + + // Integrate + Vec3 sub_step_gravity = inContext.mGravity * dt; + Vec3 sub_step_impulse = GetAccumulatedForce() * dt; + for (Vertex &v : mVertices) + if (v.mInvMass > 0.0f) + { + // Gravity + v.mVelocity += sub_step_gravity + sub_step_impulse * v.mInvMass; + + // 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; + } +} + +void SoftBodyMotionProperties::ApplyDihedralBendConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex) +{ + JPH_PROFILE_FUNCTION(); + + float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime); + + for (const DihedralBend *b = mSettings->mDihedralBendConstraints.data() + inStartIndex, *b_end = mSettings->mDihedralBendConstraints.data() + inEndIndex; b < b_end; ++b) + { + Vertex &v0 = mVertices[b->mVertex[0]]; + Vertex &v1 = mVertices[b->mVertex[1]]; + Vertex &v2 = mVertices[b->mVertex[2]]; + Vertex &v3 = mVertices[b->mVertex[3]]; + + // Get positions + Vec3 x0 = v0.mPosition; + Vec3 x1 = v1.mPosition; + Vec3 x2 = v2.mPosition; + Vec3 x3 = v3.mPosition; + + /* + x2 + e1/ \e3 + / \ + x0----x1 + \ e0 / + e2\ /e4 + x3 + */ + + // Calculate the shared edge of the triangles + Vec3 e = x1 - x0; + float e_len = e.Length(); + if (e_len < 1.0e-6f) + continue; + + // Calculate the normals of the triangles + Vec3 x1x2 = x2 - x1; + Vec3 x1x3 = x3 - x1; + Vec3 n1 = (x2 - x0).Cross(x1x2); + Vec3 n2 = x1x3.Cross(x3 - x0); + float n1_len_sq = n1.LengthSq(); + float n2_len_sq = n2.LengthSq(); + float n1_len_sq_n2_len_sq = n1_len_sq * n2_len_sq; + if (n1_len_sq_n2_len_sq < 1.0e-24f) + continue; + + // Calculate constraint equation + // As per "Strain Based Dynamics" Appendix A we need to negate the gradients when (n1 x n2) . e > 0, instead we make sure that the sign of the constraint equation is correct + float sign = Sign(n2.Cross(n1).Dot(e)); + float d = n1.Dot(n2) / sqrt(n1_len_sq_n2_len_sq); + float c = sign * ACosApproximate(d) - b->mInitialAngle; + + // Ensure the range is -PI to PI + if (c > JPH_PI) + c -= 2.0f * JPH_PI; + else if (c < -JPH_PI) + c += 2.0f * JPH_PI; + + // Calculate gradient of constraint equation + // Taken from "Strain Based Dynamics" - Matthias Muller et al. (Appendix A) + // with p1 = x2, p2 = x3, p3 = x0 and p4 = x1 + // which in turn is based on "Simulation of Clothing with Folds and Wrinkles" - R. Bridson et al. (Section 4) + n1 /= n1_len_sq; + n2 /= n2_len_sq; + Vec3 d0c = (x1x2.Dot(e) * n1 + x1x3.Dot(e) * n2) / e_len; + Vec3 d2c = e_len * n1; + Vec3 d3c = e_len * n2; + + // The sum of the gradients must be zero (see "Strain Based Dynamics" section 4) + Vec3 d1c = -d0c - d2c - d3c; + + // Get masses + float w0 = v0.mInvMass; + float w1 = v1.mInvMass; + float w2 = v2.mInvMass; + float w3 = v3.mInvMass; + + // Calculate -lambda + float denom = w0 * d0c.LengthSq() + w1 * d1c.LengthSq() + w2 * d2c.LengthSq() + w3 * d3c.LengthSq() + b->mCompliance * inv_dt_sq; + if (denom < 1.0e-12f) + continue; + float minus_lambda = c / denom; + + // Apply correction + v0.mPosition = x0 - minus_lambda * w0 * d0c; + v1.mPosition = x1 - minus_lambda * w1 * d1c; + v2.mPosition = x2 - minus_lambda * w2 * d2c; + v3.mPosition = x3 - minus_lambda * w3 * d3c; + } +} + +void SoftBodyMotionProperties::ApplyVolumeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex) +{ + JPH_PROFILE_FUNCTION(); + + float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime); + + // Satisfy volume constraints + for (const Volume *v = mSettings->mVolumeConstraints.data() + inStartIndex, *v_end = mSettings->mVolumeConstraints.data() + inEndIndex; v < v_end; ++v) + { + Vertex &v1 = mVertices[v->mVertex[0]]; + Vertex &v2 = mVertices[v->mVertex[1]]; + Vertex &v3 = mVertices[v->mVertex[2]]; + Vertex &v4 = mVertices[v->mVertex[3]]; + + Vec3 x1 = v1.mPosition; + Vec3 x2 = v2.mPosition; + Vec3 x3 = v3.mPosition; + Vec3 x4 = v4.mPosition; + + // Calculate constraint equation + Vec3 x1x2 = x2 - x1; + Vec3 x1x3 = x3 - x1; + Vec3 x1x4 = x4 - x1; + float c = abs(x1x2.Cross(x1x3).Dot(x1x4)) - v->mSixRestVolume; + + // Calculate gradient of constraint equation + Vec3 d1c = (x4 - x2).Cross(x3 - x2); + Vec3 d2c = x1x3.Cross(x1x4); + Vec3 d3c = x1x4.Cross(x1x2); + Vec3 d4c = x1x2.Cross(x1x3); + + // Get masses + float w1 = v1.mInvMass; + float w2 = v2.mInvMass; + float w3 = v3.mInvMass; + float w4 = v4.mInvMass; + + // Calculate -lambda + float denom = w1 * d1c.LengthSq() + w2 * d2c.LengthSq() + w3 * d3c.LengthSq() + w4 * d4c.LengthSq() + v->mCompliance * inv_dt_sq; + if (denom < 1.0e-12f) + continue; + float minus_lambda = c / denom; + + // Apply correction + v1.mPosition = x1 - minus_lambda * w1 * d1c; + v2.mPosition = x2 - minus_lambda * w2 * d2c; + v3.mPosition = x3 - minus_lambda * w3 * d3c; + v4.mPosition = x4 - minus_lambda * w4 * d4c; + } +} + +void SoftBodyMotionProperties::ApplySkinConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex) +{ + // Early out if nothing to do + if (mSettings->mSkinnedConstraints.empty() || !mEnableSkinConstraints) + return; + + JPH_PROFILE_FUNCTION(); + + // We're going to iterate multiple times over the skin constraints, update the skinned position accordingly. + // If we don't do this, the simulation will see a big jump and the first iteration will cause a big velocity change in the system. + float factor = mSkinStatePreviousPositionValid? inContext.mNextIteration.load(std::memory_order_relaxed) / float(mNumIterations) : 1.0f; + float prev_factor = 1.0f - factor; + + // Apply the constraints + Vertex *vertices = mVertices.data(); + const SkinState *skin_states = mSkinState.data(); + for (const Skinned *s = mSettings->mSkinnedConstraints.data() + inStartIndex, *s_end = mSettings->mSkinnedConstraints.data() + inEndIndex; s < s_end; ++s) + { + Vertex &vertex = vertices[s->mVertex]; + const SkinState &skin_state = skin_states[s->mVertex]; + float max_distance = s->mMaxDistance * mSkinnedMaxDistanceMultiplier; + + // Calculate the skinned position by interpolating from previous to current position + Vec3 skin_pos = prev_factor * skin_state.mPreviousPosition + factor * skin_state.mPosition; + + if (max_distance > 0.0f) + { + // Move vertex if it violated the back stop + if (s->mBackStopDistance < max_distance) + { + // Center of the back stop sphere + Vec3 center = skin_pos - skin_state.mNormal * (s->mBackStopDistance + s->mBackStopRadius); + + // Check if we're inside the back stop sphere + Vec3 delta = vertex.mPosition - center; + float delta_len_sq = delta.LengthSq(); + if (delta_len_sq < Square(s->mBackStopRadius)) + { + // Push the vertex to the surface of the back stop sphere + float delta_len = sqrt(delta_len_sq); + vertex.mPosition = delta_len > 0.0f? + center + delta * (s->mBackStopRadius / delta_len) + : center + skin_state.mNormal * s->mBackStopRadius; + } + } + + // Clamp vertex distance to max distance from skinned position + if (max_distance < FLT_MAX) + { + Vec3 delta = vertex.mPosition - skin_pos; + float delta_len_sq = delta.LengthSq(); + float max_distance_sq = Square(max_distance); + if (delta_len_sq > max_distance_sq) + vertex.mPosition = skin_pos + delta * sqrt(max_distance_sq / delta_len_sq); + } + } + else + { + // Kinematic: Just update the vertex position + vertex.mPosition = skin_pos; + } + } +} + +void SoftBodyMotionProperties::ApplyEdgeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex) +{ + JPH_PROFILE_FUNCTION(); + + float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime); + + // Satisfy edge constraints + for (const Edge *e = mSettings->mEdgeConstraints.data() + inStartIndex, *e_end = mSettings->mEdgeConstraints.data() + inEndIndex; e < e_end; ++e) + { + Vertex &v0 = mVertices[e->mVertex[0]]; + Vertex &v1 = mVertices[e->mVertex[1]]; + + // Get positions + Vec3 x0 = v0.mPosition; + Vec3 x1 = v1.mPosition; + + // Calculate current length + Vec3 delta = x1 - x0; + float length = delta.Length(); + + // Apply correction + float denom = length * (v0.mInvMass + v1.mInvMass + e->mCompliance * inv_dt_sq); + if (denom < 1.0e-12f) + continue; + Vec3 correction = delta * (length - e->mRestLength) / denom; + v0.mPosition = x0 + v0.mInvMass * correction; + v1.mPosition = x1 - v1.mInvMass * correction; + } +} + +void SoftBodyMotionProperties::ApplyLRAConstraints(uint inStartIndex, uint inEndIndex) +{ + JPH_PROFILE_FUNCTION(); + + // Satisfy LRA constraints + Vertex *vertices = mVertices.data(); + for (const LRA *lra = mSettings->mLRAConstraints.data() + inStartIndex, *lra_end = mSettings->mLRAConstraints.data() + inEndIndex; lra < lra_end; ++lra) + { + JPH_ASSERT(lra->mVertex[0] < mVertices.size()); + JPH_ASSERT(lra->mVertex[1] < mVertices.size()); + const Vertex &vertex0 = vertices[lra->mVertex[0]]; + Vertex &vertex1 = vertices[lra->mVertex[1]]; + + Vec3 x0 = vertex0.mPosition; + Vec3 delta = vertex1.mPosition - x0; + float delta_len_sq = delta.LengthSq(); + if (delta_len_sq > Square(lra->mMaxDistance)) + vertex1.mPosition = x0 + delta * lra->mMaxDistance / sqrt(delta_len_sq); + } +} + +void SoftBodyMotionProperties::ApplyCollisionConstraintsAndUpdateVelocities(const SoftBodyUpdateContext &inContext) +{ + JPH_PROFILE_FUNCTION(); + + float dt = inContext.mSubStepDeltaTime; + float restitution_treshold = -2.0f * inContext.mGravity.Length() * dt; + float vertex_radius = mSettings->mVertexRadius; + for (Vertex &v : mVertices) + if (v.mInvMass > 0.0f) + { + // Remember previous velocity for restitution calculations + Vec3 prev_v = v.mVelocity; + + // XPBD velocity update + v.mVelocity = (v.mPosition - v.mPreviousPosition) / dt; + + // Satisfy collision constraint + if (v.mCollidingShapeIndex >= 0) + { + // Check if there is a collision + float projected_distance = -v.mCollisionPlane.SignedDistance(v.mPosition) + vertex_radius; + if (projected_distance > 0.0f) + { + // Remember that there was a collision + v.mHasContact = true; + + // We need a contact callback if one of the vertices collided + mNeedContactCallback = true; + + // Note that we already calculated the velocity, so this does not affect the velocity (next iteration starts by setting previous position to current position) + CollidingShape &cs = mCollidingShapes[v.mCollidingShapeIndex]; + Vec3 contact_normal = v.mCollisionPlane.GetNormal(); + v.mPosition += contact_normal * projected_distance; + + // Apply friction as described in Detailed Rigid Body Simulation with Extended Position Based Dynamics - Matthias Muller et al. + // See section 3.6: + // Inverse mass: w1 = 1 / m1, w2 = 1 / m2 + (r2 x n)^T I^-1 (r2 x n) = 0 for a static object + // r2 are the contact point relative to the center of mass of body 2 + // Lagrange multiplier for contact: lambda = -c / (w1 + w2) + // Where c is the constraint equation (the distance to the plane, negative because penetrating) + // Contact normal force: fn = lambda / dt^2 + // Delta velocity due to friction dv = -vt / |vt| * min(dt * friction * fn * (w1 + w2), |vt|) = -vt * min(-friction * c / (|vt| * dt), 1) + // Note that I think there is an error in the paper, I added a mass term, see: https://github.com/matthias-research/pages/issues/29 + // Relative velocity: vr = v1 - v2 - omega2 x r2 + // Normal velocity: vn = vr . contact_normal + // Tangential velocity: vt = vr - contact_normal * vn + // Impulse: p = dv / (w1 + w2) + // Changes in particle velocities: + // v1 = v1 + p / m1 + // v2 = v2 - p / m2 (no change when colliding with a static body) + // w2 = w2 - I^-1 (r2 x p) (no change when colliding with a static body) + if (cs.mMotionType == EMotionType::Dynamic) + { + // Calculate normal and tangential velocity (equation 30) + Vec3 r2 = v.mPosition - cs.mCenterOfMassTransform.GetTranslation(); + Vec3 v2 = cs.GetPointVelocity(r2); + Vec3 relative_velocity = v.mVelocity - v2; + Vec3 v_normal = contact_normal * contact_normal.Dot(relative_velocity); + Vec3 v_tangential = relative_velocity - v_normal; + float v_tangential_length = v_tangential.Length(); + + // Calculate resulting inverse mass of vertex + float vertex_inv_mass = cs.mSoftBodyInvMassScale * v.mInvMass; + + // Calculate inverse effective mass + Vec3 r2_cross_n = r2.Cross(contact_normal); + float w2 = cs.mInvMass + r2_cross_n.Dot(cs.mInvInertia * r2_cross_n); + float w1_plus_w2 = vertex_inv_mass + w2; + if (w1_plus_w2 > 0.0f) + { + // Calculate delta relative velocity due to friction (modified equation 31) + Vec3 dv; + if (v_tangential_length > 0.0f) + dv = v_tangential * min(cs.mFriction * projected_distance / (v_tangential_length * dt), 1.0f); + else + dv = Vec3::sZero(); + + // Calculate delta relative velocity due to restitution (equation 35) + dv += v_normal; + float prev_v_normal = (prev_v - v2).Dot(contact_normal); + if (prev_v_normal < restitution_treshold) + dv += cs.mRestitution * prev_v_normal * contact_normal; + + // Calculate impulse + Vec3 p = dv / w1_plus_w2; + + // Apply impulse to particle + v.mVelocity -= p * vertex_inv_mass; + + // Apply impulse to rigid body + cs.mLinearVelocity += p * cs.mInvMass; + cs.mAngularVelocity += cs.mInvInertia * r2.Cross(p); + + // Mark that the velocities of the body we hit need to be updated + cs.mUpdateVelocities = true; + } + } + else if (cs.mSoftBodyInvMassScale > 0.0f) + { + // Body is not movable, equations are simpler + + // Calculate normal and tangential velocity (equation 30) + Vec3 v_normal = contact_normal * contact_normal.Dot(v.mVelocity); + Vec3 v_tangential = v.mVelocity - v_normal; + float v_tangential_length = v_tangential.Length(); + + // Apply friction (modified equation 31) + if (v_tangential_length > 0.0f) + v.mVelocity -= v_tangential * min(cs.mFriction * projected_distance / (v_tangential_length * dt), 1.0f); + + // Apply restitution (equation 35) + v.mVelocity -= v_normal; + float prev_v_normal = prev_v.Dot(contact_normal); + if (prev_v_normal < restitution_treshold) + v.mVelocity -= cs.mRestitution * prev_v_normal * contact_normal; + } + } + } + } +} + +void SoftBodyMotionProperties::UpdateSoftBodyState(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings) +{ + JPH_PROFILE_FUNCTION(); + + // Contact callback + if (mNeedContactCallback && ioContext.mContactListener != nullptr) + { + // Remove non-colliding sensors from the list + for (int i = int(mCollidingSensors.size()) - 1; i >= 0; --i) + if (!mCollidingSensors[i].mHasContact) + { + mCollidingSensors[i] = std::move(mCollidingSensors.back()); + mCollidingSensors.pop_back(); + } + + ioContext.mContactListener->OnSoftBodyContactAdded(*ioContext.mBody, SoftBodyManifold(this)); + } + + // Loop through vertices once more to update the global state + float dt = ioContext.mDeltaTime; + float max_linear_velocity_sq = Square(GetMaxLinearVelocity()); + float max_v_sq = 0.0f; + Vec3 linear_velocity = Vec3::sZero(), angular_velocity = Vec3::sZero(); + mLocalPredictedBounds = mLocalBounds = { }; + for (Vertex &v : mVertices) + { + // Calculate max square velocity + float v_sq = v.mVelocity.LengthSq(); + max_v_sq = max(max_v_sq, v_sq); + + // Clamp if velocity is too high + if (v_sq > max_linear_velocity_sq) + v.mVelocity *= sqrt(max_linear_velocity_sq / v_sq); + + // Calculate local linear/angular velocity + linear_velocity += v.mVelocity; + angular_velocity += v.mPosition.Cross(v.mVelocity); + + // Update local bounding box + mLocalBounds.Encapsulate(v.mPosition); + + // Create predicted position for the next frame in order to detect collisions before they happen + mLocalPredictedBounds.Encapsulate(v.mPosition + v.mVelocity * dt + ioContext.mDisplacementDueToGravity); + + // Reset collision data for the next iteration + v.ResetCollision(); + } + + // Calculate linear/angular velocity of the body by averaging all vertices and bringing the value to world space + float num_vertices_divider = float(max(int(mVertices.size()), 1)); + SetLinearVelocity(ioContext.mCenterOfMassTransform.Multiply3x3(linear_velocity / num_vertices_divider)); + SetAngularVelocity(ioContext.mCenterOfMassTransform.Multiply3x3(angular_velocity / num_vertices_divider)); + + if (mUpdatePosition) + { + // Shift the body so that the position is the center of the local bounds + Vec3 delta = mLocalBounds.GetCenter(); + ioContext.mDeltaPosition = ioContext.mCenterOfMassTransform.Multiply3x3(delta); + for (Vertex &v : mVertices) + v.mPosition -= delta; + + // Update the skin state too since we will use this position as the previous position in the next update + for (SkinState &s : mSkinState) + s.mPosition -= delta; + JPH_IF_DEBUG_RENDERER(mSkinStateTransform.SetTranslation(mSkinStateTransform.GetTranslation() + ioContext.mDeltaPosition);) + + // Offset bounds to match new position + mLocalBounds.Translate(-delta); + mLocalPredictedBounds.Translate(-delta); + } + else + ioContext.mDeltaPosition = Vec3::sZero(); + + // Test if we should go to sleep + if (GetAllowSleeping()) + { + if (max_v_sq > inPhysicsSettings.mPointVelocitySleepThreshold) + { + ResetSleepTestTimer(); + ioContext.mCanSleep = ECanSleep::CannotSleep; + } + else + ioContext.mCanSleep = AccumulateSleepTime(dt, inPhysicsSettings.mTimeBeforeSleep); + } + else + ioContext.mCanSleep = ECanSleep::CannotSleep; + + // If SkinVertices is not called after this then don't use the previous position as the skin is static + mSkinStatePreviousPositionValid = false; + + // Reset force accumulator + ResetForce(); +} + +void SoftBodyMotionProperties::UpdateRigidBodyVelocities(const SoftBodyUpdateContext &inContext, BodyInterface &inBodyInterface) +{ + JPH_PROFILE_FUNCTION(); + + // Write back velocity deltas + for (const CollidingShape &cs : mCollidingShapes) + if (cs.mUpdateVelocities) + inBodyInterface.AddLinearAndAngularVelocity(cs.mBodyID, inContext.mCenterOfMassTransform.Multiply3x3(cs.mLinearVelocity - cs.mOriginalLinearVelocity), inContext.mCenterOfMassTransform.Multiply3x3(cs.mAngularVelocity - cs.mOriginalAngularVelocity)); + + // Clear colliding shapes/sensors to avoid hanging on to references to shapes + mCollidingShapes.clear(); + mCollidingSensors.clear(); +} + +void SoftBodyMotionProperties::InitializeUpdateContext(float inDeltaTime, Body &inSoftBody, const PhysicsSystem &inSystem, SoftBodyUpdateContext &ioContext) +{ + JPH_PROFILE_FUNCTION(); + + // Store body + ioContext.mBody = &inSoftBody; + ioContext.mMotionProperties = this; + ioContext.mContactListener = inSystem.GetSoftBodyContactListener(); + ioContext.mSimShapeFilter = inSystem.GetSimShapeFilter(); + + // Convert gravity to local space + ioContext.mCenterOfMassTransform = inSoftBody.GetCenterOfMassTransform(); + ioContext.mGravity = ioContext.mCenterOfMassTransform.Multiply3x3Transposed(GetGravityFactor() * inSystem.GetGravity()); + + // Calculate delta time for sub step + ioContext.mDeltaTime = inDeltaTime; + ioContext.mSubStepDeltaTime = inDeltaTime / mNumIterations; + + // Calculate total displacement we'll have due to gravity over all sub steps + // The total displacement as produced by our integrator can be written as: Sum(i * g * dt^2, i = 0..mNumIterations). + // This is bigger than 0.5 * g * dt^2 because we first increment the velocity and then update the position + // Using Sum(i, i = 0..n) = n * (n + 1) / 2 we can write this as: + ioContext.mDisplacementDueToGravity = (0.5f * mNumIterations * (mNumIterations + 1) * Square(ioContext.mSubStepDeltaTime)) * ioContext.mGravity; +} + +void SoftBodyMotionProperties::StartNextIteration(const SoftBodyUpdateContext &ioContext) +{ + ApplyPressure(ioContext); + + IntegratePositions(ioContext); +} + +void SoftBodyMotionProperties::StartFirstIteration(SoftBodyUpdateContext &ioContext) +{ + // Start the first iteration + JPH_IF_ENABLE_ASSERTS(uint iteration =) ioContext.mNextIteration.fetch_add(1, memory_order_relaxed); + JPH_ASSERT(iteration == 0); + StartNextIteration(ioContext); + ioContext.mState.store(SoftBodyUpdateContext::EState::ApplyConstraints, memory_order_release); +} + +SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineCollisionPlanes(SoftBodyUpdateContext &ioContext) +{ + // Do a relaxed read first to see if there is any work to do (this prevents us from doing expensive atomic operations and also prevents us from continuously incrementing the counter and overflowing it) + uint num_vertices = (uint)mVertices.size(); + if (ioContext.mNextCollisionVertex.load(memory_order_relaxed) < num_vertices) + { + // Fetch next batch of vertices to process + uint next_vertex = ioContext.mNextCollisionVertex.fetch_add(SoftBodyUpdateContext::cVertexCollisionBatch, memory_order_acquire); + if (next_vertex < num_vertices) + { + // Process collision planes + uint num_vertices_to_process = min(SoftBodyUpdateContext::cVertexCollisionBatch, num_vertices - next_vertex); + DetermineCollisionPlanes(next_vertex, num_vertices_to_process); + uint vertices_processed = ioContext.mNumCollisionVerticesProcessed.fetch_add(SoftBodyUpdateContext::cVertexCollisionBatch, memory_order_release) + num_vertices_to_process; + if (vertices_processed >= num_vertices) + { + // Determine next state + if (mCollidingSensors.empty()) + StartFirstIteration(ioContext); + else + ioContext.mState.store(SoftBodyUpdateContext::EState::DetermineSensorCollisions, memory_order_release); + } + return EStatus::DidWork; + } + } + + return EStatus::NoWork; +} + +SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineSensorCollisions(SoftBodyUpdateContext &ioContext) +{ + // Do a relaxed read to see if there are more sensors to process + uint num_sensors = (uint)mCollidingSensors.size(); + if (ioContext.mNextSensorIndex.load(memory_order_relaxed) < num_sensors) + { + // Fetch next sensor to process + uint sensor_index = ioContext.mNextSensorIndex.fetch_add(1, memory_order_acquire); + if (sensor_index < num_sensors) + { + // Process this sensor + DetermineSensorCollisions(mCollidingSensors[sensor_index]); + + // Determine next state + uint sensors_processed = ioContext.mNumSensorsProcessed.fetch_add(1, memory_order_release) + 1; + if (sensors_processed >= num_sensors) + StartFirstIteration(ioContext); + return EStatus::DidWork; + } + } + + return EStatus::NoWork; +} + +void SoftBodyMotionProperties::ProcessGroup(const SoftBodyUpdateContext &ioContext, uint inGroupIndex) +{ + // Determine start and end + SoftBodySharedSettings::UpdateGroup start { 0, 0, 0, 0, 0 }; + const SoftBodySharedSettings::UpdateGroup &prev = inGroupIndex > 0? mSettings->mUpdateGroups[inGroupIndex - 1] : start; + const SoftBodySharedSettings::UpdateGroup ¤t = mSettings->mUpdateGroups[inGroupIndex]; + + // Process volume constraints + ApplyVolumeConstraints(ioContext, prev.mVolumeEndIndex, current.mVolumeEndIndex); + + // Process bend constraints + ApplyDihedralBendConstraints(ioContext, prev.mDihedralBendEndIndex, current.mDihedralBendEndIndex); + + // Process skinned constraints + ApplySkinConstraints(ioContext, prev.mSkinnedEndIndex, current.mSkinnedEndIndex); + + // Process edges + ApplyEdgeConstraints(ioContext, prev.mEdgeEndIndex, current.mEdgeEndIndex); + + // Process LRA constraints + ApplyLRAConstraints(prev.mLRAEndIndex, current.mLRAEndIndex); +} + +SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelApplyConstraints(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings) +{ + uint num_groups = (uint)mSettings->mUpdateGroups.size(); + JPH_ASSERT(num_groups > 0, "SoftBodySharedSettings::Optimize should have been called!"); + --num_groups; // Last group is the non-parallel group, we don't want to execute it in parallel + + // Do a relaxed read first to see if there is any work to do (this prevents us from doing expensive atomic operations and also prevents us from continuously incrementing the counter and overflowing it) + uint next_group = ioContext.mNextConstraintGroup.load(memory_order_relaxed); + if (next_group < num_groups || (num_groups == 0 && next_group == 0)) + { + // Fetch the next group process + next_group = ioContext.mNextConstraintGroup.fetch_add(1, memory_order_acquire); + if (next_group < num_groups || (num_groups == 0 && next_group == 0)) + { + uint num_groups_processed = 0; + if (num_groups > 0) + { + // Process this group + ProcessGroup(ioContext, next_group); + + // Increment total number of groups processed + num_groups_processed = ioContext.mNumConstraintGroupsProcessed.fetch_add(1, memory_order_relaxed) + 1; + } + + if (num_groups_processed >= num_groups) + { + // Finish the iteration + JPH_PROFILE("FinishIteration"); + + // Process non-parallel group + ProcessGroup(ioContext, num_groups); + + ApplyCollisionConstraintsAndUpdateVelocities(ioContext); + + uint iteration = ioContext.mNextIteration.fetch_add(1, memory_order_relaxed); + if (iteration < mNumIterations) + { + // Start a new iteration + StartNextIteration(ioContext); + + // Reset group logic + ioContext.mNumConstraintGroupsProcessed.store(0, memory_order_relaxed); + ioContext.mNextConstraintGroup.store(0, memory_order_release); + } + else + { + // On final iteration we update the state + UpdateSoftBodyState(ioContext, inPhysicsSettings); + + ioContext.mState.store(SoftBodyUpdateContext::EState::Done, memory_order_release); + return EStatus::Done; + } + } + + return EStatus::DidWork; + } + } + return EStatus::NoWork; +} + +SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelUpdate(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings) +{ + switch (ioContext.mState.load(memory_order_relaxed)) + { + case SoftBodyUpdateContext::EState::DetermineCollisionPlanes: + return ParallelDetermineCollisionPlanes(ioContext); + + case SoftBodyUpdateContext::EState::DetermineSensorCollisions: + return ParallelDetermineSensorCollisions(ioContext); + + case SoftBodyUpdateContext::EState::ApplyConstraints: + return ParallelApplyConstraints(ioContext, inPhysicsSettings); + + case SoftBodyUpdateContext::EState::Done: + return EStatus::Done; + + default: + JPH_ASSERT(false); + return EStatus::NoWork; + } +} + +void SoftBodyMotionProperties::SkinVertices([[maybe_unused]] RMat44Arg inCenterOfMassTransform, const Mat44 *inJointMatrices, [[maybe_unused]] uint inNumJoints, bool inHardSkinAll, TempAllocator &ioTempAllocator) +{ + // Calculate the skin matrices + uint num_skin_matrices = uint(mSettings->mInvBindMatrices.size()); + uint skin_matrices_size = num_skin_matrices * sizeof(Mat44); + Mat44 *skin_matrices = (Mat44 *)ioTempAllocator.Allocate(skin_matrices_size); + JPH_SCOPE_EXIT([&ioTempAllocator, skin_matrices, skin_matrices_size]{ ioTempAllocator.Free(skin_matrices, skin_matrices_size); }); + const Mat44 *skin_matrices_end = skin_matrices + num_skin_matrices; + const InvBind *inv_bind_matrix = mSettings->mInvBindMatrices.data(); + for (Mat44 *s = skin_matrices; s < skin_matrices_end; ++s, ++inv_bind_matrix) + *s = inJointMatrices[inv_bind_matrix->mJointIndex] * inv_bind_matrix->mInvBind; + + // Skin the vertices + JPH_IF_DEBUG_RENDERER(mSkinStateTransform = inCenterOfMassTransform;) + JPH_IF_ENABLE_ASSERTS(uint num_vertices = uint(mSettings->mVertices.size());) + JPH_ASSERT(mSkinState.size() == num_vertices); + const SoftBodySharedSettings::Vertex *in_vertices = mSettings->mVertices.data(); + for (const Skinned &s : mSettings->mSkinnedConstraints) + { + // Get bind pose + JPH_ASSERT(s.mVertex < num_vertices); + Vec3 bind_pos = Vec3::sLoadFloat3Unsafe(in_vertices[s.mVertex].mPosition); + + // Skin vertex + Vec3 pos = Vec3::sZero(); + for (const SkinWeight &w : s.mWeights) + { + // We assume that the first zero weight is the end of the list + if (w.mWeight == 0.0f) + break; + + JPH_ASSERT(w.mInvBindIndex < num_skin_matrices); + pos += w.mWeight * (skin_matrices[w.mInvBindIndex] * bind_pos); + } + SkinState &skin_state = mSkinState[s.mVertex]; + skin_state.mPreviousPosition = skin_state.mPosition; + skin_state.mPosition = pos; + } + + // Calculate the normals + for (const Skinned &s : mSettings->mSkinnedConstraints) + { + Vec3 normal = Vec3::sZero(); + uint32 num_faces = s.mNormalInfo >> 24; + if (num_faces > 0) + { + // Calculate normal + const uint32 *f = &mSettings->mSkinnedConstraintNormals[s.mNormalInfo & 0xffffff]; + const uint32 *f_end = f + num_faces; + while (f < f_end) + { + const Face &face = mSettings->mFaces[*f]; + Vec3 v0 = mSkinState[face.mVertex[0]].mPosition; + Vec3 v1 = mSkinState[face.mVertex[1]].mPosition; + Vec3 v2 = mSkinState[face.mVertex[2]].mPosition; + normal += (v1 - v0).Cross(v2 - v0).NormalizedOr(Vec3::sZero()); + ++f; + } + normal = normal.NormalizedOr(Vec3::sZero()); + } + mSkinState[s.mVertex].mNormal = normal; + } + + if (inHardSkinAll) + { + // Hard skin all vertices and reset their velocities + for (const Skinned &s : mSettings->mSkinnedConstraints) + { + Vertex &vertex = mVertices[s.mVertex]; + SkinState &skin_state = mSkinState[s.mVertex]; + skin_state.mPreviousPosition = skin_state.mPosition; + vertex.mPosition = skin_state.mPosition; + vertex.mVelocity = Vec3::sZero(); + } + } + else if (!mEnableSkinConstraints) + { + // Hard skin only the kinematic vertices as we will not solve the skin constraints later + for (const Skinned &s : mSettings->mSkinnedConstraints) + if (s.mMaxDistance == 0.0f) + { + Vertex &vertex = mVertices[s.mVertex]; + vertex.mPosition = mSkinState[s.mVertex].mPosition; + } + } + + // Indicate that the previous positions are valid for the coming update + mSkinStatePreviousPositionValid = true; +} + +void SoftBodyMotionProperties::CustomUpdate(float inDeltaTime, Body &ioSoftBody, PhysicsSystem &inSystem) +{ + JPH_PROFILE_FUNCTION(); + + // Create update context + SoftBodyUpdateContext context; + InitializeUpdateContext(inDeltaTime, ioSoftBody, inSystem, context); + + // Determine bodies we're colliding with + DetermineCollidingShapes(context, inSystem, inSystem.GetBodyLockInterface()); + + // Call the internal update until it finishes + EStatus status; + const PhysicsSettings &settings = inSystem.GetPhysicsSettings(); + while ((status = ParallelUpdate(context, settings)) == EStatus::DidWork) + continue; + JPH_ASSERT(status == EStatus::Done); + + // Update the state of the bodies we've collided with + UpdateRigidBodyVelocities(context, inSystem.GetBodyInterface()); + + // Update position of the soft body + if (mUpdatePosition) + inSystem.GetBodyInterface().SetPosition(ioSoftBody.GetID(), ioSoftBody.GetPosition() + context.mDeltaPosition, EActivation::DontActivate); +} + +#ifdef JPH_DEBUG_RENDERER + +void SoftBodyMotionProperties::DrawVertices(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const +{ + for (const Vertex &v : mVertices) + inRenderer->DrawMarker(inCenterOfMassTransform * v.mPosition, v.mInvMass > 0.0f? Color::sGreen : Color::sRed, 0.05f); +} + +void SoftBodyMotionProperties::DrawVertexVelocities(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const +{ + for (const Vertex &v : mVertices) + inRenderer->DrawArrow(inCenterOfMassTransform * v.mPosition, inCenterOfMassTransform * (v.mPosition + v.mVelocity), Color::sYellow, 0.01f); +} + +template +inline void SoftBodyMotionProperties::DrawConstraints(ESoftBodyConstraintColor inConstraintColor, const GetEndIndex &inGetEndIndex, const DrawConstraint &inDrawConstraint, ColorArg inBaseColor) const +{ + uint start = 0; + for (uint i = 0; i < (uint)mSettings->mUpdateGroups.size(); ++i) + { + uint end = inGetEndIndex(mSettings->mUpdateGroups[i]); + + Color base_color; + if (inConstraintColor != ESoftBodyConstraintColor::ConstraintType) + base_color = Color::sGetDistinctColor((uint)mSettings->mUpdateGroups.size() - i - 1); // Ensure that color 0 is always the last group + else + base_color = inBaseColor; + + for (uint idx = start; idx < end; ++idx) + { + Color color = inConstraintColor == ESoftBodyConstraintColor::ConstraintOrder? base_color * (float(idx - start) / (end - start)) : base_color; + inDrawConstraint(idx, color); + } + + start = end; + } +} + +void SoftBodyMotionProperties::DrawEdgeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const +{ + DrawConstraints(inConstraintColor, + [](const SoftBodySharedSettings::UpdateGroup &inGroup) { + return inGroup.mEdgeEndIndex; + }, + [this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) { + const Edge &e = mSettings->mEdgeConstraints[inIndex]; + inRenderer->DrawLine(inCenterOfMassTransform * mVertices[e.mVertex[0]].mPosition, inCenterOfMassTransform * mVertices[e.mVertex[1]].mPosition, inColor); + }, + Color::sWhite); +} + +void SoftBodyMotionProperties::DrawBendConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const +{ + DrawConstraints(inConstraintColor, + [](const SoftBodySharedSettings::UpdateGroup &inGroup) { + return inGroup.mDihedralBendEndIndex; + }, + [this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) { + const DihedralBend &b = mSettings->mDihedralBendConstraints[inIndex]; + + RVec3 x0 = inCenterOfMassTransform * mVertices[b.mVertex[0]].mPosition; + RVec3 x1 = inCenterOfMassTransform * mVertices[b.mVertex[1]].mPosition; + RVec3 x2 = inCenterOfMassTransform * mVertices[b.mVertex[2]].mPosition; + RVec3 x3 = inCenterOfMassTransform * mVertices[b.mVertex[3]].mPosition; + RVec3 c_edge = 0.5_r * (x0 + x1); + RVec3 c0 = (x0 + x1 + x2) / 3.0_r; + RVec3 c1 = (x0 + x1 + x3) / 3.0_r; + + inRenderer->DrawArrow(0.9_r * x0 + 0.1_r * x1, 0.1_r * x0 + 0.9_r * x1, inColor, 0.01f); + inRenderer->DrawLine(c_edge, 0.1_r * c_edge + 0.9_r * c0, inColor); + inRenderer->DrawLine(c_edge, 0.1_r * c_edge + 0.9_r * c1, inColor); + }, + Color::sGreen); +} + +void SoftBodyMotionProperties::DrawVolumeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const +{ + DrawConstraints(inConstraintColor, + [](const SoftBodySharedSettings::UpdateGroup &inGroup) { + return inGroup.mVolumeEndIndex; + }, + [this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) { + const Volume &v = mSettings->mVolumeConstraints[inIndex]; + + RVec3 x1 = inCenterOfMassTransform * mVertices[v.mVertex[0]].mPosition; + RVec3 x2 = inCenterOfMassTransform * mVertices[v.mVertex[1]].mPosition; + RVec3 x3 = inCenterOfMassTransform * mVertices[v.mVertex[2]].mPosition; + RVec3 x4 = inCenterOfMassTransform * mVertices[v.mVertex[3]].mPosition; + + inRenderer->DrawTriangle(x1, x3, x2, inColor, DebugRenderer::ECastShadow::On); + inRenderer->DrawTriangle(x2, x3, x4, inColor, DebugRenderer::ECastShadow::On); + inRenderer->DrawTriangle(x1, x4, x3, inColor, DebugRenderer::ECastShadow::On); + inRenderer->DrawTriangle(x1, x2, x4, inColor, DebugRenderer::ECastShadow::On); + }, + Color::sYellow); +} + +void SoftBodyMotionProperties::DrawSkinConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const +{ + DrawConstraints(inConstraintColor, + [](const SoftBodySharedSettings::UpdateGroup &inGroup) { + return inGroup.mSkinnedEndIndex; + }, + [this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) { + const Skinned &s = mSettings->mSkinnedConstraints[inIndex]; + const SkinState &skin_state = mSkinState[s.mVertex]; + inRenderer->DrawArrow(mSkinStateTransform * skin_state.mPosition, mSkinStateTransform * (skin_state.mPosition + 0.1f * skin_state.mNormal), inColor, 0.01f); + inRenderer->DrawLine(mSkinStateTransform * skin_state.mPosition, inCenterOfMassTransform * mVertices[s.mVertex].mPosition, Color::sBlue); + }, + Color::sOrange); +} + +void SoftBodyMotionProperties::DrawLRAConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const +{ + DrawConstraints(inConstraintColor, + [](const SoftBodySharedSettings::UpdateGroup &inGroup) { + return inGroup.mLRAEndIndex; + }, + [this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) { + const LRA &l = mSettings->mLRAConstraints[inIndex]; + inRenderer->DrawLine(inCenterOfMassTransform * mVertices[l.mVertex[0]].mPosition, inCenterOfMassTransform * mVertices[l.mVertex[1]].mPosition, inColor); + }, + Color::sGrey); +} + +void SoftBodyMotionProperties::DrawPredictedBounds(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const +{ + inRenderer->DrawWireBox(inCenterOfMassTransform, mLocalPredictedBounds, Color::sRed); +} + +#endif // JPH_DEBUG_RENDERER + +void SoftBodyMotionProperties::SaveState(StateRecorder &inStream) const +{ + MotionProperties::SaveState(inStream); + + for (const Vertex &v : mVertices) + { + inStream.Write(v.mPreviousPosition); + inStream.Write(v.mPosition); + inStream.Write(v.mVelocity); + } + + for (const SkinState &s : mSkinState) + { + inStream.Write(s.mPreviousPosition); + inStream.Write(s.mPosition); + inStream.Write(s.mNormal); + } + + inStream.Write(mLocalBounds.mMin); + inStream.Write(mLocalBounds.mMax); + inStream.Write(mLocalPredictedBounds.mMin); + inStream.Write(mLocalPredictedBounds.mMax); +} + +void SoftBodyMotionProperties::RestoreState(StateRecorder &inStream) +{ + MotionProperties::RestoreState(inStream); + + for (Vertex &v : mVertices) + { + inStream.Read(v.mPreviousPosition); + inStream.Read(v.mPosition); + inStream.Read(v.mVelocity); + } + + for (SkinState &s : mSkinState) + { + inStream.Read(s.mPreviousPosition); + inStream.Read(s.mPosition); + inStream.Read(s.mNormal); + } + + inStream.Read(mLocalBounds.mMin); + inStream.Read(mLocalBounds.mMax); + inStream.Read(mLocalPredictedBounds.mMin); + inStream.Read(mLocalPredictedBounds.mMax); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h new file mode 100644 index 000000000000..af66b7a2e4a9 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h @@ -0,0 +1,297 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; +class BodyInterface; +class BodyLockInterface; +struct PhysicsSettings; +class Body; +class Shape; +class SoftBodyCreationSettings; +class TempAllocator; +#ifdef JPH_DEBUG_RENDERER +class DebugRenderer; +enum class ESoftBodyConstraintColor; +#endif // JPH_DEBUG_RENDERER + +/// This class contains the runtime information of a soft body. +// +// Based on: XPBD, Extended Position Based Dynamics, Matthias Muller, Ten Minute Physics +// See: https://matthias-research.github.io/pages/tenMinutePhysics/09-xpbd.pdf +class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties +{ +public: + using Vertex = SoftBodyVertex; + using Edge = SoftBodySharedSettings::Edge; + using Face = SoftBodySharedSettings::Face; + using DihedralBend = SoftBodySharedSettings::DihedralBend; + using Volume = SoftBodySharedSettings::Volume; + using InvBind = SoftBodySharedSettings::InvBind; + using SkinWeight = SoftBodySharedSettings::SkinWeight; + using Skinned = SoftBodySharedSettings::Skinned; + using LRA = SoftBodySharedSettings::LRA; + + /// Initialize the soft body motion properties + void Initialize(const SoftBodyCreationSettings &inSettings); + + /// Get the shared settings of the soft body + const SoftBodySharedSettings * GetSettings() const { return mSettings; } + + /// Get the vertices of the soft body + const Array & GetVertices() const { return mVertices; } + Array & GetVertices() { return mVertices; } + + /// Access an individual vertex + const Vertex & GetVertex(uint inIndex) const { return mVertices[inIndex]; } + Vertex & GetVertex(uint inIndex) { return mVertices[inIndex]; } + + /// Get the materials of the soft body + const PhysicsMaterialList & GetMaterials() const { return mSettings->mMaterials; } + + /// Get the faces of the soft body + const Array & GetFaces() const { return mSettings->mFaces; } + + /// Access to an individual face + const Face & GetFace(uint inIndex) const { return mSettings->mFaces[inIndex]; } + + /// Get the number of solver iterations + uint32 GetNumIterations() const { return mNumIterations; } + void SetNumIterations(uint32 inNumIterations) { mNumIterations = inNumIterations; } + + /// Get the pressure of the soft body + float GetPressure() const { return mPressure; } + void SetPressure(float inPressure) { mPressure = inPressure; } + + /// Update the position of the body while simulating (set to false for something that is attached to the static world) + bool GetUpdatePosition() const { return mUpdatePosition; } + void SetUpdatePosition(bool inUpdatePosition) { mUpdatePosition = inUpdatePosition; } + + /// Global setting to turn on/off skin constraints + bool GetEnableSkinConstraints() const { return mEnableSkinConstraints; } + void SetEnableSkinConstraints(bool inEnableSkinConstraints) { mEnableSkinConstraints = inEnableSkinConstraints; } + + /// Multiplier applied to Skinned::mMaxDistance to allow tightening or loosening of the skin constraints. 0 to hard skin all vertices. + float GetSkinnedMaxDistanceMultiplier() const { return mSkinnedMaxDistanceMultiplier; } + void SetSkinnedMaxDistanceMultiplier(float inSkinnedMaxDistanceMultiplier) { mSkinnedMaxDistanceMultiplier = inSkinnedMaxDistanceMultiplier; } + + /// Get local bounding box + const AABox & GetLocalBounds() const { return mLocalBounds; } + + /// Get the volume of the soft body. Note can become negative if the shape is inside out! + float GetVolume() const { return GetVolumeTimesSix() / 6.0f; } + + /// Calculate the total mass and inertia of this body based on the current state of the vertices + void CalculateMassAndInertia(); + +#ifdef JPH_DEBUG_RENDERER + /// Draw the state of a soft body + void DrawVertices(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const; + void DrawVertexVelocities(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const; + void DrawEdgeConstraints(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; + void DrawLRAConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const; + void DrawPredictedBounds(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const; +#endif // JPH_DEBUG_RENDERER + + /// Saving state for replay + void SaveState(StateRecorder &inStream) const; + + /// Restoring state for replay + void RestoreState(StateRecorder &inStream); + + /// Skin vertices to supplied joints, information is used by the skinned constraints. + /// @param inCenterOfMassTransform Value of Body::GetCenterOfMassTransform(). + /// @param inJointMatrices The joint matrices must be expressed relative to inCenterOfMassTransform. + /// @param inNumJoints Indicates how large the inJointMatrices array is (used only for validating out of bounds). + /// @param inHardSkinAll Can be used to position all vertices on the skinned vertices and can be used to hard reset the soft body. + /// @param ioTempAllocator Allocator. + void SkinVertices(RMat44Arg inCenterOfMassTransform, const Mat44 *inJointMatrices, uint inNumJoints, bool inHardSkinAll, TempAllocator &ioTempAllocator); + + /// This function allows you to update the soft body immediately without going through the PhysicsSystem. + /// This is useful if the soft body is teleported and needs to 'settle' or it can be used if a the soft body + /// is not added to the PhysicsSystem and needs to be updated manually. One reason for not adding it to the + /// PhyicsSystem is that you might want to update a soft body immediately after updating an animated object + /// that has the soft body attached to it. If the soft body is added to the PhysicsSystem it will be updated + /// by it, so calling this function will effectively update it twice. Note that when you use this function, + /// only the current thread will be used, whereas if you update through the PhysicsSystem, multiple threads may + /// be used. + /// Note that this will bypass any sleep checks. Since the dynamic objects that the soft body touches + /// will not move during this call, there can be simulation artifacts if you call this function multiple times + /// without running the physics simulation step. + void CustomUpdate(float inDeltaTime, Body &ioSoftBody, PhysicsSystem &inSystem); + + //////////////////////////////////////////////////////////// + // FUNCTIONS BELOW THIS LINE ARE FOR INTERNAL USE ONLY + //////////////////////////////////////////////////////////// + + /// Initialize the update context. Not part of the public API. + void InitializeUpdateContext(float inDeltaTime, Body &inSoftBody, const PhysicsSystem &inSystem, SoftBodyUpdateContext &ioContext); + + /// Do a broad phase check and collect all bodies that can possibly collide with this soft body. Not part of the public API. + void DetermineCollidingShapes(const SoftBodyUpdateContext &inContext, const PhysicsSystem &inSystem, const BodyLockInterface &inBodyLockInterface); + + /// Return code for ParallelUpdate + enum class EStatus + { + NoWork = 1 << 0, ///< No work was done because other threads were still working on a batch that cannot run concurrently + DidWork = 1 << 1, ///< Work was done to progress the update + Done = 1 << 2, ///< All work is done + }; + + /// Update the soft body, will process a batch of work. Not part of the public API. + EStatus ParallelUpdate(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings); + + /// Update the velocities of all rigid bodies that we collided with. Not part of the public API. + void UpdateRigidBodyVelocities(const SoftBodyUpdateContext &inContext, BodyInterface &inBodyInterface); + +private: + // SoftBodyManifold needs to have access to CollidingShape + friend class SoftBodyManifold; + + // Information about a leaf shape that we're colliding with + struct LeafShape + { + LeafShape() = default; + LeafShape(Mat44Arg inTransform, Vec3Arg inScale, const Shape *inShape) : mTransform(inTransform), mScale(inScale), mShape(inShape) { } + + Mat44 mTransform; ///< Transform of the shape relative to the soft body + Vec3 mScale; ///< Scale of the shape + RefConst mShape; ///< Shape + }; + + // Collect information about the colliding bodies + struct CollidingShape + { + /// Get the velocity of a point on this body + Vec3 GetPointVelocity(Vec3Arg inPointRelativeToCOM) const + { + return mLinearVelocity + mAngularVelocity.Cross(inPointRelativeToCOM); + } + + Mat44 mCenterOfMassTransform; ///< Transform of the body relative to the soft body + Array mShapes; ///< Leaf shapes of the body we hit + BodyID mBodyID; ///< Body ID of the body we hit + EMotionType mMotionType; ///< Motion type of the body we hit + float mInvMass; ///< Inverse mass of the body we hit + float mFriction; ///< Combined friction of the two bodies + float mRestitution; ///< Combined restitution of the two bodies + float mSoftBodyInvMassScale; ///< Scale factor for the inverse mass of the soft body vertices + bool mUpdateVelocities; ///< If the linear/angular velocity changed and the body needs to be updated + Mat44 mInvInertia; ///< Inverse inertia in local space to the soft body + Vec3 mLinearVelocity; ///< Linear velocity of the body in local space to the soft body + Vec3 mAngularVelocity; ///< Angular velocity of the body in local space to the soft body + Vec3 mOriginalLinearVelocity; ///< Linear velocity of the body in local space to the soft body at start + Vec3 mOriginalAngularVelocity; ///< Angular velocity of the body in local space to the soft body at start + }; + + // Collect information about the colliding sensors + struct CollidingSensor + { + Mat44 mCenterOfMassTransform; ///< Transform of the body relative to the soft body + Array mShapes; ///< Leaf shapes of the body we hit + BodyID mBodyID; ///< Body ID of the body we hit + bool mHasContact; ///< If the sensor collided with the soft body + }; + + // Information about the state of all skinned vertices + struct SkinState + { + Vec3 mPreviousPosition = Vec3::sZero(); ///< Previous position of the skinned vertex, used to interpolate between the previous and current position + Vec3 mPosition = Vec3::sNaN(); ///< Current position of the skinned vertex + Vec3 mNormal = Vec3::sNaN(); ///< Normal of the skinned vertex + }; + + /// Do a narrow phase check and determine the closest feature that we can collide with + void DetermineCollisionPlanes(uint inVertexStart, uint inNumVertices); + + /// Do a narrow phase check between a single sensor and the soft body + void DetermineSensorCollisions(CollidingSensor &ioSensor); + + /// Apply pressure force and update the vertex velocities + void ApplyPressure(const SoftBodyUpdateContext &inContext); + + /// Integrate the positions of all vertices by 1 sub step + void IntegratePositions(const SoftBodyUpdateContext &inContext); + + /// Enforce all bend constraints + void ApplyDihedralBendConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex); + + /// Enforce all volume constraints + void ApplyVolumeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex); + + /// Enforce all skin constraints + void ApplySkinConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex); + + /// Enforce all edge constraints + void ApplyEdgeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex); + + /// Enforce all LRA constraints + void ApplyLRAConstraints(uint inStartIndex, uint inEndIndex); + + /// Enforce all collision constraints & update all velocities according the XPBD algorithm + void ApplyCollisionConstraintsAndUpdateVelocities(const SoftBodyUpdateContext &inContext); + + /// Update the state of the soft body (position, velocity, bounds) + void UpdateSoftBodyState(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings); + + /// Start the first solver iteration + void StartFirstIteration(SoftBodyUpdateContext &ioContext); + + /// Executes tasks that need to run on the start of an iteration (i.e. the stuff that can't run in parallel) + void StartNextIteration(const SoftBodyUpdateContext &ioContext); + + /// Helper function for ParallelUpdate that works on batches of collision planes + EStatus ParallelDetermineCollisionPlanes(SoftBodyUpdateContext &ioContext); + + /// Helper function for ParallelUpdate that works on sensor collisions + EStatus ParallelDetermineSensorCollisions(SoftBodyUpdateContext &ioContext); + + /// Helper function for ParallelUpdate that works on batches of constraints + EStatus ParallelApplyConstraints(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings); + + /// Helper function to update a single group of constraints + void ProcessGroup(const SoftBodyUpdateContext &ioContext, uint inGroupIndex); + + /// Returns 6 times the volume of the soft body + float GetVolumeTimesSix() const; + +#ifdef JPH_DEBUG_RENDERER + /// Helper function to draw constraints + template + inline void DrawConstraints(ESoftBodyConstraintColor inConstraintColor, const GetEndIndex &inGetEndIndex, const DrawConstraint &inDrawConstraint, ColorArg inBaseColor) const; + + RMat44 mSkinStateTransform = RMat44::sIdentity(); ///< The matrix that transforms mSkinState to world space +#endif // JPH_DEBUG_RENDERER + + RefConst mSettings; ///< Configuration of the particles and constraints + Array mVertices; ///< Current state of all vertices 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) + AABox mLocalBounds; ///< Bounding box of all vertices + AABox mLocalPredictedBounds; ///< Predicted bounding box for all vertices using extrapolation of velocity by last step delta time + uint32 mNumIterations; ///< Number of solver iterations + 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 + bool mUpdatePosition; ///< Update the position of the body while simulating (set to false for something that is attached to the static world) + bool 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 +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.cpp new file mode 100644 index 000000000000..6692b223eb48 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.cpp @@ -0,0 +1,338 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +uint SoftBodyShape::GetSubShapeIDBits() const +{ + // Ensure we have enough bits to encode our shape [0, n - 1] + uint32 n = (uint32)mSoftBodyMotionProperties->GetFaces().size() - 1; + return 32 - CountLeadingZeros(n); +} + +uint32 SoftBodyShape::GetFaceIndex(const SubShapeID &inSubShapeID) const +{ + SubShapeID remainder; + uint32 face_index = inSubShapeID.PopID(GetSubShapeIDBits(), remainder); + JPH_ASSERT(remainder.IsEmpty()); + return face_index; +} + +AABox SoftBodyShape::GetLocalBounds() const +{ + return mSoftBodyMotionProperties->GetLocalBounds(); +} + +bool SoftBodyShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + JPH_PROFILE_FUNCTION(); + + uint num_triangle_bits = GetSubShapeIDBits(); + uint triangle_idx = uint(-1); + + const Array &vertices = mSoftBodyMotionProperties->GetVertices(); + for (const SoftBodyMotionProperties::Face &f : mSoftBodyMotionProperties->GetFaces()) + { + Vec3 x1 = vertices[f.mVertex[0]].mPosition; + Vec3 x2 = vertices[f.mVertex[1]].mPosition; + Vec3 x3 = vertices[f.mVertex[2]].mPosition; + + float fraction = RayTriangle(inRay.mOrigin, inRay.mDirection, x1, x2, x3); + if (fraction < ioHit.mFraction) + { + // Store fraction + ioHit.mFraction = fraction; + + // Store triangle index + triangle_idx = uint(&f - mSoftBodyMotionProperties->GetFaces().data()); + } + } + + if (triangle_idx == uint(-1)) + return false; + + ioHit.mSubShapeID2 = inSubShapeIDCreator.PushID(triangle_idx, num_triangle_bits).GetID(); + return true; +} + +void SoftBodyShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + uint num_triangle_bits = GetSubShapeIDBits(); + + const Array &vertices = mSoftBodyMotionProperties->GetVertices(); + for (const SoftBodyMotionProperties::Face &f : mSoftBodyMotionProperties->GetFaces()) + { + Vec3 x1 = vertices[f.mVertex[0]].mPosition; + Vec3 x2 = vertices[f.mVertex[1]].mPosition; + 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) + continue; + + // Test ray against triangle + float fraction = RayTriangle(inRay.mOrigin, inRay.mDirection, x1, x2, x3); + if (fraction < ioCollector.GetEarlyOutFraction()) + { + // Better hit than the current hit + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + hit.mFraction = fraction; + hit.mSubShapeID2 = inSubShapeIDCreator.PushID(uint(&f - mSoftBodyMotionProperties->GetFaces().data()), num_triangle_bits).GetID(); + ioCollector.AddHit(hit); + } + } +} + +void SoftBodyShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + sCollidePointUsingRayCast(*this, inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void SoftBodyShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + /* Not implemented */ +} + +const PhysicsMaterial *SoftBodyShape::GetMaterial(const SubShapeID &inSubShapeID) const +{ + SubShapeID remainder; + uint triangle_idx = inSubShapeID.PopID(GetSubShapeIDBits(), remainder); + JPH_ASSERT(remainder.IsEmpty()); + + const SoftBodyMotionProperties::Face &f = mSoftBodyMotionProperties->GetFace(triangle_idx); + return mSoftBodyMotionProperties->GetMaterials()[f.mMaterialIndex]; +} + +Vec3 SoftBodyShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + SubShapeID remainder; + uint triangle_idx = inSubShapeID.PopID(GetSubShapeIDBits(), remainder); + JPH_ASSERT(remainder.IsEmpty()); + + const SoftBodyMotionProperties::Face &f = mSoftBodyMotionProperties->GetFace(triangle_idx); + const Array &vertices = mSoftBodyMotionProperties->GetVertices(); + + Vec3 x1 = vertices[f.mVertex[0]].mPosition; + Vec3 x2 = vertices[f.mVertex[1]].mPosition; + Vec3 x3 = vertices[f.mVertex[2]].mPosition; + + return (x2 - x1).Cross(x3 - x1).NormalizedOr(Vec3::sAxisY()); +} + +void SoftBodyShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + SubShapeID remainder; + uint triangle_idx = inSubShapeID.PopID(GetSubShapeIDBits(), remainder); + JPH_ASSERT(remainder.IsEmpty()); + + const SoftBodyMotionProperties::Face &f = mSoftBodyMotionProperties->GetFace(triangle_idx); + const Array &vertices = mSoftBodyMotionProperties->GetVertices(); + + for (uint32 i : f.mVertex) + outVertices.push_back(inCenterOfMassTransform * (inScale * vertices[i].mPosition)); +} + +void SoftBodyShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + outSubmergedVolume = 0.0f; + outTotalVolume = mSoftBodyMotionProperties->GetVolume(); + outCenterOfBuoyancy = Vec3::sZero(); +} + +#ifdef JPH_DEBUG_RENDERER + +void SoftBodyShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + const Array &vertices = mSoftBodyMotionProperties->GetVertices(); + for (const SoftBodyMotionProperties::Face &f : mSoftBodyMotionProperties->GetFaces()) + { + RVec3 x1 = inCenterOfMassTransform * vertices[f.mVertex[0]].mPosition; + RVec3 x2 = inCenterOfMassTransform * vertices[f.mVertex[1]].mPosition; + RVec3 x3 = inCenterOfMassTransform * vertices[f.mVertex[2]].mPosition; + + inRenderer->DrawTriangle(x1, x2, x3, inColor, DebugRenderer::ECastShadow::On); + } +} + +#endif // JPH_DEBUG_RENDERER + +struct SoftBodyShape::SBSGetTrianglesContext +{ + Mat44 mCenterOfMassTransform; + int mTriangleIndex; +}; + +void SoftBodyShape::GetTrianglesStart(GetTrianglesContext &ioContext, [[maybe_unused]] const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + SBSGetTrianglesContext &context = reinterpret_cast(ioContext); + context.mCenterOfMassTransform = Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale); + context.mTriangleIndex = 0; +} + +int SoftBodyShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + SBSGetTrianglesContext &context = reinterpret_cast(ioContext); + + const Array &faces = mSoftBodyMotionProperties->GetFaces(); + const Array &vertices = mSoftBodyMotionProperties->GetVertices(); + const PhysicsMaterialList &materials = mSoftBodyMotionProperties->GetMaterials(); + + int num_triangles = min(inMaxTrianglesRequested, (int)faces.size() - context.mTriangleIndex); + for (int i = 0; i < num_triangles; ++i) + { + const SoftBodyMotionProperties::Face &f = faces[context.mTriangleIndex + i]; + + Vec3 x1 = context.mCenterOfMassTransform * vertices[f.mVertex[0]].mPosition; + Vec3 x2 = context.mCenterOfMassTransform * vertices[f.mVertex[1]].mPosition; + Vec3 x3 = context.mCenterOfMassTransform * vertices[f.mVertex[2]].mPosition; + + x1.StoreFloat3(outTriangleVertices++); + x2.StoreFloat3(outTriangleVertices++); + x3.StoreFloat3(outTriangleVertices++); + + if (outMaterials != nullptr) + *outMaterials++ = materials[f.mMaterialIndex]; + } + + context.mTriangleIndex += num_triangles; + return num_triangles; +} + +Shape::Stats SoftBodyShape::GetStats() const +{ + return Stats(sizeof(*this), (uint)mSoftBodyMotionProperties->GetFaces().size()); +} + +float SoftBodyShape::GetVolume() const +{ + return mSoftBodyMotionProperties->GetVolume(); +} + +void SoftBodyShape::sCollideConvexVsSoftBody(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, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape1->GetType() == EShapeType::Convex); + const ConvexShape *shape1 = static_cast(inShape1); + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::SoftBody); + const SoftBodyShape *shape2 = static_cast(inShape2); + + const Array &vertices = shape2->mSoftBodyMotionProperties->GetVertices(); + const Array &faces = shape2->mSoftBodyMotionProperties->GetFaces(); + uint num_triangle_bits = shape2->GetSubShapeIDBits(); + + CollideConvexVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + for (const SoftBodyMotionProperties::Face &f : faces) + { + Vec3 x1 = vertices[f.mVertex[0]].mPosition; + Vec3 x2 = vertices[f.mVertex[1]].mPosition; + Vec3 x3 = vertices[f.mVertex[2]].mPosition; + + collider.Collide(x1, x2, x3, 0b111, inSubShapeIDCreator2.PushID(uint(&f - faces.data()), num_triangle_bits).GetID()); + } +} + +void SoftBodyShape::sCollideSphereVsSoftBody(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, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Sphere); + const SphereShape *shape1 = static_cast(inShape1); + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::SoftBody); + const SoftBodyShape *shape2 = static_cast(inShape2); + + const Array &vertices = shape2->mSoftBodyMotionProperties->GetVertices(); + const Array &faces = shape2->mSoftBodyMotionProperties->GetFaces(); + uint num_triangle_bits = shape2->GetSubShapeIDBits(); + + CollideSphereVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + for (const SoftBodyMotionProperties::Face &f : faces) + { + Vec3 x1 = vertices[f.mVertex[0]].mPosition; + Vec3 x2 = vertices[f.mVertex[1]].mPosition; + Vec3 x3 = vertices[f.mVertex[2]].mPosition; + + collider.Collide(x1, x2, x3, 0b111, inSubShapeIDCreator2.PushID(uint(&f - faces.data()), num_triangle_bits).GetID()); + } +} + +void SoftBodyShape::sCastConvexVsSoftBody(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::SoftBody); + const SoftBodyShape *shape = static_cast(inShape); + + const Array &vertices = shape->mSoftBodyMotionProperties->GetVertices(); + const Array &faces = shape->mSoftBodyMotionProperties->GetFaces(); + uint num_triangle_bits = shape->GetSubShapeIDBits(); + + CastConvexVsTriangles caster(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + for (const SoftBodyMotionProperties::Face &f : faces) + { + Vec3 x1 = vertices[f.mVertex[0]].mPosition; + Vec3 x2 = vertices[f.mVertex[1]].mPosition; + Vec3 x3 = vertices[f.mVertex[2]].mPosition; + + caster.Cast(x1, x2, x3, 0b111, inSubShapeIDCreator2.PushID(uint(&f - faces.data()), num_triangle_bits).GetID()); + } +} + +void SoftBodyShape::sCastSphereVsSoftBody(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::SoftBody); + const SoftBodyShape *shape = static_cast(inShape); + + const Array &vertices = shape->mSoftBodyMotionProperties->GetVertices(); + const Array &faces = shape->mSoftBodyMotionProperties->GetFaces(); + uint num_triangle_bits = shape->GetSubShapeIDBits(); + + CastSphereVsTriangles caster(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + for (const SoftBodyMotionProperties::Face &f : faces) + { + Vec3 x1 = vertices[f.mVertex[0]].mPosition; + Vec3 x2 = vertices[f.mVertex[1]].mPosition; + Vec3 x3 = vertices[f.mVertex[2]].mPosition; + + caster.Cast(x1, x2, x3, 0b111, inSubShapeIDCreator2.PushID(uint(&f - faces.data()), num_triangle_bits).GetID()); + } +} + +void SoftBodyShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::SoftBody); + f.mConstruct = nullptr; // Not supposed to be constructed by users! + f.mColor = Color::sDarkGreen; + + for (EShapeSubType s : sConvexSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::SoftBody, sCollideConvexVsSoftBody); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::SoftBody, sCastConvexVsSoftBody); + } + + // Specialized collision functions + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Sphere, EShapeSubType::SoftBody, sCollideSphereVsSoftBody); + CollisionDispatch::sRegisterCastShape(EShapeSubType::Sphere, EShapeSubType::SoftBody, sCastSphereVsSoftBody); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.h new file mode 100644 index 000000000000..70605dae409c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.h @@ -0,0 +1,73 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class SoftBodyMotionProperties; +class CollideShapeSettings; + +/// Shape used exclusively for soft bodies. Adds the ability to perform collision checks against soft bodies. +class JPH_EXPORT SoftBodyShape final : public Shape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + SoftBodyShape() : Shape(EShapeType::SoftBody, EShapeSubType::SoftBody) { } + + /// Determine amount of bits needed to encode sub shape id + uint GetSubShapeIDBits() const; + + /// Convert a sub shape ID back to a face index + uint32 GetFaceIndex(const SubShapeID &inSubShapeID) const; + + // See Shape + virtual bool MustBeStatic() const override { return false; } + virtual Vec3 GetCenterOfMass() const override { return Vec3::sZero(); } + virtual AABox GetLocalBounds() const override; + virtual uint GetSubShapeIDBitsRecursive() const override { return GetSubShapeIDBits(); } + virtual float GetInnerRadius() const override { return 0.0f; } + virtual MassProperties GetMassProperties() const override { return MassProperties(); } + virtual const PhysicsMaterial * GetMaterial(const SubShapeID &inSubShapeID) const override; + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy +#ifdef JPH_DEBUG_RENDERER // Not using JPH_IF_DEBUG_RENDERER for Doxygen + , RVec3Arg inBaseOffset +#endif + ) const override; +#ifdef JPH_DEBUG_RENDERER + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + virtual Stats GetStats() const override; + virtual float GetVolume() const override; + + // Register shape functions with the registry + static void sRegister(); + +private: + // Helper functions called by CollisionDispatch + static void sCollideConvexVsSoftBody(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 sCollideSphereVsSoftBody(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 sCastConvexVsSoftBody(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastSphereVsSoftBody(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + struct SBSGetTrianglesContext; + + friend class BodyManager; + + const SoftBodyMotionProperties *mSoftBodyMotionProperties; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp new file mode 100644 index 000000000000..7eb8dd555c78 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp @@ -0,0 +1,1032 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Vertex) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Vertex, mPosition) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Vertex, mVelocity) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Vertex, mInvMass) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Face) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Face, mVertex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Face, mMaterialIndex) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Edge) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Edge, mVertex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Edge, mRestLength) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Edge, mCompliance) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::DihedralBend) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::DihedralBend, mVertex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::DihedralBend, mCompliance) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::DihedralBend, mInitialAngle) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Volume) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Volume, mVertex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Volume, mSixRestVolume) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Volume, mCompliance) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::InvBind) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::InvBind, mJointIndex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::InvBind, mInvBind) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::SkinWeight) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::SkinWeight, mInvBindIndex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::SkinWeight, mWeight) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Skinned) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mVertex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mWeights) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mMaxDistance) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mBackStopDistance) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mBackStopRadius) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::LRA) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::LRA, mVertex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::LRA, mMaxDistance) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVertices) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mFaces) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mEdgeConstraints) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mDihedralBendConstraints) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVolumeConstraints) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mSkinnedConstraints) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mInvBindMatrices) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mLRAConstraints) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mMaterials) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVertexRadius) +} + +void SoftBodySharedSettings::CalculateClosestKinematic() +{ + // Check if we already calculated this + if (!mClosestKinematic.empty()) + return; + + // Reserve output size + mClosestKinematic.resize(mVertices.size()); + + // Create a list of connected vertices + Array> connectivity; + connectivity.resize(mVertices.size()); + for (const Edge &e : mEdgeConstraints) + { + connectivity[e.mVertex[0]].push_back(e.mVertex[1]); + connectivity[e.mVertex[1]].push_back(e.mVertex[0]); + } + + // Use Dijkstra's algorithm to find the closest kinematic vertex for each vertex + // See: https://en.wikipedia.org/wiki/Dijkstra's_algorithm + // + // An element in the open list + struct Open + { + // Order so that we get the shortest distance first + bool operator < (const Open &inRHS) const + { + return mDistance > inRHS.mDistance; + } + + uint32 mVertex; + float mDistance; + }; + + // Start with all kinematic elements + Array to_visit; + for (uint32 v = 0; v < mVertices.size(); ++v) + if (mVertices[v].mInvMass == 0.0f) + { + mClosestKinematic[v].mVertex = v; + mClosestKinematic[v].mDistance = 0.0f; + to_visit.push_back({ v, 0.0f }); + BinaryHeapPush(to_visit.begin(), to_visit.end(), std::less { }); + } + + // Visit all vertices remembering the closest kinematic vertex and its distance + JPH_IF_ENABLE_ASSERTS(float last_closest = 0.0f;) + while (!to_visit.empty()) + { + // Pop element from the open list + BinaryHeapPop(to_visit.begin(), to_visit.end(), std::less { }); + Open current = to_visit.back(); + to_visit.pop_back(); + JPH_ASSERT(current.mDistance >= last_closest); + JPH_IF_ENABLE_ASSERTS(last_closest = current.mDistance;) + + // Loop through all of its connected vertices + for (uint32 v : connectivity[current.mVertex]) + { + // Calculate distance from the current vertex to this target vertex and check if it is smaller + float new_distance = current.mDistance + (Vec3(mVertices[v].mPosition) - Vec3(mVertices[current.mVertex].mPosition)).Length(); + if (new_distance < mClosestKinematic[v].mDistance) + { + // Remember new closest vertex + mClosestKinematic[v].mVertex = mClosestKinematic[current.mVertex].mVertex; + mClosestKinematic[v].mDistance = new_distance; + to_visit.push_back({ v, new_distance }); + BinaryHeapPush(to_visit.begin(), to_visit.end(), std::less { }); + } + } + } +} + +void SoftBodySharedSettings::CreateConstraints(const VertexAttributes *inVertexAttributes, uint inVertexAttributesLength, EBendType inBendType, float inAngleTolerance) +{ + struct EdgeHelper + { + uint32 mVertex[2]; + uint32 mEdgeIdx; + }; + + // Create list of all edges + Array edges; + edges.reserve(mFaces.size() * 3); + for (const Face &f : mFaces) + for (int i = 0; i < 3; ++i) + { + uint32 v0 = f.mVertex[i]; + uint32 v1 = f.mVertex[(i + 1) % 3]; + + EdgeHelper e; + e.mVertex[0] = min(v0, v1); + e.mVertex[1] = max(v0, v1); + e.mEdgeIdx = uint32(&f - mFaces.data()) * 3 + i; + edges.push_back(e); + } + + // Sort the edges + QuickSort(edges.begin(), edges.end(), [](const EdgeHelper &inLHS, const EdgeHelper &inRHS) { return inLHS.mVertex[0] < inRHS.mVertex[0] || (inLHS.mVertex[0] == inRHS.mVertex[0] && inLHS.mVertex[1] < inRHS.mVertex[1]); }); + + // Only add edges if one of the vertices is movable + auto add_edge = [this](uint32 inVtx1, uint32 inVtx2, float inCompliance1, float inCompliance2) { + if ((mVertices[inVtx1].mInvMass > 0.0f || mVertices[inVtx2].mInvMass > 0.0f) + && inCompliance1 < FLT_MAX && inCompliance2 < FLT_MAX) + { + Edge temp_edge; + temp_edge.mVertex[0] = inVtx1; + temp_edge.mVertex[1] = inVtx2; + temp_edge.mCompliance = 0.5f * (inCompliance1 + inCompliance2); + temp_edge.mRestLength = (Vec3(mVertices[inVtx2].mPosition) - Vec3(mVertices[inVtx1].mPosition)).Length(); + JPH_ASSERT(temp_edge.mRestLength > 0.0f); + mEdgeConstraints.push_back(temp_edge); + } + }; + + // Helper function to get the attributes of a vertex + auto attr = [inVertexAttributes, inVertexAttributesLength](uint32 inVertex) { + return inVertexAttributes[min(inVertex, inVertexAttributesLength - 1)]; + }; + + // Create the constraints + float sq_sin_tolerance = Square(Sin(inAngleTolerance)); + float sq_cos_tolerance = Square(Cos(inAngleTolerance)); + mEdgeConstraints.clear(); + mEdgeConstraints.reserve(edges.size()); + for (Array::size_type i = 0; i < edges.size(); ++i) + { + const EdgeHelper &e0 = edges[i]; + + // Get attributes for the vertices of the edge + const VertexAttributes &a0 = attr(e0.mVertex[0]); + const VertexAttributes &a1 = attr(e0.mVertex[1]); + + // Flag that indicates if this edge is a shear edge (if 2 triangles form a quad-like shape and this edge is on the diagonal) + bool is_shear = false; + + // Test if there are any shared edges + for (Array::size_type j = i + 1; j < edges.size(); ++j) + { + const EdgeHelper &e1 = edges[j]; + if (e0.mVertex[0] == e1.mVertex[0] && e0.mVertex[1] == e1.mVertex[1]) + { + // Get opposing vertices + const Face &f0 = mFaces[e0.mEdgeIdx / 3]; + const Face &f1 = mFaces[e1.mEdgeIdx / 3]; + uint32 vopposite0 = f0.mVertex[(e0.mEdgeIdx + 2) % 3]; + uint32 vopposite1 = f1.mVertex[(e1.mEdgeIdx + 2) % 3]; + const VertexAttributes &a_opposite0 = attr(vopposite0); + const VertexAttributes &a_opposite1 = attr(vopposite1); + + // 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()) + { + // Faces should approximately form a quad + Vec3 e0_dir = Vec3(mVertices[vopposite0].mPosition) - Vec3(mVertices[e0.mVertex[0]].mPosition); + Vec3 e1_dir = Vec3(mVertices[vopposite1].mPosition) - Vec3(mVertices[e0.mVertex[0]].mPosition); + if (Square(e0_dir.Dot(e1_dir)) < sq_sin_tolerance * e0_dir.LengthSq() * e1_dir.LengthSq()) + { + // Shear constraint + add_edge(vopposite0, vopposite1, a_opposite0.mShearCompliance, a_opposite1.mShearCompliance); + is_shear = true; + } + } + + // Bend constraint + switch (inBendType) + { + case EBendType::None: + // Do nothing + break; + + case EBendType::Distance: + // Create an edge constraint to represent the bend constraint + // Use the bend compliance of the shared edge + if (!is_shear) + add_edge(vopposite0, vopposite1, a0.mBendCompliance, a1.mBendCompliance); + break; + + case EBendType::Dihedral: + // Test if both opposite vertices are free to move + if ((mVertices[vopposite0].mInvMass > 0.0f || mVertices[vopposite1].mInvMass > 0.0f) + && a0.mBendCompliance < FLT_MAX && a1.mBendCompliance < FLT_MAX) + { + // Create a bend constraint + // Use the bend compliance of the shared edge + mDihedralBendConstraints.emplace_back(e0.mVertex[0], e0.mVertex[1], vopposite0, vopposite1, 0.5f * (a0.mBendCompliance + a1.mBendCompliance)); + } + break; + } + } + else + { + // Start iterating from the first non-shared edge + i = j - 1; + break; + } + } + + // Create a edge constraint for the current edge + add_edge(e0.mVertex[0], e0.mVertex[1], is_shear? a0.mShearCompliance : a0.mCompliance, is_shear? a1.mShearCompliance : a1.mCompliance); + } + mEdgeConstraints.shrink_to_fit(); + + // Calculate the initial angle for all bend constraints + CalculateBendConstraintConstants(); + + // Check if any vertices have LRA constraints + bool has_lra_constraints = false; + for (const VertexAttributes *va = inVertexAttributes; va < inVertexAttributes + inVertexAttributesLength; ++va) + if (va->mLRAType != ELRAType::None) + { + has_lra_constraints = true; + break; + } + if (has_lra_constraints) + { + // Ensure we have calculated the closest kinematic vertex for each vertex + CalculateClosestKinematic(); + + // Find non-kinematic vertices + for (uint32 v = 0; v < (uint32)mVertices.size(); ++v) + if (mVertices[v].mInvMass > 0.0f) + { + // Check if a closest vertex was found + uint32 closest = mClosestKinematic[v].mVertex; + if (closest != 0xffffffff) + { + // Check which LRA constraint to create + const VertexAttributes &va = attr(v); + switch (va.mLRAType) + { + case ELRAType::None: + break; + + case ELRAType::EuclideanDistance: + mLRAConstraints.emplace_back(closest, v, va.mLRAMaxDistanceMultiplier * (Vec3(mVertices[closest].mPosition) - Vec3(mVertices[v].mPosition)).Length()); + break; + + case ELRAType::GeodesicDistance: + mLRAConstraints.emplace_back(closest, v, va.mLRAMaxDistanceMultiplier * mClosestKinematic[v].mDistance); + break; + } + } + } + } +} + +void SoftBodySharedSettings::CalculateEdgeLengths() +{ + for (Edge &e : mEdgeConstraints) + { + e.mRestLength = (Vec3(mVertices[e.mVertex[1]].mPosition) - Vec3(mVertices[e.mVertex[0]].mPosition)).Length(); + JPH_ASSERT(e.mRestLength > 0.0f); + } +} + +void SoftBodySharedSettings::CalculateLRALengths(float inMaxDistanceMultiplier) +{ + for (LRA &l : mLRAConstraints) + { + l.mMaxDistance = inMaxDistanceMultiplier * (Vec3(mVertices[l.mVertex[1]].mPosition) - Vec3(mVertices[l.mVertex[0]].mPosition)).Length(); + JPH_ASSERT(l.mMaxDistance > 0.0f); + } +} + +void SoftBodySharedSettings::CalculateBendConstraintConstants() +{ + for (DihedralBend &b : mDihedralBendConstraints) + { + // Get positions + Vec3 x0 = Vec3(mVertices[b.mVertex[0]].mPosition); + Vec3 x1 = Vec3(mVertices[b.mVertex[1]].mPosition); + Vec3 x2 = Vec3(mVertices[b.mVertex[2]].mPosition); + Vec3 x3 = Vec3(mVertices[b.mVertex[3]].mPosition); + + /* + x2 + e1/ \e3 + / \ + x0----x1 + \ e0 / + e2\ /e4 + x3 + */ + + // Calculate edges + Vec3 e0 = x1 - x0; + Vec3 e1 = x2 - x0; + Vec3 e2 = x3 - x0; + + // Normals of both triangles + Vec3 n1 = e0.Cross(e1); + Vec3 n2 = e2.Cross(e0); + float denom = sqrt(n1.LengthSq() * n2.LengthSq()); + if (denom < 1.0e-12f) + b.mInitialAngle = 0.0f; + else + { + float sign = Sign(n2.Cross(n1).Dot(e0)); + b.mInitialAngle = sign * ACosApproximate(n1.Dot(n2) / denom); // Runtime uses the approximation too + } + } +} + +void SoftBodySharedSettings::CalculateVolumeConstraintVolumes() +{ + for (Volume &v : mVolumeConstraints) + { + Vec3 x1(mVertices[v.mVertex[0]].mPosition); + Vec3 x2(mVertices[v.mVertex[1]].mPosition); + Vec3 x3(mVertices[v.mVertex[2]].mPosition); + Vec3 x4(mVertices[v.mVertex[3]].mPosition); + + Vec3 x1x2 = x2 - x1; + Vec3 x1x3 = x3 - x1; + Vec3 x1x4 = x4 - x1; + + v.mSixRestVolume = abs(x1x2.Cross(x1x3).Dot(x1x4)); + } +} + +void SoftBodySharedSettings::CalculateSkinnedConstraintNormals() +{ + // Clear any previous results + mSkinnedConstraintNormals.clear(); + + // If there are no skinned constraints, we're done + if (mSkinnedConstraints.empty()) + return; + + // First collect all vertices that are skinned + using VertexIndexSet = UnorderedSet; + VertexIndexSet skinned_vertices; + skinned_vertices.reserve(VertexIndexSet::size_type(mSkinnedConstraints.size())); + for (const Skinned &s : mSkinnedConstraints) + skinned_vertices.insert(s.mVertex); + + // Now collect all faces that connect only to skinned vertices + using ConnectedFacesMap = UnorderedMap; + ConnectedFacesMap connected_faces; + connected_faces.reserve(ConnectedFacesMap::size_type(mVertices.size())); + for (const Face &f : mFaces) + { + // Must connect to only skinned vertices + bool valid = true; + for (uint32 v : f.mVertex) + valid &= skinned_vertices.find(v) != skinned_vertices.end(); + if (!valid) + continue; + + // Store faces that connect to vertices + for (uint32 v : f.mVertex) + connected_faces[v].insert(uint32(&f - mFaces.data())); + } + + // Populate the list of connecting faces per skinned vertex + mSkinnedConstraintNormals.reserve(mFaces.size()); + for (Skinned &s : mSkinnedConstraints) + { + uint32 start = uint32(mSkinnedConstraintNormals.size()); + JPH_ASSERT((start >> 24) == 0); + ConnectedFacesMap::const_iterator connected_faces_it = connected_faces.find(s.mVertex); + if (connected_faces_it != connected_faces.cend()) + { + const VertexIndexSet &faces = connected_faces_it->second; + uint32 num = uint32(faces.size()); + JPH_ASSERT(num < 256); + mSkinnedConstraintNormals.insert(mSkinnedConstraintNormals.end(), faces.begin(), faces.end()); + QuickSort(mSkinnedConstraintNormals.begin() + start, mSkinnedConstraintNormals.begin() + start + num); + s.mNormalInfo = start + (num << 24); + } + else + s.mNormalInfo = 0; + } + mSkinnedConstraintNormals.shrink_to_fit(); +} + +void SoftBodySharedSettings::Optimize(OptimizationResults &outResults) +{ + // Clear any previous results + mUpdateGroups.clear(); + + // Create a list of connected vertices + struct Connection + { + uint32 mVertex; + uint32 mCount; + }; + Array> connectivity; + connectivity.resize(mVertices.size()); + auto add_connection = [&connectivity](uint inV1, uint inV2) { + for (int i = 0; i < 2; ++i) + { + bool found = false; + for (Connection &c : connectivity[inV1]) + if (c.mVertex == inV2) + { + c.mCount++; + found = true; + break; + } + if (!found) + connectivity[inV1].push_back({ inV2, 1 }); + + std::swap(inV1, inV2); + } + }; + for (const Edge &c : mEdgeConstraints) + add_connection(c.mVertex[0], c.mVertex[1]); + for (const LRA &c : mLRAConstraints) + add_connection(c.mVertex[0], c.mVertex[1]); + for (const DihedralBend &c : mDihedralBendConstraints) + { + add_connection(c.mVertex[0], c.mVertex[1]); + add_connection(c.mVertex[0], c.mVertex[2]); + add_connection(c.mVertex[0], c.mVertex[3]); + add_connection(c.mVertex[1], c.mVertex[2]); + add_connection(c.mVertex[1], c.mVertex[3]); + add_connection(c.mVertex[2], c.mVertex[3]); + } + for (const Volume &c : mVolumeConstraints) + { + add_connection(c.mVertex[0], c.mVertex[1]); + add_connection(c.mVertex[0], c.mVertex[2]); + add_connection(c.mVertex[0], c.mVertex[3]); + add_connection(c.mVertex[1], c.mVertex[2]); + add_connection(c.mVertex[1], c.mVertex[3]); + add_connection(c.mVertex[2], c.mVertex[3]); + } + // Skinned constraints only update 1 vertex, so we don't need special logic here + + // Maps each of the vertices to a group index + Array group_idx; + group_idx.resize(mVertices.size(), -1); + + // Which group we are currently filling and its vertices + int current_group_idx = 0; + Array current_group; + + // Start greedy algorithm to group vertices + for (;;) + { + // Find the bounding box of the ungrouped vertices + AABox bounds; + for (uint i = 0; i < (uint)mVertices.size(); ++i) + if (group_idx[i] == -1) + bounds.Encapsulate(Vec3(mVertices[i].mPosition)); + + // Determine longest and shortest axis + Vec3 bounds_size = bounds.GetSize(); + uint max_axis = bounds_size.GetHighestComponentIndex(); + uint min_axis = bounds_size.GetLowestComponentIndex(); + if (min_axis == max_axis) + min_axis = (min_axis + 1) % 3; + uint mid_axis = 3 - min_axis - max_axis; + + // Find the vertex that has the lowest value on the axis with the largest extent + uint current_vertex = UINT_MAX; + Float3 current_vertex_position { FLT_MAX, FLT_MAX, FLT_MAX }; + for (uint i = 0; i < (uint)mVertices.size(); ++i) + if (group_idx[i] == -1) + { + const Float3 &vertex_position = mVertices[i].mPosition; + float max_axis_value = vertex_position[max_axis]; + float mid_axis_value = vertex_position[mid_axis]; + float min_axis_value = vertex_position[min_axis]; + + if (max_axis_value < current_vertex_position[max_axis] + || (max_axis_value == current_vertex_position[max_axis] + && (mid_axis_value < current_vertex_position[mid_axis] + || (mid_axis_value == current_vertex_position[mid_axis] + && min_axis_value < current_vertex_position[min_axis])))) + { + current_vertex_position = mVertices[i].mPosition; + current_vertex = i; + } + } + if (current_vertex == UINT_MAX) + break; + + // Initialize the current group with 1 vertex + current_group.push_back(current_vertex); + group_idx[current_vertex] = current_group_idx; + + // Fill up the group + for (;;) + { + // Find the vertex that is most connected to the current group + uint best_vertex = UINT_MAX; + uint best_num_connections = 0; + float best_dist_sq = FLT_MAX; + for (uint i = 0; i < (uint)current_group.size(); ++i) // For all vertices in the current group + for (const Connection &c : connectivity[current_group[i]]) // For all connections to other vertices + { + uint v = c.mVertex; + if (group_idx[v] == -1) // Ungrouped vertices only + { + // Count the number of connections to this group + uint num_connections = 0; + for (const Connection &v2 : connectivity[v]) + if (group_idx[v2.mVertex] == current_group_idx) + num_connections += v2.mCount; + + // Calculate distance to group centroid + float dist_sq = (Vec3(mVertices[v].mPosition) - Vec3(mVertices[current_group.front()].mPosition)).LengthSq(); + + if (best_vertex == UINT_MAX + || num_connections > best_num_connections + || (num_connections == best_num_connections && dist_sq < best_dist_sq)) + { + best_vertex = v; + best_num_connections = num_connections; + best_dist_sq = dist_sq; + } + } + } + + // Add the best vertex to the current group + if (best_vertex != UINT_MAX) + { + current_group.push_back(best_vertex); + group_idx[best_vertex] = current_group_idx; + } + + // Create a new group? + if (current_group.size() >= SoftBodyUpdateContext::cVertexConstraintBatch // If full, yes + || (current_group.size() > SoftBodyUpdateContext::cVertexConstraintBatch / 2 && best_vertex == UINT_MAX)) // If half full and we found no connected vertex, yes + { + current_group.clear(); + current_group_idx++; + break; + } + + // If we didn't find a connected vertex, we need to find a new starting vertex + if (best_vertex == UINT_MAX) + break; + } + } + + // If the last group is more than half full, we'll keep it as a separate group, otherwise we merge it with the 'non parallel' group + if (current_group.size() > SoftBodyUpdateContext::cVertexConstraintBatch / 2) + ++current_group_idx; + + // We no longer need the current group array, free the memory + current_group.clear(); + current_group.shrink_to_fit(); + + // We're done with the connectivity list, free the memory + connectivity.clear(); + connectivity.shrink_to_fit(); + + // Assign the constraints to their groups + struct Group + { + uint GetSize() const + { + return (uint)mEdgeConstraints.size() + (uint)mLRAConstraints.size() + (uint)mDihedralBendConstraints.size() + (uint)mVolumeConstraints.size() + (uint)mSkinnedConstraints.size(); + } + + Array mEdgeConstraints; + Array mLRAConstraints; + Array mDihedralBendConstraints; + Array mVolumeConstraints; + Array mSkinnedConstraints; + }; + Array groups; + groups.resize(current_group_idx + 1); // + non parallel group + for (const Edge &e : mEdgeConstraints) + { + int g1 = group_idx[e.mVertex[0]]; + int g2 = group_idx[e.mVertex[1]]; + JPH_ASSERT(g1 >= 0 && g2 >= 0); + if (g1 == g2) // In the same group + groups[g1].mEdgeConstraints.push_back(uint(&e - mEdgeConstraints.data())); + else // In different groups -> parallel group + groups.back().mEdgeConstraints.push_back(uint(&e - mEdgeConstraints.data())); + } + for (const LRA &l : mLRAConstraints) + { + int g1 = group_idx[l.mVertex[0]]; + int g2 = group_idx[l.mVertex[1]]; + JPH_ASSERT(g1 >= 0 && g2 >= 0); + if (g1 == g2) // In the same group + groups[g1].mLRAConstraints.push_back(uint(&l - mLRAConstraints.data())); + else // In different groups -> parallel group + groups.back().mLRAConstraints.push_back(uint(&l - mLRAConstraints.data())); + } + for (const DihedralBend &d : mDihedralBendConstraints) + { + int g1 = group_idx[d.mVertex[0]]; + int g2 = group_idx[d.mVertex[1]]; + int g3 = group_idx[d.mVertex[2]]; + int g4 = group_idx[d.mVertex[3]]; + JPH_ASSERT(g1 >= 0 && g2 >= 0 && g3 >= 0 && g4 >= 0); + if (g1 == g2 && g1 == g3 && g1 == g4) // In the same group + groups[g1].mDihedralBendConstraints.push_back(uint(&d - mDihedralBendConstraints.data())); + else // In different groups -> parallel group + groups.back().mDihedralBendConstraints.push_back(uint(&d - mDihedralBendConstraints.data())); + } + for (const Volume &v : mVolumeConstraints) + { + int g1 = group_idx[v.mVertex[0]]; + int g2 = group_idx[v.mVertex[1]]; + int g3 = group_idx[v.mVertex[2]]; + int g4 = group_idx[v.mVertex[3]]; + JPH_ASSERT(g1 >= 0 && g2 >= 0 && g3 >= 0 && g4 >= 0); + if (g1 == g2 && g1 == g3 && g1 == g4) // In the same group + groups[g1].mVolumeConstraints.push_back(uint(&v - mVolumeConstraints.data())); + else // In different groups -> parallel group + groups.back().mVolumeConstraints.push_back(uint(&v - mVolumeConstraints.data())); + } + for (const Skinned &s : mSkinnedConstraints) + { + int g1 = group_idx[s.mVertex]; + JPH_ASSERT(g1 >= 0); + groups[g1].mSkinnedConstraints.push_back(uint(&s - mSkinnedConstraints.data())); + } + + // Sort the parallel groups from big to small (this means the big groups will be scheduled first and have more time to complete) + QuickSort(groups.begin(), groups.end() - 1, [](const Group &inLHS, const Group &inRHS) { return inLHS.GetSize() > inRHS.GetSize(); }); + + // Make sure we know the closest kinematic vertex so we can sort + CalculateClosestKinematic(); + + // Sort within each group + for (Group &group : groups) + { + // Sort the edge constraints + QuickSort(group.mEdgeConstraints.begin(), group.mEdgeConstraints.end(), [this](uint inLHS, uint inRHS) + { + const Edge &e1 = mEdgeConstraints[inLHS]; + const Edge &e2 = mEdgeConstraints[inRHS]; + + // First sort so that the edge with the smallest distance to a kinematic vertex comes first + float d1 = min(mClosestKinematic[e1.mVertex[0]].mDistance, mClosestKinematic[e1.mVertex[1]].mDistance); + float d2 = min(mClosestKinematic[e2.mVertex[0]].mDistance, mClosestKinematic[e2.mVertex[1]].mDistance); + if (d1 != d2) + return d1 < d2; + + // Order the edges so that the ones with the smallest index go first (hoping to get better cache locality when we process the edges). + // Note we could also re-order the vertices but that would be much more of a burden to the end user + uint32 m1 = e1.GetMinVertexIndex(); + uint32 m2 = e2.GetMinVertexIndex(); + if (m1 != m2) + return m1 < m2; + + return inLHS < inRHS; + }); + + // Sort the LRA constraints + QuickSort(group.mLRAConstraints.begin(), group.mLRAConstraints.end(), [this](uint inLHS, uint inRHS) + { + const LRA &l1 = mLRAConstraints[inLHS]; + const LRA &l2 = mLRAConstraints[inRHS]; + + // First sort so that the longest constraint comes first (meaning the shortest constraint has the most influence on the end result) + // Most of the time there will be a single LRA constraint per vertex and since the LRA constraint only modifies a single vertex, + // updating one constraint will not violate another constraint. + if (l1.mMaxDistance != l2.mMaxDistance) + return l1.mMaxDistance > l2.mMaxDistance; + + // Order constraints so that the ones with the smallest index go first + uint32 m1 = l1.GetMinVertexIndex(); + uint32 m2 = l2.GetMinVertexIndex(); + if (m1 != m2) + return m1 < m2; + + return inLHS < inRHS; + }); + + // Sort the dihedral bend constraints + QuickSort(group.mDihedralBendConstraints.begin(), group.mDihedralBendConstraints.end(), [this](uint inLHS, uint inRHS) + { + const DihedralBend &b1 = mDihedralBendConstraints[inLHS]; + const DihedralBend &b2 = mDihedralBendConstraints[inRHS]; + + // First sort so that the constraint with the smallest distance to a kinematic vertex comes first + float d1 = min( + min(mClosestKinematic[b1.mVertex[0]].mDistance, mClosestKinematic[b1.mVertex[1]].mDistance), + min(mClosestKinematic[b1.mVertex[2]].mDistance, mClosestKinematic[b1.mVertex[3]].mDistance)); + float d2 = min( + min(mClosestKinematic[b2.mVertex[0]].mDistance, mClosestKinematic[b2.mVertex[1]].mDistance), + min(mClosestKinematic[b2.mVertex[2]].mDistance, mClosestKinematic[b2.mVertex[3]].mDistance)); + if (d1 != d2) + return d1 < d2; + + // Order constraints so that the ones with the smallest index go first + uint32 m1 = b1.GetMinVertexIndex(); + uint32 m2 = b2.GetMinVertexIndex(); + if (m1 != m2) + return m1 < m2; + + return inLHS < inRHS; + }); + + // Sort the volume constraints + QuickSort(group.mVolumeConstraints.begin(), group.mVolumeConstraints.end(), [this](uint inLHS, uint inRHS) + { + const Volume &v1 = mVolumeConstraints[inLHS]; + const Volume &v2 = mVolumeConstraints[inRHS]; + + // First sort so that the constraint with the smallest distance to a kinematic vertex comes first + float d1 = min( + min(mClosestKinematic[v1.mVertex[0]].mDistance, mClosestKinematic[v1.mVertex[1]].mDistance), + min(mClosestKinematic[v1.mVertex[2]].mDistance, mClosestKinematic[v1.mVertex[3]].mDistance)); + float d2 = min( + min(mClosestKinematic[v2.mVertex[0]].mDistance, mClosestKinematic[v2.mVertex[1]].mDistance), + min(mClosestKinematic[v2.mVertex[2]].mDistance, mClosestKinematic[v2.mVertex[3]].mDistance)); + if (d1 != d2) + return d1 < d2; + + // Order constraints so that the ones with the smallest index go first + uint32 m1 = v1.GetMinVertexIndex(); + uint32 m2 = v2.GetMinVertexIndex(); + if (m1 != m2) + return m1 < m2; + + return inLHS < inRHS; + }); + + // Sort the skinned constraints + QuickSort(group.mSkinnedConstraints.begin(), group.mSkinnedConstraints.end(), [this](uint inLHS, uint inRHS) + { + const Skinned &s1 = mSkinnedConstraints[inLHS]; + const Skinned &s2 = mSkinnedConstraints[inRHS]; + + // Order the skinned constraints so that the ones with the smallest index go first (hoping to get better cache locality when we process the edges). + if (s1.mVertex != s2.mVertex) + return s1.mVertex < s2.mVertex; + + return inLHS < inRHS; + }); + } + + // Temporary store constraints as we reorder them + Array temp_edges; + temp_edges.swap(mEdgeConstraints); + mEdgeConstraints.reserve(temp_edges.size()); + outResults.mEdgeRemap.reserve(temp_edges.size()); + + Array temp_lra; + temp_lra.swap(mLRAConstraints); + mLRAConstraints.reserve(temp_lra.size()); + outResults.mLRARemap.reserve(temp_lra.size()); + + Array temp_dihedral_bend; + temp_dihedral_bend.swap(mDihedralBendConstraints); + mDihedralBendConstraints.reserve(temp_dihedral_bend.size()); + outResults.mDihedralBendRemap.reserve(temp_dihedral_bend.size()); + + Array temp_volume; + temp_volume.swap(mVolumeConstraints); + mVolumeConstraints.reserve(temp_volume.size()); + outResults.mVolumeRemap.reserve(temp_volume.size()); + + Array temp_skinned; + temp_skinned.swap(mSkinnedConstraints); + mSkinnedConstraints.reserve(temp_skinned.size()); + outResults.mSkinnedRemap.reserve(temp_skinned.size()); + + // Finalize update groups + for (const Group &group : groups) + { + // Reorder edge constraints for this group + for (uint idx : group.mEdgeConstraints) + { + mEdgeConstraints.push_back(temp_edges[idx]); + outResults.mEdgeRemap.push_back(idx); + } + + // Reorder LRA constraints for this group + for (uint idx : group.mLRAConstraints) + { + mLRAConstraints.push_back(temp_lra[idx]); + outResults.mLRARemap.push_back(idx); + } + + // Reorder dihedral bend constraints for this group + for (uint idx : group.mDihedralBendConstraints) + { + mDihedralBendConstraints.push_back(temp_dihedral_bend[idx]); + outResults.mDihedralBendRemap.push_back(idx); + } + + // Reorder volume constraints for this group + for (uint idx : group.mVolumeConstraints) + { + mVolumeConstraints.push_back(temp_volume[idx]); + outResults.mVolumeRemap.push_back(idx); + } + + // Reorder skinned constraints for this group + for (uint idx : group.mSkinnedConstraints) + { + 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() }); + } + + // Free closest kinematic buffer + mClosestKinematic.clear(); + mClosestKinematic.shrink_to_fit(); +} + +Ref SoftBodySharedSettings::Clone() const +{ + Ref clone = new SoftBodySharedSettings; + clone->mVertices = mVertices; + clone->mFaces = mFaces; + clone->mEdgeConstraints = mEdgeConstraints; + clone->mDihedralBendConstraints = mDihedralBendConstraints; + clone->mVolumeConstraints = mVolumeConstraints; + clone->mSkinnedConstraints = mSkinnedConstraints; + clone->mSkinnedConstraintNormals = mSkinnedConstraintNormals; + clone->mInvBindMatrices = mInvBindMatrices; + clone->mLRAConstraints = mLRAConstraints; + clone->mMaterials = mMaterials; + clone->mVertexRadius = mVertexRadius; + clone->mUpdateGroups = mUpdateGroups; + return clone; +} + +void SoftBodySharedSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mVertices); + inStream.Write(mFaces); + inStream.Write(mEdgeConstraints); + inStream.Write(mDihedralBendConstraints); + inStream.Write(mVolumeConstraints); + inStream.Write(mSkinnedConstraints); + inStream.Write(mSkinnedConstraintNormals); + inStream.Write(mLRAConstraints); + inStream.Write(mVertexRadius); + inStream.Write(mUpdateGroups); + + // Can't write mInvBindMatrices directly because the class contains padding + inStream.Write(mInvBindMatrices, [](const InvBind &inElement, StreamOut &inS) { + inS.Write(inElement.mJointIndex); + inS.Write(inElement.mInvBind); + }); +} + +void SoftBodySharedSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mVertices); + inStream.Read(mFaces); + inStream.Read(mEdgeConstraints); + inStream.Read(mDihedralBendConstraints); + inStream.Read(mVolumeConstraints); + inStream.Read(mSkinnedConstraints); + inStream.Read(mSkinnedConstraintNormals); + inStream.Read(mLRAConstraints); + inStream.Read(mVertexRadius); + inStream.Read(mUpdateGroups); + + inStream.Read(mInvBindMatrices, [](StreamIn &inS, InvBind &outElement) { + inS.Read(outElement.mJointIndex); + inS.Read(outElement.mInvBind); + }); +} + +void SoftBodySharedSettings::SaveWithMaterials(StreamOut &inStream, SharedSettingsToIDMap &ioSettingsMap, MaterialToIDMap &ioMaterialMap) const +{ + SharedSettingsToIDMap::const_iterator settings_iter = ioSettingsMap.find(this); + if (settings_iter == ioSettingsMap.end()) + { + // Write settings ID + uint32 settings_id = ioSettingsMap.size(); + ioSettingsMap[this] = settings_id; + inStream.Write(settings_id); + + // Write the settings + SaveBinaryState(inStream); + + // Write materials + StreamUtils::SaveObjectArray(inStream, mMaterials, &ioMaterialMap); + } + else + { + // Known settings, just write the ID + inStream.Write(settings_iter->second); + } +} + +SoftBodySharedSettings::SettingsResult SoftBodySharedSettings::sRestoreWithMaterials(StreamIn &inStream, IDToSharedSettingsMap &ioSettingsMap, IDToMaterialMap &ioMaterialMap) +{ + SettingsResult result; + + // Read settings id + uint32 settings_id; + inStream.Read(settings_id); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to read settings id"); + return result; + } + + // Check nullptr settings + if (settings_id == ~uint32(0)) + { + result.Set(nullptr); + return result; + } + + // Check if we already read this settings + if (settings_id < ioSettingsMap.size()) + { + result.Set(ioSettingsMap[settings_id]); + return result; + } + + // Create new object + Ref settings = new SoftBodySharedSettings; + + // Read state + settings->RestoreBinaryState(inStream); + + // Read materials + Result mlresult = StreamUtils::RestoreObjectArray(inStream, ioMaterialMap); + if (mlresult.HasError()) + { + result.SetError(mlresult.GetError()); + return result; + } + settings->mMaterials = mlresult.Get(); + + // Add the settings to the map + ioSettingsMap.push_back(settings); + + result.Set(settings); + return result; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.h new file mode 100644 index 000000000000..d4b05a6ccdae --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.h @@ -0,0 +1,335 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// This class defines the setup of all particles and their constraints. +/// It is used during the simulation and can be shared between multiple soft bodies. +class JPH_EXPORT SoftBodySharedSettings : public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SoftBodySharedSettings) + +public: + /// Which type of bend constraint should be created + enum class EBendType + { + None, ///< No bend constraints will be created + Distance, ///< A simple distance constraint + Dihedral, ///< A dihedral bend constraint (most expensive, but also supports triangles that are initially not in the same plane) + }; + + /// The type of long range attachment constraint to create + enum class ELRAType + { + None, ///< Don't create a LRA constraint + EuclideanDistance, ///< Create a LRA constraint based on Euclidean distance between the closest kinematic vertex and this vertex + GeodesicDistance, ///< Create a LRA constraint based on the geodesic distance between the closest kinematic vertex and this vertex (follows the edge constraints) + }; + + /// Per vertex attributes used during the CreateConstraints function. + /// For an edge or shear constraint, the compliance is averaged between the two attached vertices. + /// For a bend constraint, the compliance is averaged between the two vertices on the shared edge. + struct JPH_EXPORT VertexAttributes + { + /// Constructor + VertexAttributes() = default; + VertexAttributes(float inCompliance, float inShearCompliance, float inBendCompliance, ELRAType inLRAType = ELRAType::None, float inLRAMaxDistanceMultiplier = 1.0f) : mCompliance(inCompliance), mShearCompliance(inShearCompliance), mBendCompliance(inBendCompliance), mLRAType(inLRAType), mLRAMaxDistanceMultiplier(inLRAMaxDistanceMultiplier) { } + + float mCompliance = 0.0f; ///< The compliance of the normal edges. Set to FLT_MAX to disable regular edges for any edge involving this vertex. + float mShearCompliance = 0.0f; ///< The compliance of the shear edges. Set to FLT_MAX to disable shear edges for any edge involving this vertex. + float mBendCompliance = FLT_MAX; ///< The compliance of the bend edges. Set to FLT_MAX to disable bend edges for any bend constraint involving this vertex. + ELRAType mLRAType = ELRAType::None; ///< The type of long range attachment constraint to create. + float mLRAMaxDistanceMultiplier = 1.0f; ///< Multiplier for the max distance of the LRA constraint, e.g. 1.01 means the max distance is 1% longer than the calculated distance in the rest pose. + }; + + /// Automatically create constraints based on the faces of the soft body + /// @param inVertexAttributes A list of attributes for each vertex (1-on-1 with mVertices, note that if the list is smaller than mVertices the last element will be repeated). This defines the properties of the constraints that are created. + /// @param inVertexAttributesLength The length of inVertexAttributes + /// @param inBendType The type of bend constraint to create + /// @param inAngleTolerance Shear edges are created when two connected triangles form a quad (are roughly in the same plane and form a square with roughly 90 degree angles). This defines the tolerance (in radians). + void CreateConstraints(const VertexAttributes *inVertexAttributes, uint inVertexAttributesLength, EBendType inBendType = EBendType::Distance, float inAngleTolerance = DegreesToRadians(8.0f)); + + /// Calculate the initial lengths of all springs of the edges of this soft body (if you use CreateConstraint, this is already done) + void CalculateEdgeLengths(); + + /// Calculate the max lengths for the long range attachment constraints based on Euclidean distance (if you use CreateConstraints, this is already done) + /// @param inMaxDistanceMultiplier Multiplier for the max distance of the LRA constraint, e.g. 1.01 means the max distance is 1% longer than the calculated distance in the rest pose. + void CalculateLRALengths(float inMaxDistanceMultiplier = 1.0f); + + /// Calculate the constants for the bend constraints (if you use CreateConstraints, this is already done) + void CalculateBendConstraintConstants(); + + /// Calculates the initial volume of all tetrahedra of this soft body + void CalculateVolumeConstraintVolumes(); + + /// Calculate information needed to be able to calculate the skinned constraint normals at run-time + void CalculateSkinnedConstraintNormals(); + + /// Information about the optimization of the soft body, the indices of certain elements may have changed. + class OptimizationResults + { + public: + Array mEdgeRemap; ///< Maps old edge index to new edge index + Array mLRARemap; ///< Maps old LRA index to new LRA 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 + }; + + /// Optimize the soft body settings for simulation. This will reorder constraints so they can be executed in parallel. + void Optimize(OptimizationResults &outResults); + + /// Optimize the soft body settings without results + void Optimize() { OptimizationResults results; Optimize(results); } + + /// Clone this object + Ref Clone() const; + + /// Saves the state of this object in binary form to inStream. Doesn't store the material list. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restore the state of this object from inStream. Doesn't restore the material list. + void RestoreBinaryState(StreamIn &inStream); + + using SharedSettingsToIDMap = StreamUtils::ObjectToIDMap; + using IDToSharedSettingsMap = StreamUtils::IDToObjectMap; + using MaterialToIDMap = StreamUtils::ObjectToIDMap; + using IDToMaterialMap = StreamUtils::IDToObjectMap; + + /// Save this shared settings and its materials. Pass in an empty map ioSettingsMap / ioMaterialMap or reuse the same map while saving multiple settings objects to the same stream in order to avoid writing duplicates. + void SaveWithMaterials(StreamOut &inStream, SharedSettingsToIDMap &ioSettingsMap, MaterialToIDMap &ioMaterialMap) const; + + using SettingsResult = Result>; + + /// Restore a shape and materials. Pass in an empty map in ioSettingsMap / ioMaterialMap or reuse the same map while reading multiple settings objects from the same stream in order to restore duplicates. + static SettingsResult sRestoreWithMaterials(StreamIn &inStream, IDToSharedSettingsMap &ioSettingsMap, IDToMaterialMap &ioMaterialMap); + + /// A vertex is a particle, the data in this structure is only used during creation of the soft body and not during simulation + struct JPH_EXPORT Vertex + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Vertex) + + /// Constructor + Vertex() = default; + Vertex(const Float3 &inPosition, const Float3 &inVelocity = Float3(0, 0, 0), float inInvMass = 1.0f) : mPosition(inPosition), mVelocity(inVelocity), mInvMass(inInvMass) { } + + Float3 mPosition { 0, 0, 0 }; ///< Initial position of the vertex + Float3 mVelocity { 0, 0, 0 }; ///< Initial velocity of the vertex + float mInvMass = 1.0f; ///< Initial inverse of the mass of the vertex + }; + + /// A face defines the surface of the body + struct JPH_EXPORT Face + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Face) + + /// Constructor + Face() = default; + Face(uint32 inVertex1, uint32 inVertex2, uint32 inVertex3, uint32 inMaterialIndex = 0) : mVertex { inVertex1, inVertex2, inVertex3 }, mMaterialIndex(inMaterialIndex) { } + + /// Check if this is a degenerate face (a face which points to the same vertex twice) + bool IsDegenerate() const { return mVertex[0] == mVertex[1] || mVertex[0] == mVertex[2] || mVertex[1] == mVertex[2]; } + + uint32 mVertex[3]; ///< Indices of the vertices that form the face + uint32 mMaterialIndex = 0; ///< Index of the material of the face in SoftBodySharedSettings::mMaterials + }; + + /// An edge keeps two vertices at a constant distance using a spring: |x1 - x2| = rest length + struct JPH_EXPORT Edge + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Edge) + + /// Constructor + Edge() = default; + Edge(uint32 inVertex1, uint32 inVertex2, float inCompliance = 0.0f) : mVertex { inVertex1, inVertex2 }, mCompliance(inCompliance) { } + + /// Return the lowest vertex index of this constraint + uint32 GetMinVertexIndex() const { return min(mVertex[0], mVertex[1]); } + + uint32 mVertex[2]; ///< Indices of the vertices that form the edge + float mRestLength = 1.0f; ///< Rest length of the spring + float mCompliance = 0.0f; ///< Inverse of the stiffness of the spring + }; + + /** + * A dihedral bend constraint keeps the angle between two triangles constant along their shared edge. + * + * x2 + * / \ + * / t0 \ + * x0----x1 + * \ t1 / + * \ / + * x3 + * + * x0..x3 are the vertices, t0 and t1 are the triangles that share the edge x0..x1 + * + * Based on: + * - "Position Based Dynamics" - Matthias Muller et al. + * - "Strain Based Dynamics" - Matthias Muller et al. + * - "Simulation of Clothing with Folds and Wrinkles" - R. Bridson et al. + */ + struct JPH_EXPORT DihedralBend + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, DihedralBend) + + /// Constructor + DihedralBend() = default; + DihedralBend(uint32 inVertex1, uint32 inVertex2, uint32 inVertex3, uint32 inVertex4, float inCompliance = 0.0f) : mVertex { inVertex1, inVertex2, inVertex3, inVertex4 }, mCompliance(inCompliance) { } + + /// Return the lowest vertex index of this constraint + uint32 GetMinVertexIndex() const { return min(min(mVertex[0], mVertex[1]), min(mVertex[2], mVertex[3])); } + + uint32 mVertex[4]; ///< Indices of the vertices of the 2 triangles that share an edge (the first 2 vertices are the shared edge) + float mCompliance = 0.0f; ///< Inverse of the stiffness of the constraint + float mInitialAngle = 0.0f; ///< Initial angle between the normals of the triangles (pi - dihedral angle). + }; + + /// Volume constraint, keeps the volume of a tetrahedron constant + struct JPH_EXPORT Volume + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Volume) + + /// Constructor + Volume() = default; + Volume(uint32 inVertex1, uint32 inVertex2, uint32 inVertex3, uint32 inVertex4, float inCompliance = 0.0f) : mVertex { inVertex1, inVertex2, inVertex3, inVertex4 }, mCompliance(inCompliance) { } + + /// Return the lowest vertex index of this constraint + uint32 GetMinVertexIndex() const { return min(min(mVertex[0], mVertex[1]), min(mVertex[2], mVertex[3])); } + + uint32 mVertex[4]; ///< Indices of the vertices that form the tetrhedron + float mSixRestVolume = 1.0f; ///< 6 times the rest volume of the tetrahedron (calculated by CalculateVolumeConstraintVolumes()) + float mCompliance = 0.0f; ///< Inverse of the stiffness of the constraint + }; + + /// An inverse bind matrix take a skinned vertex from its bind pose into joint local space + class JPH_EXPORT InvBind + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, InvBind) + + public: + /// Constructor + InvBind() = default; + InvBind(uint32 inJointIndex, Mat44Arg inInvBind) : mJointIndex(inJointIndex), mInvBind(inInvBind) { } + + uint32 mJointIndex = 0; ///< Joint index to which this is attached + Mat44 mInvBind = Mat44::sIdentity(); ///< The inverse bind matrix, this takes a vertex in its bind pose (Vertex::mPosition) to joint local space + }; + + /// A joint and its skin weight + class JPH_EXPORT SkinWeight + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SkinWeight) + + public: + /// Constructor + SkinWeight() = default; + SkinWeight(uint32 inInvBindIndex, float inWeight) : mInvBindIndex(inInvBindIndex), mWeight(inWeight) { } + + uint32 mInvBindIndex = 0; ///< Index in mInvBindMatrices + float mWeight = 0.0f; ///< Weight with which it is skinned + }; + + /// A constraint that skins a vertex to joints and limits the distance that the simulated vertex can travel from this vertex + class JPH_EXPORT Skinned + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Skinned) + + public: + /// Constructor + Skinned() = default; + Skinned(uint32 inVertex, float inMaxDistance, float inBackStopDistance, float inBackStopRadius) : mVertex(inVertex), mMaxDistance(inMaxDistance), mBackStopDistance(inBackStopDistance), mBackStopRadius(inBackStopRadius) { } + + /// Normalize the weights so that they add up to 1 + void NormalizeWeights() + { + // Get the total weight + float total = 0.0f; + for (const SkinWeight &w : mWeights) + total += w.mWeight; + + // Normalize + if (total > 0.0f) + for (SkinWeight &w : mWeights) + w.mWeight /= total; + } + + /// Maximum number of skin weights + static constexpr uint cMaxSkinWeights = 4; + + uint32 mVertex = 0; ///< Index in mVertices which indicates which vertex is being skinned + SkinWeight mWeights[cMaxSkinWeights]; ///< Skin weights, the bind pose of the vertex is assumed to be stored in Vertex::mPosition. The first weight that is zero indicates the end of the list. Weights should add up to 1. + float mMaxDistance = FLT_MAX; ///< Maximum distance that this vertex can reach from the skinned vertex, disabled when FLT_MAX. 0 when you want to hard skin the vertex to the skinned vertex. + float mBackStopDistance = FLT_MAX; ///< Disabled if mBackStopDistance >= 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()) + }; + + /// A long range attachment constraint, this is a constraint that sets a max distance between a kinematic vertex and a dynamic vertex + /// See: "Long Range Attachments - A Method to Simulate Inextensible Clothing in Computer Games", Tae-Yong Kim, Nuttapong Chentanez and Matthias Mueller-Fischer + class JPH_EXPORT LRA + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, LRA) + + public: + /// Constructor + LRA() = default; + LRA(uint32 inVertex1, uint32 inVertex2, float inMaxDistance) : mVertex { inVertex1, inVertex2 }, mMaxDistance(inMaxDistance) { } + + /// Return the lowest vertex index of this constraint + uint32 GetMinVertexIndex() const { return min(mVertex[0], mVertex[1]); } + + uint32 mVertex[2]; ///< The vertices that are connected. The first vertex should be kinematic, the 2nd dynamic. + float mMaxDistance = 0.0f; ///< The maximum distance between the vertices + }; + + /// Add a face to this soft body + void AddFace(const Face &inFace) { JPH_ASSERT(!inFace.IsDegenerate()); mFaces.push_back(inFace); } + + Array mVertices; ///< The list of vertices or particles of the body + Array mFaces; ///< The list of faces of the body + Array mEdgeConstraints; ///< The list of edges or springs of the body + Array mDihedralBendConstraints; ///< The list of dihedral bend constraints of the body + Array mVolumeConstraints; ///< The list of volume constraints of the body that keep the volume of tetrahedra in the soft body constant + Array 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 + 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; + + /// Calculate the closest kinematic vertex array + void CalculateClosestKinematic(); + + /// Tracks the closest kinematic vertex + struct ClosestKinematic + { + uint32 mVertex = 0xffffffff; ///< Vertex index of closest kinematic vertex + float mDistance = FLT_MAX; ///< Distance to the closest kinematic vertex + }; + + /// Tracks the end indices of the various constraint groups + struct UpdateGroup + { + uint mEdgeEndIndex; ///< The end index of the edge constraints in this group + uint mLRAEndIndex; ///< The end index of the LRA constraints in this group + uint mDihedralBendEndIndex; ///< The end index of the dihedral bend constraints in this group + uint mVolumeEndIndex; ///< The end index of the volume constraints in this group + uint mSkinnedEndIndex; ///< The end index of the skinned constraints in this group + }; + + Array 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() +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyUpdateContext.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyUpdateContext.h new file mode 100644 index 000000000000..2f565f5693c1 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyUpdateContext.h @@ -0,0 +1,59 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class Body; +class SoftBodyMotionProperties; +class SoftBodyContactListener; +class SimShapeFilter; + +/// Temporary data used by the update of a soft body +class SoftBodyUpdateContext : public NonCopyable +{ +public: + static constexpr uint cVertexCollisionBatch = 64; ///< Number of vertices to process in a batch in DetermineCollisionPlanes + static constexpr uint cVertexConstraintBatch = 256; ///< Number of vertices to group for processing batches of constraints in ApplyEdgeConstraints + + // Input + Body * mBody; ///< Body that is being updated + SoftBodyMotionProperties * mMotionProperties; ///< Motion properties of that body + SoftBodyContactListener * mContactListener; ///< Contact listener to fire callbacks to + const SimShapeFilter * mSimShapeFilter; ///< Shape filter to use for collision detection + RMat44 mCenterOfMassTransform; ///< Transform of the body relative to the soft body + Vec3 mGravity; ///< Gravity vector in local space of the soft body + Vec3 mDisplacementDueToGravity; ///< Displacement of the center of mass due to gravity in the current time step + float mDeltaTime; ///< Delta time for the current time step + float mSubStepDeltaTime; ///< Delta time for each sub step + + /// Describes progress in the current update + enum class EState + { + DetermineCollisionPlanes, ///< Determine collision planes for vertices in parallel + DetermineSensorCollisions, ///< Determine collisions with sensors in parallel + ApplyConstraints, ///< Apply constraints in parallel + Done ///< Update is finished + }; + + // State of the update + atomic mState { EState::DetermineCollisionPlanes };///< Current state of the update + atomic mNextCollisionVertex { 0 }; ///< Next vertex to process for DetermineCollisionPlanes + atomic mNumCollisionVerticesProcessed { 0 }; ///< Number of vertices processed by DetermineCollisionPlanes, used to determine if we can go to the next step + atomic mNextSensorIndex { 0 }; ///< Next sensor to process for DetermineCollisionPlanes + atomic mNumSensorsProcessed { 0 }; ///< Number of sensors processed by DetermineSensorCollisions, used to determine if we can go to the next step + atomic mNextIteration { 0 }; ///< Next simulation iteration to process + atomic mNextConstraintGroup { 0 }; ///< Next constraint group to process + atomic mNumConstraintGroupsProcessed { 0 }; ///< Number of groups processed, used to determine if we can go to the next iteration + + // Output + Vec3 mDeltaPosition; ///< Delta position of the body in the current time step, should be applied after the update + ECanSleep mCanSleep; ///< Can the body sleep? Should be applied after the update +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyVertex.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyVertex.h new file mode 100644 index 000000000000..72d4291e216c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyVertex.h @@ -0,0 +1,36 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Run time information for a single particle of a soft body +/// Note that at run-time you should only modify the inverse mass and/or velocity of a vertex to control the soft body. +/// Modifying the position can lead to missed collisions. +/// The other members are used internally by the soft body solver. +class SoftBodyVertex +{ +public: + /// Reset collision information to prepare for a new collision check + inline void ResetCollision() + { + mLargestPenetration = -FLT_MAX; + mCollidingShapeIndex = -1; + mHasContact = false; + } + + Vec3 mPreviousPosition; ///< Internal use only. Position at the previous time step + Vec3 mPosition; ///< Position, relative to the center of mass of the soft body + Vec3 mVelocity; ///< Velocity, relative to the center of mass of the soft body + Plane mCollisionPlane; ///< Internal use only. Nearest collision plane, relative to the center of mass of the soft body + int mCollidingShapeIndex; ///< Internal use only. Index in the colliding shapes list of the body we may collide with + bool mHasContact; ///< True if the vertex has collided with anything in the last update + float mLargestPenetration; ///< Internal use only. Used while finding the collision plane, stores the largest penetration found so far + float mInvMass; ///< Inverse mass (1 / mass) +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/StateRecorder.h b/thirdparty/jolt_physics/Jolt/Physics/StateRecorder.h new file mode 100644 index 000000000000..c3bd477107f1 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/StateRecorder.h @@ -0,0 +1,136 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class Body; +class Constraint; +class BodyID; + +JPH_SUPPRESS_WARNING_PUSH +JPH_GCC_SUPPRESS_WARNING("-Wshadow") // GCC complains about the 'Constraints' value conflicting with the 'Constraints' typedef + +/// A bit field that determines which aspects of the simulation to save +enum class EStateRecorderState : uint8 +{ + None = 0, ///< Save nothing + Global = 1, ///< Save global physics system state (delta time, gravity, etc.) + Bodies = 2, ///< Save the state of bodies + Contacts = 4, ///< Save the state of contacts + Constraints = 8, ///< Save the state of constraints + All = Global | Bodies | Contacts | Constraints ///< Save all state +}; + +JPH_SUPPRESS_WARNING_POP + +/// Bitwise OR operator for EStateRecorderState +constexpr EStateRecorderState operator | (EStateRecorderState inLHS, EStateRecorderState inRHS) +{ + return EStateRecorderState(uint8(inLHS) | uint8(inRHS)); +} + +/// Bitwise AND operator for EStateRecorderState +constexpr EStateRecorderState operator & (EStateRecorderState inLHS, EStateRecorderState inRHS) +{ + return EStateRecorderState(uint8(inLHS) & uint8(inRHS)); +} + +/// Bitwise XOR operator for EStateRecorderState +constexpr EStateRecorderState operator ^ (EStateRecorderState inLHS, EStateRecorderState inRHS) +{ + return EStateRecorderState(uint8(inLHS) ^ uint8(inRHS)); +} + +/// Bitwise NOT operator for EStateRecorderState +constexpr EStateRecorderState operator ~ (EStateRecorderState inAllowedDOFs) +{ + return EStateRecorderState(~uint8(inAllowedDOFs)); +} + +/// Bitwise OR assignment operator for EStateRecorderState +constexpr EStateRecorderState & operator |= (EStateRecorderState &ioLHS, EStateRecorderState inRHS) +{ + ioLHS = ioLHS | inRHS; + return ioLHS; +} + +/// Bitwise AND assignment operator for EStateRecorderState +constexpr EStateRecorderState & operator &= (EStateRecorderState &ioLHS, EStateRecorderState inRHS) +{ + ioLHS = ioLHS & inRHS; + return ioLHS; +} + +/// Bitwise XOR assignment operator for EStateRecorderState +constexpr EStateRecorderState & operator ^= (EStateRecorderState &ioLHS, EStateRecorderState inRHS) +{ + ioLHS = ioLHS ^ inRHS; + return ioLHS; +} + +/// User callbacks that allow determining which parts of the simulation should be saved by a StateRecorder +class JPH_EXPORT StateRecorderFilter +{ +public: + /// Destructor + virtual ~StateRecorderFilter() = default; + + ///@name Functions called during SaveState + ///@{ + + /// If the state of a specific body should be saved + virtual bool ShouldSaveBody([[maybe_unused]] const Body &inBody) const { return true; } + + /// If the state of a specific constraint should be saved + virtual bool ShouldSaveConstraint([[maybe_unused]] const Constraint &inConstraint) const { return true; } + + /// If the state of a specific contact should be saved + virtual bool ShouldSaveContact([[maybe_unused]] const BodyID &inBody1, [[maybe_unused]] const BodyID &inBody2) const { return true; } + + ///@} + ///@name Functions called during RestoreState + ///@{ + + /// If the state of a specific contact should be restored + virtual bool ShouldRestoreContact([[maybe_unused]] const BodyID &inBody1, [[maybe_unused]] const BodyID &inBody2) const { return true; } + + ///@} +}; + +/// Class that records the state of a physics system. Can be used to check if the simulation is deterministic by putting the recorder in validation mode. +/// Can be used to restore the state to an earlier point in time. Note that only the state that is modified by the simulation is saved, configuration settings +/// like body friction or restitution, motion quality etc. are not saved and need to be saved by the user if desired. +class JPH_EXPORT StateRecorder : public StreamIn, public StreamOut +{ +public: + /// Constructor + StateRecorder() = default; + StateRecorder(const StateRecorder &inRHS) : mIsValidating(inRHS.mIsValidating) { } + + /// Sets the stream in validation mode. In this case the physics system ensures that before it calls ReadBytes that it will + /// ensure that those bytes contain the current state. This makes it possible to step and save the state, restore to the previous + /// step and step again and when the recorded state is not the same it can restore the expected state and any byte that changes + /// due to a ReadBytes function can be caught to find out which part of the simulation is not deterministic. + /// Note that validation only works when saving the full state of the simulation (EStateRecorderState::All, StateRecorderFilter == nullptr). + void SetValidating(bool inValidating) { mIsValidating = inValidating; } + bool IsValidating() const { return mIsValidating; } + + /// This allows splitting the state in multiple parts. While restoring, only the last part should have this flag set to true. + /// Note that you should ensure that the different parts contain information for disjoint sets of bodies, constraints and contacts. + /// E.g. if you restore the same contact twice, you get undefined behavior. In order to create disjoint sets you can use the StateRecorderFilter. + /// Note that validation is not compatible with restoring a simulation state in multiple parts. + void SetIsLastPart(bool inIsLastPart) { mIsLastPart = inIsLastPart; } + bool IsLastPart() const { return mIsLastPart; } + +private: + bool mIsValidating = false; + bool mIsLastPart = true; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/StateRecorderImpl.cpp b/thirdparty/jolt_physics/Jolt/Physics/StateRecorderImpl.cpp new file mode 100644 index 000000000000..7624135896e6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/StateRecorderImpl.cpp @@ -0,0 +1,90 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +void StateRecorderImpl::WriteBytes(const void *inData, size_t inNumBytes) +{ + mStream.write((const char *)inData, inNumBytes); +} + +void StateRecorderImpl::Rewind() +{ + mStream.seekg(0, std::stringstream::beg); +} + +void StateRecorderImpl::Clear() +{ + mStream.clear(); + mStream.str({}); +} + +void StateRecorderImpl::ReadBytes(void *outData, size_t inNumBytes) +{ + if (IsValidating()) + { + // Read data in temporary buffer to compare with current value + void *data = JPH_STACK_ALLOC(inNumBytes); + mStream.read((char *)data, inNumBytes); + if (memcmp(data, outData, inNumBytes) != 0) + { + // Mismatch, print error + Trace("Mismatch reading %u bytes", (uint)inNumBytes); + for (size_t i = 0; i < inNumBytes; ++i) + { + int b1 = reinterpret_cast(outData)[i]; + int b2 = reinterpret_cast(data)[i]; + if (b1 != b2) + Trace("Offset %d: %02X -> %02X", i, b1, b2); + } + JPH_BREAKPOINT; + } + + // Copy the temporary data to the final destination + memcpy(outData, data, inNumBytes); + return; + } + + mStream.read((char *)outData, inNumBytes); +} + +bool StateRecorderImpl::IsEqual(StateRecorderImpl &inReference) +{ + // Get length of new state + mStream.seekg(0, std::stringstream::end); + std::streamoff this_len = mStream.tellg(); + mStream.seekg(0, std::stringstream::beg); + + // Get length of old state + inReference.mStream.seekg(0, std::stringstream::end); + std::streamoff reference_len = inReference.mStream.tellg(); + inReference.mStream.seekg(0, std::stringstream::beg); + + // Compare size + bool fail = reference_len != this_len; + if (fail) + { + Trace("Failed to properly recover state, different stream length!"); + return false; + } + + // Compare byte by byte + for (std::streamoff i = 0, l = this_len; !fail && i < l; ++i) + { + fail = inReference.mStream.get() != mStream.get(); + if (fail) + { + Trace("Failed to properly recover state, different at offset %d!", (int)i); + return false; + } + } + + return true; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/StateRecorderImpl.h b/thirdparty/jolt_physics/Jolt/Physics/StateRecorderImpl.h new file mode 100644 index 000000000000..c994ece3e4c0 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/StateRecorderImpl.h @@ -0,0 +1,50 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Implementation of the StateRecorder class that uses a stringstream as underlying store and that implements checking if the state doesn't change upon reading +class JPH_EXPORT StateRecorderImpl final : public StateRecorder +{ +public: + /// Constructor + StateRecorderImpl() = default; + StateRecorderImpl(StateRecorderImpl &&inRHS) : StateRecorder(inRHS), mStream(std::move(inRHS.mStream)) { } + + /// Write a string of bytes to the binary stream + virtual void WriteBytes(const void *inData, size_t inNumBytes) override; + + /// Rewind the stream for reading + void Rewind(); + + /// Clear the stream for reuse + void Clear(); + + /// Read a string of bytes from the binary stream + virtual void ReadBytes(void *outData, size_t inNumBytes) override; + + // See StreamIn + virtual bool IsEOF() const override { return mStream.eof(); } + + // See StreamIn / StreamOut + virtual bool IsFailed() const override { return mStream.fail(); } + + /// Compare this state with a reference state and ensure they are the same + bool IsEqual(StateRecorderImpl &inReference); + + /// Convert the binary data to a string + std::string GetData() const { return mStream.str(); } + + /// Get size of the binary data in bytes + size_t GetDataSize() { return size_t(mStream.tellp()); } + +private: + std::stringstream mStream; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.cpp new file mode 100644 index 000000000000..1bfa564f76fb --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.cpp @@ -0,0 +1,293 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(MotorcycleControllerSettings) +{ + JPH_ADD_BASE_CLASS(MotorcycleControllerSettings, VehicleControllerSettings) + + JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mMaxLeanAngle) + JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringConstant) + JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringDamping) + JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringIntegrationCoefficient) + JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringIntegrationCoefficientDecay) + JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSmoothingFactor) +} + +VehicleController *MotorcycleControllerSettings::ConstructController(VehicleConstraint &inConstraint) const +{ + return new MotorcycleController(*this, inConstraint); +} + +void MotorcycleControllerSettings::SaveBinaryState(StreamOut &inStream) const +{ + WheeledVehicleControllerSettings::SaveBinaryState(inStream); + + inStream.Write(mMaxLeanAngle); + inStream.Write(mLeanSpringConstant); + inStream.Write(mLeanSpringDamping); + inStream.Write(mLeanSpringIntegrationCoefficient); + inStream.Write(mLeanSpringIntegrationCoefficientDecay); + inStream.Write(mLeanSmoothingFactor); +} + +void MotorcycleControllerSettings::RestoreBinaryState(StreamIn &inStream) +{ + WheeledVehicleControllerSettings::RestoreBinaryState(inStream); + + inStream.Read(mMaxLeanAngle); + inStream.Read(mLeanSpringConstant); + inStream.Read(mLeanSpringDamping); + inStream.Read(mLeanSpringIntegrationCoefficient); + inStream.Read(mLeanSpringIntegrationCoefficientDecay); + inStream.Read(mLeanSmoothingFactor); +} + +MotorcycleController::MotorcycleController(const MotorcycleControllerSettings &inSettings, VehicleConstraint &inConstraint) : + WheeledVehicleController(inSettings, inConstraint), + mMaxLeanAngle(inSettings.mMaxLeanAngle), + mLeanSpringConstant(inSettings.mLeanSpringConstant), + mLeanSpringDamping(inSettings.mLeanSpringDamping), + mLeanSpringIntegrationCoefficient(inSettings.mLeanSpringIntegrationCoefficient), + mLeanSpringIntegrationCoefficientDecay(inSettings.mLeanSpringIntegrationCoefficientDecay), + mLeanSmoothingFactor(inSettings.mLeanSmoothingFactor) +{ +} + +float MotorcycleController::GetWheelBase() const +{ + float low = FLT_MAX, high = -FLT_MAX; + + for (const Wheel *w : mConstraint.GetWheels()) + { + const WheelSettings *s = w->GetSettings(); + + // Measure distance along the forward axis by looking at the fully extended suspension. + // If the suspension force point is active, use that instead. + Vec3 force_point = s->mEnableSuspensionForcePoint? s->mSuspensionForcePoint : s->mPosition + s->mSuspensionDirection * s->mSuspensionMaxLength; + float value = force_point.Dot(mConstraint.GetLocalForward()); + + // Update min and max + low = min(low, value); + high = max(high, value); + } + + return high - low; +} + +void MotorcycleController::PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) +{ + WheeledVehicleController::PreCollide(inDeltaTime, inPhysicsSystem); + + const Body *body = mConstraint.GetVehicleBody(); + Vec3 forward = body->GetRotation() * mConstraint.GetLocalForward(); + float wheel_base = GetWheelBase(); + Vec3 world_up = mConstraint.GetWorldUp(); + + if (mEnableLeanController) + { + // Calculate the target lean vector, this is in the direction of the total applied impulse by the ground on the wheels + Vec3 target_lean = Vec3::sZero(); + for (const Wheel *w : mConstraint.GetWheels()) + if (w->HasContact()) + target_lean += w->GetContactNormal() * w->GetSuspensionLambda() + w->GetContactLateral() * w->GetLateralLambda(); + + // Normalize the impulse + target_lean = target_lean.NormalizedOr(world_up); + + // Smooth the impulse to avoid jittery behavior + mTargetLean = mLeanSmoothingFactor * mTargetLean + (1.0f - mLeanSmoothingFactor) * target_lean; + + // Remove forward component, we can only lean sideways + mTargetLean -= forward * mTargetLean.Dot(forward); + mTargetLean = mTargetLean.NormalizedOr(world_up); + + // Clamp the target lean against the max lean angle + Vec3 adjusted_world_up = world_up - forward * world_up.Dot(forward); + adjusted_world_up = adjusted_world_up.NormalizedOr(world_up); + float w_angle = -Sign(mTargetLean.Cross(adjusted_world_up).Dot(forward)) * ACos(mTargetLean.Dot(adjusted_world_up)); + if (abs(w_angle) > mMaxLeanAngle) + mTargetLean = Quat::sRotation(forward, Sign(w_angle) * mMaxLeanAngle) * adjusted_world_up; + + // Integrate the delta angle + Vec3 up = body->GetRotation() * mConstraint.GetLocalUp(); + float d_angle = -Sign(mTargetLean.Cross(up).Dot(forward)) * ACos(mTargetLean.Dot(up)); + mLeanSpringIntegratedDeltaAngle += d_angle * inDeltaTime; + } + else + { + // Controller not enabled, reset target lean + mTargetLean = world_up; + + // Reset integrated delta angle + mLeanSpringIntegratedDeltaAngle = 0; + } + + JPH_DET_LOG("WheeledVehicleController::PreCollide: mTargetLean: " << mTargetLean); + + // Calculate max steering angle based on the max lean angle we're willing to take + // See: https://en.wikipedia.org/wiki/Bicycle_and_motorcycle_dynamics#Leaning + // LeanAngle = Atan(Velocity^2 / (Gravity * TurnRadius)) + // And: https://en.wikipedia.org/wiki/Turning_radius (we're ignoring the tire width) + // The CasterAngle is the added according to https://en.wikipedia.org/wiki/Bicycle_and_motorcycle_dynamics#Turning (this is the same formula but without small angle approximation) + // TurnRadius = WheelBase / (Sin(SteerAngle) * Cos(CasterAngle)) + // => SteerAngle = ASin(WheelBase * Tan(LeanAngle) * Gravity / (Velocity^2 * Cos(CasterAngle)) + // The caster angle is different for each wheel so we can only calculate part of the equation here + float max_steer_angle_factor = wheel_base * Tan(mMaxLeanAngle) * (mConstraint.IsGravityOverridden()? mConstraint.GetGravityOverride() : inPhysicsSystem.GetGravity()).Length(); + + // Calculate forward velocity + float velocity = body->GetLinearVelocity().Dot(forward); + float velocity_sq = Square(velocity); + + // Decompose steering into sign and direction + float steer_strength = abs(mRightInput); + float steer_sign = -Sign(mRightInput); + + for (Wheel *w_base : mConstraint.GetWheels()) + { + WheelWV *w = static_cast(w_base); + const WheelSettingsWV *s = w->GetSettings(); + + // Check if this wheel can steer + if (s->mMaxSteerAngle != 0.0f) + { + // Calculate cos(caster angle), the angle between the steering axis and the up vector + float cos_caster_angle = s->mSteeringAxis.Dot(mConstraint.GetLocalUp()); + + // Calculate steer angle + float steer_angle = steer_strength * w->GetSettings()->mMaxSteerAngle; + + // Clamp to max steering angle + if (mEnableLeanSteeringLimit + && velocity_sq > 1.0e-6f && cos_caster_angle > 1.0e-6f) + { + float max_steer_angle = ASin(max_steer_angle_factor / (velocity_sq * cos_caster_angle)); + steer_angle = min(steer_angle, max_steer_angle); + } + + // Set steering angle + w->SetSteerAngle(steer_sign * steer_angle); + } + } + + // Reset applied impulse + mAppliedImpulse = 0; +} + +bool MotorcycleController::SolveLongitudinalAndLateralConstraints(float inDeltaTime) +{ + bool impulse = WheeledVehicleController::SolveLongitudinalAndLateralConstraints(inDeltaTime); + + if (mEnableLeanController) + { + // Only apply a lean impulse if all wheels are in contact, otherwise we can easily spin out + bool all_in_contact = true; + for (const Wheel *w : mConstraint.GetWheels()) + if (!w->HasContact() || w->GetSuspensionLambda() <= 0.0f) + { + all_in_contact = false; + break; + } + + if (all_in_contact) + { + Body *body = mConstraint.GetVehicleBody(); + const MotionProperties *mp = body->GetMotionProperties(); + + Vec3 forward = body->GetRotation() * mConstraint.GetLocalForward(); + Vec3 up = body->GetRotation() * mConstraint.GetLocalUp(); + + // Calculate delta to target angle and derivative + float d_angle = -Sign(mTargetLean.Cross(up).Dot(forward)) * ACos(mTargetLean.Dot(up)); + float ddt_angle = body->GetAngularVelocity().Dot(forward); + + // Calculate impulse to apply to get to target lean angle + float total_impulse = (mLeanSpringConstant * d_angle - mLeanSpringDamping * ddt_angle + mLeanSpringIntegrationCoefficient * mLeanSpringIntegratedDeltaAngle) * inDeltaTime; + + // Remember angular velocity pre angular impulse + Vec3 old_w = mp->GetAngularVelocity(); + + // Apply impulse taking into account the impulse we've applied earlier + float delta_impulse = total_impulse - mAppliedImpulse; + body->AddAngularImpulse(delta_impulse * forward); + mAppliedImpulse = total_impulse; + + // Calculate delta angular velocity due to angular impulse + Vec3 dw = mp->GetAngularVelocity() - old_w; + Vec3 linear_acceleration = Vec3::sZero(); + float total_lambda = 0.0f; + for (Wheel *w_base : mConstraint.GetWheels()) + { + const WheelWV *w = static_cast(w_base); + + // We weigh the importance of each contact point according to the contact force + float lambda = w->GetSuspensionLambda(); + total_lambda += lambda; + + // Linear acceleration of contact point is dw x com_to_contact + Vec3 r = Vec3(w->GetContactPosition() - body->GetCenterOfMassPosition()); + linear_acceleration += lambda * dw.Cross(r); + } + + // Apply linear impulse to COM to cancel the average velocity change on the wheels due to the angular impulse + Vec3 linear_impulse = -linear_acceleration / (total_lambda * mp->GetInverseMass()); + body->AddImpulse(linear_impulse); + + // Return true if we applied an impulse + impulse |= delta_impulse != 0.0f; + } + else + { + // Decay the integrated angle because we won't be applying a torque this frame + // Uses 1st order Taylor approximation of e^(-decay * dt) = 1 - decay * dt + mLeanSpringIntegratedDeltaAngle *= max(0.0f, 1.0f - mLeanSpringIntegrationCoefficientDecay * inDeltaTime); + } + } + + return impulse; +} + +void MotorcycleController::SaveState(StateRecorder &inStream) const +{ + WheeledVehicleController::SaveState(inStream); + + inStream.Write(mTargetLean); +} + +void MotorcycleController::RestoreState(StateRecorder &inStream) +{ + WheeledVehicleController::RestoreState(inStream); + + inStream.Read(mTargetLean); +} + +#ifdef JPH_DEBUG_RENDERER + +void MotorcycleController::Draw(DebugRenderer *inRenderer) const +{ + WheeledVehicleController::Draw(inRenderer); + + // Draw current and desired lean angle + Body *body = mConstraint.GetVehicleBody(); + RVec3 center_of_mass = body->GetCenterOfMassPosition(); + Vec3 up = body->GetRotation() * mConstraint.GetLocalUp(); + inRenderer->DrawArrow(center_of_mass, center_of_mass + up, Color::sYellow, 0.1f); + inRenderer->DrawArrow(center_of_mass, center_of_mass + mTargetLean, Color::sRed, 0.1f); +} + +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.h new file mode 100644 index 000000000000..bf8ebfa43f4b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.h @@ -0,0 +1,116 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Settings of a two wheeled motorcycle (adds a spring to balance the motorcycle) +/// Note: The motor cycle controller is still in development and may need a lot of tweaks/hacks to work properly! +class JPH_EXPORT MotorcycleControllerSettings : public WheeledVehicleControllerSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, MotorcycleControllerSettings) + +public: + // See: VehicleControllerSettings + virtual VehicleController * ConstructController(VehicleConstraint &inConstraint) const override; + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void RestoreBinaryState(StreamIn &inStream) override; + + /// How far we're willing to make the bike lean over in turns (in radians) + float mMaxLeanAngle = DegreesToRadians(45.0f); + + /// Spring constant for the lean spring + float mLeanSpringConstant = 5000.0f; + + /// Spring damping constant for the lean spring + float mLeanSpringDamping = 1000.0f; + + /// The lean spring applies an additional force equal to this coefficient * Integral(delta angle, 0, t), this effectively makes the lean spring a PID controller + float mLeanSpringIntegrationCoefficient = 0.0f; + + /// How much to decay the angle integral when the wheels are not touching the floor: new_value = e^(-decay * t) * initial_value + float mLeanSpringIntegrationCoefficientDecay = 4.0f; + + /// How much to smooth the lean angle (0 = no smoothing, 1 = lean angle never changes) + /// Note that this is frame rate dependent because the formula is: smoothing_factor * previous + (1 - smoothing_factor) * current + float mLeanSmoothingFactor = 0.8f; +}; + +/// Runtime controller class +class JPH_EXPORT MotorcycleController : public WheeledVehicleController +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + MotorcycleController(const MotorcycleControllerSettings &inSettings, VehicleConstraint &inConstraint); + + /// Get the distance between the front and back wheels + float GetWheelBase() const; + + /// Enable or disable the lean spring. This allows you to temporarily disable the lean spring to allow the motorcycle to fall over. + void EnableLeanController(bool inEnable) { mEnableLeanController = inEnable; } + + /// Check if the lean spring is enabled. + bool IsLeanControllerEnabled() const { return mEnableLeanController; } + + /// Enable or disable the lean steering limit. When enabled (default) the steering angle is limited based on the vehicle speed to prevent steering that would cause an inertial force that causes the motorcycle to topple over. + void EnableLeanSteeringLimit(bool inEnable) { mEnableLeanSteeringLimit = inEnable; } + bool IsLeanSteeringLimitEnabled() const { return mEnableLeanSteeringLimit; } + + /// Spring constant for the lean spring + void SetLeanSpringConstant(float inConstant) { mLeanSpringConstant = inConstant; } + float GetLeanSpringConstant() const { return mLeanSpringConstant; } + + /// Spring damping constant for the lean spring + void SetLeanSpringDamping(float inDamping) { mLeanSpringDamping = inDamping; } + float GetLeanSpringDamping() const { return mLeanSpringDamping; } + + /// The lean spring applies an additional force equal to this coefficient * Integral(delta angle, 0, t), this effectively makes the lean spring a PID controller + void SetLeanSpringIntegrationCoefficient(float inCoefficient) { mLeanSpringIntegrationCoefficient = inCoefficient; } + float GetLeanSpringIntegrationCoefficient() const { return mLeanSpringIntegrationCoefficient; } + + /// How much to decay the angle integral when the wheels are not touching the floor: new_value = e^(-decay * t) * initial_value + void SetLeanSpringIntegrationCoefficientDecay(float inDecay) { mLeanSpringIntegrationCoefficientDecay = inDecay; } + float GetLeanSpringIntegrationCoefficientDecay() const { return mLeanSpringIntegrationCoefficientDecay; } + + /// How much to smooth the lean angle (0 = no smoothing, 1 = lean angle never changes) + /// Note that this is frame rate dependent because the formula is: smoothing_factor * previous + (1 - smoothing_factor) * current + void SetLeanSmoothingFactor(float inFactor) { mLeanSmoothingFactor = inFactor; } + float GetLeanSmoothingFactor() const { return mLeanSmoothingFactor; } + +protected: + // See: VehicleController + virtual void PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override; + virtual bool SolveLongitudinalAndLateralConstraints(float inDeltaTime) override; + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; +#ifdef JPH_DEBUG_RENDERER + virtual void Draw(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + + // Configuration properties + bool mEnableLeanController = true; + bool mEnableLeanSteeringLimit = true; + float mMaxLeanAngle; + float mLeanSpringConstant; + float mLeanSpringDamping; + float mLeanSpringIntegrationCoefficient; + float mLeanSpringIntegrationCoefficientDecay; + float mLeanSmoothingFactor; + + // Run-time calculated target lean vector + Vec3 mTargetLean = Vec3::sZero(); + + // Integrated error for the lean spring + float mLeanSpringIntegratedDeltaAngle = 0.0f; + + // Run-time total angular impulse applied to turn the cycle towards the target lean angle + float mAppliedImpulse = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.cpp new file mode 100644 index 000000000000..d3cfcffc54e0 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.cpp @@ -0,0 +1,531 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TrackedVehicleControllerSettings) +{ + JPH_ADD_BASE_CLASS(TrackedVehicleControllerSettings, VehicleControllerSettings) + + JPH_ADD_ATTRIBUTE(TrackedVehicleControllerSettings, mEngine) + JPH_ADD_ATTRIBUTE(TrackedVehicleControllerSettings, mTransmission) + JPH_ADD_ATTRIBUTE(TrackedVehicleControllerSettings, mTracks) +} + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheelSettingsTV) +{ + JPH_ADD_ATTRIBUTE(WheelSettingsTV, mLongitudinalFriction) + JPH_ADD_ATTRIBUTE(WheelSettingsTV, mLateralFriction) +} + +void WheelSettingsTV::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mLongitudinalFriction); + inStream.Write(mLateralFriction); +} + +void WheelSettingsTV::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mLongitudinalFriction); + inStream.Read(mLateralFriction); +} + +WheelTV::WheelTV(const WheelSettingsTV &inSettings) : + Wheel(inSettings) +{ +} + +void WheelTV::CalculateAngularVelocity(const VehicleConstraint &inConstraint) +{ + const WheelSettingsTV *settings = GetSettings(); + const Wheels &wheels = inConstraint.GetWheels(); + const VehicleTrack &track = static_cast(inConstraint.GetController())->GetTracks()[mTrackIndex]; + + // Calculate angular velocity of this wheel + mAngularVelocity = track.mAngularVelocity * wheels[track.mDrivenWheel]->GetSettings()->mRadius / settings->mRadius; +} + +void WheelTV::Update(uint inWheelIndex, float inDeltaTime, const VehicleConstraint &inConstraint) +{ + CalculateAngularVelocity(inConstraint); + + // Update rotation of wheel + mAngle = fmod(mAngle + mAngularVelocity * inDeltaTime, 2.0f * JPH_PI); + + // Reset brake impulse, will be set during post collision again + mBrakeImpulse = 0.0f; + + if (mContactBody != nullptr) + { + // Friction at the point of this wheel between track and floor + const WheelSettingsTV *settings = GetSettings(); + VehicleConstraint::CombineFunction combine_friction = inConstraint.GetCombineFriction(); + mCombinedLongitudinalFriction = settings->mLongitudinalFriction; + mCombinedLateralFriction = settings->mLateralFriction; + combine_friction(inWheelIndex, mCombinedLongitudinalFriction, mCombinedLateralFriction, *mContactBody, mContactSubShapeID); + } + else + { + // No collision + mCombinedLongitudinalFriction = mCombinedLateralFriction = 0.0f; + } +} + +VehicleController *TrackedVehicleControllerSettings::ConstructController(VehicleConstraint &inConstraint) const +{ + return new TrackedVehicleController(*this, inConstraint); +} + +TrackedVehicleControllerSettings::TrackedVehicleControllerSettings() +{ + // Numbers guestimated from: https://en.wikipedia.org/wiki/M1_Abrams + mEngine.mMinRPM = 500.0f; + mEngine.mMaxRPM = 4000.0f; + mEngine.mMaxTorque = 500.0f; // Note actual torque for M1 is around 5000 but we need a reduced mass in order to keep the simulation sane + + mTransmission.mShiftDownRPM = 1000.0f; + mTransmission.mShiftUpRPM = 3500.0f; + mTransmission.mGearRatios = { 4.0f, 3.0f, 2.0f, 1.0f }; + mTransmission.mReverseGearRatios = { -4.0f, -3.0f }; +} + +void TrackedVehicleControllerSettings::SaveBinaryState(StreamOut &inStream) const +{ + mEngine.SaveBinaryState(inStream); + + mTransmission.SaveBinaryState(inStream); + + for (const VehicleTrackSettings &t : mTracks) + t.SaveBinaryState(inStream); +} + +void TrackedVehicleControllerSettings::RestoreBinaryState(StreamIn &inStream) +{ + mEngine.RestoreBinaryState(inStream); + + mTransmission.RestoreBinaryState(inStream); + + for (VehicleTrackSettings &t : mTracks) + t.RestoreBinaryState(inStream); +} + +TrackedVehicleController::TrackedVehicleController(const TrackedVehicleControllerSettings &inSettings, VehicleConstraint &inConstraint) : + VehicleController(inConstraint) +{ + // Copy engine settings + static_cast(mEngine) = inSettings.mEngine; + JPH_ASSERT(inSettings.mEngine.mMinRPM >= 0.0f); + JPH_ASSERT(inSettings.mEngine.mMinRPM <= inSettings.mEngine.mMaxRPM); + mEngine.SetCurrentRPM(mEngine.mMinRPM); + + // Copy transmission settings + static_cast(mTransmission) = inSettings.mTransmission; +#ifdef JPH_ENABLE_ASSERTS + for (float r : inSettings.mTransmission.mGearRatios) + JPH_ASSERT(r > 0.0f); + for (float r : inSettings.mTransmission.mReverseGearRatios) + JPH_ASSERT(r < 0.0f); +#endif // JPH_ENABLE_ASSERTS + JPH_ASSERT(inSettings.mTransmission.mSwitchTime >= 0.0f); + JPH_ASSERT(inSettings.mTransmission.mShiftDownRPM > 0.0f); + JPH_ASSERT(inSettings.mTransmission.mMode != ETransmissionMode::Auto || inSettings.mTransmission.mShiftUpRPM < inSettings.mEngine.mMaxRPM); + JPH_ASSERT(inSettings.mTransmission.mShiftUpRPM > inSettings.mTransmission.mShiftDownRPM); + + // Copy track settings + for (uint i = 0; i < std::size(mTracks); ++i) + { + const VehicleTrackSettings &d = inSettings.mTracks[i]; + static_cast(mTracks[i]) = d; + JPH_ASSERT(d.mInertia >= 0.0f); + JPH_ASSERT(d.mAngularDamping >= 0.0f); + JPH_ASSERT(d.mMaxBrakeTorque >= 0.0f); + JPH_ASSERT(d.mDifferentialRatio > 0.0f); + } +} + +bool TrackedVehicleController::AllowSleep() const +{ + return mForwardInput == 0.0f // No user input + && mTransmission.AllowSleep() // Transmission is not shifting + && mEngine.AllowSleep(); // Engine is idling +} + +void TrackedVehicleController::PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) +{ + Wheels &wheels = mConstraint.GetWheels(); + + // Fill in track index + for (size_t t = 0; t < std::size(mTracks); ++t) + for (uint w : mTracks[t].mWheels) + static_cast(wheels[w])->mTrackIndex = (uint)t; + + // Angular damping: dw/dt = -c * w + // Solution: w(t) = w(0) * e^(-c * t) or w2 = w1 * e^(-c * dt) + // Taylor expansion of e^(-c * dt) = 1 - c * dt + ... + // Since dt is usually in the order of 1/60 and c is a low number too this approximation is good enough + for (VehicleTrack &t : mTracks) + t.mAngularVelocity *= max(0.0f, 1.0f - t.mAngularDamping * inDeltaTime); +} + +void TrackedVehicleController::SyncLeftRightTracks() +{ + // Apply left to right ratio according to track inertias + VehicleTrack &tl = mTracks[(int)ETrackSide::Left]; + VehicleTrack &tr = mTracks[(int)ETrackSide::Right]; + + if (mLeftRatio * mRightRatio > 0.0f) + { + // Solve: (tl.mAngularVelocity + dl) / (tr.mAngularVelocity + dr) = mLeftRatio / mRightRatio and dl * tr.mInertia = -dr * tl.mInertia, where dl/dr are the delta angular velocities for left and right tracks + float impulse = (mLeftRatio * tr.mAngularVelocity - mRightRatio * tl.mAngularVelocity) / (mLeftRatio * tr.mInertia + mRightRatio * tl.mInertia); + tl.mAngularVelocity += impulse * tl.mInertia; + tr.mAngularVelocity -= impulse * tr.mInertia; + } + else + { + // Solve: (tl.mAngularVelocity + dl) / (tr.mAngularVelocity + dr) = mLeftRatio / mRightRatio and dl * tr.mInertia = dr * tl.mInertia, where dl/dr are the delta angular velocities for left and right tracks + float impulse = (mLeftRatio * tr.mAngularVelocity - mRightRatio * tl.mAngularVelocity) / (mRightRatio * tl.mInertia - mLeftRatio * tr.mInertia); + tl.mAngularVelocity += impulse * tl.mInertia; + tr.mAngularVelocity += impulse * tr.mInertia; + } +} + +void TrackedVehicleController::PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) +{ + JPH_PROFILE_FUNCTION(); + + Wheels &wheels = mConstraint.GetWheels(); + + // Update wheel angle, do this before applying torque to the wheels (as friction will slow them down again) + for (uint wheel_index = 0, num_wheels = (uint)wheels.size(); wheel_index < num_wheels; ++wheel_index) + { + WheelTV *w = static_cast(wheels[wheel_index]); + w->Update(wheel_index, inDeltaTime, mConstraint); + } + + // First calculate engine speed based on speed of all wheels + bool can_engine_apply_torque = false; + if (mTransmission.GetCurrentGear() != 0 && mTransmission.GetClutchFriction() > 1.0e-3f) + { + float transmission_ratio = mTransmission.GetCurrentRatio(); + bool forward = transmission_ratio >= 0.0f; + float fastest_wheel_speed = forward? -FLT_MAX : FLT_MAX; + for (const VehicleTrack &t : mTracks) + { + if (forward) + fastest_wheel_speed = max(fastest_wheel_speed, t.mAngularVelocity * t.mDifferentialRatio); + else + fastest_wheel_speed = min(fastest_wheel_speed, t.mAngularVelocity * t.mDifferentialRatio); + for (uint w : t.mWheels) + if (wheels[w]->HasContact()) + { + can_engine_apply_torque = true; + break; + } + } + + // Update RPM only if the tracks are connected to the engine + if (fastest_wheel_speed > -FLT_MAX && fastest_wheel_speed < FLT_MAX) + mEngine.SetCurrentRPM(fastest_wheel_speed * mTransmission.GetCurrentRatio() * VehicleEngine::cAngularVelocityToRPM); + } + else + { + // Update engine with damping + mEngine.ApplyDamping(inDeltaTime); + + // In auto transmission mode, don't accelerate the engine when switching gears + float forward_input = mTransmission.mMode == ETransmissionMode::Manual? abs(mForwardInput) : 0.0f; + + // Engine not connected to wheels, update RPM based on engine inertia alone + mEngine.ApplyTorque(mEngine.GetTorque(forward_input), inDeltaTime); + } + + // Update transmission + // Note: only allow switching gears up when the tracks are rolling in the same direction + mTransmission.Update(inDeltaTime, mEngine.GetCurrentRPM(), mForwardInput, mLeftRatio * mRightRatio > 0.0f && can_engine_apply_torque); + + // Calculate the amount of torque the transmission gives to the differentials + float transmission_ratio = mTransmission.GetCurrentRatio(); + float transmission_torque = mTransmission.GetClutchFriction() * transmission_ratio * mEngine.GetTorque(abs(mForwardInput)); + if (transmission_torque != 0.0f) + { + // Apply the transmission torque to the wheels + for (uint i = 0; i < std::size(mTracks); ++i) + { + VehicleTrack &t = mTracks[i]; + + // Get wheel rotation ratio for this track + float ratio = i == 0? mLeftRatio : mRightRatio; + + // Calculate the max angular velocity of the driven wheel of the track given current engine RPM + // Note this adds 0.1% slop to avoid numerical accuracy issues + float track_max_angular_velocity = mEngine.GetCurrentRPM() / (transmission_ratio * t.mDifferentialRatio * ratio * VehicleEngine::cAngularVelocityToRPM) * 1.001f; + + // Calculate torque on the driven wheel + float differential_torque = t.mDifferentialRatio * ratio * transmission_torque; + + // Apply torque to driven wheel + if (t.mAngularVelocity * track_max_angular_velocity < 0.0f || abs(t.mAngularVelocity) < abs(track_max_angular_velocity)) + t.mAngularVelocity += differential_torque * inDeltaTime / t.mInertia; + } + } + + // Ensure that we have the correct ratio between the two tracks + SyncLeftRightTracks(); + + // Braking + for (VehicleTrack &t : mTracks) + { + // Calculate brake torque + float brake_torque = mBrakeInput * t.mMaxBrakeTorque; + if (brake_torque > 0.0f) + { + // Calculate how much torque is needed to stop the track from rotating in this time step + float brake_torque_to_lock_track = abs(t.mAngularVelocity) * t.mInertia / inDeltaTime; + if (brake_torque > brake_torque_to_lock_track) + { + // Wheels are locked + t.mAngularVelocity = 0.0f; + brake_torque -= brake_torque_to_lock_track; + } + else + { + // Slow down the track + t.mAngularVelocity -= Sign(t.mAngularVelocity) * brake_torque * inDeltaTime / t.mInertia; + } + } + + if (brake_torque > 0.0f) + { + // Sum the radius of all wheels touching the floor + float total_radius = 0.0f; + for (uint wheel_index : t.mWheels) + { + const WheelTV *w = static_cast(wheels[wheel_index]); + + if (w->HasContact()) + total_radius += w->GetSettings()->mRadius; + } + + if (total_radius > 0.0f) + { + brake_torque /= total_radius; + for (uint wheel_index : t.mWheels) + { + WheelTV *w = static_cast(wheels[wheel_index]); + if (w->HasContact()) + { + // Impulse: p = F * dt = Torque / Wheel_Radius * dt, Torque = Total_Torque * Wheel_Radius / Summed_Radius => p = Total_Torque * dt / Summed_Radius + w->mBrakeImpulse = brake_torque * inDeltaTime; + } + } + } + } + } + + // Update wheel angular velocity based on that of the track + for (Wheel *w_base : wheels) + { + WheelTV *w = static_cast(w_base); + w->CalculateAngularVelocity(mConstraint); + } +} + +bool TrackedVehicleController::SolveLongitudinalAndLateralConstraints(float inDeltaTime) +{ + bool impulse = false; + + for (Wheel *w_base : mConstraint.GetWheels()) + if (w_base->HasContact()) + { + WheelTV *w = static_cast(w_base); + const WheelSettingsTV *settings = w->GetSettings(); + VehicleTrack &track = mTracks[w->mTrackIndex]; + + // Calculate max impulse that we can apply on the ground + float max_longitudinal_friction_impulse = w->mCombinedLongitudinalFriction * w->GetSuspensionLambda(); + + // Calculate relative velocity between wheel contact point and floor in longitudinal direction + Vec3 relative_velocity = mConstraint.GetVehicleBody()->GetPointVelocity(w->GetContactPosition()) - w->GetContactPointVelocity(); + float relative_longitudinal_velocity = relative_velocity.Dot(w->GetContactLongitudinal()); + + // Calculate brake force to apply + float min_longitudinal_impulse, max_longitudinal_impulse; + if (w->mBrakeImpulse != 0.0f) + { + // Limit brake force by max tire friction + float brake_impulse = min(w->mBrakeImpulse, max_longitudinal_friction_impulse); + + // Check which direction the brakes should be applied (we don't want to apply an impulse that would accelerate the vehicle) + if (relative_longitudinal_velocity >= 0.0f) + { + min_longitudinal_impulse = -brake_impulse; + max_longitudinal_impulse = 0.0f; + } + else + { + min_longitudinal_impulse = 0.0f; + max_longitudinal_impulse = brake_impulse; + } + + // Longitudinal impulse, note that we assume that once the wheels are locked that the brakes have more than enough torque to keep the wheels locked so we exclude any rotation deltas + impulse |= w->SolveLongitudinalConstraintPart(mConstraint, min_longitudinal_impulse, max_longitudinal_impulse); + } + else + { + // Assume we want to apply an angular impulse that makes the delta velocity between track and ground zero in one time step, calculate the amount of linear impulse needed to do that + float desired_angular_velocity = relative_longitudinal_velocity / settings->mRadius; + float linear_impulse = (track.mAngularVelocity - desired_angular_velocity) * track.mInertia / settings->mRadius; + + // Limit the impulse by max track friction + float prev_lambda = w->GetLongitudinalLambda(); + min_longitudinal_impulse = max_longitudinal_impulse = Clamp(prev_lambda + linear_impulse, -max_longitudinal_friction_impulse, max_longitudinal_friction_impulse); + + // Longitudinal impulse + impulse |= w->SolveLongitudinalConstraintPart(mConstraint, min_longitudinal_impulse, max_longitudinal_impulse); + + // Update the angular velocity of the track according to the lambda that was applied + track.mAngularVelocity -= (w->GetLongitudinalLambda() - prev_lambda) * settings->mRadius / track.mInertia; + SyncLeftRightTracks(); + } + } + + for (Wheel *w_base : mConstraint.GetWheels()) + if (w_base->HasContact()) + { + WheelTV *w = static_cast(w_base); + + // Update angular velocity of wheel for the next iteration + w->CalculateAngularVelocity(mConstraint); + + // Lateral friction + float max_lateral_friction_impulse = w->mCombinedLateralFriction * w->GetSuspensionLambda(); + impulse |= w->SolveLateralConstraintPart(mConstraint, -max_lateral_friction_impulse, max_lateral_friction_impulse); + } + + return impulse; +} + +#ifdef JPH_DEBUG_RENDERER + +void TrackedVehicleController::Draw(DebugRenderer *inRenderer) const +{ + float constraint_size = mConstraint.GetDrawConstraintSize(); + + // Draw RPM + Body *body = mConstraint.GetVehicleBody(); + Vec3 rpm_meter_up = body->GetRotation() * mConstraint.GetLocalUp(); + RVec3 rpm_meter_pos = body->GetPosition() + body->GetRotation() * mRPMMeterPosition; + Vec3 rpm_meter_fwd = body->GetRotation() * mConstraint.GetLocalForward(); + mEngine.DrawRPM(inRenderer, rpm_meter_pos, rpm_meter_fwd, rpm_meter_up, mRPMMeterSize, mTransmission.mShiftDownRPM, mTransmission.mShiftUpRPM); + + // Draw current vehicle state + String status = StringFormat("Forward: %.1f, LRatio: %.1f, RRatio: %.1f, Brake: %.1f\n" + "Gear: %d, Clutch: %.1f, EngineRPM: %.0f, V: %.1f km/h", + (double)mForwardInput, (double)mLeftRatio, (double)mRightRatio, (double)mBrakeInput, + mTransmission.GetCurrentGear(), (double)mTransmission.GetClutchFriction(), (double)mEngine.GetCurrentRPM(), (double)body->GetLinearVelocity().Length() * 3.6); + inRenderer->DrawText3D(body->GetPosition(), status, Color::sWhite, constraint_size); + + for (const VehicleTrack &t : mTracks) + { + const WheelTV *w = static_cast(mConstraint.GetWheels()[t.mDrivenWheel]); + const WheelSettings *settings = w->GetSettings(); + + // Calculate where the suspension attaches to the body in world space + RVec3 ws_position = body->GetCenterOfMassPosition() + body->GetRotation() * (settings->mPosition - body->GetShape()->GetCenterOfMass()); + + DebugRenderer::sInstance->DrawText3D(ws_position, StringFormat("W: %.1f", (double)t.mAngularVelocity), Color::sWhite, constraint_size); + } + + RMat44 body_transform = body->GetWorldTransform(); + + for (const Wheel *w_base : mConstraint.GetWheels()) + { + const WheelTV *w = static_cast(w_base); + const WheelSettings *settings = w->GetSettings(); + + // Calculate where the suspension attaches to the body in world space + RVec3 ws_position = body_transform * settings->mPosition; + Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection); + + // Draw suspension + RVec3 min_suspension_pos = ws_position + ws_direction * settings->mSuspensionMinLength; + RVec3 max_suspension_pos = ws_position + ws_direction * settings->mSuspensionMaxLength; + inRenderer->DrawLine(ws_position, min_suspension_pos, Color::sRed); + inRenderer->DrawLine(min_suspension_pos, max_suspension_pos, Color::sGreen); + + // Draw current length + RVec3 wheel_pos = ws_position + ws_direction * w->GetSuspensionLength(); + inRenderer->DrawMarker(wheel_pos, w->GetSuspensionLength() < settings->mSuspensionMinLength? Color::sRed : Color::sGreen, constraint_size); + + // Draw wheel basis + Vec3 wheel_forward, wheel_up, wheel_right; + mConstraint.GetWheelLocalBasis(w, wheel_forward, wheel_up, wheel_right); + wheel_forward = body_transform.Multiply3x3(wheel_forward); + wheel_up = body_transform.Multiply3x3(wheel_up); + wheel_right = body_transform.Multiply3x3(wheel_right); + Vec3 steering_axis = body_transform.Multiply3x3(settings->mSteeringAxis); + inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_forward, Color::sRed); + inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_up, Color::sGreen); + inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_right, Color::sBlue); + inRenderer->DrawLine(wheel_pos, wheel_pos + steering_axis, Color::sYellow); + + // Draw wheel + RMat44 wheel_transform(Vec4(wheel_up, 0.0f), Vec4(wheel_right, 0.0f), Vec4(wheel_forward, 0.0f), wheel_pos); + wheel_transform.SetRotation(wheel_transform.GetRotation() * Mat44::sRotationY(-w->GetRotationAngle())); + inRenderer->DrawCylinder(wheel_transform, settings->mWidth * 0.5f, settings->mRadius, w->GetSuspensionLength() <= settings->mSuspensionMinLength? Color::sRed : Color::sGreen, DebugRenderer::ECastShadow::Off, DebugRenderer::EDrawMode::Wireframe); + + if (w->HasContact()) + { + // Draw contact + inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactNormal(), Color::sYellow); + inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLongitudinal(), Color::sRed); + inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLateral(), Color::sBlue); + + DebugRenderer::sInstance->DrawText3D(w->GetContactPosition(), StringFormat("S: %.2f", (double)w->GetSuspensionLength()), Color::sWhite, constraint_size); + } + } +} + +#endif // JPH_DEBUG_RENDERER + +void TrackedVehicleController::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mForwardInput); + inStream.Write(mLeftRatio); + inStream.Write(mRightRatio); + inStream.Write(mBrakeInput); + + mEngine.SaveState(inStream); + mTransmission.SaveState(inStream); + + for (const VehicleTrack &t : mTracks) + t.SaveState(inStream); +} + +void TrackedVehicleController::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mForwardInput); + inStream.Read(mLeftRatio); + inStream.Read(mRightRatio); + inStream.Read(mBrakeInput); + + mEngine.RestoreState(inStream); + mTransmission.RestoreState(inStream); + + for (VehicleTrack &t : mTracks) + t.RestoreState(inStream); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.h new file mode 100644 index 000000000000..8567c97073db --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.h @@ -0,0 +1,166 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; + +/// WheelSettings object specifically for TrackedVehicleController +class JPH_EXPORT WheelSettingsTV : public WheelSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, WheelSettingsTV) + +public: + // See: WheelSettings + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void RestoreBinaryState(StreamIn &inStream) override; + + float mLongitudinalFriction = 4.0f; ///< Friction in forward direction of tire + float mLateralFriction = 2.0f; ///< Friction in sideway direction of tire +}; + +/// Wheel object specifically for TrackedVehicleController +class JPH_EXPORT WheelTV : public Wheel +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit WheelTV(const WheelSettingsTV &inWheel); + + /// Override GetSettings and cast to the correct class + const WheelSettingsTV * GetSettings() const { return StaticCast(mSettings); } + + /// Update the angular velocity of the wheel based on the angular velocity of the track + void CalculateAngularVelocity(const VehicleConstraint &inConstraint); + + /// Update the wheel rotation based on the current angular velocity + void Update(uint inWheelIndex, float inDeltaTime, const VehicleConstraint &inConstraint); + + int mTrackIndex = -1; ///< Index in mTracks to which this wheel is attached (calculated on initialization) + float mCombinedLongitudinalFriction = 0.0f; ///< Combined friction coefficient in longitudinal direction (combines terrain and track) + float mCombinedLateralFriction = 0.0f; ///< Combined friction coefficient in lateral direction (combines terrain and track) + float mBrakeImpulse = 0.0f; ///< Amount of impulse that the brakes can apply to the floor (excluding friction), spread out from brake impulse applied on track +}; + +/// Settings of a vehicle with tank tracks +/// +/// Default settings are based around what I could find about the M1 Abrams tank. +/// Note to avoid issues with very heavy objects vs very light objects the mass of the tank should be a lot lower (say 10x) than that of a real tank. That means that the engine/brake torque is also 10x less. +class JPH_EXPORT TrackedVehicleControllerSettings : public VehicleControllerSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, TrackedVehicleControllerSettings) + +public: + // Constructor + TrackedVehicleControllerSettings(); + + // See: VehicleControllerSettings + virtual VehicleController * ConstructController(VehicleConstraint &inConstraint) const override; + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void RestoreBinaryState(StreamIn &inStream) override; + + VehicleEngineSettings mEngine; ///< The properties of the engine + VehicleTransmissionSettings mTransmission; ///< The properties of the transmission (aka gear box) + VehicleTrackSettings mTracks[(int)ETrackSide::Num]; ///< List of tracks and their properties +}; + +/// Runtime controller class for vehicle with tank tracks +class JPH_EXPORT TrackedVehicleController : public VehicleController +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + TrackedVehicleController(const TrackedVehicleControllerSettings &inSettings, VehicleConstraint &inConstraint); + + /// Set input from driver + /// @param inForward Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed + /// @param inLeftRatio Value between -1 and 1 indicating an extra multiplier to the rotation rate of the left track (used for steering) + /// @param inRightRatio Value between -1 and 1 indicating an extra multiplier to the rotation rate of the right track (used for steering) + /// @param inBrake Value between 0 and 1 indicating how strong the brake pedal is pressed + void SetDriverInput(float inForward, float inLeftRatio, float inRightRatio, float inBrake) { JPH_ASSERT(inLeftRatio != 0.0f && inRightRatio != 0.0f); mForwardInput = inForward; mLeftRatio = inLeftRatio; mRightRatio = inRightRatio; mBrakeInput = inBrake; } + + /// Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed + void SetForwardInput(float inForward) { mForwardInput = inForward; } + float GetForwardInput() const { return mForwardInput; } + + /// Value between -1 and 1 indicating an extra multiplier to the rotation rate of the left track (used for steering) + void SetLeftRatio(float inLeftRatio) { JPH_ASSERT(inLeftRatio != 0.0f); mLeftRatio = inLeftRatio; } + float GetLeftRatio() const { return mLeftRatio; } + + /// Value between -1 and 1 indicating an extra multiplier to the rotation rate of the right track (used for steering) + void SetRightRatio(float inRightRatio) { JPH_ASSERT(inRightRatio != 0.0f); mRightRatio = inRightRatio; } + float GetRightRatio() const { return mRightRatio; } + + /// Value between 0 and 1 indicating how strong the brake pedal is pressed + void SetBrakeInput(float inBrake) { mBrakeInput = inBrake; } + float GetBrakeInput() const { return mBrakeInput; } + + /// Get current engine state + const VehicleEngine & GetEngine() const { return mEngine; } + + /// Get current engine state (writable interface, allows you to make changes to the configuration which will take effect the next time step) + VehicleEngine & GetEngine() { return mEngine; } + + /// Get current transmission state + const VehicleTransmission & GetTransmission() const { return mTransmission; } + + /// Get current transmission state (writable interface, allows you to make changes to the configuration which will take effect the next time step) + VehicleTransmission & GetTransmission() { return mTransmission; } + + /// Get the tracks this vehicle has + const VehicleTracks & GetTracks() const { return mTracks; } + + /// Get the tracks this vehicle has (writable interface, allows you to make changes to the configuration which will take effect the next time step) + VehicleTracks & GetTracks() { return mTracks; } + +#ifdef JPH_DEBUG_RENDERER + /// Debug drawing of RPM meter + void SetRPMMeter(Vec3Arg inPosition, float inSize) { mRPMMeterPosition = inPosition; mRPMMeterSize = inSize; } +#endif // JPH_DEBUG_RENDERER + +protected: + /// Synchronize angular velocities of left and right tracks according to their ratios + void SyncLeftRightTracks(); + + // See: VehicleController + virtual Wheel * ConstructWheel(const WheelSettings &inWheel) const override { JPH_ASSERT(IsKindOf(&inWheel, JPH_RTTI(WheelSettingsTV))); return new WheelTV(static_cast(inWheel)); } + virtual bool AllowSleep() const override; + virtual void PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override; + virtual void PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override; + virtual bool SolveLongitudinalAndLateralConstraints(float inDeltaTime) override; + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; +#ifdef JPH_DEBUG_RENDERER + virtual void Draw(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + + // Control information + float mForwardInput = 0.0f; ///< Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed + float mLeftRatio = 1.0f; ///< Value between -1 and 1 indicating an extra multiplier to the rotation rate of the left track (used for steering) + float mRightRatio = 1.0f; ///< Value between -1 and 1 indicating an extra multiplier to the rotation rate of the right track (used for steering) + float mBrakeInput = 0.0f; ///< Value between 0 and 1 indicating how strong the brake pedal is pressed + + // Simulation information + VehicleEngine mEngine; ///< Engine state of the vehicle + VehicleTransmission mTransmission; ///< Transmission state of the vehicle + VehicleTracks mTracks; ///< Tracks of the vehicle + +#ifdef JPH_DEBUG_RENDERER + // Debug settings + Vec3 mRPMMeterPosition { 0, 1, 0 }; ///< Position (in local space of the body) of the RPM meter when drawing the constraint + float mRPMMeterSize = 0.5f; ///< Size of the RPM meter when drawing the constraint +#endif // JPH_DEBUG_RENDERER +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleAntiRollBar.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleAntiRollBar.cpp new file mode 100644 index 000000000000..859bcafa175f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleAntiRollBar.cpp @@ -0,0 +1,33 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleAntiRollBar) +{ + JPH_ADD_ATTRIBUTE(VehicleAntiRollBar, mLeftWheel) + JPH_ADD_ATTRIBUTE(VehicleAntiRollBar, mRightWheel) + JPH_ADD_ATTRIBUTE(VehicleAntiRollBar, mStiffness) +} + +void VehicleAntiRollBar::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mLeftWheel); + inStream.Write(mRightWheel); + inStream.Write(mStiffness); +} + +void VehicleAntiRollBar::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mLeftWheel); + inStream.Read(mRightWheel); + inStream.Read(mStiffness); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleAntiRollBar.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleAntiRollBar.h new file mode 100644 index 000000000000..4a9c6707d4f2 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleAntiRollBar.h @@ -0,0 +1,31 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// An anti rollbar is a stiff spring that connects two wheels to reduce the amount of roll the vehicle makes in sharp corners +/// See: https://en.wikipedia.org/wiki/Anti-roll_bar +class JPH_EXPORT VehicleAntiRollBar +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleAntiRollBar) + +public: + /// Saves the contents in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restores the contents in binary form to inStream. + void RestoreBinaryState(StreamIn &inStream); + + int mLeftWheel = 0; ///< Index (in mWheels) that represents the left wheel of this anti-rollbar + int mRightWheel = 1; ///< Index (in mWheels) that represents the right wheel of this anti-rollbar + float mStiffness = 1000.0f; ///< Stiffness (spring constant in N/m) of anti rollbar, can be 0 to disable the anti-rollbar +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.cpp new file mode 100644 index 000000000000..56246835794f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.cpp @@ -0,0 +1,376 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +bool VehicleCollisionTesterRay::Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const +{ + const DefaultBroadPhaseLayerFilter default_broadphase_layer_filter = inPhysicsSystem.GetDefaultBroadPhaseLayerFilter(mObjectLayer); + const BroadPhaseLayerFilter &broadphase_layer_filter = mBroadPhaseLayerFilter != nullptr? *mBroadPhaseLayerFilter : default_broadphase_layer_filter; + + const DefaultObjectLayerFilter default_object_layer_filter = inPhysicsSystem.GetDefaultLayerFilter(mObjectLayer); + const ObjectLayerFilter &object_layer_filter = mObjectLayerFilter != nullptr? *mObjectLayerFilter : default_object_layer_filter; + + const IgnoreSingleBodyFilter default_body_filter(inVehicleBodyID); + const BodyFilter &body_filter = mBodyFilter != nullptr? *mBodyFilter : default_body_filter; + + const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings(); + float wheel_radius = wheel_settings->mRadius; + float ray_length = wheel_settings->mSuspensionMaxLength + wheel_radius; + RRayCast ray { inOrigin, ray_length * inDirection }; + + class MyCollector : public CastRayCollector + { + public: + MyCollector(PhysicsSystem &inPhysicsSystem, const RRayCast &inRay, Vec3Arg inUpDirection, float inCosMaxSlopeAngle) : + mPhysicsSystem(inPhysicsSystem), + mRay(inRay), + mUpDirection(inUpDirection), + mCosMaxSlopeAngle(inCosMaxSlopeAngle) + { + } + + virtual void AddHit(const RayCastResult &inResult) override + { + // Test if this collision is closer than the previous one + if (inResult.mFraction < GetEarlyOutFraction()) + { + // Lock the body + BodyLockRead lock(mPhysicsSystem.GetBodyLockInterfaceNoLock(), inResult.mBodyID); + JPH_ASSERT(lock.Succeeded()); // When this runs all bodies are locked so this should not fail + const Body *body = &lock.GetBody(); + + if (body->IsSensor()) + return; + + // Test that we're not hitting a vertical wall + RVec3 contact_pos = mRay.GetPointOnRay(inResult.mFraction); + Vec3 normal = body->GetWorldSpaceSurfaceNormal(inResult.mSubShapeID2, contact_pos); + if (normal.Dot(mUpDirection) > mCosMaxSlopeAngle) + { + // Update early out fraction to this hit + UpdateEarlyOutFraction(inResult.mFraction); + + // Get the contact properties + mBody = body; + mSubShapeID2 = inResult.mSubShapeID2; + mContactPosition = contact_pos; + mContactNormal = normal; + } + } + } + + // Configuration + PhysicsSystem & mPhysicsSystem; + RRayCast mRay; + Vec3 mUpDirection; + float mCosMaxSlopeAngle; + + // Resulting closest collision + const Body * mBody = nullptr; + SubShapeID mSubShapeID2; + RVec3 mContactPosition; + Vec3 mContactNormal; + }; + + RayCastSettings settings; + + MyCollector collector(inPhysicsSystem, ray, mUp, mCosMaxSlopeAngle); + inPhysicsSystem.GetNarrowPhaseQueryNoLock().CastRay(ray, settings, collector, broadphase_layer_filter, object_layer_filter, body_filter); + if (collector.mBody == nullptr) + return false; + + outBody = const_cast(collector.mBody); + outSubShapeID = collector.mSubShapeID2; + outContactPosition = collector.mContactPosition; + outContactNormal = collector.mContactNormal; + outSuspensionLength = max(0.0f, ray_length * collector.GetEarlyOutFraction() - wheel_radius); + + return true; +} + +void VehicleCollisionTesterRay::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const +{ + // Recalculate the contact points assuming the contact point is on an infinite plane + const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings(); + float d_dot_n = inDirection.Dot(ioContactNormal); + if (d_dot_n < -1.0e-6f) + { + // Reproject the contact position using the suspension ray and the plane formed by the contact position and normal + ioContactPosition = inOrigin + Vec3(ioContactPosition - inOrigin).Dot(ioContactNormal) / d_dot_n * inDirection; + + // The suspension length is simply the distance between the contact position and the suspension origin excluding the wheel radius + ioSuspensionLength = Clamp(Vec3(ioContactPosition - inOrigin).Dot(inDirection) - wheel_settings->mRadius, 0.0f, wheel_settings->mSuspensionMaxLength); + } + else + { + // If the normal is pointing away we assume there's no collision anymore + ioSuspensionLength = wheel_settings->mSuspensionMaxLength; + } +} + +bool VehicleCollisionTesterCastSphere::Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const +{ + const DefaultBroadPhaseLayerFilter default_broadphase_layer_filter = inPhysicsSystem.GetDefaultBroadPhaseLayerFilter(mObjectLayer); + const BroadPhaseLayerFilter &broadphase_layer_filter = mBroadPhaseLayerFilter != nullptr? *mBroadPhaseLayerFilter : default_broadphase_layer_filter; + + const DefaultObjectLayerFilter default_object_layer_filter = inPhysicsSystem.GetDefaultLayerFilter(mObjectLayer); + const ObjectLayerFilter &object_layer_filter = mObjectLayerFilter != nullptr? *mObjectLayerFilter : default_object_layer_filter; + + const IgnoreSingleBodyFilter default_body_filter(inVehicleBodyID); + const BodyFilter &body_filter = mBodyFilter != nullptr? *mBodyFilter : default_body_filter; + + SphereShape sphere(mRadius); + sphere.SetEmbedded(); + + const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings(); + float wheel_radius = wheel_settings->mRadius; + float shape_cast_length = wheel_settings->mSuspensionMaxLength + wheel_radius - mRadius; + RShapeCast shape_cast(&sphere, Vec3::sReplicate(1.0f), RMat44::sTranslation(inOrigin), inDirection * shape_cast_length); + + ShapeCastSettings settings; + settings.mUseShrunkenShapeAndConvexRadius = true; + settings.mReturnDeepestPoint = true; + + class MyCollector : public CastShapeCollector + { + public: + MyCollector(PhysicsSystem &inPhysicsSystem, const RShapeCast &inShapeCast, Vec3Arg inUpDirection, float inCosMaxSlopeAngle) : + mPhysicsSystem(inPhysicsSystem), + mShapeCast(inShapeCast), + mUpDirection(inUpDirection), + mCosMaxSlopeAngle(inCosMaxSlopeAngle) + { + } + + virtual void AddHit(const ShapeCastResult &inResult) override + { + // Test if this collision is closer/deeper than the previous one + float early_out = inResult.GetEarlyOutFraction(); + if (early_out < GetEarlyOutFraction()) + { + // Lock the body + BodyLockRead lock(mPhysicsSystem.GetBodyLockInterfaceNoLock(), inResult.mBodyID2); + JPH_ASSERT(lock.Succeeded()); // When this runs all bodies are locked so this should not fail + const Body *body = &lock.GetBody(); + + if (body->IsSensor()) + return; + + // Test that we're not hitting a vertical wall + Vec3 normal = -inResult.mPenetrationAxis.Normalized(); + if (normal.Dot(mUpDirection) > mCosMaxSlopeAngle) + { + // Update early out fraction to this hit + UpdateEarlyOutFraction(early_out); + + // Get the contact properties + mBody = body; + mSubShapeID2 = inResult.mSubShapeID2; + mContactPosition = mShapeCast.mCenterOfMassStart.GetTranslation() + inResult.mContactPointOn2; + mContactNormal = normal; + mFraction = inResult.mFraction; + } + } + } + + // Configuration + PhysicsSystem & mPhysicsSystem; + const RShapeCast & mShapeCast; + Vec3 mUpDirection; + float mCosMaxSlopeAngle; + + // Resulting closest collision + const Body * mBody = nullptr; + SubShapeID mSubShapeID2; + RVec3 mContactPosition; + Vec3 mContactNormal; + float mFraction; + }; + + MyCollector collector(inPhysicsSystem, shape_cast, mUp, mCosMaxSlopeAngle); + inPhysicsSystem.GetNarrowPhaseQueryNoLock().CastShape(shape_cast, settings, shape_cast.mCenterOfMassStart.GetTranslation(), collector, broadphase_layer_filter, object_layer_filter, body_filter); + if (collector.mBody == nullptr) + return false; + + outBody = const_cast(collector.mBody); + outSubShapeID = collector.mSubShapeID2; + outContactPosition = collector.mContactPosition; + outContactNormal = collector.mContactNormal; + outSuspensionLength = max(0.0f, shape_cast_length * collector.mFraction + mRadius - wheel_radius); + + return true; +} + +void VehicleCollisionTesterCastSphere::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const +{ + // Recalculate the contact points assuming the contact point is on an infinite plane + const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings(); + float d_dot_n = inDirection.Dot(ioContactNormal); + if (d_dot_n < -1.0e-6f) + { + // Reproject the contact position using the suspension cast sphere and the plane formed by the contact position and normal + // This solves x = inOrigin + fraction * inDirection and (x - ioContactPosition) . ioContactNormal = mRadius for fraction + float oc_dot_n = Vec3(ioContactPosition - inOrigin).Dot(ioContactNormal); + float fraction = (mRadius + oc_dot_n) / d_dot_n; + ioContactPosition = inOrigin + fraction * inDirection - mRadius * ioContactNormal; + + // Calculate the new suspension length in the same way as the cast sphere normally does + ioSuspensionLength = Clamp(fraction + mRadius - wheel_settings->mRadius, 0.0f, wheel_settings->mSuspensionMaxLength); + } + else + { + // If the normal is pointing away we assume there's no collision anymore + ioSuspensionLength = wheel_settings->mSuspensionMaxLength; + } +} + +bool VehicleCollisionTesterCastCylinder::Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const +{ + const DefaultBroadPhaseLayerFilter default_broadphase_layer_filter = inPhysicsSystem.GetDefaultBroadPhaseLayerFilter(mObjectLayer); + const BroadPhaseLayerFilter &broadphase_layer_filter = mBroadPhaseLayerFilter != nullptr? *mBroadPhaseLayerFilter : default_broadphase_layer_filter; + + const DefaultObjectLayerFilter default_object_layer_filter = inPhysicsSystem.GetDefaultLayerFilter(mObjectLayer); + const ObjectLayerFilter &object_layer_filter = mObjectLayerFilter != nullptr? *mObjectLayerFilter : default_object_layer_filter; + + const IgnoreSingleBodyFilter default_body_filter(inVehicleBodyID); + const BodyFilter &body_filter = mBodyFilter != nullptr? *mBodyFilter : default_body_filter; + + const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings(); + float max_suspension_length = wheel_settings->mSuspensionMaxLength; + + // Get the wheel transform given that the cylinder rotates around the Y axis + RMat44 shape_cast_start = inVehicleConstraint.GetWheelWorldTransform(inWheelIndex, Vec3::sAxisY(), Vec3::sAxisX()); + shape_cast_start.SetTranslation(inOrigin); + + // Construct a cylinder with the dimensions of the wheel + float wheel_half_width = 0.5f * wheel_settings->mWidth; + CylinderShape cylinder(wheel_half_width, wheel_settings->mRadius, min(wheel_half_width, wheel_settings->mRadius) * mConvexRadiusFraction); + cylinder.SetEmbedded(); + + RShapeCast shape_cast(&cylinder, Vec3::sReplicate(1.0f), shape_cast_start, inDirection * max_suspension_length); + + ShapeCastSettings settings; + settings.mUseShrunkenShapeAndConvexRadius = true; + settings.mReturnDeepestPoint = true; + + class MyCollector : public CastShapeCollector + { + public: + MyCollector(PhysicsSystem &inPhysicsSystem, const RShapeCast &inShapeCast) : + mPhysicsSystem(inPhysicsSystem), + mShapeCast(inShapeCast) + { + } + + virtual void AddHit(const ShapeCastResult &inResult) override + { + // Test if this collision is closer/deeper than the previous one + float early_out = inResult.GetEarlyOutFraction(); + if (early_out < GetEarlyOutFraction()) + { + // Lock the body + BodyLockRead lock(mPhysicsSystem.GetBodyLockInterfaceNoLock(), inResult.mBodyID2); + JPH_ASSERT(lock.Succeeded()); // When this runs all bodies are locked so this should not fail + const Body *body = &lock.GetBody(); + + if (body->IsSensor()) + return; + + // Update early out fraction to this hit + UpdateEarlyOutFraction(early_out); + + // Get the contact properties + mBody = body; + mSubShapeID2 = inResult.mSubShapeID2; + mContactPosition = mShapeCast.mCenterOfMassStart.GetTranslation() + inResult.mContactPointOn2; + mContactNormal = -inResult.mPenetrationAxis.Normalized(); + mFraction = inResult.mFraction; + } + } + + // Configuration + PhysicsSystem & mPhysicsSystem; + const RShapeCast & mShapeCast; + + // Resulting closest collision + const Body * mBody = nullptr; + SubShapeID mSubShapeID2; + RVec3 mContactPosition; + Vec3 mContactNormal; + float mFraction; + }; + + MyCollector collector(inPhysicsSystem, shape_cast); + inPhysicsSystem.GetNarrowPhaseQueryNoLock().CastShape(shape_cast, settings, shape_cast.mCenterOfMassStart.GetTranslation(), collector, broadphase_layer_filter, object_layer_filter, body_filter); + if (collector.mBody == nullptr) + return false; + + outBody = const_cast(collector.mBody); + outSubShapeID = collector.mSubShapeID2; + outContactPosition = collector.mContactPosition; + outContactNormal = collector.mContactNormal; + outSuspensionLength = max_suspension_length * collector.mFraction; + + return true; +} + +void VehicleCollisionTesterCastCylinder::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const +{ + // Recalculate the contact points assuming the contact point is on an infinite plane + const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings(); + float d_dot_n = inDirection.Dot(ioContactNormal); + if (d_dot_n < -1.0e-6f) + { + // Wheel size + float half_width = 0.5f * wheel_settings->mWidth; + float radius = wheel_settings->mRadius; + + // Get the inverse local space contact normal for a cylinder pointing along Y + RMat44 wheel_transform = inVehicleConstraint.GetWheelWorldTransform(inWheelIndex, Vec3::sAxisY(), Vec3::sAxisX()); + Vec3 inverse_local_normal = -wheel_transform.Multiply3x3Transposed(ioContactNormal); + + // Get the support point of this normal in local space of the cylinder + // See CylinderShape::Cylinder::GetSupport + float x = inverse_local_normal.GetX(), y = inverse_local_normal.GetY(), z = inverse_local_normal.GetZ(); + float o = sqrt(Square(x) + Square(z)); + Vec3 support_point; + if (o > 0.0f) + support_point = Vec3((radius * x) / o, Sign(y) * half_width, (radius * z) / o); + else + support_point = Vec3(0, Sign(y) * half_width, 0); + + // Rotate back to world space + support_point = wheel_transform.Multiply3x3(support_point); + + // Now we can use inOrigin + support_point as the start of a ray of our suspension to the contact plane + // as know that it is the first point on the wheel that will hit the plane + RVec3 origin = inOrigin + support_point; + + // Calculate contact position and suspension length, the is the same as VehicleCollisionTesterRay + // but we don't need to take the radius into account anymore + Vec3 oc(ioContactPosition - origin); + ioContactPosition = origin + oc.Dot(ioContactNormal) / d_dot_n * inDirection; + ioSuspensionLength = Clamp(oc.Dot(inDirection), 0.0f, wheel_settings->mSuspensionMaxLength); + } + else + { + // If the normal is pointing away we assume there's no collision anymore + ioSuspensionLength = wheel_settings->mSuspensionMaxLength; + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.h new file mode 100644 index 000000000000..7c222756c8b6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.h @@ -0,0 +1,146 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; +class VehicleConstraint; +class BroadPhaseLayerFilter; +class ObjectLayerFilter; +class BodyFilter; + +/// Class that does collision detection between wheels and ground +class JPH_EXPORT VehicleCollisionTester : public RefTarget, public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructors + VehicleCollisionTester() = default; + explicit VehicleCollisionTester(ObjectLayer inObjectLayer) : mObjectLayer(inObjectLayer) { } + + /// Virtual destructor + virtual ~VehicleCollisionTester() = default; + + /// Object layer to use for collision detection, this is used when the filters are not overridden + ObjectLayer GetObjectLayer() const { return mObjectLayer; } + void SetObjectLayer(ObjectLayer inObjectLayer) { mObjectLayer = inObjectLayer; } + + /// Access to the broad phase layer filter, when set this overrides the object layer supplied in the constructor + void SetBroadPhaseLayerFilter(const BroadPhaseLayerFilter *inFilter) { mBroadPhaseLayerFilter = inFilter; } + const BroadPhaseLayerFilter * GetBroadPhaseLayerFilter() const { return mBroadPhaseLayerFilter; } + + /// Access to the object layer filter, when set this overrides the object layer supplied in the constructor + void SetObjectLayerFilter(const ObjectLayerFilter *inFilter) { mObjectLayerFilter = inFilter; } + const ObjectLayerFilter * GetObjectLayerFilter() const { return mObjectLayerFilter; } + + /// Access to the body filter, when set this overrides the default filter that filters out the vehicle body + void SetBodyFilter(const BodyFilter *inFilter) { mBodyFilter = inFilter; } + const BodyFilter * GetBodyFilter() const { return mBodyFilter; } + + /// Do a collision test with the world + /// @param inPhysicsSystem The physics system that should be tested against + /// @param inVehicleConstraint The vehicle constraint + /// @param inWheelIndex Index of the wheel that we're testing collision for + /// @param inOrigin Origin for the test, corresponds to the world space position for the suspension attachment point + /// @param inDirection Direction for the test (unit vector, world space) + /// @param inVehicleBodyID This body should be filtered out during collision detection to avoid self collisions + /// @param outBody Body that the wheel collided with + /// @param outSubShapeID Sub shape ID that the wheel collided with + /// @param outContactPosition Contact point between wheel and floor, in world space + /// @param outContactNormal Contact normal between wheel and floor, pointing away from the floor + /// @param outSuspensionLength New length of the suspension [0, inSuspensionMaxLength] + /// @return True when collision found, false if not + virtual bool Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const = 0; + + /// Do a cheap contact properties prediction based on the contact properties from the last collision test (provided as input parameters) + /// @param inPhysicsSystem The physics system that should be tested against + /// @param inVehicleConstraint The vehicle constraint + /// @param inWheelIndex Index of the wheel that we're testing collision for + /// @param inOrigin Origin for the test, corresponds to the world space position for the suspension attachment point + /// @param inDirection Direction for the test (unit vector, world space) + /// @param inVehicleBodyID The body ID for the vehicle itself + /// @param ioBody Body that the wheel previously collided with + /// @param ioSubShapeID Sub shape ID that the wheel collided with during the last check + /// @param ioContactPosition Contact point between wheel and floor during the last check, in world space + /// @param ioContactNormal Contact normal between wheel and floor during the last check, pointing away from the floor + /// @param ioSuspensionLength New length of the suspension [0, inSuspensionMaxLength] + virtual void PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const = 0; + +protected: + const BroadPhaseLayerFilter * mBroadPhaseLayerFilter = nullptr; + const ObjectLayerFilter * mObjectLayerFilter = nullptr; + const BodyFilter * mBodyFilter = nullptr; + ObjectLayer mObjectLayer = cObjectLayerInvalid; +}; + +/// Collision tester that tests collision using a raycast +class JPH_EXPORT VehicleCollisionTesterRay : public VehicleCollisionTester +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + /// @param inObjectLayer Object layer to test collision with + /// @param inUp World space up vector, used to avoid colliding with vertical walls. + /// @param inMaxSlopeAngle Max angle (rad) that is considered for colliding wheels. This is to avoid colliding with vertical walls. + VehicleCollisionTesterRay(ObjectLayer inObjectLayer, Vec3Arg inUp = Vec3::sAxisY(), float inMaxSlopeAngle = DegreesToRadians(80.0f)) : VehicleCollisionTester(inObjectLayer), mUp(inUp), mCosMaxSlopeAngle(Cos(inMaxSlopeAngle)) { } + + // See: VehicleCollisionTester + virtual bool Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const override; + virtual void PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const override; + +private: + Vec3 mUp; + float mCosMaxSlopeAngle; +}; + +/// Collision tester that tests collision using a sphere cast +class JPH_EXPORT VehicleCollisionTesterCastSphere : public VehicleCollisionTester +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + /// @param inObjectLayer Object layer to test collision with + /// @param inUp World space up vector, used to avoid colliding with vertical walls. + /// @param inRadius Radius of sphere + /// @param inMaxSlopeAngle Max angle (rad) that is considered for colliding wheels. This is to avoid colliding with vertical walls. + VehicleCollisionTesterCastSphere(ObjectLayer inObjectLayer, float inRadius, Vec3Arg inUp = Vec3::sAxisY(), float inMaxSlopeAngle = DegreesToRadians(80.0f)) : VehicleCollisionTester(inObjectLayer), mRadius(inRadius), mUp(inUp), mCosMaxSlopeAngle(Cos(inMaxSlopeAngle)) { } + + // See: VehicleCollisionTester + virtual bool Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const override; + virtual void PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const override; + +private: + float mRadius; + Vec3 mUp; + float mCosMaxSlopeAngle; +}; + +/// Collision tester that tests collision using a cylinder shape +class JPH_EXPORT VehicleCollisionTesterCastCylinder : public VehicleCollisionTester +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + /// @param inObjectLayer Object layer to test collision with + /// @param inConvexRadiusFraction Fraction of half the wheel width (or wheel radius if it is smaller) that is used as the convex radius + VehicleCollisionTesterCastCylinder(ObjectLayer inObjectLayer, float inConvexRadiusFraction = 0.1f) : VehicleCollisionTester(inObjectLayer), mConvexRadiusFraction(inConvexRadiusFraction) { JPH_ASSERT(mConvexRadiusFraction >= 0.0f && mConvexRadiusFraction <= 1.0f); } + + // See: VehicleCollisionTester + virtual bool Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const override; + virtual void PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const override; + +private: + float mConvexRadiusFraction; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.cpp new file mode 100644 index 000000000000..fb76aa674079 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.cpp @@ -0,0 +1,697 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(VehicleConstraintSettings) +{ + JPH_ADD_BASE_CLASS(VehicleConstraintSettings, ConstraintSettings) + + JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mUp) + JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mForward) + JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mMaxPitchRollAngle) + JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mWheels) + JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mAntiRollBars) + JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mController) +} + +void VehicleConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mUp); + inStream.Write(mForward); + inStream.Write(mMaxPitchRollAngle); + + uint32 num_anti_rollbars = (uint32)mAntiRollBars.size(); + inStream.Write(num_anti_rollbars); + for (const VehicleAntiRollBar &r : mAntiRollBars) + r.SaveBinaryState(inStream); + + uint32 num_wheels = (uint32)mWheels.size(); + inStream.Write(num_wheels); + for (const WheelSettings *w : mWheels) + w->SaveBinaryState(inStream); + + inStream.Write(mController->GetRTTI()->GetHash()); + mController->SaveBinaryState(inStream); +} + +void VehicleConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mUp); + inStream.Read(mForward); + inStream.Read(mMaxPitchRollAngle); + + uint32 num_anti_rollbars = 0; + inStream.Read(num_anti_rollbars); + mAntiRollBars.resize(num_anti_rollbars); + for (VehicleAntiRollBar &r : mAntiRollBars) + r.RestoreBinaryState(inStream); + + uint32 num_wheels = 0; + inStream.Read(num_wheels); + mWheels.resize(num_wheels); + for (WheelSettings *w : mWheels) + w->RestoreBinaryState(inStream); + + uint32 hash = 0; + inStream.Read(hash); + const RTTI *rtti = Factory::sInstance->Find(hash); + mController = reinterpret_cast(rtti->CreateObject()); + mController->RestoreBinaryState(inStream); +} + +VehicleConstraint::VehicleConstraint(Body &inVehicleBody, const VehicleConstraintSettings &inSettings) : + Constraint(inSettings), + mBody(&inVehicleBody), + mForward(inSettings.mForward), + mUp(inSettings.mUp), + mWorldUp(inSettings.mUp) +{ + // Check sanity of incoming settings + JPH_ASSERT(inSettings.mUp.IsNormalized()); + JPH_ASSERT(inSettings.mForward.IsNormalized()); + JPH_ASSERT(!inSettings.mWheels.empty()); + + // Store max pitch/roll angle + SetMaxPitchRollAngle(inSettings.mMaxPitchRollAngle); + + // Copy anti-rollbar settings + mAntiRollBars.resize(inSettings.mAntiRollBars.size()); + for (uint i = 0; i < mAntiRollBars.size(); ++i) + { + const VehicleAntiRollBar &r = inSettings.mAntiRollBars[i]; + mAntiRollBars[i] = r; + JPH_ASSERT(r.mStiffness >= 0.0f); + } + + // Construct our controller class + mController = inSettings.mController->ConstructController(*this); + + // Create wheels + mWheels.resize(inSettings.mWheels.size()); + for (uint i = 0; i < mWheels.size(); ++i) + mWheels[i] = mController->ConstructWheel(*inSettings.mWheels[i]); + + // Use the body ID as a seed for the step counter so that not all vehicles will update at the same time + mCurrentStep = uint32(Hash64(inVehicleBody.GetID().GetIndex())); +} + +VehicleConstraint::~VehicleConstraint() +{ + // Destroy controller + delete mController; + + // Destroy our wheels + for (Wheel *w : mWheels) + delete w; +} + +void VehicleConstraint::GetWheelLocalBasis(const Wheel *inWheel, Vec3 &outForward, Vec3 &outUp, Vec3 &outRight) const +{ + const WheelSettings *settings = inWheel->mSettings; + + Quat steer_rotation = Quat::sRotation(settings->mSteeringAxis, inWheel->mSteerAngle); + outUp = steer_rotation * settings->mWheelUp; + outForward = steer_rotation * settings->mWheelForward; + outRight = outForward.Cross(outUp).Normalized(); + outForward = outUp.Cross(outRight).Normalized(); +} + +Mat44 VehicleConstraint::GetWheelLocalTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const +{ + JPH_ASSERT(inWheelIndex < mWheels.size()); + + const Wheel *wheel = mWheels[inWheelIndex]; + const WheelSettings *settings = wheel->mSettings; + + // Use the two vectors provided to calculate a matrix that takes us from wheel model space to X = right, Y = up, Z = forward (the space where we will rotate the wheel) + Mat44 wheel_to_rotational = Mat44(Vec4(inWheelRight, 0), Vec4(inWheelUp, 0), Vec4(inWheelUp.Cross(inWheelRight), 0), Vec4(0, 0, 0, 1)).Transposed(); + + // Calculate the matrix that takes us from the rotational space to vehicle local space + Vec3 local_forward, local_up, local_right; + GetWheelLocalBasis(wheel, local_forward, local_up, local_right); + Vec3 local_wheel_pos = settings->mPosition + settings->mSuspensionDirection * wheel->mSuspensionLength; + Mat44 rotational_to_local(Vec4(local_right, 0), Vec4(local_up, 0), Vec4(local_forward, 0), Vec4(local_wheel_pos, 1)); + + // Calculate transform of rotated wheel + return rotational_to_local * Mat44::sRotationX(wheel->mAngle) * wheel_to_rotational; +} + +RMat44 VehicleConstraint::GetWheelWorldTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const +{ + return mBody->GetWorldTransform() * GetWheelLocalTransform(inWheelIndex, inWheelRight, inWheelUp); +} + +void VehicleConstraint::OnStep(const PhysicsStepListenerContext &inContext) +{ + JPH_PROFILE_FUNCTION(); + + // Callback to higher-level systems. We do it before PreCollide, in case steering changes. + if (mPreStepCallback != nullptr) + mPreStepCallback(*this, inContext); + + if (mIsGravityOverridden) + { + // If gravity is overridden, we replace the normal gravity calculations + if (mBody->IsActive()) + { + MotionProperties *mp = mBody->GetMotionProperties(); + mp->SetGravityFactor(0.0f); + mBody->AddForce(mGravityOverride / mp->GetInverseMass()); + } + + // And we calculate the world up using the custom gravity + mWorldUp = (-mGravityOverride).NormalizedOr(mWorldUp); + } + else + { + // Calculate new world up vector by inverting gravity + mWorldUp = (-inContext.mPhysicsSystem->GetGravity()).NormalizedOr(mWorldUp); + } + + // Callback on our controller + mController->PreCollide(inContext.mDeltaTime, *inContext.mPhysicsSystem); + + // Calculate if this constraint is active by checking if our main vehicle body is active or any of the bodies we touch are active + mIsActive = mBody->IsActive(); + + // Test how often we need to update the wheels + uint num_steps_between_collisions = mIsActive? mNumStepsBetweenCollisionTestActive : mNumStepsBetweenCollisionTestInactive; + + RMat44 body_transform = mBody->GetWorldTransform(); + + // Test collision for wheels + for (uint wheel_index = 0; wheel_index < mWheels.size(); ++wheel_index) + { + Wheel *w = mWheels[wheel_index]; + const WheelSettings *settings = w->mSettings; + + // Calculate suspension origin and direction + RVec3 ws_origin = body_transform * settings->mPosition; + Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection); + + // Test if we need to update this wheel + if (num_steps_between_collisions == 0 + || (mCurrentStep + wheel_index) % num_steps_between_collisions != 0) + { + // Simplified wheel contact test + if (!w->mContactBodyID.IsInvalid()) + { + // Test if the body is still valid + w->mContactBody = inContext.mPhysicsSystem->GetBodyLockInterfaceNoLock().TryGetBody(w->mContactBodyID); + if (w->mContactBody == nullptr) + { + // It's not, forget the contact + w->mContactBodyID = BodyID(); + w->mContactSubShapeID = SubShapeID(); + w->mSuspensionLength = settings->mSuspensionMaxLength; + } + else + { + // Extrapolate the wheel contact properties + mVehicleCollisionTester->PredictContactProperties(*inContext.mPhysicsSystem, *this, wheel_index, ws_origin, ws_direction, mBody->GetID(), w->mContactBody, w->mContactSubShapeID, w->mContactPosition, w->mContactNormal, w->mSuspensionLength); + } + } + } + else + { + // Full wheel contact test, start by resetting the contact data + w->mContactBodyID = BodyID(); + w->mContactBody = nullptr; + w->mContactSubShapeID = SubShapeID(); + w->mSuspensionLength = settings->mSuspensionMaxLength; + + // Test collision to find the floor + if (mVehicleCollisionTester->Collide(*inContext.mPhysicsSystem, *this, wheel_index, ws_origin, ws_direction, mBody->GetID(), w->mContactBody, w->mContactSubShapeID, w->mContactPosition, w->mContactNormal, w->mSuspensionLength)) + { + // Store ID (pointer is not valid outside of the simulation step) + w->mContactBodyID = w->mContactBody->GetID(); + } + } + + if (w->mContactBody != nullptr) + { + // Store contact velocity, cache this as the contact body may be removed + w->mContactPointVelocity = w->mContactBody->GetPointVelocity(w->mContactPosition); + + // Determine plane constant for axle contact plane + w->mAxlePlaneConstant = RVec3(w->mContactNormal).Dot(ws_origin + w->mSuspensionLength * ws_direction); + + // Check if body is active, if so the entire vehicle should be active + mIsActive |= w->mContactBody->IsActive(); + + // Determine world space forward using steering angle and body rotation + Vec3 forward, up, right; + GetWheelLocalBasis(w, forward, up, right); + forward = body_transform.Multiply3x3(forward); + right = body_transform.Multiply3x3(right); + + // The longitudinal axis is in the up/forward plane + w->mContactLongitudinal = w->mContactNormal.Cross(right); + + // Make sure that the longitudinal axis is aligned with the forward axis + if (w->mContactLongitudinal.Dot(forward) < 0.0f) + w->mContactLongitudinal = -w->mContactLongitudinal; + + // Normalize it + w->mContactLongitudinal = w->mContactLongitudinal.NormalizedOr(w->mContactNormal.GetNormalizedPerpendicular()); + + // The lateral axis is perpendicular to contact normal and longitudinal axis + w->mContactLateral = w->mContactLongitudinal.Cross(w->mContactNormal).Normalized(); + } + } + + // Callback to higher-level systems. We do it immediately after wheel collision. + if (mPostCollideCallback != nullptr) + mPostCollideCallback(*this, inContext); + + // Calculate anti-rollbar impulses + for (const VehicleAntiRollBar &r : mAntiRollBars) + { + Wheel *lw = mWheels[r.mLeftWheel]; + Wheel *rw = mWheels[r.mRightWheel]; + + if (lw->mContactBody != nullptr && rw->mContactBody != nullptr) + { + // Calculate the impulse to apply based on the difference in suspension length + float difference = rw->mSuspensionLength - lw->mSuspensionLength; + float impulse = difference * r.mStiffness * inContext.mDeltaTime; + lw->mAntiRollBarImpulse = -impulse; + rw->mAntiRollBarImpulse = impulse; + } + else + { + // When one of the wheels is not on the ground we don't apply any impulses + lw->mAntiRollBarImpulse = rw->mAntiRollBarImpulse = 0.0f; + } + } + + // Callback on our controller + mController->PostCollide(inContext.mDeltaTime, *inContext.mPhysicsSystem); + + // Callback to higher-level systems. We do it before the sleep section, in case velocities change. + if (mPostStepCallback != nullptr) + mPostStepCallback(*this, inContext); + + // If the wheels are rotating, we don't want to go to sleep yet + bool allow_sleep = mController->AllowSleep(); + if (allow_sleep) + for (const Wheel *w : mWheels) + if (abs(w->mAngularVelocity) > DegreesToRadians(10.0f)) + { + allow_sleep = false; + break; + } + if (mBody->GetAllowSleeping() != allow_sleep) + mBody->SetAllowSleeping(allow_sleep); + + // Increment step counter + ++mCurrentStep; +} + +void VehicleConstraint::BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager) +{ + // Find dynamic bodies that our wheels are touching + BodyID *body_ids = (BodyID *)JPH_STACK_ALLOC((mWheels.size() + 1) * sizeof(BodyID)); + int num_bodies = 0; + bool needs_to_activate = false; + for (const Wheel *w : mWheels) + if (w->mContactBody != nullptr) + { + // Avoid adding duplicates + bool duplicate = false; + BodyID id = w->mContactBody->GetID(); + for (int i = 0; i < num_bodies; ++i) + if (body_ids[i] == id) + { + duplicate = true; + break; + } + if (duplicate) + continue; + + if (w->mContactBody->IsDynamic()) + { + body_ids[num_bodies++] = id; + needs_to_activate |= !w->mContactBody->IsActive(); + } + } + + // Activate bodies, note that if we get here we have already told the system that we're active so that means our main body needs to be active too + if (!mBody->IsActive()) + { + // Our main body is not active, activate it too + body_ids[num_bodies] = mBody->GetID(); + inBodyManager.ActivateBodies(body_ids, num_bodies + 1); + } + else if (needs_to_activate) + { + // Only activate bodies the wheels are touching + inBodyManager.ActivateBodies(body_ids, num_bodies); + } + + // Link the bodies into the same island + uint32 min_active_index = Body::cInactiveIndex; + for (int i = 0; i < num_bodies; ++i) + { + const Body &body = inBodyManager.GetBody(body_ids[i]); + min_active_index = min(min_active_index, body.GetIndexInActiveBodiesInternal()); + ioBuilder.LinkBodies(mBody->GetIndexInActiveBodiesInternal(), body.GetIndexInActiveBodiesInternal()); + } + + // Link the constraint in the island + ioBuilder.LinkConstraint(inConstraintIndex, mBody->GetIndexInActiveBodiesInternal(), min_active_index); +} + +uint VehicleConstraint::BuildIslandSplits(LargeIslandSplitter &ioSplitter) const +{ + return ioSplitter.AssignToNonParallelSplit(mBody); +} + +void VehicleConstraint::CalculateSuspensionForcePoint(const Wheel &inWheel, Vec3 &outR1PlusU, Vec3 &outR2) const +{ + // Determine point to apply force to + RVec3 force_point; + if (inWheel.mSettings->mEnableSuspensionForcePoint) + force_point = mBody->GetWorldTransform() * inWheel.mSettings->mSuspensionForcePoint; + else + force_point = inWheel.mContactPosition; + + // Calculate r1 + u and r2 + outR1PlusU = Vec3(force_point - mBody->GetCenterOfMassPosition()); + outR2 = Vec3(force_point - inWheel.mContactBody->GetCenterOfMassPosition()); +} + +void VehicleConstraint::CalculatePitchRollConstraintProperties(RMat44Arg inBodyTransform) +{ + // Check if a limit was specified + if (mCosMaxPitchRollAngle > -1.0f) + { + // Calculate cos of angle between world up vector and vehicle up vector + Vec3 vehicle_up = inBodyTransform.Multiply3x3(mUp); + mCosPitchRollAngle = mWorldUp.Dot(vehicle_up); + if (mCosPitchRollAngle < mCosMaxPitchRollAngle) + { + // Calculate rotation axis to rotate vehicle towards up + Vec3 rotation_axis = mWorldUp.Cross(vehicle_up); + float len = rotation_axis.Length(); + if (len > 0.0f) + mPitchRollRotationAxis = rotation_axis / len; + + mPitchRollPart.CalculateConstraintProperties(*mBody, Body::sFixedToWorld, mPitchRollRotationAxis); + } + else + mPitchRollPart.Deactivate(); + } + else + mPitchRollPart.Deactivate(); +} + +void VehicleConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + RMat44 body_transform = mBody->GetWorldTransform(); + + for (Wheel *w : mWheels) + if (w->mContactBody != nullptr) + { + const WheelSettings *settings = w->mSettings; + + Vec3 neg_contact_normal = -w->mContactNormal; + + Vec3 r1_plus_u, r2; + CalculateSuspensionForcePoint(*w, r1_plus_u, r2); + + // Suspension spring + if (settings->mSuspensionMaxLength > settings->mSuspensionMinLength) + { + float stiffness, damping; + if (settings->mSuspensionSpring.mMode == ESpringMode::FrequencyAndDamping) + { + // Calculate effective mass based on vehicle configuration (the stiffness of the spring should not be affected by the dynamics of the vehicle): K = 1 / (J M^-1 J^T) + // Note that if no suspension force point is supplied we don't know where the force is applied so we assume it is applied at average suspension length + Vec3 force_point = settings->mEnableSuspensionForcePoint? settings->mSuspensionForcePoint : settings->mPosition + 0.5f * (settings->mSuspensionMinLength + settings->mSuspensionMaxLength) * settings->mSuspensionDirection; + Vec3 force_point_x_neg_up = force_point.Cross(-mUp); + const MotionProperties *mp = mBody->GetMotionProperties(); + float effective_mass = 1.0f / (mp->GetInverseMass() + force_point_x_neg_up.Dot(mp->GetLocalSpaceInverseInertia().Multiply3x3(force_point_x_neg_up))); + + // Convert frequency and damping to stiffness and damping + float omega = 2.0f * JPH_PI * settings->mSuspensionSpring.mFrequency; + stiffness = effective_mass * Square(omega); + damping = 2.0f * effective_mass * settings->mSuspensionSpring.mDamping * omega; + } + else + { + // In this case we can simply copy the properties + stiffness = settings->mSuspensionSpring.mStiffness; + damping = settings->mSuspensionSpring.mDamping; + } + + // Calculate the damping and frequency of the suspension spring given the angle between the suspension direction and the contact normal + // If the angle between the suspension direction and the inverse of the contact normal is alpha then the force on the spring relates to the force along the contact normal as: + // + // Fspring = Fnormal * cos(alpha) + // + // The spring force is: + // + // Fspring = -k * x + // + // where k is the spring constant and x is the displacement of the spring. So we have: + // + // Fnormal * cos(alpha) = -k * x <=> Fnormal = -k / cos(alpha) * x + // + // So we can see this as a spring with spring constant: + // + // k' = k / cos(alpha) + // + // In the same way the velocity relates like: + // + // Vspring = Vnormal * cos(alpha) + // + // Which results in the modified damping constant c: + // + // c' = c / cos(alpha) + // + // Note that we clamp 1 / cos(alpha) to the range [0.1, 1] in order not to increase the stiffness / damping by too much. + Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection); + float cos_angle = max(0.1f, ws_direction.Dot(neg_contact_normal)); + stiffness /= cos_angle; + damping /= cos_angle; + + // Get the value of the constraint equation + float c = w->mSuspensionLength - settings->mSuspensionMaxLength - settings->mSuspensionPreloadLength; + + w->mSuspensionPart.CalculateConstraintPropertiesWithStiffnessAndDamping(inDeltaTime, *mBody, r1_plus_u, *w->mContactBody, r2, neg_contact_normal, w->mAntiRollBarImpulse, c, stiffness, damping); + } + else + w->mSuspensionPart.Deactivate(); + + // Check if we reached the 'max up' position and if so add a hard velocity constraint that stops any further movement in the normal direction + if (w->mSuspensionLength < settings->mSuspensionMinLength) + w->mSuspensionMaxUpPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, neg_contact_normal); + else + w->mSuspensionMaxUpPart.Deactivate(); + + // Friction and propulsion + w->mLongitudinalPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, -w->mContactLongitudinal); + w->mLateralPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, -w->mContactLateral); + } + else + { + // No contact -> disable everything + w->mSuspensionPart.Deactivate(); + w->mSuspensionMaxUpPart.Deactivate(); + w->mLongitudinalPart.Deactivate(); + w->mLateralPart.Deactivate(); + } + + CalculatePitchRollConstraintProperties(body_transform); +} + +void VehicleConstraint::ResetWarmStart() +{ + for (Wheel *w : mWheels) + { + w->mSuspensionPart.Deactivate(); + w->mSuspensionMaxUpPart.Deactivate(); + w->mLongitudinalPart.Deactivate(); + w->mLateralPart.Deactivate(); + } + + mPitchRollPart.Deactivate(); +} + +void VehicleConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + for (Wheel *w : mWheels) + if (w->mContactBody != nullptr) + { + Vec3 neg_contact_normal = -w->mContactNormal; + + w->mSuspensionPart.WarmStart(*mBody, *w->mContactBody, neg_contact_normal, inWarmStartImpulseRatio); + w->mSuspensionMaxUpPart.WarmStart(*mBody, *w->mContactBody, neg_contact_normal, inWarmStartImpulseRatio); + w->mLongitudinalPart.WarmStart(*mBody, *w->mContactBody, -w->mContactLongitudinal, 0.0f); // Don't warm start the longitudinal part (the engine/brake force, we don't want to preserve anything from the last frame) + w->mLateralPart.WarmStart(*mBody, *w->mContactBody, -w->mContactLateral, inWarmStartImpulseRatio); + } + + mPitchRollPart.WarmStart(*mBody, Body::sFixedToWorld, inWarmStartImpulseRatio); +} + +bool VehicleConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + bool impulse = false; + + // Solve suspension + for (Wheel *w : mWheels) + if (w->mContactBody != nullptr) + { + Vec3 neg_contact_normal = -w->mContactNormal; + + // Suspension spring, note that it can only push and not pull + if (w->mSuspensionPart.IsActive()) + impulse |= w->mSuspensionPart.SolveVelocityConstraint(*mBody, *w->mContactBody, neg_contact_normal, 0.0f, FLT_MAX); + + // When reaching the minimal suspension length only allow forces pushing the bodies away + if (w->mSuspensionMaxUpPart.IsActive()) + impulse |= w->mSuspensionMaxUpPart.SolveVelocityConstraint(*mBody, *w->mContactBody, neg_contact_normal, 0.0f, FLT_MAX); + } + + // Solve the horizontal movement of the vehicle + impulse |= mController->SolveLongitudinalAndLateralConstraints(inDeltaTime); + + // Apply the pitch / roll constraint to avoid the vehicle from toppling over + if (mPitchRollPart.IsActive()) + impulse |= mPitchRollPart.SolveVelocityConstraint(*mBody, Body::sFixedToWorld, mPitchRollRotationAxis, 0, FLT_MAX); + + return impulse; +} + +bool VehicleConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + bool impulse = false; + + RMat44 body_transform = mBody->GetWorldTransform(); + + for (Wheel *w : mWheels) + if (w->mContactBody != nullptr) + { + const WheelSettings *settings = w->mSettings; + + // Check if we reached the 'max up' position now that the body has possibly moved + // We do this by calculating the axle position at minimum suspension length and making sure it does not go through the + // plane defined by the contact normal and the axle position when the contact happened + // TODO: This assumes that only the vehicle moved and not the ground as we kept the axle contact plane in world space + Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection); + RVec3 ws_position = body_transform * settings->mPosition; + RVec3 min_suspension_pos = ws_position + settings->mSuspensionMinLength * ws_direction; + float max_up_error = float(RVec3(w->mContactNormal).Dot(min_suspension_pos) - w->mAxlePlaneConstant); + if (max_up_error < 0.0f) + { + Vec3 neg_contact_normal = -w->mContactNormal; + + // Recalculate constraint properties since the body may have moved + Vec3 r1_plus_u, r2; + CalculateSuspensionForcePoint(*w, r1_plus_u, r2); + w->mSuspensionMaxUpPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, neg_contact_normal); + + impulse |= w->mSuspensionMaxUpPart.SolvePositionConstraint(*mBody, *w->mContactBody, neg_contact_normal, max_up_error, inBaumgarte); + } + } + + // Apply the pitch / roll constraint to avoid the vehicle from toppling over + CalculatePitchRollConstraintProperties(body_transform); + if (mPitchRollPart.IsActive()) + impulse |= mPitchRollPart.SolvePositionConstraint(*mBody, Body::sFixedToWorld, mCosPitchRollAngle - mCosMaxPitchRollAngle, inBaumgarte); + + return impulse; +} + +#ifdef JPH_DEBUG_RENDERER + +void VehicleConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + mController->Draw(inRenderer); +} + +void VehicleConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const +{ +} + +#endif // JPH_DEBUG_RENDERER + +void VehicleConstraint::SaveState(StateRecorder &inStream) const +{ + Constraint::SaveState(inStream); + + mController->SaveState(inStream); + + for (const Wheel *w : mWheels) + { + inStream.Write(w->mAngularVelocity); + inStream.Write(w->mAngle); + inStream.Write(w->mContactBodyID); // Used by MotorcycleController::PreCollide + inStream.Write(w->mContactPosition); // Used by VehicleCollisionTester::PredictContactProperties + inStream.Write(w->mContactNormal); // Used by MotorcycleController::PreCollide + inStream.Write(w->mContactLateral); // Used by MotorcycleController::PreCollide + inStream.Write(w->mSuspensionLength); // Used by VehicleCollisionTester::PredictContactProperties + + w->mSuspensionPart.SaveState(inStream); + w->mSuspensionMaxUpPart.SaveState(inStream); + w->mLongitudinalPart.SaveState(inStream); + w->mLateralPart.SaveState(inStream); + } + + inStream.Write(mPitchRollRotationAxis); // When rotation is too small we use last frame so we need to store it + mPitchRollPart.SaveState(inStream); + inStream.Write(mCurrentStep); +} + +void VehicleConstraint::RestoreState(StateRecorder &inStream) +{ + Constraint::RestoreState(inStream); + + mController->RestoreState(inStream); + + for (Wheel *w : mWheels) + { + inStream.Read(w->mAngularVelocity); + inStream.Read(w->mAngle); + inStream.Read(w->mContactBodyID); + inStream.Read(w->mContactPosition); + inStream.Read(w->mContactNormal); + inStream.Read(w->mContactLateral); + inStream.Read(w->mSuspensionLength); + w->mContactBody = nullptr; // No longer valid + + w->mSuspensionPart.RestoreState(inStream); + w->mSuspensionMaxUpPart.RestoreState(inStream); + w->mLongitudinalPart.RestoreState(inStream); + w->mLateralPart.RestoreState(inStream); + } + + inStream.Read(mPitchRollRotationAxis); + mPitchRollPart.RestoreState(inStream); + inStream.Read(mCurrentStep); +} + +Ref VehicleConstraint::GetConstraintSettings() const +{ + JPH_ASSERT(false); // Not implemented yet + return nullptr; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.h new file mode 100644 index 000000000000..9459f8e43737 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.h @@ -0,0 +1,246 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; + +/// Configuration for constraint that simulates a wheeled vehicle. +/// +/// The properties in this constraint are largely based on "Car Physics for Games" by Marco Monster. +/// See: https://www.asawicki.info/Mirror/Car%20Physics%20for%20Games/Car%20Physics%20for%20Games.html +class JPH_EXPORT VehicleConstraintSettings : public ConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, VehicleConstraintSettings) + +public: + /// Saves the contents of the constraint settings in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const override; + + Vec3 mUp { 0, 1, 0 }; ///< Vector indicating the up direction of the vehicle (in local space to the body) + Vec3 mForward { 0, 0, 1 }; ///< Vector indicating forward direction of the vehicle (in local space to the body) + float mMaxPitchRollAngle = JPH_PI; ///< 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. + Array> mWheels; ///< List of wheels and their properties + Array mAntiRollBars; ///< List of anti rollbars and their properties + Ref mController; ///< Defines how the vehicle can accelerate / decelerate + +protected: + /// This function should not be called directly, it is used by sRestoreFromBinaryState. + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// Constraint that simulates a vehicle +/// Note: Don't forget to register the constraint as a StepListener with the PhysicsSystem! +/// +/// When the vehicle drives over very light objects (rubble) you may see the car body dip down. This is a known issue and is an artifact of the iterative solver that Jolt is using. +/// Basically if a light object is sandwiched between two heavy objects (the static floor and the car body), the light object is not able to transfer enough force from the ground to +/// the car body to keep the car body up. You can see this effect in the HeavyOnLightTest sample, the boxes on the right have a lot of penetration because they're on top of light objects. +/// +/// There are a couple of ways to improve this: +/// +/// 1. You can increase the number of velocity steps (global settings PhysicsSettings::mNumVelocitySteps or if you only want to increase it on +/// the vehicle you can use VehicleConstraintSettings::mNumVelocityStepsOverride). E.g. going from 10 to 30 steps in the HeavyOnLightTest sample makes the penetration a lot less. +/// The number of position steps can also be increased (the first prevents the body from going down, the second corrects it if the problem did +/// occur which inevitably happens due to numerical drift). This solution costs CPU cycles. +/// +/// 2. You can reduce the mass difference between the vehicle body and the rubble on the floor (by making the rubble heavier or the car lighter). +/// +/// 3. You could filter out collisions between the vehicle collision test and the rubble completely. This would make the wheels ignore the rubble but would cause the vehicle to drive +/// through it as if nothing happened. You could create fake wheels (keyframed bodies) that move along with the vehicle and that only collide with rubble (and not the vehicle or the ground). +/// This would cause the vehicle to push away the rubble without the rubble being able to affect the vehicle (unless it hits the main body of course). +/// +/// Note that when driving over rubble, you may see the wheel jump up and down quite quickly because one frame a collision is found and the next frame not. +/// To alleviate this, it may be needed to smooth the motion of the visual mesh for the wheel. +class JPH_EXPORT VehicleConstraint : public Constraint, public PhysicsStepListener +{ +public: + /// Constructor / destructor + VehicleConstraint(Body &inVehicleBody, const VehicleConstraintSettings &inSettings); + virtual ~VehicleConstraint() override; + + /// Get the type of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Vehicle; } + + /// 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); } + + /// Set the interface that tests collision between wheel and ground + void SetVehicleCollisionTester(const VehicleCollisionTester *inTester) { mVehicleCollisionTester = inTester; } + + /// 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. + using CombineFunction = function; + + /// Set the function that combines the friction of two bodies and returns it + /// Default method is the geometric mean: sqrt(friction1 * friction2). + void SetCombineFriction(const CombineFunction &inCombineFriction) { mCombineFriction = inCombineFriction; } + const CombineFunction & GetCombineFriction() const { return mCombineFriction; } + + /// Callback function to notify of current stage in PhysicsStepListener::OnStep. + using StepCallback = function; + + /// Callback function to notify that PhysicsStepListener::OnStep has started for this vehicle. Default is to do nothing. + /// Can be used to allow higher-level code to e.g. control steering. This is the last moment that the position/orientation of the vehicle can be changed. + /// Wheel collision checks have not been performed yet. + const StepCallback & GetPreStepCallback() const { return mPreStepCallback; } + void SetPreStepCallback(const StepCallback &inPreStepCallback) { mPreStepCallback = inPreStepCallback; } + + /// Callback function to notify that PhysicsStepListener::OnStep has just completed wheel collision checks. Default is to do nothing. + /// Can be used to allow higher-level code to e.g. detect tire contact or to modify the velocity of the vehicle based on the wheel contacts. + /// You should not change the position of the vehicle in this callback as the wheel collision checks have already been performed. + const StepCallback & GetPostCollideCallback() const { return mPostCollideCallback; } + void SetPostCollideCallback(const StepCallback &inPostCollideCallback) { mPostCollideCallback = inPostCollideCallback; } + + /// Callback function to notify that PhysicsStepListener::OnStep has completed for this vehicle. Default is to do nothing. + /// Can be used to allow higher-level code to e.g. control the vehicle in the air. + /// You should not change the position of the vehicle in this callback as the wheel collision checks have already been performed. + const StepCallback & GetPostStepCallback() const { return mPostStepCallback; } + void SetPostStepCallback(const StepCallback &inPostStepCallback) { mPostStepCallback = inPostStepCallback; } + + /// Override gravity for this vehicle. Note that overriding gravity will set the gravity factor of the vehicle body to 0 and apply gravity in the PhysicsStepListener instead. + void OverrideGravity(Vec3Arg inGravity) { mGravityOverride = inGravity; mIsGravityOverridden = true; } + bool IsGravityOverridden() const { return mIsGravityOverridden; } + Vec3 GetGravityOverride() const { return mGravityOverride; } + void ResetGravityOverride() { mIsGravityOverridden = false; mBody->GetMotionProperties()->SetGravityFactor(1.0f); } ///< Note that resetting the gravity override will restore the gravity factor of the vehicle body to 1. + + /// Get the local space forward vector of the vehicle + Vec3 GetLocalForward() const { return mForward; } + + /// Get the local space up vector of the vehicle + Vec3 GetLocalUp() const { return mUp; } + + /// Vector indicating the world space up direction (used to limit vehicle pitch/roll), calculated every frame by inverting gravity + Vec3 GetWorldUp() const { return mWorldUp; } + + /// Access to the vehicle body + Body * GetVehicleBody() const { return mBody; } + + /// Access to the vehicle controller interface (determines acceleration / deceleration) + const VehicleController * GetController() const { return mController; } + + /// Access to the vehicle controller interface (determines acceleration / deceleration) + VehicleController * GetController() { return mController; } + + /// Get the state of the wheels + const Wheels & GetWheels() const { return mWheels; } + + /// Get the state of a wheels (writable interface, allows you to make changes to the configuration which will take effect the next time step) + Wheels & GetWheels() { return mWheels; } + + /// Get the state of a wheel + Wheel * GetWheel(uint inIdx) { return mWheels[inIdx]; } + const Wheel * GetWheel(uint inIdx) const { return mWheels[inIdx]; } + + /// Get the basis vectors for the wheel in local space to the vehicle body (note: basis does not rotate when the wheel rotates around its axis) + /// @param inWheel Wheel to fetch basis for + /// @param outForward Forward vector for the wheel + /// @param outUp Up vector for the wheel + /// @param outRight Right vector for the wheel + void GetWheelLocalBasis(const Wheel *inWheel, Vec3 &outForward, Vec3 &outUp, Vec3 &outRight) const; + + /// Get the transform of a wheel in local space to the vehicle body, returns a matrix that transforms a cylinder aligned with the Y axis in body space (not COM space) + /// @param inWheelIndex Index of the wheel to fetch + /// @param inWheelRight Unit vector that indicates right in model space of the wheel (so if you only have 1 wheel model, you probably want to specify the opposite direction for the left and right wheels) + /// @param inWheelUp Unit vector that indicates up in model space of the wheel + Mat44 GetWheelLocalTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const; + + /// Get the transform of a wheel in world space, returns a matrix that transforms a cylinder aligned with the Y axis in world space + /// @param inWheelIndex Index of the wheel to fetch + /// @param inWheelRight Unit vector that indicates right in model space of the wheel (so if you only have 1 wheel model, you probably want to specify the opposite direction for the left and right wheels) + /// @param inWheelUp Unit vector that indicates up in model space of the wheel + RMat44 GetWheelWorldTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const; + + /// Number of simulation steps between wheel collision tests when the vehicle is active. Default is 1. 0 = never, 1 = every step, 2 = every other step, etc. + /// Note that if a vehicle has multiple wheels and the number of steps > 1, the wheels will be tested in a round robin fashion. + /// If there are multiple vehicles, the tests will be spread out based on the BodyID of the vehicle. + /// If you set this to test less than every step, you may see simulation artifacts. This setting can be used to reduce the cost of simulating vehicles in the distance. + void SetNumStepsBetweenCollisionTestActive(uint inSteps) { mNumStepsBetweenCollisionTestActive = inSteps; } + uint GetNumStepsBetweenCollisionTestActive() const { return mNumStepsBetweenCollisionTestActive; } + + /// Number of simulation steps between wheel collision tests when the vehicle is inactive. Default is 1. 0 = never, 1 = every step, 2 = every other step, etc. + /// Note that if a vehicle has multiple wheels and the number of steps > 1, the wheels will be tested in a round robin fashion. + /// If there are multiple vehicles, the tests will be spread out based on the BodyID of the vehicle. + /// This number can be lower than the number of steps when the vehicle is active as the only purpose of this test is + /// to allow the vehicle to wake up in response to bodies moving into the wheels but not touching the body of the vehicle. + void SetNumStepsBetweenCollisionTestInactive(uint inSteps) { mNumStepsBetweenCollisionTestInactive = inSteps; } + uint GetNumStepsBetweenCollisionTestInactive() const { return mNumStepsBetweenCollisionTestInactive; } + + // Generic interface of a constraint + virtual bool IsActive() const override { return mIsActive && Constraint::IsActive(); } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override { /* Do nothing */ } + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; + virtual void BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager) override; + virtual uint BuildIslandSplits(LargeIslandSplitter &ioSplitter) const override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; + virtual void DrawConstraintLimits(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + +private: + // See: PhysicsStepListener + virtual void OnStep(const PhysicsStepListenerContext &inContext) override; + + // Calculate the position where the suspension and traction forces should be applied in world space, relative to the center of mass of both bodies + void CalculateSuspensionForcePoint(const Wheel &inWheel, Vec3 &outR1PlusU, Vec3 &outR2) const; + + // Calculate the constraint properties for mPitchRollPart + void CalculatePitchRollConstraintProperties(RMat44Arg inBodyTransform); + + // Gravity override + bool mIsGravityOverridden = false; ///< If the gravity is currently overridden + Vec3 mGravityOverride = Vec3::sZero(); ///< Gravity override value, replaces PhysicsSystem::GetGravity() when mIsGravityOverridden is true + + // Simulation information + Body * mBody; ///< Body of the vehicle + Vec3 mForward; ///< Local space forward vector for the vehicle + Vec3 mUp; ///< Local space up vector for the vehicle + Vec3 mWorldUp; ///< Vector indicating the world space up direction (used to limit vehicle pitch/roll) + Wheels mWheels; ///< Wheel states of the vehicle + Array mAntiRollBars; ///< Anti rollbars of the vehicle + VehicleController * mController; ///< Controls the acceleration / deceleration of the vehicle + bool mIsActive = false; ///< If this constraint is active + uint mNumStepsBetweenCollisionTestActive = 1; ///< Number of simulation steps between wheel collision tests when the vehicle is active + uint mNumStepsBetweenCollisionTestInactive = 1; ///< Number of simulation steps between wheel collision tests when the vehicle is inactive + uint mCurrentStep = 0; ///< Current step number, used to determine when to test a wheel + + // Prevent vehicle from toppling over + float mCosMaxPitchRollAngle; ///< Cos of the max pitch/roll angle + float mCosPitchRollAngle; ///< Cos of the current pitch/roll angle + Vec3 mPitchRollRotationAxis { 0, 1, 0 }; ///< Current axis along which to apply torque to prevent the car from toppling over + AngleConstraintPart mPitchRollPart; ///< Constraint part that prevents the car from toppling over + + // Interfaces + RefConst mVehicleCollisionTester; ///< Class that performs testing of collision for the wheels + CombineFunction mCombineFriction = [](uint, float &ioLongitudinalFriction, float &ioLateralFriction, const Body &inBody2, const SubShapeID &) + { + float body_friction = inBody2.GetFriction(); + + ioLongitudinalFriction = sqrt(ioLongitudinalFriction * body_friction); + ioLateralFriction = sqrt(ioLateralFriction * body_friction); + }; + + // Callbacks + StepCallback mPreStepCallback; + StepCallback mPostCollideCallback; + StepCallback mPostStepCallback; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.cpp new file mode 100644 index 000000000000..ffa8ff1a64b1 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.cpp @@ -0,0 +1,17 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(VehicleControllerSettings) +{ + JPH_ADD_BASE_CLASS(VehicleControllerSettings, SerializableObject) +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.h new file mode 100644 index 000000000000..0d9095b189a6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.h @@ -0,0 +1,84 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; +class VehicleController; +class VehicleConstraint; +class WheelSettings; +class Wheel; +class StateRecorder; + +/// Basic settings object for interface that controls acceleration / deceleration of the vehicle +class JPH_EXPORT VehicleControllerSettings : public SerializableObject, public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, VehicleControllerSettings) + +public: + /// Saves the contents of the controller settings in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const = 0; + + /// Restore the contents of the controller settings in binary form from inStream. + virtual void RestoreBinaryState(StreamIn &inStream) = 0; + + /// Create an instance of the vehicle controller class + virtual VehicleController * ConstructController(VehicleConstraint &inConstraint) const = 0; +}; + +/// Runtime data for interface that controls acceleration / deceleration of the vehicle +class JPH_EXPORT VehicleController : public RefTarget, public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor / destructor + explicit VehicleController(VehicleConstraint &inConstraint) : mConstraint(inConstraint) { } + virtual ~VehicleController() = default; + + /// Access the vehicle constraint that this controller is part of + VehicleConstraint & GetConstraint() { return mConstraint; } + const VehicleConstraint & GetConstraint() const { return mConstraint; } + +protected: + // The functions below are only for the VehicleConstraint + friend class VehicleConstraint; + + // Create a new instance of wheel + virtual Wheel * ConstructWheel(const WheelSettings &inWheel) const = 0; + + // If the vehicle is allowed to go to sleep + virtual bool AllowSleep() const = 0; + + // Called before the wheel probes have been done + virtual void PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) = 0; + + // Called after the wheel probes have been done + virtual void PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) = 0; + + // Solve longitudinal and lateral constraint parts for all of the wheels + virtual bool SolveLongitudinalAndLateralConstraints(float inDeltaTime) = 0; + + // Saving state for replay + virtual void SaveState(StateRecorder &inStream) const = 0; + virtual void RestoreState(StateRecorder &inStream) = 0; + +#ifdef JPH_DEBUG_RENDERER + // Drawing interface + virtual void Draw(DebugRenderer *inRenderer) const = 0; +#endif // JPH_DEBUG_RENDERER + + VehicleConstraint & mConstraint; ///< The vehicle constraint we belong to +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleDifferential.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleDifferential.cpp new file mode 100644 index 000000000000..ef7cf4cb914c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleDifferential.cpp @@ -0,0 +1,81 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleDifferentialSettings) +{ + JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mLeftWheel) + JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mRightWheel) + JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mDifferentialRatio) + JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mLeftRightSplit) + JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mLimitedSlipRatio) + JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mEngineTorqueRatio) +} + +void VehicleDifferentialSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mLeftWheel); + inStream.Write(mRightWheel); + inStream.Write(mDifferentialRatio); + inStream.Write(mLeftRightSplit); + inStream.Write(mLimitedSlipRatio); + inStream.Write(mEngineTorqueRatio); +} + +void VehicleDifferentialSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mLeftWheel); + inStream.Read(mRightWheel); + inStream.Read(mDifferentialRatio); + inStream.Read(mLeftRightSplit); + inStream.Read(mLimitedSlipRatio); + inStream.Read(mEngineTorqueRatio); +} + +void VehicleDifferentialSettings::CalculateTorqueRatio(float inLeftAngularVelocity, float inRightAngularVelocity, float &outLeftTorqueFraction, float &outRightTorqueFraction) const +{ + // Start with the default torque ratio + outLeftTorqueFraction = 1.0f - mLeftRightSplit; + outRightTorqueFraction = mLeftRightSplit; + + if (mLimitedSlipRatio < FLT_MAX) + { + JPH_ASSERT(mLimitedSlipRatio > 1.0f); + + // This is a limited slip differential, adjust torque ratios according to wheel speeds + float omega_l = max(1.0e-3f, abs(inLeftAngularVelocity)); // prevent div by zero by setting a minimum velocity and ignoring that the wheels may be rotating in different directions + float omega_r = max(1.0e-3f, abs(inRightAngularVelocity)); + float omega_min = min(omega_l, omega_r); + float omega_max = max(omega_l, omega_r); + + // Map into a value that is 0 when the wheels are turning at an equal rate and 1 when the wheels are turning at mLimitedSlipRotationRatio + float alpha = min((omega_max / omega_min - 1.0f) / (mLimitedSlipRatio - 1.0f), 1.0f); + JPH_ASSERT(alpha >= 0.0f); + float one_min_alpha = 1.0f - alpha; + + if (omega_l < omega_r) + { + // Redirect more power to the left wheel + outLeftTorqueFraction = outLeftTorqueFraction * one_min_alpha + alpha; + outRightTorqueFraction = outRightTorqueFraction * one_min_alpha; + } + else + { + // Redirect more power to the right wheel + outLeftTorqueFraction = outLeftTorqueFraction * one_min_alpha; + outRightTorqueFraction = outRightTorqueFraction * one_min_alpha + alpha; + } + } + + // Assert the values add up to 1 + JPH_ASSERT(abs(outLeftTorqueFraction + outRightTorqueFraction - 1.0f) < 1.0e-6f); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleDifferential.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleDifferential.h new file mode 100644 index 000000000000..304337171700 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleDifferential.h @@ -0,0 +1,39 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class JPH_EXPORT VehicleDifferentialSettings +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleDifferentialSettings) + +public: + /// Saves the contents in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restores the contents in binary form to inStream. + void RestoreBinaryState(StreamIn &inStream); + + /// Calculate the torque ratio between left and right wheel + /// @param inLeftAngularVelocity Angular velocity of left wheel (rad / s) + /// @param inRightAngularVelocity Angular velocity of right wheel (rad / s) + /// @param outLeftTorqueFraction Fraction of torque that should go to the left wheel + /// @param outRightTorqueFraction Fraction of torque that should go to the right wheel + void CalculateTorqueRatio(float inLeftAngularVelocity, float inRightAngularVelocity, float &outLeftTorqueFraction, float &outRightTorqueFraction) const; + + int mLeftWheel = -1; ///< Index (in mWheels) that represents the left wheel of this differential (can be -1 to indicate no wheel) + int mRightWheel = -1; ///< Index (in mWheels) that represents the right wheel of this differential (can be -1 to indicate no wheel) + float mDifferentialRatio = 3.42f; ///< Ratio between rotation speed of gear box and wheels + float mLeftRightSplit = 0.5f; ///< Defines how the engine torque is split across the left and right wheel (0 = left, 0.5 = center, 1 = right) + float mLimitedSlipRatio = 1.4f; ///< Ratio max / min wheel speed. When this ratio is exceeded, all torque gets distributed to the slowest moving wheel. This allows implementing a limited slip differential. Set to FLT_MAX for an open differential. Value should be > 1. + float mEngineTorqueRatio = 1.0f; ///< How much of the engines torque is applied to this differential (0 = none, 1 = full), make sure the sum of all differentials is 1. +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.cpp new file mode 100644 index 000000000000..1352cc0d51ff --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.cpp @@ -0,0 +1,122 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleEngineSettings) +{ + JPH_ADD_ATTRIBUTE(VehicleEngineSettings, mMaxTorque) + JPH_ADD_ATTRIBUTE(VehicleEngineSettings, mMinRPM) + JPH_ADD_ATTRIBUTE(VehicleEngineSettings, mMaxRPM) + JPH_ADD_ATTRIBUTE(VehicleEngineSettings, mNormalizedTorque) +} + +VehicleEngineSettings::VehicleEngineSettings() +{ + mNormalizedTorque.Reserve(3); + mNormalizedTorque.AddPoint(0.0f, 0.8f); + mNormalizedTorque.AddPoint(0.66f, 1.0f); + mNormalizedTorque.AddPoint(1.0f, 0.8f); +} + +void VehicleEngineSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mMaxTorque); + inStream.Write(mMinRPM); + inStream.Write(mMaxRPM); + mNormalizedTorque.SaveBinaryState(inStream); +} + +void VehicleEngineSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mMaxTorque); + inStream.Read(mMinRPM); + inStream.Read(mMaxRPM); + mNormalizedTorque.RestoreBinaryState(inStream); +} + +void VehicleEngine::ApplyTorque(float inTorque, float inDeltaTime) +{ + // Accelerate engine using torque + mCurrentRPM += cAngularVelocityToRPM * inTorque * inDeltaTime / mInertia; + ClampRPM(); +} + +void VehicleEngine::ApplyDamping(float inDeltaTime) +{ + // Angular damping: dw/dt = -c * w + // Solution: w(t) = w(0) * e^(-c * t) or w2 = w1 * e^(-c * dt) + // Taylor expansion of e^(-c * dt) = 1 - c * dt + ... + // Since dt is usually in the order of 1/60 and c is a low number too this approximation is good enough + mCurrentRPM *= max(0.0f, 1.0f - mAngularDamping * inDeltaTime); + ClampRPM(); +} + +#ifdef JPH_DEBUG_RENDERER + +void VehicleEngine::DrawRPM(DebugRenderer *inRenderer, RVec3Arg inPosition, Vec3Arg inForward, Vec3Arg inUp, float inSize, float inShiftDownRPM, float inShiftUpRPM) const +{ + // Function to draw part of a pie + auto draw_pie = [this, inRenderer, inSize, inPosition, inForward, inUp](float inMinRPM, float inMaxRPM, Color inColor) { + inRenderer->DrawPie(inPosition, inSize, inForward, inUp, ConvertRPMToAngle(inMinRPM), ConvertRPMToAngle(inMaxRPM), inColor, DebugRenderer::ECastShadow::Off); + }; + + // Draw segment under min RPM + draw_pie(0, mMinRPM, Color::sGrey); + + // Draw segment until inShiftDownRPM + if (mCurrentRPM < inShiftDownRPM) + { + draw_pie(mMinRPM, mCurrentRPM, Color::sRed); + draw_pie(mCurrentRPM, inShiftDownRPM, Color::sDarkRed); + } + else + { + draw_pie(mMinRPM, inShiftDownRPM, Color::sRed); + } + + // Draw segment between inShiftDownRPM and inShiftUpRPM + if (mCurrentRPM > inShiftDownRPM && mCurrentRPM < inShiftUpRPM) + { + draw_pie(inShiftDownRPM, mCurrentRPM, Color::sOrange); + draw_pie(mCurrentRPM, inShiftUpRPM, Color::sDarkOrange); + } + else + { + draw_pie(inShiftDownRPM, inShiftUpRPM, mCurrentRPM <= inShiftDownRPM? Color::sDarkOrange : Color::sOrange); + } + + // Draw segment above inShiftUpRPM + if (mCurrentRPM > inShiftUpRPM) + { + draw_pie(inShiftUpRPM, mCurrentRPM, Color::sGreen); + draw_pie(mCurrentRPM, mMaxRPM, Color::sDarkGreen); + } + else + { + draw_pie(inShiftUpRPM, mMaxRPM, Color::sDarkGreen); + } +} + +#endif // JPH_DEBUG_RENDERER + +void VehicleEngine::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mCurrentRPM); +} + +void VehicleEngine::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mCurrentRPM); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.h new file mode 100644 index 000000000000..29212ffdab67 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.h @@ -0,0 +1,93 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_DEBUG_RENDERER + class DebugRenderer; +#endif // JPH_DEBUG_RENDERER + +/// Generic properties for a vehicle engine +class JPH_EXPORT VehicleEngineSettings +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleEngineSettings) + +public: + /// Constructor + VehicleEngineSettings(); + + /// Saves the contents in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restores the contents in binary form to inStream. + void RestoreBinaryState(StreamIn &inStream); + + float mMaxTorque = 500.0f; ///< Max amount of torque (Nm) that the engine can deliver + float mMinRPM = 1000.0f; ///< Min amount of revolutions per minute (rpm) the engine can produce without stalling + float mMaxRPM = 6000.0f; ///< Max amount of revolutions per minute (rpm) the engine can generate + LinearCurve mNormalizedTorque; ///< Y-axis: Curve that describes a ratio of the max torque the engine can produce (0 = 0, 1 = mMaxTorque). X-axis: the fraction of the RPM of the engine (0 = mMinRPM, 1 = mMaxRPM) + float mInertia = 0.5f; ///< Moment of inertia (kg m^2) of the engine + float mAngularDamping = 0.2f; ///< Angular damping factor of the wheel: dw/dt = -c * w +}; + +/// Runtime data for engine +class JPH_EXPORT VehicleEngine : public VehicleEngineSettings +{ +public: + /// Multiply an angular velocity (rad/s) with this value to get rounds per minute (RPM) + static constexpr float cAngularVelocityToRPM = 60.0f / (2.0f * JPH_PI); + + /// Clamp the RPM between min and max RPM + inline void ClampRPM() { mCurrentRPM = Clamp(mCurrentRPM, mMinRPM, mMaxRPM); } + + /// Current rotation speed of engine in rounds per minute + float GetCurrentRPM() const { return mCurrentRPM; } + + /// Update rotation speed of engine in rounds per minute + void SetCurrentRPM(float inRPM) { mCurrentRPM = inRPM; ClampRPM(); } + + /// Get current angular velocity of the engine in radians / second + inline float GetAngularVelocity() const { return mCurrentRPM / cAngularVelocityToRPM; } + + /// Get the amount of torque (N m) that the engine can supply + /// @param inAcceleration How much the gas pedal is pressed [0, 1] + float GetTorque(float inAcceleration) const { return inAcceleration * mMaxTorque * mNormalizedTorque.GetValue(mCurrentRPM / mMaxRPM); } + + /// Apply a torque to the engine rotation speed + /// @param inTorque Torque in N m + /// @param inDeltaTime Delta time in seconds + void ApplyTorque(float inTorque, float inDeltaTime); + + /// Update the engine RPM for damping + /// @param inDeltaTime Delta time in seconds + void ApplyDamping(float inDeltaTime); + +#ifdef JPH_DEBUG_RENDERER + // Function that converts RPM to an angle in radians for debugging purposes + float ConvertRPMToAngle(float inRPM) const { return (-0.75f + 1.5f * inRPM / mMaxRPM) * JPH_PI; } + + /// Debug draw a RPM meter + void DrawRPM(DebugRenderer *inRenderer, RVec3Arg inPosition, Vec3Arg inForward, Vec3Arg inUp, float inSize, float inShiftDownRPM, float inShiftUpRPM) const; +#endif // JPH_DEBUG_RENDERER + + /// If the engine is idle we allow the vehicle to sleep + bool AllowSleep() const { return mCurrentRPM <= 1.01f * mMinRPM; } + + /// Saving state for replay + void SaveState(StateRecorder &inStream) const; + void RestoreState(StateRecorder &inStream); + +private: + float mCurrentRPM = mMinRPM; ///< Current rotation speed of engine in rounds per minute +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTrack.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTrack.cpp new file mode 100644 index 000000000000..3bef2e804933 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTrack.cpp @@ -0,0 +1,52 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleTrackSettings) +{ + JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mDrivenWheel) + JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mWheels) + JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mInertia) + JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mAngularDamping) + JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mMaxBrakeTorque) + JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mDifferentialRatio) +} + +void VehicleTrackSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mDrivenWheel); + inStream.Write(mWheels); + inStream.Write(mInertia); + inStream.Write(mAngularDamping); + inStream.Write(mMaxBrakeTorque); + inStream.Write(mDifferentialRatio); +} + +void VehicleTrackSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mDrivenWheel); + inStream.Read(mWheels); + inStream.Read(mInertia); + inStream.Read(mAngularDamping); + inStream.Read(mMaxBrakeTorque); + inStream.Read(mDifferentialRatio); +} + +void VehicleTrack::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mAngularVelocity); +} + +void VehicleTrack::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mAngularVelocity); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTrack.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTrack.h new file mode 100644 index 000000000000..dae4f9842e52 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTrack.h @@ -0,0 +1,56 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// On which side of the vehicle the track is located (for steering) +enum class ETrackSide : uint +{ + Left = 0, + Right = 1, + Num = 2 +}; + +/// Generic properties for tank tracks +class JPH_EXPORT VehicleTrackSettings +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleTrackSettings) + +public: + /// Saves the contents in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restores the contents in binary form to inStream. + void RestoreBinaryState(StreamIn &inStream); + + uint mDrivenWheel; ///< Which wheel on the track is connected to the engine + Array mWheels; ///< Indices of wheels that are inside this track, should include the driven wheel too + float mInertia = 10.0f; ///< Moment of inertia (kg m^2) of the track and its wheels as seen on the driven wheel + float mAngularDamping = 0.5f; ///< Damping factor of track and its wheels: dw/dt = -c * w as seen on the driven wheel + float mMaxBrakeTorque = 15000.0f; ///< How much torque (Nm) the brakes can apply on the driven wheel + float mDifferentialRatio = 6.0f; ///< Ratio between rotation speed of gear box and driven wheel of track +}; + +/// Runtime data for tank tracks +class JPH_EXPORT VehicleTrack : public VehicleTrackSettings +{ +public: + /// Saving state for replay + void SaveState(StateRecorder &inStream) const; + void RestoreState(StateRecorder &inStream); + + float mAngularVelocity = 0.0f; ///< Angular velocity of the driven wheel, will determine the speed of the entire track +}; + +using VehicleTracks = VehicleTrack[(int)ETrackSide::Num]; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTransmission.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTransmission.cpp new file mode 100644 index 000000000000..43336a537f03 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTransmission.cpp @@ -0,0 +1,159 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleTransmissionSettings) +{ + JPH_ADD_ENUM_ATTRIBUTE(VehicleTransmissionSettings, mMode) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mGearRatios) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mReverseGearRatios) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mSwitchTime) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mClutchReleaseTime) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mSwitchLatency) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mShiftUpRPM) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mShiftDownRPM) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mClutchStrength) +} + +void VehicleTransmissionSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mMode); + inStream.Write(mGearRatios); + inStream.Write(mReverseGearRatios); + inStream.Write(mSwitchTime); + inStream.Write(mClutchReleaseTime); + inStream.Write(mSwitchLatency); + inStream.Write(mShiftUpRPM); + inStream.Write(mShiftDownRPM); + inStream.Write(mClutchStrength); +} + +void VehicleTransmissionSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mMode); + inStream.Read(mGearRatios); + inStream.Read(mReverseGearRatios); + inStream.Read(mSwitchTime); + inStream.Read(mClutchReleaseTime); + inStream.Read(mSwitchLatency); + inStream.Read(mShiftUpRPM); + inStream.Read(mShiftDownRPM); + inStream.Read(mClutchStrength); +} + +void VehicleTransmission::Update(float inDeltaTime, float inCurrentRPM, float inForwardInput, bool inCanShiftUp) +{ + // Update current gear and calculate clutch friction + if (mMode == ETransmissionMode::Auto) + { + // Switch gears based on rpm + int old_gear = mCurrentGear; + if (mCurrentGear == 0 // In neutral + || inForwardInput * float(mCurrentGear) < 0.0f) // Changing between forward / reverse + { + // Switch to first gear or reverse depending on input + mCurrentGear = inForwardInput > 0.0f? 1 : (inForwardInput < 0.0f? -1 : 0); + } + else if (mGearSwitchLatencyTimeLeft == 0.0f) // If not in the timout after switching gears + { + if (inCanShiftUp && inCurrentRPM > mShiftUpRPM) + { + if (mCurrentGear < 0) + { + // Shift up, reverse + if (mCurrentGear > -(int)mReverseGearRatios.size()) + mCurrentGear--; + } + else + { + // Shift up, forward + if (mCurrentGear < (int)mGearRatios.size()) + mCurrentGear++; + } + } + else if (inCurrentRPM < mShiftDownRPM) + { + if (mCurrentGear < 0) + { + // Shift down, reverse + int max_gear = inForwardInput != 0.0f? -1 : 0; + if (mCurrentGear < max_gear) + mCurrentGear++; + } + else + { + // Shift down, forward + int min_gear = inForwardInput != 0.0f? 1 : 0; + if (mCurrentGear > min_gear) + mCurrentGear--; + } + } + } + + if (old_gear != mCurrentGear) + { + // We've shifted gear, start switch countdown + mGearSwitchTimeLeft = old_gear != 0? mSwitchTime : 0.0f; + mClutchReleaseTimeLeft = mClutchReleaseTime; + mGearSwitchLatencyTimeLeft = mSwitchLatency; + mClutchFriction = 0.0f; + } + else if (mGearSwitchTimeLeft > 0.0f) + { + // If still switching gears, count down + mGearSwitchTimeLeft = max(0.0f, mGearSwitchTimeLeft - inDeltaTime); + mClutchFriction = 0.0f; + } + else if (mClutchReleaseTimeLeft > 0.0f) + { + // After switching the gears we slowly release the clutch + mClutchReleaseTimeLeft = max(0.0f, mClutchReleaseTimeLeft - inDeltaTime); + mClutchFriction = 1.0f - mClutchReleaseTimeLeft / mClutchReleaseTime; + } + else + { + // Clutch has full friction + mClutchFriction = 1.0f; + + // Count down switch latency + mGearSwitchLatencyTimeLeft = max(0.0f, mGearSwitchLatencyTimeLeft - inDeltaTime); + } + } +} + +float VehicleTransmission::GetCurrentRatio() const +{ + if (mCurrentGear < 0) + return mReverseGearRatios[-mCurrentGear - 1]; + else if (mCurrentGear == 0) + return 0.0f; + else + return mGearRatios[mCurrentGear - 1]; +} + +void VehicleTransmission::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mCurrentGear); + inStream.Write(mClutchFriction); + inStream.Write(mGearSwitchTimeLeft); + inStream.Write(mClutchReleaseTimeLeft); + inStream.Write(mGearSwitchLatencyTimeLeft); +} + +void VehicleTransmission::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mCurrentGear); + inStream.Read(mClutchFriction); + inStream.Read(mGearSwitchTimeLeft); + inStream.Read(mClutchReleaseTimeLeft); + inStream.Read(mGearSwitchLatencyTimeLeft); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTransmission.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTransmission.h new file mode 100644 index 000000000000..3360217abb1f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTransmission.h @@ -0,0 +1,87 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// How gears are shifted +enum class ETransmissionMode : uint8 +{ + Auto, ///< Automatically shift gear up and down + Manual, ///< Manual gear shift (call SetTransmissionInput) +}; + +/// Configuration for the transmission of a vehicle (gear box) +class JPH_EXPORT VehicleTransmissionSettings +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleTransmissionSettings) + +public: + /// Saves the contents in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restores the contents in binary form to inStream. + void RestoreBinaryState(StreamIn &inStream); + + ETransmissionMode mMode = ETransmissionMode::Auto; ///< How to switch gears + Array mGearRatios { 2.66f, 1.78f, 1.3f, 1.0f, 0.74f }; ///< Ratio in rotation rate between engine and gear box, first element is 1st gear, 2nd element 2nd gear etc. + Array mReverseGearRatios { -2.90f }; ///< Ratio in rotation rate between engine and gear box when driving in reverse + float mSwitchTime = 0.5f; ///< How long it takes to switch gears (s), only used in auto mode + float mClutchReleaseTime = 0.3f; ///< How long it takes to release the clutch (go to full friction), only used in auto mode + float mSwitchLatency = 0.5f; ///< How long to wait after releasing the clutch before another switch is attempted (s), only used in auto mode + float mShiftUpRPM = 4000.0f; ///< If RPM of engine is bigger then this we will shift a gear up, only used in auto mode + float mShiftDownRPM = 2000.0f; ///< If RPM of engine is smaller then this we will shift a gear down, only used in auto mode + float mClutchStrength = 10.0f; ///< Strength of the clutch when fully engaged. Total torque a clutch applies is Torque = ClutchStrength * (Velocity Engine - Avg Velocity Wheels At Clutch) (units: k m^2 s^-1) +}; + +/// Runtime data for transmission +class JPH_EXPORT VehicleTransmission : public VehicleTransmissionSettings +{ +public: + /// Set input from driver regarding the transmission (only relevant when transmission is set to manual mode) + /// @param inCurrentGear Current gear, -1 = reverse, 0 = neutral, 1 = 1st gear etc. + /// @param inClutchFriction Value between 0 and 1 indicating how much friction the clutch gives (0 = no friction, 1 = full friction) + void Set(int inCurrentGear, float inClutchFriction) { mCurrentGear = inCurrentGear; mClutchFriction = inClutchFriction; } + + /// Update the current gear and clutch friction if the transmission is in auto mode + /// @param inDeltaTime Time step delta time in s + /// @param inCurrentRPM Current RPM for engine + /// @param inForwardInput Hint if the user wants to drive forward (> 0) or backwards (< 0) + /// @param inCanShiftUp Indicates if we want to allow the transmission to shift up (e.g. pass false if wheels are slipping) + void Update(float inDeltaTime, float inCurrentRPM, float inForwardInput, bool inCanShiftUp); + + /// Current gear, -1 = reverse, 0 = neutral, 1 = 1st gear etc. + int GetCurrentGear() const { return mCurrentGear; } + + /// Value between 0 and 1 indicating how much friction the clutch gives (0 = no friction, 1 = full friction) + float GetClutchFriction() const { return mClutchFriction; } + + /// If the auto box is currently switching gears + bool IsSwitchingGear() const { return mGearSwitchTimeLeft > 0.0f; } + + /// Return the transmission ratio based on the current gear (ratio between engine and differential) + float GetCurrentRatio() const; + + /// Only allow sleeping when the transmission is idle + bool AllowSleep() const { return mGearSwitchTimeLeft <= 0.0f && mClutchReleaseTimeLeft <= 0.0f && mGearSwitchLatencyTimeLeft <= 0.0f; } + + /// Saving state for replay + void SaveState(StateRecorder &inStream) const; + void RestoreState(StateRecorder &inStream); + +private: + int mCurrentGear = 0; ///< Current gear, -1 = reverse, 0 = neutral, 1 = 1st gear etc. + float mClutchFriction = 1.0f; ///< Value between 0 and 1 indicating how much friction the clutch gives (0 = no friction, 1 = full friction) + float mGearSwitchTimeLeft = 0.0f; ///< When switching gears this will be > 0 and will cause the engine to not provide any torque to the wheels for a short time (used for automatic gear switching only) + float mClutchReleaseTimeLeft = 0.0f; ///< After switching gears this will be > 0 and will cause the clutch friction to go from 0 to 1 (used for automatic gear switching only) + float mGearSwitchLatencyTimeLeft = 0.0f; ///< After releasing the clutch this will be > 0 and will prevent another gear switch (used for automatic gear switching only) +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/Wheel.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/Wheel.cpp new file mode 100644 index 000000000000..e43ffb8c9a42 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/Wheel.cpp @@ -0,0 +1,93 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheelSettings) +{ + JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionForcePoint) + JPH_ADD_ATTRIBUTE(WheelSettings, mPosition) + JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionDirection) + JPH_ADD_ATTRIBUTE(WheelSettings, mSteeringAxis) + JPH_ADD_ATTRIBUTE(WheelSettings, mWheelForward) + JPH_ADD_ATTRIBUTE(WheelSettings, mWheelUp) + JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionMinLength) + JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionMaxLength) + JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionPreloadLength) + JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(WheelSettings, mSuspensionSpring.mMode, "mSuspensionSpringMode") + JPH_ADD_ATTRIBUTE_WITH_ALIAS(WheelSettings, mSuspensionSpring.mFrequency, "mSuspensionFrequency") // Renaming attributes to stay compatible with old versions of the library + JPH_ADD_ATTRIBUTE_WITH_ALIAS(WheelSettings, mSuspensionSpring.mDamping, "mSuspensionDamping") + JPH_ADD_ATTRIBUTE(WheelSettings, mRadius) + JPH_ADD_ATTRIBUTE(WheelSettings, mWidth) + JPH_ADD_ATTRIBUTE(WheelSettings, mEnableSuspensionForcePoint) +} + +void WheelSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mSuspensionForcePoint); + inStream.Write(mPosition); + inStream.Write(mSuspensionDirection); + inStream.Write(mSteeringAxis); + inStream.Write(mWheelForward); + inStream.Write(mWheelUp); + inStream.Write(mSuspensionMinLength); + inStream.Write(mSuspensionMaxLength); + inStream.Write(mSuspensionPreloadLength); + mSuspensionSpring.SaveBinaryState(inStream); + inStream.Write(mRadius); + inStream.Write(mWidth); + inStream.Write(mEnableSuspensionForcePoint); +} + +void WheelSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mSuspensionForcePoint); + inStream.Read(mPosition); + inStream.Read(mSuspensionDirection); + inStream.Read(mSteeringAxis); + inStream.Read(mWheelForward); + inStream.Read(mWheelUp); + inStream.Read(mSuspensionMinLength); + inStream.Read(mSuspensionMaxLength); + inStream.Read(mSuspensionPreloadLength); + mSuspensionSpring.RestoreBinaryState(inStream); + inStream.Read(mRadius); + inStream.Read(mWidth); + inStream.Read(mEnableSuspensionForcePoint); +} + +Wheel::Wheel(const WheelSettings &inSettings) : + mSettings(&inSettings), + mSuspensionLength(inSettings.mSuspensionMaxLength) +{ + JPH_ASSERT(inSettings.mSuspensionDirection.IsNormalized()); + JPH_ASSERT(inSettings.mSteeringAxis.IsNormalized()); + JPH_ASSERT(inSettings.mWheelForward.IsNormalized()); + JPH_ASSERT(inSettings.mWheelUp.IsNormalized()); + JPH_ASSERT(inSettings.mSuspensionMinLength >= 0.0f); + JPH_ASSERT(inSettings.mSuspensionMaxLength >= inSettings.mSuspensionMinLength); + JPH_ASSERT(inSettings.mSuspensionPreloadLength >= 0.0f); + JPH_ASSERT(inSettings.mSuspensionSpring.mFrequency > 0.0f); + JPH_ASSERT(inSettings.mSuspensionSpring.mDamping >= 0.0f); + JPH_ASSERT(inSettings.mRadius > 0.0f); + JPH_ASSERT(inSettings.mWidth >= 0.0f); +} + +bool Wheel::SolveLongitudinalConstraintPart(const VehicleConstraint &inConstraint, float inMinImpulse, float inMaxImpulse) +{ + return mLongitudinalPart.SolveVelocityConstraint(*inConstraint.GetVehicleBody(), *mContactBody, -mContactLongitudinal, inMinImpulse, inMaxImpulse); +} + +bool Wheel::SolveLateralConstraintPart(const VehicleConstraint &inConstraint, float inMinImpulse, float inMaxImpulse) +{ + return mLateralPart.SolveVelocityConstraint(*inConstraint.GetVehicleBody(), *mContactBody, -mContactLateral, inMinImpulse, inMaxImpulse); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/Wheel.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/Wheel.h new file mode 100644 index 000000000000..0e92caa7a40d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/Wheel.h @@ -0,0 +1,148 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class VehicleConstraint; + +/// Base class for wheel settings, each VehicleController can implement a derived class of this +class JPH_EXPORT WheelSettings : public SerializableObject, public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, WheelSettings) + +public: + /// Saves the contents in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const; + + /// Restores the contents in binary form to inStream. + virtual void RestoreBinaryState(StreamIn &inStream); + + Vec3 mPosition { 0, 0, 0 }; ///< Attachment point of wheel suspension in local space of the body + Vec3 mSuspensionForcePoint { 0, 0, 0 }; ///< Where tire forces (suspension and traction) are applied, in local space of the body. A good default is the center of the wheel in its neutral pose. See mEnableSuspensionForcePoint. + Vec3 mSuspensionDirection { 0, -1, 0 }; ///< Direction of the suspension in local space of the body, should point down + Vec3 mSteeringAxis { 0, 1, 0 }; ///< Direction of the steering axis in local space of the body, should point up (e.g. for a bike would be -mSuspensionDirection) + Vec3 mWheelUp { 0, 1, 0 }; ///< Up direction when the wheel is in the neutral steering position (usually VehicleConstraintSettings::mUp but can be used to give the wheel camber or for a bike would be -mSuspensionDirection) + Vec3 mWheelForward { 0, 0, 1 }; ///< Forward direction when the wheel is in the neutral steering position (usually VehicleConstraintSettings::mForward but can be used to give the wheel toe, does not need to be perpendicular to mWheelUp) + float mSuspensionMinLength = 0.3f; ///< How long the suspension is in max raised position relative to the attachment point (m) + float mSuspensionMaxLength = 0.5f; ///< How long the suspension is in max droop position relative to the attachment point (m) + float mSuspensionPreloadLength = 0.0f; ///< The natural length (m) of the suspension spring is defined as mSuspensionMaxLength + mSuspensionPreloadLength. Can be used to preload the suspension as the spring is compressed by mSuspensionPreloadLength when the suspension is in max droop position. Note that this means when the vehicle touches the ground there is a discontinuity so it will also make the vehicle more bouncy as we're updating with discrete time steps. + SpringSettings mSuspensionSpring { ESpringMode::FrequencyAndDamping, 1.5f, 0.5f }; ///< Settings for the suspension spring + float mRadius = 0.3f; ///< Radius of the wheel (m) + float mWidth = 0.1f; ///< Width of the wheel (m) + bool mEnableSuspensionForcePoint = false; ///< Enables mSuspensionForcePoint, if disabled, the forces are applied at the collision contact point. This leads to a more accurate simulation when interacting with dynamic objects but makes the vehicle less stable. When setting this to true, all forces will be applied to a fixed point on the vehicle body. +}; + +/// Base class for runtime data for a wheel, each VehicleController can implement a derived class of this +class JPH_EXPORT Wheel : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor / destructor + explicit Wheel(const WheelSettings &inSettings); + virtual ~Wheel() = default; + + /// Get settings for the wheel + const WheelSettings * GetSettings() const { return mSettings; } + + /// Get the angular velocity (rad/s) for this wheel, note that positive means the wheel is rotating such that the car moves forward + float GetAngularVelocity() const { return mAngularVelocity; } + + /// Update the angular velocity (rad/s) + void SetAngularVelocity(float inVel) { mAngularVelocity = inVel; } + + /// Get the current rotation angle of the wheel in radians [0, 2 pi] + float GetRotationAngle() const { return mAngle; } + + /// Set the current rotation angle of the wheel in radians [0, 2 pi] + void SetRotationAngle(float inAngle) { mAngle = inAngle; } + + /// Get the current steer angle of the wheel in radians [-pi, pi], positive is to the left + float GetSteerAngle() const { return mSteerAngle; } + + /// Set the current steer angle of the wheel in radians [-pi, pi] + void SetSteerAngle(float inAngle) { mSteerAngle = inAngle; } + + /// Returns true if the wheel is touching an object + inline bool HasContact() const { return !mContactBodyID.IsInvalid(); } + + /// Returns the body ID of the body that this wheel is touching + BodyID GetContactBodyID() const { return mContactBodyID; } + + /// Returns the sub shape ID where we're contacting the body + SubShapeID GetContactSubShapeID() const { return mContactSubShapeID; } + + /// Returns the current contact position in world space (note by the time you call this the vehicle has moved) + RVec3 GetContactPosition() const { JPH_ASSERT(HasContact()); return mContactPosition; } + + /// Velocity of the contact point (m / s, not relative to the wheel but in world space) + Vec3 GetContactPointVelocity() const { JPH_ASSERT(HasContact()); return mContactPointVelocity; } + + /// Returns the current contact normal in world space (note by the time you call this the vehicle has moved) + Vec3 GetContactNormal() const { JPH_ASSERT(HasContact()); return mContactNormal; } + + /// Returns longitudinal direction (direction along the wheel relative to floor) in world space (note by the time you call this the vehicle has moved) + Vec3 GetContactLongitudinal() const { JPH_ASSERT(HasContact()); return mContactLongitudinal; } + + /// Returns lateral direction (sideways direction) in world space (note by the time you call this the vehicle has moved) + Vec3 GetContactLateral() const { JPH_ASSERT(HasContact()); return mContactLateral; } + + /// Get the length of the suspension for a wheel (m) relative to the suspension attachment point (hard point) + float GetSuspensionLength() const { return mSuspensionLength; } + + /// Check if the suspension hit its upper limit + bool HasHitHardPoint() const { return mSuspensionMaxUpPart.IsActive(); } + + /// Get the total impulse (N s) that was applied by the suspension + float GetSuspensionLambda() const { return mSuspensionPart.GetTotalLambda() + mSuspensionMaxUpPart.GetTotalLambda(); } + + /// Get total impulse (N s) applied along the forward direction of the wheel + float GetLongitudinalLambda() const { return mLongitudinalPart.GetTotalLambda(); } + + /// Get total impulse (N s) applied along the sideways direction of the wheel + float GetLateralLambda() const { return mLateralPart.GetTotalLambda(); } + + /// Internal function that should only be called by the controller. Used to apply impulses in the forward direction of the vehicle. + bool SolveLongitudinalConstraintPart(const VehicleConstraint &inConstraint, float inMinImpulse, float inMaxImpulse); + + /// Internal function that should only be called by the controller. Used to apply impulses in the sideways direction of the vehicle. + bool SolveLateralConstraintPart(const VehicleConstraint &inConstraint, float inMinImpulse, float inMaxImpulse); + +protected: + friend class VehicleConstraint; + + RefConst mSettings; ///< Configuration settings for this wheel + BodyID mContactBodyID; ///< ID of body for ground + SubShapeID mContactSubShapeID; ///< Sub shape ID for ground + Body * mContactBody = nullptr; ///< Body for ground + float mSuspensionLength; ///< Current length of the suspension + RVec3 mContactPosition; ///< Position of the contact point between wheel and ground + Vec3 mContactPointVelocity; ///< Velocity of the contact point (m / s, not relative to the wheel but in world space) + Vec3 mContactNormal; ///< Normal of the contact point between wheel and ground + Vec3 mContactLongitudinal; ///< Vector perpendicular to normal in the forward direction + Vec3 mContactLateral; ///< Vector perpendicular to normal and longitudinal direction in the right direction + Real mAxlePlaneConstant; ///< Constant for the contact plane of the axle, defined as ContactNormal . (WorldSpaceSuspensionPoint + SuspensionLength * WorldSpaceSuspensionDirection) + float mAntiRollBarImpulse = 0.0f; ///< Amount of impulse applied to the suspension from the anti-rollbars + + float mSteerAngle = 0.0f; ///< Rotation around the suspension direction, positive is to the left + float mAngularVelocity = 0.0f; ///< Rotation speed of wheel, positive when the wheels cause the vehicle to move forwards (rad/s) + float mAngle = 0.0f; ///< Current rotation of the wheel (rad, [0, 2 pi]) + + AxisConstraintPart mSuspensionPart; ///< Controls movement up/down along the contact normal + AxisConstraintPart mSuspensionMaxUpPart; ///< Adds a hard limit when reaching the minimal suspension length + AxisConstraintPart mLongitudinalPart; ///< Controls movement forward/backward + AxisConstraintPart mLateralPart; ///< Controls movement sideways (slip) +}; + +using Wheels = Array; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.cpp new file mode 100644 index 000000000000..8e4a71a4fb85 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.cpp @@ -0,0 +1,845 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +//#define JPH_TRACE_VEHICLE_STATS + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheeledVehicleControllerSettings) +{ + JPH_ADD_BASE_CLASS(WheeledVehicleControllerSettings, VehicleControllerSettings) + + JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mEngine) + JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mTransmission) + JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mDifferentials) + JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mDifferentialLimitedSlipRatio) +} + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheelSettingsWV) +{ + JPH_ADD_ATTRIBUTE(WheelSettingsWV, mInertia) + JPH_ADD_ATTRIBUTE(WheelSettingsWV, mAngularDamping) + JPH_ADD_ATTRIBUTE(WheelSettingsWV, mMaxSteerAngle) + JPH_ADD_ATTRIBUTE(WheelSettingsWV, mLongitudinalFriction) + JPH_ADD_ATTRIBUTE(WheelSettingsWV, mLateralFriction) + JPH_ADD_ATTRIBUTE(WheelSettingsWV, mMaxBrakeTorque) + JPH_ADD_ATTRIBUTE(WheelSettingsWV, mMaxHandBrakeTorque) +} + +WheelSettingsWV::WheelSettingsWV() +{ + mLongitudinalFriction.Reserve(3); + mLongitudinalFriction.AddPoint(0.0f, 0.0f); + mLongitudinalFriction.AddPoint(0.06f, 1.2f); + mLongitudinalFriction.AddPoint(0.2f, 1.0f); + + mLateralFriction.Reserve(3); + mLateralFriction.AddPoint(0.0f, 0.0f); + mLateralFriction.AddPoint(3.0f, 1.2f); + mLateralFriction.AddPoint(20.0f, 1.0f); +} + +void WheelSettingsWV::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mInertia); + inStream.Write(mAngularDamping); + inStream.Write(mMaxSteerAngle); + mLongitudinalFriction.SaveBinaryState(inStream); + mLateralFriction.SaveBinaryState(inStream); + inStream.Write(mMaxBrakeTorque); + inStream.Write(mMaxHandBrakeTorque); +} + +void WheelSettingsWV::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mInertia); + inStream.Read(mAngularDamping); + inStream.Read(mMaxSteerAngle); + mLongitudinalFriction.RestoreBinaryState(inStream); + mLateralFriction.RestoreBinaryState(inStream); + inStream.Read(mMaxBrakeTorque); + inStream.Read(mMaxHandBrakeTorque); +} + +WheelWV::WheelWV(const WheelSettingsWV &inSettings) : + Wheel(inSettings) +{ + JPH_ASSERT(inSettings.mInertia >= 0.0f); + JPH_ASSERT(inSettings.mAngularDamping >= 0.0f); + JPH_ASSERT(abs(inSettings.mMaxSteerAngle) <= 0.5f * JPH_PI); + JPH_ASSERT(inSettings.mMaxBrakeTorque >= 0.0f); + JPH_ASSERT(inSettings.mMaxHandBrakeTorque >= 0.0f); +} + +void WheelWV::Update(uint inWheelIndex, float inDeltaTime, const VehicleConstraint &inConstraint) +{ + const WheelSettingsWV *settings = GetSettings(); + + // Angular damping: dw/dt = -c * w + // Solution: w(t) = w(0) * e^(-c * t) or w2 = w1 * e^(-c * dt) + // Taylor expansion of e^(-c * dt) = 1 - c * dt + ... + // Since dt is usually in the order of 1/60 and c is a low number too this approximation is good enough + mAngularVelocity *= max(0.0f, 1.0f - settings->mAngularDamping * inDeltaTime); + + // Update rotation of wheel + mAngle = fmod(mAngle + mAngularVelocity * inDeltaTime, 2.0f * JPH_PI); + + if (mContactBody != nullptr) + { + const Body *body = inConstraint.GetVehicleBody(); + + // Calculate relative velocity between wheel contact point and floor + Vec3 relative_velocity = body->GetPointVelocity(mContactPosition) - mContactPointVelocity; + + // Cancel relative velocity in the normal plane + relative_velocity -= mContactNormal.Dot(relative_velocity) * mContactNormal; + float relative_longitudinal_velocity = relative_velocity.Dot(mContactLongitudinal); + + // Calculate longitudinal friction based on difference between velocity of rolling wheel and drive surface + float relative_longitudinal_velocity_denom = Sign(relative_longitudinal_velocity) * max(1.0e-3f, abs(relative_longitudinal_velocity)); // Ensure we don't divide by zero + mLongitudinalSlip = abs((mAngularVelocity * settings->mRadius - relative_longitudinal_velocity) / relative_longitudinal_velocity_denom); + float longitudinal_slip_friction = settings->mLongitudinalFriction.GetValue(mLongitudinalSlip); + + // Calculate lateral friction based on slip angle + float relative_velocity_len = relative_velocity.Length(); + mLateralSlip = relative_velocity_len < 1.0e-3f ? 0.0f : ACos(abs(relative_longitudinal_velocity) / relative_velocity_len); + float lateral_slip_angle = RadiansToDegrees(mLateralSlip); + float lateral_slip_friction = settings->mLateralFriction.GetValue(lateral_slip_angle); + + // Tire friction + VehicleConstraint::CombineFunction combine_friction = inConstraint.GetCombineFriction(); + mCombinedLongitudinalFriction = longitudinal_slip_friction; + mCombinedLateralFriction = lateral_slip_friction; + combine_friction(inWheelIndex, mCombinedLongitudinalFriction, mCombinedLateralFriction, *mContactBody, mContactSubShapeID); + } + else + { + // No collision + mLongitudinalSlip = 0.0f; + mLateralSlip = 0.0f; + mCombinedLongitudinalFriction = mCombinedLateralFriction = 0.0f; + } +} + +VehicleController *WheeledVehicleControllerSettings::ConstructController(VehicleConstraint &inConstraint) const +{ + return new WheeledVehicleController(*this, inConstraint); +} + +void WheeledVehicleControllerSettings::SaveBinaryState(StreamOut &inStream) const +{ + mEngine.SaveBinaryState(inStream); + + mTransmission.SaveBinaryState(inStream); + + uint32 num_differentials = (uint32)mDifferentials.size(); + inStream.Write(num_differentials); + for (const VehicleDifferentialSettings &d : mDifferentials) + d.SaveBinaryState(inStream); + + inStream.Write(mDifferentialLimitedSlipRatio); +} + +void WheeledVehicleControllerSettings::RestoreBinaryState(StreamIn &inStream) +{ + mEngine.RestoreBinaryState(inStream); + + mTransmission.RestoreBinaryState(inStream); + + uint32 num_differentials = 0; + inStream.Read(num_differentials); + mDifferentials.resize(num_differentials); + for (VehicleDifferentialSettings &d : mDifferentials) + d.RestoreBinaryState(inStream); + + inStream.Read(mDifferentialLimitedSlipRatio); +} + +WheeledVehicleController::WheeledVehicleController(const WheeledVehicleControllerSettings &inSettings, VehicleConstraint &inConstraint) : + VehicleController(inConstraint) +{ + // Copy engine settings + static_cast(mEngine) = inSettings.mEngine; + JPH_ASSERT(inSettings.mEngine.mMinRPM >= 0.0f); + JPH_ASSERT(inSettings.mEngine.mMinRPM <= inSettings.mEngine.mMaxRPM); + mEngine.SetCurrentRPM(mEngine.mMinRPM); + + // Copy transmission settings + static_cast(mTransmission) = inSettings.mTransmission; +#ifdef JPH_ENABLE_ASSERTS + for (float r : inSettings.mTransmission.mGearRatios) + JPH_ASSERT(r > 0.0f); + for (float r : inSettings.mTransmission.mReverseGearRatios) + JPH_ASSERT(r < 0.0f); +#endif // JPH_ENABLE_ASSERTS + JPH_ASSERT(inSettings.mTransmission.mSwitchTime >= 0.0f); + JPH_ASSERT(inSettings.mTransmission.mShiftDownRPM > 0.0f); + JPH_ASSERT(inSettings.mTransmission.mMode != ETransmissionMode::Auto || inSettings.mTransmission.mShiftUpRPM < inSettings.mEngine.mMaxRPM); + JPH_ASSERT(inSettings.mTransmission.mShiftUpRPM > inSettings.mTransmission.mShiftDownRPM); + JPH_ASSERT(inSettings.mTransmission.mClutchStrength > 0.0f); + + // Copy differential settings + mDifferentials.resize(inSettings.mDifferentials.size()); + for (uint i = 0; i < mDifferentials.size(); ++i) + { + const VehicleDifferentialSettings &d = inSettings.mDifferentials[i]; + mDifferentials[i] = d; + JPH_ASSERT(d.mDifferentialRatio > 0.0f); + JPH_ASSERT(d.mLeftRightSplit >= 0.0f && d.mLeftRightSplit <= 1.0f); + JPH_ASSERT(d.mEngineTorqueRatio >= 0.0f); + JPH_ASSERT(d.mLimitedSlipRatio > 1.0f); + } + + mDifferentialLimitedSlipRatio = inSettings.mDifferentialLimitedSlipRatio; + JPH_ASSERT(mDifferentialLimitedSlipRatio > 1.0f); +} + +float WheeledVehicleController::GetWheelSpeedAtClutch() const +{ + float wheel_speed_at_clutch = 0.0f; + int num_driven_wheels = 0; + for (const VehicleDifferentialSettings &d : mDifferentials) + { + int wheels[] = { d.mLeftWheel, d.mRightWheel }; + for (int w : wheels) + if (w >= 0) + { + wheel_speed_at_clutch += mConstraint.GetWheel(w)->GetAngularVelocity() * d.mDifferentialRatio; + num_driven_wheels++; + } + } + return wheel_speed_at_clutch / float(num_driven_wheels) * VehicleEngine::cAngularVelocityToRPM * mTransmission.GetCurrentRatio(); +} + +bool WheeledVehicleController::AllowSleep() const +{ + return mForwardInput == 0.0f // No user input + && mTransmission.AllowSleep() // Transmission is not shifting + && mEngine.AllowSleep(); // Engine is idling +} + +void WheeledVehicleController::PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) +{ + JPH_PROFILE_FUNCTION(); + +#ifdef JPH_TRACE_VEHICLE_STATS + static bool sTracedHeader = false; + if (!sTracedHeader) + { + Trace("Time, ForwardInput, Gear, ClutchFriction, EngineRPM, WheelRPM, Velocity (km/h)"); + sTracedHeader = true; + } + static float sTime = 0.0f; + sTime += inDeltaTime; + Trace("%.3f, %.1f, %d, %.1f, %.1f, %.1f, %.1f", sTime, mForwardInput, mTransmission.GetCurrentGear(), mTransmission.GetClutchFriction(), mEngine.GetCurrentRPM(), GetWheelSpeedAtClutch(), mConstraint.GetVehicleBody()->GetLinearVelocity().Length() * 3.6f); +#endif // JPH_TRACE_VEHICLE_STATS + + for (Wheel *w_base : mConstraint.GetWheels()) + { + WheelWV *w = static_cast(w_base); + + // Set steering angle + w->SetSteerAngle(-mRightInput * w->GetSettings()->mMaxSteerAngle); + } +} + +void WheeledVehicleController::PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) +{ + JPH_PROFILE_FUNCTION(); + + // Remember old RPM so we can detect if we're increasing or decreasing + float old_engine_rpm = mEngine.GetCurrentRPM(); + + Wheels &wheels = mConstraint.GetWheels(); + + // Update wheel angle, do this before applying torque to the wheels (as friction will slow them down again) + for (uint wheel_index = 0, num_wheels = (uint)wheels.size(); wheel_index < num_wheels; ++wheel_index) + { + WheelWV *w = static_cast(wheels[wheel_index]); + w->Update(wheel_index, inDeltaTime, mConstraint); + } + + // In auto transmission mode, don't accelerate the engine when switching gears + float forward_input = abs(mForwardInput); + if (mTransmission.mMode == ETransmissionMode::Auto) + forward_input *= mTransmission.GetClutchFriction(); + + // Apply engine damping + mEngine.ApplyDamping(inDeltaTime); + + // Calculate engine torque + float engine_torque = mEngine.GetTorque(forward_input); + + // Define a struct that contains information about driven differentials (i.e. that have wheels connected) + struct DrivenDifferential + { + const VehicleDifferentialSettings * mDifferential; + float mAngularVelocity; + float mClutchToDifferentialTorqueRatio; + float mTempTorqueFactor; + }; + + // Collect driven differentials and their speeds + Array driven_differentials; + driven_differentials.reserve(mDifferentials.size()); + float differential_omega_min = FLT_MAX, differential_omega_max = 0.0f; + for (const VehicleDifferentialSettings &d : mDifferentials) + { + float avg_omega = 0.0f; + int avg_omega_denom = 0; + int indices[] = { d.mLeftWheel, d.mRightWheel }; + for (int idx : indices) + if (idx != -1) + { + avg_omega += wheels[idx]->GetAngularVelocity(); + avg_omega_denom++; + } + + if (avg_omega_denom > 0) + { + avg_omega = abs(avg_omega * d.mDifferentialRatio / float(avg_omega_denom)); // ignoring that the differentials may be rotating in different directions + driven_differentials.push_back({ &d, avg_omega, d.mEngineTorqueRatio, 0 }); + + // Remember min and max velocity + differential_omega_min = min(differential_omega_min, avg_omega); + differential_omega_max = max(differential_omega_max, avg_omega); + } + } + + if (mDifferentialLimitedSlipRatio < FLT_MAX // Limited slip differential needs to be turned on + && differential_omega_max > differential_omega_min) // There needs to be a velocity difference + { + // Calculate factor based on relative speed of a differential + float sum_factor = 0.0f; + for (DrivenDifferential &d : driven_differentials) + { + // Differential with max velocity gets factor 0, differential with min velocity 1 + d.mTempTorqueFactor = (differential_omega_max - d.mAngularVelocity) / (differential_omega_max - differential_omega_min); + sum_factor += d.mTempTorqueFactor; + } + + // Normalize the result + for (DrivenDifferential &d : driven_differentials) + d.mTempTorqueFactor /= sum_factor; + + // Prevent div by zero + differential_omega_min = max(1.0e-3f, differential_omega_min); + differential_omega_max = max(1.0e-3f, differential_omega_max); + + // Map into a value that is 0 when the wheels are turning at an equal rate and 1 when the wheels are turning at mDifferentialLimitedSlipRatio + float alpha = min((differential_omega_max / differential_omega_min - 1.0f) / (mDifferentialLimitedSlipRatio - 1.0f), 1.0f); + JPH_ASSERT(alpha >= 0.0f); + float one_min_alpha = 1.0f - alpha; + + // Update torque ratio for all differentials + for (DrivenDifferential &d : driven_differentials) + d.mClutchToDifferentialTorqueRatio = one_min_alpha * d.mClutchToDifferentialTorqueRatio + alpha * d.mTempTorqueFactor; + } + +#ifdef JPH_ENABLE_ASSERTS + // Assert the values add up to 1 + float sum_torque_factors = 0.0f; + for (DrivenDifferential &d : driven_differentials) + sum_torque_factors += d.mClutchToDifferentialTorqueRatio; + JPH_ASSERT(abs(sum_torque_factors - 1.0f) < 1.0e-6f); +#endif // JPH_ENABLE_ASSERTS + + // Define a struct that collects information about the wheels that connect to the engine + struct DrivenWheel + { + WheelWV * mWheel; + float mClutchToWheelRatio; + float mClutchToWheelTorqueRatio; + float mEstimatedAngularImpulse; + }; + Array driven_wheels; + driven_wheels.reserve(wheels.size()); + + // Collect driven wheels + float transmission_ratio = mTransmission.GetCurrentRatio(); + for (const DrivenDifferential &dd : driven_differentials) + { + VehicleDifferentialSettings d = *dd.mDifferential; + + WheelWV *wl = d.mLeftWheel != -1? static_cast(wheels[d.mLeftWheel]) : nullptr; + WheelWV *wr = d.mRightWheel != -1? static_cast(wheels[d.mRightWheel]) : nullptr; + + float clutch_to_wheel_ratio = transmission_ratio * d.mDifferentialRatio; + + if (wl != nullptr && wr != nullptr) + { + // Calculate torque ratio + float ratio_l, ratio_r; + d.CalculateTorqueRatio(wl->GetAngularVelocity(), wr->GetAngularVelocity(), ratio_l, ratio_r); + + // Add both wheels + driven_wheels.push_back({ wl, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio * ratio_l, 0.0f }); + driven_wheels.push_back({ wr, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio * ratio_r, 0.0f }); + } + else if (wl != nullptr) + { + // Only left wheel, all power to left + driven_wheels.push_back({ wl, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio, 0.0f }); + } + else if (wr != nullptr) + { + // Only right wheel, all power to right + driven_wheels.push_back({ wr, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio, 0.0f }); + } + } + + bool solved = false; + if (!driven_wheels.empty()) + { + // Define the torque at the clutch at time t as: + // + // tc(t):=S*(we(t)-sum(R(j)*ww(j,t),j,1,N)/N) + // + // Where: + // S is the total strength of clutch (= friction * strength) + // we(t) is the engine angular velocity at time t + // R(j) is the total gear ratio of clutch to wheel for wheel j + // ww(j,t) is the angular velocity of wheel j at time t + // N is the amount of wheels + // + // The torque that increases the engine angular velocity at time t is: + // + // te(t):=TE-tc(t) + // + // Where: + // TE is the torque delivered by the engine + // + // The torque that increases the wheel angular velocity for wheel i at time t is: + // + // tw(i,t):=TW(i)+R(i)*F(i)*tc(t) + // + // Where: + // TW(i) is the torque applied to the wheel outside of the engine (brake + torque due to friction with the ground) + // F(i) is the fraction of the engine torque applied from engine to wheel i + // + // Because the angular acceleration and torque are connected through: Torque = I * dw/dt + // + // We have the angular acceleration of the engine at time t: + // + // ddt_we(t):=te(t)/Ie + // + // Where: + // Ie is the inertia of the engine + // + // We have the angular acceleration of wheel i at time t: + // + // ddt_ww(i,t):=tw(i,t)/Iw(i) + // + // Where: + // Iw(i) is the inertia of wheel i + // + // We could take a simple Euler step to calculate the resulting accelerations but because the system is very stiff this turns out to be unstable, so we need to use implicit Euler instead: + // + // we(t+dt)=we(t)+dt*ddt_we(t+dt) + // + // and: + // + // ww(i,t+dt)=ww(i,t)+dt*ddt_ww(i,t+dt) + // + // Expanding both equations (the equations above are in wxMaxima format and this can easily be done by expand(%)): + // + // For wheel: + // + // ww(i,t+dt) + (S*dt*F(i)*R(i)*sum(R(j)*ww(j,t+dt),j,1,N))/(N*Iw(i)) - (S*dt*F(i)*R(i)*we(t+dt))/Iw(i) = ww(i,t)+(dt*TW(i))/Iw(i) + // + // For engine: + // + // we(t+dt) + (S*dt*we(t+dt))/Ie - (S*dt*sum(R(j)*ww(j,t+dt),j,1,N))/(Ie*N) = we(t)+(TE*dt)/Ie + // + // Defining a vector w(t) = (ww(1, t), ww(2, t), ..., ww(N, t), we(t)) we can write both equations as a matrix multiplication: + // + // a * w(t + dt) = b + // + // We then invert the matrix to get the new angular velocities. + + // Dimension of matrix is N + 1 + int n = (int)driven_wheels.size() + 1; + + // Last column of w is for the engine angular velocity + int engine = n - 1; + + // Define a and b + DynMatrix a(n, n); + DynMatrix b(n, 1); + + // Get number of driven wheels as a float + float num_driven_wheels_float = float(driven_wheels.size()); + + // Angular velocity of engine + float w_engine = mEngine.GetAngularVelocity(); + + // Calculate the total strength of the clutch + float clutch_strength = transmission_ratio != 0.0f? mTransmission.GetClutchFriction() * mTransmission.mClutchStrength : 0.0f; + + // dt / Ie + float dt_div_ie = inDeltaTime / mEngine.mInertia; + + // Calculate scale factor for impulses based on previous delta time + float impulse_scale = mPreviousDeltaTime > 0.0f? inDeltaTime / mPreviousDeltaTime : 0.0f; + + // Iterate the rows for the wheels + for (int i = 0; i < (int)driven_wheels.size(); ++i) + { + DrivenWheel &w_i = driven_wheels[i]; + const WheelSettingsWV *settings = w_i.mWheel->GetSettings(); + + // Get wheel inertia + float inertia = settings->mInertia; + + // S * R(i) + float s_r = clutch_strength * w_i.mClutchToWheelRatio; + + // dt * S * R(i) * F(i) / Iw + float dt_s_r_f_div_iw = inDeltaTime * s_r * w_i.mClutchToWheelTorqueRatio / inertia; + + // Fill in the columns of a for wheel j + for (int j = 0; j < (int)driven_wheels.size(); ++j) + { + const DrivenWheel &w_j = driven_wheels[j]; + a(i, j) = dt_s_r_f_div_iw * w_j.mClutchToWheelRatio / num_driven_wheels_float; + } + + // Add ww(i, t+dt) + a(i, i) += 1.0f; + + // Add the column for the engine + a(i, engine) = -dt_s_r_f_div_iw; + + // Calculate external angular impulse operating on the wheel: TW(i) * dt + float dt_tw = 0.0f; + + // Combine brake with hand brake torque + float brake_torque = mBrakeInput * settings->mMaxBrakeTorque + mHandBrakeInput * settings->mMaxHandBrakeTorque; + if (brake_torque > 0.0f) + { + // We're braking + // Calculate brake angular impulse + float sign; + if (w_i.mWheel->GetAngularVelocity() != 0.0f) + sign = Sign(w_i.mWheel->GetAngularVelocity()); + else + sign = Sign(mTransmission.GetCurrentRatio()); // When wheels have locked up use the transmission ratio to determine the sign + dt_tw = sign * inDeltaTime * brake_torque; + } + + if (w_i.mWheel->HasContact()) + { + // We have wheel contact with the floor + // Note that we don't know the torque due to the ground contact yet, so we use the impulse applied from the last frame to estimate it + // Wheel torque TW = force * radius = lambda / dt * radius + dt_tw += impulse_scale * w_i.mWheel->GetLongitudinalLambda() * settings->mRadius; + } + + w_i.mEstimatedAngularImpulse = dt_tw; + + // Fill in the constant b = ww(i,t)+(dt*TW(i))/Iw(i) + b(i, 0) = w_i.mWheel->GetAngularVelocity() - dt_tw / inertia; + + // To avoid looping over the wheels again, we also fill in the wheel columns of the engine row here + a(engine, i) = -dt_div_ie * s_r / num_driven_wheels_float; + } + + // Finalize the engine row + a(engine, engine) = (1.0f + dt_div_ie * clutch_strength); + b(engine, 0) = w_engine + dt_div_ie * engine_torque; + + // Solve the linear equation + if (GaussianElimination(a, b)) + { + // Update the angular velocities for the wheels + for (int i = 0; i < (int)driven_wheels.size(); ++i) + { + DrivenWheel &w_i = driven_wheels[i]; + const WheelSettingsWV *settings = w_i.mWheel->GetSettings(); + + // Get solved wheel angular velocity + float angular_velocity = b(i, 0); + + // We estimated TW and applied it in the equation above, but we haven't actually applied this torque yet so we undo it here. + // It will be applied when we solve the actual braking / the constraints with the floor. + angular_velocity += w_i.mEstimatedAngularImpulse / settings->mInertia; + + // Update angular velocity + w_i.mWheel->SetAngularVelocity(angular_velocity); + } + + // Update the engine RPM + mEngine.SetCurrentRPM(b(engine, 0) * VehicleEngine::cAngularVelocityToRPM); + + // The speeds have been solved + solved = true; + } + else + { + JPH_ASSERT(false, "New engine/wheel speeds could not be calculated!"); + } + } + + if (!solved) + { + // Engine not connected to wheels, apply all torque to engine rotation + mEngine.ApplyTorque(engine_torque, inDeltaTime); + } + + // Calculate if any of the wheels are slipping, this is used to prevent gear switching + bool wheels_slipping = false; + for (const DrivenWheel &w : driven_wheels) + wheels_slipping |= w.mClutchToWheelTorqueRatio > 0.0f && (!w.mWheel->HasContact() || w.mWheel->mLongitudinalSlip > 0.1f); + + // Only allow shifting up when we're not slipping and we're increasing our RPM. + // After a jump, we have a very high engine RPM but once we hit the ground the RPM should be decreasing and we don't want to shift up + // during that time. + bool can_shift_up = !wheels_slipping && mEngine.GetCurrentRPM() >= old_engine_rpm; + + // Update transmission + mTransmission.Update(inDeltaTime, mEngine.GetCurrentRPM(), mForwardInput, can_shift_up); + + // Braking + for (Wheel *w_base : wheels) + { + WheelWV *w = static_cast(w_base); + const WheelSettingsWV *settings = w->GetSettings(); + + // Combine brake with hand brake torque + float brake_torque = mBrakeInput * settings->mMaxBrakeTorque + mHandBrakeInput * settings->mMaxHandBrakeTorque; + if (brake_torque > 0.0f) + { + // Calculate how much torque is needed to stop the wheels from rotating in this time step + float brake_torque_to_lock_wheels = abs(w->GetAngularVelocity()) * settings->mInertia / inDeltaTime; + if (brake_torque > brake_torque_to_lock_wheels) + { + // Wheels are locked + w->SetAngularVelocity(0.0f); + w->mBrakeImpulse = (brake_torque - brake_torque_to_lock_wheels) * inDeltaTime / settings->mRadius; + } + else + { + // Slow down the wheels + w->ApplyTorque(-Sign(w->GetAngularVelocity()) * brake_torque, inDeltaTime); + w->mBrakeImpulse = 0.0f; + } + } + else + { + // Not braking + w->mBrakeImpulse = 0.0f; + } + } + + // Remember previous delta time so we can scale the impulses correctly + mPreviousDeltaTime = inDeltaTime; +} + +bool WheeledVehicleController::SolveLongitudinalAndLateralConstraints(float inDeltaTime) +{ + bool impulse = false; + + float *max_lateral_friction_impulse = (float *)JPH_STACK_ALLOC(mConstraint.GetWheels().size() * sizeof(float)); + + uint wheel_index = 0; + for (Wheel *w_base : mConstraint.GetWheels()) + { + if (w_base->HasContact()) + { + WheelWV *w = static_cast(w_base); + const WheelSettingsWV *settings = w->GetSettings(); + + // Calculate max impulse that we can apply on the ground + float max_longitudinal_friction_impulse; + mTireMaxImpulseCallback(wheel_index, + max_longitudinal_friction_impulse, max_lateral_friction_impulse[wheel_index], w->GetSuspensionLambda(), + w->mCombinedLongitudinalFriction, w->mCombinedLateralFriction, w->mLongitudinalSlip, w->mLateralSlip, inDeltaTime); + + // Calculate relative velocity between wheel contact point and floor in longitudinal direction + Vec3 relative_velocity = mConstraint.GetVehicleBody()->GetPointVelocity(w->GetContactPosition()) - w->GetContactPointVelocity(); + float relative_longitudinal_velocity = relative_velocity.Dot(w->GetContactLongitudinal()); + + // Calculate brake force to apply + float min_longitudinal_impulse, max_longitudinal_impulse; + if (w->mBrakeImpulse != 0.0f) + { + // Limit brake force by max tire friction + float brake_impulse = min(w->mBrakeImpulse, max_longitudinal_friction_impulse); + + // Check which direction the brakes should be applied (we don't want to apply an impulse that would accelerate the vehicle) + if (relative_longitudinal_velocity >= 0.0f) + { + min_longitudinal_impulse = -brake_impulse; + max_longitudinal_impulse = 0.0f; + } + else + { + min_longitudinal_impulse = 0.0f; + max_longitudinal_impulse = brake_impulse; + } + + // Longitudinal impulse, note that we assume that once the wheels are locked that the brakes have more than enough torque to keep the wheels locked so we exclude any rotation deltas + impulse |= w->SolveLongitudinalConstraintPart(mConstraint, min_longitudinal_impulse, max_longitudinal_impulse); + } + else + { + // Assume we want to apply an angular impulse that makes the delta velocity between wheel and ground zero in one time step, calculate the amount of linear impulse needed to do that + float desired_angular_velocity = relative_longitudinal_velocity / settings->mRadius; + float linear_impulse = (w->GetAngularVelocity() - desired_angular_velocity) * settings->mInertia / settings->mRadius; + + // Limit the impulse by max tire friction + float prev_lambda = w->GetLongitudinalLambda(); + min_longitudinal_impulse = max_longitudinal_impulse = Clamp(prev_lambda + linear_impulse, -max_longitudinal_friction_impulse, max_longitudinal_friction_impulse); + + // Longitudinal impulse + impulse |= w->SolveLongitudinalConstraintPart(mConstraint, min_longitudinal_impulse, max_longitudinal_impulse); + + // Update the angular velocity of the wheels according to the lambda that was applied + w->SetAngularVelocity(w->GetAngularVelocity() - (w->GetLongitudinalLambda() - prev_lambda) * settings->mRadius / settings->mInertia); + } + } + ++wheel_index; + } + + wheel_index = 0; + for (Wheel *w_base : mConstraint.GetWheels()) + { + if (w_base->HasContact()) + { + WheelWV *w = static_cast(w_base); + + // Lateral friction + float max_lateral_impulse = max_lateral_friction_impulse[wheel_index]; + impulse |= w->SolveLateralConstraintPart(mConstraint, -max_lateral_impulse, max_lateral_impulse); + } + ++wheel_index; + } + + return impulse; +} + +#ifdef JPH_DEBUG_RENDERER + +void WheeledVehicleController::Draw(DebugRenderer *inRenderer) const +{ + float constraint_size = mConstraint.GetDrawConstraintSize(); + + // Draw RPM + Body *body = mConstraint.GetVehicleBody(); + Vec3 rpm_meter_up = body->GetRotation() * mConstraint.GetLocalUp(); + RVec3 rpm_meter_pos = body->GetPosition() + body->GetRotation() * mRPMMeterPosition; + Vec3 rpm_meter_fwd = body->GetRotation() * mConstraint.GetLocalForward(); + mEngine.DrawRPM(inRenderer, rpm_meter_pos, rpm_meter_fwd, rpm_meter_up, mRPMMeterSize, mTransmission.mShiftDownRPM, mTransmission.mShiftUpRPM); + + if (mTransmission.GetCurrentRatio() != 0.0f) + { + // Calculate average wheel speed at clutch + float wheel_speed_at_clutch = GetWheelSpeedAtClutch(); + + // Draw the average wheel speed measured at clutch to compare engine RPM with wheel RPM + inRenderer->DrawLine(rpm_meter_pos, rpm_meter_pos + Quat::sRotation(rpm_meter_fwd, mEngine.ConvertRPMToAngle(wheel_speed_at_clutch)) * (rpm_meter_up * 1.1f * mRPMMeterSize), Color::sYellow); + } + + // Draw current vehicle state + String status = StringFormat("Forward: %.1f, Right: %.1f\nBrake: %.1f, HandBrake: %.1f\n" + "Gear: %d, Clutch: %.1f\nEngineRPM: %.0f, V: %.1f km/h", + (double)mForwardInput, (double)mRightInput, (double)mBrakeInput, (double)mHandBrakeInput, + mTransmission.GetCurrentGear(), (double)mTransmission.GetClutchFriction(), (double)mEngine.GetCurrentRPM(), (double)body->GetLinearVelocity().Length() * 3.6); + inRenderer->DrawText3D(body->GetPosition(), status, Color::sWhite, constraint_size); + + RMat44 body_transform = body->GetWorldTransform(); + + for (const Wheel *w_base : mConstraint.GetWheels()) + { + const WheelWV *w = static_cast(w_base); + const WheelSettings *settings = w->GetSettings(); + + // Calculate where the suspension attaches to the body in world space + RVec3 ws_position = body_transform * settings->mPosition; + Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection); + + // Draw suspension + RVec3 min_suspension_pos = ws_position + ws_direction * settings->mSuspensionMinLength; + RVec3 max_suspension_pos = ws_position + ws_direction * settings->mSuspensionMaxLength; + inRenderer->DrawLine(ws_position, min_suspension_pos, Color::sRed); + inRenderer->DrawLine(min_suspension_pos, max_suspension_pos, Color::sGreen); + + // Draw current length + RVec3 wheel_pos = ws_position + ws_direction * w->GetSuspensionLength(); + inRenderer->DrawMarker(wheel_pos, w->GetSuspensionLength() < settings->mSuspensionMinLength? Color::sRed : Color::sGreen, constraint_size); + + // Draw wheel basis + Vec3 wheel_forward, wheel_up, wheel_right; + mConstraint.GetWheelLocalBasis(w, wheel_forward, wheel_up, wheel_right); + wheel_forward = body_transform.Multiply3x3(wheel_forward); + wheel_up = body_transform.Multiply3x3(wheel_up); + wheel_right = body_transform.Multiply3x3(wheel_right); + Vec3 steering_axis = body_transform.Multiply3x3(settings->mSteeringAxis); + inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_forward, Color::sRed); + inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_up, Color::sGreen); + inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_right, Color::sBlue); + inRenderer->DrawLine(wheel_pos, wheel_pos + steering_axis, Color::sYellow); + + // Draw wheel + RMat44 wheel_transform(Vec4(wheel_up, 0.0f), Vec4(wheel_right, 0.0f), Vec4(wheel_forward, 0.0f), wheel_pos); + wheel_transform.SetRotation(wheel_transform.GetRotation() * Mat44::sRotationY(-w->GetRotationAngle())); + inRenderer->DrawCylinder(wheel_transform, settings->mWidth * 0.5f, settings->mRadius, w->GetSuspensionLength() <= settings->mSuspensionMinLength? Color::sRed : Color::sGreen, DebugRenderer::ECastShadow::Off, DebugRenderer::EDrawMode::Wireframe); + + if (w->HasContact()) + { + // Draw contact + inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactNormal(), Color::sYellow); + inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLongitudinal(), Color::sRed); + inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLateral(), Color::sBlue); + + DebugRenderer::sInstance->DrawText3D(wheel_pos, StringFormat("W: %.1f, S: %.2f\nSlipLateral: %.1f, SlipLong: %.2f\nFrLateral: %.1f, FrLong: %.1f", (double)w->GetAngularVelocity(), (double)w->GetSuspensionLength(), (double)RadiansToDegrees(w->mLateralSlip), (double)w->mLongitudinalSlip, (double)w->mCombinedLateralFriction, (double)w->mCombinedLongitudinalFriction), Color::sWhite, constraint_size); + } + else + { + // Draw 'no hit' + DebugRenderer::sInstance->DrawText3D(wheel_pos, StringFormat("W: %.1f", (double)w->GetAngularVelocity()), Color::sRed, constraint_size); + } + } +} + +#endif // JPH_DEBUG_RENDERER + +void WheeledVehicleController::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mForwardInput); + inStream.Write(mRightInput); + inStream.Write(mBrakeInput); + inStream.Write(mHandBrakeInput); + inStream.Write(mPreviousDeltaTime); + + mEngine.SaveState(inStream); + mTransmission.SaveState(inStream); +} + +void WheeledVehicleController::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mForwardInput); + inStream.Read(mRightInput); + inStream.Read(mBrakeInput); + inStream.Read(mHandBrakeInput); + inStream.Read(mPreviousDeltaTime); + + mEngine.RestoreState(inStream); + mTransmission.RestoreState(inStream); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.h new file mode 100644 index 000000000000..0bfd66fd8b4c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.h @@ -0,0 +1,199 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; + +/// WheelSettings object specifically for WheeledVehicleController +class JPH_EXPORT WheelSettingsWV : public WheelSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, WheelSettingsWV) + +public: + /// Constructor + WheelSettingsWV(); + + // See: WheelSettings + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void RestoreBinaryState(StreamIn &inStream) override; + + float mInertia = 0.9f; ///< Moment of inertia (kg m^2), for a cylinder this would be 0.5 * M * R^2 which is 0.9 for a wheel with a mass of 20 kg and radius 0.3 m + float mAngularDamping = 0.2f; ///< Angular damping factor of the wheel: dw/dt = -c * w + float mMaxSteerAngle = DegreesToRadians(70.0f); ///< How much this wheel can steer (radians) + LinearCurve mLongitudinalFriction; ///< On the Y-axis: friction in the forward direction of the tire. Friction is normally between 0 (no friction) and 1 (full friction) although friction can be a little bit higher than 1 because of the profile of a tire. On the X-axis: the slip ratio (fraction) defined as (omega_wheel * r_wheel - v_longitudinal) / |v_longitudinal|. You can see slip ratio as the amount the wheel is spinning relative to the floor: 0 means the wheel has full traction and is rolling perfectly in sync with the ground, 1 is for example when the wheel is locked and sliding over the ground. + LinearCurve mLateralFriction; ///< On the Y-axis: friction in the sideways direction of the tire. Friction is normally between 0 (no friction) and 1 (full friction) although friction can be a little bit higher than 1 because of the profile of a tire. On the X-axis: the slip angle (degrees) defined as angle between relative contact velocity and tire direction. + float mMaxBrakeTorque = 1500.0f; ///< How much torque (Nm) the brakes can apply to this wheel + float mMaxHandBrakeTorque = 4000.0f; ///< How much torque (Nm) the hand brake can apply to this wheel (usually only applied to the rear wheels) +}; + +/// Wheel object specifically for WheeledVehicleController +class JPH_EXPORT WheelWV : public Wheel +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit WheelWV(const WheelSettingsWV &inWheel); + + /// Override GetSettings and cast to the correct class + const WheelSettingsWV * GetSettings() const { return StaticCast(mSettings); } + + /// Apply a torque (N m) to the wheel for a particular delta time + void ApplyTorque(float inTorque, float inDeltaTime) + { + mAngularVelocity += inTorque * inDeltaTime / GetSettings()->mInertia; + } + + /// Update the wheel rotation based on the current angular velocity + void Update(uint inWheelIndex, float inDeltaTime, const VehicleConstraint &inConstraint); + + float mLongitudinalSlip = 0.0f; ///< Velocity difference between ground and wheel relative to ground velocity + float mLateralSlip = 0.0f; ///< Angular difference (in radians) between ground and wheel relative to ground velocity + float mCombinedLongitudinalFriction = 0.0f; ///< Combined friction coefficient in longitudinal direction (combines terrain and tires) + float mCombinedLateralFriction = 0.0f; ///< Combined friction coefficient in lateral direction (combines terrain and tires) + float mBrakeImpulse = 0.0f; ///< Amount of impulse that the brakes can apply to the floor (excluding friction) +}; + +/// Settings of a vehicle with regular wheels +/// +/// The properties in this controller are largely based on "Car Physics for Games" by Marco Monster. +/// See: https://www.asawicki.info/Mirror/Car%20Physics%20for%20Games/Car%20Physics%20for%20Games.html +class JPH_EXPORT WheeledVehicleControllerSettings : public VehicleControllerSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, WheeledVehicleControllerSettings) + +public: + // See: VehicleControllerSettings + virtual VehicleController * ConstructController(VehicleConstraint &inConstraint) const override; + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void RestoreBinaryState(StreamIn &inStream) override; + + VehicleEngineSettings mEngine; ///< The properties of the engine + VehicleTransmissionSettings mTransmission; ///< The properties of the transmission (aka gear box) + Array mDifferentials; ///< List of differentials and their properties + float mDifferentialLimitedSlipRatio = 1.4f; ///< Ratio max / min average wheel speed of each differential (measured at the clutch). When the ratio is exceeded all torque gets distributed to the differential with the minimal average velocity. This allows implementing a limited slip differential between differentials. Set to FLT_MAX for an open differential. Value should be > 1. +}; + +/// Runtime controller class +class JPH_EXPORT WheeledVehicleController : public VehicleController +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + WheeledVehicleController(const WheeledVehicleControllerSettings &inSettings, VehicleConstraint &inConstraint); + + /// Typedefs + using Differentials = Array; + + /// Set input from driver + /// @param inForward Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed + /// @param inRight Value between -1 and 1 indicating desired steering angle (1 = right) + /// @param inBrake Value between 0 and 1 indicating how strong the brake pedal is pressed + /// @param inHandBrake Value between 0 and 1 indicating how strong the hand brake is pulled + void SetDriverInput(float inForward, float inRight, float inBrake, float inHandBrake) { mForwardInput = inForward; mRightInput = inRight; mBrakeInput = inBrake; mHandBrakeInput = inHandBrake; } + + /// Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed + void SetForwardInput(float inForward) { mForwardInput = inForward; } + float GetForwardInput() const { return mForwardInput; } + + /// Value between -1 and 1 indicating desired steering angle (1 = right) + void SetRightInput(float inRight) { mRightInput = inRight; } + float GetRightInput() const { return mRightInput; } + + /// Value between 0 and 1 indicating how strong the brake pedal is pressed + void SetBrakeInput(float inBrake) { mBrakeInput = inBrake; } + float GetBrakeInput() const { return mBrakeInput; } + + /// Value between 0 and 1 indicating how strong the hand brake is pulled + void SetHandBrakeInput(float inHandBrake) { mHandBrakeInput = inHandBrake; } + float GetHandBrakeInput() const { return mHandBrakeInput; } + + /// Get current engine state + const VehicleEngine & GetEngine() const { return mEngine; } + + /// Get current engine state (writable interface, allows you to make changes to the configuration which will take effect the next time step) + VehicleEngine & GetEngine() { return mEngine; } + + /// Get current transmission state + const VehicleTransmission & GetTransmission() const { return mTransmission; } + + /// Get current transmission state (writable interface, allows you to make changes to the configuration which will take effect the next time step) + VehicleTransmission & GetTransmission() { return mTransmission; } + + /// Get the differentials this vehicle has + const Differentials & GetDifferentials() const { return mDifferentials; } + + /// Get the differentials this vehicle has (writable interface, allows you to make changes to the configuration which will take effect the next time step) + Differentials & GetDifferentials() { return mDifferentials; } + + /// Ratio max / min average wheel speed of each differential (measured at the clutch). + float GetDifferentialLimitedSlipRatio() const { return mDifferentialLimitedSlipRatio; } + void SetDifferentialLimitedSlipRatio(float inV) { mDifferentialLimitedSlipRatio = inV; } + + /// Get the average wheel speed of all driven wheels (measured at the clutch) + float GetWheelSpeedAtClutch() const; + + /// Calculate max tire impulses by combining friction, slip, and suspension impulse. Note that the actual applied impulse may be lower (e.g. when the vehicle is stationary on a horizontal surface the actual impulse applied will be 0). + using TireMaxImpulseCallback = function; + const TireMaxImpulseCallback&GetTireMaxImpulseCallback() const { return mTireMaxImpulseCallback; } + void SetTireMaxImpulseCallback(const TireMaxImpulseCallback &inTireMaxImpulseCallback) { mTireMaxImpulseCallback = inTireMaxImpulseCallback; } + +#ifdef JPH_DEBUG_RENDERER + /// Debug drawing of RPM meter + void SetRPMMeter(Vec3Arg inPosition, float inSize) { mRPMMeterPosition = inPosition; mRPMMeterSize = inSize; } +#endif // JPH_DEBUG_RENDERER + +protected: + // 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; + virtual void PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override; + virtual void PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override; + virtual bool SolveLongitudinalAndLateralConstraints(float inDeltaTime) override; + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; +#ifdef JPH_DEBUG_RENDERER + virtual void Draw(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + + // Control information + float mForwardInput = 0.0f; ///< Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed + float mRightInput = 0.0f; ///< Value between -1 and 1 indicating desired steering angle + float mBrakeInput = 0.0f; ///< Value between 0 and 1 indicating how strong the brake pedal is pressed + float mHandBrakeInput = 0.0f; ///< Value between 0 and 1 indicating how strong the hand brake is pulled + + // Simulation information + VehicleEngine mEngine; ///< Engine state of the vehicle + VehicleTransmission mTransmission; ///< Transmission state of the vehicle + Differentials mDifferentials; ///< Differential states of the vehicle + float mDifferentialLimitedSlipRatio; ///< Ratio max / min average wheel speed of each differential (measured at the clutch). + float mPreviousDeltaTime = 0.0f; ///< Delta time of the last step + + // Callback that calculates the max impulse that the tire can apply to the ground + TireMaxImpulseCallback mTireMaxImpulseCallback = + [](uint, float &outLongitudinalImpulse, float &outLateralImpulse, float inSuspensionImpulse, float inLongitudinalFriction, float inLateralFriction, float, float, float) + { + outLongitudinalImpulse = inLongitudinalFriction * inSuspensionImpulse; + outLateralImpulse = inLateralFriction * inSuspensionImpulse; + }; + +#ifdef JPH_DEBUG_RENDERER + // Debug settings + Vec3 mRPMMeterPosition { 0, 1, 0 }; ///< Position (in local space of the body) of the RPM meter when drawing the constraint + float mRPMMeterSize = 0.5f; ///< Size of the RPM meter when drawing the constraint +#endif // JPH_DEBUG_RENDERER +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/RegisterTypes.cpp b/thirdparty/jolt_physics/Jolt/RegisterTypes.cpp new file mode 100644 index 000000000000..5b61a9160a27 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/RegisterTypes.cpp @@ -0,0 +1,192 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, Skeleton) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, SkeletalAnimation) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, RagdollSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PointConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, SixDOFConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, SliderConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, SwingTwistConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, DistanceConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, HingeConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, FixedConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, ConeConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PathConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PathConstraintPath) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PathConstraintPathHermite) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, VehicleConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, WheeledVehicleControllerSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, RackAndPinionConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, GearConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PulleyConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, MotorSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PhysicsScene) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PhysicsMaterial) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, GroupFilter) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, GroupFilterTable) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, BodyCreationSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, SoftBodyCreationSettings) + +JPH_NAMESPACE_BEGIN + +bool VerifyJoltVersionIDInternal(uint64 inVersionID) +{ + return inVersionID == JPH_VERSION_ID; +} + +void RegisterTypesInternal(uint64 inVersionID) +{ + // Version check + if (!VerifyJoltVersionIDInternal(inVersionID)) + { + Trace("Version mismatch, make sure you compile the client code with the same Jolt version and compiler definitions!"); + uint64 mismatch = JPH_VERSION_ID ^ inVersionID; + auto check_bit = [mismatch](int inBit, const char *inLabel) { if (mismatch & (uint64(1) << (inBit + 23))) Trace("Mismatching define %s.", inLabel); }; + check_bit(1, "JPH_DOUBLE_PRECISION"); + check_bit(2, "JPH_CROSS_PLATFORM_DETERMINISTIC"); + check_bit(3, "JPH_FLOATING_POINT_EXCEPTIONS_ENABLED"); + check_bit(4, "JPH_PROFILE_ENABLED"); + check_bit(5, "JPH_EXTERNAL_PROFILE"); + check_bit(6, "JPH_DEBUG_RENDERER"); + check_bit(7, "JPH_DISABLE_TEMP_ALLOCATOR"); + check_bit(8, "JPH_DISABLE_CUSTOM_ALLOCATOR"); + check_bit(9, "JPH_OBJECT_LAYER_BITS"); + check_bit(10, "JPH_ENABLE_ASSERTS"); + check_bit(11, "JPH_OBJECT_STREAM"); + std::abort(); + } + +#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR + JPH_ASSERT(Allocate != nullptr && Reallocate != nullptr && Free != nullptr && AlignedAllocate != nullptr && AlignedFree != nullptr, "Need to supply an allocator first or call RegisterDefaultAllocator()"); +#endif // !JPH_DISABLE_CUSTOM_ALLOCATOR + + JPH_ASSERT(Factory::sInstance != nullptr, "Need to create a factory first!"); + + // Initialize dispatcher + CollisionDispatch::sInit(); + + // Register base classes first so that we can specialize them later + CompoundShape::sRegister(); + ConvexShape::sRegister(); + + // Register compounds before others so that we can specialize them later (register them in reverse order of collision complexity) + MutableCompoundShape::sRegister(); + StaticCompoundShape::sRegister(); + + // Leaf classes + TriangleShape::sRegister(); + PlaneShape::sRegister(); + SphereShape::sRegister(); + BoxShape::sRegister(); + CapsuleShape::sRegister(); + TaperedCapsuleShape::sRegister(); + CylinderShape::sRegister(); + TaperedCylinderShape::sRegister(); + MeshShape::sRegister(); + ConvexHullShape::sRegister(); + HeightFieldShape::sRegister(); + SoftBodyShape::sRegister(); + + // Register these last because their collision functions are simple so we want to execute them first (register them in reverse order of collision complexity) + RotatedTranslatedShape::sRegister(); + OffsetCenterOfMassShape::sRegister(); + ScaledShape::sRegister(); + EmptyShape::sRegister(); + + // Create list of all types + const RTTI *types[] = { + JPH_RTTI(SkeletalAnimation), + JPH_RTTI(Skeleton), + JPH_RTTI(CompoundShapeSettings), + JPH_RTTI(StaticCompoundShapeSettings), + JPH_RTTI(MutableCompoundShapeSettings), + JPH_RTTI(TriangleShapeSettings), + JPH_RTTI(PlaneShapeSettings), + JPH_RTTI(SphereShapeSettings), + JPH_RTTI(BoxShapeSettings), + JPH_RTTI(CapsuleShapeSettings), + JPH_RTTI(TaperedCapsuleShapeSettings), + JPH_RTTI(CylinderShapeSettings), + JPH_RTTI(TaperedCylinderShapeSettings), + JPH_RTTI(ScaledShapeSettings), + JPH_RTTI(MeshShapeSettings), + JPH_RTTI(ConvexHullShapeSettings), + JPH_RTTI(HeightFieldShapeSettings), + JPH_RTTI(RotatedTranslatedShapeSettings), + JPH_RTTI(OffsetCenterOfMassShapeSettings), + JPH_RTTI(EmptyShapeSettings), + JPH_RTTI(RagdollSettings), + JPH_RTTI(PointConstraintSettings), + JPH_RTTI(SixDOFConstraintSettings), + JPH_RTTI(SliderConstraintSettings), + JPH_RTTI(SwingTwistConstraintSettings), + JPH_RTTI(DistanceConstraintSettings), + JPH_RTTI(HingeConstraintSettings), + JPH_RTTI(FixedConstraintSettings), + JPH_RTTI(ConeConstraintSettings), + JPH_RTTI(PathConstraintSettings), + JPH_RTTI(VehicleConstraintSettings), + JPH_RTTI(WheeledVehicleControllerSettings), + JPH_RTTI(PathConstraintPath), + JPH_RTTI(PathConstraintPathHermite), + JPH_RTTI(RackAndPinionConstraintSettings), + JPH_RTTI(GearConstraintSettings), + JPH_RTTI(PulleyConstraintSettings), + JPH_RTTI(MotorSettings), + JPH_RTTI(PhysicsScene), + JPH_RTTI(PhysicsMaterial), + JPH_RTTI(PhysicsMaterialSimple), + JPH_RTTI(GroupFilter), + JPH_RTTI(GroupFilterTable), + JPH_RTTI(BodyCreationSettings), + JPH_RTTI(SoftBodyCreationSettings) + }; + + // Register them all + Factory::sInstance->Register(types, (uint)std::size(types)); + + // Initialize default physics material + if (PhysicsMaterial::sDefault == nullptr) + PhysicsMaterial::sDefault = new PhysicsMaterialSimple("Default", Color::sGrey); +} + +void UnregisterTypes() +{ + // Unregister all types + if (Factory::sInstance != nullptr) + Factory::sInstance->Clear(); + + // Delete default physics material + PhysicsMaterial::sDefault = nullptr; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/RegisterTypes.h b/thirdparty/jolt_physics/Jolt/RegisterTypes.h new file mode 100644 index 000000000000..372ef7f4f40e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/RegisterTypes.h @@ -0,0 +1,29 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Internal helper function +JPH_EXPORT extern bool VerifyJoltVersionIDInternal(uint64 inVersionID); + +/// This function can be used to verify the library ABI is compatible with your +/// application. +/// Use it in this way: `assert(VerifyJoltVersionID());`. +/// Returns `false` if the library used is not compatible with your app. +JPH_INLINE bool VerifyJoltVersionID() { return VerifyJoltVersionIDInternal(JPH_VERSION_ID); } + +/// Internal helper function +JPH_EXPORT extern void RegisterTypesInternal(uint64 inVersionID); + +/// Register all physics types with the factory and install their collision handlers with the CollisionDispatch class. +/// If you have your own custom shape types you probably need to register their handlers with the CollisionDispatch before calling this function. +/// If you implement your own default material (PhysicsMaterial::sDefault) make sure to initialize it before this function or else this function will create one for you. +JPH_INLINE void RegisterTypes() { RegisterTypesInternal(JPH_VERSION_ID); } + +/// Unregisters all types with the factory and cleans up the default material +JPH_EXPORT extern void UnregisterTypes(); + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRenderer.cpp b/thirdparty/jolt_physics/Jolt/Renderer/DebugRenderer.cpp new file mode 100644 index 000000000000..636c1e2f7a6d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRenderer.cpp @@ -0,0 +1,1107 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#ifdef JPH_DEBUG_RENDERER + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +DebugRenderer *DebugRenderer::sInstance = nullptr; + +// Number of LOD levels to create +static const int sMaxLevel = 4; + +// Distance for each LOD level, these are tweaked for an object of approx. size 1. Use the lod scale to scale these distances. +static const float sLODDistanceForLevel[] = { 5.0f, 10.0f, 40.0f, FLT_MAX }; + +DebugRenderer::Triangle::Triangle(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, ColorArg inColor) +{ + // Set position + inV1.StoreFloat3(&mV[0].mPosition); + inV2.StoreFloat3(&mV[1].mPosition); + inV3.StoreFloat3(&mV[2].mPosition); + + // Set color + mV[0].mColor = mV[1].mColor = mV[2].mColor = inColor; + + // Calculate normal + Vec3 normal = (inV2 - inV1).Cross(inV3 - inV1); + float normal_len = normal.Length(); + if (normal_len > 0.0f) + normal /= normal_len; + Float3 normal3; + normal.StoreFloat3(&normal3); + mV[0].mNormal = mV[1].mNormal = mV[2].mNormal = normal3; + + // Reset UV's + mV[0].mUV = mV[1].mUV = mV[2].mUV = { 0, 0 }; +} + +DebugRenderer::Triangle::Triangle(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, ColorArg inColor, Vec3Arg inUVOrigin, Vec3Arg inUVDirection) +{ + // Set position + inV1.StoreFloat3(&mV[0].mPosition); + inV2.StoreFloat3(&mV[1].mPosition); + inV3.StoreFloat3(&mV[2].mPosition); + + // Set color + mV[0].mColor = mV[1].mColor = mV[2].mColor = inColor; + + // Calculate normal + Vec3 normal = (inV2 - inV1).Cross(inV3 - inV1).Normalized(); + Float3 normal3; + normal.StoreFloat3(&normal3); + mV[0].mNormal = mV[1].mNormal = mV[2].mNormal = normal3; + + // Set UV's + Vec3 uv1 = inV1 - inUVOrigin; + Vec3 uv2 = inV2 - inUVOrigin; + Vec3 uv3 = inV3 - inUVOrigin; + Vec3 axis2 = normal.Cross(inUVDirection); + mV[0].mUV = { inUVDirection.Dot(uv1), axis2.Dot(uv1) }; + mV[1].mUV = { inUVDirection.Dot(uv2), axis2.Dot(uv2) }; + mV[2].mUV = { inUVDirection.Dot(uv3), axis2.Dot(uv3) }; +} + +DebugRenderer::DebugRenderer() +{ + // Store singleton + JPH_ASSERT(sInstance == nullptr); + sInstance = this; +} + +DebugRenderer::~DebugRenderer() +{ + JPH_ASSERT(sInstance == this); + sInstance = nullptr; +} + +void DebugRenderer::DrawWireBox(const AABox &inBox, ColorArg inColor) +{ + JPH_PROFILE_FUNCTION(); + + // 8 vertices + RVec3 v1(Real(inBox.mMin.GetX()), Real(inBox.mMin.GetY()), Real(inBox.mMin.GetZ())); + RVec3 v2(Real(inBox.mMin.GetX()), Real(inBox.mMin.GetY()), Real(inBox.mMax.GetZ())); + RVec3 v3(Real(inBox.mMin.GetX()), Real(inBox.mMax.GetY()), Real(inBox.mMin.GetZ())); + RVec3 v4(Real(inBox.mMin.GetX()), Real(inBox.mMax.GetY()), Real(inBox.mMax.GetZ())); + RVec3 v5(Real(inBox.mMax.GetX()), Real(inBox.mMin.GetY()), Real(inBox.mMin.GetZ())); + RVec3 v6(Real(inBox.mMax.GetX()), Real(inBox.mMin.GetY()), Real(inBox.mMax.GetZ())); + RVec3 v7(Real(inBox.mMax.GetX()), Real(inBox.mMax.GetY()), Real(inBox.mMin.GetZ())); + RVec3 v8(Real(inBox.mMax.GetX()), Real(inBox.mMax.GetY()), Real(inBox.mMax.GetZ())); + + // 12 edges + DrawLine(v1, v2, inColor); + DrawLine(v1, v3, inColor); + DrawLine(v1, v5, inColor); + DrawLine(v2, v4, inColor); + DrawLine(v2, v6, inColor); + DrawLine(v3, v4, inColor); + DrawLine(v3, v7, inColor); + DrawLine(v4, v8, inColor); + DrawLine(v5, v6, inColor); + DrawLine(v5, v7, inColor); + DrawLine(v6, v8, inColor); + DrawLine(v7, v8, inColor); +} + +void DebugRenderer::DrawWireBox(const OrientedBox &inBox, ColorArg inColor) +{ + JPH_PROFILE_FUNCTION(); + + // 8 vertices + RVec3 v1(inBox.mOrientation * Vec3(-inBox.mHalfExtents.GetX(), -inBox.mHalfExtents.GetY(), -inBox.mHalfExtents.GetZ())); + RVec3 v2(inBox.mOrientation * Vec3(-inBox.mHalfExtents.GetX(), -inBox.mHalfExtents.GetY(), inBox.mHalfExtents.GetZ())); + RVec3 v3(inBox.mOrientation * Vec3(-inBox.mHalfExtents.GetX(), inBox.mHalfExtents.GetY(), -inBox.mHalfExtents.GetZ())); + RVec3 v4(inBox.mOrientation * Vec3(-inBox.mHalfExtents.GetX(), inBox.mHalfExtents.GetY(), inBox.mHalfExtents.GetZ())); + RVec3 v5(inBox.mOrientation * Vec3(inBox.mHalfExtents.GetX(), -inBox.mHalfExtents.GetY(), -inBox.mHalfExtents.GetZ())); + RVec3 v6(inBox.mOrientation * Vec3(inBox.mHalfExtents.GetX(), -inBox.mHalfExtents.GetY(), inBox.mHalfExtents.GetZ())); + RVec3 v7(inBox.mOrientation * Vec3(inBox.mHalfExtents.GetX(), inBox.mHalfExtents.GetY(), -inBox.mHalfExtents.GetZ())); + RVec3 v8(inBox.mOrientation * Vec3(inBox.mHalfExtents.GetX(), inBox.mHalfExtents.GetY(), inBox.mHalfExtents.GetZ())); + + // 12 edges + DrawLine(v1, v2, inColor); + DrawLine(v1, v3, inColor); + DrawLine(v1, v5, inColor); + DrawLine(v2, v4, inColor); + DrawLine(v2, v6, inColor); + DrawLine(v3, v4, inColor); + DrawLine(v3, v7, inColor); + DrawLine(v4, v8, inColor); + DrawLine(v5, v6, inColor); + DrawLine(v5, v7, inColor); + DrawLine(v6, v8, inColor); + DrawLine(v7, v8, inColor); +} + +void DebugRenderer::DrawWireBox(RMat44Arg inMatrix, const AABox &inBox, ColorArg inColor) +{ + JPH_PROFILE_FUNCTION(); + + // 8 vertices + RVec3 v1 = inMatrix * Vec3(inBox.mMin.GetX(), inBox.mMin.GetY(), inBox.mMin.GetZ()); + RVec3 v2 = inMatrix * Vec3(inBox.mMin.GetX(), inBox.mMin.GetY(), inBox.mMax.GetZ()); + RVec3 v3 = inMatrix * Vec3(inBox.mMin.GetX(), inBox.mMax.GetY(), inBox.mMin.GetZ()); + RVec3 v4 = inMatrix * Vec3(inBox.mMin.GetX(), inBox.mMax.GetY(), inBox.mMax.GetZ()); + RVec3 v5 = inMatrix * Vec3(inBox.mMax.GetX(), inBox.mMin.GetY(), inBox.mMin.GetZ()); + RVec3 v6 = inMatrix * Vec3(inBox.mMax.GetX(), inBox.mMin.GetY(), inBox.mMax.GetZ()); + RVec3 v7 = inMatrix * Vec3(inBox.mMax.GetX(), inBox.mMax.GetY(), inBox.mMin.GetZ()); + RVec3 v8 = inMatrix * Vec3(inBox.mMax.GetX(), inBox.mMax.GetY(), inBox.mMax.GetZ()); + + // 12 edges + DrawLine(v1, v2, inColor); + DrawLine(v1, v3, inColor); + DrawLine(v1, v5, inColor); + DrawLine(v2, v4, inColor); + DrawLine(v2, v6, inColor); + DrawLine(v3, v4, inColor); + DrawLine(v3, v7, inColor); + DrawLine(v4, v8, inColor); + DrawLine(v5, v6, inColor); + DrawLine(v5, v7, inColor); + DrawLine(v6, v8, inColor); + DrawLine(v7, v8, inColor); +} + +void DebugRenderer::DrawMarker(RVec3Arg inPosition, ColorArg inColor, float inSize) +{ + JPH_PROFILE_FUNCTION(); + + Vec3 dx(inSize, 0, 0); + Vec3 dy(0, inSize, 0); + Vec3 dz(0, 0, inSize); + DrawLine(inPosition - dy, inPosition + dy, inColor); + DrawLine(inPosition - dx, inPosition + dx, inColor); + DrawLine(inPosition - dz, inPosition + dz, inColor); +} + +void DebugRenderer::DrawArrow(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor, float inSize) +{ + JPH_PROFILE_FUNCTION(); + + // Draw base line + DrawLine(inFrom, inTo, inColor); + + if (inSize > 0.0f) + { + // Draw arrow head + Vec3 dir = Vec3(inTo - inFrom); + float len = dir.Length(); + if (len != 0.0f) + dir = dir * (inSize / len); + else + dir = Vec3(inSize, 0, 0); + Vec3 perp = inSize * dir.GetNormalizedPerpendicular(); + DrawLine(inTo - dir + perp, inTo, inColor); + DrawLine(inTo - dir - perp, inTo, inColor); + } +} + +void DebugRenderer::DrawCoordinateSystem(RMat44Arg inTransform, float inSize) +{ + JPH_PROFILE_FUNCTION(); + + DrawArrow(inTransform.GetTranslation(), inTransform * Vec3(inSize, 0, 0), Color::sRed, 0.1f * inSize); + DrawArrow(inTransform.GetTranslation(), inTransform * Vec3(0, inSize, 0), Color::sGreen, 0.1f * inSize); + DrawArrow(inTransform.GetTranslation(), inTransform * Vec3(0, 0, inSize), Color::sBlue, 0.1f * inSize); +} + +void DebugRenderer::DrawPlane(RVec3Arg inPoint, Vec3Arg inNormal, ColorArg inColor, float inSize) +{ + // Create orthogonal basis + Vec3 perp1 = inNormal.Cross(Vec3::sAxisY()).NormalizedOr(Vec3::sAxisX()); + Vec3 perp2 = perp1.Cross(inNormal).Normalized(); + perp1 = inNormal.Cross(perp2); + + // Calculate corners + RVec3 corner1 = inPoint + inSize * (perp1 + perp2); + RVec3 corner2 = inPoint + inSize * (perp1 - perp2); + RVec3 corner3 = inPoint + inSize * (-perp1 - perp2); + RVec3 corner4 = inPoint + inSize * (-perp1 + perp2); + + // Draw cross + DrawLine(corner1, corner3, inColor); + DrawLine(corner2, corner4, inColor); + + // Draw square + DrawLine(corner1, corner2, inColor); + DrawLine(corner2, corner3, inColor); + DrawLine(corner3, corner4, inColor); + DrawLine(corner4, corner1, inColor); + + // Draw normal + DrawArrow(inPoint, inPoint + inSize * inNormal, inColor, 0.1f * inSize); +} + +void DebugRenderer::DrawWireTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor) +{ + JPH_PROFILE_FUNCTION(); + + DrawLine(inV1, inV2, inColor); + DrawLine(inV2, inV3, inColor); + DrawLine(inV3, inV1, inColor); +} + +void DebugRenderer::DrawWireSphere(RVec3Arg inCenter, float inRadius, ColorArg inColor, int inLevel) +{ + RMat44 matrix = RMat44::sTranslation(inCenter) * Mat44::sScale(inRadius); + + DrawWireUnitSphere(matrix, inColor, inLevel); +} + +void DebugRenderer::DrawWireUnitSphere(RMat44Arg inMatrix, ColorArg inColor, int inLevel) +{ + JPH_PROFILE_FUNCTION(); + + DrawWireUnitSphereRecursive(inMatrix, inColor, Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), inLevel); + DrawWireUnitSphereRecursive(inMatrix, inColor, -Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), inLevel); + DrawWireUnitSphereRecursive(inMatrix, inColor, Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), inLevel); + DrawWireUnitSphereRecursive(inMatrix, inColor, -Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), inLevel); + DrawWireUnitSphereRecursive(inMatrix, inColor, Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), inLevel); + DrawWireUnitSphereRecursive(inMatrix, inColor, -Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), inLevel); + DrawWireUnitSphereRecursive(inMatrix, inColor, Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), inLevel); + DrawWireUnitSphereRecursive(inMatrix, inColor, -Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), inLevel); +} + +void DebugRenderer::DrawWireUnitSphereRecursive(RMat44Arg inMatrix, ColorArg inColor, Vec3Arg inDir1, Vec3Arg inDir2, Vec3Arg inDir3, int inLevel) +{ + if (inLevel == 0) + { + RVec3 d1 = inMatrix * inDir1; + RVec3 d2 = inMatrix * inDir2; + RVec3 d3 = inMatrix * inDir3; + + DrawLine(d1, d2, inColor); + DrawLine(d2, d3, inColor); + DrawLine(d3, d1, inColor); + } + else + { + Vec3 center1 = (inDir1 + inDir2).Normalized(); + Vec3 center2 = (inDir2 + inDir3).Normalized(); + Vec3 center3 = (inDir3 + inDir1).Normalized(); + + DrawWireUnitSphereRecursive(inMatrix, inColor, inDir1, center1, center3, inLevel - 1); + DrawWireUnitSphereRecursive(inMatrix, inColor, center1, center2, center3, inLevel - 1); + DrawWireUnitSphereRecursive(inMatrix, inColor, center1, inDir2, center2, inLevel - 1); + DrawWireUnitSphereRecursive(inMatrix, inColor, center3, center2, inDir3, inLevel - 1); + } +} + +void DebugRenderer::Create8thSphereRecursive(Array &ioIndices, Array &ioVertices, Vec3Arg inDir1, uint32 &ioIdx1, Vec3Arg inDir2, uint32 &ioIdx2, Vec3Arg inDir3, uint32 &ioIdx3, const Float2 &inUV, SupportFunction inGetSupport, int inLevel) +{ + if (inLevel == 0) + { + if (ioIdx1 == 0xffffffff) + { + ioIdx1 = (uint32)ioVertices.size(); + Float3 position, normal; + inGetSupport(inDir1).StoreFloat3(&position); + inDir1.StoreFloat3(&normal); + ioVertices.push_back({ position, normal, inUV, Color::sWhite }); + } + + if (ioIdx2 == 0xffffffff) + { + ioIdx2 = (uint32)ioVertices.size(); + Float3 position, normal; + inGetSupport(inDir2).StoreFloat3(&position); + inDir2.StoreFloat3(&normal); + ioVertices.push_back({ position, normal, inUV, Color::sWhite }); + } + + if (ioIdx3 == 0xffffffff) + { + ioIdx3 = (uint32)ioVertices.size(); + Float3 position, normal; + inGetSupport(inDir3).StoreFloat3(&position); + inDir3.StoreFloat3(&normal); + ioVertices.push_back({ position, normal, inUV, Color::sWhite }); + } + + ioIndices.push_back(ioIdx1); + ioIndices.push_back(ioIdx2); + ioIndices.push_back(ioIdx3); + } + else + { + Vec3 center1 = (inDir1 + inDir2).Normalized(); + Vec3 center2 = (inDir2 + inDir3).Normalized(); + Vec3 center3 = (inDir3 + inDir1).Normalized(); + + uint32 idx1 = 0xffffffff; + uint32 idx2 = 0xffffffff; + uint32 idx3 = 0xffffffff; + + Create8thSphereRecursive(ioIndices, ioVertices, inDir1, ioIdx1, center1, idx1, center3, idx3, inUV, inGetSupport, inLevel - 1); + Create8thSphereRecursive(ioIndices, ioVertices, center1, idx1, center2, idx2, center3, idx3, inUV, inGetSupport, inLevel - 1); + Create8thSphereRecursive(ioIndices, ioVertices, center1, idx1, inDir2, ioIdx2, center2, idx2, inUV, inGetSupport, inLevel - 1); + Create8thSphereRecursive(ioIndices, ioVertices, center3, idx3, center2, idx2, inDir3, ioIdx3, inUV, inGetSupport, inLevel - 1); + } +} + +void DebugRenderer::Create8thSphere(Array &ioIndices, Array &ioVertices, Vec3Arg inDir1, Vec3Arg inDir2, Vec3Arg inDir3, const Float2 &inUV, SupportFunction inGetSupport, int inLevel) +{ + uint32 idx1 = 0xffffffff; + uint32 idx2 = 0xffffffff; + uint32 idx3 = 0xffffffff; + + Create8thSphereRecursive(ioIndices, ioVertices, inDir1, idx1, inDir2, idx2, inDir3, idx3, inUV, inGetSupport, inLevel); +} + +DebugRenderer::Batch DebugRenderer::CreateCylinder(float inTop, float inBottom, float inTopRadius, float inBottomRadius, int inLevel) +{ + Array cylinder_vertices; + Array cylinder_indices; + + for (int q = 0; q < 4; ++q) + { + Float2 uv = (q & 1) == 0? Float2(0.25f, 0.75f) : Float2(0.25f, 0.25f); + + uint32 center_start_idx = (uint32)cylinder_vertices.size(); + + Float3 nt(0.0f, 1.0f, 0.0f); + Float3 nb(0.0f, -1.0f, 0.0f); + cylinder_vertices.push_back({ Float3(0.0f, inTop, 0.0f), nt, uv, Color::sWhite }); + cylinder_vertices.push_back({ Float3(0.0f, inBottom, 0.0f), nb, uv, Color::sWhite }); + + uint32 vtx_start_idx = (uint32)cylinder_vertices.size(); + + int num_parts = 1 << inLevel; + for (int i = 0; i <= num_parts; ++i) + { + // Calculate top and bottom vertex + float angle = 0.5f * JPH_PI * (float(q) + float(i) / num_parts); + float s = Sin(angle); + float c = Cos(angle); + Float3 vt(inTopRadius * s, inTop, inTopRadius * c); + Float3 vb(inBottomRadius * s, inBottom, inBottomRadius * c); + + // Calculate normal + Vec3 edge = Vec3(vt) - Vec3(vb); + Float3 n; + edge.Cross(Vec3(s, 0, c).Cross(edge)).Normalized().StoreFloat3(&n); + + cylinder_vertices.push_back({ vt, nt, uv, Color::sWhite }); + cylinder_vertices.push_back({ vb, nb, uv, Color::sWhite }); + cylinder_vertices.push_back({ vt, n, uv, Color::sWhite }); + cylinder_vertices.push_back({ vb, n, uv, Color::sWhite }); + } + + for (int i = 0; i < num_parts; ++i) + { + uint32 start = vtx_start_idx + 4 * i; + + // Top + cylinder_indices.push_back(center_start_idx); + cylinder_indices.push_back(start); + cylinder_indices.push_back(start + 4); + + // Bottom + cylinder_indices.push_back(center_start_idx + 1); + cylinder_indices.push_back(start + 5); + cylinder_indices.push_back(start + 1); + + // Side + cylinder_indices.push_back(start + 2); + cylinder_indices.push_back(start + 3); + cylinder_indices.push_back(start + 7); + + cylinder_indices.push_back(start + 2); + cylinder_indices.push_back(start + 7); + cylinder_indices.push_back(start + 6); + } + } + + return CreateTriangleBatch(cylinder_vertices, cylinder_indices); +} + +void DebugRenderer::CreateQuad(Array &ioIndices, Array &ioVertices, Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, Vec3Arg inV4) +{ + // Make room + uint32 start_idx = uint32(ioVertices.size()); + ioVertices.resize(start_idx + 4); + Vertex *vertices = &ioVertices[start_idx]; + + // Set position + inV1.StoreFloat3(&vertices[0].mPosition); + inV2.StoreFloat3(&vertices[1].mPosition); + inV3.StoreFloat3(&vertices[2].mPosition); + inV4.StoreFloat3(&vertices[3].mPosition); + + // Set color + vertices[0].mColor = vertices[1].mColor = vertices[2].mColor = vertices[3].mColor = Color::sWhite; + + // Calculate normal + Vec3 normal = (inV2 - inV1).Cross(inV3 - inV1).Normalized(); + Float3 normal3; + normal.StoreFloat3(&normal3); + vertices[0].mNormal = vertices[1].mNormal = vertices[2].mNormal = vertices[3].mNormal = normal3; + + // Set UV's + vertices[0].mUV = { 0, 0 }; + vertices[1].mUV = { 2, 0 }; + vertices[2].mUV = { 2, 2 }; + vertices[3].mUV = { 0, 2 }; + + // Set indices + ioIndices.push_back(start_idx); + ioIndices.push_back(start_idx + 1); + ioIndices.push_back(start_idx + 2); + + ioIndices.push_back(start_idx); + ioIndices.push_back(start_idx + 2); + ioIndices.push_back(start_idx + 3); +} + +void DebugRenderer::Initialize() +{ + // Box + { + Array box_vertices; + Array box_indices; + + // Get corner points + Vec3 v0 = Vec3(-1, 1, -1); + Vec3 v1 = Vec3( 1, 1, -1); + Vec3 v2 = Vec3( 1, 1, 1); + Vec3 v3 = Vec3(-1, 1, 1); + Vec3 v4 = Vec3(-1, -1, -1); + Vec3 v5 = Vec3( 1, -1, -1); + Vec3 v6 = Vec3( 1, -1, 1); + Vec3 v7 = Vec3(-1, -1, 1); + + // Top + CreateQuad(box_indices, box_vertices, v0, v3, v2, v1); + + // Bottom + CreateQuad(box_indices, box_vertices, v4, v5, v6, v7); + + // Left + CreateQuad(box_indices, box_vertices, v0, v4, v7, v3); + + // Right + CreateQuad(box_indices, box_vertices, v2, v6, v5, v1); + + // Front + CreateQuad(box_indices, box_vertices, v3, v7, v6, v2); + + // Back + CreateQuad(box_indices, box_vertices, v0, v1, v5, v4); + + mBox = new Geometry(CreateTriangleBatch(box_vertices, box_indices), AABox(Vec3(-1, -1, -1), Vec3(1, 1, 1))); + } + + // Support function that returns a unit sphere + auto sphere_support = [](Vec3Arg inDirection) { return inDirection; }; + + // Construct geometries + mSphere = new Geometry(AABox(Vec3(-1, -1, -1), Vec3(1, 1, 1))); + mCapsuleBottom = new Geometry(AABox(Vec3(-1, -1, -1), Vec3(1, 0, 1))); + mCapsuleTop = new Geometry(AABox(Vec3(-1, 0, -1), Vec3(1, 1, 1))); + mCapsuleMid = new Geometry(AABox(Vec3(-1, -1, -1), Vec3(1, 1, 1))); + mOpenCone = new Geometry(AABox(Vec3(-1, 0, -1), Vec3(1, 1, 1))); + mCylinder = new Geometry(AABox(Vec3(-1, -1, -1), Vec3(1, 1, 1))); + + // Iterate over levels + for (int level = sMaxLevel; level >= 1; --level) + { + // Determine at which distance this level should be active + float distance = sLODDistanceForLevel[sMaxLevel - level]; + + // Sphere + mSphere->mLODs.push_back({ CreateTriangleBatchForConvex(sphere_support, level), distance }); + + // Capsule bottom half sphere + { + Array capsule_bottom_vertices; + Array capsule_bottom_indices; + Create8thSphere(capsule_bottom_indices, capsule_bottom_vertices, -Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), Float2(0.25f, 0.25f), sphere_support, level); + Create8thSphere(capsule_bottom_indices, capsule_bottom_vertices, -Vec3::sAxisY(), Vec3::sAxisX(), Vec3::sAxisZ(), Float2(0.25f, 0.75f), sphere_support, level); + Create8thSphere(capsule_bottom_indices, capsule_bottom_vertices, Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), Float2(0.25f, 0.25f), sphere_support, level); + Create8thSphere(capsule_bottom_indices, capsule_bottom_vertices, -Vec3::sAxisY(), -Vec3::sAxisX(), -Vec3::sAxisZ(), Float2(0.25f, 0.75f), sphere_support, level); + mCapsuleBottom->mLODs.push_back({ CreateTriangleBatch(capsule_bottom_vertices, capsule_bottom_indices), distance }); + } + + // Capsule top half sphere + { + Array capsule_top_vertices; + Array capsule_top_indices; + Create8thSphere(capsule_top_indices, capsule_top_vertices, Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), Float2(0.25f, 0.75f), sphere_support, level); + Create8thSphere(capsule_top_indices, capsule_top_vertices, Vec3::sAxisY(), -Vec3::sAxisX(), Vec3::sAxisZ(), Float2(0.25f, 0.25f), sphere_support, level); + Create8thSphere(capsule_top_indices, capsule_top_vertices, Vec3::sAxisY(), Vec3::sAxisX(), -Vec3::sAxisZ(), Float2(0.25f, 0.25f), sphere_support, level); + Create8thSphere(capsule_top_indices, capsule_top_vertices, -Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), Float2(0.25f, 0.75f), sphere_support, level); + mCapsuleTop->mLODs.push_back({ CreateTriangleBatch(capsule_top_vertices, capsule_top_indices), distance }); + } + + // Capsule middle part + { + Array capsule_mid_vertices; + Array capsule_mid_indices; + for (int q = 0; q < 4; ++q) + { + Float2 uv = (q & 1) == 0? Float2(0.25f, 0.25f) : Float2(0.25f, 0.75f); + + uint32 start_idx = (uint32)capsule_mid_vertices.size(); + + int num_parts = 1 << level; + for (int i = 0; i <= num_parts; ++i) + { + float angle = 0.5f * JPH_PI * (float(q) + float(i) / num_parts); + float s = Sin(angle); + float c = Cos(angle); + Float3 vt(s, 1.0f, c); + Float3 vb(s, -1.0f, c); + Float3 n(s, 0, c); + + capsule_mid_vertices.push_back({ vt, n, uv, Color::sWhite }); + capsule_mid_vertices.push_back({ vb, n, uv, Color::sWhite }); + } + + for (int i = 0; i < num_parts; ++i) + { + uint32 start = start_idx + 2 * i; + + capsule_mid_indices.push_back(start); + capsule_mid_indices.push_back(start + 1); + capsule_mid_indices.push_back(start + 3); + + capsule_mid_indices.push_back(start); + capsule_mid_indices.push_back(start + 3); + capsule_mid_indices.push_back(start + 2); + } + } + mCapsuleMid->mLODs.push_back({ CreateTriangleBatch(capsule_mid_vertices, capsule_mid_indices), distance }); + } + + // Open cone + { + Array open_cone_vertices; + Array open_cone_indices; + for (int q = 0; q < 4; ++q) + { + Float2 uv = (q & 1) == 0? Float2(0.25f, 0.25f) : Float2(0.25f, 0.75f); + + uint32 start_idx = (uint32)open_cone_vertices.size(); + + int num_parts = 2 << level; + Float3 vt(0, 0, 0); + for (int i = 0; i <= num_parts; ++i) + { + // Calculate bottom vertex + float angle = 0.5f * JPH_PI * (float(q) + float(i) / num_parts); + float s = Sin(angle); + float c = Cos(angle); + Float3 vb(s, 1.0f, c); + + // Calculate normal + // perpendicular = Y cross vb (perpendicular to the plane in which 0, y and vb exists) + // normal = perpendicular cross vb (normal to the edge 0 vb) + Vec3 normal = Vec3(s, -Square(s) - Square(c), c).Normalized(); + Float3 n; normal.StoreFloat3(&n); + + open_cone_vertices.push_back({ vt, n, uv, Color::sWhite }); + open_cone_vertices.push_back({ vb, n, uv, Color::sWhite }); + } + + for (int i = 0; i < num_parts; ++i) + { + uint32 start = start_idx + 2 * i; + + open_cone_indices.push_back(start); + open_cone_indices.push_back(start + 1); + open_cone_indices.push_back(start + 3); + } + } + mOpenCone->mLODs.push_back({ CreateTriangleBatch(open_cone_vertices, open_cone_indices), distance }); + } + + // Cylinder + mCylinder->mLODs.push_back({ CreateCylinder(1.0f, -1.0f, 1.0f, 1.0f, level), distance }); + } +} + +AABox DebugRenderer::sCalculateBounds(const Vertex *inVertices, int inVertexCount) +{ + AABox bounds; + for (const Vertex *v = inVertices, *v_end = inVertices + inVertexCount; v < v_end; ++v) + bounds.Encapsulate(Vec3(v->mPosition)); + return bounds; +} + +DebugRenderer::Batch DebugRenderer::CreateTriangleBatch(const VertexList &inVertices, const IndexedTriangleNoMaterialList &inTriangles) +{ + JPH_PROFILE_FUNCTION(); + + Array vertices; + + // Create render vertices + vertices.resize(inVertices.size()); + for (size_t v = 0; v < inVertices.size(); ++v) + { + vertices[v].mPosition = inVertices[v]; + vertices[v].mNormal = Float3(0, 0, 0); + vertices[v].mUV = Float2(0, 0); + vertices[v].mColor = Color::sWhite; + } + + // Calculate normals + for (size_t i = 0; i < inTriangles.size(); ++i) + { + const IndexedTriangleNoMaterial &tri = inTriangles[i]; + + // Calculate normal of face + Vec3 vtx[3]; + for (int j = 0; j < 3; ++j) + vtx[j] = Vec3::sLoadFloat3Unsafe(vertices[tri.mIdx[j]].mPosition); + Vec3 normal = ((vtx[1] - vtx[0]).Cross(vtx[2] - vtx[0])).Normalized(); + + // Add normal to all vertices in face + for (int j = 0; j < 3; ++j) + (Vec3::sLoadFloat3Unsafe(vertices[tri.mIdx[j]].mNormal) + normal).StoreFloat3(&vertices[tri.mIdx[j]].mNormal); + } + + // Renormalize vertex normals + for (size_t i = 0; i < vertices.size(); ++i) + Vec3::sLoadFloat3Unsafe(vertices[i].mNormal).Normalized().StoreFloat3(&vertices[i].mNormal); + + return CreateTriangleBatch(&vertices[0], (int)vertices.size(), &inTriangles[0].mIdx[0], (int)(3 * inTriangles.size())); +} + +DebugRenderer::Batch DebugRenderer::CreateTriangleBatchForConvex(SupportFunction inGetSupport, int inLevel, AABox *outBounds) +{ + JPH_PROFILE_FUNCTION(); + + Array vertices; + Array indices; + Create8thSphere(indices, vertices, Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), Float2(0.25f, 0.25f), inGetSupport, inLevel); + Create8thSphere(indices, vertices, Vec3::sAxisY(), -Vec3::sAxisX(), Vec3::sAxisZ(), Float2(0.25f, 0.75f), inGetSupport, inLevel); + Create8thSphere(indices, vertices, -Vec3::sAxisY(), Vec3::sAxisX(), Vec3::sAxisZ(), Float2(0.25f, 0.75f), inGetSupport, inLevel); + Create8thSphere(indices, vertices, -Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), Float2(0.25f, 0.25f), inGetSupport, inLevel); + Create8thSphere(indices, vertices, Vec3::sAxisY(), Vec3::sAxisX(), -Vec3::sAxisZ(), Float2(0.25f, 0.75f), inGetSupport, inLevel); + Create8thSphere(indices, vertices, -Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), Float2(0.25f, 0.25f), inGetSupport, inLevel); + Create8thSphere(indices, vertices, Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), Float2(0.25f, 0.25f), inGetSupport, inLevel); + Create8thSphere(indices, vertices, -Vec3::sAxisY(), -Vec3::sAxisX(), -Vec3::sAxisZ(), Float2(0.25f, 0.75f), inGetSupport, inLevel); + + if (outBounds != nullptr) + *outBounds = sCalculateBounds(&vertices[0], (int)vertices.size()); + + return CreateTriangleBatch(vertices, indices); +} + +DebugRenderer::GeometryRef DebugRenderer::CreateTriangleGeometryForConvex(SupportFunction inGetSupport) +{ + GeometryRef geometry; + + // Iterate over levels + for (int level = sMaxLevel; level >= 1; --level) + { + // Determine at which distance this level should be active + float distance = sLODDistanceForLevel[sMaxLevel - level]; + + // Create triangle batch and only calculate bounds for highest LOD level + AABox bounds; + Batch batch = CreateTriangleBatchForConvex(inGetSupport, level, geometry == nullptr? &bounds : nullptr); + + // Construct geometry in the first iteration + if (geometry == nullptr) + geometry = new Geometry(bounds); + + // Add the LOD + geometry->mLODs.push_back({ batch, distance }); + } + + return geometry; +} + +void DebugRenderer::DrawBox(const AABox &inBox, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + RMat44 m = RMat44::sScale(Vec3::sMax(inBox.GetExtent(), Vec3::sReplicate(1.0e-6f))); // Prevent div by zero when one of the edges has length 0 + m.SetTranslation(RVec3(inBox.GetCenter())); + DrawGeometry(m, inColor, mBox, ECullMode::CullBackFace, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawBox(RMat44Arg inMatrix, const AABox &inBox, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + Mat44 m = Mat44::sScale(Vec3::sMax(inBox.GetExtent(), Vec3::sReplicate(1.0e-6f))); // Prevent div by zero when one of the edges has length 0 + m.SetTranslation(inBox.GetCenter()); + DrawGeometry(inMatrix * m, inColor, mBox, ECullMode::CullBackFace, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawSphere(RVec3Arg inCenter, float inRadius, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + RMat44 matrix = RMat44::sTranslation(inCenter) * Mat44::sScale(inRadius); + + DrawUnitSphere(matrix, inColor, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawUnitSphere(RMat44Arg inMatrix, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + DrawGeometry(inMatrix, inColor, mSphere, ECullMode::CullBackFace, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawCapsule(RMat44Arg inMatrix, float inHalfHeightOfCylinder, float inRadius, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + Mat44 scale_matrix = Mat44::sScale(inRadius); + + // Calculate world space bounding box + AABox local_bounds(Vec3(-inRadius, -inHalfHeightOfCylinder - inRadius, -inRadius), Vec3(inRadius, inHalfHeightOfCylinder + inRadius, inRadius)); + AABox world_bounds = local_bounds.Transformed(inMatrix); + + float radius_sq = Square(inRadius); + + // Draw bottom half sphere + RMat44 bottom_matrix = inMatrix * Mat44::sTranslation(Vec3(0, -inHalfHeightOfCylinder, 0)) * scale_matrix; + DrawGeometry(bottom_matrix, world_bounds, radius_sq, inColor, mCapsuleBottom, ECullMode::CullBackFace, inCastShadow, inDrawMode); + + // Draw top half sphere + RMat44 top_matrix = inMatrix * Mat44::sTranslation(Vec3(0, inHalfHeightOfCylinder, 0)) * scale_matrix; + DrawGeometry(top_matrix, world_bounds, radius_sq, inColor, mCapsuleTop, ECullMode::CullBackFace, inCastShadow, inDrawMode); + + // Draw middle part + DrawGeometry(inMatrix * Mat44::sScale(Vec3(inRadius, inHalfHeightOfCylinder, inRadius)), world_bounds, radius_sq, inColor, mCapsuleMid, ECullMode::CullBackFace, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawCylinder(RMat44Arg inMatrix, float inHalfHeight, float inRadius, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + Mat44 local_transform(Vec4(inRadius, 0, 0, 0), Vec4(0, inHalfHeight, 0, 0), Vec4(0, 0, inRadius, 0), Vec4(0, 0, 0, 1)); + RMat44 transform = inMatrix * local_transform; + + DrawGeometry(transform, mCylinder->mBounds.Transformed(transform), Square(inRadius), inColor, mCylinder, ECullMode::CullBackFace, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawOpenCone(RVec3Arg inTop, Vec3Arg inAxis, Vec3Arg inPerpendicular, float inHalfAngle, float inLength, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inAxis.IsNormalized(1.0e-4f)); + JPH_ASSERT(inPerpendicular.IsNormalized(1.0e-4f)); + JPH_ASSERT(abs(inPerpendicular.Dot(inAxis)) < 1.0e-4f); + + Vec3 axis = Sign(inHalfAngle) * inLength * inAxis; + float scale = inLength * Tan(abs(inHalfAngle)); + if (scale != 0.0f) + { + Vec3 perp1 = scale * inPerpendicular; + Vec3 perp2 = scale * inAxis.Cross(inPerpendicular); + RMat44 transform(Vec4(perp1, 0), Vec4(axis, 0), Vec4(perp2, 0), inTop); + DrawGeometry(transform, inColor, mOpenCone, ECullMode::Off, inCastShadow, inDrawMode); + } +} + +DebugRenderer::Geometry *DebugRenderer::CreateSwingLimitGeometry(int inNumSegments, const Vec3 *inVertices) +{ + // Allocate space for vertices + int num_vertices = 2 * inNumSegments; + Vertex *vertices_start = (Vertex *)JPH_STACK_ALLOC(num_vertices * sizeof(Vertex)); + Vertex *vertices = vertices_start; + + for (int i = 0; i < inNumSegments; ++i) + { + // Get output vertices + Vertex &top = *(vertices++); + Vertex &bottom = *(vertices++); + + // Get local position + const Vec3 &pos = inVertices[i]; + + // Get local normal + const Vec3 &prev_pos = inVertices[(i + inNumSegments - 1) % inNumSegments]; + const Vec3 &next_pos = inVertices[(i + 1) % inNumSegments]; + Vec3 normal = 0.5f * (next_pos.Cross(pos).NormalizedOr(Vec3::sZero()) + pos.Cross(prev_pos).NormalizedOr(Vec3::sZero())); + + // Store top vertex + top.mPosition = { 0, 0, 0 }; + normal.StoreFloat3(&top.mNormal); + top.mColor = Color::sWhite; + top.mUV = { 0, 0 }; + + // Store bottom vertex + pos.StoreFloat3(&bottom.mPosition); + normal.StoreFloat3(&bottom.mNormal); + bottom.mColor = Color::sWhite; + bottom.mUV = { 0, 0 }; + } + + // Allocate space for indices + int num_indices = 3 * inNumSegments; + uint32 *indices_start = (uint32 *)JPH_STACK_ALLOC(num_indices * sizeof(uint32)); + uint32 *indices = indices_start; + + // Calculate indices + for (int i = 0; i < inNumSegments; ++i) + { + int first = 2 * i; + int second = (first + 3) % num_vertices; + int third = first + 1; + + // Triangle + *indices++ = first; + *indices++ = second; + *indices++ = third; + } + + // Convert to triangle batch + return new Geometry(CreateTriangleBatch(vertices_start, num_vertices, indices_start, num_indices), sCalculateBounds(vertices_start, num_vertices)); +} + +void DebugRenderer::DrawSwingConeLimits(RMat44Arg inMatrix, float inSwingYHalfAngle, float inSwingZHalfAngle, float inEdgeLength, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + // Assert sane input + JPH_ASSERT(inSwingYHalfAngle >= 0.0f && inSwingYHalfAngle <= JPH_PI); + JPH_ASSERT(inSwingZHalfAngle >= 0.0f && inSwingZHalfAngle <= JPH_PI); + JPH_ASSERT(inEdgeLength > 0.0f); + + // Check cache + SwingConeLimits limits { inSwingYHalfAngle, inSwingZHalfAngle }; + GeometryRef &geometry = mSwingConeLimits[limits]; + if (geometry == nullptr) + { + SwingConeBatches::iterator it = mPrevSwingConeLimits.find(limits); + if (it != mPrevSwingConeLimits.end()) + geometry = it->second; + } + if (geometry == nullptr) + { + // Number of segments to draw the cone with + const int num_segments = 64; + int half_num_segments = num_segments / 2; + + // The y and z values of the quaternion are limited to an ellipse, e1 and e2 are the radii of this ellipse + float e1 = Sin(0.5f * inSwingZHalfAngle); + float e2 = Sin(0.5f * inSwingYHalfAngle); + + // Check if the limits will draw something + if ((e1 <= 0.0f && e2 <= 0.0f) || (e2 >= 1.0f && e1 >= 1.0f)) + return; + + // Calculate squared values + float e1_sq = Square(e1); + float e2_sq = Square(e2); + + // Calculate local space vertices for shape + Vec3 ls_vertices[num_segments]; + int tgt_vertex = 0; + for (int side_iter = 0; side_iter < 2; ++side_iter) + for (int segment_iter = 0; segment_iter < half_num_segments; ++segment_iter) + { + float y, z; + if (e2_sq > e1_sq) + { + // Trace the y value of the quaternion + y = e2 - 2.0f * segment_iter * e2 / half_num_segments; + + // Calculate the corresponding z value of the quaternion + float z_sq = e1_sq - e1_sq / e2_sq * Square(y); + z = z_sq <= 0.0f? 0.0f : sqrt(z_sq); + } + else + { + // Trace the z value of the quaternion + z = -e1 + 2.0f * segment_iter * e1 / half_num_segments; + + // Calculate the corresponding y value of the quaternion + float y_sq = e2_sq - e2_sq / e1_sq * Square(z); + y = y_sq <= 0.0f? 0.0f : sqrt(y_sq); + } + + // If we're tracing the opposite side, flip the values + if (side_iter == 1) + { + z = -z; + y = -y; + } + + // Create quaternion + Vec3 q_xyz(0, y, z); + float w = sqrt(1.0f - q_xyz.LengthSq()); + Quat q(Vec4(q_xyz, w)); + + // Store vertex + ls_vertices[tgt_vertex++] = q.RotateAxisX(); + } + + geometry = CreateSwingLimitGeometry(num_segments, ls_vertices); + } + + DrawGeometry(inMatrix * Mat44::sScale(inEdgeLength), inColor, geometry, ECullMode::Off, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawSwingPyramidLimits(RMat44Arg inMatrix, float inMinSwingYAngle, float inMaxSwingYAngle, float inMinSwingZAngle, float inMaxSwingZAngle, float inEdgeLength, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + // Assert sane input + JPH_ASSERT(inMinSwingYAngle <= inMaxSwingYAngle && inMinSwingZAngle <= inMaxSwingZAngle); + JPH_ASSERT(inEdgeLength > 0.0f); + + // Check cache + SwingPyramidLimits limits { inMinSwingYAngle, inMaxSwingYAngle, inMinSwingZAngle, inMaxSwingZAngle }; + GeometryRef &geometry = mSwingPyramidLimits[limits]; + if (geometry == nullptr) + { + SwingPyramidBatches::iterator it = mPrevSwingPyramidLimits.find(limits); + if (it != mPrevSwingPyramidLimits.end()) + geometry = it->second; + } + if (geometry == nullptr) + { + // Number of segments to draw the cone with + const int num_segments = 64; + int quarter_num_segments = num_segments / 4; + + // Note that this is q = Quat::sRotation(Vec3::sAxisZ(), z) * Quat::sRotation(Vec3::sAxisY(), y) with q.x set to zero so we don't introduce twist + // This matches the calculation in SwingTwistConstraintPart::ClampSwingTwist + auto get_axis = [](float inY, float inZ) { + float hy = 0.5f * inY; + float hz = 0.5f * inZ; + float cos_hy = Cos(hy); + float cos_hz = Cos(hz); + return Quat(0, Sin(hy) * cos_hz, cos_hy * Sin(hz), cos_hy * cos_hz).Normalized().RotateAxisX(); + }; + + // Calculate local space vertices for shape + Vec3 ls_vertices[num_segments]; + int tgt_vertex = 0; + for (int segment_iter = 0; segment_iter < quarter_num_segments; ++segment_iter) + ls_vertices[tgt_vertex++] = get_axis(inMinSwingYAngle, inMaxSwingZAngle - (inMaxSwingZAngle - inMinSwingZAngle) * segment_iter / quarter_num_segments); + for (int segment_iter = 0; segment_iter < quarter_num_segments; ++segment_iter) + ls_vertices[tgt_vertex++] = get_axis(inMinSwingYAngle + (inMaxSwingYAngle - inMinSwingYAngle) * segment_iter / quarter_num_segments, inMinSwingZAngle); + for (int segment_iter = 0; segment_iter < quarter_num_segments; ++segment_iter) + ls_vertices[tgt_vertex++] = get_axis(inMaxSwingYAngle, inMinSwingZAngle + (inMaxSwingZAngle - inMinSwingZAngle) * segment_iter / quarter_num_segments); + for (int segment_iter = 0; segment_iter < quarter_num_segments; ++segment_iter) + ls_vertices[tgt_vertex++] = get_axis(inMaxSwingYAngle - (inMaxSwingYAngle - inMinSwingYAngle) * segment_iter / quarter_num_segments, inMaxSwingZAngle); + + geometry = CreateSwingLimitGeometry(num_segments, ls_vertices); + } + + DrawGeometry(inMatrix * Mat44::sScale(inEdgeLength), inColor, geometry, ECullMode::Off, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawPie(RVec3Arg inCenter, float inRadius, Vec3Arg inNormal, Vec3Arg inAxis, float inMinAngle, float inMaxAngle, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + if (inMinAngle >= inMaxAngle) + return; + + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inAxis.IsNormalized(1.0e-4f)); + JPH_ASSERT(inNormal.IsNormalized(1.0e-4f)); + JPH_ASSERT(abs(inNormal.Dot(inAxis)) < 1.0e-4f); + + // Pies have a unique batch based on the difference between min and max angle + float delta_angle = inMaxAngle - inMinAngle; + GeometryRef &geometry = mPieLimits[delta_angle]; + if (geometry == nullptr) + { + PieBatces::iterator it = mPrevPieLimits.find(delta_angle); + if (it != mPrevPieLimits.end()) + geometry = it->second; + } + if (geometry == nullptr) + { + int num_parts = (int)ceil(64.0f * delta_angle / (2.0f * JPH_PI)); + + Float3 normal = { 0, 1, 0 }; + Float3 center = { 0, 0, 0 }; + + // Allocate space for vertices + int num_vertices = num_parts + 2; + Vertex *vertices_start = (Vertex *)JPH_STACK_ALLOC(num_vertices * sizeof(Vertex)); + Vertex *vertices = vertices_start; + + // Center of circle + *vertices++ = { center, normal, { 0, 0 }, Color::sWhite }; + + // Outer edge of pie + for (int i = 0; i <= num_parts; ++i) + { + float angle = float(i) / float(num_parts) * delta_angle; + + Float3 pos = { Cos(angle), 0, Sin(angle) }; + *vertices++ = { pos, normal, { 0, 0 }, Color::sWhite }; + } + + // Allocate space for indices + int num_indices = num_parts * 3; + uint32 *indices_start = (uint32 *)JPH_STACK_ALLOC(num_indices * sizeof(uint32)); + uint32 *indices = indices_start; + + for (int i = 0; i < num_parts; ++i) + { + *indices++ = 0; + *indices++ = i + 1; + *indices++ = i + 2; + } + + // Convert to triangle batch + geometry = new Geometry(CreateTriangleBatch(vertices_start, num_vertices, indices_start, num_indices), sCalculateBounds(vertices_start, num_vertices)); + } + + // Construct matrix that transforms pie into world space + RMat44 matrix = RMat44(Vec4(inRadius * inAxis, 0), Vec4(inRadius * inNormal, 0), Vec4(inRadius * inNormal.Cross(inAxis), 0), inCenter) * Mat44::sRotationY(-inMinAngle); + + DrawGeometry(matrix, inColor, geometry, ECullMode::Off, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawTaperedCylinder(RMat44Arg inMatrix, float inTop, float inBottom, float inTopRadius, float inBottomRadius, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + TaperedCylinder tapered_cylinder { inTop, inBottom, inTopRadius, inBottomRadius }; + + GeometryRef &geometry = mTaperedCylinders[tapered_cylinder]; + if (geometry == nullptr) + { + TaperedCylinderBatces::iterator it = mPrevTaperedCylinders.find(tapered_cylinder); + if (it != mPrevTaperedCylinders.end()) + geometry = it->second; + } + if (geometry == nullptr) + { + float max_radius = max(inTopRadius, inBottomRadius); + geometry = new Geometry(AABox(Vec3(-max_radius, inBottom, -max_radius), Vec3(max_radius, inTop, max_radius))); + + for (int level = sMaxLevel; level >= 1; --level) + geometry->mLODs.push_back({ CreateCylinder(inTop, inBottom, inTopRadius, inBottomRadius, level), sLODDistanceForLevel[sMaxLevel - level] }); + } + + DrawGeometry(inMatrix, inColor, geometry, ECullMode::CullBackFace, inCastShadow, inDrawMode); +} + +void DebugRenderer::NextFrame() +{ + mPrevSwingConeLimits.clear(); + std::swap(mSwingConeLimits, mPrevSwingConeLimits); + + mPrevSwingPyramidLimits.clear(); + std::swap(mSwingPyramidLimits, mPrevSwingPyramidLimits); + + mPrevPieLimits.clear(); + std::swap(mPieLimits, mPrevPieLimits); + + mPrevTaperedCylinders.clear(); + std::swap(mTaperedCylinders, mPrevTaperedCylinders); +} + +JPH_NAMESPACE_END + +#endif // JPH_DEBUG_RENDERER diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRenderer.h b/thirdparty/jolt_physics/Jolt/Renderer/DebugRenderer.h new file mode 100644 index 000000000000..7e06ed49a4aa --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRenderer.h @@ -0,0 +1,383 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifndef JPH_DEBUG_RENDERER + #error This file should only be included when JPH_DEBUG_RENDERER is defined +#endif // !JPH_DEBUG_RENDERER + +#ifndef JPH_DEBUG_RENDERER_EXPORT + // By default export the debug renderer + #define JPH_DEBUG_RENDERER_EXPORT JPH_EXPORT +#endif // !JPH_DEBUG_RENDERER_EXPORT + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class OrientedBox; + +/// Simple triangle renderer for debugging purposes. +/// +/// Inherit from this class to provide your own implementation. +/// +/// Implement the following virtual functions: +/// - DrawLine +/// - DrawTriangle +/// - DrawText3D +/// - CreateTriangleBatch +/// - DrawGeometry +/// +/// Make sure you call Initialize() from the constructor of your implementation. +/// +/// The CreateTriangleBatch is used to prepare a batch of triangles to be drawn by a single DrawGeometry call, +/// which means that Jolt can render a complex scene much more efficiently than when each triangle in that scene would have been drawn through DrawTriangle. +/// +/// Note that an implementation that implements CreateTriangleBatch and DrawGeometry is provided by DebugRendererSimple which can be used to start quickly. +class JPH_DEBUG_RENDERER_EXPORT DebugRenderer : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + DebugRenderer(); + virtual ~DebugRenderer(); + + /// Call once after frame is complete. Releases unused dynamically generated geometry assets. + void NextFrame(); + + /// Draw line + virtual void DrawLine(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor) = 0; + + /// Draw wireframe box + void DrawWireBox(const AABox &inBox, ColorArg inColor); + void DrawWireBox(const OrientedBox &inBox, ColorArg inColor); + void DrawWireBox(RMat44Arg inMatrix, const AABox &inBox, ColorArg inColor); + + /// Draw a marker on a position + void DrawMarker(RVec3Arg inPosition, ColorArg inColor, float inSize); + + /// Draw an arrow + void DrawArrow(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor, float inSize); + + /// Draw coordinate system (3 arrows, x = red, y = green, z = blue) + void DrawCoordinateSystem(RMat44Arg inTransform, float inSize = 1.0f); + + /// Draw a plane through inPoint with normal inNormal + void DrawPlane(RVec3Arg inPoint, Vec3Arg inNormal, ColorArg inColor, float inSize); + + /// Draw wireframe triangle + void DrawWireTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor); + + /// Draw a wireframe polygon + template + void DrawWirePolygon(RMat44Arg inTransform, const VERTEX_ARRAY &inVertices, ColorArg inColor, float inArrowSize = 0.0f) { for (typename VERTEX_ARRAY::size_type i = 0; i < inVertices.size(); ++i) DrawArrow(inTransform * inVertices[i], inTransform * inVertices[(i + 1) % inVertices.size()], inColor, inArrowSize); } + + /// Draw wireframe sphere + void DrawWireSphere(RVec3Arg inCenter, float inRadius, ColorArg inColor, int inLevel = 3); + void DrawWireUnitSphere(RMat44Arg inMatrix, ColorArg inColor, int inLevel = 3); + + /// Enum that determines if a shadow should be cast or not + enum class ECastShadow + { + On, ///< This shape should cast a shadow + Off ///< This shape should not cast a shadow + }; + + /// Determines how triangles are drawn + enum class EDrawMode + { + Solid, ///< Draw as a solid shape + Wireframe, ///< Draw as wireframe + }; + + /// Draw a single back face culled triangle + virtual void DrawTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::Off) = 0; + + /// Draw a box + void DrawBox(const AABox &inBox, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + void DrawBox(RMat44Arg inMatrix, const AABox &inBox, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Draw a sphere + void DrawSphere(RVec3Arg inCenter, float inRadius, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + void DrawUnitSphere(RMat44Arg inMatrix, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Draw a capsule with one half sphere at (0, -inHalfHeightOfCylinder, 0) and the other half sphere at (0, inHalfHeightOfCylinder, 0) and radius inRadius. + /// The capsule will be transformed by inMatrix. + void DrawCapsule(RMat44Arg inMatrix, float inHalfHeightOfCylinder, float inRadius, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Draw a cylinder with top (0, inHalfHeight, 0) and bottom (0, -inHalfHeight, 0) and radius inRadius. + /// The cylinder will be transformed by inMatrix + void DrawCylinder(RMat44Arg inMatrix, float inHalfHeight, float inRadius, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Draw a bottomless cone. + /// @param inTop Top of cone, center of base is at inTop + inAxis. + /// @param inAxis Height and direction of cone + /// @param inPerpendicular Perpendicular vector to inAxis. + /// @param inHalfAngle Specifies the cone angle in radians (angle measured between inAxis and cone surface). + /// @param inLength The length of the cone. + /// @param inColor Color to use for drawing the cone. + /// @param inCastShadow determines if this geometry should cast a shadow or not. + /// @param inDrawMode determines if we draw the geometry solid or in wireframe. + void DrawOpenCone(RVec3Arg inTop, Vec3Arg inAxis, Vec3Arg inPerpendicular, float inHalfAngle, float inLength, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Draws cone rotation limits as used by the SwingTwistConstraintPart. + /// @param inMatrix Matrix that transforms from constraint space to world space + /// @param inSwingYHalfAngle See SwingTwistConstraintPart + /// @param inSwingZHalfAngle See SwingTwistConstraintPart + /// @param inEdgeLength Size of the edge of the cone shape + /// @param inColor Color to use for drawing the cone. + /// @param inCastShadow determines if this geometry should cast a shadow or not. + /// @param inDrawMode determines if we draw the geometry solid or in wireframe. + void DrawSwingConeLimits(RMat44Arg inMatrix, float inSwingYHalfAngle, float inSwingZHalfAngle, float inEdgeLength, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Draws rotation limits as used by the SwingTwistConstraintPart. + /// @param inMatrix Matrix that transforms from constraint space to world space + /// @param inMinSwingYAngle See SwingTwistConstraintPart + /// @param inMaxSwingYAngle See SwingTwistConstraintPart + /// @param inMinSwingZAngle See SwingTwistConstraintPart + /// @param inMaxSwingZAngle See SwingTwistConstraintPart + /// @param inEdgeLength Size of the edge of the cone shape + /// @param inColor Color to use for drawing the cone. + /// @param inCastShadow determines if this geometry should cast a shadow or not. + /// @param inDrawMode determines if we draw the geometry solid or in wireframe. + void DrawSwingPyramidLimits(RMat44Arg inMatrix, float inMinSwingYAngle, float inMaxSwingYAngle, float inMinSwingZAngle, float inMaxSwingZAngle, float inEdgeLength, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Draw a pie (part of a circle). + /// @param inCenter The center of the circle. + /// @param inRadius Radius of the circle. + /// @param inNormal The plane normal in which the pie resides. + /// @param inAxis The axis that defines an angle of 0 radians. + /// @param inMinAngle The pie will be drawn between [inMinAngle, inMaxAngle] (in radians). + /// @param inMaxAngle The pie will be drawn between [inMinAngle, inMaxAngle] (in radians). + /// @param inColor Color to use for drawing the pie. + /// @param inCastShadow determines if this geometry should cast a shadow or not. + /// @param inDrawMode determines if we draw the geometry solid or in wireframe. + void DrawPie(RVec3Arg inCenter, float inRadius, Vec3Arg inNormal, Vec3Arg inAxis, float inMinAngle, float inMaxAngle, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Draw a tapered cylinder + /// @param inMatrix Matrix that transforms the cylinder to world space. + /// @param inTop Top of cylinder (along Y axis) + /// @param inBottom Bottom of cylinder (along Y axis) + /// @param inTopRadius Radius at the top + /// @param inBottomRadius Radius at the bottom + /// @param inColor Color to use for drawing the pie. + /// @param inCastShadow determines if this geometry should cast a shadow or not. + /// @param inDrawMode determines if we draw the geometry solid or in wireframe. + void DrawTaperedCylinder(RMat44Arg inMatrix, float inTop, float inBottom, float inTopRadius, float inBottomRadius, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Singleton instance + static DebugRenderer * sInstance; + + /// Vertex format used by the triangle renderer + class Vertex + { + public: + Float3 mPosition; + Float3 mNormal; + Float2 mUV; + Color mColor; + }; + + /// A single triangle + class JPH_DEBUG_RENDERER_EXPORT Triangle + { + public: + Triangle() = default; + Triangle(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, ColorArg inColor); + Triangle(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, ColorArg inColor, Vec3Arg inUVOrigin, Vec3Arg inUVDirection); + + Vertex mV[3]; + }; + + /// Handle for a batch of triangles + using Batch = Ref; + + /// A single level of detail + class LOD + { + public: + Batch mTriangleBatch; + float mDistance; + }; + + /// A geometry primitive containing triangle batches for various lods + class Geometry : public RefTarget + { + public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + Geometry(const AABox &inBounds) : mBounds(inBounds) { } + Geometry(const Batch &inBatch, const AABox &inBounds) : mBounds(inBounds) { mLODs.push_back({ inBatch, FLT_MAX }); } + + /// Determine which LOD to render + /// @param inCameraPosition Current position of the camera + /// @param inWorldSpaceBounds World space bounds for this geometry (transform mBounds by model space matrix) + /// @param inLODScaleSq is the squared scale of the model matrix, it is multiplied with the LOD distances in inGeometry to calculate the real LOD distance (so a number > 1 will force a higher LOD). + /// @return The selected LOD. + const LOD & GetLOD(Vec3Arg inCameraPosition, const AABox &inWorldSpaceBounds, float inLODScaleSq) const + { + float dist_sq = inWorldSpaceBounds.GetSqDistanceTo(inCameraPosition); + for (const LOD &lod : mLODs) + if (dist_sq <= inLODScaleSq * Square(lod.mDistance)) + return lod; + + return mLODs.back(); + } + + /// All level of details for this mesh + Array mLODs; + + /// Bounding box that encapsulates all LODs + AABox mBounds; + }; + + /// Handle for a lodded triangle batch + using GeometryRef = Ref; + + /// Calculate bounding box for a batch of triangles + static AABox sCalculateBounds(const Vertex *inVertices, int inVertexCount); + + /// Create a batch of triangles that can be drawn efficiently + virtual Batch CreateTriangleBatch(const Triangle *inTriangles, int inTriangleCount) = 0; + virtual Batch CreateTriangleBatch(const Vertex *inVertices, int inVertexCount, const uint32 *inIndices, int inIndexCount) = 0; + Batch CreateTriangleBatch(const Array &inTriangles) { return CreateTriangleBatch(inTriangles.empty()? nullptr : &inTriangles[0], int(inTriangles.size())); } + Batch CreateTriangleBatch(const Array &inVertices, const Array &inIndices) { return CreateTriangleBatch(inVertices.empty()? nullptr : &inVertices[0], int(inVertices.size()), inIndices.empty()? nullptr : &inIndices[0], int(inIndices.size())); } + Batch CreateTriangleBatch(const VertexList &inVertices, const IndexedTriangleNoMaterialList &inTriangles); + + /// Create a primitive for a convex shape using its support function + using SupportFunction = function; + Batch CreateTriangleBatchForConvex(SupportFunction inGetSupport, int inLevel, AABox *outBounds = nullptr); + GeometryRef CreateTriangleGeometryForConvex(SupportFunction inGetSupport); + + /// Determines which polygons are culled + enum class ECullMode + { + CullBackFace, ///< Don't draw backfacing polygons + CullFrontFace, ///< Don't draw front facing polygons + Off ///< Don't do culling and draw both sides + }; + + /// Draw some geometry + /// @param inModelMatrix is the matrix that transforms the geometry to world space. + /// @param inWorldSpaceBounds is the bounding box of the geometry after transforming it into world space. + /// @param inLODScaleSq is the squared scale of the model matrix, it is multiplied with the LOD distances in inGeometry to calculate the real LOD distance (so a number > 1 will force a higher LOD). + /// @param inModelColor is the color with which to multiply the vertex colors in inGeometry. + /// @param inGeometry The geometry to draw. + /// @param inCullMode determines which polygons are culled. + /// @param inCastShadow determines if this geometry should cast a shadow or not. + /// @param inDrawMode determines if we draw the geometry solid or in wireframe. + virtual void DrawGeometry(RMat44Arg inModelMatrix, const AABox &inWorldSpaceBounds, float inLODScaleSq, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode = ECullMode::CullBackFace, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid) = 0; + void DrawGeometry(RMat44Arg inModelMatrix, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode = ECullMode::CullBackFace, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid) { DrawGeometry(inModelMatrix, inGeometry->mBounds.Transformed(inModelMatrix), max(max(inModelMatrix.GetAxisX().LengthSq(), inModelMatrix.GetAxisY().LengthSq()), inModelMatrix.GetAxisZ().LengthSq()), inModelColor, inGeometry, inCullMode, inCastShadow, inDrawMode); } + + /// Draw text + virtual void DrawText3D(RVec3Arg inPosition, const string_view &inString, ColorArg inColor = Color::sWhite, float inHeight = 0.5f) = 0; + +protected: + /// Initialize the system, must be called from the constructor of the DebugRenderer implementation + void Initialize(); + +private: + /// Recursive helper function for DrawWireUnitSphere + void DrawWireUnitSphereRecursive(RMat44Arg inMatrix, ColorArg inColor, Vec3Arg inDir1, Vec3Arg inDir2, Vec3Arg inDir3, int inLevel); + + /// Helper functions to create a box + void CreateQuad(Array &ioIndices, Array &ioVertices, Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, Vec3Arg inV4); + + /// Helper functions to create a vertex and index buffer for a sphere + void Create8thSphereRecursive(Array &ioIndices, Array &ioVertices, Vec3Arg inDir1, uint32 &ioIdx1, Vec3Arg inDir2, uint32 &ioIdx2, Vec3Arg inDir3, uint32 &ioIdx3, const Float2 &inUV, SupportFunction inGetSupport, int inLevel); + void Create8thSphere(Array &ioIndices, Array &ioVertices, Vec3Arg inDir1, Vec3Arg inDir2, Vec3Arg inDir3, const Float2 &inUV, SupportFunction inGetSupport, int inLevel); + + /// Helper functions to create a vertex and index buffer for a cylinder + Batch CreateCylinder(float inTop, float inBottom, float inTopRadius, float inBottomRadius, int inLevel); + + /// Helper function for DrawSwingConeLimits and DrawSwingPyramidLimits + Geometry * CreateSwingLimitGeometry(int inNumSegments, const Vec3 *inVertices); + + // Predefined shapes + GeometryRef mBox; + GeometryRef mSphere; + GeometryRef mCapsuleTop; + GeometryRef mCapsuleMid; + GeometryRef mCapsuleBottom; + GeometryRef mOpenCone; + GeometryRef mCylinder; + + struct SwingConeLimits + { + bool operator == (const SwingConeLimits &inRHS) const + { + return mSwingYHalfAngle == inRHS.mSwingYHalfAngle + && mSwingZHalfAngle == inRHS.mSwingZHalfAngle; + } + + float mSwingYHalfAngle; + float mSwingZHalfAngle; + }; + + JPH_MAKE_HASH_STRUCT(SwingConeLimits, SwingConeLimitsHasher, t.mSwingYHalfAngle, t.mSwingZHalfAngle) + + using SwingConeBatches = UnorderedMap; + SwingConeBatches mSwingConeLimits; + SwingConeBatches mPrevSwingConeLimits; + + struct SwingPyramidLimits + { + bool operator == (const SwingPyramidLimits &inRHS) const + { + return mMinSwingYAngle == inRHS.mMinSwingYAngle + && mMaxSwingYAngle == inRHS.mMaxSwingYAngle + && mMinSwingZAngle == inRHS.mMinSwingZAngle + && mMaxSwingZAngle == inRHS.mMaxSwingZAngle; + } + + float mMinSwingYAngle; + float mMaxSwingYAngle; + float mMinSwingZAngle; + float mMaxSwingZAngle; + }; + + JPH_MAKE_HASH_STRUCT(SwingPyramidLimits, SwingPyramidLimitsHasher, t.mMinSwingYAngle, t.mMaxSwingYAngle, t.mMinSwingZAngle, t.mMaxSwingZAngle) + + using SwingPyramidBatches = UnorderedMap; + SwingPyramidBatches mSwingPyramidLimits; + SwingPyramidBatches mPrevSwingPyramidLimits; + + using PieBatces = UnorderedMap; + PieBatces mPieLimits; + PieBatces mPrevPieLimits; + + struct TaperedCylinder + { + bool operator == (const TaperedCylinder &inRHS) const + { + return mTop == inRHS.mTop + && mBottom == inRHS.mBottom + && mTopRadius == inRHS.mTopRadius + && mBottomRadius == inRHS.mBottomRadius; + } + + float mTop; + float mBottom; + float mTopRadius; + float mBottomRadius; + }; + + JPH_MAKE_HASH_STRUCT(TaperedCylinder, TaperedCylinderHasher, t.mTop, t.mBottom, t.mTopRadius, t.mBottomRadius) + + using TaperedCylinderBatces = UnorderedMap; + TaperedCylinderBatces mTaperedCylinders; + TaperedCylinderBatces mPrevTaperedCylinders; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererPlayback.cpp b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererPlayback.cpp new file mode 100644 index 000000000000..bea7e4e08f87 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererPlayback.cpp @@ -0,0 +1,168 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#ifdef JPH_DEBUG_RENDERER + +#include + +JPH_NAMESPACE_BEGIN + +void DebugRendererPlayback::Parse(StreamIn &inStream) +{ + using ECommand = DebugRendererRecorder::ECommand; + + for (;;) + { + // Read the next command + ECommand command; + inStream.Read(command); + + if (inStream.IsEOF() || inStream.IsFailed()) + return; + + if (command == ECommand::CreateBatch) + { + uint32 id; + inStream.Read(id); + + uint32 triangle_count; + inStream.Read(triangle_count); + + DebugRenderer::Triangle *triangles = new DebugRenderer::Triangle [triangle_count]; + inStream.ReadBytes(triangles, triangle_count * sizeof(DebugRenderer::Triangle)); + + mBatches.insert({ id, mRenderer.CreateTriangleBatch(triangles, triangle_count) }); + + delete [] triangles; + } + else if (command == ECommand::CreateBatchIndexed) + { + uint32 id; + inStream.Read(id); + + uint32 vertex_count; + inStream.Read(vertex_count); + + DebugRenderer::Vertex *vertices = new DebugRenderer::Vertex [vertex_count]; + inStream.ReadBytes(vertices, vertex_count * sizeof(DebugRenderer::Vertex)); + + uint32 index_count; + inStream.Read(index_count); + + uint32 *indices = new uint32 [index_count]; + inStream.ReadBytes(indices, index_count * sizeof(uint32)); + + mBatches.insert({ id, mRenderer.CreateTriangleBatch(vertices, vertex_count, indices, index_count) }); + + delete [] indices; + delete [] vertices; + } + else if (command == ECommand::CreateGeometry) + { + uint32 geometry_id; + inStream.Read(geometry_id); + + AABox bounds; + inStream.Read(bounds.mMin); + inStream.Read(bounds.mMax); + + DebugRenderer::GeometryRef geometry = new DebugRenderer::Geometry(bounds); + mGeometries[geometry_id] = geometry; + + uint32 num_lods; + inStream.Read(num_lods); + for (uint32 l = 0; l < num_lods; ++l) + { + DebugRenderer::LOD lod; + inStream.Read(lod.mDistance); + + uint32 batch_id; + inStream.Read(batch_id); + lod.mTriangleBatch = mBatches.find(batch_id)->second; + + geometry->mLODs.push_back(lod); + } + } + else if (command == ECommand::EndFrame) + { + mFrames.push_back({}); + Frame &frame = mFrames.back(); + + // Read all lines + uint32 num_lines = 0; + inStream.Read(num_lines); + frame.mLines.resize(num_lines); + for (DebugRendererRecorder::LineBlob &line : frame.mLines) + { + inStream.Read(line.mFrom); + inStream.Read(line.mTo); + inStream.Read(line.mColor); + } + + // Read all triangles + uint32 num_triangles = 0; + inStream.Read(num_triangles); + frame.mTriangles.resize(num_triangles); + for (DebugRendererRecorder::TriangleBlob &triangle : frame.mTriangles) + { + inStream.Read(triangle.mV1); + inStream.Read(triangle.mV2); + inStream.Read(triangle.mV3); + inStream.Read(triangle.mColor); + inStream.Read(triangle.mCastShadow); + } + + // Read all texts + uint32 num_texts = 0; + inStream.Read(num_texts); + frame.mTexts.resize(num_texts); + for (DebugRendererRecorder::TextBlob &text : frame.mTexts) + { + inStream.Read(text.mPosition); + inStream.Read(text.mString); + inStream.Read(text.mColor); + inStream.Read(text.mHeight); + } + + // Read all geometries + uint32 num_geometries = 0; + inStream.Read(num_geometries); + frame.mGeometries.resize(num_geometries); + for (DebugRendererRecorder::GeometryBlob &geom : frame.mGeometries) + { + inStream.Read(geom.mModelMatrix); + inStream.Read(geom.mModelColor); + inStream.Read(geom.mGeometryID); + inStream.Read(geom.mCullMode); + inStream.Read(geom.mCastShadow); + inStream.Read(geom.mDrawMode); + } + } + else + JPH_ASSERT(false); + } +} + +void DebugRendererPlayback::DrawFrame(uint inFrameNumber) const +{ + const Frame &frame = mFrames[inFrameNumber]; + + for (const DebugRendererRecorder::LineBlob &line : frame.mLines) + mRenderer.DrawLine(line.mFrom, line.mTo, line.mColor); + + for (const DebugRendererRecorder::TriangleBlob &triangle : frame.mTriangles) + mRenderer.DrawTriangle(triangle.mV1, triangle.mV2, triangle.mV3, triangle.mColor, triangle.mCastShadow); + + for (const DebugRendererRecorder::TextBlob &text : frame.mTexts) + mRenderer.DrawText3D(text.mPosition, text.mString, text.mColor, text.mHeight); + + for (const DebugRendererRecorder::GeometryBlob &geom : frame.mGeometries) + mRenderer.DrawGeometry(geom.mModelMatrix, geom.mModelColor, mGeometries.find(geom.mGeometryID)->second, geom.mCullMode, geom.mCastShadow, geom.mDrawMode); +} + +JPH_NAMESPACE_END + +#endif // JPH_DEBUG_RENDERER diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererPlayback.h b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererPlayback.h new file mode 100644 index 000000000000..23ed45423898 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererPlayback.h @@ -0,0 +1,48 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifndef JPH_DEBUG_RENDERER + #error This file should only be included when JPH_DEBUG_RENDERER is defined +#endif // !JPH_DEBUG_RENDERER + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that can read a recorded stream from DebugRendererRecorder and plays it back trough a DebugRenderer +class JPH_DEBUG_RENDERER_EXPORT DebugRendererPlayback +{ +public: + /// Constructor + DebugRendererPlayback(DebugRenderer &inRenderer) : mRenderer(inRenderer) { } + + /// Parse a stream of frames + void Parse(StreamIn &inStream); + + /// Get the number of parsed frames + uint GetNumFrames() const { return (uint)mFrames.size(); } + + /// Draw a frame + void DrawFrame(uint inFrameNumber) const; + +private: + /// The debug renderer we're using to do the actual rendering + DebugRenderer & mRenderer; + + /// Mapping of ID to batch + UnorderedMap mBatches; + + /// Mapping of ID to geometry + UnorderedMap mGeometries; + + /// The list of parsed frames + using Frame = DebugRendererRecorder::Frame; + Array mFrames; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererRecorder.cpp b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererRecorder.cpp new file mode 100644 index 000000000000..2e25912957fa --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererRecorder.cpp @@ -0,0 +1,158 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#ifdef JPH_DEBUG_RENDERER + +#include + +JPH_NAMESPACE_BEGIN + +void DebugRendererRecorder::DrawLine(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor) +{ + lock_guard lock(mMutex); + + mCurrentFrame.mLines.push_back({ inFrom, inTo, inColor }); +} + +void DebugRendererRecorder::DrawTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor, ECastShadow inCastShadow) +{ + lock_guard lock(mMutex); + + mCurrentFrame.mTriangles.push_back({ inV1, inV2, inV3, inColor, inCastShadow }); +} + +DebugRenderer::Batch DebugRendererRecorder::CreateTriangleBatch(const Triangle *inTriangles, int inTriangleCount) +{ + if (inTriangles == nullptr || inTriangleCount == 0) + return new BatchImpl(0); + + lock_guard lock(mMutex); + + mStream.Write(ECommand::CreateBatch); + + uint32 batch_id = mNextBatchID++; + JPH_ASSERT(batch_id != 0); + mStream.Write(batch_id); + mStream.Write((uint32)inTriangleCount); + mStream.WriteBytes(inTriangles, inTriangleCount * sizeof(Triangle)); + + return new BatchImpl(batch_id); +} + +DebugRenderer::Batch DebugRendererRecorder::CreateTriangleBatch(const Vertex *inVertices, int inVertexCount, const uint32 *inIndices, int inIndexCount) +{ + if (inVertices == nullptr || inVertexCount == 0 || inIndices == nullptr || inIndexCount == 0) + return new BatchImpl(0); + + lock_guard lock(mMutex); + + mStream.Write(ECommand::CreateBatchIndexed); + + uint32 batch_id = mNextBatchID++; + JPH_ASSERT(batch_id != 0); + mStream.Write(batch_id); + mStream.Write((uint32)inVertexCount); + mStream.WriteBytes(inVertices, inVertexCount * sizeof(Vertex)); + mStream.Write((uint32)inIndexCount); + mStream.WriteBytes(inIndices, inIndexCount * sizeof(uint32)); + + return new BatchImpl(batch_id); +} + +void DebugRendererRecorder::DrawGeometry(RMat44Arg inModelMatrix, const AABox &inWorldSpaceBounds, float inLODScaleSq, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + lock_guard lock(mMutex); + + // See if this geometry was used before + uint32 &geometry_id = mGeometries[inGeometry]; + if (geometry_id == 0) + { + mStream.Write(ECommand::CreateGeometry); + + // Create a new ID + geometry_id = mNextGeometryID++; + JPH_ASSERT(geometry_id != 0); + mStream.Write(geometry_id); + + // Save bounds + mStream.Write(inGeometry->mBounds.mMin); + mStream.Write(inGeometry->mBounds.mMax); + + // Save the LODs + mStream.Write((uint32)inGeometry->mLODs.size()); + for (const LOD & lod : inGeometry->mLODs) + { + mStream.Write(lod.mDistance); + mStream.Write(static_cast(lod.mTriangleBatch.GetPtr())->mID); + } + } + + mCurrentFrame.mGeometries.push_back({ inModelMatrix, inModelColor, geometry_id, inCullMode, inCastShadow, inDrawMode }); +} + +void DebugRendererRecorder::DrawText3D(RVec3Arg inPosition, const string_view &inString, ColorArg inColor, float inHeight) +{ + lock_guard lock(mMutex); + + mCurrentFrame.mTexts.push_back({ inPosition, inString, inColor, inHeight }); +} + +void DebugRendererRecorder::EndFrame() +{ + lock_guard lock(mMutex); + + mStream.Write(ECommand::EndFrame); + + // Write all lines + mStream.Write((uint32)mCurrentFrame.mLines.size()); + for (const LineBlob &line : mCurrentFrame.mLines) + { + mStream.Write(line.mFrom); + mStream.Write(line.mTo); + mStream.Write(line.mColor); + } + mCurrentFrame.mLines.clear(); + + // Write all triangles + mStream.Write((uint32)mCurrentFrame.mTriangles.size()); + for (const TriangleBlob &triangle : mCurrentFrame.mTriangles) + { + mStream.Write(triangle.mV1); + mStream.Write(triangle.mV2); + mStream.Write(triangle.mV3); + mStream.Write(triangle.mColor); + mStream.Write(triangle.mCastShadow); + } + mCurrentFrame.mTriangles.clear(); + + // Write all texts + mStream.Write((uint32)mCurrentFrame.mTexts.size()); + for (const TextBlob &text : mCurrentFrame.mTexts) + { + mStream.Write(text.mPosition); + mStream.Write(text.mString); + mStream.Write(text.mColor); + mStream.Write(text.mHeight); + } + mCurrentFrame.mTexts.clear(); + + // Write all geometries + mStream.Write((uint32)mCurrentFrame.mGeometries.size()); + for (const GeometryBlob &geom : mCurrentFrame.mGeometries) + { + mStream.Write(geom.mModelMatrix); + mStream.Write(geom.mModelColor); + mStream.Write(geom.mGeometryID); + mStream.Write(geom.mCullMode); + mStream.Write(geom.mCastShadow); + mStream.Write(geom.mDrawMode); + } + mCurrentFrame.mGeometries.clear(); +} + +JPH_NAMESPACE_END + +#endif // JPH_DEBUG_RENDERER diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererRecorder.h b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererRecorder.h new file mode 100644 index 000000000000..9608e03c908b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererRecorder.h @@ -0,0 +1,130 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifndef JPH_DEBUG_RENDERER + #error This file should only be included when JPH_DEBUG_RENDERER is defined +#endif // !JPH_DEBUG_RENDERER + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Implementation of DebugRenderer that records the API invocations to be played back later +class JPH_DEBUG_RENDERER_EXPORT DebugRendererRecorder final : public DebugRenderer +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + DebugRendererRecorder(StreamOut &inStream) : mStream(inStream) { Initialize(); } + + /// Implementation of DebugRenderer interface + virtual void DrawLine(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor) override; + virtual void DrawTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor, ECastShadow inCastShadow) override; + virtual Batch CreateTriangleBatch(const Triangle *inTriangles, int inTriangleCount) override; + virtual Batch CreateTriangleBatch(const Vertex *inVertices, int inVertexCount, const uint32 *inIndices, int inIndexCount) override; + virtual void DrawGeometry(RMat44Arg inModelMatrix, const AABox &inWorldSpaceBounds, float inLODScaleSq, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode, ECastShadow inCastShadow, EDrawMode inDrawMode) override; + virtual void DrawText3D(RVec3Arg inPosition, const string_view &inString, ColorArg inColor, float inHeight) override; + + /// Mark the end of a frame + void EndFrame(); + + /// Control commands written into the stream + enum class ECommand : uint8 + { + CreateBatch, + CreateBatchIndexed, + CreateGeometry, + EndFrame + }; + + /// Holds a single line segment + struct LineBlob + { + RVec3 mFrom; + RVec3 mTo; + Color mColor; + }; + + /// Holds a single triangle + struct TriangleBlob + { + RVec3 mV1; + RVec3 mV2; + RVec3 mV3; + Color mColor; + ECastShadow mCastShadow; + }; + + /// Holds a single text entry + struct TextBlob + { + TextBlob() = default; + TextBlob(RVec3Arg inPosition, const string_view &inString, ColorArg inColor, float inHeight) : mPosition(inPosition), mString(inString), mColor(inColor), mHeight(inHeight) { } + + RVec3 mPosition; + String mString; + Color mColor; + float mHeight; + }; + + /// Holds a single geometry draw call + struct GeometryBlob + { + RMat44 mModelMatrix; + Color mModelColor; + uint32 mGeometryID; + ECullMode mCullMode; + ECastShadow mCastShadow; + EDrawMode mDrawMode; + }; + + /// All information for a single frame + struct Frame + { + Array mLines; + Array mTriangles; + Array mTexts; + Array mGeometries; + }; + +private: + /// Implementation specific batch object + class BatchImpl : public RefTargetVirtual + { + public: + JPH_OVERRIDE_NEW_DELETE + + BatchImpl(uint32 inID) : mID(inID) { } + + virtual void AddRef() override { ++mRefCount; } + virtual void Release() override { if (--mRefCount == 0) delete this; } + + atomic mRefCount = 0; + uint32 mID; + }; + + /// Lock that prevents concurrent access to the internal structures + Mutex mMutex; + + /// Stream that recorded data will be sent to + StreamOut & mStream; + + /// Next available ID + uint32 mNextBatchID = 1; + uint32 mNextGeometryID = 1; + + /// Cached geometries and their IDs + UnorderedMap mGeometries; + + /// Data that is being accumulated for the current frame + Frame mCurrentFrame; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererSimple.cpp b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererSimple.cpp new file mode 100644 index 000000000000..a404d95a002c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererSimple.cpp @@ -0,0 +1,80 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#ifdef JPH_DEBUG_RENDERER + +#include + +JPH_NAMESPACE_BEGIN + +DebugRendererSimple::DebugRendererSimple() +{ + Initialize(); +} + +DebugRenderer::Batch DebugRendererSimple::CreateTriangleBatch(const Triangle *inTriangles, int inTriangleCount) +{ + BatchImpl *batch = new BatchImpl; + if (inTriangles == nullptr || inTriangleCount == 0) + return batch; + + batch->mTriangles.assign(inTriangles, inTriangles + inTriangleCount); + return batch; +} + +DebugRenderer::Batch DebugRendererSimple::CreateTriangleBatch(const Vertex *inVertices, int inVertexCount, const uint32 *inIndices, int inIndexCount) +{ + BatchImpl *batch = new BatchImpl; + if (inVertices == nullptr || inVertexCount == 0 || inIndices == nullptr || inIndexCount == 0) + return batch; + + // Convert indexed triangle list to triangle list + batch->mTriangles.resize(inIndexCount / 3); + for (size_t t = 0; t < batch->mTriangles.size(); ++t) + { + Triangle &triangle = batch->mTriangles[t]; + triangle.mV[0] = inVertices[inIndices[t * 3 + 0]]; + triangle.mV[1] = inVertices[inIndices[t * 3 + 1]]; + triangle.mV[2] = inVertices[inIndices[t * 3 + 2]]; + } + + return batch; +} + +void DebugRendererSimple::DrawGeometry(RMat44Arg inModelMatrix, const AABox &inWorldSpaceBounds, float inLODScaleSq, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + // Figure out which LOD to use + const LOD *lod = inGeometry->mLODs.data(); + if (mCameraPosSet) + lod = &inGeometry->GetLOD(Vec3(mCameraPos), inWorldSpaceBounds, inLODScaleSq); + + // Draw the batch + const BatchImpl *batch = static_cast(lod->mTriangleBatch.GetPtr()); + for (const Triangle &triangle : batch->mTriangles) + { + RVec3 v0 = inModelMatrix * Vec3(triangle.mV[0].mPosition); + RVec3 v1 = inModelMatrix * Vec3(triangle.mV[1].mPosition); + RVec3 v2 = inModelMatrix * Vec3(triangle.mV[2].mPosition); + Color color = inModelColor * triangle.mV[0].mColor; + + switch (inDrawMode) + { + case EDrawMode::Wireframe: + DrawLine(v0, v1, color); + DrawLine(v1, v2, color); + DrawLine(v2, v0, color); + break; + + case EDrawMode::Solid: + DrawTriangle(v0, v1, v2, color, inCastShadow); + break; + } + } +} + +JPH_NAMESPACE_END + +#endif // JPH_DEBUG_RENDERER diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererSimple.h b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererSimple.h new file mode 100644 index 000000000000..4a23ab758b71 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererSimple.h @@ -0,0 +1,88 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifndef JPH_DEBUG_RENDERER + #error This file should only be included when JPH_DEBUG_RENDERER is defined +#endif // !JPH_DEBUG_RENDERER + +#include + +JPH_NAMESPACE_BEGIN + +/// Inherit from this class to simplify implementing a debug renderer, start with this implementation: +/// +/// class MyDebugRenderer : public JPH::DebugRendererSimple +/// { +/// public: +/// virtual void DrawLine(JPH::RVec3Arg inFrom, JPH::RVec3Arg inTo, JPH::ColorArg inColor) override +/// { +/// // Implement +/// } +/// +/// virtual void DrawTriangle(JPH::RVec3Arg inV1, JPH::RVec3Arg inV2, JPH::RVec3Arg inV3, JPH::ColorArg inColor, ECastShadow inCastShadow) override +/// { +/// // Implement +/// } +/// +/// virtual void DrawText3D(JPH::RVec3Arg inPosition, const string_view &inString, JPH::ColorArg inColor, float inHeight) override +/// { +/// // Implement +/// } +/// }; +/// +/// Note that this class is meant to be a quick start for implementing a debug renderer, it is not the most efficient way to implement a debug renderer. +class JPH_DEBUG_RENDERER_EXPORT DebugRendererSimple : public DebugRenderer +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + DebugRendererSimple(); + + /// Should be called every frame by the application to provide the camera position. + /// This is used to determine the correct LOD for rendering. + void SetCameraPos(RVec3Arg inCameraPos) + { + mCameraPos = inCameraPos; + mCameraPosSet = true; + } + + /// Fallback implementation that uses DrawLine to draw a triangle (override this if you have a version that renders solid triangles) + virtual void DrawTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor, ECastShadow inCastShadow) override + { + DrawLine(inV1, inV2, inColor); + DrawLine(inV2, inV3, inColor); + DrawLine(inV3, inV1, inColor); + } + +protected: + /// Implementation of DebugRenderer interface + virtual Batch CreateTriangleBatch(const Triangle *inTriangles, int inTriangleCount) override; + virtual Batch CreateTriangleBatch(const Vertex *inVertices, int inVertexCount, const uint32 *inIndices, int inIndexCount) override; + virtual void DrawGeometry(RMat44Arg inModelMatrix, const AABox &inWorldSpaceBounds, float inLODScaleSq, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode, ECastShadow inCastShadow, EDrawMode inDrawMode) override; + +private: + /// Implementation specific batch object + class BatchImpl : public RefTargetVirtual + { + public: + JPH_OVERRIDE_NEW_DELETE + + virtual void AddRef() override { ++mRefCount; } + virtual void Release() override { if (--mRefCount == 0) delete this; } + + Array mTriangles; + + private: + atomic mRefCount = 0; + }; + + /// Last provided camera position + RVec3 mCameraPos; + bool mCameraPosSet = false; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/SkeletalAnimation.cpp b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletalAnimation.cpp new file mode 100644 index 000000000000..4bf87007e9a8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletalAnimation.cpp @@ -0,0 +1,110 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SkeletalAnimation::JointState) +{ + JPH_ADD_ATTRIBUTE(JointState, mRotation) + JPH_ADD_ATTRIBUTE(JointState, mTranslation) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SkeletalAnimation::Keyframe) +{ + JPH_ADD_BASE_CLASS(Keyframe, JointState) + + JPH_ADD_ATTRIBUTE(Keyframe, mTime) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SkeletalAnimation::AnimatedJoint) +{ + JPH_ADD_ATTRIBUTE(AnimatedJoint, mJointName) + JPH_ADD_ATTRIBUTE(AnimatedJoint, mKeyframes) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SkeletalAnimation) +{ + JPH_ADD_ATTRIBUTE(SkeletalAnimation, mAnimatedJoints) + JPH_ADD_ATTRIBUTE(SkeletalAnimation, mIsLooping) +} + + +void SkeletalAnimation::JointState::FromMatrix(Mat44Arg inMatrix) +{ + mRotation = inMatrix.GetQuaternion(); + mTranslation = inMatrix.GetTranslation(); +} + +float SkeletalAnimation::GetDuration() const +{ + if (!mAnimatedJoints.empty() && !mAnimatedJoints[0].mKeyframes.empty()) + return mAnimatedJoints[0].mKeyframes.back().mTime; + else + return 0.0f; +} + +void SkeletalAnimation::ScaleJoints(float inScale) +{ + for (SkeletalAnimation::AnimatedJoint &j : mAnimatedJoints) + for (SkeletalAnimation::Keyframe &k : j.mKeyframes) + k.mTranslation *= inScale; +} + +void SkeletalAnimation::Sample(float inTime, SkeletonPose &ioPose) const +{ + // Correct time when animation is looping + JPH_ASSERT(inTime >= 0.0f); + float duration = GetDuration(); + float time = duration > 0.0f && mIsLooping? fmod(inTime, duration) : inTime; + + for (const AnimatedJoint &aj : mAnimatedJoints) + { + // Do binary search for keyframe + int high = (int)aj.mKeyframes.size(), low = -1; + while (high - low > 1) + { + int probe = (high + low) / 2; + if (aj.mKeyframes[probe].mTime < time) + low = probe; + else + high = probe; + } + + JointState &state = ioPose.GetJoint(ioPose.GetSkeleton()->GetJointIndex(aj.mJointName)); + + if (low == -1) + { + // Before first key, return first key + state = static_cast(aj.mKeyframes.front()); + } + else if (high == (int)aj.mKeyframes.size()) + { + // Beyond last key, return last key + state = static_cast(aj.mKeyframes.back()); + } + else + { + // Interpolate + const Keyframe &s1 = aj.mKeyframes[low]; + const Keyframe &s2 = aj.mKeyframes[low + 1]; + + float fraction = (time - s1.mTime) / (s2.mTime - s1.mTime); + JPH_ASSERT(fraction >= 0.0f && fraction <= 1.0f); + + state.mTranslation = (1.0f - fraction) * s1.mTranslation + fraction * s2.mTranslation; + JPH_ASSERT(s1.mRotation.IsNormalized()); + JPH_ASSERT(s2.mRotation.IsNormalized()); + state.mRotation = s1.mRotation.SLERP(s2.mRotation, fraction); + JPH_ASSERT(state.mRotation.IsNormalized()); + } + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/SkeletalAnimation.h b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletalAnimation.h new file mode 100644 index 000000000000..344c6046fa10 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletalAnimation.h @@ -0,0 +1,77 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class SkeletonPose; + +/// Resource for a skinned animation +class JPH_EXPORT SkeletalAnimation : public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SkeletalAnimation) + +public: + /// Contains the current state of a joint, a local space transformation relative to its parent joint + class JointState + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, JointState) + + public: + /// Convert from a local space matrix + void FromMatrix(Mat44Arg inMatrix); + + /// Convert to matrix representation + inline Mat44 ToMatrix() const { return Mat44::sRotationTranslation(mRotation, mTranslation); } + + Quat mRotation = Quat::sIdentity(); ///< Local space rotation of the joint + Vec3 mTranslation = Vec3::sZero(); ///< Local space translation of the joint + }; + + /// Contains the state of a single joint at a particular time + class Keyframe : public JointState + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Keyframe) + + public: + float mTime = 0.0f; ///< Time of keyframe in seconds + }; + + using KeyframeVector = Array; + + /// Contains the animation for a single joint + class AnimatedJoint + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, AnimatedJoint) + + public: + String mJointName; ///< Name of the joint + KeyframeVector mKeyframes; ///< List of keyframes over time + }; + + using AnimatedJointVector = Array; + + /// Get the length (in seconds) of this animation + float GetDuration() const; + + /// Scale the size of all joints by inScale + void ScaleJoints(float inScale); + + /// Get the (interpolated) joint transforms at time inTime + void Sample(float inTime, SkeletonPose &ioPose) const; + + /// Get joint samples + const AnimatedJointVector & GetAnimatedJoints() const { return mAnimatedJoints; } + AnimatedJointVector & GetAnimatedJoints() { return mAnimatedJoints; } + +private: + AnimatedJointVector mAnimatedJoints; ///< List of joints and keyframes + bool mIsLooping = true; ///< If this animation loops back to start +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/Skeleton.cpp b/thirdparty/jolt_physics/Jolt/Skeleton/Skeleton.cpp new file mode 100644 index 000000000000..5ab7f56e792a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Skeleton/Skeleton.cpp @@ -0,0 +1,82 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(Skeleton::Joint) +{ + JPH_ADD_ATTRIBUTE(Joint, mName) + JPH_ADD_ATTRIBUTE(Joint, mParentName) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(Skeleton) +{ + JPH_ADD_ATTRIBUTE(Skeleton, mJoints) +} + +int Skeleton::GetJointIndex(const string_view &inName) const +{ + for (int i = 0; i < (int)mJoints.size(); ++i) + if (mJoints[i].mName == inName) + return i; + + return -1; +} + +void Skeleton::CalculateParentJointIndices() +{ + for (Joint &j : mJoints) + j.mParentJointIndex = GetJointIndex(j.mParentName); +} + +bool Skeleton::AreJointsCorrectlyOrdered() const +{ + for (int i = 0; i < (int)mJoints.size(); ++i) + if (mJoints[i].mParentJointIndex >= i) + return false; + + return true; +} + +void Skeleton::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write((uint32)mJoints.size()); + for (const Joint &j : mJoints) + { + inStream.Write(j.mName); + inStream.Write(j.mParentJointIndex); + inStream.Write(j.mParentName); + } +} + +Skeleton::SkeletonResult Skeleton::sRestoreFromBinaryState(StreamIn &inStream) +{ + Ref skeleton = new Skeleton; + + uint32 len = 0; + inStream.Read(len); + skeleton->mJoints.resize(len); + for (Joint &j : skeleton->mJoints) + { + inStream.Read(j.mName); + inStream.Read(j.mParentJointIndex); + inStream.Read(j.mParentName); + } + + SkeletonResult result; + if (inStream.IsEOF() || inStream.IsFailed()) + result.SetError("Failed to read skeleton from stream"); + else + result.Set(skeleton); + return result; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/Skeleton.h b/thirdparty/jolt_physics/Jolt/Skeleton/Skeleton.h new file mode 100644 index 000000000000..f53df31dfdaa --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Skeleton/Skeleton.h @@ -0,0 +1,72 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; + +/// Resource that contains the joint hierarchy for a skeleton +class JPH_EXPORT Skeleton : public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Skeleton) + +public: + using SkeletonResult = Result>; + + /// Declare internal structure for a joint + class Joint + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Joint) + + public: + Joint() = default; + Joint(const string_view &inName, const string_view &inParentName, int inParentJointIndex) : mName(inName), mParentName(inParentName), mParentJointIndex(inParentJointIndex) { } + + String mName; ///< Name of the joint + String mParentName; ///< Name of parent joint + int mParentJointIndex = -1; ///< Index of parent joint (in mJoints) or -1 if it has no parent + }; + + using JointVector = Array; + + ///@name Access to the joints + ///@{ + const JointVector & GetJoints() const { return mJoints; } + JointVector & GetJoints() { return mJoints; } + int GetJointCount() const { return (int)mJoints.size(); } + const Joint & GetJoint(int inJoint) const { return mJoints[inJoint]; } + Joint & GetJoint(int inJoint) { return mJoints[inJoint]; } + uint AddJoint(const string_view &inName, const string_view &inParentName = string_view()) { mJoints.emplace_back(inName, inParentName, -1); return (uint)mJoints.size() - 1; } + uint AddJoint(const string_view &inName, int inParentIndex) { mJoints.emplace_back(inName, inParentIndex >= 0? mJoints[inParentIndex].mName : String(), inParentIndex); return (uint)mJoints.size() - 1; } + ///@} + + /// Find joint by name + int GetJointIndex(const string_view &inName) const; + + /// Fill in parent joint indices based on name + void CalculateParentJointIndices(); + + /// Many of the algorithms that use the Skeleton class require that parent joints are in the mJoints array before their children. + /// This function returns true if this is the case, false if not. + bool AreJointsCorrectlyOrdered() const; + + /// Saves the state of this object in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restore the state of this object from inStream. + static SkeletonResult sRestoreFromBinaryState(StreamIn &inStream); + +private: + /// Joints + JointVector mJoints; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonMapper.cpp b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonMapper.cpp new file mode 100644 index 000000000000..add2956d541b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonMapper.cpp @@ -0,0 +1,237 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +void SkeletonMapper::Initialize(const Skeleton *inSkeleton1, const Mat44 *inNeutralPose1, const Skeleton *inSkeleton2, const Mat44 *inNeutralPose2, const CanMapJoint &inCanMapJoint) +{ + JPH_ASSERT(mMappings.empty() && mChains.empty() && mUnmapped.empty()); // Should not be initialized yet + + // Count joints + int n1 = inSkeleton1->GetJointCount(); + int n2 = inSkeleton2->GetJointCount(); + JPH_ASSERT(n1 <= n2, "Skeleton 1 should be the low detail skeleton!"); + + // Keep track of mapped joints (initialize to false) + Array mapped1(n1, false); + Array mapped2(n2, false); + + // Find joints that can be mapped directly + for (int j1 = 0; j1 < n1; ++j1) + for (int j2 = 0; j2 < n2; ++j2) + if (inCanMapJoint(inSkeleton1, j1, inSkeleton2, j2)) + { + // Calculate the transform that takes this joint from skeleton 1 to 2 + Mat44 joint_1_to_2 = inNeutralPose1[j1].Inversed() * inNeutralPose2[j2]; + + // Ensure bottom right element is 1 (numerical imprecision in the inverse can make this not so) + joint_1_to_2(3, 3) = 1.0f; + + mMappings.emplace_back(j1, j2, joint_1_to_2); + mapped1[j1] = true; + mapped2[j2] = true; + break; + } + + Array cur_chain; // Taken out of the loop to minimize amount of allocations + + // Find joint chains + for (int m1 = 0; m1 < (int)mMappings.size(); ++m1) + { + Array chain2; + int chain2_m = -1; + + for (int m2 = m1 + 1; m2 < (int)mMappings.size(); ++m2) + { + // Find the chain from back from m2 to m1 + int start = mMappings[m1].mJointIdx2; + int end = mMappings[m2].mJointIdx2; + int cur = end; + cur_chain.clear(); // Should preserve memory + do + { + cur_chain.push_back(cur); + cur = inSkeleton2->GetJoint(cur).mParentJointIndex; + } + while (cur >= 0 && cur != start && !mapped2[cur]); + cur_chain.push_back(start); + + if (cur == start // This should be the correct chain + && cur_chain.size() > 2 // It should have joints between the mapped joints + && cur_chain.size() > chain2.size()) // And it should be the longest so far + { + chain2.swap(cur_chain); + chain2_m = m2; + } + } + + if (!chain2.empty()) + { + // Get the chain for 1 + Array chain1; + int start = mMappings[m1].mJointIdx1; + int cur = mMappings[chain2_m].mJointIdx1; + do + { + chain1.push_back(cur); + cur = inSkeleton1->GetJoint(cur).mParentJointIndex; + } + while (cur >= 0 && cur != start && !mapped1[cur]); + chain1.push_back(start); + + // If the chain exists in 1 too + if (cur == start) + { + // Reverse the chains + std::reverse(chain1.begin(), chain1.end()); + std::reverse(chain2.begin(), chain2.end()); + + // Mark elements mapped + for (int j1 : chain1) + mapped1[j1] = true; + for (int j2 : chain2) + mapped2[j2] = true; + + // Insert the chain + mChains.emplace_back(std::move(chain1), std::move(chain2)); + } + } + } + + // Collect unmapped joints from 2 + for (int j2 = 0; j2 < n2; ++j2) + if (!mapped2[j2]) + mUnmapped.emplace_back(j2, inSkeleton2->GetJoint(j2).mParentJointIndex); +} + +void SkeletonMapper::LockTranslations(const Skeleton *inSkeleton2, const bool *inLockedTranslations, const Mat44 *inNeutralPose2) +{ + JPH_ASSERT(inSkeleton2->AreJointsCorrectlyOrdered()); + + int n = inSkeleton2->GetJointCount(); + + // Copy locked joints to array but don't actually include the first joint (this is physics driven) + for (int i = 0; i < n; ++i) + if (inLockedTranslations[i]) + { + Locked l; + l.mJointIdx = i; + l.mParentJointIdx = inSkeleton2->GetJoint(i).mParentJointIndex; + if (l.mParentJointIdx >= 0) + l.mTranslation = inNeutralPose2[l.mParentJointIdx].Inversed() * inNeutralPose2[i].GetTranslation(); + else + l.mTranslation = inNeutralPose2[i].GetTranslation(); + mLockedTranslations.push_back(l); + } +} + +void SkeletonMapper::LockAllTranslations(const Skeleton *inSkeleton2, const Mat44 *inNeutralPose2) +{ + JPH_ASSERT(!mMappings.empty(), "Call Initialize first!"); + JPH_ASSERT(inSkeleton2->AreJointsCorrectlyOrdered()); + + // The first mapping is the top most one (remember that joints should be ordered so that parents go before children). + // Because we created the mappings from the lowest joint first, this should contain the first mappable joint. + int root_idx = mMappings[0].mJointIdx2; + + // Create temp array to hold locked joints + int n = inSkeleton2->GetJointCount(); + bool *locked_translations = (bool *)JPH_STACK_ALLOC(n * sizeof(bool)); + memset(locked_translations, 0, n * sizeof(bool)); + + // Mark root as locked + locked_translations[root_idx] = true; + + // Loop over all joints and propagate the locked flag to all children + for (int i = root_idx + 1; i < n; ++i) + { + int parent_idx = inSkeleton2->GetJoint(i).mParentJointIndex; + if (parent_idx >= 0) + locked_translations[i] = locked_translations[parent_idx]; + } + + // Unmark root because we don't actually want to include this (this determines the position of the entire ragdoll) + locked_translations[root_idx] = false; + + // Call the generic function + LockTranslations(inSkeleton2, locked_translations, inNeutralPose2); +} + +void SkeletonMapper::Map(const Mat44 *inPose1ModelSpace, const Mat44 *inPose2LocalSpace, Mat44 *outPose2ModelSpace) const +{ + // Apply direct mappings + for (const Mapping &m : mMappings) + outPose2ModelSpace[m.mJointIdx2] = inPose1ModelSpace[m.mJointIdx1] * m.mJoint1To2; + + // Apply chain mappings + for (const Chain &c : mChains) + { + // Calculate end of chain given local space transforms of the joints of the chain + Mat44 &chain_start = outPose2ModelSpace[c.mJointIndices2.front()]; + Mat44 chain_end = chain_start; + for (int j = 1; j < (int)c.mJointIndices2.size(); ++j) + chain_end = chain_end * inPose2LocalSpace[c.mJointIndices2[j]]; + + // Calculate the direction in world space for skeleton 1 and skeleton 2 and the rotation between them + Vec3 actual = chain_end.GetTranslation() - chain_start.GetTranslation(); + Vec3 desired = inPose1ModelSpace[c.mJointIndices1.back()].GetTranslation() - inPose1ModelSpace[c.mJointIndices1.front()].GetTranslation(); + Quat rotation = Quat::sFromTo(actual, desired); + + // Rotate the start of the chain + chain_start.SetRotation(Mat44::sRotation(rotation) * chain_start.GetRotation()); + + // Update all joints but the first and the last joint using their local space transforms + for (int j = 1; j < (int)c.mJointIndices2.size() - 1; ++j) + { + int parent = c.mJointIndices2[j - 1]; + int child = c.mJointIndices2[j]; + outPose2ModelSpace[child] = outPose2ModelSpace[parent] * inPose2LocalSpace[child]; + } + } + + // All unmapped joints take the local pose and convert it to model space + for (const Unmapped &u : mUnmapped) + if (u.mParentJointIdx >= 0) + { + JPH_ASSERT(u.mParentJointIdx < u.mJointIdx, "Joints must be ordered: parents first"); + outPose2ModelSpace[u.mJointIdx] = outPose2ModelSpace[u.mParentJointIdx] * inPose2LocalSpace[u.mJointIdx]; + } + else + outPose2ModelSpace[u.mJointIdx] = inPose2LocalSpace[u.mJointIdx]; + + // Update all locked joint translations + for (const Locked &l : mLockedTranslations) + outPose2ModelSpace[l.mJointIdx].SetTranslation(outPose2ModelSpace[l.mParentJointIdx] * l.mTranslation); +} + +void SkeletonMapper::MapReverse(const Mat44 *inPose2ModelSpace, Mat44 *outPose1ModelSpace) const +{ + // Normally each joint in skeleton 1 should be present in the mapping, so we only need to apply the direct mappings + for (const Mapping &m : mMappings) + outPose1ModelSpace[m.mJointIdx1] = inPose2ModelSpace[m.mJointIdx2] * m.mJoint2To1; +} + +int SkeletonMapper::GetMappedJointIdx(int inJoint1Idx) const +{ + for (const Mapping &m : mMappings) + if (m.mJointIdx1 == inJoint1Idx) + return m.mJointIdx2; + + return -1; +} + +bool SkeletonMapper::IsJointTranslationLocked(int inJoint2Idx) const +{ + for (const Locked &l : mLockedTranslations) + if (l.mJointIdx == inJoint2Idx) + return true; + + return false; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonMapper.h b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonMapper.h new file mode 100644 index 000000000000..05cc8866a4b7 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonMapper.h @@ -0,0 +1,145 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that is able to map a low detail (ragdoll) skeleton to a high detail (animation) skeleton and vice versa +class JPH_EXPORT SkeletonMapper : public RefTarget +{ +public: + /// A joint that maps 1-on-1 to a joint in the other skeleton + class Mapping + { + public: + Mapping() = default; + Mapping(int inJointIdx1, int inJointIdx2, Mat44Arg inJoint1To2) : mJointIdx1(inJointIdx1), mJointIdx2(inJointIdx2), mJoint1To2(inJoint1To2), mJoint2To1(inJoint1To2.Inversed()) + { + // Ensure bottom right element is 1 (numerical imprecision in the inverse can make this not so) + mJoint2To1(3, 3) = 1.0f; + } + + int mJointIdx1; ///< Index of joint from skeleton 1 + int mJointIdx2; ///< Corresponding index of joint from skeleton 2 + Mat44 mJoint1To2; ///< Transforms this joint from skeleton 1 to 2 + Mat44 mJoint2To1; ///< Inverse of the transform above + }; + + /// A joint chain that starts with a 1-on-1 mapped joint and ends with a 1-on-1 mapped joint with intermediate joints that cannot be mapped + class Chain + { + public: + Chain() = default; + Chain(Array &&inJointIndices1, Array &&inJointIndices2) : mJointIndices1(std::move(inJointIndices1)), mJointIndices2(std::move(inJointIndices2)) { } + + Array mJointIndices1; ///< Joint chain from skeleton 1 + Array mJointIndices2; ///< Corresponding joint chain from skeleton 2 + }; + + /// Joints that could not be mapped from skeleton 1 to 2 + class Unmapped + { + public: + Unmapped() = default; + Unmapped(int inJointIdx, int inParentJointIdx) : mJointIdx(inJointIdx), mParentJointIdx(inParentJointIdx) { } + + int mJointIdx; ///< Joint index of unmappable joint + int mParentJointIdx; ///< Parent joint index of unmappable joint + }; + + /// Joints that should have their translation locked (fixed) + class Locked + { + public: + int mJointIdx; ///< Joint index of joint with locked translation (in skeleton 2) + int mParentJointIdx; ///< Parent joint index of joint with locked translation (in skeleton 2) + Vec3 mTranslation; ///< Translation of neutral pose + }; + + /// A function that is called to determine if a joint can be mapped from source to target skeleton + using CanMapJoint = function; + + /// Default function that checks if the names of the joints are equal + static bool sDefaultCanMapJoint(const Skeleton *inSkeleton1, int inIndex1, const Skeleton *inSkeleton2, int inIndex2) + { + return inSkeleton1->GetJoint(inIndex1).mName == inSkeleton2->GetJoint(inIndex2).mName; + } + + /// Initialize the skeleton mapper. Skeleton 1 should be the (low detail) ragdoll skeleton and skeleton 2 the (high detail) animation skeleton. + /// We assume that each joint in skeleton 1 can be mapped to a joint in skeleton 2 (if not mapping from animation skeleton to ragdoll skeleton will be undefined). + /// Skeleton 2 should have the same hierarchy as skeleton 1 but can contain extra joints between those in skeleton 1 and it can have extra joints at the root and leaves of the skeleton. + /// @param inSkeleton1 Source skeleton to map from. + /// @param inNeutralPose1 Neutral pose of the source skeleton (model space) + /// @param inSkeleton2 Target skeleton to map to. + /// @param inNeutralPose2 Neutral pose of the target skeleton (model space), inNeutralPose1 and inNeutralPose2 must match as closely as possible, preferably the position of the mappable joints should be identical. + /// @param inCanMapJoint Function that checks if joints in skeleton 1 and skeleton 2 are equal. + void Initialize(const Skeleton *inSkeleton1, const Mat44 *inNeutralPose1, const Skeleton *inSkeleton2, const Mat44 *inNeutralPose2, const CanMapJoint &inCanMapJoint = sDefaultCanMapJoint); + + /// This can be called so lock the translation of a specified set of joints in skeleton 2. + /// Because constraints are never 100% rigid, there's always a little bit of stretch in the ragdoll when the ragdoll is under stress. + /// Locking the translations of the pose will remove the visual stretch from the ragdoll but will introduce a difference between the + /// physical simulation and the visual representation. + /// @param inSkeleton2 Target skeleton to map to. + /// @param inLockedTranslations An array of bools the size of inSkeleton2->GetJointCount(), for each joint indicating if the joint is locked. + /// @param inNeutralPose2 Neutral pose to take reference translations from + void LockTranslations(const Skeleton *inSkeleton2, const bool *inLockedTranslations, const Mat44 *inNeutralPose2); + + /// After Initialize(), this can be called to lock the translation of all joints in skeleton 2 below the first mapped joint to those of the neutral pose. + /// Because constraints are never 100% rigid, there's always a little bit of stretch in the ragdoll when the ragdoll is under stress. + /// Locking the translations of the pose will remove the visual stretch from the ragdoll but will introduce a difference between the + /// physical simulation and the visual representation. + /// @param inSkeleton2 Target skeleton to map to. + /// @param inNeutralPose2 Neutral pose to take reference translations from + void LockAllTranslations(const Skeleton *inSkeleton2, const Mat44 *inNeutralPose2); + + /// Map a pose. Joints that were directly mappable will be copied in model space from pose 1 to pose 2. Any joints that are only present in skeleton 2 + /// will get their model space transform calculated through the local space transforms of pose 2. Joints that are part of a joint chain between two + /// mapped joints will be reoriented towards the next joint in skeleton 1. This means that it is possible for unmapped joints to have some animation, + /// but very extreme animation poses will show artifacts. + /// @param inPose1ModelSpace Pose on skeleton 1 in model space + /// @param inPose2LocalSpace Pose on skeleton 2 in local space (used for the joints that cannot be mapped) + /// @param outPose2ModelSpace Model space pose on skeleton 2 (the output of the mapping) + void Map(const Mat44 *inPose1ModelSpace, const Mat44 *inPose2LocalSpace, Mat44 *outPose2ModelSpace) const; + + /// Reverse map a pose, this will only use the mappings and not the chains (it assumes that all joints in skeleton 1 are mapped) + /// @param inPose2ModelSpace Model space pose on skeleton 2 + /// @param outPose1ModelSpace When the function returns this will contain the model space pose for skeleton 1 + void MapReverse(const Mat44 *inPose2ModelSpace, Mat44 *outPose1ModelSpace) const; + + /// Search through the directly mapped joints (mMappings) and find inJoint1Idx, returns the corresponding Joint2Idx or -1 if not found. + int GetMappedJointIdx(int inJoint1Idx) const; + + /// Search through the locked translations (mLockedTranslations) and find if joint inJoint2Idx is locked. + bool IsJointTranslationLocked(int inJoint2Idx) const; + + using MappingVector = Array; + using ChainVector = Array; + using UnmappedVector = Array; + using LockedVector = Array; + + ///@name Access to the mapped joints + ///@{ + const MappingVector & GetMappings() const { return mMappings; } + MappingVector & GetMappings() { return mMappings; } + const ChainVector & GetChains() const { return mChains; } + ChainVector & GetChains() { return mChains; } + const UnmappedVector & GetUnmapped() const { return mUnmapped; } + UnmappedVector & GetUnmapped() { return mUnmapped; } + const LockedVector & GetLockedTranslations() const { return mLockedTranslations; } + LockedVector & GetLockedTranslations() { return mLockedTranslations; } + ///@} + +private: + /// Joint mappings + MappingVector mMappings; + ChainVector mChains; + UnmappedVector mUnmapped; ///< Joint indices that could not be mapped from 1 to 2 (these are indices in 2) + LockedVector mLockedTranslations; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonPose.cpp b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonPose.cpp new file mode 100644 index 000000000000..c64bf049716a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonPose.cpp @@ -0,0 +1,87 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +void SkeletonPose::SetSkeleton(const Skeleton *inSkeleton) +{ + mSkeleton = inSkeleton; + + mJoints.resize(mSkeleton->GetJointCount()); + mJointMatrices.resize(mSkeleton->GetJointCount()); +} + +void SkeletonPose::CalculateJointMatrices() +{ + for (int i = 0; i < (int)mJoints.size(); ++i) + { + mJointMatrices[i] = mJoints[i].ToMatrix(); + + int parent = mSkeleton->GetJoint(i).mParentJointIndex; + if (parent >= 0) + { + JPH_ASSERT(parent < i, "Joints must be ordered: parents first"); + mJointMatrices[i] = mJointMatrices[parent] * mJointMatrices[i]; + } + } +} + +void SkeletonPose::CalculateJointStates() +{ + for (int i = 0; i < (int)mJoints.size(); ++i) + { + Mat44 local_transform; + int parent = mSkeleton->GetJoint(i).mParentJointIndex; + if (parent >= 0) + local_transform = mJointMatrices[parent].Inversed() * mJointMatrices[i]; + else + local_transform = mJointMatrices[i]; + + JointState &joint = mJoints[i]; + joint.mTranslation = local_transform.GetTranslation(); + joint.mRotation = local_transform.GetQuaternion(); + } +} + +void SkeletonPose::CalculateLocalSpaceJointMatrices(Mat44 *outMatrices) const +{ + for (int i = 0; i < (int)mJoints.size(); ++i) + outMatrices[i] = mJoints[i].ToMatrix(); +} + +#ifdef JPH_DEBUG_RENDERER +void SkeletonPose::Draw(const DrawSettings &inDrawSettings, DebugRenderer *inRenderer, RMat44Arg inOffset) const +{ + RMat44 offset = inOffset * RMat44::sTranslation(mRootOffset); + + const Skeleton::JointVector &joints = mSkeleton->GetJoints(); + + for (int b = 0; b < mSkeleton->GetJointCount(); ++b) + { + RMat44 joint_transform = offset * mJointMatrices[b]; + + if (inDrawSettings.mDrawJoints) + { + int parent = joints[b].mParentJointIndex; + if (parent >= 0) + inRenderer->DrawLine(offset * mJointMatrices[parent].GetTranslation(), joint_transform.GetTranslation(), Color::sGreen); + } + + if (inDrawSettings.mDrawJointOrientations) + inRenderer->DrawCoordinateSystem(joint_transform, 0.05f); + + if (inDrawSettings.mDrawJointNames) + inRenderer->DrawText3D(joint_transform.GetTranslation(), joints[b].mName, Color::sWhite, 0.05f); + } +} +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonPose.h b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonPose.h new file mode 100644 index 000000000000..326227e447a9 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonPose.h @@ -0,0 +1,82 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_DEBUG_RENDERER +class DebugRenderer; +#endif // JPH_DEBUG_RENDERER + +/// Instance of a skeleton, contains the pose the current skeleton is in +class JPH_EXPORT SkeletonPose +{ +public: + JPH_OVERRIDE_NEW_DELETE + + using JointState = SkeletalAnimation::JointState; + using JointStateVector = Array; + using Mat44Vector = Array; + + ///@name Skeleton + ///@{ + void SetSkeleton(const Skeleton *inSkeleton); + const Skeleton * GetSkeleton() const { return mSkeleton; } + ///@} + + /// Extra offset applied to the root (and therefore also to all of its children) + void SetRootOffset(RVec3Arg inOffset) { mRootOffset = inOffset; } + RVec3 GetRootOffset() const { return mRootOffset; } + + ///@name Properties of the joints + ///@{ + uint GetJointCount() const { return (uint)mJoints.size(); } + const JointStateVector & GetJoints() const { return mJoints; } + JointStateVector & GetJoints() { return mJoints; } + const JointState & GetJoint(int inJoint) const { return mJoints[inJoint]; } + JointState & GetJoint(int inJoint) { return mJoints[inJoint]; } + ///@} + + ///@name Joint matrices + ///@{ + const Mat44Vector & GetJointMatrices() const { return mJointMatrices; } + Mat44Vector & GetJointMatrices() { return mJointMatrices; } + const Mat44 & GetJointMatrix(int inJoint) const { return mJointMatrices[inJoint]; } + Mat44 & GetJointMatrix(int inJoint) { return mJointMatrices[inJoint]; } + ///@} + + /// Convert the joint states to joint matrices + void CalculateJointMatrices(); + + /// Convert joint matrices to joint states + void CalculateJointStates(); + + /// Outputs the joint matrices in local space (ensure that outMatrices has GetJointCount() elements, assumes that values in GetJoints() is up to date) + void CalculateLocalSpaceJointMatrices(Mat44 *outMatrices) const; + +#ifdef JPH_DEBUG_RENDERER + /// Draw settings + struct DrawSettings + { + bool mDrawJoints = true; + bool mDrawJointOrientations = true; + bool mDrawJointNames = false; + }; + + /// Draw current pose + void Draw(const DrawSettings &inDrawSettings, DebugRenderer *inRenderer, RMat44Arg inOffset = RMat44::sIdentity()) const; +#endif // JPH_DEBUG_RENDERER + +private: + RefConst mSkeleton; ///< Skeleton definition + RVec3 mRootOffset { RVec3::sZero() }; ///< Extra offset applied to the root (and therefore also to all of its children) + JointStateVector mJoints; ///< Local joint orientations (local to parent Joint) + Mat44Vector mJointMatrices; ///< Local joint matrices (local to world matrix) +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouper.h b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouper.h new file mode 100644 index 000000000000..9d75691f68c1 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouper.h @@ -0,0 +1,27 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// A class that groups triangles in batches of N (according to closeness) +class JPH_EXPORT TriangleGrouper : public NonCopyable +{ +public: + /// Virtual destructor + virtual ~TriangleGrouper() = default; + + /// Group a batch of indexed triangles + /// @param inVertices The list of vertices + /// @param inTriangles The list of indexed triangles (indexes into inVertices) + /// @param inGroupSize How big each group should be + /// @param outGroupedTriangleIndices An ordered list of indices (indexing into inTriangles), contains groups of inGroupSize large worth of indices to triangles that are grouped together. If the triangle count is not an exact multiple of inGroupSize the last batch will be smaller. + virtual void Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array &outGroupedTriangleIndices) = 0; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.cpp b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.cpp new file mode 100644 index 000000000000..800160818e28 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.cpp @@ -0,0 +1,95 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +void TriangleGrouperClosestCentroid::Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array &outGroupedTriangleIndices) +{ + const uint triangle_count = (uint)inTriangles.size(); + const uint num_batches = (triangle_count + inGroupSize - 1) / inGroupSize; + + Array centroids; + centroids.resize(triangle_count); + + outGroupedTriangleIndices.resize(triangle_count); + + for (uint t = 0; t < triangle_count; ++t) + { + // Store centroid + centroids[t] = inTriangles[t].GetCentroid(inVertices); + + // Initialize sort table + outGroupedTriangleIndices[t] = t; + } + + Array::const_iterator triangles_end = outGroupedTriangleIndices.end(); + + // Sort per batch + for (uint b = 0; b < num_batches - 1; ++b) + { + // Get iterators + Array::iterator batch_begin = outGroupedTriangleIndices.begin() + b * inGroupSize; + Array::iterator batch_end = batch_begin + inGroupSize; + Array::iterator batch_begin_plus_1 = batch_begin + 1; + Array::iterator batch_end_minus_1 = batch_end - 1; + + // Find triangle with centroid with lowest X coordinate + Array::iterator lowest_iter = batch_begin; + float lowest_val = centroids[*lowest_iter].GetX(); + for (Array::iterator other = batch_begin; other != triangles_end; ++other) + { + float val = centroids[*other].GetX(); + if (val < lowest_val) + { + lowest_iter = other; + lowest_val = val; + } + } + + // Make this triangle the first in a new batch + std::swap(*batch_begin, *lowest_iter); + Vec3 first_centroid = centroids[*batch_begin]; + + // Sort remaining triangles in batch on distance to first triangle + QuickSort(batch_begin_plus_1, batch_end, + [&first_centroid, ¢roids](uint inLHS, uint inRHS) + { + return (centroids[inLHS] - first_centroid).LengthSq() < (centroids[inRHS] - first_centroid).LengthSq(); + }); + + // Loop over remaining triangles + float furthest_dist = (centroids[*batch_end_minus_1] - first_centroid).LengthSq(); + for (Array::iterator other = batch_end; other != triangles_end; ++other) + { + // Check if this triangle is closer than the furthest triangle in the batch + float dist = (centroids[*other] - first_centroid).LengthSq(); + if (dist < furthest_dist) + { + // Replace furthest triangle + uint other_val = *other; + *other = *batch_end_minus_1; + + // Find first element that is bigger than this one and insert the current item before it + Array::iterator upper = std::upper_bound(batch_begin_plus_1, batch_end, dist, + [&first_centroid, ¢roids](float inLHS, uint inRHS) + { + return inLHS < (centroids[inRHS] - first_centroid).LengthSq(); + }); + std::copy_backward(upper, batch_end_minus_1, batch_end); + *upper = other_val; + + // Calculate new furthest distance + furthest_dist = (centroids[*batch_end_minus_1] - first_centroid).LengthSq(); + } + } + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.h b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.h new file mode 100644 index 000000000000..58322741646c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.h @@ -0,0 +1,21 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// A class that groups triangles in batches of N. +/// Starts with centroid with lowest X coordinate and finds N closest centroids, this repeats until all groups have been found. +/// Time complexity: O(N^2) +class JPH_EXPORT TriangleGrouperClosestCentroid : public TriangleGrouper +{ +public: + // See: TriangleGrouper::Group + virtual void Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array &outGroupedTriangleIndices) override; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperMorton.cpp b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperMorton.cpp new file mode 100644 index 000000000000..a6e5a564426c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperMorton.cpp @@ -0,0 +1,49 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +void TriangleGrouperMorton::Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array &outGroupedTriangleIndices) +{ + const uint triangle_count = (uint)inTriangles.size(); + + Array centroids; + centroids.resize(triangle_count); + + outGroupedTriangleIndices.resize(triangle_count); + + for (uint t = 0; t < triangle_count; ++t) + { + // Store centroid + centroids[t] = inTriangles[t].GetCentroid(inVertices); + + // Initialize sort table + outGroupedTriangleIndices[t] = t; + } + + // Get bounding box of all centroids + AABox centroid_bounds; + for (uint t = 0; t < triangle_count; ++t) + centroid_bounds.Encapsulate(centroids[t]); + + // Make sure box is not degenerate + centroid_bounds.EnsureMinimalEdgeLength(1.0e-5f); + + // Calculate morton code for each centroid + Array morton_codes; + morton_codes.resize(triangle_count); + for (uint t = 0; t < triangle_count; ++t) + morton_codes[t] = MortonCode::sGetMortonCode(centroids[t], centroid_bounds); + + // Sort triangles based on morton code + QuickSort(outGroupedTriangleIndices.begin(), outGroupedTriangleIndices.end(), [&morton_codes](uint inLHS, uint inRHS) { return morton_codes[inLHS] < morton_codes[inRHS]; }); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperMorton.h b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperMorton.h new file mode 100644 index 000000000000..a35f9af004ee --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperMorton.h @@ -0,0 +1,20 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// A class that groups triangles in batches of N according to morton code of centroid. +/// Time complexity: O(N log(N)) +class JPH_EXPORT TriangleGrouperMorton : public TriangleGrouper +{ +public: + // See: TriangleGrouper::Group + virtual void Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array &outGroupedTriangleIndices) override; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitter.cpp b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitter.cpp new file mode 100644 index 000000000000..50d15117d322 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitter.cpp @@ -0,0 +1,67 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +TriangleSplitter::TriangleSplitter(const VertexList &inVertices, const IndexedTriangleList &inTriangles) : + mVertices(inVertices), + mTriangles(inTriangles) +{ + mSortedTriangleIdx.resize(inTriangles.size()); + mCentroids.resize(inTriangles.size()); + + for (uint t = 0; t < inTriangles.size(); ++t) + { + // Initially triangles start unsorted + mSortedTriangleIdx[t] = t; + + // Calculate centroid + inTriangles[t].GetCentroid(inVertices).StoreFloat3(&mCentroids[t]); + } +} + +bool TriangleSplitter::SplitInternal(const Range &inTriangles, uint inDimension, float inSplit, Range &outLeft, Range &outRight) +{ + // Divide triangles + uint start = inTriangles.mBegin, end = inTriangles.mEnd; + while (start < end) + { + // Search for first element that is on the right hand side of the split plane + while (start < end && mCentroids[mSortedTriangleIdx[start]][inDimension] < inSplit) + ++start; + + // Search for the first element that is on the left hand side of the split plane + while (start < end && mCentroids[mSortedTriangleIdx[end - 1]][inDimension] >= inSplit) + --end; + + if (start < end) + { + // Swap the two elements + std::swap(mSortedTriangleIdx[start], mSortedTriangleIdx[end - 1]); + ++start; + --end; + } + } + JPH_ASSERT(start == end); + +#ifdef JPH_ENABLE_ASSERTS + // Validate division algorithm + JPH_ASSERT(inTriangles.mBegin <= start); + JPH_ASSERT(start <= inTriangles.mEnd); + for (uint i = inTriangles.mBegin; i < start; ++i) + JPH_ASSERT(mCentroids[mSortedTriangleIdx[i]][inDimension] < inSplit); + for (uint i = start; i < inTriangles.mEnd; ++i) + JPH_ASSERT(mCentroids[mSortedTriangleIdx[i]][inDimension] >= inSplit); +#endif + + outLeft = Range(inTriangles.mBegin, start); + outRight = Range(start, inTriangles.mEnd); + return outLeft.Count() > 0 && outRight.Count() > 0; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitter.h b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitter.h new file mode 100644 index 000000000000..a66672238fcd --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitter.h @@ -0,0 +1,84 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// A class that splits a triangle list into two parts for building a tree +class JPH_EXPORT TriangleSplitter : public NonCopyable +{ +public: + /// Constructor + TriangleSplitter(const VertexList &inVertices, const IndexedTriangleList &inTriangles); + + /// Virtual destructor + virtual ~TriangleSplitter() = default; + + struct Stats + { + const char * mSplitterName = nullptr; + int mLeafSize = 0; + }; + + /// Get stats of splitter + virtual void GetStats(Stats &outStats) const = 0; + + /// Helper struct to indicate triangle range before and after the split + struct Range + { + /// Constructor + Range() = default; + Range(uint inBegin, uint inEnd) : mBegin(inBegin), mEnd(inEnd) { } + + /// Get number of triangles in range + uint Count() const + { + return mEnd - mBegin; + } + + /// Start and end index (end = 1 beyond end) + uint mBegin; + uint mEnd; + }; + + /// Range of triangles to start with + Range GetInitialRange() const + { + return Range(0, (uint)mSortedTriangleIdx.size()); + } + + /// Split triangles into two groups left and right, returns false if no split could be made + /// @param inTriangles The range of triangles (in mSortedTriangleIdx) to process + /// @param outLeft On return this will contain the ranges for the left subpart. mSortedTriangleIdx may have been shuffled. + /// @param outRight On return this will contain the ranges for the right subpart. mSortedTriangleIdx may have been shuffled. + /// @return Returns true when a split was found + virtual bool Split(const Range &inTriangles, Range &outLeft, Range &outRight) = 0; + + /// Get the list of vertices + const VertexList & GetVertices() const + { + return mVertices; + } + + /// Get triangle by index + const IndexedTriangle & GetTriangle(uint inIdx) const + { + return mTriangles[mSortedTriangleIdx[inIdx]]; + } + +protected: + /// Helper function to split triangles based on dimension and split value + bool SplitInternal(const Range &inTriangles, uint inDimension, float inSplit, Range &outLeft, Range &outRight); + + const VertexList & mVertices; ///< Vertices of the indexed triangles + const IndexedTriangleList & mTriangles; ///< Unsorted triangles + Array mCentroids; ///< Unsorted centroids of triangles + Array mSortedTriangleIdx; ///< Indices to sort triangles +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterBinning.cpp b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterBinning.cpp new file mode 100644 index 000000000000..cdeb79bf750d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterBinning.cpp @@ -0,0 +1,136 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + + JPH_NAMESPACE_BEGIN + +TriangleSplitterBinning::TriangleSplitterBinning(const VertexList &inVertices, const IndexedTriangleList &inTriangles, uint inMinNumBins, uint inMaxNumBins, uint inNumTrianglesPerBin) : + TriangleSplitter(inVertices, inTriangles), + mMinNumBins(inMinNumBins), + mMaxNumBins(inMaxNumBins), + mNumTrianglesPerBin(inNumTrianglesPerBin) +{ + mBins.resize(mMaxNumBins * 3); // mMaxNumBins per dimension +} + +bool TriangleSplitterBinning::Split(const Range &inTriangles, Range &outLeft, Range &outRight) +{ + // Calculate bounds for this range + AABox centroid_bounds; + for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; ++t) + centroid_bounds.Encapsulate(Vec3(mCentroids[mSortedTriangleIdx[t]])); + + // Convert bounds to min coordinate and size + // Prevent division by zero if one of the dimensions is zero + constexpr float cMinSize = 1.0e-5f; + Vec3 bounds_min = centroid_bounds.mMin; + Vec3 bounds_size = Vec3::sMax(centroid_bounds.mMax - bounds_min, Vec3::sReplicate(cMinSize)); + + float best_cp = FLT_MAX; + uint best_dim = 0xffffffff; + float best_split = 0; + + // Bin in all dimensions + uint num_bins = Clamp(inTriangles.Count() / mNumTrianglesPerBin, mMinNumBins, mMaxNumBins); + + // Initialize bins + for (uint dim = 0; dim < 3; ++dim) + { + // Get bounding box size for this dimension + float bounds_min_dim = bounds_min[dim]; + float bounds_size_dim = bounds_size[dim]; + + // Get the bins for this dimension + Bin *bins_dim = &mBins[num_bins * dim]; + + for (uint b = 0; b < num_bins; ++b) + { + Bin &bin = bins_dim[b]; + bin.mBounds.SetEmpty(); + bin.mMinCentroid = bounds_min_dim + bounds_size_dim * (b + 1) / num_bins; + bin.mNumTriangles = 0; + } + } + + // Bin all triangles in all dimensions at once + for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; ++t) + { + Vec3 centroid_pos(mCentroids[mSortedTriangleIdx[t]]); + + AABox triangle_bounds = AABox::sFromTriangle(mVertices, GetTriangle(t)); + + Vec3 bin_no_f = (centroid_pos - bounds_min) / bounds_size * float(num_bins); + UVec4 bin_no = UVec4::sMin(bin_no_f.ToInt(), UVec4::sReplicate(num_bins - 1)); + + for (uint dim = 0; dim < 3; ++dim) + { + // Select bin + Bin &bin = mBins[num_bins * dim + bin_no[dim]]; + + // Accumulate triangle in bin + bin.mBounds.Encapsulate(triangle_bounds); + bin.mMinCentroid = min(bin.mMinCentroid, centroid_pos[dim]); + bin.mNumTriangles++; + } + } + + for (uint dim = 0; dim < 3; ++dim) + { + // Skip axis if too small + if (bounds_size[dim] <= cMinSize) + continue; + + // Get the bins for this dimension + Bin *bins_dim = &mBins[num_bins * dim]; + + // Calculate totals left to right + AABox prev_bounds; + int prev_triangles = 0; + for (uint b = 0; b < num_bins; ++b) + { + Bin &bin = bins_dim[b]; + bin.mBoundsAccumulatedLeft = prev_bounds; // Don't include this node as we'll take a split on the left side of the bin + bin.mNumTrianglesAccumulatedLeft = prev_triangles; + prev_bounds.Encapsulate(bin.mBounds); + prev_triangles += bin.mNumTriangles; + } + + // Calculate totals right to left + prev_bounds.SetEmpty(); + prev_triangles = 0; + for (int b = num_bins - 1; b >= 0; --b) + { + Bin &bin = bins_dim[b]; + prev_bounds.Encapsulate(bin.mBounds); + prev_triangles += bin.mNumTriangles; + bin.mBoundsAccumulatedRight = prev_bounds; + bin.mNumTrianglesAccumulatedRight = prev_triangles; + } + + // Get best splitting plane + for (uint b = 1; b < num_bins; ++b) // Start at 1 since selecting bin 0 would result in everything ending up on the right side + { + // Calculate surface area heuristic and see if it is better than the current best + const Bin &bin = bins_dim[b]; + float cp = bin.mBoundsAccumulatedLeft.GetSurfaceArea() * bin.mNumTrianglesAccumulatedLeft + bin.mBoundsAccumulatedRight.GetSurfaceArea() * bin.mNumTrianglesAccumulatedRight; + if (cp < best_cp) + { + best_cp = cp; + best_dim = dim; + best_split = bin.mMinCentroid; + } + } + } + + // No split found? + if (best_dim == 0xffffffff) + return false; + + return SplitInternal(inTriangles, best_dim, best_split, outLeft, outRight); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterBinning.h b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterBinning.h new file mode 100644 index 000000000000..2cb35c9cc7bd --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterBinning.h @@ -0,0 +1,52 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Binning splitter approach taken from: Realtime Ray Tracing on GPU with BVH-based Packet Traversal by Johannes Gunther et al. +class JPH_EXPORT TriangleSplitterBinning : public TriangleSplitter +{ +public: + /// Constructor + TriangleSplitterBinning(const VertexList &inVertices, const IndexedTriangleList &inTriangles, uint inMinNumBins = 8, uint inMaxNumBins = 128, uint inNumTrianglesPerBin = 6); + + // See TriangleSplitter::GetStats + virtual void GetStats(Stats &outStats) const override + { + outStats.mSplitterName = "TriangleSplitterBinning"; + } + + // See TriangleSplitter::Split + virtual bool Split(const Range &inTriangles, Range &outLeft, Range &outRight) override; + +private: + // Configuration + const uint mMinNumBins; + const uint mMaxNumBins; + const uint mNumTrianglesPerBin; + + struct Bin + { + // Properties of this bin + AABox mBounds; + float mMinCentroid; + uint mNumTriangles; + + // Accumulated data from left most / right most bin to current (including this bin) + AABox mBoundsAccumulatedLeft; + AABox mBoundsAccumulatedRight; + uint mNumTrianglesAccumulatedLeft; + uint mNumTrianglesAccumulatedRight; + }; + + // Scratch area to store the bins + Array mBins; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.cpp b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.cpp new file mode 100644 index 000000000000..333def988922 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.cpp @@ -0,0 +1,170 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +TriangleSplitterFixedLeafSize::TriangleSplitterFixedLeafSize(const VertexList &inVertices, const IndexedTriangleList &inTriangles, uint inLeafSize, uint inMinNumBins, uint inMaxNumBins, uint inNumTrianglesPerBin) : + TriangleSplitter(inVertices, inTriangles), + mLeafSize(inLeafSize), + mMinNumBins(inMinNumBins), + mMaxNumBins(inMaxNumBins), + mNumTrianglesPerBin(inNumTrianglesPerBin) +{ + // Group the triangles + TriangleGrouperClosestCentroid grouper; + grouper.Group(inVertices, inTriangles, mLeafSize, mSortedTriangleIdx); + + // Pad triangles so that we have a multiple of mLeafSize + const uint num_triangles = (uint)inTriangles.size(); + const uint num_groups = (num_triangles + mLeafSize - 1) / mLeafSize; + const uint last_triangle_idx = mSortedTriangleIdx.back(); + for (uint t = num_triangles, t_end = num_groups * mLeafSize; t < t_end; ++t) + mSortedTriangleIdx.push_back(last_triangle_idx); +} + +Vec3 TriangleSplitterFixedLeafSize::GetCentroidForGroup(uint inFirstTriangleInGroup) +{ + JPH_ASSERT(inFirstTriangleInGroup % mLeafSize == 0); + AABox box; + for (uint g = 0; g < mLeafSize; ++g) + box.Encapsulate(mVertices, GetTriangle(inFirstTriangleInGroup + g)); + return box.GetCenter(); +} + +bool TriangleSplitterFixedLeafSize::Split(const Range &inTriangles, Range &outLeft, Range &outRight) +{ + // Cannot split anything smaller than leaf size + JPH_ASSERT(inTriangles.Count() > mLeafSize); + JPH_ASSERT(inTriangles.Count() % mLeafSize == 0); + + // Calculate bounds for this range + AABox centroid_bounds; + for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; t += mLeafSize) + centroid_bounds.Encapsulate(GetCentroidForGroup(t)); + + float best_cp = FLT_MAX; + uint best_dim = 0xffffffff; + float best_split = 0; + + // Bin in all dimensions + uint num_bins = Clamp(inTriangles.Count() / mNumTrianglesPerBin, mMinNumBins, mMaxNumBins); + Array bins(num_bins); + for (uint dim = 0; dim < 3; ++dim) + { + float bounds_min = centroid_bounds.mMin[dim]; + float bounds_size = centroid_bounds.mMax[dim] - bounds_min; + + // Skip axis if too small + if (bounds_size < 1.0e-5f) + continue; + + // Initialize bins + for (uint b = 0; b < num_bins; ++b) + { + Bin &bin = bins[b]; + bin.mBounds.SetEmpty(); + bin.mMinCentroid = bounds_min + bounds_size * (b + 1) / num_bins; + bin.mNumTriangles = 0; + } + + // Bin all triangles + for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; t += mLeafSize) + { + // Calculate average centroid for group + float centroid_pos = GetCentroidForGroup(t)[dim]; + + // Select bin + uint bin_no = min(uint((centroid_pos - bounds_min) / bounds_size * num_bins), num_bins - 1); + Bin &bin = bins[bin_no]; + + // Put all triangles of group in same bin + for (uint g = 0; g < mLeafSize; ++g) + bin.mBounds.Encapsulate(mVertices, GetTriangle(t + g)); + bin.mMinCentroid = min(bin.mMinCentroid, centroid_pos); + bin.mNumTriangles += mLeafSize; + } + + // Calculate totals left to right + AABox prev_bounds; + int prev_triangles = 0; + for (uint b = 0; b < num_bins; ++b) + { + Bin &bin = bins[b]; + bin.mBoundsAccumulatedLeft = prev_bounds; // Don't include this node as we'll take a split on the left side of the bin + bin.mNumTrianglesAccumulatedLeft = prev_triangles; + prev_bounds.Encapsulate(bin.mBounds); + prev_triangles += bin.mNumTriangles; + } + + // Calculate totals right to left + prev_bounds.SetEmpty(); + prev_triangles = 0; + for (int b = num_bins - 1; b >= 0; --b) + { + Bin &bin = bins[b]; + prev_bounds.Encapsulate(bin.mBounds); + prev_triangles += bin.mNumTriangles; + bin.mBoundsAccumulatedRight = prev_bounds; + bin.mNumTrianglesAccumulatedRight = prev_triangles; + } + + // Get best splitting plane + for (uint b = 1; b < num_bins; ++b) // Start at 1 since selecting bin 0 would result in everything ending up on the right side + { + // Calculate surface area heuristic and see if it is better than the current best + const Bin &bin = bins[b]; + float cp = bin.mBoundsAccumulatedLeft.GetSurfaceArea() * bin.mNumTrianglesAccumulatedLeft + bin.mBoundsAccumulatedRight.GetSurfaceArea() * bin.mNumTrianglesAccumulatedRight; + if (cp < best_cp) + { + best_cp = cp; + best_dim = dim; + best_split = bin.mMinCentroid; + } + } + } + + // No split found? + if (best_dim == 0xffffffff) + return false; + + // Divide triangles + uint start = inTriangles.mBegin, end = inTriangles.mEnd; + while (start < end) + { + // Search for first element that is on the right hand side of the split plane + while (start < end && GetCentroidForGroup(start)[best_dim] < best_split) + start += mLeafSize; + + // Search for the first element that is on the left hand side of the split plane + while (start < end && GetCentroidForGroup(end - mLeafSize)[best_dim] >= best_split) + end -= mLeafSize; + + if (start < end) + { + // Swap the two elements + for (uint g = 0; g < mLeafSize; ++g) + std::swap(mSortedTriangleIdx[start + g], mSortedTriangleIdx[end - mLeafSize + g]); + start += mLeafSize; + end -= mLeafSize; + } + } + JPH_ASSERT(start == end); + + // No suitable split found, doing random split in half + if (start == inTriangles.mBegin || start == inTriangles.mEnd) + start = inTriangles.mBegin + (inTriangles.Count() / mLeafSize + 1) / 2 * mLeafSize; + + outLeft = Range(inTriangles.mBegin, start); + outRight = Range(start, inTriangles.mEnd); + JPH_ASSERT(outLeft.mEnd > outLeft.mBegin && outRight.mEnd > outRight.mBegin); + JPH_ASSERT(outLeft.Count() % mLeafSize == 0 && outRight.Count() % mLeafSize == 0); + return true; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.h b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.h new file mode 100644 index 000000000000..029121daea56 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.h @@ -0,0 +1,55 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Same as TriangleSplitterBinning, but ensuring that leaves have a fixed amount of triangles +/// The resulting tree should be suitable for processing on GPU where we want all threads to process an equal amount of triangles +class JPH_EXPORT TriangleSplitterFixedLeafSize : public TriangleSplitter +{ +public: + /// Constructor + TriangleSplitterFixedLeafSize(const VertexList &inVertices, const IndexedTriangleList &inTriangles, uint inLeafSize, uint inMinNumBins = 8, uint inMaxNumBins = 128, uint inNumTrianglesPerBin = 6); + + // See TriangleSplitter::GetStats + virtual void GetStats(Stats &outStats) const override + { + outStats.mSplitterName = "TriangleSplitterFixedLeafSize"; + outStats.mLeafSize = mLeafSize; + } + + // See TriangleSplitter::Split + virtual bool Split(const Range &inTriangles, Range &outLeft, Range &outRight) override; + +private: + /// Get centroid for group + Vec3 GetCentroidForGroup(uint inFirstTriangleInGroup); + + // Configuration + const uint mLeafSize; + const uint mMinNumBins; + const uint mMaxNumBins; + const uint mNumTrianglesPerBin; + + struct Bin + { + // Properties of this bin + AABox mBounds; + float mMinCentroid; + uint mNumTriangles; + + // Accumulated data from left most / right most bin to current (including this bin) + AABox mBoundsAccumulatedLeft; + AABox mBoundsAccumulatedRight; + uint mNumTrianglesAccumulatedLeft; + uint mNumTrianglesAccumulatedRight; + }; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.cpp b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.cpp new file mode 100644 index 000000000000..f8115ab8999c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.cpp @@ -0,0 +1,31 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +TriangleSplitterLongestAxis::TriangleSplitterLongestAxis(const VertexList &inVertices, const IndexedTriangleList &inTriangles) : + TriangleSplitter(inVertices, inTriangles) +{ +} + +bool TriangleSplitterLongestAxis::Split(const Range &inTriangles, Range &outLeft, Range &outRight) +{ + // Calculate bounding box for triangles + AABox bounds; + for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; ++t) + bounds.Encapsulate(mVertices, GetTriangle(t)); + + // Calculate split plane + uint dimension = bounds.GetExtent().GetHighestComponentIndex(); + float split = bounds.GetCenter()[dimension]; + + return SplitInternal(inTriangles, dimension, split, outLeft, outRight); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.h b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.h new file mode 100644 index 000000000000..daf0d437019b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.h @@ -0,0 +1,28 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Splitter using center of bounding box with longest axis +class JPH_EXPORT TriangleSplitterLongestAxis : public TriangleSplitter +{ +public: + /// Constructor + TriangleSplitterLongestAxis(const VertexList &inVertices, const IndexedTriangleList &inTriangles); + + // See TriangleSplitter::GetStats + virtual void GetStats(Stats &outStats) const override + { + outStats.mSplitterName = "TriangleSplitterLongestAxis"; + } + + // See TriangleSplitter::Split + virtual bool Split(const Range &inTriangles, Range &outLeft, Range &outRight) override; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMean.cpp b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMean.cpp new file mode 100644 index 000000000000..e884246fea59 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMean.cpp @@ -0,0 +1,40 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +TriangleSplitterMean::TriangleSplitterMean(const VertexList &inVertices, const IndexedTriangleList &inTriangles) : + TriangleSplitter(inVertices, inTriangles) +{ +} + +bool TriangleSplitterMean::Split(const Range &inTriangles, Range &outLeft, Range &outRight) +{ + // Calculate mean value for these triangles + Vec3 mean = Vec3::sZero(); + for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; ++t) + mean += Vec3(mCentroids[mSortedTriangleIdx[t]]); + mean *= 1.0f / inTriangles.Count(); + + // Calculate deviation + Vec3 deviation = Vec3::sZero(); + for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; ++t) + { + Vec3 delta = Vec3(mCentroids[mSortedTriangleIdx[t]]) - mean; + deviation += delta * delta; + } + deviation *= 1.0f / inTriangles.Count(); + + // Calculate split plane + uint dimension = deviation.GetHighestComponentIndex(); + float split = mean[dimension]; + + return SplitInternal(inTriangles, dimension, split, outLeft, outRight); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMean.h b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMean.h new file mode 100644 index 000000000000..737d76e1c1f9 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMean.h @@ -0,0 +1,28 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Splitter using mean of axis with biggest centroid deviation +class JPH_EXPORT TriangleSplitterMean : public TriangleSplitter +{ +public: + /// Constructor + TriangleSplitterMean(const VertexList &inVertices, const IndexedTriangleList &inTriangles); + + // See TriangleSplitter::GetStats + virtual void GetStats(Stats &outStats) const override + { + outStats.mSplitterName = "TriangleSplitterMean"; + } + + // See TriangleSplitter::Split + virtual bool Split(const Range &inTriangles, Range &outLeft, Range &outRight) override; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMorton.cpp b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMorton.cpp new file mode 100644 index 000000000000..35b0f4212b71 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMorton.cpp @@ -0,0 +1,63 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +TriangleSplitterMorton::TriangleSplitterMorton(const VertexList &inVertices, const IndexedTriangleList &inTriangles) : + TriangleSplitter(inVertices, inTriangles) +{ + // Calculate bounds of centroids + AABox bounds; + for (uint t = 0; t < inTriangles.size(); ++t) + bounds.Encapsulate(Vec3(mCentroids[t])); + + // Make sure box is not degenerate + bounds.EnsureMinimalEdgeLength(1.0e-5f); + + // Calculate morton codes + mMortonCodes.resize(inTriangles.size()); + for (uint t = 0; t < inTriangles.size(); ++t) + mMortonCodes[t] = MortonCode::sGetMortonCode(Vec3(mCentroids[t]), bounds); + + // Sort triangles on morton code + const Array &morton_codes = mMortonCodes; + QuickSort(mSortedTriangleIdx.begin(), mSortedTriangleIdx.end(), [&morton_codes](uint inLHS, uint inRHS) { return morton_codes[inLHS] < morton_codes[inRHS]; }); +} + +bool TriangleSplitterMorton::Split(const Range &inTriangles, Range &outLeft, Range &outRight) +{ + uint32 first_code = mMortonCodes[mSortedTriangleIdx[inTriangles.mBegin]]; + uint32 last_code = mMortonCodes[mSortedTriangleIdx[inTriangles.mEnd - 1]]; + + uint common_prefix = CountLeadingZeros(first_code ^ last_code); + + // Use binary search to find where the next bit differs + uint split = inTriangles.mBegin; // Initial guess + uint step = inTriangles.Count(); + do + { + step = (step + 1) >> 1; // Exponential decrease + uint new_split = split + step; // Proposed new position + if (new_split < inTriangles.mEnd) + { + uint32 split_code = mMortonCodes[mSortedTriangleIdx[new_split]]; + uint split_prefix = CountLeadingZeros(first_code ^ split_code); + if (split_prefix > common_prefix) + split = new_split; // Accept proposal + } + } + while (step > 1); + + outLeft = Range(inTriangles.mBegin, split + 1); + outRight = Range(split + 1, inTriangles.mEnd); + return outLeft.Count() > 0 && outRight.Count() > 0; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMorton.h b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMorton.h new file mode 100644 index 000000000000..2f48c0ea96c1 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMorton.h @@ -0,0 +1,32 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Splitter using Morton codes, see: http://devblogs.nvidia.com/parallelforall/thinking-parallel-part-iii-tree-construction-gpu/ +class JPH_EXPORT TriangleSplitterMorton : public TriangleSplitter +{ +public: + /// Constructor + TriangleSplitterMorton(const VertexList &inVertices, const IndexedTriangleList &inTriangles); + + // See TriangleSplitter::GetStats + virtual void GetStats(Stats &outStats) const override + { + outStats.mSplitterName = "TriangleSplitterMorton"; + } + + // See TriangleSplitter::Split + virtual bool Split(const Range &inTriangles, Range &outLeft, Range &outRight) override; + +private: + // Precalculated Morton codes + Array mMortonCodes; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/LICENSE b/thirdparty/jolt_physics/LICENSE new file mode 100644 index 000000000000..4f0976848529 --- /dev/null +++ b/thirdparty/jolt_physics/LICENSE @@ -0,0 +1,7 @@ +Copyright 2021 Jorrit Rouwe + +Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.