jolt_space_3d.cpp 21 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547
  1. /**************************************************************************/
  2. /* jolt_space_3d.cpp */
  3. /**************************************************************************/
  4. /* This file is part of: */
  5. /* GODOT ENGINE */
  6. /* https://godotengine.org */
  7. /**************************************************************************/
  8. /* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
  9. /* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
  10. /* */
  11. /* Permission is hereby granted, free of charge, to any person obtaining */
  12. /* a copy of this software and associated documentation files (the */
  13. /* "Software"), to deal in the Software without restriction, including */
  14. /* without limitation the rights to use, copy, modify, merge, publish, */
  15. /* distribute, sublicense, and/or sell copies of the Software, and to */
  16. /* permit persons to whom the Software is furnished to do so, subject to */
  17. /* the following conditions: */
  18. /* */
  19. /* The above copyright notice and this permission notice shall be */
  20. /* included in all copies or substantial portions of the Software. */
  21. /* */
  22. /* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
  23. /* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
  24. /* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
  25. /* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
  26. /* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
  27. /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
  28. /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
  29. /**************************************************************************/
  30. #include "jolt_space_3d.h"
  31. #include "../joints/jolt_joint_3d.h"
  32. #include "../jolt_physics_server_3d.h"
  33. #include "../jolt_project_settings.h"
  34. #include "../misc/jolt_stream_wrappers.h"
  35. #include "../objects/jolt_area_3d.h"
  36. #include "../objects/jolt_body_3d.h"
  37. #include "../shapes/jolt_custom_shape_type.h"
  38. #include "../shapes/jolt_shape_3d.h"
  39. #include "jolt_contact_listener_3d.h"
  40. #include "jolt_layers.h"
  41. #include "jolt_physics_direct_space_state_3d.h"
  42. #include "jolt_temp_allocator.h"
  43. #include "core/io/file_access.h"
  44. #include "core/os/time.h"
  45. #include "core/string/print_string.h"
  46. #include "core/variant/variant_utility.h"
  47. #include "Jolt/Physics/PhysicsScene.h"
  48. namespace {
  49. constexpr double DEFAULT_CONTACT_RECYCLE_RADIUS = 0.01;
  50. constexpr double DEFAULT_CONTACT_MAX_SEPARATION = 0.05;
  51. constexpr double DEFAULT_CONTACT_MAX_ALLOWED_PENETRATION = 0.01;
  52. constexpr double DEFAULT_CONTACT_DEFAULT_BIAS = 0.8;
  53. constexpr double DEFAULT_SLEEP_THRESHOLD_LINEAR = 0.1;
  54. constexpr double DEFAULT_SLEEP_THRESHOLD_ANGULAR = 8.0 * Math::PI / 180;
  55. constexpr double DEFAULT_SOLVER_ITERATIONS = 8;
  56. } // namespace
  57. void JoltSpace3D::_pre_step(float p_step) {
  58. while (needs_optimization_list.first()) {
  59. JoltShapedObject3D *object = needs_optimization_list.first()->self();
  60. needs_optimization_list.remove(needs_optimization_list.first());
  61. object->commit_shapes(true);
  62. }
  63. contact_listener->pre_step();
  64. const JPH::BodyLockInterface &lock_iface = get_lock_iface();
  65. const JPH::BodyID *active_rigid_bodies = physics_system->GetActiveBodiesUnsafe(JPH::EBodyType::RigidBody);
  66. const JPH::uint32 active_rigid_body_count = physics_system->GetNumActiveBodies(JPH::EBodyType::RigidBody);
  67. for (JPH::uint32 i = 0; i < active_rigid_body_count; i++) {
  68. JPH::Body *jolt_body = lock_iface.TryGetBody(active_rigid_bodies[i]);
  69. JoltObject3D *object = reinterpret_cast<JoltObject3D *>(jolt_body->GetUserData());
  70. object->pre_step(p_step, *jolt_body);
  71. }
  72. }
  73. void JoltSpace3D::_post_step(float p_step) {
  74. contact_listener->post_step();
  75. while (shapes_changed_list.first()) {
  76. JoltShapedObject3D *object = shapes_changed_list.first()->self();
  77. shapes_changed_list.remove(shapes_changed_list.first());
  78. object->clear_previous_shape();
  79. }
  80. }
  81. JoltSpace3D::JoltSpace3D(JPH::JobSystem *p_job_system) :
  82. job_system(p_job_system),
  83. temp_allocator(new JoltTempAllocator()),
  84. layers(new JoltLayers()),
  85. contact_listener(new JoltContactListener3D(this)),
  86. physics_system(new JPH::PhysicsSystem()) {
  87. physics_system->Init((JPH::uint)JoltProjectSettings::max_bodies, 0, (JPH::uint)JoltProjectSettings::max_body_pairs, (JPH::uint)JoltProjectSettings::max_contact_constraints, *layers, *layers, *layers);
  88. JPH::PhysicsSettings settings;
  89. settings.mBaumgarte = JoltProjectSettings::baumgarte_stabilization_factor;
  90. settings.mSpeculativeContactDistance = JoltProjectSettings::speculative_contact_distance;
  91. settings.mPenetrationSlop = JoltProjectSettings::penetration_slop;
  92. settings.mLinearCastThreshold = JoltProjectSettings::ccd_movement_threshold;
  93. settings.mLinearCastMaxPenetration = JoltProjectSettings::ccd_max_penetration;
  94. settings.mBodyPairCacheMaxDeltaPositionSq = JoltProjectSettings::body_pair_cache_distance_sq;
  95. settings.mBodyPairCacheCosMaxDeltaRotationDiv2 = JoltProjectSettings::body_pair_cache_angle_cos_div2;
  96. settings.mNumVelocitySteps = (JPH::uint)JoltProjectSettings::simulation_velocity_steps;
  97. settings.mNumPositionSteps = (JPH::uint)JoltProjectSettings::simulation_position_steps;
  98. settings.mMinVelocityForRestitution = JoltProjectSettings::bounce_velocity_threshold;
  99. settings.mTimeBeforeSleep = JoltProjectSettings::sleep_time_threshold;
  100. settings.mPointVelocitySleepThreshold = JoltProjectSettings::sleep_velocity_threshold;
  101. settings.mUseBodyPairContactCache = JoltProjectSettings::body_pair_contact_cache_enabled;
  102. settings.mAllowSleeping = JoltProjectSettings::sleep_allowed;
  103. physics_system->SetPhysicsSettings(settings);
  104. physics_system->SetGravity(JPH::Vec3::sZero());
  105. physics_system->SetContactListener(contact_listener);
  106. physics_system->SetSoftBodyContactListener(contact_listener);
  107. 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) {
  108. return Math::abs(MIN(p_body1.GetFriction(), p_body2.GetFriction()));
  109. });
  110. 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) {
  111. return CLAMP(p_body1.GetRestitution() + p_body2.GetRestitution(), 0.0f, 1.0f);
  112. });
  113. }
  114. JoltSpace3D::~JoltSpace3D() {
  115. if (direct_state != nullptr) {
  116. memdelete(direct_state);
  117. direct_state = nullptr;
  118. }
  119. if (physics_system != nullptr) {
  120. delete physics_system;
  121. physics_system = nullptr;
  122. }
  123. if (contact_listener != nullptr) {
  124. delete contact_listener;
  125. contact_listener = nullptr;
  126. }
  127. if (layers != nullptr) {
  128. delete layers;
  129. layers = nullptr;
  130. }
  131. if (temp_allocator != nullptr) {
  132. delete temp_allocator;
  133. temp_allocator = nullptr;
  134. }
  135. }
  136. void JoltSpace3D::step(float p_step) {
  137. stepping = true;
  138. last_step = p_step;
  139. _pre_step(p_step);
  140. const JPH::EPhysicsUpdateError update_error = physics_system->Update(p_step, 1, temp_allocator, job_system);
  141. if ((update_error & JPH::EPhysicsUpdateError::ManifoldCacheFull) != JPH::EPhysicsUpdateError::None) {
  142. WARN_PRINT_ONCE(vformat("Jolt Physics manifold cache exceeded capacity and contacts were ignored. "
  143. "Consider increasing maximum number of contact constraints in project settings. "
  144. "Maximum number of contact constraints is currently set to %d.",
  145. JoltProjectSettings::max_contact_constraints));
  146. }
  147. if ((update_error & JPH::EPhysicsUpdateError::BodyPairCacheFull) != JPH::EPhysicsUpdateError::None) {
  148. WARN_PRINT_ONCE(vformat("Jolt Physics body pair cache exceeded capacity and contacts were ignored. "
  149. "Consider increasing maximum number of body pairs in project settings. "
  150. "Maximum number of body pairs is currently set to %d.",
  151. JoltProjectSettings::max_body_pairs));
  152. }
  153. if ((update_error & JPH::EPhysicsUpdateError::ContactConstraintsFull) != JPH::EPhysicsUpdateError::None) {
  154. WARN_PRINT_ONCE(vformat("Jolt Physics contact constraint buffer exceeded capacity and contacts were ignored. "
  155. "Consider increasing maximum number of contact constraints in project settings. "
  156. "Maximum number of contact constraints is currently set to %d.",
  157. JoltProjectSettings::max_contact_constraints));
  158. }
  159. _post_step(p_step);
  160. bodies_added_since_optimizing = 0;
  161. stepping = false;
  162. }
  163. void JoltSpace3D::call_queries() {
  164. while (body_call_queries_list.first()) {
  165. JoltBody3D *body = body_call_queries_list.first()->self();
  166. body_call_queries_list.remove(body_call_queries_list.first());
  167. body->call_queries();
  168. }
  169. while (area_call_queries_list.first()) {
  170. JoltArea3D *body = area_call_queries_list.first()->self();
  171. area_call_queries_list.remove(area_call_queries_list.first());
  172. body->call_queries();
  173. }
  174. }
  175. double JoltSpace3D::get_param(PhysicsServer3D::SpaceParameter p_param) const {
  176. switch (p_param) {
  177. case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: {
  178. return DEFAULT_CONTACT_RECYCLE_RADIUS;
  179. }
  180. case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION: {
  181. return DEFAULT_CONTACT_MAX_SEPARATION;
  182. }
  183. case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION: {
  184. return DEFAULT_CONTACT_MAX_ALLOWED_PENETRATION;
  185. }
  186. case PhysicsServer3D::SPACE_PARAM_CONTACT_DEFAULT_BIAS: {
  187. return DEFAULT_CONTACT_DEFAULT_BIAS;
  188. }
  189. case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: {
  190. return DEFAULT_SLEEP_THRESHOLD_LINEAR;
  191. }
  192. case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: {
  193. return DEFAULT_SLEEP_THRESHOLD_ANGULAR;
  194. }
  195. case PhysicsServer3D::SPACE_PARAM_BODY_TIME_TO_SLEEP: {
  196. return JoltProjectSettings::sleep_time_threshold;
  197. }
  198. case PhysicsServer3D::SPACE_PARAM_SOLVER_ITERATIONS: {
  199. return DEFAULT_SOLVER_ITERATIONS;
  200. }
  201. default: {
  202. ERR_FAIL_V_MSG(0.0, vformat("Unhandled space parameter: '%d'. This should not happen. Please report this.", p_param));
  203. }
  204. }
  205. }
  206. void JoltSpace3D::set_param(PhysicsServer3D::SpaceParameter p_param, double p_value) {
  207. switch (p_param) {
  208. case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: {
  209. WARN_PRINT("Space-specific contact recycle radius is not supported when using Jolt Physics. Any such value will be ignored.");
  210. } break;
  211. case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION: {
  212. WARN_PRINT("Space-specific contact max separation is not supported when using Jolt Physics. Any such value will be ignored.");
  213. } break;
  214. case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION: {
  215. WARN_PRINT("Space-specific contact max allowed penetration is not supported when using Jolt Physics. Any such value will be ignored.");
  216. } break;
  217. case PhysicsServer3D::SPACE_PARAM_CONTACT_DEFAULT_BIAS: {
  218. WARN_PRINT("Space-specific contact default bias is not supported when using Jolt Physics. Any such value will be ignored.");
  219. } break;
  220. case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: {
  221. WARN_PRINT("Space-specific linear velocity sleep threshold is not supported when using Jolt Physics. Any such value will be ignored.");
  222. } break;
  223. case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: {
  224. WARN_PRINT("Space-specific angular velocity sleep threshold is not supported when using Jolt Physics. Any such value will be ignored.");
  225. } break;
  226. case PhysicsServer3D::SPACE_PARAM_BODY_TIME_TO_SLEEP: {
  227. WARN_PRINT("Space-specific body sleep time is not supported when using Jolt Physics. Any such value will be ignored.");
  228. } break;
  229. case PhysicsServer3D::SPACE_PARAM_SOLVER_ITERATIONS: {
  230. WARN_PRINT("Space-specific solver iterations is not supported when using Jolt Physics. Any such value will be ignored.");
  231. } break;
  232. default: {
  233. ERR_FAIL_MSG(vformat("Unhandled space parameter: '%d'. This should not happen. Please report this.", p_param));
  234. } break;
  235. }
  236. }
  237. JPH::BodyInterface &JoltSpace3D::get_body_iface() {
  238. return physics_system->GetBodyInterfaceNoLock();
  239. }
  240. const JPH::BodyInterface &JoltSpace3D::get_body_iface() const {
  241. return physics_system->GetBodyInterfaceNoLock();
  242. }
  243. const JPH::BodyLockInterface &JoltSpace3D::get_lock_iface() const {
  244. return physics_system->GetBodyLockInterfaceNoLock();
  245. }
  246. const JPH::BroadPhaseQuery &JoltSpace3D::get_broad_phase_query() const {
  247. return physics_system->GetBroadPhaseQuery();
  248. }
  249. const JPH::NarrowPhaseQuery &JoltSpace3D::get_narrow_phase_query() const {
  250. return physics_system->GetNarrowPhaseQueryNoLock();
  251. }
  252. JPH::ObjectLayer JoltSpace3D::map_to_object_layer(JPH::BroadPhaseLayer p_broad_phase_layer, uint32_t p_collision_layer, uint32_t p_collision_mask) {
  253. return layers->to_object_layer(p_broad_phase_layer, p_collision_layer, p_collision_mask);
  254. }
  255. 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 {
  256. layers->from_object_layer(p_object_layer, r_broad_phase_layer, r_collision_layer, r_collision_mask);
  257. }
  258. JPH::Body *JoltSpace3D::try_get_jolt_body(const JPH::BodyID &p_body_id) const {
  259. return get_lock_iface().TryGetBody(p_body_id);
  260. }
  261. JoltObject3D *JoltSpace3D::try_get_object(const JPH::BodyID &p_body_id) const {
  262. const JPH::Body *jolt_body = try_get_jolt_body(p_body_id);
  263. if (unlikely(jolt_body == nullptr)) {
  264. return nullptr;
  265. }
  266. return reinterpret_cast<JoltObject3D *>(jolt_body->GetUserData());
  267. }
  268. JoltShapedObject3D *JoltSpace3D::try_get_shaped(const JPH::BodyID &p_body_id) const {
  269. JoltObject3D *object = try_get_object(p_body_id);
  270. if (unlikely(object == nullptr)) {
  271. return nullptr;
  272. }
  273. return object->as_shaped();
  274. }
  275. JoltBody3D *JoltSpace3D::try_get_body(const JPH::BodyID &p_body_id) const {
  276. JoltObject3D *object = try_get_object(p_body_id);
  277. if (unlikely(object == nullptr)) {
  278. return nullptr;
  279. }
  280. return object->as_body();
  281. }
  282. JoltArea3D *JoltSpace3D::try_get_area(const JPH::BodyID &p_body_id) const {
  283. JoltObject3D *object = try_get_object(p_body_id);
  284. if (unlikely(object == nullptr)) {
  285. return nullptr;
  286. }
  287. return object->as_area();
  288. }
  289. JoltSoftBody3D *JoltSpace3D::try_get_soft_body(const JPH::BodyID &p_body_id) const {
  290. JoltObject3D *object = try_get_object(p_body_id);
  291. if (unlikely(object == nullptr)) {
  292. return nullptr;
  293. }
  294. return object->as_soft_body();
  295. }
  296. JoltPhysicsDirectSpaceState3D *JoltSpace3D::get_direct_state() {
  297. if (direct_state == nullptr) {
  298. direct_state = memnew(JoltPhysicsDirectSpaceState3D(this));
  299. }
  300. return direct_state;
  301. }
  302. void JoltSpace3D::set_default_area(JoltArea3D *p_area) {
  303. if (default_area == p_area) {
  304. return;
  305. }
  306. if (default_area != nullptr) {
  307. default_area->set_default_area(false);
  308. }
  309. default_area = p_area;
  310. if (default_area != nullptr) {
  311. default_area->set_default_area(true);
  312. }
  313. }
  314. JPH::Body *JoltSpace3D::add_rigid_body(const JoltObject3D &p_object, const JPH::BodyCreationSettings &p_settings, bool p_sleeping) {
  315. JPH::BodyInterface &body_iface = get_body_iface();
  316. JPH::Body *jolt_body = body_iface.CreateBody(p_settings);
  317. if (unlikely(jolt_body == nullptr)) {
  318. ERR_PRINT_ONCE(vformat("Failed to create underlying Jolt Physics body for '%s'. "
  319. "Consider increasing maximum number of bodies in project settings. "
  320. "Maximum number of bodies is currently set to %d.",
  321. p_object.to_string(), JoltProjectSettings::max_bodies));
  322. return nullptr;
  323. }
  324. body_iface.AddBody(jolt_body->GetID(), p_sleeping ? JPH::EActivation::DontActivate : JPH::EActivation::Activate);
  325. bodies_added_since_optimizing += 1;
  326. return jolt_body;
  327. }
  328. JPH::Body *JoltSpace3D::add_soft_body(const JoltObject3D &p_object, const JPH::SoftBodyCreationSettings &p_settings, bool p_sleeping) {
  329. JPH::BodyInterface &body_iface = get_body_iface();
  330. JPH::Body *jolt_body = body_iface.CreateSoftBody(p_settings);
  331. if (unlikely(jolt_body == nullptr)) {
  332. ERR_PRINT_ONCE(vformat("Failed to create underlying Jolt Physics body for '%s'. "
  333. "Consider increasing maximum number of bodies in project settings. "
  334. "Maximum number of bodies is currently set to %d.",
  335. p_object.to_string(), JoltProjectSettings::max_bodies));
  336. return nullptr;
  337. }
  338. body_iface.AddBody(jolt_body->GetID(), p_sleeping ? JPH::EActivation::DontActivate : JPH::EActivation::Activate);
  339. bodies_added_since_optimizing += 1;
  340. return jolt_body;
  341. }
  342. void JoltSpace3D::remove_body(const JPH::BodyID &p_body_id) {
  343. JPH::BodyInterface &body_iface = get_body_iface();
  344. body_iface.RemoveBody(p_body_id);
  345. body_iface.DestroyBody(p_body_id);
  346. }
  347. void JoltSpace3D::try_optimize() {
  348. // This makes assumptions about the underlying acceleration structure of Jolt's broad-phase, which currently uses a
  349. // quadtree, and which gets walked with a fixed-size node stack of 128. This means that when the quadtree is
  350. // completely unbalanced, as is the case if we add bodies one-by-one without ever stepping the simulation, like in
  351. // the editor viewport, we would exceed this stack size (resulting in an incomplete search) as soon as we perform a
  352. // physics query after having added somewhere in the order of 128 * 3 bodies. We leave a hefty margin just in case.
  353. if (likely(bodies_added_since_optimizing < 128)) {
  354. return;
  355. }
  356. physics_system->OptimizeBroadPhase();
  357. bodies_added_since_optimizing = 0;
  358. }
  359. void JoltSpace3D::enqueue_call_queries(SelfList<JoltBody3D> *p_body) {
  360. if (!p_body->in_list()) {
  361. body_call_queries_list.add(p_body);
  362. }
  363. }
  364. void JoltSpace3D::enqueue_call_queries(SelfList<JoltArea3D> *p_area) {
  365. if (!p_area->in_list()) {
  366. area_call_queries_list.add(p_area);
  367. }
  368. }
  369. void JoltSpace3D::dequeue_call_queries(SelfList<JoltBody3D> *p_body) {
  370. if (p_body->in_list()) {
  371. body_call_queries_list.remove(p_body);
  372. }
  373. }
  374. void JoltSpace3D::dequeue_call_queries(SelfList<JoltArea3D> *p_area) {
  375. if (p_area->in_list()) {
  376. area_call_queries_list.remove(p_area);
  377. }
  378. }
  379. void JoltSpace3D::enqueue_shapes_changed(SelfList<JoltShapedObject3D> *p_object) {
  380. if (!p_object->in_list()) {
  381. shapes_changed_list.add(p_object);
  382. }
  383. }
  384. void JoltSpace3D::dequeue_shapes_changed(SelfList<JoltShapedObject3D> *p_object) {
  385. if (p_object->in_list()) {
  386. shapes_changed_list.remove(p_object);
  387. }
  388. }
  389. void JoltSpace3D::enqueue_needs_optimization(SelfList<JoltShapedObject3D> *p_object) {
  390. if (!p_object->in_list()) {
  391. needs_optimization_list.add(p_object);
  392. }
  393. }
  394. void JoltSpace3D::dequeue_needs_optimization(SelfList<JoltShapedObject3D> *p_object) {
  395. if (p_object->in_list()) {
  396. needs_optimization_list.remove(p_object);
  397. }
  398. }
  399. void JoltSpace3D::add_joint(JPH::Constraint *p_jolt_ref) {
  400. physics_system->AddConstraint(p_jolt_ref);
  401. }
  402. void JoltSpace3D::add_joint(JoltJoint3D *p_joint) {
  403. add_joint(p_joint->get_jolt_ref());
  404. }
  405. void JoltSpace3D::remove_joint(JPH::Constraint *p_jolt_ref) {
  406. physics_system->RemoveConstraint(p_jolt_ref);
  407. }
  408. void JoltSpace3D::remove_joint(JoltJoint3D *p_joint) {
  409. remove_joint(p_joint->get_jolt_ref());
  410. }
  411. #ifdef DEBUG_ENABLED
  412. void JoltSpace3D::dump_debug_snapshot(const String &p_dir) {
  413. const Dictionary datetime = Time::get_singleton()->get_datetime_dict_from_system();
  414. const String datetime_str = vformat("%04d-%02d-%02d_%02d-%02d-%02d", datetime["year"], datetime["month"], datetime["day"], datetime["hour"], datetime["minute"], datetime["second"]);
  415. const String path = p_dir + vformat("/jolt_snapshot_%s_%d.bin", datetime_str, rid.get_id());
  416. Ref<FileAccess> file_access = FileAccess::open(path, FileAccess::ModeFlags::WRITE);
  417. 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()));
  418. JPH::PhysicsScene physics_scene;
  419. physics_scene.FromPhysicsSystem(physics_system);
  420. for (JPH::BodyCreationSettings &settings : physics_scene.GetBodies()) {
  421. const JoltObject3D *object = reinterpret_cast<const JoltObject3D *>(settings.mUserData);
  422. if (const JoltBody3D *body = object->as_body()) {
  423. // 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.
  424. settings.mGravityFactor = body->get_gravity_scale();
  425. settings.mLinearDamping = body->get_total_linear_damp();
  426. settings.mAngularDamping = body->get_total_angular_damp();
  427. }
  428. settings.SetShape(JoltShape3D::without_custom_shapes(settings.GetShape()));
  429. }
  430. JoltStreamOutputWrapper output_stream(file_access);
  431. physics_scene.SaveBinaryState(output_stream, true, false);
  432. 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())));
  433. print_line(vformat("Snapshot of physics space with RID '%d' saved to '%s'.", rid.get_id(), path));
  434. }
  435. const PackedVector3Array &JoltSpace3D::get_debug_contacts() const {
  436. return contact_listener->get_debug_contacts();
  437. }
  438. int JoltSpace3D::get_debug_contact_count() const {
  439. return contact_listener->get_debug_contact_count();
  440. }
  441. int JoltSpace3D::get_max_debug_contacts() const {
  442. return contact_listener->get_max_debug_contacts();
  443. }
  444. void JoltSpace3D::set_max_debug_contacts(int p_count) {
  445. contact_listener->set_max_debug_contacts(p_count);
  446. }
  447. #endif