From cc39dca9f7d960d1bb137f1dcbbf1da5cec8a505 Mon Sep 17 00:00:00 2001 From: PouleyKetchoupp Date: Mon, 18 Oct 2021 12:24:30 -0700 Subject: Rename Godot Physics classes from *SW to Godot* Also moved MT physics server wrappers to the main servers folder, since they don't have to be implementation specific. --- scene/register_scene_types.cpp | 2 - servers/physics_2d/area_2d_sw.cpp | 305 --- servers/physics_2d/area_2d_sw.h | 195 -- servers/physics_2d/area_pair_2d_sw.cpp | 190 -- servers/physics_2d/area_pair_2d_sw.h | 74 - servers/physics_2d/body_2d_sw.cpp | 677 ------ servers/physics_2d/body_2d_sw.h | 353 --- servers/physics_2d/body_direct_state_2d_sw.cpp | 178 -- servers/physics_2d/body_direct_state_2d_sw.h | 91 - servers/physics_2d/body_pair_2d_sw.cpp | 545 ----- servers/physics_2d/body_pair_2d_sw.h | 98 - servers/physics_2d/broad_phase_2d_bvh.cpp | 113 - servers/physics_2d/broad_phase_2d_bvh.h | 73 - servers/physics_2d/broad_phase_2d_sw.cpp | 36 - servers/physics_2d/broad_phase_2d_sw.h | 71 - servers/physics_2d/collision_object_2d_sw.cpp | 243 -- servers/physics_2d/collision_object_2d_sw.h | 189 -- servers/physics_2d/collision_solver_2d_sat.cpp | 1408 ------------ servers/physics_2d/collision_solver_2d_sat.h | 38 - servers/physics_2d/collision_solver_2d_sw.cpp | 264 --- servers/physics_2d/collision_solver_2d_sw.h | 50 - servers/physics_2d/constraint_2d_sw.h | 70 - servers/physics_2d/godot_area_2d.cpp | 305 +++ servers/physics_2d/godot_area_2d.h | 196 ++ servers/physics_2d/godot_area_pair_2d.cpp | 190 ++ servers/physics_2d/godot_area_pair_2d.h | 74 + servers/physics_2d/godot_body_2d.cpp | 677 ++++++ servers/physics_2d/godot_body_2d.h | 354 +++ servers/physics_2d/godot_body_direct_state_2d.cpp | 178 ++ servers/physics_2d/godot_body_direct_state_2d.h | 91 + servers/physics_2d/godot_body_pair_2d.cpp | 545 +++++ servers/physics_2d/godot_body_pair_2d.h | 98 + servers/physics_2d/godot_broad_phase_2d.cpp | 36 + servers/physics_2d/godot_broad_phase_2d.h | 71 + servers/physics_2d/godot_broad_phase_2d_bvh.cpp | 113 + servers/physics_2d/godot_broad_phase_2d_bvh.h | 74 + servers/physics_2d/godot_collision_object_2d.cpp | 243 ++ servers/physics_2d/godot_collision_object_2d.h | 190 ++ servers/physics_2d/godot_collision_solver_2d.cpp | 264 +++ servers/physics_2d/godot_collision_solver_2d.h | 50 + .../physics_2d/godot_collision_solver_2d_sat.cpp | 1408 ++++++++++++ servers/physics_2d/godot_collision_solver_2d_sat.h | 38 + servers/physics_2d/godot_constraint_2d.h | 70 + servers/physics_2d/godot_joints_2d.cpp | 486 ++++ servers/physics_2d/godot_joints_2d.h | 178 ++ servers/physics_2d/godot_physics_server_2d.cpp | 1350 +++++++++++ servers/physics_2d/godot_physics_server_2d.h | 299 +++ servers/physics_2d/godot_shape_2d.cpp | 995 ++++++++ servers/physics_2d/godot_shape_2d.h | 536 +++++ servers/physics_2d/godot_space_2d.cpp | 1213 ++++++++++ servers/physics_2d/godot_space_2d.h | 212 ++ servers/physics_2d/godot_step_2d.cpp | 308 +++ servers/physics_2d/godot_step_2d.h | 63 + servers/physics_2d/joints_2d_sw.cpp | 486 ---- servers/physics_2d/joints_2d_sw.h | 178 -- servers/physics_2d/physics_server_2d_sw.cpp | 1349 ----------- servers/physics_2d/physics_server_2d_sw.h | 299 --- servers/physics_2d/physics_server_2d_wrap_mt.cpp | 137 -- servers/physics_2d/physics_server_2d_wrap_mt.h | 333 --- servers/physics_2d/shape_2d_sw.cpp | 995 -------- servers/physics_2d/shape_2d_sw.h | 536 ----- servers/physics_2d/space_2d_sw.cpp | 1212 ---------- servers/physics_2d/space_2d_sw.h | 211 -- servers/physics_2d/step_2d_sw.cpp | 308 --- servers/physics_2d/step_2d_sw.h | 63 - servers/physics_3d/area_3d_sw.cpp | 336 --- servers/physics_3d/area_3d_sw.h | 245 -- servers/physics_3d/area_pair_3d_sw.cpp | 272 --- servers/physics_3d/area_pair_3d_sw.h | 92 - servers/physics_3d/body_3d_sw.cpp | 790 ------- servers/physics_3d/body_3d_sw.h | 368 --- servers/physics_3d/body_direct_state_3d_sw.cpp | 190 -- servers/physics_3d/body_direct_state_3d_sw.h | 94 - servers/physics_3d/body_pair_3d_sw.cpp | 907 -------- servers/physics_3d/body_pair_3d_sw.h | 143 -- servers/physics_3d/broad_phase_3d_bvh.cpp | 117 - servers/physics_3d/broad_phase_3d_bvh.h | 72 - servers/physics_3d/broad_phase_3d_sw.cpp | 36 - servers/physics_3d/broad_phase_3d_sw.h | 72 - servers/physics_3d/collision_object_3d_sw.cpp | 240 -- servers/physics_3d/collision_object_3d_sw.h | 185 -- servers/physics_3d/collision_solver_3d_sat.cpp | 2396 ------------------- servers/physics_3d/collision_solver_3d_sat.h | 38 - servers/physics_3d/collision_solver_3d_sw.cpp | 572 ----- servers/physics_3d/collision_solver_3d_sw.h | 57 - servers/physics_3d/constraint_3d_sw.h | 81 - servers/physics_3d/gjk_epa.cpp | 28 +- servers/physics_3d/gjk_epa.h | 8 +- servers/physics_3d/godot_area_3d.cpp | 337 +++ servers/physics_3d/godot_area_3d.h | 246 ++ servers/physics_3d/godot_area_pair_3d.cpp | 273 +++ servers/physics_3d/godot_area_pair_3d.h | 92 + servers/physics_3d/godot_body_3d.cpp | 757 +++++++ servers/physics_3d/godot_body_3d.h | 369 +++ servers/physics_3d/godot_body_direct_state_3d.cpp | 190 ++ servers/physics_3d/godot_body_direct_state_3d.h | 94 + servers/physics_3d/godot_body_pair_3d.cpp | 908 ++++++++ servers/physics_3d/godot_body_pair_3d.h | 144 ++ servers/physics_3d/godot_broad_phase_3d.cpp | 36 + servers/physics_3d/godot_broad_phase_3d.h | 72 + servers/physics_3d/godot_broad_phase_3d_bvh.cpp | 118 + servers/physics_3d/godot_broad_phase_3d_bvh.h | 73 + servers/physics_3d/godot_collision_object_3d.cpp | 241 ++ servers/physics_3d/godot_collision_object_3d.h | 186 ++ servers/physics_3d/godot_collision_solver_3d.cpp | 572 +++++ servers/physics_3d/godot_collision_solver_3d.h | 57 + .../physics_3d/godot_collision_solver_3d_sat.cpp | 2397 ++++++++++++++++++++ servers/physics_3d/godot_collision_solver_3d_sat.h | 38 + servers/physics_3d/godot_constraint_3d.h | 81 + servers/physics_3d/godot_joint_3d.h | 68 + servers/physics_3d/godot_physics_server_3d.cpp | 1749 ++++++++++++++ servers/physics_3d/godot_physics_server_3d.h | 380 ++++ servers/physics_3d/godot_shape_3d.cpp | 2202 ++++++++++++++++++ servers/physics_3d/godot_shape_3d.h | 510 +++++ servers/physics_3d/godot_soft_body_3d.cpp | 1323 +++++++++++ servers/physics_3d/godot_soft_body_3d.h | 279 +++ servers/physics_3d/godot_space_3d.cpp | 1235 ++++++++++ servers/physics_3d/godot_space_3d.h | 217 ++ servers/physics_3d/godot_step_3d.cpp | 420 ++++ servers/physics_3d/godot_step_3d.h | 64 + .../physics_3d/joints/cone_twist_joint_3d_sw.cpp | 360 --- servers/physics_3d/joints/cone_twist_joint_3d_sw.h | 142 -- .../physics_3d/joints/generic_6dof_joint_3d_sw.cpp | 671 ------ .../physics_3d/joints/generic_6dof_joint_3d_sw.h | 347 --- .../joints/godot_cone_twist_joint_3d.cpp | 360 +++ .../physics_3d/joints/godot_cone_twist_joint_3d.h | 142 ++ .../joints/godot_generic_6dof_joint_3d.cpp | 670 ++++++ .../joints/godot_generic_6dof_joint_3d.h | 322 +++ servers/physics_3d/joints/godot_hinge_joint_3d.cpp | 468 ++++ servers/physics_3d/joints/godot_hinge_joint_3d.h | 116 + .../physics_3d/joints/godot_jacobian_entry_3d.h | 169 ++ servers/physics_3d/joints/godot_pin_joint_3d.cpp | 179 ++ servers/physics_3d/joints/godot_pin_joint_3d.h | 95 + .../physics_3d/joints/godot_slider_joint_3d.cpp | 493 ++++ servers/physics_3d/joints/godot_slider_joint_3d.h | 246 ++ servers/physics_3d/joints/hinge_joint_3d_sw.cpp | 468 ---- servers/physics_3d/joints/hinge_joint_3d_sw.h | 116 - servers/physics_3d/joints/jacobian_entry_3d_sw.h | 169 -- servers/physics_3d/joints/pin_joint_3d_sw.cpp | 179 -- servers/physics_3d/joints/pin_joint_3d_sw.h | 95 - servers/physics_3d/joints/slider_joint_3d_sw.cpp | 495 ---- servers/physics_3d/joints/slider_joint_3d_sw.h | 246 -- servers/physics_3d/joints_3d_sw.h | 68 - servers/physics_3d/physics_server_3d_sw.cpp | 1748 -------------- servers/physics_3d/physics_server_3d_sw.h | 379 ---- servers/physics_3d/physics_server_3d_wrap_mt.cpp | 137 -- servers/physics_3d/physics_server_3d_wrap_mt.h | 409 ---- servers/physics_3d/shape_3d_sw.cpp | 2202 ------------------ servers/physics_3d/shape_3d_sw.h | 510 ----- servers/physics_3d/soft_body_3d_sw.cpp | 1322 ----------- servers/physics_3d/soft_body_3d_sw.h | 279 --- servers/physics_3d/space_3d_sw.cpp | 1234 ---------- servers/physics_3d/space_3d_sw.h | 216 -- servers/physics_3d/step_3d_sw.cpp | 419 ---- servers/physics_3d/step_3d_sw.h | 64 - servers/physics_server_2d.h | 6 +- servers/physics_server_2d_wrap_mt.cpp | 137 ++ servers/physics_server_2d_wrap_mt.h | 333 +++ servers/physics_server_3d.h | 6 +- servers/physics_server_3d_wrap_mt.cpp | 137 ++ servers/physics_server_3d_wrap_mt.h | 409 ++++ servers/register_server_types.cpp | 12 +- 162 files changed, 30939 insertions(+), 30978 deletions(-) delete mode 100644 servers/physics_2d/area_2d_sw.cpp delete mode 100644 servers/physics_2d/area_2d_sw.h delete mode 100644 servers/physics_2d/area_pair_2d_sw.cpp delete mode 100644 servers/physics_2d/area_pair_2d_sw.h delete mode 100644 servers/physics_2d/body_2d_sw.cpp delete mode 100644 servers/physics_2d/body_2d_sw.h delete mode 100644 servers/physics_2d/body_direct_state_2d_sw.cpp delete mode 100644 servers/physics_2d/body_direct_state_2d_sw.h delete mode 100644 servers/physics_2d/body_pair_2d_sw.cpp delete mode 100644 servers/physics_2d/body_pair_2d_sw.h delete mode 100644 servers/physics_2d/broad_phase_2d_bvh.cpp delete mode 100644 servers/physics_2d/broad_phase_2d_bvh.h delete mode 100644 servers/physics_2d/broad_phase_2d_sw.cpp delete mode 100644 servers/physics_2d/broad_phase_2d_sw.h delete mode 100644 servers/physics_2d/collision_object_2d_sw.cpp delete mode 100644 servers/physics_2d/collision_object_2d_sw.h delete mode 100644 servers/physics_2d/collision_solver_2d_sat.cpp delete mode 100644 servers/physics_2d/collision_solver_2d_sat.h delete mode 100644 servers/physics_2d/collision_solver_2d_sw.cpp delete mode 100644 servers/physics_2d/collision_solver_2d_sw.h delete mode 100644 servers/physics_2d/constraint_2d_sw.h create mode 100644 servers/physics_2d/godot_area_2d.cpp create mode 100644 servers/physics_2d/godot_area_2d.h create mode 100644 servers/physics_2d/godot_area_pair_2d.cpp create mode 100644 servers/physics_2d/godot_area_pair_2d.h create mode 100644 servers/physics_2d/godot_body_2d.cpp create mode 100644 servers/physics_2d/godot_body_2d.h create mode 100644 servers/physics_2d/godot_body_direct_state_2d.cpp create mode 100644 servers/physics_2d/godot_body_direct_state_2d.h create mode 100644 servers/physics_2d/godot_body_pair_2d.cpp create mode 100644 servers/physics_2d/godot_body_pair_2d.h create mode 100644 servers/physics_2d/godot_broad_phase_2d.cpp create mode 100644 servers/physics_2d/godot_broad_phase_2d.h create mode 100644 servers/physics_2d/godot_broad_phase_2d_bvh.cpp create mode 100644 servers/physics_2d/godot_broad_phase_2d_bvh.h create mode 100644 servers/physics_2d/godot_collision_object_2d.cpp create mode 100644 servers/physics_2d/godot_collision_object_2d.h create mode 100644 servers/physics_2d/godot_collision_solver_2d.cpp create mode 100644 servers/physics_2d/godot_collision_solver_2d.h create mode 100644 servers/physics_2d/godot_collision_solver_2d_sat.cpp create mode 100644 servers/physics_2d/godot_collision_solver_2d_sat.h create mode 100644 servers/physics_2d/godot_constraint_2d.h create mode 100644 servers/physics_2d/godot_joints_2d.cpp create mode 100644 servers/physics_2d/godot_joints_2d.h create mode 100644 servers/physics_2d/godot_physics_server_2d.cpp create mode 100644 servers/physics_2d/godot_physics_server_2d.h create mode 100644 servers/physics_2d/godot_shape_2d.cpp create mode 100644 servers/physics_2d/godot_shape_2d.h create mode 100644 servers/physics_2d/godot_space_2d.cpp create mode 100644 servers/physics_2d/godot_space_2d.h create mode 100644 servers/physics_2d/godot_step_2d.cpp create mode 100644 servers/physics_2d/godot_step_2d.h delete mode 100644 servers/physics_2d/joints_2d_sw.cpp delete mode 100644 servers/physics_2d/joints_2d_sw.h delete mode 100644 servers/physics_2d/physics_server_2d_sw.cpp delete mode 100644 servers/physics_2d/physics_server_2d_sw.h delete mode 100644 servers/physics_2d/physics_server_2d_wrap_mt.cpp delete mode 100644 servers/physics_2d/physics_server_2d_wrap_mt.h delete mode 100644 servers/physics_2d/shape_2d_sw.cpp delete mode 100644 servers/physics_2d/shape_2d_sw.h delete mode 100644 servers/physics_2d/space_2d_sw.cpp delete mode 100644 servers/physics_2d/space_2d_sw.h delete mode 100644 servers/physics_2d/step_2d_sw.cpp delete mode 100644 servers/physics_2d/step_2d_sw.h delete mode 100644 servers/physics_3d/area_3d_sw.cpp delete mode 100644 servers/physics_3d/area_3d_sw.h delete mode 100644 servers/physics_3d/area_pair_3d_sw.cpp delete mode 100644 servers/physics_3d/area_pair_3d_sw.h delete mode 100644 servers/physics_3d/body_3d_sw.cpp delete mode 100644 servers/physics_3d/body_3d_sw.h delete mode 100644 servers/physics_3d/body_direct_state_3d_sw.cpp delete mode 100644 servers/physics_3d/body_direct_state_3d_sw.h delete mode 100644 servers/physics_3d/body_pair_3d_sw.cpp delete mode 100644 servers/physics_3d/body_pair_3d_sw.h delete mode 100644 servers/physics_3d/broad_phase_3d_bvh.cpp delete mode 100644 servers/physics_3d/broad_phase_3d_bvh.h delete mode 100644 servers/physics_3d/broad_phase_3d_sw.cpp delete mode 100644 servers/physics_3d/broad_phase_3d_sw.h delete mode 100644 servers/physics_3d/collision_object_3d_sw.cpp delete mode 100644 servers/physics_3d/collision_object_3d_sw.h delete mode 100644 servers/physics_3d/collision_solver_3d_sat.cpp delete mode 100644 servers/physics_3d/collision_solver_3d_sat.h delete mode 100644 servers/physics_3d/collision_solver_3d_sw.cpp delete mode 100644 servers/physics_3d/collision_solver_3d_sw.h delete mode 100644 servers/physics_3d/constraint_3d_sw.h create mode 100644 servers/physics_3d/godot_area_3d.cpp create mode 100644 servers/physics_3d/godot_area_3d.h create mode 100644 servers/physics_3d/godot_area_pair_3d.cpp create mode 100644 servers/physics_3d/godot_area_pair_3d.h create mode 100644 servers/physics_3d/godot_body_3d.cpp create mode 100644 servers/physics_3d/godot_body_3d.h create mode 100644 servers/physics_3d/godot_body_direct_state_3d.cpp create mode 100644 servers/physics_3d/godot_body_direct_state_3d.h create mode 100644 servers/physics_3d/godot_body_pair_3d.cpp create mode 100644 servers/physics_3d/godot_body_pair_3d.h create mode 100644 servers/physics_3d/godot_broad_phase_3d.cpp create mode 100644 servers/physics_3d/godot_broad_phase_3d.h create mode 100644 servers/physics_3d/godot_broad_phase_3d_bvh.cpp create mode 100644 servers/physics_3d/godot_broad_phase_3d_bvh.h create mode 100644 servers/physics_3d/godot_collision_object_3d.cpp create mode 100644 servers/physics_3d/godot_collision_object_3d.h create mode 100644 servers/physics_3d/godot_collision_solver_3d.cpp create mode 100644 servers/physics_3d/godot_collision_solver_3d.h create mode 100644 servers/physics_3d/godot_collision_solver_3d_sat.cpp create mode 100644 servers/physics_3d/godot_collision_solver_3d_sat.h create mode 100644 servers/physics_3d/godot_constraint_3d.h create mode 100644 servers/physics_3d/godot_joint_3d.h create mode 100644 servers/physics_3d/godot_physics_server_3d.cpp create mode 100644 servers/physics_3d/godot_physics_server_3d.h create mode 100644 servers/physics_3d/godot_shape_3d.cpp create mode 100644 servers/physics_3d/godot_shape_3d.h create mode 100644 servers/physics_3d/godot_soft_body_3d.cpp create mode 100644 servers/physics_3d/godot_soft_body_3d.h create mode 100644 servers/physics_3d/godot_space_3d.cpp create mode 100644 servers/physics_3d/godot_space_3d.h create mode 100644 servers/physics_3d/godot_step_3d.cpp create mode 100644 servers/physics_3d/godot_step_3d.h delete mode 100644 servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp delete mode 100644 servers/physics_3d/joints/cone_twist_joint_3d_sw.h delete mode 100644 servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp delete mode 100644 servers/physics_3d/joints/generic_6dof_joint_3d_sw.h create mode 100644 servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp create mode 100644 servers/physics_3d/joints/godot_cone_twist_joint_3d.h create mode 100644 servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp create mode 100644 servers/physics_3d/joints/godot_generic_6dof_joint_3d.h create mode 100644 servers/physics_3d/joints/godot_hinge_joint_3d.cpp create mode 100644 servers/physics_3d/joints/godot_hinge_joint_3d.h create mode 100644 servers/physics_3d/joints/godot_jacobian_entry_3d.h create mode 100644 servers/physics_3d/joints/godot_pin_joint_3d.cpp create mode 100644 servers/physics_3d/joints/godot_pin_joint_3d.h create mode 100644 servers/physics_3d/joints/godot_slider_joint_3d.cpp create mode 100644 servers/physics_3d/joints/godot_slider_joint_3d.h delete mode 100644 servers/physics_3d/joints/hinge_joint_3d_sw.cpp delete mode 100644 servers/physics_3d/joints/hinge_joint_3d_sw.h delete mode 100644 servers/physics_3d/joints/jacobian_entry_3d_sw.h delete mode 100644 servers/physics_3d/joints/pin_joint_3d_sw.cpp delete mode 100644 servers/physics_3d/joints/pin_joint_3d_sw.h delete mode 100644 servers/physics_3d/joints/slider_joint_3d_sw.cpp delete mode 100644 servers/physics_3d/joints/slider_joint_3d_sw.h delete mode 100644 servers/physics_3d/joints_3d_sw.h delete mode 100644 servers/physics_3d/physics_server_3d_sw.cpp delete mode 100644 servers/physics_3d/physics_server_3d_sw.h delete mode 100644 servers/physics_3d/physics_server_3d_wrap_mt.cpp delete mode 100644 servers/physics_3d/physics_server_3d_wrap_mt.h delete mode 100644 servers/physics_3d/shape_3d_sw.cpp delete mode 100644 servers/physics_3d/shape_3d_sw.h delete mode 100644 servers/physics_3d/soft_body_3d_sw.cpp delete mode 100644 servers/physics_3d/soft_body_3d_sw.h delete mode 100644 servers/physics_3d/space_3d_sw.cpp delete mode 100644 servers/physics_3d/space_3d_sw.h delete mode 100644 servers/physics_3d/step_3d_sw.cpp delete mode 100644 servers/physics_3d/step_3d_sw.h create mode 100644 servers/physics_server_2d_wrap_mt.cpp create mode 100644 servers/physics_server_2d_wrap_mt.h create mode 100644 servers/physics_server_3d_wrap_mt.cpp create mode 100644 servers/physics_server_3d_wrap_mt.h diff --git a/scene/register_scene_types.cpp b/scene/register_scene_types.cpp index bf8f7291be..6b2f610227 100644 --- a/scene/register_scene_types.cpp +++ b/scene/register_scene_types.cpp @@ -940,10 +940,8 @@ void register_scene_types() { ClassDB::add_compatibility_class("Path", "Path3D"); ClassDB::add_compatibility_class("PathFollow", "PathFollow3D"); ClassDB::add_compatibility_class("PhysicalBone", "PhysicalBone3D"); - ClassDB::add_compatibility_class("Physics2DDirectBodyStateSW", "PhysicsDirectBodyState2DSW"); ClassDB::add_compatibility_class("Physics2DDirectBodyState", "PhysicsDirectBodyState2D"); ClassDB::add_compatibility_class("Physics2DDirectSpaceState", "PhysicsDirectSpaceState2D"); - ClassDB::add_compatibility_class("Physics2DServerSW", "PhysicsServer2DSW"); ClassDB::add_compatibility_class("Physics2DServer", "PhysicsServer2D"); ClassDB::add_compatibility_class("Physics2DShapeQueryParameters", "PhysicsShapeQueryParameters2D"); ClassDB::add_compatibility_class("Physics2DTestMotionResult", "PhysicsTestMotionResult2D"); diff --git a/servers/physics_2d/area_2d_sw.cpp b/servers/physics_2d/area_2d_sw.cpp deleted file mode 100644 index c85b1575e3..0000000000 --- a/servers/physics_2d/area_2d_sw.cpp +++ /dev/null @@ -1,305 +0,0 @@ -/*************************************************************************/ -/* area_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "area_2d_sw.h" -#include "body_2d_sw.h" -#include "space_2d_sw.h" - -Area2DSW::BodyKey::BodyKey(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { - rid = p_body->get_self(); - instance_id = p_body->get_instance_id(); - body_shape = p_body_shape; - area_shape = p_area_shape; -} - -Area2DSW::BodyKey::BodyKey(Area2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { - rid = p_body->get_self(); - instance_id = p_body->get_instance_id(); - body_shape = p_body_shape; - area_shape = p_area_shape; -} - -void Area2DSW::_shapes_changed() { - if (!moved_list.in_list() && get_space()) { - get_space()->area_add_to_moved_list(&moved_list); - } -} - -void Area2DSW::set_transform(const Transform2D &p_transform) { - if (!moved_list.in_list() && get_space()) { - get_space()->area_add_to_moved_list(&moved_list); - } - - _set_transform(p_transform); - _set_inv_transform(p_transform.affine_inverse()); -} - -void Area2DSW::set_space(Space2DSW *p_space) { - if (get_space()) { - if (monitor_query_list.in_list()) { - get_space()->area_remove_from_monitor_query_list(&monitor_query_list); - } - if (moved_list.in_list()) { - get_space()->area_remove_from_moved_list(&moved_list); - } - } - - monitored_bodies.clear(); - monitored_areas.clear(); - - _set_space(p_space); -} - -void Area2DSW::set_monitor_callback(ObjectID p_id, const StringName &p_method) { - if (p_id == monitor_callback_id) { - monitor_callback_method = p_method; - return; - } - - _unregister_shapes(); - - monitor_callback_id = p_id; - monitor_callback_method = p_method; - - monitored_bodies.clear(); - monitored_areas.clear(); - - _shape_changed(); - - if (!moved_list.in_list() && get_space()) { - get_space()->area_add_to_moved_list(&moved_list); - } -} - -void Area2DSW::set_area_monitor_callback(ObjectID p_id, const StringName &p_method) { - if (p_id == area_monitor_callback_id) { - area_monitor_callback_method = p_method; - return; - } - - _unregister_shapes(); - - area_monitor_callback_id = p_id; - area_monitor_callback_method = p_method; - - monitored_bodies.clear(); - monitored_areas.clear(); - - _shape_changed(); - - if (!moved_list.in_list() && get_space()) { - get_space()->area_add_to_moved_list(&moved_list); - } -} - -void Area2DSW::set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode p_mode) { - bool do_override = p_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED; - if (do_override == (space_override_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED)) { - return; - } - _unregister_shapes(); - space_override_mode = p_mode; - _shape_changed(); -} - -void Area2DSW::set_param(PhysicsServer2D::AreaParameter p_param, const Variant &p_value) { - switch (p_param) { - case PhysicsServer2D::AREA_PARAM_GRAVITY: - gravity = p_value; - break; - case PhysicsServer2D::AREA_PARAM_GRAVITY_VECTOR: - gravity_vector = p_value; - break; - case PhysicsServer2D::AREA_PARAM_GRAVITY_IS_POINT: - gravity_is_point = p_value; - break; - case PhysicsServer2D::AREA_PARAM_GRAVITY_DISTANCE_SCALE: - gravity_distance_scale = p_value; - break; - case PhysicsServer2D::AREA_PARAM_GRAVITY_POINT_ATTENUATION: - point_attenuation = p_value; - break; - case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP: - linear_damp = p_value; - break; - case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP: - angular_damp = p_value; - break; - case PhysicsServer2D::AREA_PARAM_PRIORITY: - priority = p_value; - break; - } -} - -Variant Area2DSW::get_param(PhysicsServer2D::AreaParameter p_param) const { - switch (p_param) { - case PhysicsServer2D::AREA_PARAM_GRAVITY: - return gravity; - case PhysicsServer2D::AREA_PARAM_GRAVITY_VECTOR: - return gravity_vector; - case PhysicsServer2D::AREA_PARAM_GRAVITY_IS_POINT: - return gravity_is_point; - case PhysicsServer2D::AREA_PARAM_GRAVITY_DISTANCE_SCALE: - return gravity_distance_scale; - case PhysicsServer2D::AREA_PARAM_GRAVITY_POINT_ATTENUATION: - return point_attenuation; - case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP: - return linear_damp; - case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP: - return angular_damp; - case PhysicsServer2D::AREA_PARAM_PRIORITY: - return priority; - } - - return Variant(); -} - -void Area2DSW::_queue_monitor_update() { - ERR_FAIL_COND(!get_space()); - - if (!monitor_query_list.in_list()) { - get_space()->area_add_to_monitor_query_list(&monitor_query_list); - } -} - -void Area2DSW::set_monitorable(bool p_monitorable) { - if (monitorable == p_monitorable) { - return; - } - - monitorable = p_monitorable; - _set_static(!monitorable); -} - -void Area2DSW::call_queries() { - if (monitor_callback_id.is_valid() && !monitored_bodies.is_empty()) { - Variant res[5]; - Variant *resptr[5]; - for (int i = 0; i < 5; i++) { - resptr[i] = &res[i]; - } - - Object *obj = ObjectDB::get_instance(monitor_callback_id); - if (!obj) { - monitored_bodies.clear(); - monitor_callback_id = ObjectID(); - return; - } - - for (Map::Element *E = monitored_bodies.front(); E;) { - if (E->get().state == 0) { // Nothing happened - Map::Element *next = E->next(); - monitored_bodies.erase(E); - E = next; - continue; - } - - res[0] = E->get().state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED; - res[1] = E->key().rid; - res[2] = E->key().instance_id; - res[3] = E->key().body_shape; - res[4] = E->key().area_shape; - - Map::Element *next = E->next(); - monitored_bodies.erase(E); - E = next; - - Callable::CallError ce; - obj->call(monitor_callback_method, (const Variant **)resptr, 5, ce); - } - } - - if (area_monitor_callback_id.is_valid() && !monitored_areas.is_empty()) { - Variant res[5]; - Variant *resptr[5]; - for (int i = 0; i < 5; i++) { - resptr[i] = &res[i]; - } - - Object *obj = ObjectDB::get_instance(area_monitor_callback_id); - if (!obj) { - monitored_areas.clear(); - area_monitor_callback_id = ObjectID(); - return; - } - - for (Map::Element *E = monitored_areas.front(); E;) { - if (E->get().state == 0) { // Nothing happened - Map::Element *next = E->next(); - monitored_areas.erase(E); - E = next; - continue; - } - - res[0] = E->get().state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED; - res[1] = E->key().rid; - res[2] = E->key().instance_id; - res[3] = E->key().body_shape; - res[4] = E->key().area_shape; - - Map::Element *next = E->next(); - monitored_areas.erase(E); - E = next; - - Callable::CallError ce; - obj->call(area_monitor_callback_method, (const Variant **)resptr, 5, ce); - } - } -} - -void Area2DSW::compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const { - if (is_gravity_point()) { - const real_t gravity_distance_scale = get_gravity_distance_scale(); - Vector2 v = get_transform().xform(get_gravity_vector()) - p_position; - if (gravity_distance_scale > 0) { - const real_t v_length = v.length(); - if (v_length > 0) { - const real_t v_scaled = v_length * gravity_distance_scale; - r_gravity = (v.normalized() * (get_gravity() / (v_scaled * v_scaled))); - } else { - r_gravity = Vector2(); - } - } else { - r_gravity = v.normalized() * get_gravity(); - } - } else { - r_gravity = get_gravity_vector() * get_gravity(); - } -} - -Area2DSW::Area2DSW() : - CollisionObject2DSW(TYPE_AREA), - monitor_query_list(this), - moved_list(this) { - _set_static(true); //areas are not active by default -} - -Area2DSW::~Area2DSW() { -} diff --git a/servers/physics_2d/area_2d_sw.h b/servers/physics_2d/area_2d_sw.h deleted file mode 100644 index 0b7c791ed5..0000000000 --- a/servers/physics_2d/area_2d_sw.h +++ /dev/null @@ -1,195 +0,0 @@ -/*************************************************************************/ -/* area_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 AREA_2D_SW_H -#define AREA_2D_SW_H - -#include "collision_object_2d_sw.h" -#include "core/templates/self_list.h" -#include "servers/physics_server_2d.h" - -class Space2DSW; -class Body2DSW; -class Constraint2DSW; - -class Area2DSW : public CollisionObject2DSW { - PhysicsServer2D::AreaSpaceOverrideMode space_override_mode = PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED; - real_t gravity = 9.80665; - Vector2 gravity_vector = Vector2(0, -1); - bool gravity_is_point = false; - real_t gravity_distance_scale = 0.0; - real_t point_attenuation = 1.0; - real_t linear_damp = 0.1; - real_t angular_damp = 1.0; - int priority = 0; - bool monitorable = false; - - ObjectID monitor_callback_id; - StringName monitor_callback_method; - - ObjectID area_monitor_callback_id; - StringName area_monitor_callback_method; - - SelfList monitor_query_list; - SelfList moved_list; - - struct BodyKey { - RID rid; - ObjectID instance_id; - uint32_t body_shape = 0; - uint32_t area_shape = 0; - - _FORCE_INLINE_ bool operator<(const BodyKey &p_key) const { - if (rid == p_key.rid) { - if (body_shape == p_key.body_shape) { - return area_shape < p_key.area_shape; - } else { - return body_shape < p_key.body_shape; - } - } else { - return rid < p_key.rid; - } - } - - _FORCE_INLINE_ BodyKey() {} - BodyKey(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); - BodyKey(Area2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); - }; - - struct BodyState { - int state = 0; - _FORCE_INLINE_ void inc() { state++; } - _FORCE_INLINE_ void dec() { state--; } - }; - - Map monitored_bodies; - Map monitored_areas; - - Set constraints; - - virtual void _shapes_changed(); - void _queue_monitor_update(); - -public: - void set_monitor_callback(ObjectID p_id, const StringName &p_method); - _FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id.is_valid(); } - - void set_area_monitor_callback(ObjectID p_id, const StringName &p_method); - _FORCE_INLINE_ bool has_area_monitor_callback() const { return area_monitor_callback_id.is_valid(); } - - _FORCE_INLINE_ void add_body_to_query(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); - _FORCE_INLINE_ void remove_body_from_query(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); - - _FORCE_INLINE_ void add_area_to_query(Area2DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape); - _FORCE_INLINE_ void remove_area_from_query(Area2DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape); - - void set_param(PhysicsServer2D::AreaParameter p_param, const Variant &p_value); - Variant get_param(PhysicsServer2D::AreaParameter p_param) const; - - void set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode p_mode); - PhysicsServer2D::AreaSpaceOverrideMode get_space_override_mode() const { return space_override_mode; } - - _FORCE_INLINE_ void set_gravity(real_t p_gravity) { gravity = p_gravity; } - _FORCE_INLINE_ real_t get_gravity() const { return gravity; } - - _FORCE_INLINE_ void set_gravity_vector(const Vector2 &p_gravity) { gravity_vector = p_gravity; } - _FORCE_INLINE_ Vector2 get_gravity_vector() const { return gravity_vector; } - - _FORCE_INLINE_ void set_gravity_as_point(bool p_enable) { gravity_is_point = p_enable; } - _FORCE_INLINE_ bool is_gravity_point() const { return gravity_is_point; } - - _FORCE_INLINE_ void set_gravity_distance_scale(real_t scale) { gravity_distance_scale = scale; } - _FORCE_INLINE_ real_t get_gravity_distance_scale() const { return gravity_distance_scale; } - - _FORCE_INLINE_ void set_point_attenuation(real_t p_point_attenuation) { point_attenuation = p_point_attenuation; } - _FORCE_INLINE_ real_t get_point_attenuation() const { return point_attenuation; } - - _FORCE_INLINE_ void set_linear_damp(real_t p_linear_damp) { linear_damp = p_linear_damp; } - _FORCE_INLINE_ real_t get_linear_damp() const { return linear_damp; } - - _FORCE_INLINE_ void set_angular_damp(real_t p_angular_damp) { angular_damp = p_angular_damp; } - _FORCE_INLINE_ real_t get_angular_damp() const { return angular_damp; } - - _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } - _FORCE_INLINE_ int get_priority() const { return priority; } - - _FORCE_INLINE_ void add_constraint(Constraint2DSW *p_constraint) { constraints.insert(p_constraint); } - _FORCE_INLINE_ void remove_constraint(Constraint2DSW *p_constraint) { constraints.erase(p_constraint); } - _FORCE_INLINE_ const Set &get_constraints() const { return constraints; } - _FORCE_INLINE_ void clear_constraints() { constraints.clear(); } - - void set_monitorable(bool p_monitorable); - _FORCE_INLINE_ bool is_monitorable() const { return monitorable; } - - void set_transform(const Transform2D &p_transform); - - void set_space(Space2DSW *p_space); - - void call_queries(); - - void compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const; - - Area2DSW(); - ~Area2DSW(); -}; - -void Area2DSW::add_body_to_query(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { - BodyKey bk(p_body, p_body_shape, p_area_shape); - monitored_bodies[bk].inc(); - if (!monitor_query_list.in_list()) { - _queue_monitor_update(); - } -} - -void Area2DSW::remove_body_from_query(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { - BodyKey bk(p_body, p_body_shape, p_area_shape); - monitored_bodies[bk].dec(); - if (!monitor_query_list.in_list()) { - _queue_monitor_update(); - } -} - -void Area2DSW::add_area_to_query(Area2DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { - BodyKey bk(p_area, p_area_shape, p_self_shape); - monitored_areas[bk].inc(); - if (!monitor_query_list.in_list()) { - _queue_monitor_update(); - } -} - -void Area2DSW::remove_area_from_query(Area2DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { - BodyKey bk(p_area, p_area_shape, p_self_shape); - monitored_areas[bk].dec(); - if (!monitor_query_list.in_list()) { - _queue_monitor_update(); - } -} - -#endif // AREA_2D_SW_H diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp deleted file mode 100644 index 4f1148c26f..0000000000 --- a/servers/physics_2d/area_pair_2d_sw.cpp +++ /dev/null @@ -1,190 +0,0 @@ -/*************************************************************************/ -/* area_pair_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "area_pair_2d_sw.h" -#include "collision_solver_2d_sw.h" - -bool AreaPair2DSW::setup(real_t p_step) { - bool result = false; - if (area->collides_with(body) && CollisionSolver2DSW::solve(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), Vector2(), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), Vector2(), nullptr, this)) { - result = true; - } - - process_collision = false; - if (result != colliding) { - if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { - process_collision = true; - } else if (area->has_monitor_callback()) { - process_collision = true; - } - - colliding = result; - } - - return process_collision; -} - -bool AreaPair2DSW::pre_solve(real_t p_step) { - if (!process_collision) { - return false; - } - - if (colliding) { - if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { - body->add_area(area); - } - - if (area->has_monitor_callback()) { - area->add_body_to_query(body, body_shape, area_shape); - } - } else { - if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { - body->remove_area(area); - } - - if (area->has_monitor_callback()) { - area->remove_body_from_query(body, body_shape, area_shape); - } - } - - return false; // Never do any post solving. -} - -void AreaPair2DSW::solve(real_t p_step) { - // Nothing to do. -} - -AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape) { - body = p_body; - area = p_area; - body_shape = p_body_shape; - area_shape = p_area_shape; - body->add_constraint(this, 0); - area->add_constraint(this); - if (p_body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { //need to be active to process pair - p_body->set_active(true); - } -} - -AreaPair2DSW::~AreaPair2DSW() { - if (colliding) { - if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { - body->remove_area(area); - } - if (area->has_monitor_callback()) { - area->remove_body_from_query(body, body_shape, area_shape); - } - } - body->remove_constraint(this, 0); - area->remove_constraint(this); -} - -////////////////////////////////// - -bool Area2Pair2DSW::setup(real_t p_step) { - bool result_a = area_a->collides_with(area_b); - bool result_b = area_b->collides_with(area_a); - if ((result_a || result_b) && !CollisionSolver2DSW::solve(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), Vector2(), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), Vector2(), nullptr, this)) { - result_a = false; - result_b = false; - } - - bool process_collision = false; - - process_collision_a = false; - if (result_a != colliding_a) { - if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { - process_collision_a = true; - process_collision = true; - } - colliding_a = result_a; - } - - process_collision_b = false; - if (result_b != colliding_b) { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { - process_collision_b = true; - process_collision = true; - } - colliding_b = result_b; - } - - return process_collision; -} - -bool Area2Pair2DSW::pre_solve(real_t p_step) { - if (process_collision_a) { - if (colliding_a) { - area_a->add_area_to_query(area_b, shape_b, shape_a); - } else { - area_a->remove_area_from_query(area_b, shape_b, shape_a); - } - } - - if (process_collision_b) { - if (colliding_b) { - area_b->add_area_to_query(area_a, shape_a, shape_b); - } else { - area_b->remove_area_from_query(area_a, shape_a, shape_b); - } - } - - return false; // Never do any post solving. -} - -void Area2Pair2DSW::solve(real_t p_step) { - // Nothing to do. -} - -Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b) { - area_a = p_area_a; - area_b = p_area_b; - shape_a = p_shape_a; - shape_b = p_shape_b; - area_a->add_constraint(this); - area_b->add_constraint(this); -} - -Area2Pair2DSW::~Area2Pair2DSW() { - if (colliding_a) { - if (area_a->has_area_monitor_callback()) { - area_a->remove_area_from_query(area_b, shape_b, shape_a); - } - } - - if (colliding_b) { - if (area_b->has_area_monitor_callback()) { - area_b->remove_area_from_query(area_a, shape_a, shape_b); - } - } - - area_a->remove_constraint(this); - area_b->remove_constraint(this); -} diff --git a/servers/physics_2d/area_pair_2d_sw.h b/servers/physics_2d/area_pair_2d_sw.h deleted file mode 100644 index 66e9f1afee..0000000000 --- a/servers/physics_2d/area_pair_2d_sw.h +++ /dev/null @@ -1,74 +0,0 @@ -/*************************************************************************/ -/* area_pair_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 AREA_PAIR_2D_SW_H -#define AREA_PAIR_2D_SW_H - -#include "area_2d_sw.h" -#include "body_2d_sw.h" -#include "constraint_2d_sw.h" - -class AreaPair2DSW : public Constraint2DSW { - Body2DSW *body = nullptr; - Area2DSW *area = nullptr; - int body_shape = 0; - int area_shape = 0; - bool colliding = false; - bool process_collision = false; - -public: - virtual bool setup(real_t p_step) override; - virtual bool pre_solve(real_t p_step) override; - virtual void solve(real_t p_step) override; - - AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape); - ~AreaPair2DSW(); -}; - -class Area2Pair2DSW : public Constraint2DSW { - Area2DSW *area_a = nullptr; - Area2DSW *area_b = nullptr; - int shape_a = 0; - int shape_b = 0; - bool colliding_a = false; - bool colliding_b = false; - bool process_collision_a = false; - bool process_collision_b = false; - -public: - virtual bool setup(real_t p_step) override; - virtual bool pre_solve(real_t p_step) override; - virtual void solve(real_t p_step) override; - - Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b); - ~Area2Pair2DSW(); -}; - -#endif // AREA_PAIR_2D_SW_H diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp deleted file mode 100644 index 38b98b7bca..0000000000 --- a/servers/physics_2d/body_2d_sw.cpp +++ /dev/null @@ -1,677 +0,0 @@ -/*************************************************************************/ -/* body_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "body_2d_sw.h" - -#include "area_2d_sw.h" -#include "body_direct_state_2d_sw.h" -#include "space_2d_sw.h" - -void Body2DSW::_mass_properties_changed() { - if (get_space() && !mass_properties_update_list.in_list() && (calculate_inertia || calculate_center_of_mass)) { - get_space()->body_add_to_mass_properties_update_list(&mass_properties_update_list); - } -} - -void Body2DSW::update_mass_properties() { - //update shapes and motions - - switch (mode) { - case PhysicsServer2D::BODY_MODE_DYNAMIC: { - real_t total_area = 0; - for (int i = 0; i < get_shape_count(); i++) { - if (is_shape_disabled(i)) { - continue; - } - total_area += get_shape_aabb(i).get_area(); - } - - if (calculate_center_of_mass) { - // We have to recompute the center of mass. - center_of_mass = Vector2(); - - if (total_area != 0.0) { - for (int i = 0; i < get_shape_count(); i++) { - if (is_shape_disabled(i)) { - continue; - } - - real_t area = get_shape_aabb(i).get_area(); - - real_t mass = area * this->mass / total_area; - - // NOTE: we assume that the shape origin is also its center of mass. - center_of_mass += mass * get_shape_transform(i).get_origin(); - } - - center_of_mass /= mass; - } - } - - if (calculate_inertia) { - inertia = 0; - - for (int i = 0; i < get_shape_count(); i++) { - if (is_shape_disabled(i)) { - continue; - } - - const Shape2DSW *shape = get_shape(i); - - real_t area = get_shape_aabb(i).get_area(); - if (area == 0.0) { - continue; - } - - real_t mass = area * this->mass / total_area; - - Transform2D mtx = get_shape_transform(i); - Vector2 scale = mtx.get_scale(); - Vector2 shape_origin = mtx.get_origin() - center_of_mass; - inertia += shape->get_moment_of_inertia(mass, scale) + mass * shape_origin.length_squared(); - } - } - - _inv_inertia = inertia > 0.0 ? (1.0 / inertia) : 0.0; - - if (mass) { - _inv_mass = 1.0 / mass; - } else { - _inv_mass = 0; - } - - } break; - case PhysicsServer2D::BODY_MODE_KINEMATIC: - case PhysicsServer2D::BODY_MODE_STATIC: { - _inv_inertia = 0; - _inv_mass = 0; - } break; - case PhysicsServer2D::BODY_MODE_DYNAMIC_LINEAR: { - _inv_inertia = 0; - _inv_mass = 1.0 / mass; - - } break; - } -} - -void Body2DSW::reset_mass_properties() { - calculate_inertia = true; - calculate_center_of_mass = true; - _mass_properties_changed(); -} - -void Body2DSW::set_active(bool p_active) { - if (active == p_active) { - return; - } - - active = p_active; - - if (active) { - if (mode == PhysicsServer2D::BODY_MODE_STATIC) { - // Static bodies can't be active. - active = false; - } else if (get_space()) { - get_space()->body_add_to_active_list(&active_list); - } - } else if (get_space()) { - get_space()->body_remove_from_active_list(&active_list); - } -} - -void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value) { - switch (p_param) { - case PhysicsServer2D::BODY_PARAM_BOUNCE: { - bounce = p_value; - } break; - case PhysicsServer2D::BODY_PARAM_FRICTION: { - friction = p_value; - } break; - case PhysicsServer2D::BODY_PARAM_MASS: { - real_t mass_value = p_value; - ERR_FAIL_COND(mass_value <= 0); - mass = mass_value; - if (mode >= PhysicsServer2D::BODY_MODE_DYNAMIC) { - _mass_properties_changed(); - } - } break; - case PhysicsServer2D::BODY_PARAM_INERTIA: { - real_t inertia_value = p_value; - if (inertia_value <= 0.0) { - calculate_inertia = true; - if (mode == PhysicsServer2D::BODY_MODE_DYNAMIC) { - _mass_properties_changed(); - } - } else { - calculate_inertia = false; - inertia = inertia_value; - if (mode == PhysicsServer2D::BODY_MODE_DYNAMIC) { - _inv_inertia = 1.0 / inertia; - } - } - } break; - case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: { - calculate_center_of_mass = false; - center_of_mass = p_value; - } break; - case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: { - gravity_scale = p_value; - } break; - case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP: { - linear_damp = p_value; - } break; - case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP: { - angular_damp = p_value; - } break; - default: { - } - } -} - -Variant Body2DSW::get_param(PhysicsServer2D::BodyParameter p_param) const { - switch (p_param) { - case PhysicsServer2D::BODY_PARAM_BOUNCE: { - return bounce; - } - case PhysicsServer2D::BODY_PARAM_FRICTION: { - return friction; - } - case PhysicsServer2D::BODY_PARAM_MASS: { - return mass; - } - case PhysicsServer2D::BODY_PARAM_INERTIA: { - return inertia; - } - case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: { - return center_of_mass; - } - case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: { - return gravity_scale; - } - case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP: { - return linear_damp; - } - case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP: { - return angular_damp; - } - default: { - } - } - - return 0; -} - -void Body2DSW::set_mode(PhysicsServer2D::BodyMode p_mode) { - PhysicsServer2D::BodyMode prev = mode; - mode = p_mode; - - switch (p_mode) { - //CLEAR UP EVERYTHING IN CASE IT NOT WORKS! - case PhysicsServer2D::BODY_MODE_STATIC: - case PhysicsServer2D::BODY_MODE_KINEMATIC: { - _set_inv_transform(get_transform().affine_inverse()); - _inv_mass = 0; - _inv_inertia = 0; - _set_static(p_mode == PhysicsServer2D::BODY_MODE_STATIC); - set_active(p_mode == PhysicsServer2D::BODY_MODE_KINEMATIC && contacts.size()); - linear_velocity = Vector2(); - angular_velocity = 0; - if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC && prev != mode) { - first_time_kinematic = true; - } - } break; - case PhysicsServer2D::BODY_MODE_DYNAMIC: { - _inv_mass = mass > 0 ? (1.0 / mass) : 0; - if (!calculate_inertia) { - _inv_inertia = 1.0 / inertia; - } - _mass_properties_changed(); - _set_static(false); - set_active(true); - - } break; - case PhysicsServer2D::BODY_MODE_DYNAMIC_LINEAR: { - _inv_mass = mass > 0 ? (1.0 / mass) : 0; - _inv_inertia = 0; - angular_velocity = 0; - _set_static(false); - set_active(true); - } - } -} - -PhysicsServer2D::BodyMode Body2DSW::get_mode() const { - return mode; -} - -void Body2DSW::_shapes_changed() { - _mass_properties_changed(); - wakeup_neighbours(); -} - -void Body2DSW::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_variant) { - switch (p_state) { - case PhysicsServer2D::BODY_STATE_TRANSFORM: { - if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { - new_transform = p_variant; - //wakeup_neighbours(); - set_active(true); - if (first_time_kinematic) { - _set_transform(p_variant); - _set_inv_transform(get_transform().affine_inverse()); - first_time_kinematic = false; - } - } else if (mode == PhysicsServer2D::BODY_MODE_STATIC) { - _set_transform(p_variant); - _set_inv_transform(get_transform().affine_inverse()); - wakeup_neighbours(); - } else { - Transform2D t = p_variant; - t.orthonormalize(); - new_transform = get_transform(); //used as old to compute motion - if (t == new_transform) { - break; - } - _set_transform(t); - _set_inv_transform(get_transform().inverse()); - } - wakeup(); - - } break; - case PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY: { - linear_velocity = p_variant; - constant_linear_velocity = linear_velocity; - wakeup(); - - } break; - case PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY: { - angular_velocity = p_variant; - constant_angular_velocity = angular_velocity; - wakeup(); - - } break; - case PhysicsServer2D::BODY_STATE_SLEEPING: { - if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { - break; - } - bool do_sleep = p_variant; - if (do_sleep) { - linear_velocity = Vector2(); - //biased_linear_velocity=Vector3(); - angular_velocity = 0; - //biased_angular_velocity=Vector3(); - set_active(false); - } else { - if (mode != PhysicsServer2D::BODY_MODE_STATIC) { - set_active(true); - } - } - } break; - case PhysicsServer2D::BODY_STATE_CAN_SLEEP: { - can_sleep = p_variant; - if (mode >= PhysicsServer2D::BODY_MODE_DYNAMIC && !active && !can_sleep) { - set_active(true); - } - - } break; - } -} - -Variant Body2DSW::get_state(PhysicsServer2D::BodyState p_state) const { - switch (p_state) { - case PhysicsServer2D::BODY_STATE_TRANSFORM: { - return get_transform(); - } - case PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY: { - return linear_velocity; - } - case PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY: { - return angular_velocity; - } - case PhysicsServer2D::BODY_STATE_SLEEPING: { - return !is_active(); - } - case PhysicsServer2D::BODY_STATE_CAN_SLEEP: { - return can_sleep; - } - } - - return Variant(); -} - -void Body2DSW::set_space(Space2DSW *p_space) { - if (get_space()) { - wakeup_neighbours(); - - if (mass_properties_update_list.in_list()) { - get_space()->body_remove_from_mass_properties_update_list(&mass_properties_update_list); - } - if (active_list.in_list()) { - get_space()->body_remove_from_active_list(&active_list); - } - if (direct_state_query_list.in_list()) { - get_space()->body_remove_from_state_query_list(&direct_state_query_list); - } - } - - _set_space(p_space); - - if (get_space()) { - _mass_properties_changed(); - if (active) { - get_space()->body_add_to_active_list(&active_list); - } - } -} - -void Body2DSW::_compute_area_gravity_and_damping(const Area2DSW *p_area) { - Vector2 area_gravity; - p_area->compute_gravity(get_transform().get_origin(), area_gravity); - gravity += area_gravity; - - area_linear_damp += p_area->get_linear_damp(); - area_angular_damp += p_area->get_angular_damp(); -} - -void Body2DSW::integrate_forces(real_t p_step) { - if (mode == PhysicsServer2D::BODY_MODE_STATIC) { - return; - } - - Area2DSW *def_area = get_space()->get_default_area(); - // Area2DSW *damp_area = def_area; - ERR_FAIL_COND(!def_area); - - int ac = areas.size(); - bool stopped = false; - gravity = Vector2(0, 0); - area_angular_damp = 0; - area_linear_damp = 0; - if (ac) { - areas.sort(); - const AreaCMP *aa = &areas[0]; - // damp_area = aa[ac-1].area; - for (int i = ac - 1; i >= 0 && !stopped; i--) { - PhysicsServer2D::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode(); - switch (mode) { - case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE: - case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { - _compute_area_gravity_and_damping(aa[i].area); - stopped = mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; - } break; - case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE: - case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { - gravity = Vector2(0, 0); - area_angular_damp = 0; - area_linear_damp = 0; - _compute_area_gravity_and_damping(aa[i].area); - stopped = mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE; - } break; - default: { - } - } - } - } - if (!stopped) { - _compute_area_gravity_and_damping(def_area); - } - gravity *= gravity_scale; - - // If less than 0, override dampenings with that of the Body2D - if (angular_damp >= 0) { - area_angular_damp = angular_damp; - } - /* - else - area_angular_damp=damp_area->get_angular_damp(); - */ - - if (linear_damp >= 0) { - area_linear_damp = linear_damp; - } - /* - else - area_linear_damp=damp_area->get_linear_damp(); - */ - - Vector2 motion; - bool do_motion = false; - - if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { - //compute motion, angular and etc. velocities from prev transform - motion = new_transform.get_origin() - get_transform().get_origin(); - linear_velocity = constant_linear_velocity + motion / p_step; - - real_t rot = new_transform.get_rotation() - get_transform().get_rotation(); - angular_velocity = constant_angular_velocity + remainder(rot, 2.0 * Math_PI) / p_step; - - do_motion = true; - - /* - for(int i=0;ibody_add_to_state_query_list(&direct_state_query_list); - } - - if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { - _set_transform(new_transform, false); - _set_inv_transform(new_transform.affine_inverse()); - if (contacts.size() == 0 && linear_velocity == Vector2() && angular_velocity == 0) { - set_active(false); //stopped moving, deactivate - } - return; - } - - real_t total_angular_velocity = angular_velocity + biased_angular_velocity; - Vector2 total_linear_velocity = linear_velocity + biased_linear_velocity; - - real_t angle = get_transform().get_rotation() + total_angular_velocity * p_step; - Vector2 pos = get_transform().get_origin() + total_linear_velocity * p_step; - - real_t center_of_mass_distance = center_of_mass.length(); - if (center_of_mass_distance > CMP_EPSILON) { - // Calculate displacement due to center of mass offset. - real_t prev_angle = get_transform().get_rotation(); - real_t angle_base = Math::atan2(center_of_mass.y, center_of_mass.x); - Vector2 point1(Math::cos(angle_base + prev_angle), Math::sin(angle_base + prev_angle)); - Vector2 point2(Math::cos(angle_base + angle), Math::sin(angle_base + angle)); - pos += center_of_mass_distance * (point1 - point2); - } - - _set_transform(Transform2D(angle, pos), continuous_cd_mode == PhysicsServer2D::CCD_MODE_DISABLED); - _set_inv_transform(get_transform().inverse()); - - if (continuous_cd_mode != PhysicsServer2D::CCD_MODE_DISABLED) { - new_transform = get_transform(); - } -} - -void Body2DSW::wakeup_neighbours() { - for (const Pair &E : constraint_list) { - const Constraint2DSW *c = E.first; - Body2DSW **n = c->get_body_ptr(); - int bc = c->get_body_count(); - - for (int i = 0; i < bc; i++) { - if (i == E.second) { - continue; - } - Body2DSW *b = n[i]; - if (b->mode < PhysicsServer2D::BODY_MODE_DYNAMIC) { - continue; - } - - if (!b->is_active()) { - b->set_active(true); - } - } - } -} - -void Body2DSW::call_queries() { - if (fi_callback_data) { - if (!fi_callback_data->callable.get_object()) { - set_force_integration_callback(Callable()); - } else { - Variant direct_state_variant = get_direct_state(); - const Variant *vp[2] = { &direct_state_variant, &fi_callback_data->udata }; - - Callable::CallError ce; - Variant rv; - if (fi_callback_data->udata.get_type() != Variant::NIL) { - fi_callback_data->callable.call(vp, 2, rv, ce); - - } else { - fi_callback_data->callable.call(vp, 1, rv, ce); - } - } - } - - if (body_state_callback) { - (body_state_callback)(body_state_callback_instance, get_direct_state()); - } -} - -bool Body2DSW::sleep_test(real_t p_step) { - if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { - return true; - } else if (!can_sleep) { - return false; - } - - if (Math::abs(angular_velocity) < get_space()->get_body_angular_velocity_sleep_threshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_threshold() * get_space()->get_body_linear_velocity_sleep_threshold()) { - still_time += p_step; - - return still_time > get_space()->get_body_time_to_sleep(); - } else { - still_time = 0; //maybe this should be set to 0 on set_active? - return false; - } -} - -void Body2DSW::set_state_sync_callback(void *p_instance, PhysicsServer2D::BodyStateCallback p_callback) { - body_state_callback_instance = p_instance; - body_state_callback = p_callback; -} - -void Body2DSW::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) { - if (p_callable.get_object()) { - if (!fi_callback_data) { - fi_callback_data = memnew(ForceIntegrationCallbackData); - } - fi_callback_data->callable = p_callable; - fi_callback_data->udata = p_udata; - } else if (fi_callback_data) { - memdelete(fi_callback_data); - fi_callback_data = nullptr; - } -} - -PhysicsDirectBodyState2DSW *Body2DSW::get_direct_state() { - if (!direct_state) { - direct_state = memnew(PhysicsDirectBodyState2DSW); - direct_state->body = this; - } - return direct_state; -} - -Body2DSW::Body2DSW() : - CollisionObject2DSW(TYPE_BODY), - active_list(this), - mass_properties_update_list(this), - direct_state_query_list(this) { - _set_static(false); -} - -Body2DSW::~Body2DSW() { - if (fi_callback_data) { - memdelete(fi_callback_data); - } - if (direct_state) { - memdelete(direct_state); - } -} diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h deleted file mode 100644 index 822ff76fae..0000000000 --- a/servers/physics_2d/body_2d_sw.h +++ /dev/null @@ -1,353 +0,0 @@ -/*************************************************************************/ -/* body_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 BODY_2D_SW_H -#define BODY_2D_SW_H - -#include "area_2d_sw.h" -#include "collision_object_2d_sw.h" -#include "core/templates/list.h" -#include "core/templates/pair.h" -#include "core/templates/vset.h" - -class Constraint2DSW; -class PhysicsDirectBodyState2DSW; - -class Body2DSW : public CollisionObject2DSW { - PhysicsServer2D::BodyMode mode = PhysicsServer2D::BODY_MODE_DYNAMIC; - - Vector2 biased_linear_velocity; - real_t biased_angular_velocity = 0.0; - - Vector2 linear_velocity; - real_t angular_velocity = 0.0; - - Vector2 constant_linear_velocity; - real_t constant_angular_velocity = 0.0; - - real_t linear_damp = -1.0; - real_t angular_damp = -1.0; - real_t gravity_scale = 1.0; - - real_t bounce = 0.0; - real_t friction = 1.0; - - real_t mass = 1.0; - real_t _inv_mass = 1.0; - - real_t inertia = 0.0; - real_t _inv_inertia = 0.0; - - Vector2 center_of_mass; - - bool calculate_inertia = true; - bool calculate_center_of_mass = true; - - Vector2 gravity; - real_t area_linear_damp = 0.0; - real_t area_angular_damp = 0.0; - - real_t still_time = 0.0; - - Vector2 applied_force; - real_t applied_torque = 0.0; - - SelfList active_list; - SelfList mass_properties_update_list; - SelfList direct_state_query_list; - - VSet exceptions; - PhysicsServer2D::CCDMode continuous_cd_mode = PhysicsServer2D::CCD_MODE_DISABLED; - bool omit_force_integration = false; - bool active = true; - bool can_sleep = true; - bool first_time_kinematic = false; - void _mass_properties_changed(); - virtual void _shapes_changed(); - Transform2D new_transform; - - List> constraint_list; - - struct AreaCMP { - Area2DSW *area = nullptr; - int refCount = 0; - _FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); } - _FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); } - _FORCE_INLINE_ AreaCMP() {} - _FORCE_INLINE_ AreaCMP(Area2DSW *p_area) { - area = p_area; - refCount = 1; - } - }; - - Vector areas; - - struct Contact { - Vector2 local_pos; - Vector2 local_normal; - real_t depth = 0.0; - int local_shape = 0; - Vector2 collider_pos; - int collider_shape = 0; - ObjectID collider_instance_id; - RID collider; - Vector2 collider_velocity_at_pos; - }; - - Vector contacts; //no contacts by default - int contact_count = 0; - - void *body_state_callback_instance = nullptr; - PhysicsServer2D::BodyStateCallback body_state_callback = nullptr; - - struct ForceIntegrationCallbackData { - Callable callable; - Variant udata; - }; - - ForceIntegrationCallbackData *fi_callback_data = nullptr; - - PhysicsDirectBodyState2DSW *direct_state = nullptr; - - uint64_t island_step = 0; - - _FORCE_INLINE_ void _compute_area_gravity_and_damping(const Area2DSW *p_area); - - friend class PhysicsDirectBodyState2DSW; // i give up, too many functions to expose - -public: - void set_state_sync_callback(void *p_instance, PhysicsServer2D::BodyStateCallback p_callback); - void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant()); - - PhysicsDirectBodyState2DSW *get_direct_state(); - - _FORCE_INLINE_ void add_area(Area2DSW *p_area) { - int index = areas.find(AreaCMP(p_area)); - if (index > -1) { - areas.write[index].refCount += 1; - } else { - areas.ordered_insert(AreaCMP(p_area)); - } - } - - _FORCE_INLINE_ void remove_area(Area2DSW *p_area) { - int index = areas.find(AreaCMP(p_area)); - if (index > -1) { - areas.write[index].refCount -= 1; - if (areas[index].refCount < 1) { - areas.remove(index); - } - } - } - - _FORCE_INLINE_ void set_max_contacts_reported(int p_size) { - contacts.resize(p_size); - contact_count = 0; - if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC && p_size) { - set_active(true); - } - } - - _FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); } - - _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.is_empty(); } - _FORCE_INLINE_ void add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos); - - _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); } - _FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); } - _FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); } - _FORCE_INLINE_ const VSet &get_exceptions() const { return exceptions; } - - _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } - _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } - - _FORCE_INLINE_ void add_constraint(Constraint2DSW *p_constraint, int p_pos) { constraint_list.push_back({ p_constraint, p_pos }); } - _FORCE_INLINE_ void remove_constraint(Constraint2DSW *p_constraint, int p_pos) { constraint_list.erase({ p_constraint, p_pos }); } - const List> &get_constraint_list() const { return constraint_list; } - _FORCE_INLINE_ void clear_constraint_list() { constraint_list.clear(); } - - _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; } - _FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; } - - _FORCE_INLINE_ void set_linear_velocity(const Vector2 &p_velocity) { linear_velocity = p_velocity; } - _FORCE_INLINE_ Vector2 get_linear_velocity() const { return linear_velocity; } - - _FORCE_INLINE_ void set_angular_velocity(real_t p_velocity) { angular_velocity = p_velocity; } - _FORCE_INLINE_ real_t get_angular_velocity() const { return angular_velocity; } - - _FORCE_INLINE_ void set_biased_linear_velocity(const Vector2 &p_velocity) { biased_linear_velocity = p_velocity; } - _FORCE_INLINE_ Vector2 get_biased_linear_velocity() const { return biased_linear_velocity; } - - _FORCE_INLINE_ void set_biased_angular_velocity(real_t p_velocity) { biased_angular_velocity = p_velocity; } - _FORCE_INLINE_ real_t get_biased_angular_velocity() const { return biased_angular_velocity; } - - _FORCE_INLINE_ void apply_central_impulse(const Vector2 &p_impulse) { - linear_velocity += p_impulse * _inv_mass; - } - - _FORCE_INLINE_ void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) { - linear_velocity += p_impulse * _inv_mass; - angular_velocity += _inv_inertia * (p_position - center_of_mass).cross(p_impulse); - } - - _FORCE_INLINE_ void apply_torque_impulse(real_t p_torque) { - angular_velocity += _inv_inertia * p_torque; - } - - _FORCE_INLINE_ void apply_bias_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) { - biased_linear_velocity += p_impulse * _inv_mass; - biased_angular_velocity += _inv_inertia * (p_position - center_of_mass).cross(p_impulse); - } - - void set_active(bool p_active); - _FORCE_INLINE_ bool is_active() const { return active; } - - _FORCE_INLINE_ void wakeup() { - if ((!get_space()) || mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { - return; - } - set_active(true); - } - - void set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value); - Variant get_param(PhysicsServer2D::BodyParameter p_param) const; - - void set_mode(PhysicsServer2D::BodyMode p_mode); - PhysicsServer2D::BodyMode get_mode() const; - - void set_state(PhysicsServer2D::BodyState p_state, const Variant &p_variant); - Variant get_state(PhysicsServer2D::BodyState p_state) const; - - void set_applied_force(const Vector2 &p_force) { applied_force = p_force; } - Vector2 get_applied_force() const { return applied_force; } - - void set_applied_torque(real_t p_torque) { applied_torque = p_torque; } - real_t get_applied_torque() const { return applied_torque; } - - _FORCE_INLINE_ void add_central_force(const Vector2 &p_force) { - applied_force += p_force; - } - - _FORCE_INLINE_ void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) { - applied_force += p_force; - applied_torque += (p_position - center_of_mass).cross(p_force); - } - - _FORCE_INLINE_ void add_torque(real_t p_torque) { - applied_torque += p_torque; - } - - _FORCE_INLINE_ void set_continuous_collision_detection_mode(PhysicsServer2D::CCDMode p_mode) { continuous_cd_mode = p_mode; } - _FORCE_INLINE_ PhysicsServer2D::CCDMode get_continuous_collision_detection_mode() const { return continuous_cd_mode; } - - void set_space(Space2DSW *p_space); - - void update_mass_properties(); - void reset_mass_properties(); - - _FORCE_INLINE_ Vector2 get_center_of_mass() const { return center_of_mass; } - _FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; } - _FORCE_INLINE_ real_t get_inv_inertia() const { return _inv_inertia; } - _FORCE_INLINE_ real_t get_friction() const { return friction; } - _FORCE_INLINE_ Vector2 get_gravity() const { return gravity; } - _FORCE_INLINE_ real_t get_bounce() const { return bounce; } - _FORCE_INLINE_ real_t get_linear_damp() const { return linear_damp; } - _FORCE_INLINE_ real_t get_angular_damp() const { return angular_damp; } - - void integrate_forces(real_t p_step); - void integrate_velocities(real_t p_step); - - _FORCE_INLINE_ Vector2 get_velocity_in_local_point(const Vector2 &rel_pos) const { - return linear_velocity + Vector2(-angular_velocity * rel_pos.y, angular_velocity * rel_pos.x); - } - - _FORCE_INLINE_ Vector2 get_motion() const { - if (mode > PhysicsServer2D::BODY_MODE_KINEMATIC) { - return new_transform.get_origin() - get_transform().get_origin(); - } else if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { - return get_transform().get_origin() - new_transform.get_origin(); //kinematic simulates forward - } - return Vector2(); - } - - void call_queries(); - void wakeup_neighbours(); - - bool sleep_test(real_t p_step); - - Body2DSW(); - ~Body2DSW(); -}; - -//add contact inline - -void Body2DSW::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos) { - int c_max = contacts.size(); - - if (c_max == 0) { - return; - } - - Contact *c = contacts.ptrw(); - - int idx = -1; - - if (contact_count < c_max) { - idx = contact_count++; - } else { - real_t least_depth = 1e20; - int least_deep = -1; - for (int i = 0; i < c_max; i++) { - if (i == 0 || c[i].depth < least_depth) { - least_deep = i; - least_depth = c[i].depth; - } - } - - if (least_deep >= 0 && least_depth < p_depth) { - idx = least_deep; - } - if (idx == -1) { - return; //none least deepe than this - } - } - - c[idx].local_pos = p_local_pos; - c[idx].local_normal = p_local_normal; - c[idx].depth = p_depth; - c[idx].local_shape = p_local_shape; - c[idx].collider_pos = p_collider_pos; - c[idx].collider_shape = p_collider_shape; - c[idx].collider_instance_id = p_collider_instance_id; - c[idx].collider = p_collider; - c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos; -} - -#endif // BODY_2D_SW_H diff --git a/servers/physics_2d/body_direct_state_2d_sw.cpp b/servers/physics_2d/body_direct_state_2d_sw.cpp deleted file mode 100644 index b0673b9006..0000000000 --- a/servers/physics_2d/body_direct_state_2d_sw.cpp +++ /dev/null @@ -1,178 +0,0 @@ -/*************************************************************************/ -/* body_direct_state_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "body_direct_state_2d_sw.h" - -#include "body_2d_sw.h" -#include "physics_server_2d_sw.h" -#include "space_2d_sw.h" - -Vector2 PhysicsDirectBodyState2DSW::get_total_gravity() const { - return body->gravity; -} - -real_t PhysicsDirectBodyState2DSW::get_total_angular_damp() const { - return body->area_angular_damp; -} - -real_t PhysicsDirectBodyState2DSW::get_total_linear_damp() const { - return body->area_linear_damp; -} - -Vector2 PhysicsDirectBodyState2DSW::get_center_of_mass() const { - return body->get_center_of_mass(); -} - -real_t PhysicsDirectBodyState2DSW::get_inverse_mass() const { - return body->get_inv_mass(); -} - -real_t PhysicsDirectBodyState2DSW::get_inverse_inertia() const { - return body->get_inv_inertia(); -} - -void PhysicsDirectBodyState2DSW::set_linear_velocity(const Vector2 &p_velocity) { - body->wakeup(); - body->set_linear_velocity(p_velocity); -} - -Vector2 PhysicsDirectBodyState2DSW::get_linear_velocity() const { - return body->get_linear_velocity(); -} - -void PhysicsDirectBodyState2DSW::set_angular_velocity(real_t p_velocity) { - body->wakeup(); - body->set_angular_velocity(p_velocity); -} - -real_t PhysicsDirectBodyState2DSW::get_angular_velocity() const { - return body->get_angular_velocity(); -} - -void PhysicsDirectBodyState2DSW::set_transform(const Transform2D &p_transform) { - body->set_state(PhysicsServer2D::BODY_STATE_TRANSFORM, p_transform); -} - -Transform2D PhysicsDirectBodyState2DSW::get_transform() const { - return body->get_transform(); -} - -Vector2 PhysicsDirectBodyState2DSW::get_velocity_at_local_position(const Vector2 &p_position) const { - return body->get_velocity_in_local_point(p_position); -} - -void PhysicsDirectBodyState2DSW::add_central_force(const Vector2 &p_force) { - body->wakeup(); - body->add_central_force(p_force); -} - -void PhysicsDirectBodyState2DSW::add_force(const Vector2 &p_force, const Vector2 &p_position) { - body->wakeup(); - body->add_force(p_force, p_position); -} - -void PhysicsDirectBodyState2DSW::add_torque(real_t p_torque) { - body->wakeup(); - body->add_torque(p_torque); -} - -void PhysicsDirectBodyState2DSW::apply_central_impulse(const Vector2 &p_impulse) { - body->wakeup(); - body->apply_central_impulse(p_impulse); -} - -void PhysicsDirectBodyState2DSW::apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position) { - body->wakeup(); - body->apply_impulse(p_impulse, p_position); -} - -void PhysicsDirectBodyState2DSW::apply_torque_impulse(real_t p_torque) { - body->wakeup(); - body->apply_torque_impulse(p_torque); -} - -void PhysicsDirectBodyState2DSW::set_sleep_state(bool p_enable) { - body->set_active(!p_enable); -} - -bool PhysicsDirectBodyState2DSW::is_sleeping() const { - return !body->is_active(); -} - -int PhysicsDirectBodyState2DSW::get_contact_count() const { - return body->contact_count; -} - -Vector2 PhysicsDirectBodyState2DSW::get_contact_local_position(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); - return body->contacts[p_contact_idx].local_pos; -} - -Vector2 PhysicsDirectBodyState2DSW::get_contact_local_normal(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); - return body->contacts[p_contact_idx].local_normal; -} - -int PhysicsDirectBodyState2DSW::get_contact_local_shape(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1); - return body->contacts[p_contact_idx].local_shape; -} - -RID PhysicsDirectBodyState2DSW::get_contact_collider(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID()); - return body->contacts[p_contact_idx].collider; -} -Vector2 PhysicsDirectBodyState2DSW::get_contact_collider_position(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); - return body->contacts[p_contact_idx].collider_pos; -} - -ObjectID PhysicsDirectBodyState2DSW::get_contact_collider_id(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID()); - return body->contacts[p_contact_idx].collider_instance_id; -} - -int PhysicsDirectBodyState2DSW::get_contact_collider_shape(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0); - return body->contacts[p_contact_idx].collider_shape; -} - -Vector2 PhysicsDirectBodyState2DSW::get_contact_collider_velocity_at_position(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); - return body->contacts[p_contact_idx].collider_velocity_at_pos; -} - -PhysicsDirectSpaceState2D *PhysicsDirectBodyState2DSW::get_space_state() { - return body->get_space()->get_direct_state(); -} - -real_t PhysicsDirectBodyState2DSW::get_step() const { - return body->get_space()->get_last_step(); -} diff --git a/servers/physics_2d/body_direct_state_2d_sw.h b/servers/physics_2d/body_direct_state_2d_sw.h deleted file mode 100644 index 4266b24842..0000000000 --- a/servers/physics_2d/body_direct_state_2d_sw.h +++ /dev/null @@ -1,91 +0,0 @@ -/*************************************************************************/ -/* body_direct_state_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 BODY_DIRECT_STATE_2D_SW_H -#define BODY_DIRECT_STATE_2D_SW_H - -#include "servers/physics_server_2d.h" - -class Body2DSW; - -class PhysicsDirectBodyState2DSW : public PhysicsDirectBodyState2D { - GDCLASS(PhysicsDirectBodyState2DSW, PhysicsDirectBodyState2D); - -public: - Body2DSW *body = nullptr; - - virtual Vector2 get_total_gravity() const override; - virtual real_t get_total_angular_damp() const override; - virtual real_t get_total_linear_damp() const override; - - virtual Vector2 get_center_of_mass() const override; - virtual real_t get_inverse_mass() const override; - virtual real_t get_inverse_inertia() const override; - - virtual void set_linear_velocity(const Vector2 &p_velocity) override; - virtual Vector2 get_linear_velocity() const override; - - virtual void set_angular_velocity(real_t p_velocity) override; - virtual real_t get_angular_velocity() const override; - - virtual void set_transform(const Transform2D &p_transform) override; - virtual Transform2D get_transform() const override; - - virtual Vector2 get_velocity_at_local_position(const Vector2 &p_position) const override; - - virtual void add_central_force(const Vector2 &p_force) override; - virtual void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override; - virtual void add_torque(real_t p_torque) override; - virtual void apply_central_impulse(const Vector2 &p_impulse) override; - virtual void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override; - virtual void apply_torque_impulse(real_t p_torque) override; - - virtual void set_sleep_state(bool p_enable) override; - virtual bool is_sleeping() const override; - - virtual int get_contact_count() const override; - - virtual Vector2 get_contact_local_position(int p_contact_idx) const override; - virtual Vector2 get_contact_local_normal(int p_contact_idx) const override; - virtual int get_contact_local_shape(int p_contact_idx) const override; - - virtual RID get_contact_collider(int p_contact_idx) const override; - virtual Vector2 get_contact_collider_position(int p_contact_idx) const override; - virtual ObjectID get_contact_collider_id(int p_contact_idx) const override; - virtual int get_contact_collider_shape(int p_contact_idx) const override; - - virtual Vector2 get_contact_collider_velocity_at_position(int p_contact_idx) const override; - - virtual PhysicsDirectSpaceState2D *get_space_state() override; - - virtual real_t get_step() const override; -}; - -#endif // BODY_2D_SW_H diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp deleted file mode 100644 index 8bcc4609f4..0000000000 --- a/servers/physics_2d/body_pair_2d_sw.cpp +++ /dev/null @@ -1,545 +0,0 @@ -/*************************************************************************/ -/* body_pair_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "body_pair_2d_sw.h" -#include "collision_solver_2d_sw.h" -#include "space_2d_sw.h" - -#define POSITION_CORRECTION -#define ACCUMULATE_IMPULSES - -void BodyPair2DSW::_add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self) { - BodyPair2DSW *self = (BodyPair2DSW *)p_self; - - self->_contact_added_callback(p_point_A, p_point_B); -} - -void BodyPair2DSW::_contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B) { - // check if we already have the contact - - Vector2 local_A = A->get_inv_transform().basis_xform(p_point_A); - Vector2 local_B = B->get_inv_transform().basis_xform(p_point_B - offset_B); - - int new_index = contact_count; - - ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1)); - - Contact contact; - - contact.acc_normal_impulse = 0; - contact.acc_bias_impulse = 0; - contact.acc_tangent_impulse = 0; - contact.local_A = local_A; - contact.local_B = local_B; - contact.reused = true; - contact.normal = (p_point_A - p_point_B).normalized(); - contact.mass_normal = 0; // will be computed in setup() - - // attempt to determine if the contact will be reused - - real_t recycle_radius_2 = space->get_contact_recycle_radius() * space->get_contact_recycle_radius(); - - for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; - if ( - c.local_A.distance_squared_to(local_A) < (recycle_radius_2) && - c.local_B.distance_squared_to(local_B) < (recycle_radius_2)) { - contact.acc_normal_impulse = c.acc_normal_impulse; - contact.acc_tangent_impulse = c.acc_tangent_impulse; - contact.acc_bias_impulse = c.acc_bias_impulse; - new_index = i; - break; - } - } - - // figure out if the contact amount must be reduced to fit the new contact - - if (new_index == MAX_CONTACTS) { - // remove the contact with the minimum depth - - int least_deep = -1; - real_t min_depth = 1e10; - - const Transform2D &transform_A = A->get_transform(); - const Transform2D &transform_B = B->get_transform(); - - for (int i = 0; i <= contact_count; i++) { - Contact &c = (i == contact_count) ? contact : contacts[i]; - Vector2 global_A = transform_A.basis_xform(c.local_A); - Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; - - Vector2 axis = global_A - global_B; - real_t depth = axis.dot(c.normal); - - if (depth < min_depth) { - min_depth = depth; - least_deep = i; - } - } - - ERR_FAIL_COND(least_deep == -1); - - if (least_deep < contact_count) { //replace the last deep contact by the new one - - contacts[least_deep] = contact; - } - - return; - } - - contacts[new_index] = contact; - - if (new_index == contact_count) { - contact_count++; - } -} - -void BodyPair2DSW::_validate_contacts() { - //make sure to erase contacts that are no longer valid - - real_t max_separation = space->get_contact_max_separation(); - real_t max_separation2 = max_separation * max_separation; - - const Transform2D &transform_A = A->get_transform(); - const Transform2D &transform_B = B->get_transform(); - - for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; - - bool erase = false; - if (!c.reused) { - //was left behind in previous frame - erase = true; - } else { - c.reused = false; - - Vector2 global_A = transform_A.basis_xform(c.local_A); - Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; - Vector2 axis = global_A - global_B; - real_t depth = axis.dot(c.normal); - - if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) { - erase = true; - } - } - - if (erase) { - // contact no longer needed, remove - - if ((i + 1) < contact_count) { - // swap with the last one - SWAP(contacts[i], contacts[contact_count - 1]); - } - - i--; - contact_count--; - } - } -} - -bool BodyPair2DSW::_test_ccd(real_t p_step, Body2DSW *p_A, int p_shape_A, const Transform2D &p_xform_A, Body2DSW *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result) { - Vector2 motion = p_A->get_linear_velocity() * p_step; - real_t mlen = motion.length(); - if (mlen < CMP_EPSILON) { - return false; - } - - Vector2 mnormal = motion / mlen; - - real_t min, max; - p_A->get_shape(p_shape_A)->project_rangev(mnormal, p_xform_A, min, max); - bool fast_object = mlen > (max - min) * 0.3; //going too fast in that direction - - if (!fast_object) { //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis - return false; - } - - //cast a segment from support in motion normal, in the same direction of motion by motion length - //support is the worst case collision point, so real collision happened before - int a; - Vector2 s[2]; - p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(), s, a); - Vector2 from = p_xform_A.xform(s[0]); - Vector2 to = from + motion; - - Transform2D from_inv = p_xform_B.affine_inverse(); - - Vector2 local_from = from_inv.xform(from - mnormal * mlen * 0.1); //start from a little inside the bounding box - Vector2 local_to = from_inv.xform(to); - - Vector2 rpos, rnorm; - if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) { - return false; - } - - //ray hit something - - Vector2 hitpos = p_xform_B.xform(rpos); - - Vector2 contact_A = to; - Vector2 contact_B = hitpos; - - //create a contact - - if (p_swap_result) { - _contact_added_callback(contact_B, contact_A); - } else { - _contact_added_callback(contact_A, contact_B); - } - - return true; -} - -real_t combine_bounce(Body2DSW *A, Body2DSW *B) { - return CLAMP(A->get_bounce() + B->get_bounce(), 0, 1); -} - -real_t combine_friction(Body2DSW *A, Body2DSW *B) { - return ABS(MIN(A->get_friction(), B->get_friction())); -} - -bool BodyPair2DSW::setup(real_t p_step) { - if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) { - collided = false; - return false; - } - - collide_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && A->collides_with(B); - collide_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && B->collides_with(A); - - report_contacts_only = false; - if (!collide_A && !collide_B) { - if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) { - report_contacts_only = true; - } else { - collided = false; - return false; - } - } - - //use local A coordinates to avoid numerical issues on collision detection - offset_B = B->get_transform().get_origin() - A->get_transform().get_origin(); - - _validate_contacts(); - - const Vector2 &offset_A = A->get_transform().get_origin(); - Transform2D xform_Au = A->get_transform().untranslated(); - Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A); - - Transform2D xform_Bu = B->get_transform(); - xform_Bu.elements[2] -= offset_A; - Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B); - - Shape2DSW *shape_A_ptr = A->get_shape(shape_A); - Shape2DSW *shape_B_ptr = B->get_shape(shape_B); - - Vector2 motion_A, motion_B; - - if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_SHAPE) { - motion_A = A->get_motion(); - } - if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_SHAPE) { - motion_B = B->get_motion(); - } - - bool prev_collided = collided; - - collided = CollisionSolver2DSW::solve(shape_A_ptr, xform_A, motion_A, shape_B_ptr, xform_B, motion_B, _add_contact, this, &sep_axis); - if (!collided) { - //test ccd (currently just a raycast) - - if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) { - if (_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B)) { - collided = true; - } - } - - if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_B) { - if (_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A, true)) { - collided = true; - } - } - - if (!collided) { - oneway_disabled = false; - return false; - } - } - - if (oneway_disabled) { - return false; - } - - if (!prev_collided) { - if (shape_B_ptr->allows_one_way_collision() && A->is_shape_set_as_one_way_collision(shape_A)) { - Vector2 direction = xform_A.get_axis(1).normalized(); - bool valid = false; - for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; - if (!c.reused) { - continue; - } - if (c.normal.dot(direction) > -CMP_EPSILON) { //greater (normal inverted) - continue; - } - valid = true; - break; - } - if (!valid) { - collided = false; - oneway_disabled = true; - return false; - } - } - - if (shape_A_ptr->allows_one_way_collision() && B->is_shape_set_as_one_way_collision(shape_B)) { - Vector2 direction = xform_B.get_axis(1).normalized(); - bool valid = false; - for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; - if (!c.reused) { - continue; - } - if (c.normal.dot(direction) < CMP_EPSILON) { //less (normal ok) - continue; - } - valid = true; - break; - } - if (!valid) { - collided = false; - oneway_disabled = true; - return false; - } - } - } - - return true; -} - -bool BodyPair2DSW::pre_solve(real_t p_step) { - if (!collided || oneway_disabled) { - return false; - } - - real_t max_penetration = space->get_contact_max_allowed_penetration(); - - real_t bias = 0.3; - - Shape2DSW *shape_A_ptr = A->get_shape(shape_A); - Shape2DSW *shape_B_ptr = B->get_shape(shape_B); - - if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) { - if (shape_A_ptr->get_custom_bias() == 0) { - bias = shape_B_ptr->get_custom_bias(); - } else if (shape_B_ptr->get_custom_bias() == 0) { - bias = shape_A_ptr->get_custom_bias(); - } else { - bias = (shape_B_ptr->get_custom_bias() + shape_A_ptr->get_custom_bias()) * 0.5; - } - } - - real_t inv_dt = 1.0 / p_step; - - bool do_process = false; - - const Vector2 &offset_A = A->get_transform().get_origin(); - const Transform2D &transform_A = A->get_transform(); - const Transform2D &transform_B = B->get_transform(); - - real_t inv_inertia_A = collide_A ? A->get_inv_inertia() : 0.0; - real_t inv_inertia_B = collide_B ? B->get_inv_inertia() : 0.0; - - real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0; - real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0; - - for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; - c.active = false; - - Vector2 global_A = transform_A.basis_xform(c.local_A); - Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; - - Vector2 axis = global_A - global_B; - real_t depth = axis.dot(c.normal); - - if (depth <= 0.0 || !c.reused) { - continue; - } - -#ifdef DEBUG_ENABLED - if (space->is_debugging_contacts()) { - space->add_debug_contact(global_A + offset_A); - space->add_debug_contact(global_B + offset_A); - } -#endif - - c.rA = global_A; - c.rB = global_B - offset_B; - - if (A->can_report_contacts()) { - Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x); - A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, global_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB + B->get_linear_velocity()); - } - - if (B->can_report_contacts()) { - Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x); - B->add_contact(global_B + offset_A, c.normal, depth, shape_B, global_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA + A->get_linear_velocity()); - } - - if (report_contacts_only) { - collided = false; - continue; - } - - // Precompute normal mass, tangent mass, and bias. - real_t rnA = c.rA.dot(c.normal); - real_t rnB = c.rB.dot(c.normal); - real_t kNormal = inv_mass_A + inv_mass_B; - kNormal += inv_inertia_A * (c.rA.dot(c.rA) - rnA * rnA) + inv_inertia_B * (c.rB.dot(c.rB) - rnB * rnB); - c.mass_normal = 1.0f / kNormal; - - Vector2 tangent = c.normal.orthogonal(); - real_t rtA = c.rA.dot(tangent); - real_t rtB = c.rB.dot(tangent); - real_t kTangent = inv_mass_A + inv_mass_B; - kTangent += inv_inertia_A * (c.rA.dot(c.rA) - rtA * rtA) + inv_inertia_B * (c.rB.dot(c.rB) - rtB * rtB); - c.mass_tangent = 1.0f / kTangent; - - c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration); - c.depth = depth; - //c.acc_bias_impulse=0; - -#ifdef ACCUMULATE_IMPULSES - { - // Apply normal + friction impulse - Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent; - - if (collide_A) { - A->apply_impulse(-P, c.rA); - } - if (collide_B) { - B->apply_impulse(P, c.rB); - } - } -#endif - - c.bounce = combine_bounce(A, B); - if (c.bounce) { - Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x); - Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x); - Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; - c.bounce = c.bounce * dv.dot(c.normal); - } - - c.active = true; - do_process = true; - } - - return do_process; -} - -void BodyPair2DSW::solve(real_t p_step) { - if (!collided || oneway_disabled) { - return; - } - - for (int i = 0; i < contact_count; ++i) { - Contact &c = contacts[i]; - - if (!c.active) { - continue; - } - - // Relative velocity at contact - - Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x); - Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x); - Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; - - Vector2 crbA(-A->get_biased_angular_velocity() * c.rA.y, A->get_biased_angular_velocity() * c.rA.x); - Vector2 crbB(-B->get_biased_angular_velocity() * c.rB.y, B->get_biased_angular_velocity() * c.rB.x); - Vector2 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA; - - real_t vn = dv.dot(c.normal); - real_t vbn = dbv.dot(c.normal); - Vector2 tangent = c.normal.orthogonal(); - real_t vt = dv.dot(tangent); - - real_t jbn = (c.bias - vbn) * c.mass_normal; - real_t jbnOld = c.acc_bias_impulse; - c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f); - - Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld); - - if (collide_A) { - A->apply_bias_impulse(-jb, c.rA); - } - if (collide_B) { - B->apply_bias_impulse(jb, c.rB); - } - - real_t jn = -(c.bounce + vn) * c.mass_normal; - real_t jnOld = c.acc_normal_impulse; - c.acc_normal_impulse = MAX(jnOld + jn, 0.0f); - - real_t friction = combine_friction(A, B); - - real_t jtMax = friction * c.acc_normal_impulse; - real_t jt = -vt * c.mass_tangent; - real_t jtOld = c.acc_tangent_impulse; - c.acc_tangent_impulse = CLAMP(jtOld + jt, -jtMax, jtMax); - - Vector2 j = c.normal * (c.acc_normal_impulse - jnOld) + tangent * (c.acc_tangent_impulse - jtOld); - - if (collide_A) { - A->apply_impulse(-j, c.rA); - } - if (collide_B) { - B->apply_impulse(j, c.rB); - } - } -} - -BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_shape_B) : - Constraint2DSW(_arr, 2) { - A = p_A; - B = p_B; - shape_A = p_shape_A; - shape_B = p_shape_B; - space = A->get_space(); - A->add_constraint(this, 0); - B->add_constraint(this, 1); -} - -BodyPair2DSW::~BodyPair2DSW() { - A->remove_constraint(this, 0); - B->remove_constraint(this, 1); -} diff --git a/servers/physics_2d/body_pair_2d_sw.h b/servers/physics_2d/body_pair_2d_sw.h deleted file mode 100644 index db4f3eba69..0000000000 --- a/servers/physics_2d/body_pair_2d_sw.h +++ /dev/null @@ -1,98 +0,0 @@ -/*************************************************************************/ -/* body_pair_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 BODY_PAIR_2D_SW_H -#define BODY_PAIR_2D_SW_H - -#include "body_2d_sw.h" -#include "constraint_2d_sw.h" - -class BodyPair2DSW : public Constraint2DSW { - enum { - MAX_CONTACTS = 2 - }; - union { - struct { - Body2DSW *A; - Body2DSW *B; - }; - - Body2DSW *_arr[2] = { nullptr, nullptr }; - }; - - int shape_A = 0; - int shape_B = 0; - - bool collide_A = false; - bool collide_B = false; - - Space2DSW *space = nullptr; - - struct Contact { - Vector2 position; - Vector2 normal; - Vector2 local_A, local_B; - real_t acc_normal_impulse = 0.0; // accumulated normal impulse (Pn) - real_t acc_tangent_impulse = 0.0; // accumulated tangent impulse (Pt) - real_t acc_bias_impulse = 0.0; // accumulated normal impulse for position bias (Pnb) - real_t mass_normal, mass_tangent = 0.0; - real_t bias = 0.0; - - real_t depth = 0.0; - bool active = false; - Vector2 rA, rB; - bool reused = false; - real_t bounce = 0.0; - }; - - Vector2 offset_B; //use local A coordinates to avoid numerical issues on collision detection - - Vector2 sep_axis; - Contact contacts[MAX_CONTACTS]; - int contact_count = 0; - bool collided = false; - bool oneway_disabled = false; - bool report_contacts_only = false; - - bool _test_ccd(real_t p_step, Body2DSW *p_A, int p_shape_A, const Transform2D &p_xform_A, Body2DSW *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result = false); - void _validate_contacts(); - static void _add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self); - _FORCE_INLINE_ void _contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B); - -public: - virtual bool setup(real_t p_step) override; - virtual bool pre_solve(real_t p_step) override; - virtual void solve(real_t p_step) override; - - BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_shape_B); - ~BodyPair2DSW(); -}; - -#endif // BODY_PAIR_2D_SW_H diff --git a/servers/physics_2d/broad_phase_2d_bvh.cpp b/servers/physics_2d/broad_phase_2d_bvh.cpp deleted file mode 100644 index 0df7086c5a..0000000000 --- a/servers/physics_2d/broad_phase_2d_bvh.cpp +++ /dev/null @@ -1,113 +0,0 @@ -/*************************************************************************/ -/* broad_phase_2d_bvh.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "broad_phase_2d_bvh.h" -#include "collision_object_2d_sw.h" - -BroadPhase2DSW::ID BroadPhase2DBVH::create(CollisionObject2DSW *p_object, int p_subindex, const Rect2 &p_aabb, bool p_static) { - ID oid = bvh.create(p_object, true, p_aabb, p_subindex, !p_static, 1 << p_object->get_type(), p_static ? 0 : 0xFFFFF); // Pair everything, don't care? - return oid + 1; -} - -void BroadPhase2DBVH::move(ID p_id, const Rect2 &p_aabb) { - bvh.move(p_id - 1, p_aabb); -} - -void BroadPhase2DBVH::set_static(ID p_id, bool p_static) { - CollisionObject2DSW *it = bvh.get(p_id - 1); - bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF, false); // Pair everything, don't care? -} - -void BroadPhase2DBVH::remove(ID p_id) { - bvh.erase(p_id - 1); -} - -CollisionObject2DSW *BroadPhase2DBVH::get_object(ID p_id) const { - CollisionObject2DSW *it = bvh.get(p_id - 1); - ERR_FAIL_COND_V(!it, nullptr); - return it; -} - -bool BroadPhase2DBVH::is_static(ID p_id) const { - return !bvh.is_pairable(p_id - 1); -} - -int BroadPhase2DBVH::get_subindex(ID p_id) const { - return bvh.get_subindex(p_id - 1); -} - -int BroadPhase2DBVH::cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { - return bvh.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices); -} - -int BroadPhase2DBVH::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { - return bvh.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices); -} - -void *BroadPhase2DBVH::_pair_callback(void *self, uint32_t p_A, CollisionObject2DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject2DSW *p_object_B, int subindex_B) { - BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(self); - if (!bpo->pair_callback) { - return nullptr; - } - - return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata); -} - -void BroadPhase2DBVH::_unpair_callback(void *self, uint32_t p_A, CollisionObject2DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject2DSW *p_object_B, int subindex_B, void *pairdata) { - BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(self); - if (!bpo->unpair_callback) { - return; - } - - bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata); -} - -void BroadPhase2DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { - pair_callback = p_pair_callback; - pair_userdata = p_userdata; -} - -void BroadPhase2DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { - unpair_callback = p_unpair_callback; - unpair_userdata = p_userdata; -} - -void BroadPhase2DBVH::update() { - bvh.update(); -} - -BroadPhase2DSW *BroadPhase2DBVH::_create() { - return memnew(BroadPhase2DBVH); -} - -BroadPhase2DBVH::BroadPhase2DBVH() { - bvh.set_pair_callback(_pair_callback, this); - bvh.set_unpair_callback(_unpair_callback, this); -} diff --git a/servers/physics_2d/broad_phase_2d_bvh.h b/servers/physics_2d/broad_phase_2d_bvh.h deleted file mode 100644 index ea02a98417..0000000000 --- a/servers/physics_2d/broad_phase_2d_bvh.h +++ /dev/null @@ -1,73 +0,0 @@ -/*************************************************************************/ -/* broad_phase_2d_bvh.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 BROAD_PHASE_2D_BVH_H -#define BROAD_PHASE_2D_BVH_H - -#include "broad_phase_2d_sw.h" -#include "core/math/bvh.h" -#include "core/math/rect2.h" -#include "core/math/vector2.h" - -class BroadPhase2DBVH : public BroadPhase2DSW { - BVH_Manager bvh; - - static void *_pair_callback(void *, uint32_t, CollisionObject2DSW *, int, uint32_t, CollisionObject2DSW *, int); - static void _unpair_callback(void *, uint32_t, CollisionObject2DSW *, int, uint32_t, CollisionObject2DSW *, int, void *); - - PairCallback pair_callback = nullptr; - void *pair_userdata = nullptr; - UnpairCallback unpair_callback = nullptr; - void *unpair_userdata = nullptr; - -public: - // 0 is an invalid ID - virtual ID create(CollisionObject2DSW *p_object, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false); - virtual void move(ID p_id, const Rect2 &p_aabb); - virtual void set_static(ID p_id, bool p_static); - virtual void remove(ID p_id); - - virtual CollisionObject2DSW *get_object(ID p_id) const; - virtual bool is_static(ID p_id) const; - virtual int get_subindex(ID p_id) const; - - virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr); - virtual int cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr); - - virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata); - virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata); - - virtual void update(); - - static BroadPhase2DSW *_create(); - BroadPhase2DBVH(); -}; - -#endif // BROAD_PHASE_2D_BVH_H diff --git a/servers/physics_2d/broad_phase_2d_sw.cpp b/servers/physics_2d/broad_phase_2d_sw.cpp deleted file mode 100644 index 7f0af48b1f..0000000000 --- a/servers/physics_2d/broad_phase_2d_sw.cpp +++ /dev/null @@ -1,36 +0,0 @@ -/*************************************************************************/ -/* broad_phase_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "broad_phase_2d_sw.h" - -BroadPhase2DSW::CreateFunction BroadPhase2DSW::create_func = nullptr; - -BroadPhase2DSW::~BroadPhase2DSW() { -} diff --git a/servers/physics_2d/broad_phase_2d_sw.h b/servers/physics_2d/broad_phase_2d_sw.h deleted file mode 100644 index 0f82f06b9c..0000000000 --- a/servers/physics_2d/broad_phase_2d_sw.h +++ /dev/null @@ -1,71 +0,0 @@ -/*************************************************************************/ -/* broad_phase_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 BROAD_PHASE_2D_SW_H -#define BROAD_PHASE_2D_SW_H - -#include "core/math/math_funcs.h" -#include "core/math/rect2.h" - -class CollisionObject2DSW; - -class BroadPhase2DSW { -public: - typedef BroadPhase2DSW *(*CreateFunction)(); - - static CreateFunction create_func; - - typedef uint32_t ID; - - typedef void *(*PairCallback)(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_userdata); - typedef void (*UnpairCallback)(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, void *p_userdata); - - // 0 is an invalid ID - virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false) = 0; - virtual void move(ID p_id, const Rect2 &p_aabb) = 0; - virtual void set_static(ID p_id, bool p_static) = 0; - virtual void remove(ID p_id) = 0; - - virtual CollisionObject2DSW *get_object(ID p_id) const = 0; - virtual bool is_static(ID p_id) const = 0; - virtual int get_subindex(ID p_id) const = 0; - - virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; - virtual int cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; - - virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata) = 0; - virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) = 0; - - virtual void update() = 0; - - virtual ~BroadPhase2DSW(); -}; - -#endif // BROAD_PHASE_2D_SW_H diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp deleted file mode 100644 index bc7d277152..0000000000 --- a/servers/physics_2d/collision_object_2d_sw.cpp +++ /dev/null @@ -1,243 +0,0 @@ -/*************************************************************************/ -/* collision_object_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "collision_object_2d_sw.h" -#include "servers/physics_2d/physics_server_2d_sw.h" -#include "space_2d_sw.h" - -void CollisionObject2DSW::add_shape(Shape2DSW *p_shape, const Transform2D &p_transform, bool p_disabled) { - Shape s; - s.shape = p_shape; - s.xform = p_transform; - s.xform_inv = s.xform.affine_inverse(); - s.bpid = 0; //needs update - s.disabled = p_disabled; - s.one_way_collision = false; - s.one_way_collision_margin = 0; - shapes.push_back(s); - p_shape->add_owner(this); - - if (!pending_shape_update_list.in_list()) { - PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); - } -} - -void CollisionObject2DSW::set_shape(int p_index, Shape2DSW *p_shape) { - ERR_FAIL_INDEX(p_index, shapes.size()); - shapes[p_index].shape->remove_owner(this); - shapes.write[p_index].shape = p_shape; - - p_shape->add_owner(this); - - if (!pending_shape_update_list.in_list()) { - PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); - } -} - -void CollisionObject2DSW::set_shape_transform(int p_index, const Transform2D &p_transform) { - ERR_FAIL_INDEX(p_index, shapes.size()); - - shapes.write[p_index].xform = p_transform; - shapes.write[p_index].xform_inv = p_transform.affine_inverse(); - - if (!pending_shape_update_list.in_list()) { - PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); - } -} - -void CollisionObject2DSW::set_shape_disabled(int p_idx, bool p_disabled) { - ERR_FAIL_INDEX(p_idx, shapes.size()); - - CollisionObject2DSW::Shape &shape = shapes.write[p_idx]; - if (shape.disabled == p_disabled) { - return; - } - - shape.disabled = p_disabled; - - if (!space) { - return; - } - - if (p_disabled && shape.bpid != 0) { - space->get_broadphase()->remove(shape.bpid); - shape.bpid = 0; - if (!pending_shape_update_list.in_list()) { - PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); - } - } else if (!p_disabled && shape.bpid == 0) { - if (!pending_shape_update_list.in_list()) { - PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); - } - } -} - -void CollisionObject2DSW::remove_shape(Shape2DSW *p_shape) { - //remove a shape, all the times it appears - for (int i = 0; i < shapes.size(); i++) { - if (shapes[i].shape == p_shape) { - remove_shape(i); - i--; - } - } -} - -void CollisionObject2DSW::remove_shape(int p_index) { - //remove anything from shape to be erased to end, so subindices don't change - ERR_FAIL_INDEX(p_index, shapes.size()); - for (int i = p_index; i < shapes.size(); i++) { - if (shapes[i].bpid == 0) { - continue; - } - //should never get here with a null owner - space->get_broadphase()->remove(shapes[i].bpid); - shapes.write[i].bpid = 0; - } - shapes[p_index].shape->remove_owner(this); - shapes.remove(p_index); - - if (!pending_shape_update_list.in_list()) { - PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); - } - // _update_shapes(); - // _shapes_changed(); -} - -void CollisionObject2DSW::_set_static(bool p_static) { - if (_static == p_static) { - return; - } - _static = p_static; - - if (!space) { - return; - } - for (int i = 0; i < get_shape_count(); i++) { - const Shape &s = shapes[i]; - if (s.bpid > 0) { - space->get_broadphase()->set_static(s.bpid, _static); - } - } -} - -void CollisionObject2DSW::_unregister_shapes() { - for (int i = 0; i < shapes.size(); i++) { - Shape &s = shapes.write[i]; - if (s.bpid > 0) { - space->get_broadphase()->remove(s.bpid); - s.bpid = 0; - } - } -} - -void CollisionObject2DSW::_update_shapes() { - if (!space) { - return; - } - - for (int i = 0; i < shapes.size(); i++) { - Shape &s = shapes.write[i]; - if (s.disabled) { - continue; - } - - //not quite correct, should compute the next matrix.. - Rect2 shape_aabb = s.shape->get_aabb(); - Transform2D xform = transform * s.xform; - shape_aabb = xform.xform(shape_aabb); - shape_aabb.grow_by((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05); - s.aabb_cache = shape_aabb; - - if (s.bpid == 0) { - s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); - space->get_broadphase()->set_static(s.bpid, _static); - } - - space->get_broadphase()->move(s.bpid, shape_aabb); - } -} - -void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) { - if (!space) { - return; - } - - for (int i = 0; i < shapes.size(); i++) { - Shape &s = shapes.write[i]; - if (s.disabled) { - continue; - } - - //not quite correct, should compute the next matrix.. - Rect2 shape_aabb = s.shape->get_aabb(); - Transform2D xform = transform * s.xform; - shape_aabb = xform.xform(shape_aabb); - shape_aabb = shape_aabb.merge(Rect2(shape_aabb.position + p_motion, shape_aabb.size)); //use motion - s.aabb_cache = shape_aabb; - - if (s.bpid == 0) { - s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); - space->get_broadphase()->set_static(s.bpid, _static); - } - - space->get_broadphase()->move(s.bpid, shape_aabb); - } -} - -void CollisionObject2DSW::_set_space(Space2DSW *p_space) { - if (space) { - space->remove_object(this); - - for (int i = 0; i < shapes.size(); i++) { - Shape &s = shapes.write[i]; - if (s.bpid) { - space->get_broadphase()->remove(s.bpid); - s.bpid = 0; - } - } - } - - space = p_space; - - if (space) { - space->add_object(this); - _update_shapes(); - } -} - -void CollisionObject2DSW::_shape_changed() { - _update_shapes(); - _shapes_changed(); -} - -CollisionObject2DSW::CollisionObject2DSW(Type p_type) : - pending_shape_update_list(this) { - type = p_type; -} diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h deleted file mode 100644 index ca258a906a..0000000000 --- a/servers/physics_2d/collision_object_2d_sw.h +++ /dev/null @@ -1,189 +0,0 @@ -/*************************************************************************/ -/* collision_object_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 COLLISION_OBJECT_2D_SW_H -#define COLLISION_OBJECT_2D_SW_H - -#include "broad_phase_2d_sw.h" -#include "core/templates/self_list.h" -#include "servers/physics_server_2d.h" -#include "shape_2d_sw.h" - -class Space2DSW; - -class CollisionObject2DSW : public ShapeOwner2DSW { -public: - enum Type { - TYPE_AREA, - TYPE_BODY - }; - -private: - Type type; - RID self; - ObjectID instance_id; - ObjectID canvas_instance_id; - bool pickable = true; - - struct Shape { - Transform2D xform; - Transform2D xform_inv; - BroadPhase2DSW::ID bpid = 0; - Rect2 aabb_cache; //for rayqueries - Shape2DSW *shape = nullptr; - bool disabled = false; - bool one_way_collision = false; - real_t one_way_collision_margin = 0.0; - }; - - Vector shapes; - Space2DSW *space = nullptr; - Transform2D transform; - Transform2D inv_transform; - uint32_t collision_mask = 1; - uint32_t collision_layer = 1; - bool _static = true; - - SelfList pending_shape_update_list; - - void _update_shapes(); - -protected: - void _update_shapes_with_motion(const Vector2 &p_motion); - void _unregister_shapes(); - - _FORCE_INLINE_ void _set_transform(const Transform2D &p_transform, bool p_update_shapes = true) { - transform = p_transform; - if (p_update_shapes) { - _update_shapes(); - } - } - _FORCE_INLINE_ void _set_inv_transform(const Transform2D &p_transform) { inv_transform = p_transform; } - void _set_static(bool p_static); - - virtual void _shapes_changed() = 0; - void _set_space(Space2DSW *p_space); - - CollisionObject2DSW(Type p_type); - -public: - _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } - _FORCE_INLINE_ RID get_self() const { return self; } - - _FORCE_INLINE_ void set_instance_id(const ObjectID &p_instance_id) { instance_id = p_instance_id; } - _FORCE_INLINE_ ObjectID get_instance_id() const { return instance_id; } - - _FORCE_INLINE_ void set_canvas_instance_id(const ObjectID &p_canvas_instance_id) { canvas_instance_id = p_canvas_instance_id; } - _FORCE_INLINE_ ObjectID get_canvas_instance_id() const { return canvas_instance_id; } - - void _shape_changed(); - - _FORCE_INLINE_ Type get_type() const { return type; } - void add_shape(Shape2DSW *p_shape, const Transform2D &p_transform = Transform2D(), bool p_disabled = false); - void set_shape(int p_index, Shape2DSW *p_shape); - void set_shape_transform(int p_index, const Transform2D &p_transform); - - _FORCE_INLINE_ int get_shape_count() const { return shapes.size(); } - _FORCE_INLINE_ Shape2DSW *get_shape(int p_index) const { - CRASH_BAD_INDEX(p_index, shapes.size()); - return shapes[p_index].shape; - } - _FORCE_INLINE_ const Transform2D &get_shape_transform(int p_index) const { - CRASH_BAD_INDEX(p_index, shapes.size()); - return shapes[p_index].xform; - } - _FORCE_INLINE_ const Transform2D &get_shape_inv_transform(int p_index) const { - CRASH_BAD_INDEX(p_index, shapes.size()); - return shapes[p_index].xform_inv; - } - _FORCE_INLINE_ const Rect2 &get_shape_aabb(int p_index) const { - CRASH_BAD_INDEX(p_index, shapes.size()); - return shapes[p_index].aabb_cache; - } - - _FORCE_INLINE_ const Transform2D &get_transform() const { return transform; } - _FORCE_INLINE_ const Transform2D &get_inv_transform() const { return inv_transform; } - _FORCE_INLINE_ Space2DSW *get_space() const { return space; } - - void set_shape_disabled(int p_idx, bool p_disabled); - _FORCE_INLINE_ bool is_shape_disabled(int p_idx) const { - ERR_FAIL_INDEX_V(p_idx, shapes.size(), false); - return shapes[p_idx].disabled; - } - - _FORCE_INLINE_ void set_shape_as_one_way_collision(int p_idx, bool p_one_way_collision, real_t p_margin) { - CRASH_BAD_INDEX(p_idx, shapes.size()); - shapes.write[p_idx].one_way_collision = p_one_way_collision; - shapes.write[p_idx].one_way_collision_margin = p_margin; - } - _FORCE_INLINE_ bool is_shape_set_as_one_way_collision(int p_idx) const { - CRASH_BAD_INDEX(p_idx, shapes.size()); - return shapes[p_idx].one_way_collision; - } - - _FORCE_INLINE_ real_t get_shape_one_way_collision_margin(int p_idx) const { - CRASH_BAD_INDEX(p_idx, shapes.size()); - return shapes[p_idx].one_way_collision_margin; - } - - void set_collision_mask(uint32_t p_mask) { - collision_mask = p_mask; - _shape_changed(); - } - _FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; } - - void set_collision_layer(uint32_t p_layer) { - collision_layer = p_layer; - _shape_changed(); - } - _FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; } - - void remove_shape(Shape2DSW *p_shape); - void remove_shape(int p_index); - - virtual void set_space(Space2DSW *p_space) = 0; - - _FORCE_INLINE_ bool is_static() const { return _static; } - - void set_pickable(bool p_pickable) { pickable = p_pickable; } - _FORCE_INLINE_ bool is_pickable() const { return pickable; } - - _FORCE_INLINE_ bool collides_with(CollisionObject2DSW *p_other) const { - return p_other->collision_layer & collision_mask; - } - - _FORCE_INLINE_ bool interacts_with(CollisionObject2DSW *p_other) const { - return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask; - } - - virtual ~CollisionObject2DSW() {} -}; - -#endif // COLLISION_OBJECT_2D_SW_H diff --git a/servers/physics_2d/collision_solver_2d_sat.cpp b/servers/physics_2d/collision_solver_2d_sat.cpp deleted file mode 100644 index 2e67cc6520..0000000000 --- a/servers/physics_2d/collision_solver_2d_sat.cpp +++ /dev/null @@ -1,1408 +0,0 @@ -/*************************************************************************/ -/* collision_solver_2d_sat.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "collision_solver_2d_sat.h" - -#include "core/math/geometry_2d.h" - -struct _CollectorCallback2D { - CollisionSolver2DSW::CallbackResult callback; - void *userdata = nullptr; - bool swap = false; - bool collided = false; - Vector2 normal; - Vector2 *sep_axis = nullptr; - - _FORCE_INLINE_ void call(const Vector2 &p_point_A, const Vector2 &p_point_B) { - /* - if (normal.dot(p_point_A) >= normal.dot(p_point_B)) - return; - */ - if (swap) { - callback(p_point_B, p_point_A, userdata); - } else { - callback(p_point_A, p_point_B, userdata); - } - } -}; - -typedef void (*GenerateContactsFunc)(const Vector2 *, int, const Vector2 *, int, _CollectorCallback2D *); - -_FORCE_INLINE_ static void _generate_contacts_point_point(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { -#ifdef DEBUG_ENABLED - ERR_FAIL_COND(p_point_count_A != 1); - ERR_FAIL_COND(p_point_count_B != 1); -#endif - - p_collector->call(*p_points_A, *p_points_B); -} - -_FORCE_INLINE_ static void _generate_contacts_point_edge(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { -#ifdef DEBUG_ENABLED - ERR_FAIL_COND(p_point_count_A != 1); - ERR_FAIL_COND(p_point_count_B != 2); -#endif - - Vector2 closest_B = Geometry2D::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B); - p_collector->call(*p_points_A, closest_B); -} - -struct _generate_contacts_Pair { - bool a = false; - int idx = 0; - real_t d = 0.0; - _FORCE_INLINE_ bool operator<(const _generate_contacts_Pair &l) const { return d < l.d; } -}; - -_FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { -#ifdef DEBUG_ENABLED - ERR_FAIL_COND(p_point_count_A != 2); - ERR_FAIL_COND(p_point_count_B != 2); // circle is actually a 4x3 matrix -#endif - - Vector2 n = p_collector->normal; - Vector2 t = n.orthogonal(); - real_t dA = n.dot(p_points_A[0]); - real_t dB = n.dot(p_points_B[0]); - - _generate_contacts_Pair dvec[4]; - - dvec[0].d = t.dot(p_points_A[0]); - dvec[0].a = true; - dvec[0].idx = 0; - dvec[1].d = t.dot(p_points_A[1]); - dvec[1].a = true; - dvec[1].idx = 1; - dvec[2].d = t.dot(p_points_B[0]); - dvec[2].a = false; - dvec[2].idx = 0; - dvec[3].d = t.dot(p_points_B[1]); - dvec[3].a = false; - dvec[3].idx = 1; - - SortArray<_generate_contacts_Pair> sa; - sa.sort(dvec, 4); - - for (int i = 1; i <= 2; i++) { - if (dvec[i].a) { - Vector2 a = p_points_A[dvec[i].idx]; - Vector2 b = n.plane_project(dB, a); - if (n.dot(a) > n.dot(b) - CMP_EPSILON) { - continue; - } - p_collector->call(a, b); - } else { - Vector2 b = p_points_B[dvec[i].idx]; - Vector2 a = n.plane_project(dA, b); - if (n.dot(a) > n.dot(b) - CMP_EPSILON) { - continue; - } - p_collector->call(a, b); - } - } -} - -static void _generate_contacts_from_supports(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { -#ifdef DEBUG_ENABLED - ERR_FAIL_COND(p_point_count_A < 1); - ERR_FAIL_COND(p_point_count_B < 1); -#endif - - static const GenerateContactsFunc generate_contacts_func_table[2][2] = { - { - _generate_contacts_point_point, - _generate_contacts_point_edge, - }, - { - nullptr, - _generate_contacts_edge_edge, - } - }; - - int pointcount_B = 0; - int pointcount_A = 0; - const Vector2 *points_A = nullptr; - const Vector2 *points_B = nullptr; - - if (p_point_count_A > p_point_count_B) { - //swap - p_collector->swap = !p_collector->swap; - p_collector->normal = -p_collector->normal; - - pointcount_B = p_point_count_A; - pointcount_A = p_point_count_B; - points_A = p_points_B; - points_B = p_points_A; - } else { - pointcount_B = p_point_count_B; - pointcount_A = p_point_count_A; - points_A = p_points_A; - points_B = p_points_B; - } - - int version_A = (pointcount_A > 2 ? 2 : pointcount_A) - 1; - int version_B = (pointcount_B > 2 ? 2 : pointcount_B) - 1; - - GenerateContactsFunc contacts_func = generate_contacts_func_table[version_A][version_B]; - ERR_FAIL_COND(!contacts_func); - contacts_func(points_A, pointcount_A, points_B, pointcount_B, p_collector); -} - -template -class SeparatorAxisTest2D { - const ShapeA *shape_A = nullptr; - const ShapeB *shape_B = nullptr; - const Transform2D *transform_A = nullptr; - const Transform2D *transform_B = nullptr; - real_t best_depth = 1e15; - Vector2 best_axis; -#ifdef DEBUG_ENABLED - int best_axis_count = 0; - int best_axis_index = -1; -#endif - Vector2 motion_A; - Vector2 motion_B; - real_t margin_A = 0.0; - real_t margin_B = 0.0; - _CollectorCallback2D *callback; - -public: - _FORCE_INLINE_ bool test_previous_axis() { - if (callback && callback->sep_axis && *callback->sep_axis != Vector2()) { - return test_axis(*callback->sep_axis); - } else { -#ifdef DEBUG_ENABLED - best_axis_count++; -#endif - } - return true; - } - - _FORCE_INLINE_ bool test_cast() { - if (castA) { - Vector2 na = motion_A.normalized(); - if (!test_axis(na)) { - return false; - } - if (!test_axis(na.orthogonal())) { - return false; - } - } - - if (castB) { - Vector2 nb = motion_B.normalized(); - if (!test_axis(nb)) { - return false; - } - if (!test_axis(nb.orthogonal())) { - return false; - } - } - - return true; - } - - _FORCE_INLINE_ bool test_axis(const Vector2 &p_axis) { - Vector2 axis = p_axis; - - if (Math::is_zero_approx(axis.x) && - Math::is_zero_approx(axis.y)) { - // strange case, try an upwards separator - axis = Vector2(0.0, 1.0); - } - - real_t min_A, max_A, min_B, max_B; - - if (castA) { - shape_A->project_range_cast(motion_A, axis, *transform_A, min_A, max_A); - } else { - shape_A->project_range(axis, *transform_A, min_A, max_A); - } - - if (castB) { - shape_B->project_range_cast(motion_B, axis, *transform_B, min_B, max_B); - } else { - shape_B->project_range(axis, *transform_B, min_B, max_B); - } - - if (withMargin) { - min_A -= margin_A; - max_A += margin_A; - min_B -= margin_B; - max_B += margin_B; - } - - min_B -= (max_A - min_A) * 0.5; - max_B += (max_A - min_A) * 0.5; - - real_t dmin = min_B - (min_A + max_A) * 0.5; - real_t dmax = max_B - (min_A + max_A) * 0.5; - - if (dmin > 0.0 || dmax < 0.0) { - if (callback && callback->sep_axis) { - *callback->sep_axis = axis; - } -#ifdef DEBUG_ENABLED - best_axis_count++; -#endif - - return false; // doesn't contain 0 - } - - //use the smallest depth - - dmin = Math::abs(dmin); - - if (dmax < dmin) { - if (dmax < best_depth) { - best_depth = dmax; - best_axis = axis; -#ifdef DEBUG_ENABLED - best_axis_index = best_axis_count; -#endif - } - } else { - if (dmin < best_depth) { - best_depth = dmin; - best_axis = -axis; // keep it as A axis -#ifdef DEBUG_ENABLED - best_axis_index = best_axis_count; -#endif - } - } - -#ifdef DEBUG_ENABLED - best_axis_count++; -#endif - - return true; - } - - _FORCE_INLINE_ void generate_contacts() { - // nothing to do, don't generate - if (best_axis == Vector2(0.0, 0.0)) { - return; - } - - if (callback) { - callback->collided = true; - - if (!callback->callback) { - return; //only collide, no callback - } - } - static const int max_supports = 2; - - Vector2 supports_A[max_supports]; - int support_count_A; - if (castA) { - shape_A->get_supports_transformed_cast(motion_A, -best_axis, *transform_A, supports_A, support_count_A); - } else { - shape_A->get_supports(transform_A->basis_xform_inv(-best_axis).normalized(), supports_A, support_count_A); - for (int i = 0; i < support_count_A; i++) { - supports_A[i] = transform_A->xform(supports_A[i]); - } - } - - if (withMargin) { - for (int i = 0; i < support_count_A; i++) { - supports_A[i] += -best_axis * margin_A; - } - } - - Vector2 supports_B[max_supports]; - int support_count_B; - if (castB) { - shape_B->get_supports_transformed_cast(motion_B, best_axis, *transform_B, supports_B, support_count_B); - } else { - shape_B->get_supports(transform_B->basis_xform_inv(best_axis).normalized(), supports_B, support_count_B); - for (int i = 0; i < support_count_B; i++) { - supports_B[i] = transform_B->xform(supports_B[i]); - } - } - - if (withMargin) { - for (int i = 0; i < support_count_B; i++) { - supports_B[i] += best_axis * margin_B; - } - } - if (callback) { - callback->normal = best_axis; - _generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback); - - if (callback->sep_axis && *callback->sep_axis != Vector2()) { - *callback->sep_axis = Vector2(); //invalidate previous axis (no test) - } - } - } - - _FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A, const Transform2D &p_transform_a, const ShapeB *p_shape_B, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_A = Vector2(), const Vector2 &p_motion_B = Vector2(), real_t p_margin_A = 0, real_t p_margin_B = 0) { - margin_A = p_margin_A; - margin_B = p_margin_B; - shape_A = p_shape_A; - shape_B = p_shape_B; - transform_A = &p_transform_a; - transform_B = &p_transform_b; - motion_A = p_motion_A; - motion_B = p_motion_B; - callback = p_collector; - } -}; - -/****** SAT TESTS *******/ - -#define TEST_POINT(m_a, m_b) \ - ((!separator.test_axis(((m_a) - (m_b)).normalized())) || \ - (castA && !separator.test_axis(((m_a) + p_motion_a - (m_b)).normalized())) || \ - (castB && !separator.test_axis(((m_a) - ((m_b) + p_motion_b)).normalized())) || \ - (castA && castB && !separator.test_axis(((m_a) + p_motion_a - ((m_b) + p_motion_b)).normalized()))) - -typedef void (*CollisionFunc)(const Shape2DSW *, const Transform2D &, const Shape2DSW *, const Transform2D &, _CollectorCallback2D *p_collector, const Vector2 &, const Vector2 &, real_t, real_t); - -template -static void _collision_segment_segment(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const SegmentShape2DSW *segment_A = static_cast(p_a); - const SegmentShape2DSW *segment_B = static_cast(p_b); - - SeparatorAxisTest2D separator(segment_A, p_transform_a, segment_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - //this collision is kind of pointless - - if (!separator.test_cast()) { - return; - } - - if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { - return; - } - if (!separator.test_axis(segment_B->get_xformed_normal(p_transform_b))) { - return; - } - - if (withMargin) { - //points grow to circles - - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_a()))) { - return; - } - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_b()))) { - return; - } - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_a()))) { - return; - } - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_b()))) { - return; - } - } - - separator.generate_contacts(); -} - -template -static void _collision_segment_circle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const SegmentShape2DSW *segment_A = static_cast(p_a); - const CircleShape2DSW *circle_B = static_cast(p_b); - - SeparatorAxisTest2D separator(segment_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - //segment normal - if (!separator.test_axis( - (p_transform_a.xform(segment_A->get_b()) - p_transform_a.xform(segment_A->get_a())).normalized().orthogonal())) { - return; - } - - //endpoint a vs circle - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.get_origin())) { - return; - } - //endpoint b vs circle - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.get_origin())) { - return; - } - - separator.generate_contacts(); -} - -template -static void _collision_segment_rectangle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const SegmentShape2DSW *segment_A = static_cast(p_a); - const RectangleShape2DSW *rectangle_B = static_cast(p_b); - - SeparatorAxisTest2D separator(segment_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { - return; - } - - if (!separator.test_axis(p_transform_b.elements[0].normalized())) { - return; - } - - if (!separator.test_axis(p_transform_b.elements[1].normalized())) { - return; - } - - if (withMargin) { - Transform2D inv = p_transform_b.affine_inverse(); - - Vector2 a = p_transform_a.xform(segment_A->get_a()); - Vector2 b = p_transform_a.xform(segment_A->get_b()); - - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a))) { - return; - } - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b))) { - return; - } - - if (castA) { - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a + p_motion_a))) { - return; - } - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b + p_motion_a))) { - return; - } - } - - if (castB) { - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b))) { - return; - } - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b))) { - return; - } - } - - if (castA && castB) { - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b + p_motion_a))) { - return; - } - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b + p_motion_a))) { - return; - } - } - } - - separator.generate_contacts(); -} - -template -static void _collision_segment_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const SegmentShape2DSW *segment_A = static_cast(p_a); - const CapsuleShape2DSW *capsule_B = static_cast(p_b); - - SeparatorAxisTest2D separator(segment_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { - return; - } - - if (!separator.test_axis(p_transform_b.elements[0].normalized())) { - return; - } - - real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); - - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) { - return; - } - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) { - return; - } - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) { - return; - } - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) { - return; - } - - separator.generate_contacts(); -} - -template -static void _collision_segment_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const SegmentShape2DSW *segment_A = static_cast(p_a); - const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - - SeparatorAxisTest2D separator(segment_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { - return; - } - - for (int i = 0; i < convex_B->get_point_count(); i++) { - if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { - return; - } - - if (withMargin) { - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(convex_B->get_point(i)))) { - return; - } - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(convex_B->get_point(i)))) { - return; - } - } - } - - separator.generate_contacts(); -} - -///////// - -template -static void _collision_circle_circle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const CircleShape2DSW *circle_A = static_cast(p_a); - const CircleShape2DSW *circle_B = static_cast(p_b); - - SeparatorAxisTest2D separator(circle_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.get_origin())) { - return; - } - - separator.generate_contacts(); -} - -template -static void _collision_circle_rectangle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const CircleShape2DSW *circle_A = static_cast(p_a); - const RectangleShape2DSW *rectangle_B = static_cast(p_b); - - SeparatorAxisTest2D separator(circle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - const Vector2 &sphere = p_transform_a.elements[2]; - const Vector2 *axis = &p_transform_b.elements[0]; - //const Vector2& half_extents = rectangle_B->get_half_extents(); - - if (!separator.test_axis(axis[0].normalized())) { - return; - } - - if (!separator.test_axis(axis[1].normalized())) { - return; - } - - Transform2D binv = p_transform_b.affine_inverse(); - { - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphere))) { - return; - } - } - - if (castA) { - Vector2 sphereofs = sphere + p_motion_a; - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) { - return; - } - } - - if (castB) { - Vector2 sphereofs = sphere - p_motion_b; - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) { - return; - } - } - - if (castA && castB) { - Vector2 sphereofs = sphere - p_motion_b + p_motion_a; - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) { - return; - } - } - - separator.generate_contacts(); -} - -template -static void _collision_circle_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const CircleShape2DSW *circle_A = static_cast(p_a); - const CapsuleShape2DSW *capsule_B = static_cast(p_b); - - SeparatorAxisTest2D separator(circle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - //capsule axis - if (!separator.test_axis(p_transform_b.elements[0].normalized())) { - return; - } - - real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); - - //capsule endpoints - if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) { - return; - } - if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) { - return; - } - - separator.generate_contacts(); -} - -template -static void _collision_circle_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const CircleShape2DSW *circle_A = static_cast(p_a); - const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - - SeparatorAxisTest2D separator(circle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - //poly faces and poly points vs circle - for (int i = 0; i < convex_B->get_point_count(); i++) { - if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.xform(convex_B->get_point(i)))) { - return; - } - - if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { - return; - } - } - - separator.generate_contacts(); -} - -///////// - -template -static void _collision_rectangle_rectangle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const RectangleShape2DSW *rectangle_A = static_cast(p_a); - const RectangleShape2DSW *rectangle_B = static_cast(p_b); - - SeparatorAxisTest2D separator(rectangle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - //box faces A - if (!separator.test_axis(p_transform_a.elements[0].normalized())) { - return; - } - - if (!separator.test_axis(p_transform_a.elements[1].normalized())) { - return; - } - - //box faces B - if (!separator.test_axis(p_transform_b.elements[0].normalized())) { - return; - } - - if (!separator.test_axis(p_transform_b.elements[1].normalized())) { - return; - } - - if (withMargin) { - Transform2D invA = p_transform_a.affine_inverse(); - Transform2D invB = p_transform_b.affine_inverse(); - - if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, p_transform_b, invB))) { - return; - } - - if (castA || castB) { - Transform2D aofs = p_transform_a; - aofs.elements[2] += p_motion_a; - - Transform2D bofs = p_transform_b; - bofs.elements[2] += p_motion_b; - - Transform2D aofsinv = aofs.affine_inverse(); - Transform2D bofsinv = bofs.affine_inverse(); - - if (castA) { - if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, p_transform_b, invB))) { - return; - } - } - - if (castB) { - if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, bofs, bofsinv))) { - return; - } - } - - if (castA && castB) { - if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, bofs, bofsinv))) { - return; - } - } - } - } - - separator.generate_contacts(); -} - -template -static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const RectangleShape2DSW *rectangle_A = static_cast(p_a); - const CapsuleShape2DSW *capsule_B = static_cast(p_b); - - SeparatorAxisTest2D separator(rectangle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - //box faces - if (!separator.test_axis(p_transform_a.elements[0].normalized())) { - return; - } - - if (!separator.test_axis(p_transform_a.elements[1].normalized())) { - return; - } - - //capsule axis - if (!separator.test_axis(p_transform_b.elements[0].normalized())) { - return; - } - - //box endpoints to capsule circles - - Transform2D boxinv = p_transform_a.affine_inverse(); - - real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); - - for (int i = 0; i < 2; i++) { - { - Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; - - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { - return; - } - } - - if (castA) { - Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; - capsule_endpoint -= p_motion_a; - - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { - return; - } - } - - if (castB) { - Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; - capsule_endpoint += p_motion_b; - - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { - return; - } - } - - if (castA && castB) { - Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; - capsule_endpoint -= p_motion_a; - capsule_endpoint += p_motion_b; - - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { - return; - } - } - - capsule_dir *= -1.0; - } - - separator.generate_contacts(); -} - -template -static void _collision_rectangle_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const RectangleShape2DSW *rectangle_A = static_cast(p_a); - const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - - SeparatorAxisTest2D separator(rectangle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - //box faces - if (!separator.test_axis(p_transform_a.elements[0].normalized())) { - return; - } - - if (!separator.test_axis(p_transform_a.elements[1].normalized())) { - return; - } - - //convex faces - Transform2D boxinv; - if (withMargin) { - boxinv = p_transform_a.affine_inverse(); - } - for (int i = 0; i < convex_B->get_point_count(); i++) { - if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { - return; - } - - if (withMargin) { - //all points vs all points need to be tested if margin exist - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i))))) { - return; - } - if (castA) { - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) - p_motion_a))) { - return; - } - } - if (castB) { - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b))) { - return; - } - } - if (castA && castB) { - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b - p_motion_a))) { - return; - } - } - } - } - - separator.generate_contacts(); -} - -///////// - -template -static void _collision_capsule_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const CapsuleShape2DSW *capsule_A = static_cast(p_a); - const CapsuleShape2DSW *capsule_B = static_cast(p_b); - - SeparatorAxisTest2D separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - //capsule axis - - if (!separator.test_axis(p_transform_b.elements[0].normalized())) { - return; - } - - if (!separator.test_axis(p_transform_a.elements[0].normalized())) { - return; - } - - //capsule endpoints - - real_t capsule_dir_A = capsule_A->get_height() * 0.5 - capsule_A->get_radius(); - for (int i = 0; i < 2; i++) { - Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_dir_A; - - real_t capsule_dir_B = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); - for (int j = 0; j < 2; j++) { - Vector2 capsule_endpoint_B = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir_B; - - if (TEST_POINT(capsule_endpoint_A, capsule_endpoint_B)) { - return; - } - - capsule_dir_B *= -1.0; - } - - capsule_dir_A *= -1.0; - } - - separator.generate_contacts(); -} - -template -static void _collision_capsule_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const CapsuleShape2DSW *capsule_A = static_cast(p_a); - const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - - SeparatorAxisTest2D separator(capsule_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - //capsule axis - - if (!separator.test_axis(p_transform_a.elements[0].normalized())) { - return; - } - - //poly vs capsule - for (int i = 0; i < convex_B->get_point_count(); i++) { - Vector2 cpoint = p_transform_b.xform(convex_B->get_point(i)); - - real_t capsule_dir = capsule_A->get_height() * 0.5 - capsule_A->get_radius(); - for (int j = 0; j < 2; j++) { - Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_dir; - - if (TEST_POINT(capsule_endpoint_A, cpoint)) { - return; - } - - capsule_dir *= -1.0; - } - - if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { - return; - } - } - - separator.generate_contacts(); -} - -///////// - -template -static void _collision_convex_polygon_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const ConvexPolygonShape2DSW *convex_A = static_cast(p_a); - const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - - SeparatorAxisTest2D separator(convex_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_cast()) { - return; - } - - for (int i = 0; i < convex_A->get_point_count(); i++) { - if (!separator.test_axis(convex_A->get_xformed_segment_normal(p_transform_a, i))) { - return; - } - } - - for (int i = 0; i < convex_B->get_point_count(); i++) { - if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { - return; - } - } - - if (withMargin) { - for (int i = 0; i < convex_A->get_point_count(); i++) { - for (int j = 0; j < convex_B->get_point_count(); j++) { - if (TEST_POINT(p_transform_a.xform(convex_A->get_point(i)), p_transform_b.xform(convex_B->get_point(j)))) { - return; - } - } - } - } - - separator.generate_contacts(); -} - -//////// - -bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector2 *sep_axis, real_t p_margin_A, real_t p_margin_B) { - PhysicsServer2D::ShapeType type_A = p_shape_A->get_type(); - - ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_WORLD_BOUNDARY, false); - ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_SEPARATION_RAY, false); - ERR_FAIL_COND_V(p_shape_A->is_concave(), false); - - PhysicsServer2D::ShapeType type_B = p_shape_B->get_type(); - - ERR_FAIL_COND_V(type_B == PhysicsServer2D::SHAPE_WORLD_BOUNDARY, false); - ERR_FAIL_COND_V(type_B == PhysicsServer2D::SHAPE_SEPARATION_RAY, false); - ERR_FAIL_COND_V(p_shape_B->is_concave(), false); - - static const CollisionFunc collision_table[5][5] = { - { _collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon }, - { nullptr, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon }, - { nullptr, - nullptr, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon }, - { nullptr, - nullptr, - nullptr, - _collision_capsule_capsule, - _collision_capsule_convex_polygon }, - { nullptr, - nullptr, - nullptr, - nullptr, - _collision_convex_polygon_convex_polygon } - - }; - - static const CollisionFunc collision_table_castA[5][5] = { - { _collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon }, - { nullptr, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon }, - { nullptr, - nullptr, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon }, - { nullptr, - nullptr, - nullptr, - _collision_capsule_capsule, - _collision_capsule_convex_polygon }, - { nullptr, - nullptr, - nullptr, - nullptr, - _collision_convex_polygon_convex_polygon } - - }; - - static const CollisionFunc collision_table_castB[5][5] = { - { _collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon }, - { nullptr, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon }, - { nullptr, - nullptr, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon }, - { nullptr, - nullptr, - nullptr, - _collision_capsule_capsule, - _collision_capsule_convex_polygon }, - { nullptr, - nullptr, - nullptr, - nullptr, - _collision_convex_polygon_convex_polygon } - - }; - - static const CollisionFunc collision_table_castA_castB[5][5] = { - { _collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon }, - { nullptr, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon }, - { nullptr, - nullptr, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon }, - { nullptr, - nullptr, - nullptr, - _collision_capsule_capsule, - _collision_capsule_convex_polygon }, - { nullptr, - nullptr, - nullptr, - nullptr, - _collision_convex_polygon_convex_polygon } - - }; - - static const CollisionFunc collision_table_margin[5][5] = { - { _collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon }, - { nullptr, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon }, - { nullptr, - nullptr, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon }, - { nullptr, - nullptr, - nullptr, - _collision_capsule_capsule, - _collision_capsule_convex_polygon }, - { nullptr, - nullptr, - nullptr, - nullptr, - _collision_convex_polygon_convex_polygon } - - }; - - static const CollisionFunc collision_table_castA_margin[5][5] = { - { _collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon }, - { nullptr, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon }, - { nullptr, - nullptr, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon }, - { nullptr, - nullptr, - nullptr, - _collision_capsule_capsule, - _collision_capsule_convex_polygon }, - { nullptr, - nullptr, - nullptr, - nullptr, - _collision_convex_polygon_convex_polygon } - - }; - - static const CollisionFunc collision_table_castB_margin[5][5] = { - { _collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon }, - { nullptr, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon }, - { nullptr, - nullptr, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon }, - { nullptr, - nullptr, - nullptr, - _collision_capsule_capsule, - _collision_capsule_convex_polygon }, - { nullptr, - nullptr, - nullptr, - nullptr, - _collision_convex_polygon_convex_polygon } - - }; - - static const CollisionFunc collision_table_castA_castB_margin[5][5] = { - { _collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon }, - { nullptr, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon }, - { nullptr, - nullptr, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon }, - { nullptr, - nullptr, - nullptr, - _collision_capsule_capsule, - _collision_capsule_convex_polygon }, - { nullptr, - nullptr, - nullptr, - nullptr, - _collision_convex_polygon_convex_polygon } - - }; - - _CollectorCallback2D callback; - callback.callback = p_result_callback; - callback.swap = p_swap; - callback.userdata = p_userdata; - callback.collided = false; - callback.sep_axis = sep_axis; - - const Shape2DSW *A = p_shape_A; - const Shape2DSW *B = p_shape_B; - const Transform2D *transform_A = &p_transform_A; - const Transform2D *transform_B = &p_transform_B; - const Vector2 *motion_A = &p_motion_A; - const Vector2 *motion_B = &p_motion_B; - real_t margin_A = p_margin_A, margin_B = p_margin_B; - - if (type_A > type_B) { - SWAP(A, B); - SWAP(transform_A, transform_B); - SWAP(type_A, type_B); - SWAP(motion_A, motion_B); - SWAP(margin_A, margin_B); - callback.swap = !callback.swap; - } - - CollisionFunc collision_func; - - if (p_margin_A || p_margin_B) { - if (*motion_A == Vector2() && *motion_B == Vector2()) { - collision_func = collision_table_margin[type_A - 2][type_B - 2]; - } else if (*motion_A != Vector2() && *motion_B == Vector2()) { - collision_func = collision_table_castA_margin[type_A - 2][type_B - 2]; - } else if (*motion_A == Vector2() && *motion_B != Vector2()) { - collision_func = collision_table_castB_margin[type_A - 2][type_B - 2]; - } else { - collision_func = collision_table_castA_castB_margin[type_A - 2][type_B - 2]; - } - } else { - if (*motion_A == Vector2() && *motion_B == Vector2()) { - collision_func = collision_table[type_A - 2][type_B - 2]; - } else if (*motion_A != Vector2() && *motion_B == Vector2()) { - collision_func = collision_table_castA[type_A - 2][type_B - 2]; - } else if (*motion_A == Vector2() && *motion_B != Vector2()) { - collision_func = collision_table_castB[type_A - 2][type_B - 2]; - } else { - collision_func = collision_table_castA_castB[type_A - 2][type_B - 2]; - } - } - - ERR_FAIL_COND_V(!collision_func, false); - - collision_func(A, *transform_A, B, *transform_B, &callback, *motion_A, *motion_B, margin_A, margin_B); - - return callback.collided; -} diff --git a/servers/physics_2d/collision_solver_2d_sat.h b/servers/physics_2d/collision_solver_2d_sat.h deleted file mode 100644 index 49cc5176f9..0000000000 --- a/servers/physics_2d/collision_solver_2d_sat.h +++ /dev/null @@ -1,38 +0,0 @@ -/*************************************************************************/ -/* collision_solver_2d_sat.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 COLLISION_SOLVER_2D_SAT_H -#define COLLISION_SOLVER_2D_SAT_H - -#include "collision_solver_2d_sw.h" - -bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector2 *sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); - -#endif // COLLISION_SOLVER_2D_SAT_H diff --git a/servers/physics_2d/collision_solver_2d_sw.cpp b/servers/physics_2d/collision_solver_2d_sw.cpp deleted file mode 100644 index 527bb1b0b2..0000000000 --- a/servers/physics_2d/collision_solver_2d_sw.cpp +++ /dev/null @@ -1,264 +0,0 @@ -/*************************************************************************/ -/* collision_solver_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "collision_solver_2d_sw.h" -#include "collision_solver_2d_sat.h" - -#define collision_solver sat_2d_calculate_penetration -//#define collision_solver gjk_epa_calculate_penetration - -bool CollisionSolver2DSW::solve_static_world_boundary(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { - const WorldBoundaryShape2DSW *world_boundary = static_cast(p_shape_A); - if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) { - return false; - } - - Vector2 n = p_transform_A.basis_xform(world_boundary->get_normal()).normalized(); - Vector2 p = p_transform_A.xform(world_boundary->get_normal() * world_boundary->get_d()); - real_t d = n.dot(p); - - Vector2 supports[2]; - int support_count; - - p_shape_B->get_supports(p_transform_B.affine_inverse().basis_xform(-n).normalized(), supports, support_count); - - bool found = false; - - for (int i = 0; i < support_count; i++) { - supports[i] = p_transform_B.xform(supports[i]); - real_t pd = n.dot(supports[i]); - if (pd >= d) { - continue; - } - found = true; - - Vector2 support_A = supports[i] - n * (pd - d); - - if (p_result_callback) { - if (p_swap_result) { - p_result_callback(supports[i], support_A, p_userdata); - } else { - p_result_callback(support_A, supports[i], p_userdata); - } - } - } - - return found; -} - -bool CollisionSolver2DSW::solve_separation_ray(const Shape2DSW *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis, real_t p_margin) { - const SeparationRayShape2DSW *ray = static_cast(p_shape_A); - if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY) { - return false; - } - - Vector2 from = p_transform_A.get_origin(); - Vector2 to = from + p_transform_A[1] * (ray->get_length() + p_margin); - if (p_motion_A != Vector2()) { - //not the best but should be enough - Vector2 normal = (to - from).normalized(); - to += normal * MAX(0.0, normal.dot(p_motion_A)); - } - Vector2 support_A = to; - - Transform2D invb = p_transform_B.affine_inverse(); - from = invb.xform(from); - to = invb.xform(to); - - Vector2 p, n; - if (!p_shape_B->intersect_segment(from, to, p, n)) { - if (r_sep_axis) { - *r_sep_axis = p_transform_A[1].normalized(); - } - return false; - } - - // Discard contacts when the ray is fully contained inside the shape. - if (n == Vector2()) { - if (r_sep_axis) { - *r_sep_axis = p_transform_A[1].normalized(); - } - return false; - } - - // Discard contacts in the wrong direction. - if (n.dot(from - to) < CMP_EPSILON) { - if (r_sep_axis) { - *r_sep_axis = p_transform_A[1].normalized(); - } - return false; - } - - Vector2 support_B = p_transform_B.xform(p); - if (ray->get_slide_on_slope()) { - Vector2 global_n = invb.basis_xform_inv(n).normalized(); - support_B = support_A + (support_B - support_A).length() * global_n; - } - - if (p_result_callback) { - if (p_swap_result) { - p_result_callback(support_B, support_A, p_userdata); - } else { - p_result_callback(support_A, support_B, p_userdata); - } - } - return true; -} - -struct _ConcaveCollisionInfo2D { - const Transform2D *transform_A = nullptr; - const Shape2DSW *shape_A = nullptr; - const Transform2D *transform_B = nullptr; - Vector2 motion_A; - Vector2 motion_B; - real_t margin_A = 0.0; - real_t margin_B = 0.0; - CollisionSolver2DSW::CallbackResult result_callback; - void *userdata = nullptr; - bool swap_result = false; - bool collided = false; - int aabb_tests = 0; - int collisions = 0; - Vector2 *sep_axis = nullptr; -}; - -bool CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex) { - _ConcaveCollisionInfo2D &cinfo = *(_ConcaveCollisionInfo2D *)(p_userdata); - cinfo.aabb_tests++; - - bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex, *cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, cinfo.sep_axis, cinfo.margin_A, cinfo.margin_B); - if (!collided) { - return false; - } - - cinfo.collided = true; - cinfo.collisions++; - - // Stop at first collision if contacts are not needed. - return !cinfo.result_callback; -} - -bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { - const ConcaveShape2DSW *concave_B = static_cast(p_shape_B); - - _ConcaveCollisionInfo2D cinfo; - cinfo.transform_A = &p_transform_A; - cinfo.shape_A = p_shape_A; - cinfo.transform_B = &p_transform_B; - cinfo.motion_A = p_motion_A; - cinfo.result_callback = p_result_callback; - cinfo.userdata = p_userdata; - cinfo.swap_result = p_swap_result; - cinfo.collided = false; - cinfo.collisions = 0; - cinfo.sep_axis = r_sep_axis; - cinfo.margin_A = p_margin_A; - cinfo.margin_B = p_margin_B; - - cinfo.aabb_tests = 0; - - Transform2D rel_transform = p_transform_A; - rel_transform.elements[2] -= p_transform_B.get_origin(); - - //quickly compute a local Rect2 - - Rect2 local_aabb; - for (int i = 0; i < 2; i++) { - Vector2 axis(p_transform_B.elements[i]); - real_t axis_scale = 1.0 / axis.length(); - axis *= axis_scale; - - real_t smin, smax; - p_shape_A->project_rangev(axis, rel_transform, smin, smax); - smin *= axis_scale; - smax *= axis_scale; - - local_aabb.position[i] = smin; - local_aabb.size[i] = smax - smin; - } - - concave_B->cull(local_aabb, concave_callback, &cinfo); - - return cinfo.collided; -} - -bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { - PhysicsServer2D::ShapeType type_A = p_shape_A->get_type(); - PhysicsServer2D::ShapeType type_B = p_shape_B->get_type(); - bool concave_A = p_shape_A->is_concave(); - bool concave_B = p_shape_B->is_concave(); - real_t margin_A = p_margin_A, margin_B = p_margin_B; - - bool swap = false; - - if (type_A > type_B) { - SWAP(type_A, type_B); - SWAP(concave_A, concave_B); - SWAP(margin_A, margin_B); - swap = true; - } - - if (type_A == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) { - if (type_B == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) { - return false; - } - - if (swap) { - return solve_static_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); - } else { - return solve_static_world_boundary(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); - } - - } else if (type_A == PhysicsServer2D::SHAPE_SEPARATION_RAY) { - if (type_B == PhysicsServer2D::SHAPE_SEPARATION_RAY) { - return false; //no ray-ray - } - - if (swap) { - return solve_separation_ray(p_shape_B, p_motion_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, r_sep_axis, p_margin_B); - } else { - return solve_separation_ray(p_shape_A, p_motion_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, r_sep_axis, p_margin_A); - } - - } else if (concave_B) { - if (concave_A) { - return false; - } - - if (!swap) { - return solve_concave(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, r_sep_axis, margin_A, margin_B); - } else { - return solve_concave(p_shape_B, p_transform_B, p_motion_B, p_shape_A, p_transform_A, p_motion_A, p_result_callback, p_userdata, true, r_sep_axis, margin_A, margin_B); - } - - } else { - return collision_solver(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, r_sep_axis, margin_A, margin_B); - } -} diff --git a/servers/physics_2d/collision_solver_2d_sw.h b/servers/physics_2d/collision_solver_2d_sw.h deleted file mode 100644 index b87247b89a..0000000000 --- a/servers/physics_2d/collision_solver_2d_sw.h +++ /dev/null @@ -1,50 +0,0 @@ -/*************************************************************************/ -/* collision_solver_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 COLLISION_SOLVER_2D_SW_H -#define COLLISION_SOLVER_2D_SW_H - -#include "shape_2d_sw.h" - -class CollisionSolver2DSW { -public: - typedef void (*CallbackResult)(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata); - -private: - static bool solve_static_world_boundary(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); - static bool concave_callback(void *p_userdata, Shape2DSW *p_convex); - static bool solve_concave(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); - static bool solve_separation_ray(const Shape2DSW *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis = nullptr, real_t p_margin = 0); - -public: - static bool solve(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); -}; - -#endif // COLLISION_SOLVER_2D_SW_H diff --git a/servers/physics_2d/constraint_2d_sw.h b/servers/physics_2d/constraint_2d_sw.h deleted file mode 100644 index df300d666d..0000000000 --- a/servers/physics_2d/constraint_2d_sw.h +++ /dev/null @@ -1,70 +0,0 @@ -/*************************************************************************/ -/* constraint_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 CONSTRAINT_2D_SW_H -#define CONSTRAINT_2D_SW_H - -#include "body_2d_sw.h" - -class Constraint2DSW { - Body2DSW **_body_ptr; - int _body_count; - uint64_t island_step = 0; - bool disabled_collisions_between_bodies = true; - - RID self; - -protected: - Constraint2DSW(Body2DSW **p_body_ptr = nullptr, int p_body_count = 0) { - _body_ptr = p_body_ptr; - _body_count = p_body_count; - } - -public: - _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } - _FORCE_INLINE_ RID get_self() const { return self; } - - _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } - _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } - - _FORCE_INLINE_ Body2DSW **get_body_ptr() const { return _body_ptr; } - _FORCE_INLINE_ int get_body_count() const { return _body_count; } - - _FORCE_INLINE_ void disable_collisions_between_bodies(const bool p_disabled) { disabled_collisions_between_bodies = p_disabled; } - _FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; } - - virtual bool setup(real_t p_step) = 0; - virtual bool pre_solve(real_t p_step) = 0; - virtual void solve(real_t p_step) = 0; - - virtual ~Constraint2DSW() {} -}; - -#endif // CONSTRAINT_2D_SW_H diff --git a/servers/physics_2d/godot_area_2d.cpp b/servers/physics_2d/godot_area_2d.cpp new file mode 100644 index 0000000000..7cb202dd1f --- /dev/null +++ b/servers/physics_2d/godot_area_2d.cpp @@ -0,0 +1,305 @@ +/*************************************************************************/ +/* godot_area_2d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_area_2d.h" +#include "godot_body_2d.h" +#include "godot_space_2d.h" + +GodotArea2D::BodyKey::BodyKey(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + rid = p_body->get_self(); + instance_id = p_body->get_instance_id(); + body_shape = p_body_shape; + area_shape = p_area_shape; +} + +GodotArea2D::BodyKey::BodyKey(GodotArea2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + rid = p_body->get_self(); + instance_id = p_body->get_instance_id(); + body_shape = p_body_shape; + area_shape = p_area_shape; +} + +void GodotArea2D::_shapes_changed() { + if (!moved_list.in_list() && get_space()) { + get_space()->area_add_to_moved_list(&moved_list); + } +} + +void GodotArea2D::set_transform(const Transform2D &p_transform) { + if (!moved_list.in_list() && get_space()) { + get_space()->area_add_to_moved_list(&moved_list); + } + + _set_transform(p_transform); + _set_inv_transform(p_transform.affine_inverse()); +} + +void GodotArea2D::set_space(GodotSpace2D *p_space) { + if (get_space()) { + if (monitor_query_list.in_list()) { + get_space()->area_remove_from_monitor_query_list(&monitor_query_list); + } + if (moved_list.in_list()) { + get_space()->area_remove_from_moved_list(&moved_list); + } + } + + monitored_bodies.clear(); + monitored_areas.clear(); + + _set_space(p_space); +} + +void GodotArea2D::set_monitor_callback(ObjectID p_id, const StringName &p_method) { + if (p_id == monitor_callback_id) { + monitor_callback_method = p_method; + return; + } + + _unregister_shapes(); + + monitor_callback_id = p_id; + monitor_callback_method = p_method; + + monitored_bodies.clear(); + monitored_areas.clear(); + + _shape_changed(); + + if (!moved_list.in_list() && get_space()) { + get_space()->area_add_to_moved_list(&moved_list); + } +} + +void GodotArea2D::set_area_monitor_callback(ObjectID p_id, const StringName &p_method) { + if (p_id == area_monitor_callback_id) { + area_monitor_callback_method = p_method; + return; + } + + _unregister_shapes(); + + area_monitor_callback_id = p_id; + area_monitor_callback_method = p_method; + + monitored_bodies.clear(); + monitored_areas.clear(); + + _shape_changed(); + + if (!moved_list.in_list() && get_space()) { + get_space()->area_add_to_moved_list(&moved_list); + } +} + +void GodotArea2D::set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode p_mode) { + bool do_override = p_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED; + if (do_override == (space_override_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED)) { + return; + } + _unregister_shapes(); + space_override_mode = p_mode; + _shape_changed(); +} + +void GodotArea2D::set_param(PhysicsServer2D::AreaParameter p_param, const Variant &p_value) { + switch (p_param) { + case PhysicsServer2D::AREA_PARAM_GRAVITY: + gravity = p_value; + break; + case PhysicsServer2D::AREA_PARAM_GRAVITY_VECTOR: + gravity_vector = p_value; + break; + case PhysicsServer2D::AREA_PARAM_GRAVITY_IS_POINT: + gravity_is_point = p_value; + break; + case PhysicsServer2D::AREA_PARAM_GRAVITY_DISTANCE_SCALE: + gravity_distance_scale = p_value; + break; + case PhysicsServer2D::AREA_PARAM_GRAVITY_POINT_ATTENUATION: + point_attenuation = p_value; + break; + case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP: + linear_damp = p_value; + break; + case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP: + angular_damp = p_value; + break; + case PhysicsServer2D::AREA_PARAM_PRIORITY: + priority = p_value; + break; + } +} + +Variant GodotArea2D::get_param(PhysicsServer2D::AreaParameter p_param) const { + switch (p_param) { + case PhysicsServer2D::AREA_PARAM_GRAVITY: + return gravity; + case PhysicsServer2D::AREA_PARAM_GRAVITY_VECTOR: + return gravity_vector; + case PhysicsServer2D::AREA_PARAM_GRAVITY_IS_POINT: + return gravity_is_point; + case PhysicsServer2D::AREA_PARAM_GRAVITY_DISTANCE_SCALE: + return gravity_distance_scale; + case PhysicsServer2D::AREA_PARAM_GRAVITY_POINT_ATTENUATION: + return point_attenuation; + case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP: + return linear_damp; + case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP: + return angular_damp; + case PhysicsServer2D::AREA_PARAM_PRIORITY: + return priority; + } + + return Variant(); +} + +void GodotArea2D::_queue_monitor_update() { + ERR_FAIL_COND(!get_space()); + + if (!monitor_query_list.in_list()) { + get_space()->area_add_to_monitor_query_list(&monitor_query_list); + } +} + +void GodotArea2D::set_monitorable(bool p_monitorable) { + if (monitorable == p_monitorable) { + return; + } + + monitorable = p_monitorable; + _set_static(!monitorable); +} + +void GodotArea2D::call_queries() { + if (monitor_callback_id.is_valid() && !monitored_bodies.is_empty()) { + Variant res[5]; + Variant *resptr[5]; + for (int i = 0; i < 5; i++) { + resptr[i] = &res[i]; + } + + Object *obj = ObjectDB::get_instance(monitor_callback_id); + if (!obj) { + monitored_bodies.clear(); + monitor_callback_id = ObjectID(); + return; + } + + for (Map::Element *E = monitored_bodies.front(); E;) { + if (E->get().state == 0) { // Nothing happened + Map::Element *next = E->next(); + monitored_bodies.erase(E); + E = next; + continue; + } + + res[0] = E->get().state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED; + res[1] = E->key().rid; + res[2] = E->key().instance_id; + res[3] = E->key().body_shape; + res[4] = E->key().area_shape; + + Map::Element *next = E->next(); + monitored_bodies.erase(E); + E = next; + + Callable::CallError ce; + obj->call(monitor_callback_method, (const Variant **)resptr, 5, ce); + } + } + + if (area_monitor_callback_id.is_valid() && !monitored_areas.is_empty()) { + Variant res[5]; + Variant *resptr[5]; + for (int i = 0; i < 5; i++) { + resptr[i] = &res[i]; + } + + Object *obj = ObjectDB::get_instance(area_monitor_callback_id); + if (!obj) { + monitored_areas.clear(); + area_monitor_callback_id = ObjectID(); + return; + } + + for (Map::Element *E = monitored_areas.front(); E;) { + if (E->get().state == 0) { // Nothing happened + Map::Element *next = E->next(); + monitored_areas.erase(E); + E = next; + continue; + } + + res[0] = E->get().state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED; + res[1] = E->key().rid; + res[2] = E->key().instance_id; + res[3] = E->key().body_shape; + res[4] = E->key().area_shape; + + Map::Element *next = E->next(); + monitored_areas.erase(E); + E = next; + + Callable::CallError ce; + obj->call(area_monitor_callback_method, (const Variant **)resptr, 5, ce); + } + } +} + +void GodotArea2D::compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const { + if (is_gravity_point()) { + const real_t gravity_distance_scale = get_gravity_distance_scale(); + Vector2 v = get_transform().xform(get_gravity_vector()) - p_position; + if (gravity_distance_scale > 0) { + const real_t v_length = v.length(); + if (v_length > 0) { + const real_t v_scaled = v_length * gravity_distance_scale; + r_gravity = (v.normalized() * (get_gravity() / (v_scaled * v_scaled))); + } else { + r_gravity = Vector2(); + } + } else { + r_gravity = v.normalized() * get_gravity(); + } + } else { + r_gravity = get_gravity_vector() * get_gravity(); + } +} + +GodotArea2D::GodotArea2D() : + GodotCollisionObject2D(TYPE_AREA), + monitor_query_list(this), + moved_list(this) { + _set_static(true); //areas are not active by default +} + +GodotArea2D::~GodotArea2D() { +} diff --git a/servers/physics_2d/godot_area_2d.h b/servers/physics_2d/godot_area_2d.h new file mode 100644 index 0000000000..daa03d39e3 --- /dev/null +++ b/servers/physics_2d/godot_area_2d.h @@ -0,0 +1,196 @@ +/*************************************************************************/ +/* godot_area_2d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_AREA_2D_H +#define GODOT_AREA_2D_H + +#include "godot_collision_object_2d.h" + +#include "core/templates/self_list.h" +#include "servers/physics_server_2d.h" + +class GodotSpace2D; +class GodotBody2D; +class GodotConstraint2D; + +class GodotArea2D : public GodotCollisionObject2D { + PhysicsServer2D::AreaSpaceOverrideMode space_override_mode = PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED; + real_t gravity = 9.80665; + Vector2 gravity_vector = Vector2(0, -1); + bool gravity_is_point = false; + real_t gravity_distance_scale = 0.0; + real_t point_attenuation = 1.0; + real_t linear_damp = 0.1; + real_t angular_damp = 1.0; + int priority = 0; + bool monitorable = false; + + ObjectID monitor_callback_id; + StringName monitor_callback_method; + + ObjectID area_monitor_callback_id; + StringName area_monitor_callback_method; + + SelfList monitor_query_list; + SelfList moved_list; + + struct BodyKey { + RID rid; + ObjectID instance_id; + uint32_t body_shape = 0; + uint32_t area_shape = 0; + + _FORCE_INLINE_ bool operator<(const BodyKey &p_key) const { + if (rid == p_key.rid) { + if (body_shape == p_key.body_shape) { + return area_shape < p_key.area_shape; + } else { + return body_shape < p_key.body_shape; + } + } else { + return rid < p_key.rid; + } + } + + _FORCE_INLINE_ BodyKey() {} + BodyKey(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + BodyKey(GodotArea2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + }; + + struct BodyState { + int state = 0; + _FORCE_INLINE_ void inc() { state++; } + _FORCE_INLINE_ void dec() { state--; } + }; + + Map monitored_bodies; + Map monitored_areas; + + Set constraints; + + virtual void _shapes_changed(); + void _queue_monitor_update(); + +public: + void set_monitor_callback(ObjectID p_id, const StringName &p_method); + _FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id.is_valid(); } + + void set_area_monitor_callback(ObjectID p_id, const StringName &p_method); + _FORCE_INLINE_ bool has_area_monitor_callback() const { return area_monitor_callback_id.is_valid(); } + + _FORCE_INLINE_ void add_body_to_query(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + _FORCE_INLINE_ void remove_body_from_query(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + + _FORCE_INLINE_ void add_area_to_query(GodotArea2D *p_area, uint32_t p_area_shape, uint32_t p_self_shape); + _FORCE_INLINE_ void remove_area_from_query(GodotArea2D *p_area, uint32_t p_area_shape, uint32_t p_self_shape); + + void set_param(PhysicsServer2D::AreaParameter p_param, const Variant &p_value); + Variant get_param(PhysicsServer2D::AreaParameter p_param) const; + + void set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode p_mode); + PhysicsServer2D::AreaSpaceOverrideMode get_space_override_mode() const { return space_override_mode; } + + _FORCE_INLINE_ void set_gravity(real_t p_gravity) { gravity = p_gravity; } + _FORCE_INLINE_ real_t get_gravity() const { return gravity; } + + _FORCE_INLINE_ void set_gravity_vector(const Vector2 &p_gravity) { gravity_vector = p_gravity; } + _FORCE_INLINE_ Vector2 get_gravity_vector() const { return gravity_vector; } + + _FORCE_INLINE_ void set_gravity_as_point(bool p_enable) { gravity_is_point = p_enable; } + _FORCE_INLINE_ bool is_gravity_point() const { return gravity_is_point; } + + _FORCE_INLINE_ void set_gravity_distance_scale(real_t scale) { gravity_distance_scale = scale; } + _FORCE_INLINE_ real_t get_gravity_distance_scale() const { return gravity_distance_scale; } + + _FORCE_INLINE_ void set_point_attenuation(real_t p_point_attenuation) { point_attenuation = p_point_attenuation; } + _FORCE_INLINE_ real_t get_point_attenuation() const { return point_attenuation; } + + _FORCE_INLINE_ void set_linear_damp(real_t p_linear_damp) { linear_damp = p_linear_damp; } + _FORCE_INLINE_ real_t get_linear_damp() const { return linear_damp; } + + _FORCE_INLINE_ void set_angular_damp(real_t p_angular_damp) { angular_damp = p_angular_damp; } + _FORCE_INLINE_ real_t get_angular_damp() const { return angular_damp; } + + _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } + _FORCE_INLINE_ int get_priority() const { return priority; } + + _FORCE_INLINE_ void add_constraint(GodotConstraint2D *p_constraint) { constraints.insert(p_constraint); } + _FORCE_INLINE_ void remove_constraint(GodotConstraint2D *p_constraint) { constraints.erase(p_constraint); } + _FORCE_INLINE_ const Set &get_constraints() const { return constraints; } + _FORCE_INLINE_ void clear_constraints() { constraints.clear(); } + + void set_monitorable(bool p_monitorable); + _FORCE_INLINE_ bool is_monitorable() const { return monitorable; } + + void set_transform(const Transform2D &p_transform); + + void set_space(GodotSpace2D *p_space); + + void call_queries(); + + void compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const; + + GodotArea2D(); + ~GodotArea2D(); +}; + +void GodotArea2D::add_body_to_query(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + BodyKey bk(p_body, p_body_shape, p_area_shape); + monitored_bodies[bk].inc(); + if (!monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +void GodotArea2D::remove_body_from_query(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + BodyKey bk(p_body, p_body_shape, p_area_shape); + monitored_bodies[bk].dec(); + if (!monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +void GodotArea2D::add_area_to_query(GodotArea2D *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { + BodyKey bk(p_area, p_area_shape, p_self_shape); + monitored_areas[bk].inc(); + if (!monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +void GodotArea2D::remove_area_from_query(GodotArea2D *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { + BodyKey bk(p_area, p_area_shape, p_self_shape); + monitored_areas[bk].dec(); + if (!monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +#endif // GODOT_AREA_2D_H diff --git a/servers/physics_2d/godot_area_pair_2d.cpp b/servers/physics_2d/godot_area_pair_2d.cpp new file mode 100644 index 0000000000..fdb95aa262 --- /dev/null +++ b/servers/physics_2d/godot_area_pair_2d.cpp @@ -0,0 +1,190 @@ +/*************************************************************************/ +/* godot_area_pair_2d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_area_pair_2d.h" +#include "godot_collision_solver_2d.h" + +bool GodotAreaPair2D::setup(real_t p_step) { + bool result = false; + if (area->collides_with(body) && GodotCollisionSolver2D::solve(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), Vector2(), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), Vector2(), nullptr, this)) { + result = true; + } + + process_collision = false; + if (result != colliding) { + if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + process_collision = true; + } else if (area->has_monitor_callback()) { + process_collision = true; + } + + colliding = result; + } + + return process_collision; +} + +bool GodotAreaPair2D::pre_solve(real_t p_step) { + if (!process_collision) { + return false; + } + + if (colliding) { + if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + body->add_area(area); + } + + if (area->has_monitor_callback()) { + area->add_body_to_query(body, body_shape, area_shape); + } + } else { + if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + body->remove_area(area); + } + + if (area->has_monitor_callback()) { + area->remove_body_from_query(body, body_shape, area_shape); + } + } + + return false; // Never do any post solving. +} + +void GodotAreaPair2D::solve(real_t p_step) { + // Nothing to do. +} + +GodotAreaPair2D::GodotAreaPair2D(GodotBody2D *p_body, int p_body_shape, GodotArea2D *p_area, int p_area_shape) { + body = p_body; + area = p_area; + body_shape = p_body_shape; + area_shape = p_area_shape; + body->add_constraint(this, 0); + area->add_constraint(this); + if (p_body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { //need to be active to process pair + p_body->set_active(true); + } +} + +GodotAreaPair2D::~GodotAreaPair2D() { + if (colliding) { + if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + body->remove_area(area); + } + if (area->has_monitor_callback()) { + area->remove_body_from_query(body, body_shape, area_shape); + } + } + body->remove_constraint(this, 0); + area->remove_constraint(this); +} + +////////////////////////////////// + +bool GodotArea2Pair2D::setup(real_t p_step) { + bool result_a = area_a->collides_with(area_b); + bool result_b = area_b->collides_with(area_a); + if ((result_a || result_b) && !GodotCollisionSolver2D::solve(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), Vector2(), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), Vector2(), nullptr, this)) { + result_a = false; + result_b = false; + } + + bool process_collision = false; + + process_collision_a = false; + if (result_a != colliding_a) { + if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { + process_collision_a = true; + process_collision = true; + } + colliding_a = result_a; + } + + process_collision_b = false; + if (result_b != colliding_b) { + if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { + process_collision_b = true; + process_collision = true; + } + colliding_b = result_b; + } + + return process_collision; +} + +bool GodotArea2Pair2D::pre_solve(real_t p_step) { + if (process_collision_a) { + if (colliding_a) { + area_a->add_area_to_query(area_b, shape_b, shape_a); + } else { + area_a->remove_area_from_query(area_b, shape_b, shape_a); + } + } + + if (process_collision_b) { + if (colliding_b) { + area_b->add_area_to_query(area_a, shape_a, shape_b); + } else { + area_b->remove_area_from_query(area_a, shape_a, shape_b); + } + } + + return false; // Never do any post solving. +} + +void GodotArea2Pair2D::solve(real_t p_step) { + // Nothing to do. +} + +GodotArea2Pair2D::GodotArea2Pair2D(GodotArea2D *p_area_a, int p_shape_a, GodotArea2D *p_area_b, int p_shape_b) { + area_a = p_area_a; + area_b = p_area_b; + shape_a = p_shape_a; + shape_b = p_shape_b; + area_a->add_constraint(this); + area_b->add_constraint(this); +} + +GodotArea2Pair2D::~GodotArea2Pair2D() { + if (colliding_a) { + if (area_a->has_area_monitor_callback()) { + area_a->remove_area_from_query(area_b, shape_b, shape_a); + } + } + + if (colliding_b) { + if (area_b->has_area_monitor_callback()) { + area_b->remove_area_from_query(area_a, shape_a, shape_b); + } + } + + area_a->remove_constraint(this); + area_b->remove_constraint(this); +} diff --git a/servers/physics_2d/godot_area_pair_2d.h b/servers/physics_2d/godot_area_pair_2d.h new file mode 100644 index 0000000000..7a9677f714 --- /dev/null +++ b/servers/physics_2d/godot_area_pair_2d.h @@ -0,0 +1,74 @@ +/*************************************************************************/ +/* godot_area_pair_2d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_AREA_PAIR_2D_H +#define GODOT_AREA_PAIR_2D_H + +#include "godot_area_2d.h" +#include "godot_body_2d.h" +#include "godot_constraint_2d.h" + +class GodotAreaPair2D : public GodotConstraint2D { + GodotBody2D *body = nullptr; + GodotArea2D *area = nullptr; + int body_shape = 0; + int area_shape = 0; + bool colliding = false; + bool process_collision = false; + +public: + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + GodotAreaPair2D(GodotBody2D *p_body, int p_body_shape, GodotArea2D *p_area, int p_area_shape); + ~GodotAreaPair2D(); +}; + +class GodotArea2Pair2D : public GodotConstraint2D { + GodotArea2D *area_a = nullptr; + GodotArea2D *area_b = nullptr; + int shape_a = 0; + int shape_b = 0; + bool colliding_a = false; + bool colliding_b = false; + bool process_collision_a = false; + bool process_collision_b = false; + +public: + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + GodotArea2Pair2D(GodotArea2D *p_area_a, int p_shape_a, GodotArea2D *p_area_b, int p_shape_b); + ~GodotArea2Pair2D(); +}; + +#endif // GODOT_AREA_PAIR_2D_H diff --git a/servers/physics_2d/godot_body_2d.cpp b/servers/physics_2d/godot_body_2d.cpp new file mode 100644 index 0000000000..b0885a1f7b --- /dev/null +++ b/servers/physics_2d/godot_body_2d.cpp @@ -0,0 +1,677 @@ +/*************************************************************************/ +/* godot_body_2d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_body_2d.h" + +#include "godot_area_2d.h" +#include "godot_body_direct_state_2d.h" +#include "godot_space_2d.h" + +void GodotBody2D::_mass_properties_changed() { + if (get_space() && !mass_properties_update_list.in_list() && (calculate_inertia || calculate_center_of_mass)) { + get_space()->body_add_to_mass_properties_update_list(&mass_properties_update_list); + } +} + +void GodotBody2D::update_mass_properties() { + //update shapes and motions + + switch (mode) { + case PhysicsServer2D::BODY_MODE_DYNAMIC: { + real_t total_area = 0; + for (int i = 0; i < get_shape_count(); i++) { + if (is_shape_disabled(i)) { + continue; + } + total_area += get_shape_aabb(i).get_area(); + } + + if (calculate_center_of_mass) { + // We have to recompute the center of mass. + center_of_mass = Vector2(); + + if (total_area != 0.0) { + for (int i = 0; i < get_shape_count(); i++) { + if (is_shape_disabled(i)) { + continue; + } + + real_t area = get_shape_aabb(i).get_area(); + + real_t mass = area * this->mass / total_area; + + // NOTE: we assume that the shape origin is also its center of mass. + center_of_mass += mass * get_shape_transform(i).get_origin(); + } + + center_of_mass /= mass; + } + } + + if (calculate_inertia) { + inertia = 0; + + for (int i = 0; i < get_shape_count(); i++) { + if (is_shape_disabled(i)) { + continue; + } + + const GodotShape2D *shape = get_shape(i); + + real_t area = get_shape_aabb(i).get_area(); + if (area == 0.0) { + continue; + } + + real_t mass = area * this->mass / total_area; + + Transform2D mtx = get_shape_transform(i); + Vector2 scale = mtx.get_scale(); + Vector2 shape_origin = mtx.get_origin() - center_of_mass; + inertia += shape->get_moment_of_inertia(mass, scale) + mass * shape_origin.length_squared(); + } + } + + _inv_inertia = inertia > 0.0 ? (1.0 / inertia) : 0.0; + + if (mass) { + _inv_mass = 1.0 / mass; + } else { + _inv_mass = 0; + } + + } break; + case PhysicsServer2D::BODY_MODE_KINEMATIC: + case PhysicsServer2D::BODY_MODE_STATIC: { + _inv_inertia = 0; + _inv_mass = 0; + } break; + case PhysicsServer2D::BODY_MODE_DYNAMIC_LINEAR: { + _inv_inertia = 0; + _inv_mass = 1.0 / mass; + + } break; + } +} + +void GodotBody2D::reset_mass_properties() { + calculate_inertia = true; + calculate_center_of_mass = true; + _mass_properties_changed(); +} + +void GodotBody2D::set_active(bool p_active) { + if (active == p_active) { + return; + } + + active = p_active; + + if (active) { + if (mode == PhysicsServer2D::BODY_MODE_STATIC) { + // Static bodies can't be active. + active = false; + } else if (get_space()) { + get_space()->body_add_to_active_list(&active_list); + } + } else if (get_space()) { + get_space()->body_remove_from_active_list(&active_list); + } +} + +void GodotBody2D::set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value) { + switch (p_param) { + case PhysicsServer2D::BODY_PARAM_BOUNCE: { + bounce = p_value; + } break; + case PhysicsServer2D::BODY_PARAM_FRICTION: { + friction = p_value; + } break; + case PhysicsServer2D::BODY_PARAM_MASS: { + real_t mass_value = p_value; + ERR_FAIL_COND(mass_value <= 0); + mass = mass_value; + if (mode >= PhysicsServer2D::BODY_MODE_DYNAMIC) { + _mass_properties_changed(); + } + } break; + case PhysicsServer2D::BODY_PARAM_INERTIA: { + real_t inertia_value = p_value; + if (inertia_value <= 0.0) { + calculate_inertia = true; + if (mode == PhysicsServer2D::BODY_MODE_DYNAMIC) { + _mass_properties_changed(); + } + } else { + calculate_inertia = false; + inertia = inertia_value; + if (mode == PhysicsServer2D::BODY_MODE_DYNAMIC) { + _inv_inertia = 1.0 / inertia; + } + } + } break; + case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: { + calculate_center_of_mass = false; + center_of_mass = p_value; + } break; + case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: { + gravity_scale = p_value; + } break; + case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP: { + linear_damp = p_value; + } break; + case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP: { + angular_damp = p_value; + } break; + default: { + } + } +} + +Variant GodotBody2D::get_param(PhysicsServer2D::BodyParameter p_param) const { + switch (p_param) { + case PhysicsServer2D::BODY_PARAM_BOUNCE: { + return bounce; + } + case PhysicsServer2D::BODY_PARAM_FRICTION: { + return friction; + } + case PhysicsServer2D::BODY_PARAM_MASS: { + return mass; + } + case PhysicsServer2D::BODY_PARAM_INERTIA: { + return inertia; + } + case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: { + return center_of_mass; + } + case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: { + return gravity_scale; + } + case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP: { + return linear_damp; + } + case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP: { + return angular_damp; + } + default: { + } + } + + return 0; +} + +void GodotBody2D::set_mode(PhysicsServer2D::BodyMode p_mode) { + PhysicsServer2D::BodyMode prev = mode; + mode = p_mode; + + switch (p_mode) { + //CLEAR UP EVERYTHING IN CASE IT NOT WORKS! + case PhysicsServer2D::BODY_MODE_STATIC: + case PhysicsServer2D::BODY_MODE_KINEMATIC: { + _set_inv_transform(get_transform().affine_inverse()); + _inv_mass = 0; + _inv_inertia = 0; + _set_static(p_mode == PhysicsServer2D::BODY_MODE_STATIC); + set_active(p_mode == PhysicsServer2D::BODY_MODE_KINEMATIC && contacts.size()); + linear_velocity = Vector2(); + angular_velocity = 0; + if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC && prev != mode) { + first_time_kinematic = true; + } + } break; + case PhysicsServer2D::BODY_MODE_DYNAMIC: { + _inv_mass = mass > 0 ? (1.0 / mass) : 0; + if (!calculate_inertia) { + _inv_inertia = 1.0 / inertia; + } + _mass_properties_changed(); + _set_static(false); + set_active(true); + + } break; + case PhysicsServer2D::BODY_MODE_DYNAMIC_LINEAR: { + _inv_mass = mass > 0 ? (1.0 / mass) : 0; + _inv_inertia = 0; + angular_velocity = 0; + _set_static(false); + set_active(true); + } + } +} + +PhysicsServer2D::BodyMode GodotBody2D::get_mode() const { + return mode; +} + +void GodotBody2D::_shapes_changed() { + _mass_properties_changed(); + wakeup_neighbours(); +} + +void GodotBody2D::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_variant) { + switch (p_state) { + case PhysicsServer2D::BODY_STATE_TRANSFORM: { + if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { + new_transform = p_variant; + //wakeup_neighbours(); + set_active(true); + if (first_time_kinematic) { + _set_transform(p_variant); + _set_inv_transform(get_transform().affine_inverse()); + first_time_kinematic = false; + } + } else if (mode == PhysicsServer2D::BODY_MODE_STATIC) { + _set_transform(p_variant); + _set_inv_transform(get_transform().affine_inverse()); + wakeup_neighbours(); + } else { + Transform2D t = p_variant; + t.orthonormalize(); + new_transform = get_transform(); //used as old to compute motion + if (t == new_transform) { + break; + } + _set_transform(t); + _set_inv_transform(get_transform().inverse()); + } + wakeup(); + + } break; + case PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY: { + linear_velocity = p_variant; + constant_linear_velocity = linear_velocity; + wakeup(); + + } break; + case PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY: { + angular_velocity = p_variant; + constant_angular_velocity = angular_velocity; + wakeup(); + + } break; + case PhysicsServer2D::BODY_STATE_SLEEPING: { + if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { + break; + } + bool do_sleep = p_variant; + if (do_sleep) { + linear_velocity = Vector2(); + //biased_linear_velocity=Vector3(); + angular_velocity = 0; + //biased_angular_velocity=Vector3(); + set_active(false); + } else { + if (mode != PhysicsServer2D::BODY_MODE_STATIC) { + set_active(true); + } + } + } break; + case PhysicsServer2D::BODY_STATE_CAN_SLEEP: { + can_sleep = p_variant; + if (mode >= PhysicsServer2D::BODY_MODE_DYNAMIC && !active && !can_sleep) { + set_active(true); + } + + } break; + } +} + +Variant GodotBody2D::get_state(PhysicsServer2D::BodyState p_state) const { + switch (p_state) { + case PhysicsServer2D::BODY_STATE_TRANSFORM: { + return get_transform(); + } + case PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY: { + return linear_velocity; + } + case PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY: { + return angular_velocity; + } + case PhysicsServer2D::BODY_STATE_SLEEPING: { + return !is_active(); + } + case PhysicsServer2D::BODY_STATE_CAN_SLEEP: { + return can_sleep; + } + } + + return Variant(); +} + +void GodotBody2D::set_space(GodotSpace2D *p_space) { + if (get_space()) { + wakeup_neighbours(); + + if (mass_properties_update_list.in_list()) { + get_space()->body_remove_from_mass_properties_update_list(&mass_properties_update_list); + } + if (active_list.in_list()) { + get_space()->body_remove_from_active_list(&active_list); + } + if (direct_state_query_list.in_list()) { + get_space()->body_remove_from_state_query_list(&direct_state_query_list); + } + } + + _set_space(p_space); + + if (get_space()) { + _mass_properties_changed(); + if (active) { + get_space()->body_add_to_active_list(&active_list); + } + } +} + +void GodotBody2D::_compute_area_gravity_and_damping(const GodotArea2D *p_area) { + Vector2 area_gravity; + p_area->compute_gravity(get_transform().get_origin(), area_gravity); + gravity += area_gravity; + + area_linear_damp += p_area->get_linear_damp(); + area_angular_damp += p_area->get_angular_damp(); +} + +void GodotBody2D::integrate_forces(real_t p_step) { + if (mode == PhysicsServer2D::BODY_MODE_STATIC) { + return; + } + + GodotArea2D *def_area = get_space()->get_default_area(); + // GodotArea2D *damp_area = def_area; + ERR_FAIL_COND(!def_area); + + int ac = areas.size(); + bool stopped = false; + gravity = Vector2(0, 0); + area_angular_damp = 0; + area_linear_damp = 0; + if (ac) { + areas.sort(); + const AreaCMP *aa = &areas[0]; + // damp_area = aa[ac-1].area; + for (int i = ac - 1; i >= 0 && !stopped; i--) { + PhysicsServer2D::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode(); + switch (mode) { + case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE: + case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + _compute_area_gravity_and_damping(aa[i].area); + stopped = mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + } break; + case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE: + case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { + gravity = Vector2(0, 0); + area_angular_damp = 0; + area_linear_damp = 0; + _compute_area_gravity_and_damping(aa[i].area); + stopped = mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE; + } break; + default: { + } + } + } + } + if (!stopped) { + _compute_area_gravity_and_damping(def_area); + } + gravity *= gravity_scale; + + // If less than 0, override dampenings with that of the Body2D + if (angular_damp >= 0) { + area_angular_damp = angular_damp; + } + /* + else + area_angular_damp=damp_area->get_angular_damp(); + */ + + if (linear_damp >= 0) { + area_linear_damp = linear_damp; + } + /* + else + area_linear_damp=damp_area->get_linear_damp(); + */ + + Vector2 motion; + bool do_motion = false; + + if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { + //compute motion, angular and etc. velocities from prev transform + motion = new_transform.get_origin() - get_transform().get_origin(); + linear_velocity = constant_linear_velocity + motion / p_step; + + real_t rot = new_transform.get_rotation() - get_transform().get_rotation(); + angular_velocity = constant_angular_velocity + remainder(rot, 2.0 * Math_PI) / p_step; + + do_motion = true; + + /* + for(int i=0;ibody_add_to_state_query_list(&direct_state_query_list); + } + + if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { + _set_transform(new_transform, false); + _set_inv_transform(new_transform.affine_inverse()); + if (contacts.size() == 0 && linear_velocity == Vector2() && angular_velocity == 0) { + set_active(false); //stopped moving, deactivate + } + return; + } + + real_t total_angular_velocity = angular_velocity + biased_angular_velocity; + Vector2 total_linear_velocity = linear_velocity + biased_linear_velocity; + + real_t angle = get_transform().get_rotation() + total_angular_velocity * p_step; + Vector2 pos = get_transform().get_origin() + total_linear_velocity * p_step; + + real_t center_of_mass_distance = center_of_mass.length(); + if (center_of_mass_distance > CMP_EPSILON) { + // Calculate displacement due to center of mass offset. + real_t prev_angle = get_transform().get_rotation(); + real_t angle_base = Math::atan2(center_of_mass.y, center_of_mass.x); + Vector2 point1(Math::cos(angle_base + prev_angle), Math::sin(angle_base + prev_angle)); + Vector2 point2(Math::cos(angle_base + angle), Math::sin(angle_base + angle)); + pos += center_of_mass_distance * (point1 - point2); + } + + _set_transform(Transform2D(angle, pos), continuous_cd_mode == PhysicsServer2D::CCD_MODE_DISABLED); + _set_inv_transform(get_transform().inverse()); + + if (continuous_cd_mode != PhysicsServer2D::CCD_MODE_DISABLED) { + new_transform = get_transform(); + } +} + +void GodotBody2D::wakeup_neighbours() { + for (const Pair &E : constraint_list) { + const GodotConstraint2D *c = E.first; + GodotBody2D **n = c->get_body_ptr(); + int bc = c->get_body_count(); + + for (int i = 0; i < bc; i++) { + if (i == E.second) { + continue; + } + GodotBody2D *b = n[i]; + if (b->mode < PhysicsServer2D::BODY_MODE_DYNAMIC) { + continue; + } + + if (!b->is_active()) { + b->set_active(true); + } + } + } +} + +void GodotBody2D::call_queries() { + if (fi_callback_data) { + if (!fi_callback_data->callable.get_object()) { + set_force_integration_callback(Callable()); + } else { + Variant direct_state_variant = get_direct_state(); + const Variant *vp[2] = { &direct_state_variant, &fi_callback_data->udata }; + + Callable::CallError ce; + Variant rv; + if (fi_callback_data->udata.get_type() != Variant::NIL) { + fi_callback_data->callable.call(vp, 2, rv, ce); + + } else { + fi_callback_data->callable.call(vp, 1, rv, ce); + } + } + } + + if (body_state_callback) { + (body_state_callback)(body_state_callback_instance, get_direct_state()); + } +} + +bool GodotBody2D::sleep_test(real_t p_step) { + if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { + return true; + } else if (!can_sleep) { + return false; + } + + if (Math::abs(angular_velocity) < get_space()->get_body_angular_velocity_sleep_threshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_threshold() * get_space()->get_body_linear_velocity_sleep_threshold()) { + still_time += p_step; + + return still_time > get_space()->get_body_time_to_sleep(); + } else { + still_time = 0; //maybe this should be set to 0 on set_active? + return false; + } +} + +void GodotBody2D::set_state_sync_callback(void *p_instance, PhysicsServer2D::BodyStateCallback p_callback) { + body_state_callback_instance = p_instance; + body_state_callback = p_callback; +} + +void GodotBody2D::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) { + if (p_callable.get_object()) { + if (!fi_callback_data) { + fi_callback_data = memnew(ForceIntegrationCallbackData); + } + fi_callback_data->callable = p_callable; + fi_callback_data->udata = p_udata; + } else if (fi_callback_data) { + memdelete(fi_callback_data); + fi_callback_data = nullptr; + } +} + +GodotPhysicsDirectBodyState2D *GodotBody2D::get_direct_state() { + if (!direct_state) { + direct_state = memnew(GodotPhysicsDirectBodyState2D); + direct_state->body = this; + } + return direct_state; +} + +GodotBody2D::GodotBody2D() : + GodotCollisionObject2D(TYPE_BODY), + active_list(this), + mass_properties_update_list(this), + direct_state_query_list(this) { + _set_static(false); +} + +GodotBody2D::~GodotBody2D() { + if (fi_callback_data) { + memdelete(fi_callback_data); + } + if (direct_state) { + memdelete(direct_state); + } +} diff --git a/servers/physics_2d/godot_body_2d.h b/servers/physics_2d/godot_body_2d.h new file mode 100644 index 0000000000..c4f3578f1b --- /dev/null +++ b/servers/physics_2d/godot_body_2d.h @@ -0,0 +1,354 @@ +/*************************************************************************/ +/* godot_body_2d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_BODY_2D_H +#define GODOT_BODY_2D_H + +#include "godot_area_2d.h" +#include "godot_collision_object_2d.h" + +#include "core/templates/list.h" +#include "core/templates/pair.h" +#include "core/templates/vset.h" + +class GodotConstraint2D; +class GodotPhysicsDirectBodyState2D; + +class GodotBody2D : public GodotCollisionObject2D { + PhysicsServer2D::BodyMode mode = PhysicsServer2D::BODY_MODE_DYNAMIC; + + Vector2 biased_linear_velocity; + real_t biased_angular_velocity = 0.0; + + Vector2 linear_velocity; + real_t angular_velocity = 0.0; + + Vector2 constant_linear_velocity; + real_t constant_angular_velocity = 0.0; + + real_t linear_damp = -1.0; + real_t angular_damp = -1.0; + real_t gravity_scale = 1.0; + + real_t bounce = 0.0; + real_t friction = 1.0; + + real_t mass = 1.0; + real_t _inv_mass = 1.0; + + real_t inertia = 0.0; + real_t _inv_inertia = 0.0; + + Vector2 center_of_mass; + + bool calculate_inertia = true; + bool calculate_center_of_mass = true; + + Vector2 gravity; + real_t area_linear_damp = 0.0; + real_t area_angular_damp = 0.0; + + real_t still_time = 0.0; + + Vector2 applied_force; + real_t applied_torque = 0.0; + + SelfList active_list; + SelfList mass_properties_update_list; + SelfList direct_state_query_list; + + VSet exceptions; + PhysicsServer2D::CCDMode continuous_cd_mode = PhysicsServer2D::CCD_MODE_DISABLED; + bool omit_force_integration = false; + bool active = true; + bool can_sleep = true; + bool first_time_kinematic = false; + void _mass_properties_changed(); + virtual void _shapes_changed(); + Transform2D new_transform; + + List> constraint_list; + + struct AreaCMP { + GodotArea2D *area = nullptr; + int refCount = 0; + _FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); } + _FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); } + _FORCE_INLINE_ AreaCMP() {} + _FORCE_INLINE_ AreaCMP(GodotArea2D *p_area) { + area = p_area; + refCount = 1; + } + }; + + Vector areas; + + struct Contact { + Vector2 local_pos; + Vector2 local_normal; + real_t depth = 0.0; + int local_shape = 0; + Vector2 collider_pos; + int collider_shape = 0; + ObjectID collider_instance_id; + RID collider; + Vector2 collider_velocity_at_pos; + }; + + Vector contacts; //no contacts by default + int contact_count = 0; + + void *body_state_callback_instance = nullptr; + PhysicsServer2D::BodyStateCallback body_state_callback = nullptr; + + struct ForceIntegrationCallbackData { + Callable callable; + Variant udata; + }; + + ForceIntegrationCallbackData *fi_callback_data = nullptr; + + GodotPhysicsDirectBodyState2D *direct_state = nullptr; + + uint64_t island_step = 0; + + _FORCE_INLINE_ void _compute_area_gravity_and_damping(const GodotArea2D *p_area); + + friend class GodotPhysicsDirectBodyState2D; // i give up, too many functions to expose + +public: + void set_state_sync_callback(void *p_instance, PhysicsServer2D::BodyStateCallback p_callback); + void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant()); + + GodotPhysicsDirectBodyState2D *get_direct_state(); + + _FORCE_INLINE_ void add_area(GodotArea2D *p_area) { + int index = areas.find(AreaCMP(p_area)); + if (index > -1) { + areas.write[index].refCount += 1; + } else { + areas.ordered_insert(AreaCMP(p_area)); + } + } + + _FORCE_INLINE_ void remove_area(GodotArea2D *p_area) { + int index = areas.find(AreaCMP(p_area)); + if (index > -1) { + areas.write[index].refCount -= 1; + if (areas[index].refCount < 1) { + areas.remove(index); + } + } + } + + _FORCE_INLINE_ void set_max_contacts_reported(int p_size) { + contacts.resize(p_size); + contact_count = 0; + if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC && p_size) { + set_active(true); + } + } + + _FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); } + + _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.is_empty(); } + _FORCE_INLINE_ void add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos); + + _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); } + _FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); } + _FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); } + _FORCE_INLINE_ const VSet &get_exceptions() const { return exceptions; } + + _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } + _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } + + _FORCE_INLINE_ void add_constraint(GodotConstraint2D *p_constraint, int p_pos) { constraint_list.push_back({ p_constraint, p_pos }); } + _FORCE_INLINE_ void remove_constraint(GodotConstraint2D *p_constraint, int p_pos) { constraint_list.erase({ p_constraint, p_pos }); } + const List> &get_constraint_list() const { return constraint_list; } + _FORCE_INLINE_ void clear_constraint_list() { constraint_list.clear(); } + + _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; } + _FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; } + + _FORCE_INLINE_ void set_linear_velocity(const Vector2 &p_velocity) { linear_velocity = p_velocity; } + _FORCE_INLINE_ Vector2 get_linear_velocity() const { return linear_velocity; } + + _FORCE_INLINE_ void set_angular_velocity(real_t p_velocity) { angular_velocity = p_velocity; } + _FORCE_INLINE_ real_t get_angular_velocity() const { return angular_velocity; } + + _FORCE_INLINE_ void set_biased_linear_velocity(const Vector2 &p_velocity) { biased_linear_velocity = p_velocity; } + _FORCE_INLINE_ Vector2 get_biased_linear_velocity() const { return biased_linear_velocity; } + + _FORCE_INLINE_ void set_biased_angular_velocity(real_t p_velocity) { biased_angular_velocity = p_velocity; } + _FORCE_INLINE_ real_t get_biased_angular_velocity() const { return biased_angular_velocity; } + + _FORCE_INLINE_ void apply_central_impulse(const Vector2 &p_impulse) { + linear_velocity += p_impulse * _inv_mass; + } + + _FORCE_INLINE_ void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) { + linear_velocity += p_impulse * _inv_mass; + angular_velocity += _inv_inertia * (p_position - center_of_mass).cross(p_impulse); + } + + _FORCE_INLINE_ void apply_torque_impulse(real_t p_torque) { + angular_velocity += _inv_inertia * p_torque; + } + + _FORCE_INLINE_ void apply_bias_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) { + biased_linear_velocity += p_impulse * _inv_mass; + biased_angular_velocity += _inv_inertia * (p_position - center_of_mass).cross(p_impulse); + } + + void set_active(bool p_active); + _FORCE_INLINE_ bool is_active() const { return active; } + + _FORCE_INLINE_ void wakeup() { + if ((!get_space()) || mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { + return; + } + set_active(true); + } + + void set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value); + Variant get_param(PhysicsServer2D::BodyParameter p_param) const; + + void set_mode(PhysicsServer2D::BodyMode p_mode); + PhysicsServer2D::BodyMode get_mode() const; + + void set_state(PhysicsServer2D::BodyState p_state, const Variant &p_variant); + Variant get_state(PhysicsServer2D::BodyState p_state) const; + + void set_applied_force(const Vector2 &p_force) { applied_force = p_force; } + Vector2 get_applied_force() const { return applied_force; } + + void set_applied_torque(real_t p_torque) { applied_torque = p_torque; } + real_t get_applied_torque() const { return applied_torque; } + + _FORCE_INLINE_ void add_central_force(const Vector2 &p_force) { + applied_force += p_force; + } + + _FORCE_INLINE_ void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) { + applied_force += p_force; + applied_torque += (p_position - center_of_mass).cross(p_force); + } + + _FORCE_INLINE_ void add_torque(real_t p_torque) { + applied_torque += p_torque; + } + + _FORCE_INLINE_ void set_continuous_collision_detection_mode(PhysicsServer2D::CCDMode p_mode) { continuous_cd_mode = p_mode; } + _FORCE_INLINE_ PhysicsServer2D::CCDMode get_continuous_collision_detection_mode() const { return continuous_cd_mode; } + + void set_space(GodotSpace2D *p_space); + + void update_mass_properties(); + void reset_mass_properties(); + + _FORCE_INLINE_ Vector2 get_center_of_mass() const { return center_of_mass; } + _FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; } + _FORCE_INLINE_ real_t get_inv_inertia() const { return _inv_inertia; } + _FORCE_INLINE_ real_t get_friction() const { return friction; } + _FORCE_INLINE_ Vector2 get_gravity() const { return gravity; } + _FORCE_INLINE_ real_t get_bounce() const { return bounce; } + _FORCE_INLINE_ real_t get_linear_damp() const { return linear_damp; } + _FORCE_INLINE_ real_t get_angular_damp() const { return angular_damp; } + + void integrate_forces(real_t p_step); + void integrate_velocities(real_t p_step); + + _FORCE_INLINE_ Vector2 get_velocity_in_local_point(const Vector2 &rel_pos) const { + return linear_velocity + Vector2(-angular_velocity * rel_pos.y, angular_velocity * rel_pos.x); + } + + _FORCE_INLINE_ Vector2 get_motion() const { + if (mode > PhysicsServer2D::BODY_MODE_KINEMATIC) { + return new_transform.get_origin() - get_transform().get_origin(); + } else if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { + return get_transform().get_origin() - new_transform.get_origin(); //kinematic simulates forward + } + return Vector2(); + } + + void call_queries(); + void wakeup_neighbours(); + + bool sleep_test(real_t p_step); + + GodotBody2D(); + ~GodotBody2D(); +}; + +//add contact inline + +void GodotBody2D::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos) { + int c_max = contacts.size(); + + if (c_max == 0) { + return; + } + + Contact *c = contacts.ptrw(); + + int idx = -1; + + if (contact_count < c_max) { + idx = contact_count++; + } else { + real_t least_depth = 1e20; + int least_deep = -1; + for (int i = 0; i < c_max; i++) { + if (i == 0 || c[i].depth < least_depth) { + least_deep = i; + least_depth = c[i].depth; + } + } + + if (least_deep >= 0 && least_depth < p_depth) { + idx = least_deep; + } + if (idx == -1) { + return; //none least deepe than this + } + } + + c[idx].local_pos = p_local_pos; + c[idx].local_normal = p_local_normal; + c[idx].depth = p_depth; + c[idx].local_shape = p_local_shape; + c[idx].collider_pos = p_collider_pos; + c[idx].collider_shape = p_collider_shape; + c[idx].collider_instance_id = p_collider_instance_id; + c[idx].collider = p_collider; + c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos; +} + +#endif // GODOT_BODY_2D_H diff --git a/servers/physics_2d/godot_body_direct_state_2d.cpp b/servers/physics_2d/godot_body_direct_state_2d.cpp new file mode 100644 index 0000000000..300c302c79 --- /dev/null +++ b/servers/physics_2d/godot_body_direct_state_2d.cpp @@ -0,0 +1,178 @@ +/*************************************************************************/ +/* godot_body_direct_state_2d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_body_direct_state_2d.h" + +#include "godot_body_2d.h" +#include "godot_physics_server_2d.h" +#include "godot_space_2d.h" + +Vector2 GodotPhysicsDirectBodyState2D::get_total_gravity() const { + return body->gravity; +} + +real_t GodotPhysicsDirectBodyState2D::get_total_angular_damp() const { + return body->area_angular_damp; +} + +real_t GodotPhysicsDirectBodyState2D::get_total_linear_damp() const { + return body->area_linear_damp; +} + +Vector2 GodotPhysicsDirectBodyState2D::get_center_of_mass() const { + return body->get_center_of_mass(); +} + +real_t GodotPhysicsDirectBodyState2D::get_inverse_mass() const { + return body->get_inv_mass(); +} + +real_t GodotPhysicsDirectBodyState2D::get_inverse_inertia() const { + return body->get_inv_inertia(); +} + +void GodotPhysicsDirectBodyState2D::set_linear_velocity(const Vector2 &p_velocity) { + body->wakeup(); + body->set_linear_velocity(p_velocity); +} + +Vector2 GodotPhysicsDirectBodyState2D::get_linear_velocity() const { + return body->get_linear_velocity(); +} + +void GodotPhysicsDirectBodyState2D::set_angular_velocity(real_t p_velocity) { + body->wakeup(); + body->set_angular_velocity(p_velocity); +} + +real_t GodotPhysicsDirectBodyState2D::get_angular_velocity() const { + return body->get_angular_velocity(); +} + +void GodotPhysicsDirectBodyState2D::set_transform(const Transform2D &p_transform) { + body->set_state(PhysicsServer2D::BODY_STATE_TRANSFORM, p_transform); +} + +Transform2D GodotPhysicsDirectBodyState2D::get_transform() const { + return body->get_transform(); +} + +Vector2 GodotPhysicsDirectBodyState2D::get_velocity_at_local_position(const Vector2 &p_position) const { + return body->get_velocity_in_local_point(p_position); +} + +void GodotPhysicsDirectBodyState2D::add_central_force(const Vector2 &p_force) { + body->wakeup(); + body->add_central_force(p_force); +} + +void GodotPhysicsDirectBodyState2D::add_force(const Vector2 &p_force, const Vector2 &p_position) { + body->wakeup(); + body->add_force(p_force, p_position); +} + +void GodotPhysicsDirectBodyState2D::add_torque(real_t p_torque) { + body->wakeup(); + body->add_torque(p_torque); +} + +void GodotPhysicsDirectBodyState2D::apply_central_impulse(const Vector2 &p_impulse) { + body->wakeup(); + body->apply_central_impulse(p_impulse); +} + +void GodotPhysicsDirectBodyState2D::apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position) { + body->wakeup(); + body->apply_impulse(p_impulse, p_position); +} + +void GodotPhysicsDirectBodyState2D::apply_torque_impulse(real_t p_torque) { + body->wakeup(); + body->apply_torque_impulse(p_torque); +} + +void GodotPhysicsDirectBodyState2D::set_sleep_state(bool p_enable) { + body->set_active(!p_enable); +} + +bool GodotPhysicsDirectBodyState2D::is_sleeping() const { + return !body->is_active(); +} + +int GodotPhysicsDirectBodyState2D::get_contact_count() const { + return body->contact_count; +} + +Vector2 GodotPhysicsDirectBodyState2D::get_contact_local_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); + return body->contacts[p_contact_idx].local_pos; +} + +Vector2 GodotPhysicsDirectBodyState2D::get_contact_local_normal(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); + return body->contacts[p_contact_idx].local_normal; +} + +int GodotPhysicsDirectBodyState2D::get_contact_local_shape(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1); + return body->contacts[p_contact_idx].local_shape; +} + +RID GodotPhysicsDirectBodyState2D::get_contact_collider(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID()); + return body->contacts[p_contact_idx].collider; +} +Vector2 GodotPhysicsDirectBodyState2D::get_contact_collider_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); + return body->contacts[p_contact_idx].collider_pos; +} + +ObjectID GodotPhysicsDirectBodyState2D::get_contact_collider_id(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID()); + return body->contacts[p_contact_idx].collider_instance_id; +} + +int GodotPhysicsDirectBodyState2D::get_contact_collider_shape(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0); + return body->contacts[p_contact_idx].collider_shape; +} + +Vector2 GodotPhysicsDirectBodyState2D::get_contact_collider_velocity_at_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); + return body->contacts[p_contact_idx].collider_velocity_at_pos; +} + +PhysicsDirectSpaceState2D *GodotPhysicsDirectBodyState2D::get_space_state() { + return body->get_space()->get_direct_state(); +} + +real_t GodotPhysicsDirectBodyState2D::get_step() const { + return body->get_space()->get_last_step(); +} diff --git a/servers/physics_2d/godot_body_direct_state_2d.h b/servers/physics_2d/godot_body_direct_state_2d.h new file mode 100644 index 0000000000..2f3e8e5095 --- /dev/null +++ b/servers/physics_2d/godot_body_direct_state_2d.h @@ -0,0 +1,91 @@ +/*************************************************************************/ +/* godot_body_direct_state_2d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_BODY_DIRECT_STATE_2D_H +#define GODOT_BODY_DIRECT_STATE_2D_H + +#include "servers/physics_server_2d.h" + +class GodotBody2D; + +class GodotPhysicsDirectBodyState2D : public PhysicsDirectBodyState2D { + GDCLASS(GodotPhysicsDirectBodyState2D, PhysicsDirectBodyState2D); + +public: + GodotBody2D *body = nullptr; + + virtual Vector2 get_total_gravity() const override; + virtual real_t get_total_angular_damp() const override; + virtual real_t get_total_linear_damp() const override; + + virtual Vector2 get_center_of_mass() const override; + virtual real_t get_inverse_mass() const override; + virtual real_t get_inverse_inertia() const override; + + virtual void set_linear_velocity(const Vector2 &p_velocity) override; + virtual Vector2 get_linear_velocity() const override; + + virtual void set_angular_velocity(real_t p_velocity) override; + virtual real_t get_angular_velocity() const override; + + virtual void set_transform(const Transform2D &p_transform) override; + virtual Transform2D get_transform() const override; + + virtual Vector2 get_velocity_at_local_position(const Vector2 &p_position) const override; + + virtual void add_central_force(const Vector2 &p_force) override; + virtual void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override; + virtual void add_torque(real_t p_torque) override; + virtual void apply_central_impulse(const Vector2 &p_impulse) override; + virtual void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override; + virtual void apply_torque_impulse(real_t p_torque) override; + + virtual void set_sleep_state(bool p_enable) override; + virtual bool is_sleeping() const override; + + virtual int get_contact_count() const override; + + virtual Vector2 get_contact_local_position(int p_contact_idx) const override; + virtual Vector2 get_contact_local_normal(int p_contact_idx) const override; + virtual int get_contact_local_shape(int p_contact_idx) const override; + + virtual RID get_contact_collider(int p_contact_idx) const override; + virtual Vector2 get_contact_collider_position(int p_contact_idx) const override; + virtual ObjectID get_contact_collider_id(int p_contact_idx) const override; + virtual int get_contact_collider_shape(int p_contact_idx) const override; + + virtual Vector2 get_contact_collider_velocity_at_position(int p_contact_idx) const override; + + virtual PhysicsDirectSpaceState2D *get_space_state() override; + + virtual real_t get_step() const override; +}; + +#endif // GODOT_BODY_DIRECT_STATE_2D_H diff --git a/servers/physics_2d/godot_body_pair_2d.cpp b/servers/physics_2d/godot_body_pair_2d.cpp new file mode 100644 index 0000000000..97eeefbfe6 --- /dev/null +++ b/servers/physics_2d/godot_body_pair_2d.cpp @@ -0,0 +1,545 @@ +/*************************************************************************/ +/* godot_body_pair_2d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_body_pair_2d.h" +#include "godot_collision_solver_2d.h" +#include "godot_space_2d.h" + +#define POSITION_CORRECTION +#define ACCUMULATE_IMPULSES + +void GodotBodyPair2D::_add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self) { + GodotBodyPair2D *self = (GodotBodyPair2D *)p_self; + + self->_contact_added_callback(p_point_A, p_point_B); +} + +void GodotBodyPair2D::_contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B) { + // check if we already have the contact + + Vector2 local_A = A->get_inv_transform().basis_xform(p_point_A); + Vector2 local_B = B->get_inv_transform().basis_xform(p_point_B - offset_B); + + int new_index = contact_count; + + ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1)); + + Contact contact; + + contact.acc_normal_impulse = 0; + contact.acc_bias_impulse = 0; + contact.acc_tangent_impulse = 0; + contact.local_A = local_A; + contact.local_B = local_B; + contact.reused = true; + contact.normal = (p_point_A - p_point_B).normalized(); + contact.mass_normal = 0; // will be computed in setup() + + // attempt to determine if the contact will be reused + + real_t recycle_radius_2 = space->get_contact_recycle_radius() * space->get_contact_recycle_radius(); + + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + if ( + c.local_A.distance_squared_to(local_A) < (recycle_radius_2) && + c.local_B.distance_squared_to(local_B) < (recycle_radius_2)) { + contact.acc_normal_impulse = c.acc_normal_impulse; + contact.acc_tangent_impulse = c.acc_tangent_impulse; + contact.acc_bias_impulse = c.acc_bias_impulse; + new_index = i; + break; + } + } + + // figure out if the contact amount must be reduced to fit the new contact + + if (new_index == MAX_CONTACTS) { + // remove the contact with the minimum depth + + int least_deep = -1; + real_t min_depth = 1e10; + + const Transform2D &transform_A = A->get_transform(); + const Transform2D &transform_B = B->get_transform(); + + for (int i = 0; i <= contact_count; i++) { + Contact &c = (i == contact_count) ? contact : contacts[i]; + Vector2 global_A = transform_A.basis_xform(c.local_A); + Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; + + Vector2 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + + if (depth < min_depth) { + min_depth = depth; + least_deep = i; + } + } + + ERR_FAIL_COND(least_deep == -1); + + if (least_deep < contact_count) { //replace the last deep contact by the new one + + contacts[least_deep] = contact; + } + + return; + } + + contacts[new_index] = contact; + + if (new_index == contact_count) { + contact_count++; + } +} + +void GodotBodyPair2D::_validate_contacts() { + //make sure to erase contacts that are no longer valid + + real_t max_separation = space->get_contact_max_separation(); + real_t max_separation2 = max_separation * max_separation; + + const Transform2D &transform_A = A->get_transform(); + const Transform2D &transform_B = B->get_transform(); + + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + + bool erase = false; + if (!c.reused) { + //was left behind in previous frame + erase = true; + } else { + c.reused = false; + + Vector2 global_A = transform_A.basis_xform(c.local_A); + Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; + Vector2 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + + if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) { + erase = true; + } + } + + if (erase) { + // contact no longer needed, remove + + if ((i + 1) < contact_count) { + // swap with the last one + SWAP(contacts[i], contacts[contact_count - 1]); + } + + i--; + contact_count--; + } + } +} + +bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result) { + Vector2 motion = p_A->get_linear_velocity() * p_step; + real_t mlen = motion.length(); + if (mlen < CMP_EPSILON) { + return false; + } + + Vector2 mnormal = motion / mlen; + + real_t min, max; + p_A->get_shape(p_shape_A)->project_rangev(mnormal, p_xform_A, min, max); + bool fast_object = mlen > (max - min) * 0.3; //going too fast in that direction + + if (!fast_object) { //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis + return false; + } + + //cast a segment from support in motion normal, in the same direction of motion by motion length + //support is the worst case collision point, so real collision happened before + int a; + Vector2 s[2]; + p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(), s, a); + Vector2 from = p_xform_A.xform(s[0]); + Vector2 to = from + motion; + + Transform2D from_inv = p_xform_B.affine_inverse(); + + Vector2 local_from = from_inv.xform(from - mnormal * mlen * 0.1); //start from a little inside the bounding box + Vector2 local_to = from_inv.xform(to); + + Vector2 rpos, rnorm; + if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) { + return false; + } + + //ray hit something + + Vector2 hitpos = p_xform_B.xform(rpos); + + Vector2 contact_A = to; + Vector2 contact_B = hitpos; + + //create a contact + + if (p_swap_result) { + _contact_added_callback(contact_B, contact_A); + } else { + _contact_added_callback(contact_A, contact_B); + } + + return true; +} + +real_t combine_bounce(GodotBody2D *A, GodotBody2D *B) { + return CLAMP(A->get_bounce() + B->get_bounce(), 0, 1); +} + +real_t combine_friction(GodotBody2D *A, GodotBody2D *B) { + return ABS(MIN(A->get_friction(), B->get_friction())); +} + +bool GodotBodyPair2D::setup(real_t p_step) { + if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) { + collided = false; + return false; + } + + collide_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && A->collides_with(B); + collide_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && B->collides_with(A); + + report_contacts_only = false; + if (!collide_A && !collide_B) { + if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) { + report_contacts_only = true; + } else { + collided = false; + return false; + } + } + + //use local A coordinates to avoid numerical issues on collision detection + offset_B = B->get_transform().get_origin() - A->get_transform().get_origin(); + + _validate_contacts(); + + const Vector2 &offset_A = A->get_transform().get_origin(); + Transform2D xform_Au = A->get_transform().untranslated(); + Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A); + + Transform2D xform_Bu = B->get_transform(); + xform_Bu.elements[2] -= offset_A; + Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B); + + GodotShape2D *shape_A_ptr = A->get_shape(shape_A); + GodotShape2D *shape_B_ptr = B->get_shape(shape_B); + + Vector2 motion_A, motion_B; + + if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_SHAPE) { + motion_A = A->get_motion(); + } + if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_SHAPE) { + motion_B = B->get_motion(); + } + + bool prev_collided = collided; + + collided = GodotCollisionSolver2D::solve(shape_A_ptr, xform_A, motion_A, shape_B_ptr, xform_B, motion_B, _add_contact, this, &sep_axis); + if (!collided) { + //test ccd (currently just a raycast) + + if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) { + if (_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B)) { + collided = true; + } + } + + if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_B) { + if (_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A, true)) { + collided = true; + } + } + + if (!collided) { + oneway_disabled = false; + return false; + } + } + + if (oneway_disabled) { + return false; + } + + if (!prev_collided) { + if (shape_B_ptr->allows_one_way_collision() && A->is_shape_set_as_one_way_collision(shape_A)) { + Vector2 direction = xform_A.get_axis(1).normalized(); + bool valid = false; + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + if (!c.reused) { + continue; + } + if (c.normal.dot(direction) > -CMP_EPSILON) { //greater (normal inverted) + continue; + } + valid = true; + break; + } + if (!valid) { + collided = false; + oneway_disabled = true; + return false; + } + } + + if (shape_A_ptr->allows_one_way_collision() && B->is_shape_set_as_one_way_collision(shape_B)) { + Vector2 direction = xform_B.get_axis(1).normalized(); + bool valid = false; + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + if (!c.reused) { + continue; + } + if (c.normal.dot(direction) < CMP_EPSILON) { //less (normal ok) + continue; + } + valid = true; + break; + } + if (!valid) { + collided = false; + oneway_disabled = true; + return false; + } + } + } + + return true; +} + +bool GodotBodyPair2D::pre_solve(real_t p_step) { + if (!collided || oneway_disabled) { + return false; + } + + real_t max_penetration = space->get_contact_max_allowed_penetration(); + + real_t bias = 0.3; + + GodotShape2D *shape_A_ptr = A->get_shape(shape_A); + GodotShape2D *shape_B_ptr = B->get_shape(shape_B); + + if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) { + if (shape_A_ptr->get_custom_bias() == 0) { + bias = shape_B_ptr->get_custom_bias(); + } else if (shape_B_ptr->get_custom_bias() == 0) { + bias = shape_A_ptr->get_custom_bias(); + } else { + bias = (shape_B_ptr->get_custom_bias() + shape_A_ptr->get_custom_bias()) * 0.5; + } + } + + real_t inv_dt = 1.0 / p_step; + + bool do_process = false; + + const Vector2 &offset_A = A->get_transform().get_origin(); + const Transform2D &transform_A = A->get_transform(); + const Transform2D &transform_B = B->get_transform(); + + real_t inv_inertia_A = collide_A ? A->get_inv_inertia() : 0.0; + real_t inv_inertia_B = collide_B ? B->get_inv_inertia() : 0.0; + + real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0; + real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0; + + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + c.active = false; + + Vector2 global_A = transform_A.basis_xform(c.local_A); + Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; + + Vector2 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + + if (depth <= 0.0 || !c.reused) { + continue; + } + +#ifdef DEBUG_ENABLED + if (space->is_debugging_contacts()) { + space->add_debug_contact(global_A + offset_A); + space->add_debug_contact(global_B + offset_A); + } +#endif + + c.rA = global_A; + c.rB = global_B - offset_B; + + if (A->can_report_contacts()) { + Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x); + A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, global_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB + B->get_linear_velocity()); + } + + if (B->can_report_contacts()) { + Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x); + B->add_contact(global_B + offset_A, c.normal, depth, shape_B, global_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA + A->get_linear_velocity()); + } + + if (report_contacts_only) { + collided = false; + continue; + } + + // Precompute normal mass, tangent mass, and bias. + real_t rnA = c.rA.dot(c.normal); + real_t rnB = c.rB.dot(c.normal); + real_t kNormal = inv_mass_A + inv_mass_B; + kNormal += inv_inertia_A * (c.rA.dot(c.rA) - rnA * rnA) + inv_inertia_B * (c.rB.dot(c.rB) - rnB * rnB); + c.mass_normal = 1.0f / kNormal; + + Vector2 tangent = c.normal.orthogonal(); + real_t rtA = c.rA.dot(tangent); + real_t rtB = c.rB.dot(tangent); + real_t kTangent = inv_mass_A + inv_mass_B; + kTangent += inv_inertia_A * (c.rA.dot(c.rA) - rtA * rtA) + inv_inertia_B * (c.rB.dot(c.rB) - rtB * rtB); + c.mass_tangent = 1.0f / kTangent; + + c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration); + c.depth = depth; + //c.acc_bias_impulse=0; + +#ifdef ACCUMULATE_IMPULSES + { + // Apply normal + friction impulse + Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent; + + if (collide_A) { + A->apply_impulse(-P, c.rA); + } + if (collide_B) { + B->apply_impulse(P, c.rB); + } + } +#endif + + c.bounce = combine_bounce(A, B); + if (c.bounce) { + Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x); + Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x); + Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; + c.bounce = c.bounce * dv.dot(c.normal); + } + + c.active = true; + do_process = true; + } + + return do_process; +} + +void GodotBodyPair2D::solve(real_t p_step) { + if (!collided || oneway_disabled) { + return; + } + + for (int i = 0; i < contact_count; ++i) { + Contact &c = contacts[i]; + + if (!c.active) { + continue; + } + + // Relative velocity at contact + + Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x); + Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x); + Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; + + Vector2 crbA(-A->get_biased_angular_velocity() * c.rA.y, A->get_biased_angular_velocity() * c.rA.x); + Vector2 crbB(-B->get_biased_angular_velocity() * c.rB.y, B->get_biased_angular_velocity() * c.rB.x); + Vector2 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA; + + real_t vn = dv.dot(c.normal); + real_t vbn = dbv.dot(c.normal); + Vector2 tangent = c.normal.orthogonal(); + real_t vt = dv.dot(tangent); + + real_t jbn = (c.bias - vbn) * c.mass_normal; + real_t jbnOld = c.acc_bias_impulse; + c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f); + + Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld); + + if (collide_A) { + A->apply_bias_impulse(-jb, c.rA); + } + if (collide_B) { + B->apply_bias_impulse(jb, c.rB); + } + + real_t jn = -(c.bounce + vn) * c.mass_normal; + real_t jnOld = c.acc_normal_impulse; + c.acc_normal_impulse = MAX(jnOld + jn, 0.0f); + + real_t friction = combine_friction(A, B); + + real_t jtMax = friction * c.acc_normal_impulse; + real_t jt = -vt * c.mass_tangent; + real_t jtOld = c.acc_tangent_impulse; + c.acc_tangent_impulse = CLAMP(jtOld + jt, -jtMax, jtMax); + + Vector2 j = c.normal * (c.acc_normal_impulse - jnOld) + tangent * (c.acc_tangent_impulse - jtOld); + + if (collide_A) { + A->apply_impulse(-j, c.rA); + } + if (collide_B) { + B->apply_impulse(j, c.rB); + } + } +} + +GodotBodyPair2D::GodotBodyPair2D(GodotBody2D *p_A, int p_shape_A, GodotBody2D *p_B, int p_shape_B) : + GodotConstraint2D(_arr, 2) { + A = p_A; + B = p_B; + shape_A = p_shape_A; + shape_B = p_shape_B; + space = A->get_space(); + A->add_constraint(this, 0); + B->add_constraint(this, 1); +} + +GodotBodyPair2D::~GodotBodyPair2D() { + A->remove_constraint(this, 0); + B->remove_constraint(this, 1); +} diff --git a/servers/physics_2d/godot_body_pair_2d.h b/servers/physics_2d/godot_body_pair_2d.h new file mode 100644 index 0000000000..0938ab542b --- /dev/null +++ b/servers/physics_2d/godot_body_pair_2d.h @@ -0,0 +1,98 @@ +/*************************************************************************/ +/* godot_body_pair_2d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_BODY_PAIR_2D_H +#define GODOT_BODY_PAIR_2D_H + +#include "godot_body_2d.h" +#include "godot_constraint_2d.h" + +class GodotBodyPair2D : public GodotConstraint2D { + enum { + MAX_CONTACTS = 2 + }; + union { + struct { + GodotBody2D *A; + GodotBody2D *B; + }; + + GodotBody2D *_arr[2] = { nullptr, nullptr }; + }; + + int shape_A = 0; + int shape_B = 0; + + bool collide_A = false; + bool collide_B = false; + + GodotSpace2D *space = nullptr; + + struct Contact { + Vector2 position; + Vector2 normal; + Vector2 local_A, local_B; + real_t acc_normal_impulse = 0.0; // accumulated normal impulse (Pn) + real_t acc_tangent_impulse = 0.0; // accumulated tangent impulse (Pt) + real_t acc_bias_impulse = 0.0; // accumulated normal impulse for position bias (Pnb) + real_t mass_normal, mass_tangent = 0.0; + real_t bias = 0.0; + + real_t depth = 0.0; + bool active = false; + Vector2 rA, rB; + bool reused = false; + real_t bounce = 0.0; + }; + + Vector2 offset_B; //use local A coordinates to avoid numerical issues on collision detection + + Vector2 sep_axis; + Contact contacts[MAX_CONTACTS]; + int contact_count = 0; + bool collided = false; + bool oneway_disabled = false; + bool report_contacts_only = false; + + bool _test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result = false); + void _validate_contacts(); + static void _add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self); + _FORCE_INLINE_ void _contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B); + +public: + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + GodotBodyPair2D(GodotBody2D *p_A, int p_shape_A, GodotBody2D *p_B, int p_shape_B); + ~GodotBodyPair2D(); +}; + +#endif // GODOT_BODY_PAIR_2D_H diff --git a/servers/physics_2d/godot_broad_phase_2d.cpp b/servers/physics_2d/godot_broad_phase_2d.cpp new file mode 100644 index 0000000000..4b35f8d996 --- /dev/null +++ b/servers/physics_2d/godot_broad_phase_2d.cpp @@ -0,0 +1,36 @@ +/*************************************************************************/ +/* godot_broad_phase_2d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_broad_phase_2d.h" + +GodotBroadPhase2D::CreateFunction GodotBroadPhase2D::create_func = nullptr; + +GodotBroadPhase2D::~GodotBroadPhase2D() { +} diff --git a/servers/physics_2d/godot_broad_phase_2d.h b/servers/physics_2d/godot_broad_phase_2d.h new file mode 100644 index 0000000000..7017a6e41f --- /dev/null +++ b/servers/physics_2d/godot_broad_phase_2d.h @@ -0,0 +1,71 @@ +/*************************************************************************/ +/* godot_broad_phase_2d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_BROAD_PHASE_2D_H +#define GODOT_BROAD_PHASE_2D_H + +#include "core/math/math_funcs.h" +#include "core/math/rect2.h" + +class GodotCollisionObject2D; + +class GodotBroadPhase2D { +public: + typedef GodotBroadPhase2D *(*CreateFunction)(); + + static CreateFunction create_func; + + typedef uint32_t ID; + + typedef void *(*PairCallback)(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_userdata); + typedef void (*UnpairCallback)(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_data, void *p_userdata); + + // 0 is an invalid ID + virtual ID create(GodotCollisionObject2D *p_object_, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false) = 0; + virtual void move(ID p_id, const Rect2 &p_aabb) = 0; + virtual void set_static(ID p_id, bool p_static) = 0; + virtual void remove(ID p_id) = 0; + + virtual GodotCollisionObject2D *get_object(ID p_id) const = 0; + virtual bool is_static(ID p_id) const = 0; + virtual int get_subindex(ID p_id) const = 0; + + virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; + virtual int cull_aabb(const Rect2 &p_aabb, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; + + virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata) = 0; + virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) = 0; + + virtual void update() = 0; + + virtual ~GodotBroadPhase2D(); +}; + +#endif // GODOT_BROAD_PHASE_2D_H diff --git a/servers/physics_2d/godot_broad_phase_2d_bvh.cpp b/servers/physics_2d/godot_broad_phase_2d_bvh.cpp new file mode 100644 index 0000000000..9ec6b0a6b7 --- /dev/null +++ b/servers/physics_2d/godot_broad_phase_2d_bvh.cpp @@ -0,0 +1,113 @@ +/*************************************************************************/ +/* godot_broad_phase_2d_bvh.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_broad_phase_2d_bvh.h" +#include "godot_collision_object_2d.h" + +GodotBroadPhase2D::ID GodotBroadPhase2DBVH::create(GodotCollisionObject2D *p_object, int p_subindex, const Rect2 &p_aabb, bool p_static) { + ID oid = bvh.create(p_object, true, p_aabb, p_subindex, !p_static, 1 << p_object->get_type(), p_static ? 0 : 0xFFFFF); // Pair everything, don't care? + return oid + 1; +} + +void GodotBroadPhase2DBVH::move(ID p_id, const Rect2 &p_aabb) { + bvh.move(p_id - 1, p_aabb); +} + +void GodotBroadPhase2DBVH::set_static(ID p_id, bool p_static) { + GodotCollisionObject2D *it = bvh.get(p_id - 1); + bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF, false); // Pair everything, don't care? +} + +void GodotBroadPhase2DBVH::remove(ID p_id) { + bvh.erase(p_id - 1); +} + +GodotCollisionObject2D *GodotBroadPhase2DBVH::get_object(ID p_id) const { + GodotCollisionObject2D *it = bvh.get(p_id - 1); + ERR_FAIL_COND_V(!it, nullptr); + return it; +} + +bool GodotBroadPhase2DBVH::is_static(ID p_id) const { + return !bvh.is_pairable(p_id - 1); +} + +int GodotBroadPhase2DBVH::get_subindex(ID p_id) const { + return bvh.get_subindex(p_id - 1); +} + +int GodotBroadPhase2DBVH::cull_segment(const Vector2 &p_from, const Vector2 &p_to, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices) { + return bvh.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices); +} + +int GodotBroadPhase2DBVH::cull_aabb(const Rect2 &p_aabb, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices) { + return bvh.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices); +} + +void *GodotBroadPhase2DBVH::_pair_callback(void *self, uint32_t p_A, GodotCollisionObject2D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject2D *p_object_B, int subindex_B) { + GodotBroadPhase2DBVH *bpo = (GodotBroadPhase2DBVH *)(self); + if (!bpo->pair_callback) { + return nullptr; + } + + return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata); +} + +void GodotBroadPhase2DBVH::_unpair_callback(void *self, uint32_t p_A, GodotCollisionObject2D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject2D *p_object_B, int subindex_B, void *pairdata) { + GodotBroadPhase2DBVH *bpo = (GodotBroadPhase2DBVH *)(self); + if (!bpo->unpair_callback) { + return; + } + + bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata); +} + +void GodotBroadPhase2DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { + pair_callback = p_pair_callback; + pair_userdata = p_userdata; +} + +void GodotBroadPhase2DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { + unpair_callback = p_unpair_callback; + unpair_userdata = p_userdata; +} + +void GodotBroadPhase2DBVH::update() { + bvh.update(); +} + +GodotBroadPhase2D *GodotBroadPhase2DBVH::_create() { + return memnew(GodotBroadPhase2DBVH); +} + +GodotBroadPhase2DBVH::GodotBroadPhase2DBVH() { + bvh.set_pair_callback(_pair_callback, this); + bvh.set_unpair_callback(_unpair_callback, this); +} diff --git a/servers/physics_2d/godot_broad_phase_2d_bvh.h b/servers/physics_2d/godot_broad_phase_2d_bvh.h new file mode 100644 index 0000000000..19b49f3499 --- /dev/null +++ b/servers/physics_2d/godot_broad_phase_2d_bvh.h @@ -0,0 +1,74 @@ +/*************************************************************************/ +/* godot_broad_phase_2d_bvh.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_BROAD_PHASE_2D_BVH_H +#define GODOT_BROAD_PHASE_2D_BVH_H + +#include "godot_broad_phase_2d.h" + +#include "core/math/bvh.h" +#include "core/math/rect2.h" +#include "core/math/vector2.h" + +class GodotBroadPhase2DBVH : public GodotBroadPhase2D { + BVH_Manager bvh; + + static void *_pair_callback(void *, uint32_t, GodotCollisionObject2D *, int, uint32_t, GodotCollisionObject2D *, int); + static void _unpair_callback(void *, uint32_t, GodotCollisionObject2D *, int, uint32_t, GodotCollisionObject2D *, int, void *); + + PairCallback pair_callback = nullptr; + void *pair_userdata = nullptr; + UnpairCallback unpair_callback = nullptr; + void *unpair_userdata = nullptr; + +public: + // 0 is an invalid ID + virtual ID create(GodotCollisionObject2D *p_object, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false); + virtual void move(ID p_id, const Rect2 &p_aabb); + virtual void set_static(ID p_id, bool p_static); + virtual void remove(ID p_id); + + virtual GodotCollisionObject2D *get_object(ID p_id) const; + virtual bool is_static(ID p_id) const; + virtual int get_subindex(ID p_id) const; + + virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr); + virtual int cull_aabb(const Rect2 &p_aabb, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr); + + virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata); + virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata); + + virtual void update(); + + static GodotBroadPhase2D *_create(); + GodotBroadPhase2DBVH(); +}; + +#endif // GODOT_BROAD_PHASE_2D_BVH_H diff --git a/servers/physics_2d/godot_collision_object_2d.cpp b/servers/physics_2d/godot_collision_object_2d.cpp new file mode 100644 index 0000000000..3d4ebbedcd --- /dev/null +++ b/servers/physics_2d/godot_collision_object_2d.cpp @@ -0,0 +1,243 @@ +/*************************************************************************/ +/* godot_collision_object_2d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_collision_object_2d.h" +#include "godot_physics_server_2d.h" +#include "godot_space_2d.h" + +void GodotCollisionObject2D::add_shape(GodotShape2D *p_shape, const Transform2D &p_transform, bool p_disabled) { + Shape s; + s.shape = p_shape; + s.xform = p_transform; + s.xform_inv = s.xform.affine_inverse(); + s.bpid = 0; //needs update + s.disabled = p_disabled; + s.one_way_collision = false; + s.one_way_collision_margin = 0; + shapes.push_back(s); + p_shape->add_owner(this); + + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } +} + +void GodotCollisionObject2D::set_shape(int p_index, GodotShape2D *p_shape) { + ERR_FAIL_INDEX(p_index, shapes.size()); + shapes[p_index].shape->remove_owner(this); + shapes.write[p_index].shape = p_shape; + + p_shape->add_owner(this); + + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } +} + +void GodotCollisionObject2D::set_shape_transform(int p_index, const Transform2D &p_transform) { + ERR_FAIL_INDEX(p_index, shapes.size()); + + shapes.write[p_index].xform = p_transform; + shapes.write[p_index].xform_inv = p_transform.affine_inverse(); + + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } +} + +void GodotCollisionObject2D::set_shape_disabled(int p_idx, bool p_disabled) { + ERR_FAIL_INDEX(p_idx, shapes.size()); + + GodotCollisionObject2D::Shape &shape = shapes.write[p_idx]; + if (shape.disabled == p_disabled) { + return; + } + + shape.disabled = p_disabled; + + if (!space) { + return; + } + + if (p_disabled && shape.bpid != 0) { + space->get_broadphase()->remove(shape.bpid); + shape.bpid = 0; + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } + } else if (!p_disabled && shape.bpid == 0) { + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } + } +} + +void GodotCollisionObject2D::remove_shape(GodotShape2D *p_shape) { + //remove a shape, all the times it appears + for (int i = 0; i < shapes.size(); i++) { + if (shapes[i].shape == p_shape) { + remove_shape(i); + i--; + } + } +} + +void GodotCollisionObject2D::remove_shape(int p_index) { + //remove anything from shape to be erased to end, so subindices don't change + ERR_FAIL_INDEX(p_index, shapes.size()); + for (int i = p_index; i < shapes.size(); i++) { + if (shapes[i].bpid == 0) { + continue; + } + //should never get here with a null owner + space->get_broadphase()->remove(shapes[i].bpid); + shapes.write[i].bpid = 0; + } + shapes[p_index].shape->remove_owner(this); + shapes.remove(p_index); + + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } + // _update_shapes(); + // _shapes_changed(); +} + +void GodotCollisionObject2D::_set_static(bool p_static) { + if (_static == p_static) { + return; + } + _static = p_static; + + if (!space) { + return; + } + for (int i = 0; i < get_shape_count(); i++) { + const Shape &s = shapes[i]; + if (s.bpid > 0) { + space->get_broadphase()->set_static(s.bpid, _static); + } + } +} + +void GodotCollisionObject2D::_unregister_shapes() { + for (int i = 0; i < shapes.size(); i++) { + Shape &s = shapes.write[i]; + if (s.bpid > 0) { + space->get_broadphase()->remove(s.bpid); + s.bpid = 0; + } + } +} + +void GodotCollisionObject2D::_update_shapes() { + if (!space) { + return; + } + + for (int i = 0; i < shapes.size(); i++) { + Shape &s = shapes.write[i]; + if (s.disabled) { + continue; + } + + //not quite correct, should compute the next matrix.. + Rect2 shape_aabb = s.shape->get_aabb(); + Transform2D xform = transform * s.xform; + shape_aabb = xform.xform(shape_aabb); + shape_aabb.grow_by((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05); + s.aabb_cache = shape_aabb; + + if (s.bpid == 0) { + s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); + space->get_broadphase()->set_static(s.bpid, _static); + } + + space->get_broadphase()->move(s.bpid, shape_aabb); + } +} + +void GodotCollisionObject2D::_update_shapes_with_motion(const Vector2 &p_motion) { + if (!space) { + return; + } + + for (int i = 0; i < shapes.size(); i++) { + Shape &s = shapes.write[i]; + if (s.disabled) { + continue; + } + + //not quite correct, should compute the next matrix.. + Rect2 shape_aabb = s.shape->get_aabb(); + Transform2D xform = transform * s.xform; + shape_aabb = xform.xform(shape_aabb); + shape_aabb = shape_aabb.merge(Rect2(shape_aabb.position + p_motion, shape_aabb.size)); //use motion + s.aabb_cache = shape_aabb; + + if (s.bpid == 0) { + s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); + space->get_broadphase()->set_static(s.bpid, _static); + } + + space->get_broadphase()->move(s.bpid, shape_aabb); + } +} + +void GodotCollisionObject2D::_set_space(GodotSpace2D *p_space) { + if (space) { + space->remove_object(this); + + for (int i = 0; i < shapes.size(); i++) { + Shape &s = shapes.write[i]; + if (s.bpid) { + space->get_broadphase()->remove(s.bpid); + s.bpid = 0; + } + } + } + + space = p_space; + + if (space) { + space->add_object(this); + _update_shapes(); + } +} + +void GodotCollisionObject2D::_shape_changed() { + _update_shapes(); + _shapes_changed(); +} + +GodotCollisionObject2D::GodotCollisionObject2D(Type p_type) : + pending_shape_update_list(this) { + type = p_type; +} diff --git a/servers/physics_2d/godot_collision_object_2d.h b/servers/physics_2d/godot_collision_object_2d.h new file mode 100644 index 0000000000..7233857808 --- /dev/null +++ b/servers/physics_2d/godot_collision_object_2d.h @@ -0,0 +1,190 @@ +/*************************************************************************/ +/* godot_collision_object_2d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_COLLISION_OBJECT_2D_H +#define GODOT_COLLISION_OBJECT_2D_H + +#include "godot_broad_phase_2d.h" +#include "godot_shape_2d.h" + +#include "core/templates/self_list.h" +#include "servers/physics_server_2d.h" + +class GodotSpace2D; + +class GodotCollisionObject2D : public GodotShapeOwner2D { +public: + enum Type { + TYPE_AREA, + TYPE_BODY + }; + +private: + Type type; + RID self; + ObjectID instance_id; + ObjectID canvas_instance_id; + bool pickable = true; + + struct Shape { + Transform2D xform; + Transform2D xform_inv; + GodotBroadPhase2D::ID bpid = 0; + Rect2 aabb_cache; //for rayqueries + GodotShape2D *shape = nullptr; + bool disabled = false; + bool one_way_collision = false; + real_t one_way_collision_margin = 0.0; + }; + + Vector shapes; + GodotSpace2D *space = nullptr; + Transform2D transform; + Transform2D inv_transform; + uint32_t collision_mask = 1; + uint32_t collision_layer = 1; + bool _static = true; + + SelfList pending_shape_update_list; + + void _update_shapes(); + +protected: + void _update_shapes_with_motion(const Vector2 &p_motion); + void _unregister_shapes(); + + _FORCE_INLINE_ void _set_transform(const Transform2D &p_transform, bool p_update_shapes = true) { + transform = p_transform; + if (p_update_shapes) { + _update_shapes(); + } + } + _FORCE_INLINE_ void _set_inv_transform(const Transform2D &p_transform) { inv_transform = p_transform; } + void _set_static(bool p_static); + + virtual void _shapes_changed() = 0; + void _set_space(GodotSpace2D *p_space); + + GodotCollisionObject2D(Type p_type); + +public: + _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + _FORCE_INLINE_ void set_instance_id(const ObjectID &p_instance_id) { instance_id = p_instance_id; } + _FORCE_INLINE_ ObjectID get_instance_id() const { return instance_id; } + + _FORCE_INLINE_ void set_canvas_instance_id(const ObjectID &p_canvas_instance_id) { canvas_instance_id = p_canvas_instance_id; } + _FORCE_INLINE_ ObjectID get_canvas_instance_id() const { return canvas_instance_id; } + + void _shape_changed(); + + _FORCE_INLINE_ Type get_type() const { return type; } + void add_shape(GodotShape2D *p_shape, const Transform2D &p_transform = Transform2D(), bool p_disabled = false); + void set_shape(int p_index, GodotShape2D *p_shape); + void set_shape_transform(int p_index, const Transform2D &p_transform); + + _FORCE_INLINE_ int get_shape_count() const { return shapes.size(); } + _FORCE_INLINE_ GodotShape2D *get_shape(int p_index) const { + CRASH_BAD_INDEX(p_index, shapes.size()); + return shapes[p_index].shape; + } + _FORCE_INLINE_ const Transform2D &get_shape_transform(int p_index) const { + CRASH_BAD_INDEX(p_index, shapes.size()); + return shapes[p_index].xform; + } + _FORCE_INLINE_ const Transform2D &get_shape_inv_transform(int p_index) const { + CRASH_BAD_INDEX(p_index, shapes.size()); + return shapes[p_index].xform_inv; + } + _FORCE_INLINE_ const Rect2 &get_shape_aabb(int p_index) const { + CRASH_BAD_INDEX(p_index, shapes.size()); + return shapes[p_index].aabb_cache; + } + + _FORCE_INLINE_ const Transform2D &get_transform() const { return transform; } + _FORCE_INLINE_ const Transform2D &get_inv_transform() const { return inv_transform; } + _FORCE_INLINE_ GodotSpace2D *get_space() const { return space; } + + void set_shape_disabled(int p_idx, bool p_disabled); + _FORCE_INLINE_ bool is_shape_disabled(int p_idx) const { + ERR_FAIL_INDEX_V(p_idx, shapes.size(), false); + return shapes[p_idx].disabled; + } + + _FORCE_INLINE_ void set_shape_as_one_way_collision(int p_idx, bool p_one_way_collision, real_t p_margin) { + CRASH_BAD_INDEX(p_idx, shapes.size()); + shapes.write[p_idx].one_way_collision = p_one_way_collision; + shapes.write[p_idx].one_way_collision_margin = p_margin; + } + _FORCE_INLINE_ bool is_shape_set_as_one_way_collision(int p_idx) const { + CRASH_BAD_INDEX(p_idx, shapes.size()); + return shapes[p_idx].one_way_collision; + } + + _FORCE_INLINE_ real_t get_shape_one_way_collision_margin(int p_idx) const { + CRASH_BAD_INDEX(p_idx, shapes.size()); + return shapes[p_idx].one_way_collision_margin; + } + + void set_collision_mask(uint32_t p_mask) { + collision_mask = p_mask; + _shape_changed(); + } + _FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; } + + void set_collision_layer(uint32_t p_layer) { + collision_layer = p_layer; + _shape_changed(); + } + _FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; } + + void remove_shape(GodotShape2D *p_shape); + void remove_shape(int p_index); + + virtual void set_space(GodotSpace2D *p_space) = 0; + + _FORCE_INLINE_ bool is_static() const { return _static; } + + void set_pickable(bool p_pickable) { pickable = p_pickable; } + _FORCE_INLINE_ bool is_pickable() const { return pickable; } + + _FORCE_INLINE_ bool collides_with(GodotCollisionObject2D *p_other) const { + return p_other->collision_layer & collision_mask; + } + + _FORCE_INLINE_ bool interacts_with(GodotCollisionObject2D *p_other) const { + return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask; + } + + virtual ~GodotCollisionObject2D() {} +}; + +#endif // GODOT_COLLISION_OBJECT_2D_H diff --git a/servers/physics_2d/godot_collision_solver_2d.cpp b/servers/physics_2d/godot_collision_solver_2d.cpp new file mode 100644 index 0000000000..25371b9885 --- /dev/null +++ b/servers/physics_2d/godot_collision_solver_2d.cpp @@ -0,0 +1,264 @@ +/*************************************************************************/ +/* godot_collision_solver_2d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_collision_solver_2d.h" +#include "godot_collision_solver_2d_sat.h" + +#define collision_solver sat_2d_calculate_penetration +//#define collision_solver gjk_epa_calculate_penetration + +bool GodotCollisionSolver2D::solve_static_world_boundary(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { + const GodotWorldBoundaryShape2D *world_boundary = static_cast(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) { + return false; + } + + Vector2 n = p_transform_A.basis_xform(world_boundary->get_normal()).normalized(); + Vector2 p = p_transform_A.xform(world_boundary->get_normal() * world_boundary->get_d()); + real_t d = n.dot(p); + + Vector2 supports[2]; + int support_count; + + p_shape_B->get_supports(p_transform_B.affine_inverse().basis_xform(-n).normalized(), supports, support_count); + + bool found = false; + + for (int i = 0; i < support_count; i++) { + supports[i] = p_transform_B.xform(supports[i]); + real_t pd = n.dot(supports[i]); + if (pd >= d) { + continue; + } + found = true; + + Vector2 support_A = supports[i] - n * (pd - d); + + if (p_result_callback) { + if (p_swap_result) { + p_result_callback(supports[i], support_A, p_userdata); + } else { + p_result_callback(support_A, supports[i], p_userdata); + } + } + } + + return found; +} + +bool GodotCollisionSolver2D::solve_separation_ray(const GodotShape2D *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis, real_t p_margin) { + const GodotSeparationRayShape2D *ray = static_cast(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY) { + return false; + } + + Vector2 from = p_transform_A.get_origin(); + Vector2 to = from + p_transform_A[1] * (ray->get_length() + p_margin); + if (p_motion_A != Vector2()) { + //not the best but should be enough + Vector2 normal = (to - from).normalized(); + to += normal * MAX(0.0, normal.dot(p_motion_A)); + } + Vector2 support_A = to; + + Transform2D invb = p_transform_B.affine_inverse(); + from = invb.xform(from); + to = invb.xform(to); + + Vector2 p, n; + if (!p_shape_B->intersect_segment(from, to, p, n)) { + if (r_sep_axis) { + *r_sep_axis = p_transform_A[1].normalized(); + } + return false; + } + + // Discard contacts when the ray is fully contained inside the shape. + if (n == Vector2()) { + if (r_sep_axis) { + *r_sep_axis = p_transform_A[1].normalized(); + } + return false; + } + + // Discard contacts in the wrong direction. + if (n.dot(from - to) < CMP_EPSILON) { + if (r_sep_axis) { + *r_sep_axis = p_transform_A[1].normalized(); + } + return false; + } + + Vector2 support_B = p_transform_B.xform(p); + if (ray->get_slide_on_slope()) { + Vector2 global_n = invb.basis_xform_inv(n).normalized(); + support_B = support_A + (support_B - support_A).length() * global_n; + } + + if (p_result_callback) { + if (p_swap_result) { + p_result_callback(support_B, support_A, p_userdata); + } else { + p_result_callback(support_A, support_B, p_userdata); + } + } + return true; +} + +struct _ConcaveCollisionInfo2D { + const Transform2D *transform_A = nullptr; + const GodotShape2D *shape_A = nullptr; + const Transform2D *transform_B = nullptr; + Vector2 motion_A; + Vector2 motion_B; + real_t margin_A = 0.0; + real_t margin_B = 0.0; + GodotCollisionSolver2D::CallbackResult result_callback; + void *userdata = nullptr; + bool swap_result = false; + bool collided = false; + int aabb_tests = 0; + int collisions = 0; + Vector2 *sep_axis = nullptr; +}; + +bool GodotCollisionSolver2D::concave_callback(void *p_userdata, GodotShape2D *p_convex) { + _ConcaveCollisionInfo2D &cinfo = *(_ConcaveCollisionInfo2D *)(p_userdata); + cinfo.aabb_tests++; + + bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex, *cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, cinfo.sep_axis, cinfo.margin_A, cinfo.margin_B); + if (!collided) { + return false; + } + + cinfo.collided = true; + cinfo.collisions++; + + // Stop at first collision if contacts are not needed. + return !cinfo.result_callback; +} + +bool GodotCollisionSolver2D::solve_concave(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { + const GodotConcaveShape2D *concave_B = static_cast(p_shape_B); + + _ConcaveCollisionInfo2D cinfo; + cinfo.transform_A = &p_transform_A; + cinfo.shape_A = p_shape_A; + cinfo.transform_B = &p_transform_B; + cinfo.motion_A = p_motion_A; + cinfo.result_callback = p_result_callback; + cinfo.userdata = p_userdata; + cinfo.swap_result = p_swap_result; + cinfo.collided = false; + cinfo.collisions = 0; + cinfo.sep_axis = r_sep_axis; + cinfo.margin_A = p_margin_A; + cinfo.margin_B = p_margin_B; + + cinfo.aabb_tests = 0; + + Transform2D rel_transform = p_transform_A; + rel_transform.elements[2] -= p_transform_B.get_origin(); + + //quickly compute a local Rect2 + + Rect2 local_aabb; + for (int i = 0; i < 2; i++) { + Vector2 axis(p_transform_B.elements[i]); + real_t axis_scale = 1.0 / axis.length(); + axis *= axis_scale; + + real_t smin, smax; + p_shape_A->project_rangev(axis, rel_transform, smin, smax); + smin *= axis_scale; + smax *= axis_scale; + + local_aabb.position[i] = smin; + local_aabb.size[i] = smax - smin; + } + + concave_B->cull(local_aabb, concave_callback, &cinfo); + + return cinfo.collided; +} + +bool GodotCollisionSolver2D::solve(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { + PhysicsServer2D::ShapeType type_A = p_shape_A->get_type(); + PhysicsServer2D::ShapeType type_B = p_shape_B->get_type(); + bool concave_A = p_shape_A->is_concave(); + bool concave_B = p_shape_B->is_concave(); + real_t margin_A = p_margin_A, margin_B = p_margin_B; + + bool swap = false; + + if (type_A > type_B) { + SWAP(type_A, type_B); + SWAP(concave_A, concave_B); + SWAP(margin_A, margin_B); + swap = true; + } + + if (type_A == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) { + if (type_B == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) { + return false; + } + + if (swap) { + return solve_static_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); + } else { + return solve_static_world_boundary(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); + } + + } else if (type_A == PhysicsServer2D::SHAPE_SEPARATION_RAY) { + if (type_B == PhysicsServer2D::SHAPE_SEPARATION_RAY) { + return false; //no ray-ray + } + + if (swap) { + return solve_separation_ray(p_shape_B, p_motion_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, r_sep_axis, p_margin_B); + } else { + return solve_separation_ray(p_shape_A, p_motion_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, r_sep_axis, p_margin_A); + } + + } else if (concave_B) { + if (concave_A) { + return false; + } + + if (!swap) { + return solve_concave(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, r_sep_axis, margin_A, margin_B); + } else { + return solve_concave(p_shape_B, p_transform_B, p_motion_B, p_shape_A, p_transform_A, p_motion_A, p_result_callback, p_userdata, true, r_sep_axis, margin_A, margin_B); + } + + } else { + return collision_solver(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, r_sep_axis, margin_A, margin_B); + } +} diff --git a/servers/physics_2d/godot_collision_solver_2d.h b/servers/physics_2d/godot_collision_solver_2d.h new file mode 100644 index 0000000000..f10815a444 --- /dev/null +++ b/servers/physics_2d/godot_collision_solver_2d.h @@ -0,0 +1,50 @@ +/*************************************************************************/ +/* godot_collision_solver_2d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_COLLISION_SOLVER_2D_H +#define GODOT_COLLISION_SOLVER_2D_H + +#include "godot_shape_2d.h" + +class GodotCollisionSolver2D { +public: + typedef void (*CallbackResult)(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata); + +private: + static bool solve_static_world_boundary(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool concave_callback(void *p_userdata, GodotShape2D *p_convex); + static bool solve_concave(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); + static bool solve_separation_ray(const GodotShape2D *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis = nullptr, real_t p_margin = 0); + +public: + static bool solve(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); +}; + +#endif // GODOT_COLLISION_SOLVER_2D_H diff --git a/servers/physics_2d/godot_collision_solver_2d_sat.cpp b/servers/physics_2d/godot_collision_solver_2d_sat.cpp new file mode 100644 index 0000000000..63053e8259 --- /dev/null +++ b/servers/physics_2d/godot_collision_solver_2d_sat.cpp @@ -0,0 +1,1408 @@ +/*************************************************************************/ +/* godot_collision_solver_2d_sat.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_collision_solver_2d_sat.h" + +#include "core/math/geometry_2d.h" + +struct _CollectorCallback2D { + GodotCollisionSolver2D::CallbackResult callback; + void *userdata = nullptr; + bool swap = false; + bool collided = false; + Vector2 normal; + Vector2 *sep_axis = nullptr; + + _FORCE_INLINE_ void call(const Vector2 &p_point_A, const Vector2 &p_point_B) { + /* + if (normal.dot(p_point_A) >= normal.dot(p_point_B)) + return; + */ + if (swap) { + callback(p_point_B, p_point_A, userdata); + } else { + callback(p_point_A, p_point_B, userdata); + } + } +}; + +typedef void (*GenerateContactsFunc)(const Vector2 *, int, const Vector2 *, int, _CollectorCallback2D *); + +_FORCE_INLINE_ static void _generate_contacts_point_point(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 1); + ERR_FAIL_COND(p_point_count_B != 1); +#endif + + p_collector->call(*p_points_A, *p_points_B); +} + +_FORCE_INLINE_ static void _generate_contacts_point_edge(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 1); + ERR_FAIL_COND(p_point_count_B != 2); +#endif + + Vector2 closest_B = Geometry2D::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B); + p_collector->call(*p_points_A, closest_B); +} + +struct _generate_contacts_Pair { + bool a = false; + int idx = 0; + real_t d = 0.0; + _FORCE_INLINE_ bool operator<(const _generate_contacts_Pair &l) const { return d < l.d; } +}; + +_FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 2); + ERR_FAIL_COND(p_point_count_B != 2); // circle is actually a 4x3 matrix +#endif + + Vector2 n = p_collector->normal; + Vector2 t = n.orthogonal(); + real_t dA = n.dot(p_points_A[0]); + real_t dB = n.dot(p_points_B[0]); + + _generate_contacts_Pair dvec[4]; + + dvec[0].d = t.dot(p_points_A[0]); + dvec[0].a = true; + dvec[0].idx = 0; + dvec[1].d = t.dot(p_points_A[1]); + dvec[1].a = true; + dvec[1].idx = 1; + dvec[2].d = t.dot(p_points_B[0]); + dvec[2].a = false; + dvec[2].idx = 0; + dvec[3].d = t.dot(p_points_B[1]); + dvec[3].a = false; + dvec[3].idx = 1; + + SortArray<_generate_contacts_Pair> sa; + sa.sort(dvec, 4); + + for (int i = 1; i <= 2; i++) { + if (dvec[i].a) { + Vector2 a = p_points_A[dvec[i].idx]; + Vector2 b = n.plane_project(dB, a); + if (n.dot(a) > n.dot(b) - CMP_EPSILON) { + continue; + } + p_collector->call(a, b); + } else { + Vector2 b = p_points_B[dvec[i].idx]; + Vector2 a = n.plane_project(dA, b); + if (n.dot(a) > n.dot(b) - CMP_EPSILON) { + continue; + } + p_collector->call(a, b); + } + } +} + +static void _generate_contacts_from_supports(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A < 1); + ERR_FAIL_COND(p_point_count_B < 1); +#endif + + static const GenerateContactsFunc generate_contacts_func_table[2][2] = { + { + _generate_contacts_point_point, + _generate_contacts_point_edge, + }, + { + nullptr, + _generate_contacts_edge_edge, + } + }; + + int pointcount_B = 0; + int pointcount_A = 0; + const Vector2 *points_A = nullptr; + const Vector2 *points_B = nullptr; + + if (p_point_count_A > p_point_count_B) { + //swap + p_collector->swap = !p_collector->swap; + p_collector->normal = -p_collector->normal; + + pointcount_B = p_point_count_A; + pointcount_A = p_point_count_B; + points_A = p_points_B; + points_B = p_points_A; + } else { + pointcount_B = p_point_count_B; + pointcount_A = p_point_count_A; + points_A = p_points_A; + points_B = p_points_B; + } + + int version_A = (pointcount_A > 2 ? 2 : pointcount_A) - 1; + int version_B = (pointcount_B > 2 ? 2 : pointcount_B) - 1; + + GenerateContactsFunc contacts_func = generate_contacts_func_table[version_A][version_B]; + ERR_FAIL_COND(!contacts_func); + contacts_func(points_A, pointcount_A, points_B, pointcount_B, p_collector); +} + +template +class SeparatorAxisTest2D { + const ShapeA *shape_A = nullptr; + const ShapeB *shape_B = nullptr; + const Transform2D *transform_A = nullptr; + const Transform2D *transform_B = nullptr; + real_t best_depth = 1e15; + Vector2 best_axis; +#ifdef DEBUG_ENABLED + int best_axis_count = 0; + int best_axis_index = -1; +#endif + Vector2 motion_A; + Vector2 motion_B; + real_t margin_A = 0.0; + real_t margin_B = 0.0; + _CollectorCallback2D *callback; + +public: + _FORCE_INLINE_ bool test_previous_axis() { + if (callback && callback->sep_axis && *callback->sep_axis != Vector2()) { + return test_axis(*callback->sep_axis); + } else { +#ifdef DEBUG_ENABLED + best_axis_count++; +#endif + } + return true; + } + + _FORCE_INLINE_ bool test_cast() { + if (castA) { + Vector2 na = motion_A.normalized(); + if (!test_axis(na)) { + return false; + } + if (!test_axis(na.orthogonal())) { + return false; + } + } + + if (castB) { + Vector2 nb = motion_B.normalized(); + if (!test_axis(nb)) { + return false; + } + if (!test_axis(nb.orthogonal())) { + return false; + } + } + + return true; + } + + _FORCE_INLINE_ bool test_axis(const Vector2 &p_axis) { + Vector2 axis = p_axis; + + if (Math::is_zero_approx(axis.x) && + Math::is_zero_approx(axis.y)) { + // strange case, try an upwards separator + axis = Vector2(0.0, 1.0); + } + + real_t min_A, max_A, min_B, max_B; + + if (castA) { + shape_A->project_range_cast(motion_A, axis, *transform_A, min_A, max_A); + } else { + shape_A->project_range(axis, *transform_A, min_A, max_A); + } + + if (castB) { + shape_B->project_range_cast(motion_B, axis, *transform_B, min_B, max_B); + } else { + shape_B->project_range(axis, *transform_B, min_B, max_B); + } + + if (withMargin) { + min_A -= margin_A; + max_A += margin_A; + min_B -= margin_B; + max_B += margin_B; + } + + min_B -= (max_A - min_A) * 0.5; + max_B += (max_A - min_A) * 0.5; + + real_t dmin = min_B - (min_A + max_A) * 0.5; + real_t dmax = max_B - (min_A + max_A) * 0.5; + + if (dmin > 0.0 || dmax < 0.0) { + if (callback && callback->sep_axis) { + *callback->sep_axis = axis; + } +#ifdef DEBUG_ENABLED + best_axis_count++; +#endif + + return false; // doesn't contain 0 + } + + //use the smallest depth + + dmin = Math::abs(dmin); + + if (dmax < dmin) { + if (dmax < best_depth) { + best_depth = dmax; + best_axis = axis; +#ifdef DEBUG_ENABLED + best_axis_index = best_axis_count; +#endif + } + } else { + if (dmin < best_depth) { + best_depth = dmin; + best_axis = -axis; // keep it as A axis +#ifdef DEBUG_ENABLED + best_axis_index = best_axis_count; +#endif + } + } + +#ifdef DEBUG_ENABLED + best_axis_count++; +#endif + + return true; + } + + _FORCE_INLINE_ void generate_contacts() { + // nothing to do, don't generate + if (best_axis == Vector2(0.0, 0.0)) { + return; + } + + if (callback) { + callback->collided = true; + + if (!callback->callback) { + return; //only collide, no callback + } + } + static const int max_supports = 2; + + Vector2 supports_A[max_supports]; + int support_count_A; + if (castA) { + shape_A->get_supports_transformed_cast(motion_A, -best_axis, *transform_A, supports_A, support_count_A); + } else { + shape_A->get_supports(transform_A->basis_xform_inv(-best_axis).normalized(), supports_A, support_count_A); + for (int i = 0; i < support_count_A; i++) { + supports_A[i] = transform_A->xform(supports_A[i]); + } + } + + if (withMargin) { + for (int i = 0; i < support_count_A; i++) { + supports_A[i] += -best_axis * margin_A; + } + } + + Vector2 supports_B[max_supports]; + int support_count_B; + if (castB) { + shape_B->get_supports_transformed_cast(motion_B, best_axis, *transform_B, supports_B, support_count_B); + } else { + shape_B->get_supports(transform_B->basis_xform_inv(best_axis).normalized(), supports_B, support_count_B); + for (int i = 0; i < support_count_B; i++) { + supports_B[i] = transform_B->xform(supports_B[i]); + } + } + + if (withMargin) { + for (int i = 0; i < support_count_B; i++) { + supports_B[i] += best_axis * margin_B; + } + } + if (callback) { + callback->normal = best_axis; + _generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback); + + if (callback->sep_axis && *callback->sep_axis != Vector2()) { + *callback->sep_axis = Vector2(); //invalidate previous axis (no test) + } + } + } + + _FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A, const Transform2D &p_transform_a, const ShapeB *p_shape_B, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_A = Vector2(), const Vector2 &p_motion_B = Vector2(), real_t p_margin_A = 0, real_t p_margin_B = 0) { + margin_A = p_margin_A; + margin_B = p_margin_B; + shape_A = p_shape_A; + shape_B = p_shape_B; + transform_A = &p_transform_a; + transform_B = &p_transform_b; + motion_A = p_motion_A; + motion_B = p_motion_B; + callback = p_collector; + } +}; + +/****** SAT TESTS *******/ + +#define TEST_POINT(m_a, m_b) \ + ((!separator.test_axis(((m_a) - (m_b)).normalized())) || \ + (castA && !separator.test_axis(((m_a) + p_motion_a - (m_b)).normalized())) || \ + (castB && !separator.test_axis(((m_a) - ((m_b) + p_motion_b)).normalized())) || \ + (castA && castB && !separator.test_axis(((m_a) + p_motion_a - ((m_b) + p_motion_b)).normalized()))) + +typedef void (*CollisionFunc)(const GodotShape2D *, const Transform2D &, const GodotShape2D *, const Transform2D &, _CollectorCallback2D *p_collector, const Vector2 &, const Vector2 &, real_t, real_t); + +template +static void _collision_segment_segment(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotSegmentShape2D *segment_A = static_cast(p_a); + const GodotSegmentShape2D *segment_B = static_cast(p_b); + + SeparatorAxisTest2D separator(segment_A, p_transform_a, segment_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + //this collision is kind of pointless + + if (!separator.test_cast()) { + return; + } + + if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { + return; + } + if (!separator.test_axis(segment_B->get_xformed_normal(p_transform_b))) { + return; + } + + if (withMargin) { + //points grow to circles + + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_a()))) { + return; + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_b()))) { + return; + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_a()))) { + return; + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_b()))) { + return; + } + } + + separator.generate_contacts(); +} + +template +static void _collision_segment_circle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotSegmentShape2D *segment_A = static_cast(p_a); + const GodotCircleShape2D *circle_B = static_cast(p_b); + + SeparatorAxisTest2D separator(segment_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //segment normal + if (!separator.test_axis( + (p_transform_a.xform(segment_A->get_b()) - p_transform_a.xform(segment_A->get_a())).normalized().orthogonal())) { + return; + } + + //endpoint a vs circle + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.get_origin())) { + return; + } + //endpoint b vs circle + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.get_origin())) { + return; + } + + separator.generate_contacts(); +} + +template +static void _collision_segment_rectangle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotSegmentShape2D *segment_A = static_cast(p_a); + const GodotRectangleShape2D *rectangle_B = static_cast(p_b); + + SeparatorAxisTest2D separator(segment_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { + return; + } + + if (!separator.test_axis(p_transform_b.elements[0].normalized())) { + return; + } + + if (!separator.test_axis(p_transform_b.elements[1].normalized())) { + return; + } + + if (withMargin) { + Transform2D inv = p_transform_b.affine_inverse(); + + Vector2 a = p_transform_a.xform(segment_A->get_a()); + Vector2 b = p_transform_a.xform(segment_A->get_b()); + + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a))) { + return; + } + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b))) { + return; + } + + if (castA) { + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a + p_motion_a))) { + return; + } + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b + p_motion_a))) { + return; + } + } + + if (castB) { + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b))) { + return; + } + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b))) { + return; + } + } + + if (castA && castB) { + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b + p_motion_a))) { + return; + } + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b + p_motion_a))) { + return; + } + } + } + + separator.generate_contacts(); +} + +template +static void _collision_segment_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotSegmentShape2D *segment_A = static_cast(p_a); + const GodotCapsuleShape2D *capsule_B = static_cast(p_b); + + SeparatorAxisTest2D separator(segment_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { + return; + } + + if (!separator.test_axis(p_transform_b.elements[0].normalized())) { + return; + } + + real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); + + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) { + return; + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) { + return; + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) { + return; + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) { + return; + } + + separator.generate_contacts(); +} + +template +static void _collision_segment_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotSegmentShape2D *segment_A = static_cast(p_a); + const GodotConvexPolygonShape2D *convex_B = static_cast(p_b); + + SeparatorAxisTest2D separator(segment_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { + return; + } + + for (int i = 0; i < convex_B->get_point_count(); i++) { + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { + return; + } + + if (withMargin) { + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(convex_B->get_point(i)))) { + return; + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(convex_B->get_point(i)))) { + return; + } + } + } + + separator.generate_contacts(); +} + +///////// + +template +static void _collision_circle_circle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotCircleShape2D *circle_A = static_cast(p_a); + const GodotCircleShape2D *circle_B = static_cast(p_b); + + SeparatorAxisTest2D separator(circle_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.get_origin())) { + return; + } + + separator.generate_contacts(); +} + +template +static void _collision_circle_rectangle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotCircleShape2D *circle_A = static_cast(p_a); + const GodotRectangleShape2D *rectangle_B = static_cast(p_b); + + SeparatorAxisTest2D separator(circle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + const Vector2 &sphere = p_transform_a.elements[2]; + const Vector2 *axis = &p_transform_b.elements[0]; + //const Vector2& half_extents = rectangle_B->get_half_extents(); + + if (!separator.test_axis(axis[0].normalized())) { + return; + } + + if (!separator.test_axis(axis[1].normalized())) { + return; + } + + Transform2D binv = p_transform_b.affine_inverse(); + { + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphere))) { + return; + } + } + + if (castA) { + Vector2 sphereofs = sphere + p_motion_a; + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) { + return; + } + } + + if (castB) { + Vector2 sphereofs = sphere - p_motion_b; + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) { + return; + } + } + + if (castA && castB) { + Vector2 sphereofs = sphere - p_motion_b + p_motion_a; + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) { + return; + } + } + + separator.generate_contacts(); +} + +template +static void _collision_circle_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotCircleShape2D *circle_A = static_cast(p_a); + const GodotCapsuleShape2D *capsule_B = static_cast(p_b); + + SeparatorAxisTest2D separator(circle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //capsule axis + if (!separator.test_axis(p_transform_b.elements[0].normalized())) { + return; + } + + real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); + + //capsule endpoints + if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) { + return; + } + if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) { + return; + } + + separator.generate_contacts(); +} + +template +static void _collision_circle_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotCircleShape2D *circle_A = static_cast(p_a); + const GodotConvexPolygonShape2D *convex_B = static_cast(p_b); + + SeparatorAxisTest2D separator(circle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //poly faces and poly points vs circle + for (int i = 0; i < convex_B->get_point_count(); i++) { + if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.xform(convex_B->get_point(i)))) { + return; + } + + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { + return; + } + } + + separator.generate_contacts(); +} + +///////// + +template +static void _collision_rectangle_rectangle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotRectangleShape2D *rectangle_A = static_cast(p_a); + const GodotRectangleShape2D *rectangle_B = static_cast(p_b); + + SeparatorAxisTest2D separator(rectangle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //box faces A + if (!separator.test_axis(p_transform_a.elements[0].normalized())) { + return; + } + + if (!separator.test_axis(p_transform_a.elements[1].normalized())) { + return; + } + + //box faces B + if (!separator.test_axis(p_transform_b.elements[0].normalized())) { + return; + } + + if (!separator.test_axis(p_transform_b.elements[1].normalized())) { + return; + } + + if (withMargin) { + Transform2D invA = p_transform_a.affine_inverse(); + Transform2D invB = p_transform_b.affine_inverse(); + + if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, p_transform_b, invB))) { + return; + } + + if (castA || castB) { + Transform2D aofs = p_transform_a; + aofs.elements[2] += p_motion_a; + + Transform2D bofs = p_transform_b; + bofs.elements[2] += p_motion_b; + + Transform2D aofsinv = aofs.affine_inverse(); + Transform2D bofsinv = bofs.affine_inverse(); + + if (castA) { + if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, p_transform_b, invB))) { + return; + } + } + + if (castB) { + if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, bofs, bofsinv))) { + return; + } + } + + if (castA && castB) { + if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, bofs, bofsinv))) { + return; + } + } + } + } + + separator.generate_contacts(); +} + +template +static void _collision_rectangle_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotRectangleShape2D *rectangle_A = static_cast(p_a); + const GodotCapsuleShape2D *capsule_B = static_cast(p_b); + + SeparatorAxisTest2D separator(rectangle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //box faces + if (!separator.test_axis(p_transform_a.elements[0].normalized())) { + return; + } + + if (!separator.test_axis(p_transform_a.elements[1].normalized())) { + return; + } + + //capsule axis + if (!separator.test_axis(p_transform_b.elements[0].normalized())) { + return; + } + + //box endpoints to capsule circles + + Transform2D boxinv = p_transform_a.affine_inverse(); + + real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); + + for (int i = 0; i < 2; i++) { + { + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; + + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { + return; + } + } + + if (castA) { + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; + capsule_endpoint -= p_motion_a; + + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { + return; + } + } + + if (castB) { + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; + capsule_endpoint += p_motion_b; + + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { + return; + } + } + + if (castA && castB) { + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; + capsule_endpoint -= p_motion_a; + capsule_endpoint += p_motion_b; + + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { + return; + } + } + + capsule_dir *= -1.0; + } + + separator.generate_contacts(); +} + +template +static void _collision_rectangle_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotRectangleShape2D *rectangle_A = static_cast(p_a); + const GodotConvexPolygonShape2D *convex_B = static_cast(p_b); + + SeparatorAxisTest2D separator(rectangle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //box faces + if (!separator.test_axis(p_transform_a.elements[0].normalized())) { + return; + } + + if (!separator.test_axis(p_transform_a.elements[1].normalized())) { + return; + } + + //convex faces + Transform2D boxinv; + if (withMargin) { + boxinv = p_transform_a.affine_inverse(); + } + for (int i = 0; i < convex_B->get_point_count(); i++) { + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { + return; + } + + if (withMargin) { + //all points vs all points need to be tested if margin exist + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i))))) { + return; + } + if (castA) { + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) - p_motion_a))) { + return; + } + } + if (castB) { + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b))) { + return; + } + } + if (castA && castB) { + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b - p_motion_a))) { + return; + } + } + } + } + + separator.generate_contacts(); +} + +///////// + +template +static void _collision_capsule_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotCapsuleShape2D *capsule_A = static_cast(p_a); + const GodotCapsuleShape2D *capsule_B = static_cast(p_b); + + SeparatorAxisTest2D separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //capsule axis + + if (!separator.test_axis(p_transform_b.elements[0].normalized())) { + return; + } + + if (!separator.test_axis(p_transform_a.elements[0].normalized())) { + return; + } + + //capsule endpoints + + real_t capsule_dir_A = capsule_A->get_height() * 0.5 - capsule_A->get_radius(); + for (int i = 0; i < 2; i++) { + Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_dir_A; + + real_t capsule_dir_B = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); + for (int j = 0; j < 2; j++) { + Vector2 capsule_endpoint_B = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir_B; + + if (TEST_POINT(capsule_endpoint_A, capsule_endpoint_B)) { + return; + } + + capsule_dir_B *= -1.0; + } + + capsule_dir_A *= -1.0; + } + + separator.generate_contacts(); +} + +template +static void _collision_capsule_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotCapsuleShape2D *capsule_A = static_cast(p_a); + const GodotConvexPolygonShape2D *convex_B = static_cast(p_b); + + SeparatorAxisTest2D separator(capsule_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //capsule axis + + if (!separator.test_axis(p_transform_a.elements[0].normalized())) { + return; + } + + //poly vs capsule + for (int i = 0; i < convex_B->get_point_count(); i++) { + Vector2 cpoint = p_transform_b.xform(convex_B->get_point(i)); + + real_t capsule_dir = capsule_A->get_height() * 0.5 - capsule_A->get_radius(); + for (int j = 0; j < 2; j++) { + Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_dir; + + if (TEST_POINT(capsule_endpoint_A, cpoint)) { + return; + } + + capsule_dir *= -1.0; + } + + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { + return; + } + } + + separator.generate_contacts(); +} + +///////// + +template +static void _collision_convex_polygon_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotConvexPolygonShape2D *convex_A = static_cast(p_a); + const GodotConvexPolygonShape2D *convex_B = static_cast(p_b); + + SeparatorAxisTest2D separator(convex_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + for (int i = 0; i < convex_A->get_point_count(); i++) { + if (!separator.test_axis(convex_A->get_xformed_segment_normal(p_transform_a, i))) { + return; + } + } + + for (int i = 0; i < convex_B->get_point_count(); i++) { + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { + return; + } + } + + if (withMargin) { + for (int i = 0; i < convex_A->get_point_count(); i++) { + for (int j = 0; j < convex_B->get_point_count(); j++) { + if (TEST_POINT(p_transform_a.xform(convex_A->get_point(i)), p_transform_b.xform(convex_B->get_point(j)))) { + return; + } + } + } + } + + separator.generate_contacts(); +} + +//////// + +bool sat_2d_calculate_penetration(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, GodotCollisionSolver2D::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector2 *sep_axis, real_t p_margin_A, real_t p_margin_B) { + PhysicsServer2D::ShapeType type_A = p_shape_A->get_type(); + + ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_WORLD_BOUNDARY, false); + ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_SEPARATION_RAY, false); + ERR_FAIL_COND_V(p_shape_A->is_concave(), false); + + PhysicsServer2D::ShapeType type_B = p_shape_B->get_type(); + + ERR_FAIL_COND_V(type_B == PhysicsServer2D::SHAPE_WORLD_BOUNDARY, false); + ERR_FAIL_COND_V(type_B == PhysicsServer2D::SHAPE_SEPARATION_RAY, false); + ERR_FAIL_COND_V(p_shape_B->is_concave(), false); + + static const CollisionFunc collision_table[5][5] = { + { _collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon }, + { nullptr, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon }, + { nullptr, + nullptr, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule, + _collision_capsule_convex_polygon }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon } + + }; + + static const CollisionFunc collision_table_castA[5][5] = { + { _collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon }, + { nullptr, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon }, + { nullptr, + nullptr, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule, + _collision_capsule_convex_polygon }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon } + + }; + + static const CollisionFunc collision_table_castB[5][5] = { + { _collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon }, + { nullptr, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon }, + { nullptr, + nullptr, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule, + _collision_capsule_convex_polygon }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon } + + }; + + static const CollisionFunc collision_table_castA_castB[5][5] = { + { _collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon }, + { nullptr, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon }, + { nullptr, + nullptr, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule, + _collision_capsule_convex_polygon }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon } + + }; + + static const CollisionFunc collision_table_margin[5][5] = { + { _collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon }, + { nullptr, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon }, + { nullptr, + nullptr, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule, + _collision_capsule_convex_polygon }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon } + + }; + + static const CollisionFunc collision_table_castA_margin[5][5] = { + { _collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon }, + { nullptr, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon }, + { nullptr, + nullptr, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule, + _collision_capsule_convex_polygon }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon } + + }; + + static const CollisionFunc collision_table_castB_margin[5][5] = { + { _collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon }, + { nullptr, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon }, + { nullptr, + nullptr, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule, + _collision_capsule_convex_polygon }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon } + + }; + + static const CollisionFunc collision_table_castA_castB_margin[5][5] = { + { _collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon }, + { nullptr, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon }, + { nullptr, + nullptr, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule, + _collision_capsule_convex_polygon }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon } + + }; + + _CollectorCallback2D callback; + callback.callback = p_result_callback; + callback.swap = p_swap; + callback.userdata = p_userdata; + callback.collided = false; + callback.sep_axis = sep_axis; + + const GodotShape2D *A = p_shape_A; + const GodotShape2D *B = p_shape_B; + const Transform2D *transform_A = &p_transform_A; + const Transform2D *transform_B = &p_transform_B; + const Vector2 *motion_A = &p_motion_A; + const Vector2 *motion_B = &p_motion_B; + real_t margin_A = p_margin_A, margin_B = p_margin_B; + + if (type_A > type_B) { + SWAP(A, B); + SWAP(transform_A, transform_B); + SWAP(type_A, type_B); + SWAP(motion_A, motion_B); + SWAP(margin_A, margin_B); + callback.swap = !callback.swap; + } + + CollisionFunc collision_func; + + if (p_margin_A || p_margin_B) { + if (*motion_A == Vector2() && *motion_B == Vector2()) { + collision_func = collision_table_margin[type_A - 2][type_B - 2]; + } else if (*motion_A != Vector2() && *motion_B == Vector2()) { + collision_func = collision_table_castA_margin[type_A - 2][type_B - 2]; + } else if (*motion_A == Vector2() && *motion_B != Vector2()) { + collision_func = collision_table_castB_margin[type_A - 2][type_B - 2]; + } else { + collision_func = collision_table_castA_castB_margin[type_A - 2][type_B - 2]; + } + } else { + if (*motion_A == Vector2() && *motion_B == Vector2()) { + collision_func = collision_table[type_A - 2][type_B - 2]; + } else if (*motion_A != Vector2() && *motion_B == Vector2()) { + collision_func = collision_table_castA[type_A - 2][type_B - 2]; + } else if (*motion_A == Vector2() && *motion_B != Vector2()) { + collision_func = collision_table_castB[type_A - 2][type_B - 2]; + } else { + collision_func = collision_table_castA_castB[type_A - 2][type_B - 2]; + } + } + + ERR_FAIL_COND_V(!collision_func, false); + + collision_func(A, *transform_A, B, *transform_B, &callback, *motion_A, *motion_B, margin_A, margin_B); + + return callback.collided; +} diff --git a/servers/physics_2d/godot_collision_solver_2d_sat.h b/servers/physics_2d/godot_collision_solver_2d_sat.h new file mode 100644 index 0000000000..1517b90a19 --- /dev/null +++ b/servers/physics_2d/godot_collision_solver_2d_sat.h @@ -0,0 +1,38 @@ +/*************************************************************************/ +/* godot_collision_solver_2d_sat.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_COLLISION_SOLVER_2D_SAT_H +#define GODOT_COLLISION_SOLVER_2D_SAT_H + +#include "godot_collision_solver_2d.h" + +bool sat_2d_calculate_penetration(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, GodotCollisionSolver2D::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector2 *sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); + +#endif // GODOT_COLLISION_SOLVER_2D_SAT_H diff --git a/servers/physics_2d/godot_constraint_2d.h b/servers/physics_2d/godot_constraint_2d.h new file mode 100644 index 0000000000..84f975e583 --- /dev/null +++ b/servers/physics_2d/godot_constraint_2d.h @@ -0,0 +1,70 @@ +/*************************************************************************/ +/* godot_constraint_2d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_CONSTRAINT_2D_H +#define GODOT_CONSTRAINT_2D_H + +#include "godot_body_2d.h" + +class GodotConstraint2D { + GodotBody2D **_body_ptr; + int _body_count; + uint64_t island_step = 0; + bool disabled_collisions_between_bodies = true; + + RID self; + +protected: + GodotConstraint2D(GodotBody2D **p_body_ptr = nullptr, int p_body_count = 0) { + _body_ptr = p_body_ptr; + _body_count = p_body_count; + } + +public: + _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } + _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } + + _FORCE_INLINE_ GodotBody2D **get_body_ptr() const { return _body_ptr; } + _FORCE_INLINE_ int get_body_count() const { return _body_count; } + + _FORCE_INLINE_ void disable_collisions_between_bodies(const bool p_disabled) { disabled_collisions_between_bodies = p_disabled; } + _FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; } + + virtual bool setup(real_t p_step) = 0; + virtual bool pre_solve(real_t p_step) = 0; + virtual void solve(real_t p_step) = 0; + + virtual ~GodotConstraint2D() {} +}; + +#endif // GODOT_CONSTRAINT_2D_H diff --git a/servers/physics_2d/godot_joints_2d.cpp b/servers/physics_2d/godot_joints_2d.cpp new file mode 100644 index 0000000000..7c08c2f4b4 --- /dev/null +++ b/servers/physics_2d/godot_joints_2d.cpp @@ -0,0 +1,486 @@ +/*************************************************************************/ +/* godot_joints_2d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_joints_2d.h" + +#include "godot_space_2d.h" + +//based on chipmunk joint constraints + +/* Copyright (c) 2007 Scott Lembcke + * + * 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. + */ + +void GodotJoint2D::copy_settings_from(GodotJoint2D *p_joint) { + set_self(p_joint->get_self()); + set_max_force(p_joint->get_max_force()); + set_bias(p_joint->get_bias()); + set_max_bias(p_joint->get_max_bias()); + disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies()); +} + +static inline real_t k_scalar(GodotBody2D *a, GodotBody2D *b, const Vector2 &rA, const Vector2 &rB, const Vector2 &n) { + real_t value = 0.0; + + { + value += a->get_inv_mass(); + real_t rcn = (rA - a->get_center_of_mass()).cross(n); + value += a->get_inv_inertia() * rcn * rcn; + } + + if (b) { + value += b->get_inv_mass(); + real_t rcn = (rB - b->get_center_of_mass()).cross(n); + value += b->get_inv_inertia() * rcn * rcn; + } + + return value; +} + +static inline Vector2 +relative_velocity(GodotBody2D *a, GodotBody2D *b, Vector2 rA, Vector2 rB) { + Vector2 sum = a->get_linear_velocity() - (rA - a->get_center_of_mass()).orthogonal() * a->get_angular_velocity(); + if (b) { + return (b->get_linear_velocity() - (rB - b->get_center_of_mass()).orthogonal() * b->get_angular_velocity()) - sum; + } else { + return -sum; + } +} + +static inline real_t +normal_relative_velocity(GodotBody2D *a, GodotBody2D *b, Vector2 rA, Vector2 rB, Vector2 n) { + return relative_velocity(a, b, rA, rB).dot(n); +} + +bool GodotPinJoint2D::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + + GodotSpace2D *space = A->get_space(); + ERR_FAIL_COND_V(!space, false); + + rA = A->get_transform().basis_xform(anchor_A); + rB = B ? B->get_transform().basis_xform(anchor_B) : anchor_B; + + real_t B_inv_mass = B ? B->get_inv_mass() : 0.0; + + Transform2D K1; + K1[0].x = A->get_inv_mass() + B_inv_mass; + K1[1].x = 0.0f; + K1[0].y = 0.0f; + K1[1].y = A->get_inv_mass() + B_inv_mass; + + Transform2D K2; + K2[0].x = A->get_inv_inertia() * rA.y * rA.y; + K2[1].x = -A->get_inv_inertia() * rA.x * rA.y; + K2[0].y = -A->get_inv_inertia() * rA.x * rA.y; + K2[1].y = A->get_inv_inertia() * rA.x * rA.x; + + Transform2D K; + K[0] = K1[0] + K2[0]; + K[1] = K1[1] + K2[1]; + + if (B) { + Transform2D K3; + K3[0].x = B->get_inv_inertia() * rB.y * rB.y; + K3[1].x = -B->get_inv_inertia() * rB.x * rB.y; + K3[0].y = -B->get_inv_inertia() * rB.x * rB.y; + K3[1].y = B->get_inv_inertia() * rB.x * rB.x; + + K[0] += K3[0]; + K[1] += K3[1]; + } + + K[0].x += softness; + K[1].y += softness; + + M = K.affine_inverse(); + + Vector2 gA = rA + A->get_transform().get_origin(); + Vector2 gB = B ? rB + B->get_transform().get_origin() : rB; + + Vector2 delta = gB - gA; + + bias = delta * -(get_bias() == 0 ? space->get_constraint_bias() : get_bias()) * (1.0 / p_step); + + return true; +} + +inline Vector2 custom_cross(const Vector2 &p_vec, real_t p_other) { + return Vector2(p_other * p_vec.y, -p_other * p_vec.x); +} + +bool GodotPinJoint2D::pre_solve(real_t p_step) { + // Apply accumulated impulse. + if (dynamic_A) { + A->apply_impulse(-P, rA); + } + if (B && dynamic_B) { + B->apply_impulse(P, rB); + } + + return true; +} + +void GodotPinJoint2D::solve(real_t p_step) { + // compute relative velocity + Vector2 vA = A->get_linear_velocity() - custom_cross(rA - A->get_center_of_mass(), A->get_angular_velocity()); + + Vector2 rel_vel; + if (B) { + rel_vel = B->get_linear_velocity() - custom_cross(rB - B->get_center_of_mass(), B->get_angular_velocity()) - vA; + } else { + rel_vel = -vA; + } + + Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness, softness) * P); + + if (dynamic_A) { + A->apply_impulse(-impulse, rA); + } + if (B && dynamic_B) { + B->apply_impulse(impulse, rB); + } + + P += impulse; +} + +void GodotPinJoint2D::set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value) { + if (p_param == PhysicsServer2D::PIN_JOINT_SOFTNESS) { + softness = p_value; + } +} + +real_t GodotPinJoint2D::get_param(PhysicsServer2D::PinJointParam p_param) const { + if (p_param == PhysicsServer2D::PIN_JOINT_SOFTNESS) { + return softness; + } + ERR_FAIL_V(0); +} + +GodotPinJoint2D::GodotPinJoint2D(const Vector2 &p_pos, GodotBody2D *p_body_a, GodotBody2D *p_body_b) : + GodotJoint2D(_arr, p_body_b ? 2 : 1) { + A = p_body_a; + B = p_body_b; + anchor_A = p_body_a->get_inv_transform().xform(p_pos); + anchor_B = p_body_b ? p_body_b->get_inv_transform().xform(p_pos) : p_pos; + + p_body_a->add_constraint(this, 0); + if (p_body_b) { + p_body_b->add_constraint(this, 1); + } +} + +////////////////////////////////////////////// +////////////////////////////////////////////// +////////////////////////////////////////////// + +static inline void +k_tensor(GodotBody2D *a, GodotBody2D *b, Vector2 r1, Vector2 r2, Vector2 *k1, Vector2 *k2) { + // calculate mass matrix + // If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross... + real_t k11, k12, k21, k22; + real_t m_sum = a->get_inv_mass() + b->get_inv_mass(); + + // start with I*m_sum + k11 = m_sum; + k12 = 0.0f; + k21 = 0.0f; + k22 = m_sum; + + r1 -= a->get_center_of_mass(); + r2 -= b->get_center_of_mass(); + + // add the influence from r1 + real_t a_i_inv = a->get_inv_inertia(); + real_t r1xsq = r1.x * r1.x * a_i_inv; + real_t r1ysq = r1.y * r1.y * a_i_inv; + real_t r1nxy = -r1.x * r1.y * a_i_inv; + k11 += r1ysq; + k12 += r1nxy; + k21 += r1nxy; + k22 += r1xsq; + + // add the influnce from r2 + real_t b_i_inv = b->get_inv_inertia(); + real_t r2xsq = r2.x * r2.x * b_i_inv; + real_t r2ysq = r2.y * r2.y * b_i_inv; + real_t r2nxy = -r2.x * r2.y * b_i_inv; + k11 += r2ysq; + k12 += r2nxy; + k21 += r2nxy; + k22 += r2xsq; + + // invert + real_t determinant = k11 * k22 - k12 * k21; + ERR_FAIL_COND(determinant == 0.0); + + real_t det_inv = 1.0f / determinant; + *k1 = Vector2(k22 * det_inv, -k12 * det_inv); + *k2 = Vector2(-k21 * det_inv, k11 * det_inv); +} + +static _FORCE_INLINE_ Vector2 +mult_k(const Vector2 &vr, const Vector2 &k1, const Vector2 &k2) { + return Vector2(vr.dot(k1), vr.dot(k2)); +} + +bool GodotGrooveJoint2D::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + + GodotSpace2D *space = A->get_space(); + ERR_FAIL_COND_V(!space, false); + + // calculate endpoints in worldspace + Vector2 ta = A->get_transform().xform(A_groove_1); + Vector2 tb = A->get_transform().xform(A_groove_2); + + // calculate axis + Vector2 n = -(tb - ta).orthogonal().normalized(); + real_t d = ta.dot(n); + + xf_normal = n; + rB = B->get_transform().basis_xform(B_anchor); + + // calculate tangential distance along the axis of rB + real_t td = (B->get_transform().get_origin() + rB).cross(n); + // calculate clamping factor and rB + if (td <= ta.cross(n)) { + clamp = 1.0f; + rA = ta - A->get_transform().get_origin(); + } else if (td >= tb.cross(n)) { + clamp = -1.0f; + rA = tb - A->get_transform().get_origin(); + } else { + clamp = 0.0f; + //joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p); + rA = ((-n.orthogonal() * -td) + n * d) - A->get_transform().get_origin(); + } + + // Calculate mass tensor + k_tensor(A, B, rA, rB, &k1, &k2); + + // compute max impulse + jn_max = get_max_force() * p_step; + + // calculate bias velocity + //cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); + //joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias); + + Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA); + + real_t _b = get_bias(); + gbias = (delta * -(_b == 0 ? space->get_constraint_bias() : _b) * (1.0 / p_step)).limit_length(get_max_bias()); + + correct = true; + return true; +} + +bool GodotGrooveJoint2D::pre_solve(real_t p_step) { + // Apply accumulated impulse. + if (dynamic_A) { + A->apply_impulse(-jn_acc, rA); + } + if (dynamic_B) { + B->apply_impulse(jn_acc, rB); + } + + return true; +} + +void GodotGrooveJoint2D::solve(real_t p_step) { + // compute impulse + Vector2 vr = relative_velocity(A, B, rA, rB); + + Vector2 j = mult_k(gbias - vr, k1, k2); + Vector2 jOld = jn_acc; + j += jOld; + + jn_acc = (((clamp * j.cross(xf_normal)) > 0) ? j : j.project(xf_normal)).limit_length(jn_max); + + j = jn_acc - jOld; + + if (dynamic_A) { + A->apply_impulse(-j, rA); + } + if (dynamic_B) { + B->apply_impulse(j, rB); + } +} + +GodotGrooveJoint2D::GodotGrooveJoint2D(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, GodotBody2D *p_body_a, GodotBody2D *p_body_b) : + GodotJoint2D(_arr, 2) { + A = p_body_a; + B = p_body_b; + + A_groove_1 = A->get_inv_transform().xform(p_a_groove1); + A_groove_2 = A->get_inv_transform().xform(p_a_groove2); + B_anchor = B->get_inv_transform().xform(p_b_anchor); + A_groove_normal = -(A_groove_2 - A_groove_1).normalized().orthogonal(); + + A->add_constraint(this, 0); + B->add_constraint(this, 1); +} + +////////////////////////////////////////////// +////////////////////////////////////////////// +////////////////////////////////////////////// + +bool GodotDampedSpringJoint2D::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + + rA = A->get_transform().basis_xform(anchor_A); + rB = B->get_transform().basis_xform(anchor_B); + + Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA); + real_t dist = delta.length(); + + if (dist) { + n = delta / dist; + } else { + n = Vector2(); + } + + real_t k = k_scalar(A, B, rA, rB, n); + n_mass = 1.0f / k; + + target_vrn = 0.0f; + v_coef = 1.0f - Math::exp(-damping * (p_step)*k); + + // Calculate spring force. + real_t f_spring = (rest_length - dist) * stiffness; + j = n * f_spring * (p_step); + + return true; +} + +bool GodotDampedSpringJoint2D::pre_solve(real_t p_step) { + // Apply spring force. + if (dynamic_A) { + A->apply_impulse(-j, rA); + } + if (dynamic_B) { + B->apply_impulse(j, rB); + } + + return true; +} + +void GodotDampedSpringJoint2D::solve(real_t p_step) { + // compute relative velocity + real_t vrn = normal_relative_velocity(A, B, rA, rB, n) - target_vrn; + + // compute velocity loss from drag + // not 100% certain this is derived correctly, though it makes sense + real_t v_damp = -vrn * v_coef; + target_vrn = vrn + v_damp; + Vector2 j = n * v_damp * n_mass; + + if (dynamic_A) { + A->apply_impulse(-j, rA); + } + if (dynamic_B) { + B->apply_impulse(j, rB); + } +} + +void GodotDampedSpringJoint2D::set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value) { + switch (p_param) { + case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: { + rest_length = p_value; + } break; + case PhysicsServer2D::DAMPED_SPRING_DAMPING: { + damping = p_value; + } break; + case PhysicsServer2D::DAMPED_SPRING_STIFFNESS: { + stiffness = p_value; + } break; + } +} + +real_t GodotDampedSpringJoint2D::get_param(PhysicsServer2D::DampedSpringParam p_param) const { + switch (p_param) { + case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: { + return rest_length; + } break; + case PhysicsServer2D::DAMPED_SPRING_DAMPING: { + return damping; + } break; + case PhysicsServer2D::DAMPED_SPRING_STIFFNESS: { + return stiffness; + } break; + } + + ERR_FAIL_V(0); +} + +GodotDampedSpringJoint2D::GodotDampedSpringJoint2D(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, GodotBody2D *p_body_a, GodotBody2D *p_body_b) : + GodotJoint2D(_arr, 2) { + A = p_body_a; + B = p_body_b; + anchor_A = A->get_inv_transform().xform(p_anchor_a); + anchor_B = B->get_inv_transform().xform(p_anchor_b); + + rest_length = p_anchor_a.distance_to(p_anchor_b); + + A->add_constraint(this, 0); + B->add_constraint(this, 1); +} diff --git a/servers/physics_2d/godot_joints_2d.h b/servers/physics_2d/godot_joints_2d.h new file mode 100644 index 0000000000..4c97190d01 --- /dev/null +++ b/servers/physics_2d/godot_joints_2d.h @@ -0,0 +1,178 @@ +/*************************************************************************/ +/* godot_joints_2d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_JOINTS_2D_H +#define GODOT_JOINTS_2D_H + +#include "godot_body_2d.h" +#include "godot_constraint_2d.h" + +class GodotJoint2D : public GodotConstraint2D { + real_t bias = 0; + real_t max_bias = 3.40282e+38; + real_t max_force = 3.40282e+38; + +protected: + bool dynamic_A = false; + bool dynamic_B = false; + +public: + _FORCE_INLINE_ void set_max_force(real_t p_force) { max_force = p_force; } + _FORCE_INLINE_ real_t get_max_force() const { return max_force; } + + _FORCE_INLINE_ void set_bias(real_t p_bias) { bias = p_bias; } + _FORCE_INLINE_ real_t get_bias() const { return bias; } + + _FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias = p_bias; } + _FORCE_INLINE_ real_t get_max_bias() const { return max_bias; } + + virtual bool setup(real_t p_step) override { return false; } + virtual bool pre_solve(real_t p_step) override { return false; } + virtual void solve(real_t p_step) override {} + + void copy_settings_from(GodotJoint2D *p_joint); + + virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_MAX; } + GodotJoint2D(GodotBody2D **p_body_ptr = nullptr, int p_body_count = 0) : + GodotConstraint2D(p_body_ptr, p_body_count) {} + + virtual ~GodotJoint2D() { + for (int i = 0; i < get_body_count(); i++) { + GodotBody2D *body = get_body_ptr()[i]; + if (body) { + body->remove_constraint(this, i); + } + } + }; +}; + +class GodotPinJoint2D : public GodotJoint2D { + union { + struct { + GodotBody2D *A; + GodotBody2D *B; + }; + + GodotBody2D *_arr[2] = { nullptr, nullptr }; + }; + + Transform2D M; + Vector2 rA, rB; + Vector2 anchor_A; + Vector2 anchor_B; + Vector2 bias; + Vector2 P; + real_t softness = 0.0; + +public: + virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_PIN; } + + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + void set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value); + real_t get_param(PhysicsServer2D::PinJointParam p_param) const; + + GodotPinJoint2D(const Vector2 &p_pos, GodotBody2D *p_body_a, GodotBody2D *p_body_b = nullptr); +}; + +class GodotGrooveJoint2D : public GodotJoint2D { + union { + struct { + GodotBody2D *A; + GodotBody2D *B; + }; + + GodotBody2D *_arr[2] = { nullptr, nullptr }; + }; + + Vector2 A_groove_1; + Vector2 A_groove_2; + Vector2 A_groove_normal; + Vector2 B_anchor; + Vector2 jn_acc; + Vector2 gbias; + real_t jn_max = 0.0; + real_t clamp = 0.0; + Vector2 xf_normal; + Vector2 rA, rB; + Vector2 k1, k2; + + bool correct = false; + +public: + virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_GROOVE; } + + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + GodotGrooveJoint2D(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, GodotBody2D *p_body_a, GodotBody2D *p_body_b); +}; + +class GodotDampedSpringJoint2D : public GodotJoint2D { + union { + struct { + GodotBody2D *A; + GodotBody2D *B; + }; + + GodotBody2D *_arr[2] = { nullptr, nullptr }; + }; + + Vector2 anchor_A; + Vector2 anchor_B; + + real_t rest_length = 0.0; + real_t damping = 1.5; + real_t stiffness = 20.0; + + Vector2 rA, rB; + Vector2 n; + Vector2 j; + real_t n_mass = 0.0; + real_t target_vrn = 0.0; + real_t v_coef = 0.0; + +public: + virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_DAMPED_SPRING; } + + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + void set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value); + real_t get_param(PhysicsServer2D::DampedSpringParam p_param) const; + + GodotDampedSpringJoint2D(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, GodotBody2D *p_body_a, GodotBody2D *p_body_b); +}; + +#endif // GODOT_JOINTS_2D_H diff --git a/servers/physics_2d/godot_physics_server_2d.cpp b/servers/physics_2d/godot_physics_server_2d.cpp new file mode 100644 index 0000000000..c86f87fc03 --- /dev/null +++ b/servers/physics_2d/godot_physics_server_2d.cpp @@ -0,0 +1,1350 @@ +/*************************************************************************/ +/* godot_physics_server_2d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_physics_server_2d.h" + +#include "godot_body_direct_state_2d.h" +#include "godot_broad_phase_2d_bvh.h" +#include "godot_collision_solver_2d.h" + +#include "core/config/project_settings.h" +#include "core/debugger/engine_debugger.h" +#include "core/os/os.h" + +#define FLUSH_QUERY_CHECK(m_object) \ + ERR_FAIL_COND_MSG(m_object->get_space() && flushing_queries, "Can't change this state while flushing queries. Use call_deferred() or set_deferred() to change monitoring state instead."); + +RID GodotPhysicsServer2D::_shape_create(ShapeType p_shape) { + GodotShape2D *shape = nullptr; + switch (p_shape) { + case SHAPE_WORLD_BOUNDARY: { + shape = memnew(GodotWorldBoundaryShape2D); + } break; + case SHAPE_SEPARATION_RAY: { + shape = memnew(GodotSeparationRayShape2D); + } break; + case SHAPE_SEGMENT: { + shape = memnew(GodotSegmentShape2D); + } break; + case SHAPE_CIRCLE: { + shape = memnew(GodotCircleShape2D); + } break; + case SHAPE_RECTANGLE: { + shape = memnew(GodotRectangleShape2D); + } break; + case SHAPE_CAPSULE: { + shape = memnew(GodotCapsuleShape2D); + } break; + case SHAPE_CONVEX_POLYGON: { + shape = memnew(GodotConvexPolygonShape2D); + } break; + case SHAPE_CONCAVE_POLYGON: { + shape = memnew(GodotConcavePolygonShape2D); + } break; + case SHAPE_CUSTOM: { + ERR_FAIL_V(RID()); + + } break; + } + + RID id = shape_owner.make_rid(shape); + shape->set_self(id); + + return id; +} + +RID GodotPhysicsServer2D::world_boundary_shape_create() { + return _shape_create(SHAPE_WORLD_BOUNDARY); +} + +RID GodotPhysicsServer2D::separation_ray_shape_create() { + return _shape_create(SHAPE_SEPARATION_RAY); +} + +RID GodotPhysicsServer2D::segment_shape_create() { + return _shape_create(SHAPE_SEGMENT); +} + +RID GodotPhysicsServer2D::circle_shape_create() { + return _shape_create(SHAPE_CIRCLE); +} + +RID GodotPhysicsServer2D::rectangle_shape_create() { + return _shape_create(SHAPE_RECTANGLE); +} + +RID GodotPhysicsServer2D::capsule_shape_create() { + return _shape_create(SHAPE_CAPSULE); +} + +RID GodotPhysicsServer2D::convex_polygon_shape_create() { + return _shape_create(SHAPE_CONVEX_POLYGON); +} + +RID GodotPhysicsServer2D::concave_polygon_shape_create() { + return _shape_create(SHAPE_CONCAVE_POLYGON); +} + +void GodotPhysicsServer2D::shape_set_data(RID p_shape, const Variant &p_data) { + GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND(!shape); + shape->set_data(p_data); +}; + +void GodotPhysicsServer2D::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) { + GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND(!shape); + shape->set_custom_bias(p_bias); +} + +PhysicsServer2D::ShapeType GodotPhysicsServer2D::shape_get_type(RID p_shape) const { + const GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND_V(!shape, SHAPE_CUSTOM); + return shape->get_type(); +}; + +Variant GodotPhysicsServer2D::shape_get_data(RID p_shape) const { + const GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND_V(!shape, Variant()); + ERR_FAIL_COND_V(!shape->is_configured(), Variant()); + return shape->get_data(); +}; + +real_t GodotPhysicsServer2D::shape_get_custom_solver_bias(RID p_shape) const { + const GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND_V(!shape, 0); + return shape->get_custom_bias(); +} + +void GodotPhysicsServer2D::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) { + CollCbkData *cbk = (CollCbkData *)p_userdata; + + if (cbk->max == 0) { + return; + } + + Vector2 rel_dir = (p_point_A - p_point_B); + real_t rel_length2 = rel_dir.length_squared(); + if (cbk->valid_dir != Vector2()) { + if (cbk->valid_depth < 10e20) { + if (rel_length2 > cbk->valid_depth * cbk->valid_depth || + (rel_length2 > CMP_EPSILON && cbk->valid_dir.dot(rel_dir.normalized()) < CMP_EPSILON)) { + cbk->invalid_by_dir++; + return; + } + } else { + if (rel_length2 > 0 && cbk->valid_dir.dot(rel_dir.normalized()) < CMP_EPSILON) { + return; + } + } + } + + if (cbk->amount == cbk->max) { + //find least deep + real_t min_depth = 1e20; + int min_depth_idx = 0; + for (int i = 0; i < cbk->amount; i++) { + real_t d = cbk->ptr[i * 2 + 0].distance_squared_to(cbk->ptr[i * 2 + 1]); + if (d < min_depth) { + min_depth = d; + min_depth_idx = i; + } + } + + if (rel_length2 < min_depth) { + return; + } + cbk->ptr[min_depth_idx * 2 + 0] = p_point_A; + cbk->ptr[min_depth_idx * 2 + 1] = p_point_B; + cbk->passed++; + + } else { + cbk->ptr[cbk->amount * 2 + 0] = p_point_A; + cbk->ptr[cbk->amount * 2 + 1] = p_point_B; + cbk->amount++; + cbk->passed++; + } +} + +bool GodotPhysicsServer2D::shape_collide(RID p_shape_A, const Transform2D &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Transform2D &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) { + GodotShape2D *shape_A = shape_owner.get_or_null(p_shape_A); + ERR_FAIL_COND_V(!shape_A, false); + GodotShape2D *shape_B = shape_owner.get_or_null(p_shape_B); + ERR_FAIL_COND_V(!shape_B, false); + + if (p_result_max == 0) { + return GodotCollisionSolver2D::solve(shape_A, p_xform_A, p_motion_A, shape_B, p_xform_B, p_motion_B, nullptr, nullptr); + } + + CollCbkData cbk; + cbk.max = p_result_max; + cbk.amount = 0; + cbk.passed = 0; + cbk.ptr = r_results; + + bool res = GodotCollisionSolver2D::solve(shape_A, p_xform_A, p_motion_A, shape_B, p_xform_B, p_motion_B, _shape_col_cbk, &cbk); + r_result_count = cbk.amount; + return res; +} + +RID GodotPhysicsServer2D::space_create() { + GodotSpace2D *space = memnew(GodotSpace2D); + RID id = space_owner.make_rid(space); + space->set_self(id); + RID area_id = area_create(); + GodotArea2D *area = area_owner.get_or_null(area_id); + ERR_FAIL_COND_V(!area, RID()); + space->set_default_area(area); + area->set_space(space); + area->set_priority(-1); + + return id; +}; + +void GodotPhysicsServer2D::space_set_active(RID p_space, bool p_active) { + GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND(!space); + if (p_active) { + active_spaces.insert(space); + } else { + active_spaces.erase(space); + } +} + +bool GodotPhysicsServer2D::space_is_active(RID p_space) const { + const GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND_V(!space, false); + + return active_spaces.has(space); +} + +void GodotPhysicsServer2D::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) { + GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND(!space); + + space->set_param(p_param, p_value); +} + +real_t GodotPhysicsServer2D::space_get_param(RID p_space, SpaceParameter p_param) const { + const GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND_V(!space, 0); + return space->get_param(p_param); +} + +void GodotPhysicsServer2D::space_set_debug_contacts(RID p_space, int p_max_contacts) { + GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND(!space); + space->set_debug_contacts(p_max_contacts); +} + +Vector GodotPhysicsServer2D::space_get_contacts(RID p_space) const { + GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND_V(!space, Vector()); + return space->get_debug_contacts(); +} + +int GodotPhysicsServer2D::space_get_contact_count(RID p_space) const { + GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND_V(!space, 0); + return space->get_debug_contact_count(); +} + +PhysicsDirectSpaceState2D *GodotPhysicsServer2D::space_get_direct_state(RID p_space) { + GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND_V(!space, nullptr); + ERR_FAIL_COND_V_MSG((using_threads && !doing_sync) || space->is_locked(), nullptr, "Space state is inaccessible right now, wait for iteration or physics process notification."); + + return space->get_direct_state(); +} + +RID GodotPhysicsServer2D::area_create() { + GodotArea2D *area = memnew(GodotArea2D); + RID rid = area_owner.make_rid(area); + area->set_self(rid); + return rid; +}; + +void GodotPhysicsServer2D::area_set_space(RID p_area, RID p_space) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + GodotSpace2D *space = nullptr; + if (p_space.is_valid()) { + space = space_owner.get_or_null(p_space); + ERR_FAIL_COND(!space); + } + + if (area->get_space() == space) { + return; //pointless + } + + area->clear_constraints(); + area->set_space(space); +}; + +RID GodotPhysicsServer2D::area_get_space(RID p_area) const { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, RID()); + + GodotSpace2D *space = area->get_space(); + if (!space) { + return RID(); + } + return space->get_self(); +}; + +void GodotPhysicsServer2D::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + area->set_space_override_mode(p_mode); +} + +PhysicsServer2D::AreaSpaceOverrideMode GodotPhysicsServer2D::area_get_space_override_mode(RID p_area) const { + const GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, AREA_SPACE_OVERRIDE_DISABLED); + + return area->get_space_override_mode(); +} + +void GodotPhysicsServer2D::area_add_shape(RID p_area, RID p_shape, const Transform2D &p_transform, bool p_disabled) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND(!shape); + + area->add_shape(shape, p_transform, p_disabled); +} + +void GodotPhysicsServer2D::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND(!shape); + ERR_FAIL_COND(!shape->is_configured()); + + area->set_shape(p_shape_idx, shape); +} + +void GodotPhysicsServer2D::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform2D &p_transform) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + area->set_shape_transform(p_shape_idx, p_transform); +} + +void GodotPhysicsServer2D::area_set_shape_disabled(RID p_area, int p_shape, bool p_disabled) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + ERR_FAIL_INDEX(p_shape, area->get_shape_count()); + FLUSH_QUERY_CHECK(area); + + area->set_shape_disabled(p_shape, p_disabled); +} + +int GodotPhysicsServer2D::area_get_shape_count(RID p_area) const { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, -1); + + return area->get_shape_count(); +} + +RID GodotPhysicsServer2D::area_get_shape(RID p_area, int p_shape_idx) const { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, RID()); + + GodotShape2D *shape = area->get_shape(p_shape_idx); + ERR_FAIL_COND_V(!shape, RID()); + + return shape->get_self(); +} + +Transform2D GodotPhysicsServer2D::area_get_shape_transform(RID p_area, int p_shape_idx) const { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, Transform2D()); + + return area->get_shape_transform(p_shape_idx); +} + +void GodotPhysicsServer2D::area_remove_shape(RID p_area, int p_shape_idx) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + area->remove_shape(p_shape_idx); +} + +void GodotPhysicsServer2D::area_clear_shapes(RID p_area) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + while (area->get_shape_count()) { + area->remove_shape(0); + } +} + +void GodotPhysicsServer2D::area_attach_object_instance_id(RID p_area, ObjectID p_id) { + if (space_owner.owns(p_area)) { + GodotSpace2D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + area->set_instance_id(p_id); +} + +ObjectID GodotPhysicsServer2D::area_get_object_instance_id(RID p_area) const { + if (space_owner.owns(p_area)) { + GodotSpace2D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, ObjectID()); + return area->get_instance_id(); +} + +void GodotPhysicsServer2D::area_attach_canvas_instance_id(RID p_area, ObjectID p_id) { + if (space_owner.owns(p_area)) { + GodotSpace2D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + area->set_canvas_instance_id(p_id); +} + +ObjectID GodotPhysicsServer2D::area_get_canvas_instance_id(RID p_area) const { + if (space_owner.owns(p_area)) { + GodotSpace2D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, ObjectID()); + return area->get_canvas_instance_id(); +} + +void GodotPhysicsServer2D::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) { + if (space_owner.owns(p_area)) { + GodotSpace2D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + area->set_param(p_param, p_value); +}; + +void GodotPhysicsServer2D::area_set_transform(RID p_area, const Transform2D &p_transform) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + area->set_transform(p_transform); +}; + +Variant GodotPhysicsServer2D::area_get_param(RID p_area, AreaParameter p_param) const { + if (space_owner.owns(p_area)) { + GodotSpace2D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, Variant()); + + return area->get_param(p_param); +}; + +Transform2D GodotPhysicsServer2D::area_get_transform(RID p_area) const { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, Transform2D()); + + return area->get_transform(); +}; + +void GodotPhysicsServer2D::area_set_pickable(RID p_area, bool p_pickable) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + area->set_pickable(p_pickable); +} + +void GodotPhysicsServer2D::area_set_monitorable(RID p_area, bool p_monitorable) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + FLUSH_QUERY_CHECK(area); + + area->set_monitorable(p_monitorable); +} + +void GodotPhysicsServer2D::area_set_collision_mask(RID p_area, uint32_t p_mask) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + area->set_collision_mask(p_mask); +} + +void GodotPhysicsServer2D::area_set_collision_layer(RID p_area, uint32_t p_layer) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + area->set_collision_layer(p_layer); +} + +void GodotPhysicsServer2D::area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + area->set_monitor_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method); +} + +void GodotPhysicsServer2D::area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + area->set_area_monitor_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method); +} + +/* BODY API */ + +RID GodotPhysicsServer2D::body_create() { + GodotBody2D *body = memnew(GodotBody2D); + RID rid = body_owner.make_rid(body); + body->set_self(rid); + return rid; +} + +void GodotPhysicsServer2D::body_set_space(RID p_body, RID p_space) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + GodotSpace2D *space = nullptr; + if (p_space.is_valid()) { + space = space_owner.get_or_null(p_space); + ERR_FAIL_COND(!space); + } + + if (body->get_space() == space) { + return; //pointless + } + + body->clear_constraint_list(); + body->set_space(space); +}; + +RID GodotPhysicsServer2D::body_get_space(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, RID()); + + GodotSpace2D *space = body->get_space(); + if (!space) { + return RID(); + } + return space->get_self(); +}; + +void GodotPhysicsServer2D::body_set_mode(RID p_body, BodyMode p_mode) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + FLUSH_QUERY_CHECK(body); + + body->set_mode(p_mode); +}; + +PhysicsServer2D::BodyMode GodotPhysicsServer2D::body_get_mode(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, BODY_MODE_STATIC); + + return body->get_mode(); +}; + +void GodotPhysicsServer2D::body_add_shape(RID p_body, RID p_shape, const Transform2D &p_transform, bool p_disabled) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND(!shape); + + body->add_shape(shape, p_transform, p_disabled); +} + +void GodotPhysicsServer2D::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND(!shape); + ERR_FAIL_COND(!shape->is_configured()); + + body->set_shape(p_shape_idx, shape); +} + +void GodotPhysicsServer2D::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform2D &p_transform) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_shape_transform(p_shape_idx, p_transform); +} + +int GodotPhysicsServer2D::body_get_shape_count(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, -1); + + return body->get_shape_count(); +} + +RID GodotPhysicsServer2D::body_get_shape(RID p_body, int p_shape_idx) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, RID()); + + GodotShape2D *shape = body->get_shape(p_shape_idx); + ERR_FAIL_COND_V(!shape, RID()); + + return shape->get_self(); +} + +Transform2D GodotPhysicsServer2D::body_get_shape_transform(RID p_body, int p_shape_idx) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, Transform2D()); + + return body->get_shape_transform(p_shape_idx); +} + +void GodotPhysicsServer2D::body_remove_shape(RID p_body, int p_shape_idx) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->remove_shape(p_shape_idx); +} + +void GodotPhysicsServer2D::body_clear_shapes(RID p_body) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + while (body->get_shape_count()) { + body->remove_shape(0); + } +} + +void GodotPhysicsServer2D::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count()); + FLUSH_QUERY_CHECK(body); + + body->set_shape_disabled(p_shape_idx, p_disabled); +} + +void GodotPhysicsServer2D::body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, real_t p_margin) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count()); + FLUSH_QUERY_CHECK(body); + + body->set_shape_as_one_way_collision(p_shape_idx, p_enable, p_margin); +} + +void GodotPhysicsServer2D::body_set_continuous_collision_detection_mode(RID p_body, CCDMode p_mode) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + body->set_continuous_collision_detection_mode(p_mode); +} + +GodotPhysicsServer2D::CCDMode GodotPhysicsServer2D::body_get_continuous_collision_detection_mode(RID p_body) const { + const GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, CCD_MODE_DISABLED); + + return body->get_continuous_collision_detection_mode(); +} + +void GodotPhysicsServer2D::body_attach_object_instance_id(RID p_body, ObjectID p_id) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_instance_id(p_id); +}; + +ObjectID GodotPhysicsServer2D::body_get_object_instance_id(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, ObjectID()); + + return body->get_instance_id(); +}; + +void GodotPhysicsServer2D::body_attach_canvas_instance_id(RID p_body, ObjectID p_id) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_canvas_instance_id(p_id); +}; + +ObjectID GodotPhysicsServer2D::body_get_canvas_instance_id(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, ObjectID()); + + return body->get_canvas_instance_id(); +}; + +void GodotPhysicsServer2D::body_set_collision_layer(RID p_body, uint32_t p_layer) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + body->set_collision_layer(p_layer); +}; + +uint32_t GodotPhysicsServer2D::body_get_collision_layer(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, 0); + + return body->get_collision_layer(); +}; + +void GodotPhysicsServer2D::body_set_collision_mask(RID p_body, uint32_t p_mask) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + body->set_collision_mask(p_mask); +}; + +uint32_t GodotPhysicsServer2D::body_get_collision_mask(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, 0); + + return body->get_collision_mask(); +}; + +void GodotPhysicsServer2D::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_param(p_param, p_value); +}; + +Variant GodotPhysicsServer2D::body_get_param(RID p_body, BodyParameter p_param) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, 0); + + return body->get_param(p_param); +}; + +void GodotPhysicsServer2D::body_reset_mass_properties(RID p_body) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + return body->reset_mass_properties(); +} + +void GodotPhysicsServer2D::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_state(p_state, p_variant); +}; + +Variant GodotPhysicsServer2D::body_get_state(RID p_body, BodyState p_state) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, Variant()); + + return body->get_state(p_state); +}; + +void GodotPhysicsServer2D::body_set_applied_force(RID p_body, const Vector2 &p_force) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_applied_force(p_force); + body->wakeup(); +}; + +Vector2 GodotPhysicsServer2D::body_get_applied_force(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, Vector2()); + return body->get_applied_force(); +}; + +void GodotPhysicsServer2D::body_set_applied_torque(RID p_body, real_t p_torque) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_applied_torque(p_torque); + body->wakeup(); +}; + +real_t GodotPhysicsServer2D::body_get_applied_torque(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, 0); + + return body->get_applied_torque(); +}; + +void GodotPhysicsServer2D::body_apply_central_impulse(RID p_body, const Vector2 &p_impulse) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->apply_central_impulse(p_impulse); + body->wakeup(); +} + +void GodotPhysicsServer2D::body_apply_torque_impulse(RID p_body, real_t p_torque) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + _update_shapes(); + + body->apply_torque_impulse(p_torque); + body->wakeup(); +} + +void GodotPhysicsServer2D::body_apply_impulse(RID p_body, const Vector2 &p_impulse, const Vector2 &p_position) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + _update_shapes(); + + body->apply_impulse(p_impulse, p_position); + body->wakeup(); +}; + +void GodotPhysicsServer2D::body_add_central_force(RID p_body, const Vector2 &p_force) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->add_central_force(p_force); + body->wakeup(); +}; + +void GodotPhysicsServer2D::body_add_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->add_force(p_force, p_position); + body->wakeup(); +}; + +void GodotPhysicsServer2D::body_add_torque(RID p_body, real_t p_torque) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->add_torque(p_torque); + body->wakeup(); +}; + +void GodotPhysicsServer2D::body_set_axis_velocity(RID p_body, const Vector2 &p_axis_velocity) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + _update_shapes(); + + Vector2 v = body->get_linear_velocity(); + Vector2 axis = p_axis_velocity.normalized(); + v -= axis * axis.dot(v); + v += p_axis_velocity; + body->set_linear_velocity(v); + body->wakeup(); +}; + +void GodotPhysicsServer2D::body_add_collision_exception(RID p_body, RID p_body_b) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->add_exception(p_body_b); + body->wakeup(); +}; + +void GodotPhysicsServer2D::body_remove_collision_exception(RID p_body, RID p_body_b) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->remove_exception(p_body_b); + body->wakeup(); +}; + +void GodotPhysicsServer2D::body_get_collision_exceptions(RID p_body, List *p_exceptions) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + for (int i = 0; i < body->get_exceptions().size(); i++) { + p_exceptions->push_back(body->get_exceptions()[i]); + } +}; + +void GodotPhysicsServer2D::body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); +}; + +real_t GodotPhysicsServer2D::body_get_contacts_reported_depth_threshold(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, 0); + return 0; +}; + +void GodotPhysicsServer2D::body_set_omit_force_integration(RID p_body, bool p_omit) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_omit_force_integration(p_omit); +}; + +bool GodotPhysicsServer2D::body_is_omitting_force_integration(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, false); + return body->get_omit_force_integration(); +}; + +void GodotPhysicsServer2D::body_set_max_contacts_reported(RID p_body, int p_contacts) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + body->set_max_contacts_reported(p_contacts); +} + +int GodotPhysicsServer2D::body_get_max_contacts_reported(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, -1); + return body->get_max_contacts_reported(); +} + +void GodotPhysicsServer2D::body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + body->set_state_sync_callback(p_instance, p_callback); +} + +void GodotPhysicsServer2D::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + body->set_force_integration_callback(p_callable, p_udata); +} + +bool GodotPhysicsServer2D::body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, false); + ERR_FAIL_INDEX_V(p_body_shape, body->get_shape_count(), false); + + return shape_collide(body->get_shape(p_body_shape)->get_self(), body->get_transform() * body->get_shape_transform(p_body_shape), Vector2(), p_shape, p_shape_xform, p_motion, r_results, p_result_max, r_result_count); +} + +void GodotPhysicsServer2D::body_set_pickable(RID p_body, bool p_pickable) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + body->set_pickable(p_pickable); +} + +bool GodotPhysicsServer2D::body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, false); + ERR_FAIL_COND_V(!body->get_space(), false); + ERR_FAIL_COND_V(body->get_space()->is_locked(), false); + + _update_shapes(); + + return body->get_space()->test_body_motion(body, p_parameters, r_result); +} + +PhysicsDirectBodyState2D *GodotPhysicsServer2D::body_get_direct_state(RID p_body) { + ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); + + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, nullptr); + + ERR_FAIL_COND_V(!body->get_space(), nullptr); + ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); + + return body->get_direct_state(); +} + +/* JOINT API */ + +RID GodotPhysicsServer2D::joint_create() { + GodotJoint2D *joint = memnew(GodotJoint2D); + RID joint_rid = joint_owner.make_rid(joint); + joint->set_self(joint_rid); + return joint_rid; +} + +void GodotPhysicsServer2D::joint_clear(RID p_joint) { + GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + if (joint->get_type() != JOINT_TYPE_MAX) { + GodotJoint2D *empty_joint = memnew(GodotJoint2D); + empty_joint->copy_settings_from(joint); + + joint_owner.replace(p_joint, empty_joint); + memdelete(joint); + } +} + +void GodotPhysicsServer2D::joint_set_param(RID p_joint, JointParam p_param, real_t p_value) { + GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(!joint); + + switch (p_param) { + case JOINT_PARAM_BIAS: + joint->set_bias(p_value); + break; + case JOINT_PARAM_MAX_BIAS: + joint->set_max_bias(p_value); + break; + case JOINT_PARAM_MAX_FORCE: + joint->set_max_force(p_value); + break; + } +} + +real_t GodotPhysicsServer2D::joint_get_param(RID p_joint, JointParam p_param) const { + const GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!joint, -1); + + switch (p_param) { + case JOINT_PARAM_BIAS: + return joint->get_bias(); + break; + case JOINT_PARAM_MAX_BIAS: + return joint->get_max_bias(); + break; + case JOINT_PARAM_MAX_FORCE: + return joint->get_max_force(); + break; + } + + return 0; +} + +void GodotPhysicsServer2D::joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) { + GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(!joint); + + joint->disable_collisions_between_bodies(p_disable); + + if (2 == joint->get_body_count()) { + GodotBody2D *body_a = *joint->get_body_ptr(); + GodotBody2D *body_b = *(joint->get_body_ptr() + 1); + + if (p_disable) { + body_add_collision_exception(body_a->get_self(), body_b->get_self()); + body_add_collision_exception(body_b->get_self(), body_a->get_self()); + } else { + body_remove_collision_exception(body_a->get_self(), body_b->get_self()); + body_remove_collision_exception(body_b->get_self(), body_a->get_self()); + } + } +} + +bool GodotPhysicsServer2D::joint_is_disabled_collisions_between_bodies(RID p_joint) const { + const GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!joint, true); + + return joint->is_disabled_collisions_between_bodies(); +} + +void GodotPhysicsServer2D::joint_make_pin(RID p_joint, const Vector2 &p_pos, RID p_body_a, RID p_body_b) { + GodotBody2D *A = body_owner.get_or_null(p_body_a); + ERR_FAIL_COND(!A); + GodotBody2D *B = nullptr; + if (body_owner.owns(p_body_b)) { + B = body_owner.get_or_null(p_body_b); + ERR_FAIL_COND(!B); + } + + GodotJoint2D *prev_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); + + GodotJoint2D *joint = memnew(GodotPinJoint2D(p_pos, A, B)); + + joint_owner.replace(p_joint, joint); + joint->copy_settings_from(prev_joint); + memdelete(prev_joint); +} + +void GodotPhysicsServer2D::joint_make_groove(RID p_joint, const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) { + GodotBody2D *A = body_owner.get_or_null(p_body_a); + ERR_FAIL_COND(!A); + + GodotBody2D *B = body_owner.get_or_null(p_body_b); + ERR_FAIL_COND(!B); + + GodotJoint2D *prev_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); + + GodotJoint2D *joint = memnew(GodotGrooveJoint2D(p_a_groove1, p_a_groove2, p_b_anchor, A, B)); + + joint_owner.replace(p_joint, joint); + joint->copy_settings_from(prev_joint); + memdelete(prev_joint); +} + +void GodotPhysicsServer2D::joint_make_damped_spring(RID p_joint, const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b) { + GodotBody2D *A = body_owner.get_or_null(p_body_a); + ERR_FAIL_COND(!A); + + GodotBody2D *B = body_owner.get_or_null(p_body_b); + ERR_FAIL_COND(!B); + + GodotJoint2D *prev_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); + + GodotJoint2D *joint = memnew(GodotDampedSpringJoint2D(p_anchor_a, p_anchor_b, A, B)); + + joint_owner.replace(p_joint, joint); + joint->copy_settings_from(prev_joint); + memdelete(prev_joint); +} + +void GodotPhysicsServer2D::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) { + GodotJoint2D *j = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(!j); + ERR_FAIL_COND(j->get_type() != JOINT_TYPE_PIN); + + GodotPinJoint2D *pin_joint = static_cast(j); + pin_joint->set_param(p_param, p_value); +} + +real_t GodotPhysicsServer2D::pin_joint_get_param(RID p_joint, PinJointParam p_param) const { + GodotJoint2D *j = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!j, 0); + ERR_FAIL_COND_V(j->get_type() != JOINT_TYPE_PIN, 0); + + GodotPinJoint2D *pin_joint = static_cast(j); + return pin_joint->get_param(p_param); +} + +void GodotPhysicsServer2D::damped_spring_joint_set_param(RID p_joint, DampedSpringParam p_param, real_t p_value) { + GodotJoint2D *j = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(!j); + ERR_FAIL_COND(j->get_type() != JOINT_TYPE_DAMPED_SPRING); + + GodotDampedSpringJoint2D *dsj = static_cast(j); + dsj->set_param(p_param, p_value); +} + +real_t GodotPhysicsServer2D::damped_spring_joint_get_param(RID p_joint, DampedSpringParam p_param) const { + GodotJoint2D *j = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!j, 0); + ERR_FAIL_COND_V(j->get_type() != JOINT_TYPE_DAMPED_SPRING, 0); + + GodotDampedSpringJoint2D *dsj = static_cast(j); + return dsj->get_param(p_param); +} + +PhysicsServer2D::JointType GodotPhysicsServer2D::joint_get_type(RID p_joint) const { + GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!joint, JOINT_TYPE_PIN); + + return joint->get_type(); +} + +void GodotPhysicsServer2D::free(RID p_rid) { + _update_shapes(); // just in case + + if (shape_owner.owns(p_rid)) { + GodotShape2D *shape = shape_owner.get_or_null(p_rid); + + while (shape->get_owners().size()) { + GodotShapeOwner2D *so = shape->get_owners().front()->key(); + so->remove_shape(shape); + } + + shape_owner.free(p_rid); + memdelete(shape); + } else if (body_owner.owns(p_rid)) { + GodotBody2D *body = body_owner.get_or_null(p_rid); + + /* + if (body->get_state_query()) + _clear_query(body->get_state_query()); + + if (body->get_direct_state_query()) + _clear_query(body->get_direct_state_query()); + */ + + body_set_space(p_rid, RID()); + + while (body->get_shape_count()) { + body->remove_shape(0); + } + + body_owner.free(p_rid); + memdelete(body); + + } else if (area_owner.owns(p_rid)) { + GodotArea2D *area = area_owner.get_or_null(p_rid); + + /* + if (area->get_monitor_query()) + _clear_query(area->get_monitor_query()); + */ + + area->set_space(nullptr); + + while (area->get_shape_count()) { + area->remove_shape(0); + } + + area_owner.free(p_rid); + memdelete(area); + } else if (space_owner.owns(p_rid)) { + GodotSpace2D *space = space_owner.get_or_null(p_rid); + + while (space->get_objects().size()) { + GodotCollisionObject2D *co = (GodotCollisionObject2D *)space->get_objects().front()->get(); + co->set_space(nullptr); + } + + active_spaces.erase(space); + free(space->get_default_area()->get_self()); + space_owner.free(p_rid); + memdelete(space); + } else if (joint_owner.owns(p_rid)) { + GodotJoint2D *joint = joint_owner.get_or_null(p_rid); + + joint_owner.free(p_rid); + memdelete(joint); + + } else { + ERR_FAIL_MSG("Invalid ID."); + } +}; + +void GodotPhysicsServer2D::set_active(bool p_active) { + active = p_active; +}; + +void GodotPhysicsServer2D::set_collision_iterations(int p_iterations) { + iterations = p_iterations; +}; + +void GodotPhysicsServer2D::init() { + doing_sync = false; + iterations = 8; // 8? + stepper = memnew(GodotStep2D); +}; + +void GodotPhysicsServer2D::step(real_t p_step) { + if (!active) { + return; + } + + _update_shapes(); + + island_count = 0; + active_objects = 0; + collision_pairs = 0; + for (Set::Element *E = active_spaces.front(); E; E = E->next()) { + stepper->step((GodotSpace2D *)E->get(), p_step, iterations); + island_count += E->get()->get_island_count(); + active_objects += E->get()->get_active_objects(); + collision_pairs += E->get()->get_collision_pairs(); + } +}; + +void GodotPhysicsServer2D::sync() { + doing_sync = true; +}; + +void GodotPhysicsServer2D::flush_queries() { + if (!active) { + return; + } + + flushing_queries = true; + + uint64_t time_beg = OS::get_singleton()->get_ticks_usec(); + + for (Set::Element *E = active_spaces.front(); E; E = E->next()) { + GodotSpace2D *space = (GodotSpace2D *)E->get(); + space->call_queries(); + } + + flushing_queries = false; + + if (EngineDebugger::is_profiling("servers")) { + uint64_t total_time[GodotSpace2D::ELAPSED_TIME_MAX]; + static const char *time_name[GodotSpace2D::ELAPSED_TIME_MAX] = { + "integrate_forces", + "generate_islands", + "setup_constraints", + "solve_constraints", + "integrate_velocities" + }; + + for (int i = 0; i < GodotSpace2D::ELAPSED_TIME_MAX; i++) { + total_time[i] = 0; + } + + for (Set::Element *E = active_spaces.front(); E; E = E->next()) { + for (int i = 0; i < GodotSpace2D::ELAPSED_TIME_MAX; i++) { + total_time[i] += E->get()->get_elapsed_time(GodotSpace2D::ElapsedTime(i)); + } + } + + Array values; + values.resize(GodotSpace2D::ELAPSED_TIME_MAX * 2); + for (int i = 0; i < GodotSpace2D::ELAPSED_TIME_MAX; i++) { + values[i * 2 + 0] = time_name[i]; + values[i * 2 + 1] = USEC_TO_SEC(total_time[i]); + } + values.push_back("flush_queries"); + values.push_back(USEC_TO_SEC(OS::get_singleton()->get_ticks_usec() - time_beg)); + + values.push_front("physics_2d"); + EngineDebugger::profiler_add_frame_data("servers", values); + } +} + +void GodotPhysicsServer2D::end_sync() { + doing_sync = false; +} + +void GodotPhysicsServer2D::finish() { + memdelete(stepper); +}; + +void GodotPhysicsServer2D::_update_shapes() { + while (pending_shape_update_list.first()) { + pending_shape_update_list.first()->self()->_shape_changed(); + pending_shape_update_list.remove(pending_shape_update_list.first()); + } +} + +int GodotPhysicsServer2D::get_process_info(ProcessInfo p_info) { + switch (p_info) { + case INFO_ACTIVE_OBJECTS: { + return active_objects; + } break; + case INFO_COLLISION_PAIRS: { + return collision_pairs; + } break; + case INFO_ISLAND_COUNT: { + return island_count; + } break; + } + + return 0; +} + +GodotPhysicsServer2D *GodotPhysicsServer2D::godot_singleton = nullptr; + +GodotPhysicsServer2D::GodotPhysicsServer2D(bool p_using_threads) { + godot_singleton = this; + GodotBroadPhase2D::create_func = GodotBroadPhase2DBVH::_create; + + using_threads = p_using_threads; +}; diff --git a/servers/physics_2d/godot_physics_server_2d.h b/servers/physics_2d/godot_physics_server_2d.h new file mode 100644 index 0000000000..a8a1e71d13 --- /dev/null +++ b/servers/physics_2d/godot_physics_server_2d.h @@ -0,0 +1,299 @@ +/*************************************************************************/ +/* godot_physics_server_2d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_PHYSICS_SERVER_2D_H +#define GODOT_PHYSICS_SERVER_2D_H + +#include "godot_joints_2d.h" +#include "godot_shape_2d.h" +#include "godot_space_2d.h" +#include "godot_step_2d.h" + +#include "core/templates/rid_owner.h" +#include "servers/physics_server_2d.h" + +class GodotPhysicsServer2D : public PhysicsServer2D { + GDCLASS(GodotPhysicsServer2D, PhysicsServer2D); + + friend class GodotPhysicsDirectSpaceState2D; + friend class GodotPhysicsDirectBodyState2D; + bool active = true; + int iterations = 0; + bool doing_sync = false; + + int island_count = 0; + int active_objects = 0; + int collision_pairs = 0; + + bool using_threads = false; + + bool flushing_queries = false; + + GodotStep2D *stepper = nullptr; + Set active_spaces; + + mutable RID_PtrOwner shape_owner; + mutable RID_PtrOwner space_owner; + mutable RID_PtrOwner area_owner; + mutable RID_PtrOwner body_owner; + mutable RID_PtrOwner joint_owner; + + static GodotPhysicsServer2D *godot_singleton; + + friend class GodotCollisionObject2D; + SelfList::List pending_shape_update_list; + void _update_shapes(); + + RID _shape_create(ShapeType p_shape); + +public: + struct CollCbkData { + Vector2 valid_dir; + real_t valid_depth = 0.0; + int max = 0; + int amount = 0; + int passed = 0; + int invalid_by_dir = 0; + Vector2 *ptr = nullptr; + }; + + virtual RID world_boundary_shape_create() override; + virtual RID separation_ray_shape_create() override; + virtual RID segment_shape_create() override; + virtual RID circle_shape_create() override; + virtual RID rectangle_shape_create() override; + virtual RID capsule_shape_create() override; + virtual RID convex_polygon_shape_create() override; + virtual RID concave_polygon_shape_create() override; + + static void _shape_col_cbk(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata); + + virtual void shape_set_data(RID p_shape, const Variant &p_data) override; + virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias) override; + + virtual ShapeType shape_get_type(RID p_shape) const override; + virtual Variant shape_get_data(RID p_shape) const override; + virtual real_t shape_get_custom_solver_bias(RID p_shape) const override; + + virtual bool shape_collide(RID p_shape_A, const Transform2D &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Transform2D &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) override; + + /* SPACE API */ + + virtual RID space_create() override; + virtual void space_set_active(RID p_space, bool p_active) override; + virtual bool space_is_active(RID p_space) const override; + + virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) override; + virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const override; + + virtual void space_set_debug_contacts(RID p_space, int p_max_contacts) override; + virtual Vector space_get_contacts(RID p_space) const override; + virtual int space_get_contact_count(RID p_space) const override; + + // this function only works on physics process, errors and returns null otherwise + virtual PhysicsDirectSpaceState2D *space_get_direct_state(RID p_space) override; + + /* AREA API */ + + virtual RID area_create() override; + + virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) override; + virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const 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 Transform2D &p_transform = Transform2D(), bool p_disabled = false) override; + virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape) override; + virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform2D &p_transform) override; + + virtual int area_get_shape_count(RID p_area) const override; + virtual RID area_get_shape(RID p_area, int p_shape_idx) const override; + virtual Transform2D 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, bool p_disabled) 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_attach_canvas_instance_id(RID p_area, ObjectID p_id) override; + virtual ObjectID area_get_canvas_instance_id(RID p_area) const override; + + virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) override; + virtual void area_set_transform(RID p_area, const Transform2D &p_transform) override; + + virtual Variant area_get_param(RID p_area, AreaParameter p_param) const override; + virtual Transform2D area_get_transform(RID p_area) const override; + virtual void area_set_monitorable(RID p_area, bool p_monitorable) override; + virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) override; + virtual void area_set_collision_layer(RID p_area, uint32_t p_layer) override; + + virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) override; + virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) override; + + virtual void area_set_pickable(RID p_area, bool p_pickable) override; + + /* BODY API */ + + // create a body of a given type + virtual RID body_create() override; + + virtual void body_set_space(RID p_body, RID p_space) override; + virtual RID body_get_space(RID p_body) const override; + + virtual void body_set_mode(RID p_body, BodyMode p_mode) override; + virtual BodyMode body_get_mode(RID p_body) const override; + + virtual void body_add_shape(RID p_body, RID p_shape, const Transform2D &p_transform = Transform2D(), bool p_disabled = false) override; + virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape) override; + virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform2D &p_transform) override; + + virtual int body_get_shape_count(RID p_body) const override; + virtual RID body_get_shape(RID p_body, int p_shape_idx) const override; + virtual Transform2D body_get_shape_transform(RID p_body, int p_shape_idx) 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_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) override; + virtual void body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, real_t p_margin) 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_attach_canvas_instance_id(RID p_body, ObjectID p_id) override; + virtual ObjectID body_get_canvas_instance_id(RID p_body) const override; + + virtual void body_set_continuous_collision_detection_mode(RID p_body, CCDMode p_mode) override; + virtual CCDMode body_get_continuous_collision_detection_mode(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_param(RID p_body, BodyParameter p_param, const Variant &p_value) override; + virtual Variant body_get_param(RID p_body, BodyParameter p_param) const override; + + virtual void body_reset_mass_properties(RID p_body) override; + + virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override; + virtual Variant body_get_state(RID p_body, BodyState p_state) const override; + + virtual void body_set_applied_force(RID p_body, const Vector2 &p_force) override; + virtual Vector2 body_get_applied_force(RID p_body) const override; + + virtual void body_set_applied_torque(RID p_body, real_t p_torque) override; + virtual real_t body_get_applied_torque(RID p_body) const override; + + virtual void body_add_central_force(RID p_body, const Vector2 &p_force) override; + virtual void body_add_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position = Vector2()) override; + virtual void body_add_torque(RID p_body, real_t p_torque) override; + + virtual void body_apply_central_impulse(RID p_body, const Vector2 &p_impulse) override; + virtual void body_apply_torque_impulse(RID p_body, real_t p_torque) override; + virtual void body_apply_impulse(RID p_body, const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override; + virtual void body_set_axis_velocity(RID p_body, const Vector2 &p_axis_velocity) override; + + virtual void body_add_collision_exception(RID p_body, RID p_body_b) override; + virtual void body_remove_collision_exception(RID p_body, RID p_body_b) override; + virtual void body_get_collision_exceptions(RID p_body, List *p_exceptions) override; + + virtual void body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) override; + virtual real_t body_get_contacts_reported_depth_threshold(RID p_body) const override; + + virtual void body_set_omit_force_integration(RID p_body, bool p_omit) override; + virtual bool body_is_omitting_force_integration(RID p_body) const override; + + virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) override; + virtual int body_get_max_contacts_reported(RID p_body) const override; + + virtual void body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) override; + virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override; + + virtual bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override; + + virtual void body_set_pickable(RID p_body, bool p_pickable) override; + + virtual bool body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result = nullptr) override; + + // this function only works on physics process, errors and returns null otherwise + virtual PhysicsDirectBodyState2D *body_get_direct_state(RID p_body) override; + + /* JOINT API */ + + virtual RID joint_create() override; + + virtual void joint_clear(RID p_joint) override; + + virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value) override; + virtual real_t joint_get_param(RID p_joint, JointParam p_param) const override; + + virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disabled) override; + virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const override; + + virtual void joint_make_pin(RID p_joint, const Vector2 &p_anchor, RID p_body_a, RID p_body_b = RID()) override; + virtual void joint_make_groove(RID p_joint, const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) override; + virtual void joint_make_damped_spring(RID p_joint, const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b = RID()) override; + + virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) override; + virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const override; + virtual void damped_spring_joint_set_param(RID p_joint, DampedSpringParam p_param, real_t p_value) override; + virtual real_t damped_spring_joint_get_param(RID p_joint, DampedSpringParam p_param) const override; + + virtual JointType joint_get_type(RID p_joint) const override; + + /* MISC */ + + virtual void free(RID p_rid) override; + + virtual void set_active(bool p_active) override; + virtual void init() override; + virtual void step(real_t p_step) override; + virtual void sync() override; + virtual void flush_queries() override; + virtual void end_sync() override; + virtual void finish() override; + + virtual void set_collision_iterations(int p_iterations) override; + + virtual bool is_flushing_queries() const override { return flushing_queries; } + + int get_process_info(ProcessInfo p_info) override; + + GodotPhysicsServer2D(bool p_using_threads = false); + ~GodotPhysicsServer2D() {} +}; + +#endif // GODOT_PHYSICS_SERVER_2D_H diff --git a/servers/physics_2d/godot_shape_2d.cpp b/servers/physics_2d/godot_shape_2d.cpp new file mode 100644 index 0000000000..3604004324 --- /dev/null +++ b/servers/physics_2d/godot_shape_2d.cpp @@ -0,0 +1,995 @@ +/*************************************************************************/ +/* godot_shape_2d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_shape_2d.h" + +#include "core/math/geometry_2d.h" +#include "core/templates/sort_array.h" + +void GodotShape2D::configure(const Rect2 &p_aabb) { + aabb = p_aabb; + configured = true; + for (const KeyValue &E : owners) { + GodotShapeOwner2D *co = (GodotShapeOwner2D *)E.key; + co->_shape_changed(); + } +} + +Vector2 GodotShape2D::get_support(const Vector2 &p_normal) const { + Vector2 res[2]; + int amnt; + get_supports(p_normal, res, amnt); + return res[0]; +} + +void GodotShape2D::add_owner(GodotShapeOwner2D *p_owner) { + Map::Element *E = owners.find(p_owner); + if (E) { + E->get()++; + } else { + owners[p_owner] = 1; + } +} + +void GodotShape2D::remove_owner(GodotShapeOwner2D *p_owner) { + Map::Element *E = owners.find(p_owner); + ERR_FAIL_COND(!E); + E->get()--; + if (E->get() == 0) { + owners.erase(E); + } +} + +bool GodotShape2D::is_owner(GodotShapeOwner2D *p_owner) const { + return owners.has(p_owner); +} + +const Map &GodotShape2D::get_owners() const { + return owners; +} + +GodotShape2D::~GodotShape2D() { + ERR_FAIL_COND(owners.size()); +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + +void GodotWorldBoundaryShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + r_amount = 0; +} + +bool GodotWorldBoundaryShape2D::contains_point(const Vector2 &p_point) const { + return normal.dot(p_point) < d; +} + +bool GodotWorldBoundaryShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + Vector2 segment = p_begin - p_end; + real_t den = normal.dot(segment); + + //printf("den is %i\n",den); + if (Math::abs(den) <= CMP_EPSILON) { + return false; + } + + real_t dist = (normal.dot(p_begin) - d) / den; + //printf("dist is %i\n",dist); + + if (dist < -CMP_EPSILON || dist > (1.0 + CMP_EPSILON)) { + return false; + } + + r_point = p_begin + segment * -dist; + r_normal = normal; + + return true; +} + +real_t GodotWorldBoundaryShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + return 0; +} + +void GodotWorldBoundaryShape2D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY); + Array arr = p_data; + ERR_FAIL_COND(arr.size() != 2); + normal = arr[0]; + d = arr[1]; + configure(Rect2(Vector2(-1e4, -1e4), Vector2(1e4 * 2, 1e4 * 2))); +} + +Variant GodotWorldBoundaryShape2D::get_data() const { + Array arr; + arr.resize(2); + arr[0] = normal; + arr[1] = d; + return arr; +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + +void GodotSeparationRayShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + r_amount = 1; + + if (p_normal.y > 0) { + *r_supports = Vector2(0, length); + } else { + *r_supports = Vector2(); + } +} + +bool GodotSeparationRayShape2D::contains_point(const Vector2 &p_point) const { + return false; +} + +bool GodotSeparationRayShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + return false; //rays can't be intersected +} + +real_t GodotSeparationRayShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + return 0; //rays are mass-less +} + +void GodotSeparationRayShape2D::set_data(const Variant &p_data) { + Dictionary d = p_data; + length = d["length"]; + slide_on_slope = d["slide_on_slope"]; + configure(Rect2(0, 0, 0.001, length)); +} + +Variant GodotSeparationRayShape2D::get_data() const { + Dictionary d; + d["length"] = length; + d["slide_on_slope"] = slide_on_slope; + return d; +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + +void GodotSegmentShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + if (Math::abs(p_normal.dot(n)) > _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) { + r_supports[0] = a; + r_supports[1] = b; + r_amount = 2; + return; + } + + real_t dp = p_normal.dot(b - a); + if (dp > 0) { + *r_supports = b; + } else { + *r_supports = a; + } + r_amount = 1; +} + +bool GodotSegmentShape2D::contains_point(const Vector2 &p_point) const { + return false; +} + +bool GodotSegmentShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + if (!Geometry2D::segment_intersects_segment(p_begin, p_end, a, b, &r_point)) { + return false; + } + + if (n.dot(p_begin) > n.dot(a)) { + r_normal = n; + } else { + r_normal = -n; + } + + return true; +} + +real_t GodotSegmentShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + return p_mass * ((a * p_scale).distance_squared_to(b * p_scale)) / 12; +} + +void GodotSegmentShape2D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::RECT2); + + Rect2 r = p_data; + a = r.position; + b = r.size; + n = (b - a).orthogonal(); + + Rect2 aabb; + aabb.position = a; + aabb.expand_to(b); + if (aabb.size.x == 0) { + aabb.size.x = 0.001; + } + if (aabb.size.y == 0) { + aabb.size.y = 0.001; + } + configure(aabb); +} + +Variant GodotSegmentShape2D::get_data() const { + Rect2 r; + r.position = a; + r.size = b; + return r; +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + +void GodotCircleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + r_amount = 1; + *r_supports = p_normal * radius; +} + +bool GodotCircleShape2D::contains_point(const Vector2 &p_point) const { + return p_point.length_squared() < radius * radius; +} + +bool GodotCircleShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + Vector2 line_vec = p_end - p_begin; + + real_t a, b, c; + + a = line_vec.dot(line_vec); + b = 2 * p_begin.dot(line_vec); + c = p_begin.dot(p_begin) - radius * radius; + + real_t sqrtterm = b * b - 4 * a * c; + + if (sqrtterm < 0) { + return false; + } + sqrtterm = Math::sqrt(sqrtterm); + real_t res = (-b - sqrtterm) / (2 * a); + + if (res < 0 || res > 1 + CMP_EPSILON) { + return false; + } + + r_point = p_begin + line_vec * res; + r_normal = r_point.normalized(); + return true; +} + +real_t GodotCircleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + real_t a = radius * p_scale.x; + real_t b = radius * p_scale.y; + return p_mass * (a * a + b * b) / 4; +} + +void GodotCircleShape2D::set_data(const Variant &p_data) { + ERR_FAIL_COND(!p_data.is_num()); + radius = p_data; + configure(Rect2(-radius, -radius, radius * 2, radius * 2)); +} + +Variant GodotCircleShape2D::get_data() const { + return radius; +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + +void GodotRectangleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + for (int i = 0; i < 2; i++) { + Vector2 ag; + ag[i] = 1.0; + real_t dp = ag.dot(p_normal); + if (Math::abs(dp) < _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) { + continue; + } + + real_t sgn = dp > 0 ? 1.0 : -1.0; + + r_amount = 2; + + r_supports[0][i] = half_extents[i] * sgn; + r_supports[0][i ^ 1] = half_extents[i ^ 1]; + + r_supports[1][i] = half_extents[i] * sgn; + r_supports[1][i ^ 1] = -half_extents[i ^ 1]; + + return; + } + + /* USE POINT */ + + r_amount = 1; + r_supports[0] = Vector2( + (p_normal.x < 0) ? -half_extents.x : half_extents.x, + (p_normal.y < 0) ? -half_extents.y : half_extents.y); +} + +bool GodotRectangleShape2D::contains_point(const Vector2 &p_point) const { + real_t x = p_point.x; + real_t y = p_point.y; + real_t edge_x = half_extents.x; + real_t edge_y = half_extents.y; + return (x >= -edge_x) && (x < edge_x) && (y >= -edge_y) && (y < edge_y); +} + +bool GodotRectangleShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + return get_aabb().intersects_segment(p_begin, p_end, &r_point, &r_normal); +} + +real_t GodotRectangleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + Vector2 he2 = half_extents * 2 * p_scale; + return p_mass * he2.dot(he2) / 12.0; +} + +void GodotRectangleShape2D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR2); + + half_extents = p_data; + configure(Rect2(-half_extents, half_extents * 2.0)); +} + +Variant GodotRectangleShape2D::get_data() const { + return half_extents; +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + +void GodotCapsuleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + Vector2 n = p_normal; + + real_t d = n.y; + + if (Math::abs(d) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_THRESHOLD)) { + // make it flat + n.y = 0.0; + n.normalize(); + n *= radius; + + r_amount = 2; + r_supports[0] = n; + r_supports[0].y += height * 0.5 - radius; + r_supports[1] = n; + r_supports[1].y -= height * 0.5 - radius; + + } else { + real_t h = height * 0.5 - radius; + + n *= radius; + n.y += (d > 0) ? h : -h; + r_amount = 1; + *r_supports = n; + } +} + +bool GodotCapsuleShape2D::contains_point(const Vector2 &p_point) const { + Vector2 p = p_point; + p.y = Math::abs(p.y); + p.y -= height * 0.5 - radius; + if (p.y < 0) { + p.y = 0; + } + + return p.length_squared() < radius * radius; +} + +bool GodotCapsuleShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + real_t d = 1e10; + Vector2 n = (p_end - p_begin).normalized(); + bool collided = false; + + //try spheres + for (int i = 0; i < 2; i++) { + Vector2 begin = p_begin; + Vector2 end = p_end; + real_t ofs = (i == 0) ? -height * 0.5 + radius : height * 0.5 - radius; + begin.y += ofs; + end.y += ofs; + + Vector2 line_vec = end - begin; + + real_t a, b, c; + + a = line_vec.dot(line_vec); + b = 2 * begin.dot(line_vec); + c = begin.dot(begin) - radius * radius; + + real_t sqrtterm = b * b - 4 * a * c; + + if (sqrtterm < 0) { + continue; + } + + sqrtterm = Math::sqrt(sqrtterm); + real_t res = (-b - sqrtterm) / (2 * a); + + if (res < 0 || res > 1 + CMP_EPSILON) { + continue; + } + + Vector2 point = begin + line_vec * res; + Vector2 pointf(point.x, point.y - ofs); + real_t pd = n.dot(pointf); + if (pd < d) { + r_point = pointf; + r_normal = point.normalized(); + d = pd; + collided = true; + } + } + + Vector2 rpos, rnorm; + if (Rect2(Point2(-radius, -height * 0.5 + radius), Size2(radius * 2.0, height - radius * 2)).intersects_segment(p_begin, p_end, &rpos, &rnorm)) { + real_t pd = n.dot(rpos); + if (pd < d) { + r_point = rpos; + r_normal = rnorm; + d = pd; + collided = true; + } + } + + //return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal); + return collided; //todo +} + +real_t GodotCapsuleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + Vector2 he2 = Vector2(radius * 2, height) * p_scale; + return p_mass * he2.dot(he2) / 12.0; +} + +void GodotCapsuleShape2D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY && p_data.get_type() != Variant::VECTOR2); + + if (p_data.get_type() == Variant::ARRAY) { + Array arr = p_data; + ERR_FAIL_COND(arr.size() != 2); + height = arr[0]; + radius = arr[1]; + } else { + Point2 p = p_data; + radius = p.x; + height = p.y; + } + + Point2 he(radius, height * 0.5); + configure(Rect2(-he, he * 2)); +} + +Variant GodotCapsuleShape2D::get_data() const { + return Point2(height, radius); +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + +void GodotConvexPolygonShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + int support_idx = -1; + real_t d = -1e10; + r_amount = 0; + + for (int i = 0; i < point_count; i++) { + //test point + real_t ld = p_normal.dot(points[i].pos); + if (ld > d) { + support_idx = i; + d = ld; + } + + //test segment + if (points[i].normal.dot(p_normal) > _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) { + r_amount = 2; + r_supports[0] = points[i].pos; + r_supports[1] = points[(i + 1) % point_count].pos; + return; + } + } + + ERR_FAIL_COND_MSG(support_idx == -1, "Convex polygon shape support not found."); + + r_amount = 1; + r_supports[0] = points[support_idx].pos; +} + +bool GodotConvexPolygonShape2D::contains_point(const Vector2 &p_point) const { + bool out = false; + bool in = false; + + for (int i = 0; i < point_count; i++) { + real_t d = points[i].normal.dot(p_point) - points[i].normal.dot(points[i].pos); + if (d > 0) { + out = true; + } else { + in = true; + } + } + + return in != out; +} + +bool GodotConvexPolygonShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + Vector2 n = (p_end - p_begin).normalized(); + real_t d = 1e10; + bool inters = false; + + for (int i = 0; i < point_count; i++) { + //hmm.. no can do.. + /* + if (d.dot(points[i].normal)>=0) + continue; + */ + + Vector2 res; + + if (!Geometry2D::segment_intersects_segment(p_begin, p_end, points[i].pos, points[(i + 1) % point_count].pos, &res)) { + continue; + } + + real_t nd = n.dot(res); + if (nd < d) { + d = nd; + r_point = res; + r_normal = points[i].normal; + inters = true; + } + } + + return inters; +} + +real_t GodotConvexPolygonShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + ERR_FAIL_COND_V_MSG(point_count == 0, 0, "Convex polygon shape has no points."); + Rect2 aabb; + aabb.position = points[0].pos * p_scale; + for (int i = 0; i < point_count; i++) { + aabb.expand_to(points[i].pos * p_scale); + } + + return p_mass * aabb.size.dot(aabb.size) / 12.0; +} + +void GodotConvexPolygonShape2D::set_data(const Variant &p_data) { +#ifdef REAL_T_IS_DOUBLE + ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY); +#else + ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY); +#endif + + if (points) { + memdelete_arr(points); + } + points = nullptr; + point_count = 0; + + if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) { + Vector arr = p_data; + ERR_FAIL_COND(arr.size() == 0); + point_count = arr.size(); + points = memnew_arr(Point, point_count); + const Vector2 *r = arr.ptr(); + + for (int i = 0; i < point_count; i++) { + points[i].pos = r[i]; + } + + for (int i = 0; i < point_count; i++) { + Vector2 p = points[i].pos; + Vector2 pn = points[(i + 1) % point_count].pos; + points[i].normal = (pn - p).orthogonal().normalized(); + } + } else { + Vector dvr = p_data; + point_count = dvr.size() / 4; + ERR_FAIL_COND(point_count == 0); + + points = memnew_arr(Point, point_count); + const real_t *r = dvr.ptr(); + + for (int i = 0; i < point_count; i++) { + int idx = i << 2; + points[i].pos.x = r[idx + 0]; + points[i].pos.y = r[idx + 1]; + points[i].normal.x = r[idx + 2]; + points[i].normal.y = r[idx + 3]; + } + } + + ERR_FAIL_COND(point_count == 0); + Rect2 aabb; + aabb.position = points[0].pos; + for (int i = 1; i < point_count; i++) { + aabb.expand_to(points[i].pos); + } + + configure(aabb); +} + +Variant GodotConvexPolygonShape2D::get_data() const { + Vector dvr; + + dvr.resize(point_count); + + for (int i = 0; i < point_count; i++) { + dvr.set(i, points[i].pos); + } + + return dvr; +} + +GodotConvexPolygonShape2D::~GodotConvexPolygonShape2D() { + if (points) { + memdelete_arr(points); + } +} + +////////////////////////////////////////////////// + +void GodotConcavePolygonShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + real_t d = -1e10; + int idx = -1; + for (int i = 0; i < points.size(); i++) { + real_t ld = p_normal.dot(points[i]); + if (ld > d) { + d = ld; + idx = i; + } + } + + r_amount = 1; + ERR_FAIL_COND(idx == -1); + *r_supports = points[idx]; +} + +bool GodotConcavePolygonShape2D::contains_point(const Vector2 &p_point) const { + return false; //sorry +} + +bool GodotConcavePolygonShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + if (segments.size() == 0 || points.size() == 0) { + return false; + } + + uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth); + + enum { + TEST_AABB_BIT = 0, + VISIT_LEFT_BIT = 1, + VISIT_RIGHT_BIT = 2, + VISIT_DONE_BIT = 3, + VISITED_BIT_SHIFT = 29, + NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1, + VISITED_BIT_MASK = ~NODE_IDX_MASK, + + }; + + Vector2 n = (p_end - p_begin).normalized(); + real_t d = 1e10; + bool inters = false; + + /* + for(int i=0;i> VISITED_BIT_SHIFT) { + case TEST_AABB_BIT: { + bool valid = bvh.aabb.intersects_segment(p_begin, p_end); + if (!valid) { + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + + } else { + if (bvh.left < 0) { + const Segment &s = segmentptr[bvh.right]; + Vector2 a = pointptr[s.points[0]]; + Vector2 b = pointptr[s.points[1]]; + + Vector2 res; + + if (Geometry2D::segment_intersects_segment(p_begin, p_end, a, b, &res)) { + real_t nd = n.dot(res); + if (nd < d) { + d = nd; + r_point = res; + r_normal = (b - a).orthogonal().normalized(); + inters = true; + } + } + + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + + } else { + stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; + } + } + } + continue; + case VISIT_LEFT_BIT: { + stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = bvh.left | TEST_AABB_BIT; + level++; + } + continue; + case VISIT_RIGHT_BIT: { + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = bvh.right | TEST_AABB_BIT; + level++; + } + continue; + case VISIT_DONE_BIT: { + if (level == 0) { + done = true; + break; + } else { + level--; + } + } + continue; + } + + if (done) { + break; + } + } + + if (inters) { + if (n.dot(r_normal) > 0) { + r_normal = -r_normal; + } + } + + return inters; +} + +int GodotConcavePolygonShape2D::_generate_bvh(BVH *p_bvh, int p_len, int p_depth) { + if (p_len == 1) { + bvh_depth = MAX(p_depth, bvh_depth); + bvh.push_back(*p_bvh); + return bvh.size() - 1; + } + + //else sort best + + Rect2 global_aabb = p_bvh[0].aabb; + for (int i = 1; i < p_len; i++) { + global_aabb = global_aabb.merge(p_bvh[i].aabb); + } + + if (global_aabb.size.x > global_aabb.size.y) { + SortArray sort; + sort.sort(p_bvh, p_len); + + } else { + SortArray sort; + sort.sort(p_bvh, p_len); + } + + int median = p_len / 2; + + BVH node; + node.aabb = global_aabb; + int node_idx = bvh.size(); + bvh.push_back(node); + + int l = _generate_bvh(p_bvh, median, p_depth + 1); + int r = _generate_bvh(&p_bvh[median], p_len - median, p_depth + 1); + bvh.write[node_idx].left = l; + bvh.write[node_idx].right = r; + + return node_idx; +} + +void GodotConcavePolygonShape2D::set_data(const Variant &p_data) { +#ifdef REAL_T_IS_DOUBLE + ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY); +#else + ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY); +#endif + + Rect2 aabb; + + if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) { + Vector p2arr = p_data; + int len = p2arr.size(); + ERR_FAIL_COND(len % 2); + + segments.clear(); + points.clear(); + bvh.clear(); + bvh_depth = 1; + + if (len == 0) { + configure(aabb); + return; + } + + const Vector2 *arr = p2arr.ptr(); + + Map pointmap; + for (int i = 0; i < len; i += 2) { + Point2 p1 = arr[i]; + Point2 p2 = arr[i + 1]; + int idx_p1, idx_p2; + + if (pointmap.has(p1)) { + idx_p1 = pointmap[p1]; + } else { + idx_p1 = pointmap.size(); + pointmap[p1] = idx_p1; + } + + if (pointmap.has(p2)) { + idx_p2 = pointmap[p2]; + } else { + idx_p2 = pointmap.size(); + pointmap[p2] = idx_p2; + } + + Segment s; + s.points[0] = idx_p1; + s.points[1] = idx_p2; + segments.push_back(s); + } + + points.resize(pointmap.size()); + aabb.position = pointmap.front()->key(); + for (const KeyValue &E : pointmap) { + aabb.expand_to(E.key); + points.write[E.value] = E.key; + } + + Vector main_vbh; + main_vbh.resize(segments.size()); + for (int i = 0; i < main_vbh.size(); i++) { + main_vbh.write[i].aabb.position = points[segments[i].points[0]]; + main_vbh.write[i].aabb.expand_to(points[segments[i].points[1]]); + main_vbh.write[i].left = -1; + main_vbh.write[i].right = i; + } + + _generate_bvh(main_vbh.ptrw(), main_vbh.size(), 1); + + } else { + //dictionary with arrays + } + + configure(aabb); +} + +Variant GodotConcavePolygonShape2D::get_data() const { + Vector rsegments; + int len = segments.size(); + rsegments.resize(len * 2); + Vector2 *w = rsegments.ptrw(); + for (int i = 0; i < len; i++) { + w[(i << 1) + 0] = points[segments[i].points[0]]; + w[(i << 1) + 1] = points[segments[i].points[1]]; + } + + return rsegments; +} + +void GodotConcavePolygonShape2D::cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const { + uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth); + + enum { + TEST_AABB_BIT = 0, + VISIT_LEFT_BIT = 1, + VISIT_RIGHT_BIT = 2, + VISIT_DONE_BIT = 3, + VISITED_BIT_SHIFT = 29, + NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1, + VISITED_BIT_MASK = ~NODE_IDX_MASK, + + }; + + /* + for(int i=0;i> VISITED_BIT_SHIFT) { + case TEST_AABB_BIT: { + bool valid = p_local_aabb.intersects(bvh.aabb); + if (!valid) { + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + + } else { + if (bvh.left < 0) { + const Segment &s = segmentptr[bvh.right]; + Vector2 a = pointptr[s.points[0]]; + Vector2 b = pointptr[s.points[1]]; + + GodotSegmentShape2D ss(a, b, (b - a).orthogonal().normalized()); + + if (p_callback(p_userdata, &ss)) { + return; + } + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + + } else { + stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; + } + } + } + continue; + case VISIT_LEFT_BIT: { + stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = bvh.left | TEST_AABB_BIT; + level++; + } + continue; + case VISIT_RIGHT_BIT: { + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = bvh.right | TEST_AABB_BIT; + level++; + } + continue; + case VISIT_DONE_BIT: { + if (level == 0) { + return; + } else { + level--; + } + } + continue; + } + } +} diff --git a/servers/physics_2d/godot_shape_2d.h b/servers/physics_2d/godot_shape_2d.h new file mode 100644 index 0000000000..25d113aafb --- /dev/null +++ b/servers/physics_2d/godot_shape_2d.h @@ -0,0 +1,536 @@ +/*************************************************************************/ +/* godot_shape_2d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_SHAPE_2D_H +#define GODOT_SHAPE_2D_H + +#include "servers/physics_server_2d.h" +#define _SEGMENT_IS_VALID_SUPPORT_THRESHOLD 0.99998 + +class GodotShape2D; + +class GodotShapeOwner2D { +public: + virtual void _shape_changed() = 0; + virtual void remove_shape(GodotShape2D *p_shape) = 0; + + virtual ~GodotShapeOwner2D() {} +}; + +class GodotShape2D { + RID self; + Rect2 aabb; + bool configured = false; + real_t custom_bias = 0.0; + + Map owners; + +protected: + void configure(const Rect2 &p_aabb); + +public: + _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + virtual PhysicsServer2D::ShapeType get_type() const = 0; + + _FORCE_INLINE_ Rect2 get_aabb() const { return aabb; } + _FORCE_INLINE_ bool is_configured() const { return configured; } + + virtual bool allows_one_way_collision() const { return true; } + + virtual bool is_concave() const { return false; } + + virtual bool contains_point(const Vector2 &p_point) const = 0; + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const = 0; + virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const = 0; + virtual Vector2 get_support(const Vector2 &p_normal) const; + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const = 0; + + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const = 0; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const = 0; + virtual void set_data(const Variant &p_data) = 0; + virtual Variant get_data() const = 0; + + _FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias = p_bias; } + _FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; } + + void add_owner(GodotShapeOwner2D *p_owner); + void remove_owner(GodotShapeOwner2D *p_owner); + bool is_owner(GodotShapeOwner2D *p_owner) const; + const Map &get_owners() const; + + _FORCE_INLINE_ void get_supports_transformed_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_xform, Vector2 *r_supports, int &r_amount) const { + get_supports(p_xform.basis_xform_inv(p_normal).normalized(), r_supports, r_amount); + for (int i = 0; i < r_amount; i++) { + r_supports[i] = p_xform.xform(r_supports[i]); + } + + if (r_amount == 1) { + if (Math::abs(p_normal.dot(p_cast.normalized())) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_THRESHOLD)) { + //make line because they are parallel + r_amount = 2; + r_supports[1] = r_supports[0] + p_cast; + } else if (p_cast.dot(p_normal) > 0) { + //normal points towards cast, add cast + r_supports[0] += p_cast; + } + + } else { + if (Math::abs(p_normal.dot(p_cast.normalized())) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_THRESHOLD)) { + //optimize line and make it larger because they are parallel + if ((r_supports[1] - r_supports[0]).dot(p_cast) > 0) { + //larger towards 1 + r_supports[1] += p_cast; + } else { + //larger towards 0 + r_supports[0] += p_cast; + } + } else if (p_cast.dot(p_normal) > 0) { + //normal points towards cast, add cast + r_supports[0] += p_cast; + r_supports[1] += p_cast; + } + } + } + GodotShape2D() {} + virtual ~GodotShape2D(); +}; + +//let the optimizer do the magic +#define DEFAULT_PROJECT_RANGE_CAST \ + virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { \ + project_range_cast(p_cast, p_normal, p_transform, r_min, r_max); \ + } \ + _FORCE_INLINE_ void project_range_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { \ + real_t mina, maxa; \ + real_t minb, maxb; \ + Transform2D ofsb = p_transform; \ + ofsb.elements[2] += p_cast; \ + project_range(p_normal, p_transform, mina, maxa); \ + project_range(p_normal, ofsb, minb, maxb); \ + r_min = MIN(mina, minb); \ + r_max = MAX(maxa, maxb); \ + } + +class GodotWorldBoundaryShape2D : public GodotShape2D { + Vector2 normal; + real_t d = 0.0; + +public: + _FORCE_INLINE_ Vector2 get_normal() const { return normal; } + _FORCE_INLINE_ real_t get_d() const { return d; } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_WORLD_BOUNDARY; } + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + //real large + r_min = -1e10; + r_max = 1e10; + } + + virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { + project_range_cast(p_cast, p_normal, p_transform, r_min, r_max); + } + + _FORCE_INLINE_ void project_range_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + //real large + r_min = -1e10; + r_max = 1e10; + } +}; + +class GodotSeparationRayShape2D : public GodotShape2D { + real_t length = 0.0; + bool slide_on_slope = false; + +public: + _FORCE_INLINE_ real_t get_length() const { return length; } + _FORCE_INLINE_ bool get_slide_on_slope() const { return slide_on_slope; } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_SEPARATION_RAY; } + + virtual bool allows_one_way_collision() const override { return false; } + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + //real large + r_max = p_normal.dot(p_transform.get_origin()); + r_min = p_normal.dot(p_transform.xform(Vector2(0, length))); + if (r_max < r_min) { + SWAP(r_max, r_min); + } + } + + DEFAULT_PROJECT_RANGE_CAST + + _FORCE_INLINE_ GodotSeparationRayShape2D() {} + _FORCE_INLINE_ GodotSeparationRayShape2D(real_t p_length) { length = p_length; } +}; + +class GodotSegmentShape2D : public GodotShape2D { + Vector2 a; + Vector2 b; + Vector2 n; + +public: + _FORCE_INLINE_ const Vector2 &get_a() const { return a; } + _FORCE_INLINE_ const Vector2 &get_b() const { return b; } + _FORCE_INLINE_ const Vector2 &get_normal() const { return n; } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_SEGMENT; } + + _FORCE_INLINE_ Vector2 get_xformed_normal(const Transform2D &p_xform) const { + return (p_xform.xform(b) - p_xform.xform(a)).normalized().orthogonal(); + } + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + //real large + r_max = p_normal.dot(p_transform.xform(a)); + r_min = p_normal.dot(p_transform.xform(b)); + if (r_max < r_min) { + SWAP(r_max, r_min); + } + } + + DEFAULT_PROJECT_RANGE_CAST + + _FORCE_INLINE_ GodotSegmentShape2D() {} + _FORCE_INLINE_ GodotSegmentShape2D(const Vector2 &p_a, const Vector2 &p_b, const Vector2 &p_n) { + a = p_a; + b = p_b; + n = p_n; + } +}; + +class GodotCircleShape2D : public GodotShape2D { + real_t radius; + +public: + _FORCE_INLINE_ const real_t &get_radius() const { return radius; } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CIRCLE; } + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + //real large + real_t d = p_normal.dot(p_transform.get_origin()); + + // figure out scale at point + Vector2 local_normal = p_transform.basis_xform_inv(p_normal); + real_t scale = local_normal.length(); + + r_min = d - (radius)*scale; + r_max = d + (radius)*scale; + } + + DEFAULT_PROJECT_RANGE_CAST +}; + +class GodotRectangleShape2D : public GodotShape2D { + Vector2 half_extents; + +public: + _FORCE_INLINE_ const Vector2 &get_half_extents() const { return half_extents; } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_RECTANGLE; } + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + // no matter the angle, the box is mirrored anyway + r_max = -1e20; + r_min = 1e20; + for (int i = 0; i < 4; i++) { + real_t d = p_normal.dot(p_transform.xform(Vector2(((i & 1) * 2 - 1) * half_extents.x, ((i >> 1) * 2 - 1) * half_extents.y))); + + if (d > r_max) { + r_max = d; + } + if (d < r_min) { + r_min = d; + } + } + } + + _FORCE_INLINE_ Vector2 get_circle_axis(const Transform2D &p_xform, const Transform2D &p_xform_inv, const Vector2 &p_circle) const { + Vector2 local_v = p_xform_inv.xform(p_circle); + + Vector2 he( + (local_v.x < 0) ? -half_extents.x : half_extents.x, + (local_v.y < 0) ? -half_extents.y : half_extents.y); + + return (p_xform.xform(he) - p_circle).normalized(); + } + + _FORCE_INLINE_ Vector2 get_box_axis(const Transform2D &p_xform, const Transform2D &p_xform_inv, const GodotRectangleShape2D *p_B, const Transform2D &p_B_xform, const Transform2D &p_B_xform_inv) const { + Vector2 a, b; + + { + Vector2 local_v = p_xform_inv.xform(p_B_xform.get_origin()); + + Vector2 he( + (local_v.x < 0) ? -half_extents.x : half_extents.x, + (local_v.y < 0) ? -half_extents.y : half_extents.y); + + a = p_xform.xform(he); + } + { + Vector2 local_v = p_B_xform_inv.xform(p_xform.get_origin()); + + Vector2 he( + (local_v.x < 0) ? -p_B->half_extents.x : p_B->half_extents.x, + (local_v.y < 0) ? -p_B->half_extents.y : p_B->half_extents.y); + + b = p_B_xform.xform(he); + } + + return (a - b).normalized(); + } + + DEFAULT_PROJECT_RANGE_CAST +}; + +class GodotCapsuleShape2D : public GodotShape2D { + real_t radius = 0.0; + real_t height = 0.0; + +public: + _FORCE_INLINE_ const real_t &get_radius() const { return radius; } + _FORCE_INLINE_ const real_t &get_height() const { return height; } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CAPSULE; } + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + // no matter the angle, the box is mirrored anyway + Vector2 n = p_transform.basis_xform_inv(p_normal).normalized(); + real_t h = height * 0.5 - radius; + + n *= radius; + n.y += (n.y > 0) ? h : -h; + + r_max = p_normal.dot(p_transform.xform(n)); + r_min = p_normal.dot(p_transform.xform(-n)); + + if (r_max < r_min) { + SWAP(r_max, r_min); + } + + //ERR_FAIL_COND( r_max < r_min ); + } + + DEFAULT_PROJECT_RANGE_CAST +}; + +class GodotConvexPolygonShape2D : public GodotShape2D { + struct Point { + Vector2 pos; + Vector2 normal; //normal to next segment + }; + + Point *points = nullptr; + int point_count = 0; + +public: + _FORCE_INLINE_ int get_point_count() const { return point_count; } + _FORCE_INLINE_ const Vector2 &get_point(int p_idx) const { return points[p_idx].pos; } + _FORCE_INLINE_ const Vector2 &get_segment_normal(int p_idx) const { return points[p_idx].normal; } + _FORCE_INLINE_ Vector2 get_xformed_segment_normal(const Transform2D &p_xform, int p_idx) const { + Vector2 a = points[p_idx].pos; + p_idx++; + Vector2 b = points[p_idx == point_count ? 0 : p_idx].pos; + return (p_xform.xform(b) - p_xform.xform(a)).normalized().orthogonal(); + } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CONVEX_POLYGON; } + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + if (!points || point_count <= 0) { + r_min = r_max = 0; + return; + } + + r_min = r_max = p_normal.dot(p_transform.xform(points[0].pos)); + for (int i = 1; i < point_count; i++) { + real_t d = p_normal.dot(p_transform.xform(points[i].pos)); + if (d > r_max) { + r_max = d; + } + if (d < r_min) { + r_min = d; + } + } + } + + DEFAULT_PROJECT_RANGE_CAST + + GodotConvexPolygonShape2D() {} + ~GodotConvexPolygonShape2D(); +}; + +class GodotConcaveShape2D : public GodotShape2D { +public: + virtual bool is_concave() const override { return true; } + + // Returns true to stop the query. + typedef bool (*QueryCallback)(void *p_userdata, GodotShape2D *p_convex); + + virtual void cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0; +}; + +class GodotConcavePolygonShape2D : public GodotConcaveShape2D { + struct Segment { + int points[2] = {}; + }; + + Vector segments; + Vector points; + + struct BVH { + Rect2 aabb; + int left = 0, right = 0; + }; + + Vector bvh; + int bvh_depth = 0; + + struct BVH_CompareX { + _FORCE_INLINE_ bool operator()(const BVH &a, const BVH &b) const { + return (a.aabb.position.x + a.aabb.size.x * 0.5) < (b.aabb.position.x + b.aabb.size.x * 0.5); + } + }; + + struct BVH_CompareY { + _FORCE_INLINE_ bool operator()(const BVH &a, const BVH &b) const { + return (a.aabb.position.y + a.aabb.size.y * 0.5) < (b.aabb.position.y + b.aabb.size.y * 0.5); + } + }; + + int _generate_bvh(BVH *p_bvh, int p_len, int p_depth); + +public: + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CONCAVE_POLYGON; } + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { + r_min = 0; + r_max = 0; + ERR_FAIL_MSG("Unsupported call to project_rangev in GodotConcavePolygonShape2D"); + } + + void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + r_min = 0; + r_max = 0; + ERR_FAIL_MSG("Unsupported call to project_range in GodotConcavePolygonShape2D"); + } + + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override { return 0; } + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + virtual void cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override; + + DEFAULT_PROJECT_RANGE_CAST +}; + +#undef DEFAULT_PROJECT_RANGE_CAST + +#endif // GODOT_SHAPE_2D_H diff --git a/servers/physics_2d/godot_space_2d.cpp b/servers/physics_2d/godot_space_2d.cpp new file mode 100644 index 0000000000..d72014a8ed --- /dev/null +++ b/servers/physics_2d/godot_space_2d.cpp @@ -0,0 +1,1213 @@ +/*************************************************************************/ +/* godot_space_2d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_space_2d.h" + +#include "godot_collision_solver_2d.h" +#include "godot_physics_server_2d.h" + +#include "core/os/os.h" +#include "core/templates/pair.h" + +#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05 + +_FORCE_INLINE_ static bool _can_collide_with(GodotCollisionObject2D *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { + if (!(p_object->get_collision_layer() & p_collision_mask)) { + return false; + } + + if (p_object->get_type() == GodotCollisionObject2D::TYPE_AREA && !p_collide_with_areas) { + return false; + } + + if (p_object->get_type() == GodotCollisionObject2D::TYPE_BODY && !p_collide_with_bodies) { + return false; + } + + return true; +} + +int GodotPhysicsDirectSpaceState2D::_intersect_point_impl(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point, bool p_filter_by_canvas, ObjectID p_canvas_instance_id) { + if (p_result_max <= 0) { + return 0; + } + + Rect2 aabb; + aabb.position = p_point - Vector2(0.00001, 0.00001); + aabb.size = Vector2(0.00002, 0.00002); + + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + int cc = 0; + + for (int i = 0; i < amount; i++) { + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + continue; + } + + if (p_exclude.has(space->intersection_query_results[i]->get_self())) { + continue; + } + + const GodotCollisionObject2D *col_obj = space->intersection_query_results[i]; + + if (p_pick_point && !col_obj->is_pickable()) { + continue; + } + + if (p_filter_by_canvas && col_obj->get_canvas_instance_id() != p_canvas_instance_id) { + continue; + } + + int shape_idx = space->intersection_query_subindex_results[i]; + + GodotShape2D *shape = col_obj->get_shape(shape_idx); + + Vector2 local_point = (col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).affine_inverse().xform(p_point); + + if (!shape->contains_point(local_point)) { + continue; + } + + if (cc >= p_result_max) { + continue; + } + + r_results[cc].collider_id = col_obj->get_instance_id(); + if (r_results[cc].collider_id.is_valid()) { + r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id); + } + r_results[cc].rid = col_obj->get_self(); + r_results[cc].shape = shape_idx; + + cc++; + } + + return cc; +} + +int GodotPhysicsDirectSpaceState2D::intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point) { + return _intersect_point_impl(p_point, r_results, p_result_max, p_exclude, p_collision_mask, p_collide_with_bodies, p_collide_with_areas, p_pick_point); +} + +int GodotPhysicsDirectSpaceState2D::intersect_point_on_canvas(const Vector2 &p_point, ObjectID p_canvas_instance_id, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point) { + return _intersect_point_impl(p_point, r_results, p_result_max, p_exclude, p_collision_mask, p_collide_with_bodies, p_collide_with_areas, p_pick_point, true, p_canvas_instance_id); +} + +bool GodotPhysicsDirectSpaceState2D::intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { + ERR_FAIL_COND_V(space->locked, false); + + Vector2 begin, end; + Vector2 normal; + begin = p_from; + end = p_to; + normal = (end - begin).normalized(); + + int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + //todo, create another array that references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision + + bool collided = false; + Vector2 res_point, res_normal; + int res_shape; + const GodotCollisionObject2D *res_obj; + real_t min_d = 1e10; + + for (int i = 0; i < amount; i++) { + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + continue; + } + + if (p_exclude.has(space->intersection_query_results[i]->get_self())) { + continue; + } + + const GodotCollisionObject2D *col_obj = space->intersection_query_results[i]; + + int shape_idx = space->intersection_query_subindex_results[i]; + Transform2D inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform(); + + Vector2 local_from = inv_xform.xform(begin); + Vector2 local_to = inv_xform.xform(end); + + /*local_from = col_obj->get_inv_transform().xform(begin); + local_from = col_obj->get_shape_inv_transform(shape_idx).xform(local_from); + + local_to = col_obj->get_inv_transform().xform(end); + local_to = col_obj->get_shape_inv_transform(shape_idx).xform(local_to);*/ + + const GodotShape2D *shape = col_obj->get_shape(shape_idx); + + Vector2 shape_point, shape_normal; + + if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) { + Transform2D xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + shape_point = xform.xform(shape_point); + + real_t ld = normal.dot(shape_point); + + if (ld < min_d) { + min_d = ld; + res_point = shape_point; + res_normal = inv_xform.basis_xform_inv(shape_normal).normalized(); + res_shape = shape_idx; + res_obj = col_obj; + collided = true; + } + } + } + + if (!collided) { + return false; + } + + r_result.collider_id = res_obj->get_instance_id(); + if (r_result.collider_id.is_valid()) { + r_result.collider = ObjectDB::get_instance(r_result.collider_id); + } + r_result.normal = res_normal; + r_result.position = res_point; + r_result.rid = res_obj->get_self(); + r_result.shape = res_shape; + + return true; +} + +int GodotPhysicsDirectSpaceState2D::intersect_shape(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { + if (p_result_max <= 0) { + return 0; + } + + GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_shape); + ERR_FAIL_COND_V(!shape, 0); + + Rect2 aabb = p_xform.xform(shape->get_aabb()); + aabb = aabb.grow(p_margin); + + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + int cc = 0; + + for (int i = 0; i < amount; i++) { + if (cc >= p_result_max) { + break; + } + + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + continue; + } + + if (p_exclude.has(space->intersection_query_results[i]->get_self())) { + continue; + } + + const GodotCollisionObject2D *col_obj = space->intersection_query_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; + + if (!GodotCollisionSolver2D::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), nullptr, nullptr, nullptr, p_margin)) { + continue; + } + + r_results[cc].collider_id = col_obj->get_instance_id(); + if (r_results[cc].collider_id.is_valid()) { + r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id); + } + r_results[cc].rid = col_obj->get_self(); + r_results[cc].shape = shape_idx; + + cc++; + } + + return cc; +} + +bool GodotPhysicsDirectSpaceState2D::cast_motion(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { + GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_shape); + ERR_FAIL_COND_V(!shape, false); + + Rect2 aabb = p_xform.xform(shape->get_aabb()); + aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion + aabb = aabb.grow(p_margin); + + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + real_t best_safe = 1; + real_t best_unsafe = 1; + + for (int i = 0; i < amount; i++) { + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + continue; + } + + if (p_exclude.has(space->intersection_query_results[i]->get_self())) { + continue; //ignore excluded + } + + const GodotCollisionObject2D *col_obj = space->intersection_query_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; + + Transform2D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + //test initial overlap, does it collide if going all the way? + if (!GodotCollisionSolver2D::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_margin)) { + continue; + } + + //test initial overlap, ignore objects it's inside of. + if (GodotCollisionSolver2D::solve(shape, p_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_margin)) { + continue; + } + + Vector2 mnormal = p_motion.normalized(); + + //just do kinematic solving + real_t low = 0.0; + real_t hi = 1.0; + real_t fraction_coeff = 0.5; + for (int j = 0; j < 8; j++) { //steps should be customizable.. + real_t fraction = low + (hi - low) * fraction_coeff; + + Vector2 sep = mnormal; //important optimization for this to work fast enough + bool collided = GodotCollisionSolver2D::solve(shape, p_xform, p_motion * fraction, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, &sep, p_margin); + + if (collided) { + hi = fraction; + if ((j == 0) || (low > 0.0)) { // Did it not collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When colliding again, converge faster towards low fraction + // for more accurate results with long motions that collide near the start. + fraction_coeff = 0.25; + } + } else { + low = fraction; + if ((j == 0) || (hi < 1.0)) { // Did it collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When not colliding again, converge faster towards high fraction + // for more accurate results with long motions that collide near the end. + fraction_coeff = 0.75; + } + } + } + + if (low < best_safe) { + best_safe = low; + best_unsafe = hi; + } + } + + p_closest_safe = best_safe; + p_closest_unsafe = best_unsafe; + + return true; +} + +bool GodotPhysicsDirectSpaceState2D::collide_shape(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { + if (p_result_max <= 0) { + return false; + } + + GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_shape); + ERR_FAIL_COND_V(!shape, 0); + + Rect2 aabb = p_shape_xform.xform(shape->get_aabb()); + aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion + aabb = aabb.grow(p_margin); + + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + bool collided = false; + r_result_count = 0; + + GodotPhysicsServer2D::CollCbkData cbk; + cbk.max = p_result_max; + cbk.amount = 0; + cbk.passed = 0; + cbk.ptr = r_results; + GodotCollisionSolver2D::CallbackResult cbkres = GodotPhysicsServer2D::_shape_col_cbk; + + GodotPhysicsServer2D::CollCbkData *cbkptr = &cbk; + + for (int i = 0; i < amount; i++) { + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + continue; + } + + const GodotCollisionObject2D *col_obj = space->intersection_query_results[i]; + + if (p_exclude.has(col_obj->get_self())) { + continue; + } + + int shape_idx = space->intersection_query_subindex_results[i]; + + cbk.valid_dir = Vector2(); + cbk.valid_depth = 0; + + if (GodotCollisionSolver2D::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, nullptr, p_margin)) { + collided = cbk.amount > 0; + } + } + + r_result_count = cbk.amount; + + return collided; +} + +struct _RestCallbackData2D { + const GodotCollisionObject2D *object = nullptr; + const GodotCollisionObject2D *best_object = nullptr; + int local_shape = 0; + int best_local_shape = 0; + int shape = 0; + int best_shape = 0; + Vector2 best_contact; + Vector2 best_normal; + real_t best_len = 0.0; + Vector2 valid_dir; + real_t valid_depth = 0.0; + real_t min_allowed_depth = 0.0; +}; + +static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) { + _RestCallbackData2D *rd = (_RestCallbackData2D *)p_userdata; + + Vector2 contact_rel = p_point_B - p_point_A; + real_t len = contact_rel.length(); + + if (len < rd->min_allowed_depth) { + return; + } + + if (len <= rd->best_len) { + return; + } + + Vector2 normal = contact_rel / len; + + if (rd->valid_dir != Vector2()) { + if (len > rd->valid_depth) { + return; + } + + if (rd->valid_dir.dot(normal) > -CMP_EPSILON) { + return; + } + } + + rd->best_len = len; + rd->best_contact = p_point_B; + rd->best_normal = normal; + rd->best_object = rd->object; + rd->best_shape = rd->shape; + rd->best_local_shape = rd->local_shape; +} + +bool GodotPhysicsDirectSpaceState2D::rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, ShapeRestInfo *r_info, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { + GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_shape); + ERR_FAIL_COND_V(!shape, 0); + + real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; + + Rect2 aabb = p_shape_xform.xform(shape->get_aabb()); + aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion + aabb = aabb.grow(p_margin); + + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + _RestCallbackData2D rcd; + rcd.best_len = 0; + rcd.best_object = nullptr; + rcd.best_shape = 0; + rcd.min_allowed_depth = min_contact_depth; + + for (int i = 0; i < amount; i++) { + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + continue; + } + + const GodotCollisionObject2D *col_obj = space->intersection_query_results[i]; + + if (p_exclude.has(col_obj->get_self())) { + continue; + } + + int shape_idx = space->intersection_query_subindex_results[i]; + + rcd.valid_dir = Vector2(); + rcd.object = col_obj; + rcd.shape = shape_idx; + rcd.local_shape = 0; + bool sc = GodotCollisionSolver2D::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, nullptr, p_margin); + if (!sc) { + continue; + } + } + + if (rcd.best_len == 0 || !rcd.best_object) { + return false; + } + + r_info->collider_id = rcd.best_object->get_instance_id(); + r_info->shape = rcd.best_shape; + r_info->normal = rcd.best_normal; + r_info->point = rcd.best_contact; + r_info->rid = rcd.best_object->get_self(); + if (rcd.best_object->get_type() == GodotCollisionObject2D::TYPE_BODY) { + const GodotBody2D *body = static_cast(rcd.best_object); + Vector2 rel_vec = r_info->point - (body->get_transform().get_origin() + body->get_center_of_mass()); + r_info->linear_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); + + } else { + r_info->linear_velocity = Vector2(); + } + + return true; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////// + +int GodotSpace2D::_cull_aabb_for_body(GodotBody2D *p_body, const Rect2 &p_aabb) { + int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results); + + for (int i = 0; i < amount; i++) { + bool keep = true; + + if (intersection_query_results[i] == p_body) { + keep = false; + } else if (intersection_query_results[i]->get_type() == GodotCollisionObject2D::TYPE_AREA) { + keep = false; + } else if (!p_body->collides_with(static_cast(intersection_query_results[i]))) { + keep = false; + } else if (static_cast(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) { + keep = false; + } + + if (!keep) { + if (i < amount - 1) { + SWAP(intersection_query_results[i], intersection_query_results[amount - 1]); + SWAP(intersection_query_subindex_results[i], intersection_query_subindex_results[amount - 1]); + } + + amount--; + i--; + } + } + + return amount; +} + +bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::MotionParameters &p_parameters, PhysicsServer2D::MotionResult *r_result) { + //give me back regular physics engine logic + //this is madness + //and most people using this function will think + //what it does is simpler than using physics + //this took about a week to get right.. + //but is it right? who knows at this point.. + + if (r_result) { + r_result->collider_id = ObjectID(); + r_result->collider_shape = 0; + } + Rect2 body_aabb; + + bool shapes_found = false; + + for (int i = 0; i < p_body->get_shape_count(); i++) { + if (p_body->is_shape_disabled(i)) { + continue; + } + + if (!shapes_found) { + body_aabb = p_body->get_shape_aabb(i); + shapes_found = true; + } else { + body_aabb = body_aabb.merge(p_body->get_shape_aabb(i)); + } + } + + if (!shapes_found) { + if (r_result) { + *r_result = PhysicsServer2D::MotionResult(); + r_result->travel = p_parameters.motion; + } + return false; + } + + // Undo the currently transform the physics server is aware of and apply the provided one + body_aabb = p_parameters.from.xform(p_body->get_inv_transform().xform(body_aabb)); + body_aabb = body_aabb.grow(p_parameters.margin); + + static const int max_excluded_shape_pairs = 32; + ExcludedShapeSW excluded_shape_pairs[max_excluded_shape_pairs]; + int excluded_shape_pair_count = 0; + + real_t min_contact_depth = p_parameters.margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; + + real_t motion_length = p_parameters.motion.length(); + Vector2 motion_normal = p_parameters.motion / motion_length; + + Transform2D body_transform = p_parameters.from; + + bool recovered = false; + + { + //STEP 1, FREE BODY IF STUCK + + const int max_results = 32; + int recover_attempts = 4; + Vector2 sr[max_results * 2]; + + do { + GodotPhysicsServer2D::CollCbkData cbk; + cbk.max = max_results; + cbk.amount = 0; + cbk.passed = 0; + cbk.ptr = sr; + cbk.invalid_by_dir = 0; + excluded_shape_pair_count = 0; //last step is the one valid + + GodotPhysicsServer2D::CollCbkData *cbkptr = &cbk; + GodotCollisionSolver2D::CallbackResult cbkres = GodotPhysicsServer2D::_shape_col_cbk; + + bool collided = false; + + int amount = _cull_aabb_for_body(p_body, body_aabb); + + for (int j = 0; j < p_body->get_shape_count(); j++) { + if (p_body->is_shape_disabled(j)) { + continue; + } + + GodotShape2D *body_shape = p_body->get_shape(j); + Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); + + for (int i = 0; i < amount; i++) { + const GodotCollisionObject2D *col_obj = intersection_query_results[i]; + if (p_parameters.exclude_bodies.has(col_obj->get_self())) { + continue; + } + if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { + continue; + } + + int shape_idx = intersection_query_subindex_results[i]; + + Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + + if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) { + cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); + + real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); + cbk.valid_depth = MAX(owc_margin, p_parameters.margin); //user specified, but never less than actual margin or it won't work + cbk.invalid_by_dir = 0; + + if (col_obj->get_type() == GodotCollisionObject2D::TYPE_BODY) { + const GodotBody2D *b = static_cast(col_obj); + if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_DYNAMIC) { + //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction + Vector2 lv = b->get_linear_velocity(); + //compute displacement from linear velocity + Vector2 motion = lv * last_step; + real_t motion_len = motion.length(); + motion.normalize(); + cbk.valid_depth += motion_len * MAX(motion.dot(-cbk.valid_dir), 0.0); + } + } + } else { + cbk.valid_dir = Vector2(); + cbk.valid_depth = 0; + cbk.invalid_by_dir = 0; + } + + int current_passed = cbk.passed; //save how many points passed collision + bool did_collide = false; + + GodotShape2D *against_shape = col_obj->get_shape(shape_idx); + if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, p_parameters.margin)) { + did_collide = cbk.passed > current_passed; //more passed, so collision actually existed + } + + if (!did_collide && cbk.invalid_by_dir > 0) { + //this shape must be excluded + if (excluded_shape_pair_count < max_excluded_shape_pairs) { + ExcludedShapeSW esp; + esp.local_shape = body_shape; + esp.against_object = col_obj; + esp.against_shape_index = shape_idx; + excluded_shape_pairs[excluded_shape_pair_count++] = esp; + } + } + + if (did_collide) { + collided = true; + } + } + } + + if (!collided) { + break; + } + + recovered = true; + + Vector2 recover_motion; + for (int i = 0; i < cbk.amount; i++) { + Vector2 a = sr[i * 2 + 0]; + Vector2 b = sr[i * 2 + 1]; + + // Compute plane on b towards a. + Vector2 n = (a - b).normalized(); + real_t d = n.dot(b); + + // Compute depth on recovered motion. + real_t depth = n.dot(a + recover_motion) - d; + if (depth > min_contact_depth + CMP_EPSILON) { + // Only recover if there is penetration. + recover_motion -= n * (depth - min_contact_depth) * 0.4; + } + } + + if (recover_motion == Vector2()) { + collided = false; + break; + } + + body_transform.elements[2] += recover_motion; + body_aabb.position += recover_motion; + + recover_attempts--; + + } while (recover_attempts); + } + + real_t safe = 1.0; + real_t unsafe = 1.0; + int best_shape = -1; + + { + // STEP 2 ATTEMPT MOTION + + Rect2 motion_aabb = body_aabb; + motion_aabb.position += p_parameters.motion; + motion_aabb = motion_aabb.merge(body_aabb); + + int amount = _cull_aabb_for_body(p_body, motion_aabb); + + for (int body_shape_idx = 0; body_shape_idx < p_body->get_shape_count(); body_shape_idx++) { + if (p_body->is_shape_disabled(body_shape_idx)) { + continue; + } + + GodotShape2D *body_shape = p_body->get_shape(body_shape_idx); + + // Colliding separation rays allows to properly snap to the ground, + // otherwise it's not needed in regular motion. + if (!p_parameters.collide_separation_ray && (body_shape->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY)) { + // When slide on slope is on, separation ray shape acts like a regular shape. + if (!static_cast(body_shape)->get_slide_on_slope()) { + continue; + } + } + + Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(body_shape_idx); + + bool stuck = false; + + real_t best_safe = 1; + real_t best_unsafe = 1; + + for (int i = 0; i < amount; i++) { + const GodotCollisionObject2D *col_obj = intersection_query_results[i]; + if (p_parameters.exclude_bodies.has(col_obj->get_self())) { + continue; + } + if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { + continue; + } + + int col_shape_idx = intersection_query_subindex_results[i]; + GodotShape2D *against_shape = col_obj->get_shape(col_shape_idx); + + bool excluded = false; + + for (int k = 0; k < excluded_shape_pair_count; k++) { + if (excluded_shape_pairs[k].local_shape == body_shape && excluded_shape_pairs[k].against_object == col_obj && excluded_shape_pairs[k].against_shape_index == col_shape_idx) { + excluded = true; + break; + } + } + + if (excluded) { + continue; + } + + Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(col_shape_idx); + //test initial overlap, does it collide if going all the way? + if (!GodotCollisionSolver2D::solve(body_shape, body_shape_xform, p_parameters.motion, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) { + continue; + } + + //test initial overlap + if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) { + if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) { + Vector2 direction = col_obj_shape_xform.get_axis(1).normalized(); + if (motion_normal.dot(direction) < 0) { + continue; + } + } + + stuck = true; + break; + } + + //just do kinematic solving + real_t low = 0.0; + real_t hi = 1.0; + real_t fraction_coeff = 0.5; + for (int k = 0; k < 8; k++) { //steps should be customizable.. + real_t fraction = low + (hi - low) * fraction_coeff; + + Vector2 sep = motion_normal; //important optimization for this to work fast enough + bool collided = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, p_parameters.motion * fraction, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0); + + if (collided) { + hi = fraction; + if ((k == 0) || (low > 0.0)) { // Did it not collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When colliding again, converge faster towards low fraction + // for more accurate results with long motions that collide near the start. + fraction_coeff = 0.25; + } + } else { + low = fraction; + if ((k == 0) || (hi < 1.0)) { // Did it collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When not colliding again, converge faster towards high fraction + // for more accurate results with long motions that collide near the end. + fraction_coeff = 0.75; + } + } + } + + if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) { + Vector2 cd[2]; + GodotPhysicsServer2D::CollCbkData cbk; + cbk.max = 1; + cbk.amount = 0; + cbk.passed = 0; + cbk.ptr = cd; + cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); + + cbk.valid_depth = 10e20; + + Vector2 sep = motion_normal; //important optimization for this to work fast enough + bool collided = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, p_parameters.motion * (hi + contact_max_allowed_penetration), col_obj->get_shape(col_shape_idx), col_obj_shape_xform, Vector2(), GodotPhysicsServer2D::_shape_col_cbk, &cbk, &sep, 0); + if (!collided || cbk.amount == 0) { + continue; + } + } + + if (low < best_safe) { + best_safe = low; + best_unsafe = hi; + } + } + + if (stuck) { + safe = 0; + unsafe = 0; + best_shape = body_shape_idx; //sadly it's the best + break; + } + if (best_safe == 1.0) { + continue; + } + if (best_safe < safe) { + safe = best_safe; + unsafe = best_unsafe; + best_shape = body_shape_idx; + } + } + } + + bool collided = false; + + if (recovered || (safe < 1)) { + if (safe >= 1) { + best_shape = -1; //no best shape with cast, reset to -1 + } + + //it collided, let's get the rest info in unsafe advance + Transform2D ugt = body_transform; + ugt.elements[2] += p_parameters.motion * unsafe; + + _RestCallbackData2D rcd; + rcd.best_len = 0; + rcd.best_object = nullptr; + rcd.best_shape = 0; + + // Allowed depth can't be lower than motion length, in order to handle contacts at low speed. + rcd.min_allowed_depth = MIN(motion_length, min_contact_depth); + + int from_shape = best_shape != -1 ? best_shape : 0; + int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count(); + + for (int j = from_shape; j < to_shape; j++) { + if (p_body->is_shape_disabled(j)) { + continue; + } + + Transform2D body_shape_xform = ugt * p_body->get_shape_transform(j); + GodotShape2D *body_shape = p_body->get_shape(j); + + body_aabb.position += p_parameters.motion * unsafe; + + int amount = _cull_aabb_for_body(p_body, body_aabb); + + for (int i = 0; i < amount; i++) { + const GodotCollisionObject2D *col_obj = intersection_query_results[i]; + if (p_parameters.exclude_bodies.has(col_obj->get_self())) { + continue; + } + if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { + continue; + } + + int shape_idx = intersection_query_subindex_results[i]; + + GodotShape2D *against_shape = col_obj->get_shape(shape_idx); + + bool excluded = false; + for (int k = 0; k < excluded_shape_pair_count; k++) { + if (excluded_shape_pairs[k].local_shape == body_shape && excluded_shape_pairs[k].against_object == col_obj && excluded_shape_pairs[k].against_shape_index == shape_idx) { + excluded = true; + break; + } + } + if (excluded) { + continue; + } + + Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + + if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) { + rcd.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); + + real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); + rcd.valid_depth = MAX(owc_margin, p_parameters.margin); //user specified, but never less than actual margin or it won't work + + if (col_obj->get_type() == GodotCollisionObject2D::TYPE_BODY) { + const GodotBody2D *b = static_cast(col_obj); + if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_DYNAMIC) { + //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction + Vector2 lv = b->get_linear_velocity(); + //compute displacement from linear velocity + Vector2 motion = lv * last_step; + real_t motion_len = motion.length(); + motion.normalize(); + rcd.valid_depth += motion_len * MAX(motion.dot(-rcd.valid_dir), 0.0); + } + } + } else { + rcd.valid_dir = Vector2(); + rcd.valid_depth = 0; + } + + rcd.object = col_obj; + rcd.shape = shape_idx; + rcd.local_shape = j; + bool sc = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), _rest_cbk_result, &rcd, nullptr, p_parameters.margin); + if (!sc) { + continue; + } + } + } + + if (rcd.best_len != 0) { + if (r_result) { + r_result->collider = rcd.best_object->get_self(); + r_result->collider_id = rcd.best_object->get_instance_id(); + r_result->collider_shape = rcd.best_shape; + r_result->collision_local_shape = rcd.best_local_shape; + r_result->collision_normal = rcd.best_normal; + r_result->collision_point = rcd.best_contact; + r_result->collision_depth = rcd.best_len; + r_result->collision_safe_fraction = safe; + r_result->collision_unsafe_fraction = unsafe; + + const GodotBody2D *body = static_cast(rcd.best_object); + Vector2 rel_vec = r_result->collision_point - (body->get_transform().get_origin() + body->get_center_of_mass()); + r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); + + r_result->travel = safe * p_parameters.motion; + r_result->remainder = p_parameters.motion - safe * p_parameters.motion; + r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin()); + } + + collided = true; + } + } + + if (!collided && r_result) { + r_result->travel = p_parameters.motion; + r_result->remainder = Vector2(); + r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin()); + } + + return collided; +} + +void *GodotSpace2D::_broadphase_pair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_self) { + if (!A->interacts_with(B)) { + return nullptr; + } + + GodotCollisionObject2D::Type type_A = A->get_type(); + GodotCollisionObject2D::Type type_B = B->get_type(); + if (type_A > type_B) { + SWAP(A, B); + SWAP(p_subindex_A, p_subindex_B); + SWAP(type_A, type_B); + } + + GodotSpace2D *self = (GodotSpace2D *)p_self; + self->collision_pairs++; + + if (type_A == GodotCollisionObject2D::TYPE_AREA) { + GodotArea2D *area = static_cast(A); + if (type_B == GodotCollisionObject2D::TYPE_AREA) { + GodotArea2D *area_b = static_cast(B); + GodotArea2Pair2D *area2_pair = memnew(GodotArea2Pair2D(area_b, p_subindex_B, area, p_subindex_A)); + return area2_pair; + } else { + GodotBody2D *body = static_cast(B); + GodotAreaPair2D *area_pair = memnew(GodotAreaPair2D(body, p_subindex_B, area, p_subindex_A)); + return area_pair; + } + + } else { + GodotBodyPair2D *b = memnew(GodotBodyPair2D((GodotBody2D *)A, p_subindex_A, (GodotBody2D *)B, p_subindex_B)); + return b; + } + + return nullptr; +} + +void GodotSpace2D::_broadphase_unpair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_data, void *p_self) { + if (!p_data) { + return; + } + + GodotSpace2D *self = (GodotSpace2D *)p_self; + self->collision_pairs--; + GodotConstraint2D *c = (GodotConstraint2D *)p_data; + memdelete(c); +} + +const SelfList::List &GodotSpace2D::get_active_body_list() const { + return active_list; +} + +void GodotSpace2D::body_add_to_active_list(SelfList *p_body) { + active_list.add(p_body); +} + +void GodotSpace2D::body_remove_from_active_list(SelfList *p_body) { + active_list.remove(p_body); +} + +void GodotSpace2D::body_add_to_mass_properties_update_list(SelfList *p_body) { + mass_properties_update_list.add(p_body); +} + +void GodotSpace2D::body_remove_from_mass_properties_update_list(SelfList *p_body) { + mass_properties_update_list.remove(p_body); +} + +GodotBroadPhase2D *GodotSpace2D::get_broadphase() { + return broadphase; +} + +void GodotSpace2D::add_object(GodotCollisionObject2D *p_object) { + ERR_FAIL_COND(objects.has(p_object)); + objects.insert(p_object); +} + +void GodotSpace2D::remove_object(GodotCollisionObject2D *p_object) { + ERR_FAIL_COND(!objects.has(p_object)); + objects.erase(p_object); +} + +const Set &GodotSpace2D::get_objects() const { + return objects; +} + +void GodotSpace2D::body_add_to_state_query_list(SelfList *p_body) { + state_query_list.add(p_body); +} + +void GodotSpace2D::body_remove_from_state_query_list(SelfList *p_body) { + state_query_list.remove(p_body); +} + +void GodotSpace2D::area_add_to_monitor_query_list(SelfList *p_area) { + monitor_query_list.add(p_area); +} + +void GodotSpace2D::area_remove_from_monitor_query_list(SelfList *p_area) { + monitor_query_list.remove(p_area); +} + +void GodotSpace2D::area_add_to_moved_list(SelfList *p_area) { + area_moved_list.add(p_area); +} + +void GodotSpace2D::area_remove_from_moved_list(SelfList *p_area) { + area_moved_list.remove(p_area); +} + +const SelfList::List &GodotSpace2D::get_moved_area_list() const { + return area_moved_list; +} + +void GodotSpace2D::call_queries() { + while (state_query_list.first()) { + GodotBody2D *b = state_query_list.first()->self(); + state_query_list.remove(state_query_list.first()); + b->call_queries(); + } + + while (monitor_query_list.first()) { + GodotArea2D *a = monitor_query_list.first()->self(); + monitor_query_list.remove(monitor_query_list.first()); + a->call_queries(); + } +} + +void GodotSpace2D::setup() { + contact_debug_count = 0; + + while (mass_properties_update_list.first()) { + mass_properties_update_list.first()->self()->update_mass_properties(); + mass_properties_update_list.remove(mass_properties_update_list.first()); + } +} + +void GodotSpace2D::update() { + broadphase->update(); +} + +void GodotSpace2D::set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_value) { + switch (p_param) { + case PhysicsServer2D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: + contact_recycle_radius = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_SEPARATION: + contact_max_separation = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: + contact_max_allowed_penetration = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: + body_linear_velocity_sleep_threshold = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: + body_angular_velocity_sleep_threshold = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_BODY_TIME_TO_SLEEP: + body_time_to_sleep = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: + constraint_bias = p_value; + break; + } +} + +real_t GodotSpace2D::get_param(PhysicsServer2D::SpaceParameter p_param) const { + switch (p_param) { + case PhysicsServer2D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: + return contact_recycle_radius; + case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_SEPARATION: + return contact_max_separation; + case PhysicsServer2D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: + return contact_max_allowed_penetration; + case PhysicsServer2D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: + return body_linear_velocity_sleep_threshold; + case PhysicsServer2D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: + return body_angular_velocity_sleep_threshold; + case PhysicsServer2D::SPACE_PARAM_BODY_TIME_TO_SLEEP: + return body_time_to_sleep; + case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: + return constraint_bias; + } + return 0; +} + +void GodotSpace2D::lock() { + locked = true; +} + +void GodotSpace2D::unlock() { + locked = false; +} + +bool GodotSpace2D::is_locked() const { + return locked; +} + +GodotPhysicsDirectSpaceState2D *GodotSpace2D::get_direct_state() { + return direct_access; +} + +GodotSpace2D::GodotSpace2D() { + body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_linear", 2.0); + body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_angular", Math::deg2rad(8.0)); + body_time_to_sleep = GLOBAL_DEF("physics/2d/time_before_sleep", 0.5); + ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/time_before_sleep", PropertyInfo(Variant::FLOAT, "physics/2d/time_before_sleep", PROPERTY_HINT_RANGE, "0,5,0.01,or_greater")); + + broadphase = GodotBroadPhase2D::create_func(); + broadphase->set_pair_callback(_broadphase_pair, this); + broadphase->set_unpair_callback(_broadphase_unpair, this); + + direct_access = memnew(GodotPhysicsDirectSpaceState2D); + direct_access->space = this; +} + +GodotSpace2D::~GodotSpace2D() { + memdelete(broadphase); + memdelete(direct_access); +} diff --git a/servers/physics_2d/godot_space_2d.h b/servers/physics_2d/godot_space_2d.h new file mode 100644 index 0000000000..97e2928a9d --- /dev/null +++ b/servers/physics_2d/godot_space_2d.h @@ -0,0 +1,212 @@ +/*************************************************************************/ +/* godot_space_2d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_SPACE_2D_H +#define GODOT_SPACE_2D_H + +#include "godot_area_2d.h" +#include "godot_area_pair_2d.h" +#include "godot_body_2d.h" +#include "godot_body_pair_2d.h" +#include "godot_broad_phase_2d.h" +#include "godot_collision_object_2d.h" + +#include "core/config/project_settings.h" +#include "core/templates/hash_map.h" +#include "core/typedefs.h" + +class GodotPhysicsDirectSpaceState2D : public PhysicsDirectSpaceState2D { + GDCLASS(GodotPhysicsDirectSpaceState2D, PhysicsDirectSpaceState2D); + + int _intersect_point_impl(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point, bool p_filter_by_canvas = false, ObjectID p_canvas_instance_id = ObjectID()); + +public: + GodotSpace2D *space = nullptr; + + virtual int intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_point = false) override; + virtual int intersect_point_on_canvas(const Vector2 &p_point, ObjectID p_canvas_instance_id, ShapeResult *r_results, int p_result_max, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_point = false) override; + virtual bool intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual int intersect_shape(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual bool cast_motion(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual bool collide_shape(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual bool rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, ShapeRestInfo *r_info, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + + GodotPhysicsDirectSpaceState2D() {} +}; + +class GodotSpace2D { +public: + enum ElapsedTime { + ELAPSED_TIME_INTEGRATE_FORCES, + ELAPSED_TIME_GENERATE_ISLANDS, + ELAPSED_TIME_SETUP_CONSTRAINTS, + ELAPSED_TIME_SOLVE_CONSTRAINTS, + ELAPSED_TIME_INTEGRATE_VELOCITIES, + ELAPSED_TIME_MAX + + }; + +private: + struct ExcludedShapeSW { + GodotShape2D *local_shape = nullptr; + const GodotCollisionObject2D *against_object = nullptr; + int against_shape_index = 0; + }; + + uint64_t elapsed_time[ELAPSED_TIME_MAX] = {}; + + GodotPhysicsDirectSpaceState2D *direct_access = nullptr; + RID self; + + GodotBroadPhase2D *broadphase; + SelfList::List active_list; + SelfList::List mass_properties_update_list; + SelfList::List state_query_list; + SelfList::List monitor_query_list; + SelfList::List area_moved_list; + + static void *_broadphase_pair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_self); + static void _broadphase_unpair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_data, void *p_self); + + Set objects; + + GodotArea2D *area = nullptr; + + real_t contact_recycle_radius = 1.0; + real_t contact_max_separation = 1.5; + real_t contact_max_allowed_penetration = 0.3; + real_t constraint_bias = 0.2; + + enum { + INTERSECTION_QUERY_MAX = 2048 + }; + + GodotCollisionObject2D *intersection_query_results[INTERSECTION_QUERY_MAX]; + int intersection_query_subindex_results[INTERSECTION_QUERY_MAX]; + + real_t body_linear_velocity_sleep_threshold = 0.0; + real_t body_angular_velocity_sleep_threshold = 0.0; + real_t body_time_to_sleep = 0.0; + + bool locked = false; + + real_t last_step = 0.001; + + int island_count = 0; + int active_objects = 0; + int collision_pairs = 0; + + int _cull_aabb_for_body(GodotBody2D *p_body, const Rect2 &p_aabb); + + Vector contact_debug; + int contact_debug_count = 0; + + friend class GodotPhysicsDirectSpaceState2D; + +public: + _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + void set_default_area(GodotArea2D *p_area) { area = p_area; } + GodotArea2D *get_default_area() const { return area; } + + const SelfList::List &get_active_body_list() const; + void body_add_to_active_list(SelfList *p_body); + void body_remove_from_active_list(SelfList *p_body); + void body_add_to_mass_properties_update_list(SelfList *p_body); + void body_remove_from_mass_properties_update_list(SelfList *p_body); + void area_add_to_moved_list(SelfList *p_area); + void area_remove_from_moved_list(SelfList *p_area); + const SelfList::List &get_moved_area_list() const; + + void body_add_to_state_query_list(SelfList *p_body); + void body_remove_from_state_query_list(SelfList *p_body); + + void area_add_to_monitor_query_list(SelfList *p_area); + void area_remove_from_monitor_query_list(SelfList *p_area); + + GodotBroadPhase2D *get_broadphase(); + + void add_object(GodotCollisionObject2D *p_object); + void remove_object(GodotCollisionObject2D *p_object); + const Set &get_objects() const; + + _FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; } + _FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; } + _FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; } + _FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; } + _FORCE_INLINE_ real_t get_body_linear_velocity_sleep_threshold() const { return body_linear_velocity_sleep_threshold; } + _FORCE_INLINE_ real_t get_body_angular_velocity_sleep_threshold() const { return body_angular_velocity_sleep_threshold; } + _FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; } + + void update(); + void setup(); + void call_queries(); + + bool is_locked() const; + void lock(); + void unlock(); + + real_t get_last_step() const { return last_step; } + void set_last_step(real_t p_step) { last_step = p_step; } + + void set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_value); + real_t get_param(PhysicsServer2D::SpaceParameter p_param) const; + + void set_island_count(int p_island_count) { island_count = p_island_count; } + int get_island_count() const { return island_count; } + + void set_active_objects(int p_active_objects) { active_objects = p_active_objects; } + int get_active_objects() const { return active_objects; } + + int get_collision_pairs() const { return collision_pairs; } + + bool test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::MotionParameters &p_parameters, PhysicsServer2D::MotionResult *r_result); + + void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); } + _FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.is_empty(); } + _FORCE_INLINE_ void add_debug_contact(const Vector2 &p_contact) { + if (contact_debug_count < contact_debug.size()) { + contact_debug.write[contact_debug_count++] = p_contact; + } + } + _FORCE_INLINE_ Vector get_debug_contacts() { return contact_debug; } + _FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; } + + GodotPhysicsDirectSpaceState2D *get_direct_state(); + + void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; } + uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; } + + GodotSpace2D(); + ~GodotSpace2D(); +}; + +#endif // GODOT_SPACE_2D_H diff --git a/servers/physics_2d/godot_step_2d.cpp b/servers/physics_2d/godot_step_2d.cpp new file mode 100644 index 0000000000..3010315571 --- /dev/null +++ b/servers/physics_2d/godot_step_2d.cpp @@ -0,0 +1,308 @@ +/*************************************************************************/ +/* godot_step_2d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_step_2d.h" + +#include "core/os/os.h" + +#define BODY_ISLAND_COUNT_RESERVE 128 +#define BODY_ISLAND_SIZE_RESERVE 512 +#define ISLAND_COUNT_RESERVE 128 +#define ISLAND_SIZE_RESERVE 512 +#define CONSTRAINT_COUNT_RESERVE 1024 + +void GodotStep2D::_populate_island(GodotBody2D *p_body, LocalVector &p_body_island, LocalVector &p_constraint_island) { + p_body->set_island_step(_step); + + if (p_body->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) { + // Only dynamic bodies are tested for activation. + p_body_island.push_back(p_body); + } + + for (const Pair &E : p_body->get_constraint_list()) { + GodotConstraint2D *constraint = (GodotConstraint2D *)E.first; + if (constraint->get_island_step() == _step) { + continue; // Already processed. + } + constraint->set_island_step(_step); + p_constraint_island.push_back(constraint); + all_constraints.push_back(constraint); + + for (int i = 0; i < constraint->get_body_count(); i++) { + if (i == E.second) { + continue; + } + GodotBody2D *other_body = constraint->get_body_ptr()[i]; + if (other_body->get_island_step() == _step) { + continue; // Already processed. + } + if (other_body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC) { + continue; // Static bodies don't connect islands. + } + _populate_island(other_body, p_body_island, p_constraint_island); + } + } +} + +void GodotStep2D::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) { + GodotConstraint2D *constraint = all_constraints[p_constraint_index]; + constraint->setup(delta); +} + +void GodotStep2D::_pre_solve_island(LocalVector &p_constraint_island) const { + uint32_t constraint_count = p_constraint_island.size(); + uint32_t valid_constraint_count = 0; + for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { + GodotConstraint2D *constraint = p_constraint_island[constraint_index]; + if (p_constraint_island[constraint_index]->pre_solve(delta)) { + // Keep this constraint for solving. + p_constraint_island[valid_constraint_count++] = constraint; + } + } + p_constraint_island.resize(valid_constraint_count); +} + +void GodotStep2D::_solve_island(uint32_t p_island_index, void *p_userdata) const { + const LocalVector &constraint_island = constraint_islands[p_island_index]; + + for (int i = 0; i < iterations; i++) { + uint32_t constraint_count = constraint_island.size(); + for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { + constraint_island[constraint_index]->solve(delta); + } + } +} + +void GodotStep2D::_check_suspend(LocalVector &p_body_island) const { + bool can_sleep = true; + + uint32_t body_count = p_body_island.size(); + for (uint32_t body_index = 0; body_index < body_count; ++body_index) { + GodotBody2D *body = p_body_island[body_index]; + + if (!body->sleep_test(delta)) { + can_sleep = false; + } + } + + // Put all to sleep or wake up everyone. + for (uint32_t body_index = 0; body_index < body_count; ++body_index) { + GodotBody2D *body = p_body_island[body_index]; + + bool active = body->is_active(); + + if (active == can_sleep) { + body->set_active(!can_sleep); + } + } +} + +void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta, int p_iterations) { + p_space->lock(); // can't access space during this + + p_space->setup(); //update inertias, etc + + p_space->set_last_step(p_delta); + + iterations = p_iterations; + delta = p_delta; + + const SelfList::List *body_list = &p_space->get_active_body_list(); + + /* INTEGRATE FORCES */ + + uint64_t profile_begtime = OS::get_singleton()->get_ticks_usec(); + uint64_t profile_endtime = 0; + + int active_count = 0; + + const SelfList *b = body_list->first(); + while (b) { + b->self()->integrate_forces(p_delta); + b = b->next(); + active_count++; + } + + p_space->set_active_objects(active_count); + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* GENERATE CONSTRAINT ISLANDS FOR MOVING AREAS */ + + uint32_t island_count = 0; + + const SelfList::List &aml = p_space->get_moved_area_list(); + + while (aml.first()) { + for (const Set::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { + GodotConstraint2D *constraint = E->get(); + if (constraint->get_island_step() == _step) { + continue; + } + constraint->set_island_step(_step); + + // Each constraint can be on a separate island for areas as there's no solving phase. + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + + all_constraints.push_back(constraint); + constraint_island.push_back(constraint); + } + p_space->area_remove_from_moved_list((SelfList *)aml.first()); //faster to remove here + } + + /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */ + + b = body_list->first(); + + uint32_t body_island_count = 0; + + while (b) { + GodotBody2D *body = b->self(); + + if (body->get_island_step() != _step) { + ++body_island_count; + if (body_islands.size() < body_island_count) { + body_islands.resize(body_island_count); + } + LocalVector &body_island = body_islands[body_island_count - 1]; + body_island.clear(); + body_island.reserve(BODY_ISLAND_SIZE_RESERVE); + + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + constraint_island.reserve(ISLAND_SIZE_RESERVE); + + _populate_island(body, body_island, constraint_island); + + if (body_island.is_empty()) { + --body_island_count; + } + + if (constraint_island.is_empty()) { + --island_count; + } + } + b = b->next(); + } + + p_space->set_island_count((int)island_count); + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* SETUP CONSTRAINTS / PROCESS COLLISIONS */ + + uint32_t total_contraint_count = all_constraints.size(); + work_pool.do_work(total_contraint_count, this, &GodotStep2D::_setup_contraint, nullptr); + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* PRE-SOLVE CONSTRAINT ISLANDS */ + + // Warning: This doesn't run on threads, because it involves thread-unsafe processing. + for (uint32_t island_index = 0; island_index < island_count; ++island_index) { + _pre_solve_island(constraint_islands[island_index]); + } + + /* SOLVE CONSTRAINT ISLANDS */ + + // Warning: _solve_island modifies the constraint islands for optimization purpose, + // their content is not reliable after these calls and shouldn't be used anymore. + if (island_count > 1) { + work_pool.do_work(island_count, this, &GodotStep2D::_solve_island, nullptr); + } else if (island_count > 0) { + _solve_island(0); + } + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* INTEGRATE VELOCITIES */ + + b = body_list->first(); + while (b) { + const SelfList *n = b->next(); + b->self()->integrate_velocities(p_delta); + b = n; // in case it shuts itself down + } + + /* SLEEP / WAKE UP ISLANDS */ + + for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) { + _check_suspend(body_islands[island_index]); + } + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime); + //profile_begtime=profile_endtime; + } + + all_constraints.clear(); + + p_space->update(); + p_space->unlock(); + _step++; +} + +GodotStep2D::GodotStep2D() { + body_islands.reserve(BODY_ISLAND_COUNT_RESERVE); + constraint_islands.reserve(ISLAND_COUNT_RESERVE); + all_constraints.reserve(CONSTRAINT_COUNT_RESERVE); + + work_pool.init(); +} + +GodotStep2D::~GodotStep2D() { + work_pool.finish(); +} diff --git a/servers/physics_2d/godot_step_2d.h b/servers/physics_2d/godot_step_2d.h new file mode 100644 index 0000000000..efec243632 --- /dev/null +++ b/servers/physics_2d/godot_step_2d.h @@ -0,0 +1,63 @@ +/*************************************************************************/ +/* godot_step_2d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_STEP_2D_H +#define GODOT_STEP_2D_H + +#include "godot_space_2d.h" + +#include "core/templates/local_vector.h" +#include "core/templates/thread_work_pool.h" + +class GodotStep2D { + uint64_t _step = 1; + + int iterations = 0; + real_t delta = 0.0; + + ThreadWorkPool work_pool; + + LocalVector> body_islands; + LocalVector> constraint_islands; + LocalVector all_constraints; + + void _populate_island(GodotBody2D *p_body, LocalVector &p_body_island, LocalVector &p_constraint_island); + void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr); + void _pre_solve_island(LocalVector &p_constraint_island) const; + void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr) const; + void _check_suspend(LocalVector &p_body_island) const; + +public: + void step(GodotSpace2D *p_space, real_t p_delta, int p_iterations); + GodotStep2D(); + ~GodotStep2D(); +}; + +#endif // GODOT_STEP_2D_H diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp deleted file mode 100644 index b46397b8e6..0000000000 --- a/servers/physics_2d/joints_2d_sw.cpp +++ /dev/null @@ -1,486 +0,0 @@ -/*************************************************************************/ -/* joints_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "joints_2d_sw.h" - -#include "space_2d_sw.h" - -//based on chipmunk joint constraints - -/* Copyright (c) 2007 Scott Lembcke - * - * 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. - */ - -void Joint2DSW::copy_settings_from(Joint2DSW *p_joint) { - set_self(p_joint->get_self()); - set_max_force(p_joint->get_max_force()); - set_bias(p_joint->get_bias()); - set_max_bias(p_joint->get_max_bias()); - disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies()); -} - -static inline real_t k_scalar(Body2DSW *a, Body2DSW *b, const Vector2 &rA, const Vector2 &rB, const Vector2 &n) { - real_t value = 0.0; - - { - value += a->get_inv_mass(); - real_t rcn = (rA - a->get_center_of_mass()).cross(n); - value += a->get_inv_inertia() * rcn * rcn; - } - - if (b) { - value += b->get_inv_mass(); - real_t rcn = (rB - b->get_center_of_mass()).cross(n); - value += b->get_inv_inertia() * rcn * rcn; - } - - return value; -} - -static inline Vector2 -relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB) { - Vector2 sum = a->get_linear_velocity() - (rA - a->get_center_of_mass()).orthogonal() * a->get_angular_velocity(); - if (b) { - return (b->get_linear_velocity() - (rB - b->get_center_of_mass()).orthogonal() * b->get_angular_velocity()) - sum; - } else { - return -sum; - } -} - -static inline real_t -normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vector2 n) { - return relative_velocity(a, b, rA, rB).dot(n); -} - -bool PinJoint2DSW::setup(real_t p_step) { - dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); - dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); - - if (!dynamic_A && !dynamic_B) { - return false; - } - - Space2DSW *space = A->get_space(); - ERR_FAIL_COND_V(!space, false); - - rA = A->get_transform().basis_xform(anchor_A); - rB = B ? B->get_transform().basis_xform(anchor_B) : anchor_B; - - real_t B_inv_mass = B ? B->get_inv_mass() : 0.0; - - Transform2D K1; - K1[0].x = A->get_inv_mass() + B_inv_mass; - K1[1].x = 0.0f; - K1[0].y = 0.0f; - K1[1].y = A->get_inv_mass() + B_inv_mass; - - Transform2D K2; - K2[0].x = A->get_inv_inertia() * rA.y * rA.y; - K2[1].x = -A->get_inv_inertia() * rA.x * rA.y; - K2[0].y = -A->get_inv_inertia() * rA.x * rA.y; - K2[1].y = A->get_inv_inertia() * rA.x * rA.x; - - Transform2D K; - K[0] = K1[0] + K2[0]; - K[1] = K1[1] + K2[1]; - - if (B) { - Transform2D K3; - K3[0].x = B->get_inv_inertia() * rB.y * rB.y; - K3[1].x = -B->get_inv_inertia() * rB.x * rB.y; - K3[0].y = -B->get_inv_inertia() * rB.x * rB.y; - K3[1].y = B->get_inv_inertia() * rB.x * rB.x; - - K[0] += K3[0]; - K[1] += K3[1]; - } - - K[0].x += softness; - K[1].y += softness; - - M = K.affine_inverse(); - - Vector2 gA = rA + A->get_transform().get_origin(); - Vector2 gB = B ? rB + B->get_transform().get_origin() : rB; - - Vector2 delta = gB - gA; - - bias = delta * -(get_bias() == 0 ? space->get_constraint_bias() : get_bias()) * (1.0 / p_step); - - return true; -} - -inline Vector2 custom_cross(const Vector2 &p_vec, real_t p_other) { - return Vector2(p_other * p_vec.y, -p_other * p_vec.x); -} - -bool PinJoint2DSW::pre_solve(real_t p_step) { - // Apply accumulated impulse. - if (dynamic_A) { - A->apply_impulse(-P, rA); - } - if (B && dynamic_B) { - B->apply_impulse(P, rB); - } - - return true; -} - -void PinJoint2DSW::solve(real_t p_step) { - // compute relative velocity - Vector2 vA = A->get_linear_velocity() - custom_cross(rA - A->get_center_of_mass(), A->get_angular_velocity()); - - Vector2 rel_vel; - if (B) { - rel_vel = B->get_linear_velocity() - custom_cross(rB - B->get_center_of_mass(), B->get_angular_velocity()) - vA; - } else { - rel_vel = -vA; - } - - Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness, softness) * P); - - if (dynamic_A) { - A->apply_impulse(-impulse, rA); - } - if (B && dynamic_B) { - B->apply_impulse(impulse, rB); - } - - P += impulse; -} - -void PinJoint2DSW::set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value) { - if (p_param == PhysicsServer2D::PIN_JOINT_SOFTNESS) { - softness = p_value; - } -} - -real_t PinJoint2DSW::get_param(PhysicsServer2D::PinJointParam p_param) const { - if (p_param == PhysicsServer2D::PIN_JOINT_SOFTNESS) { - return softness; - } - ERR_FAIL_V(0); -} - -PinJoint2DSW::PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p_body_b) : - Joint2DSW(_arr, p_body_b ? 2 : 1) { - A = p_body_a; - B = p_body_b; - anchor_A = p_body_a->get_inv_transform().xform(p_pos); - anchor_B = p_body_b ? p_body_b->get_inv_transform().xform(p_pos) : p_pos; - - p_body_a->add_constraint(this, 0); - if (p_body_b) { - p_body_b->add_constraint(this, 1); - } -} - -////////////////////////////////////////////// -////////////////////////////////////////////// -////////////////////////////////////////////// - -static inline void -k_tensor(Body2DSW *a, Body2DSW *b, Vector2 r1, Vector2 r2, Vector2 *k1, Vector2 *k2) { - // calculate mass matrix - // If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross... - real_t k11, k12, k21, k22; - real_t m_sum = a->get_inv_mass() + b->get_inv_mass(); - - // start with I*m_sum - k11 = m_sum; - k12 = 0.0f; - k21 = 0.0f; - k22 = m_sum; - - r1 -= a->get_center_of_mass(); - r2 -= b->get_center_of_mass(); - - // add the influence from r1 - real_t a_i_inv = a->get_inv_inertia(); - real_t r1xsq = r1.x * r1.x * a_i_inv; - real_t r1ysq = r1.y * r1.y * a_i_inv; - real_t r1nxy = -r1.x * r1.y * a_i_inv; - k11 += r1ysq; - k12 += r1nxy; - k21 += r1nxy; - k22 += r1xsq; - - // add the influnce from r2 - real_t b_i_inv = b->get_inv_inertia(); - real_t r2xsq = r2.x * r2.x * b_i_inv; - real_t r2ysq = r2.y * r2.y * b_i_inv; - real_t r2nxy = -r2.x * r2.y * b_i_inv; - k11 += r2ysq; - k12 += r2nxy; - k21 += r2nxy; - k22 += r2xsq; - - // invert - real_t determinant = k11 * k22 - k12 * k21; - ERR_FAIL_COND(determinant == 0.0); - - real_t det_inv = 1.0f / determinant; - *k1 = Vector2(k22 * det_inv, -k12 * det_inv); - *k2 = Vector2(-k21 * det_inv, k11 * det_inv); -} - -static _FORCE_INLINE_ Vector2 -mult_k(const Vector2 &vr, const Vector2 &k1, const Vector2 &k2) { - return Vector2(vr.dot(k1), vr.dot(k2)); -} - -bool GrooveJoint2DSW::setup(real_t p_step) { - dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); - dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); - - if (!dynamic_A && !dynamic_B) { - return false; - } - - Space2DSW *space = A->get_space(); - ERR_FAIL_COND_V(!space, false); - - // calculate endpoints in worldspace - Vector2 ta = A->get_transform().xform(A_groove_1); - Vector2 tb = A->get_transform().xform(A_groove_2); - - // calculate axis - Vector2 n = -(tb - ta).orthogonal().normalized(); - real_t d = ta.dot(n); - - xf_normal = n; - rB = B->get_transform().basis_xform(B_anchor); - - // calculate tangential distance along the axis of rB - real_t td = (B->get_transform().get_origin() + rB).cross(n); - // calculate clamping factor and rB - if (td <= ta.cross(n)) { - clamp = 1.0f; - rA = ta - A->get_transform().get_origin(); - } else if (td >= tb.cross(n)) { - clamp = -1.0f; - rA = tb - A->get_transform().get_origin(); - } else { - clamp = 0.0f; - //joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p); - rA = ((-n.orthogonal() * -td) + n * d) - A->get_transform().get_origin(); - } - - // Calculate mass tensor - k_tensor(A, B, rA, rB, &k1, &k2); - - // compute max impulse - jn_max = get_max_force() * p_step; - - // calculate bias velocity - //cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); - //joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias); - - Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA); - - real_t _b = get_bias(); - gbias = (delta * -(_b == 0 ? space->get_constraint_bias() : _b) * (1.0 / p_step)).limit_length(get_max_bias()); - - correct = true; - return true; -} - -bool GrooveJoint2DSW::pre_solve(real_t p_step) { - // Apply accumulated impulse. - if (dynamic_A) { - A->apply_impulse(-jn_acc, rA); - } - if (dynamic_B) { - B->apply_impulse(jn_acc, rB); - } - - return true; -} - -void GrooveJoint2DSW::solve(real_t p_step) { - // compute impulse - Vector2 vr = relative_velocity(A, B, rA, rB); - - Vector2 j = mult_k(gbias - vr, k1, k2); - Vector2 jOld = jn_acc; - j += jOld; - - jn_acc = (((clamp * j.cross(xf_normal)) > 0) ? j : j.project(xf_normal)).limit_length(jn_max); - - j = jn_acc - jOld; - - if (dynamic_A) { - A->apply_impulse(-j, rA); - } - if (dynamic_B) { - B->apply_impulse(j, rB); - } -} - -GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, Body2DSW *p_body_a, Body2DSW *p_body_b) : - Joint2DSW(_arr, 2) { - A = p_body_a; - B = p_body_b; - - A_groove_1 = A->get_inv_transform().xform(p_a_groove1); - A_groove_2 = A->get_inv_transform().xform(p_a_groove2); - B_anchor = B->get_inv_transform().xform(p_b_anchor); - A_groove_normal = -(A_groove_2 - A_groove_1).normalized().orthogonal(); - - A->add_constraint(this, 0); - B->add_constraint(this, 1); -} - -////////////////////////////////////////////// -////////////////////////////////////////////// -////////////////////////////////////////////// - -bool DampedSpringJoint2DSW::setup(real_t p_step) { - dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); - dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); - - if (!dynamic_A && !dynamic_B) { - return false; - } - - rA = A->get_transform().basis_xform(anchor_A); - rB = B->get_transform().basis_xform(anchor_B); - - Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA); - real_t dist = delta.length(); - - if (dist) { - n = delta / dist; - } else { - n = Vector2(); - } - - real_t k = k_scalar(A, B, rA, rB, n); - n_mass = 1.0f / k; - - target_vrn = 0.0f; - v_coef = 1.0f - Math::exp(-damping * (p_step)*k); - - // Calculate spring force. - real_t f_spring = (rest_length - dist) * stiffness; - j = n * f_spring * (p_step); - - return true; -} - -bool DampedSpringJoint2DSW::pre_solve(real_t p_step) { - // Apply spring force. - if (dynamic_A) { - A->apply_impulse(-j, rA); - } - if (dynamic_B) { - B->apply_impulse(j, rB); - } - - return true; -} - -void DampedSpringJoint2DSW::solve(real_t p_step) { - // compute relative velocity - real_t vrn = normal_relative_velocity(A, B, rA, rB, n) - target_vrn; - - // compute velocity loss from drag - // not 100% certain this is derived correctly, though it makes sense - real_t v_damp = -vrn * v_coef; - target_vrn = vrn + v_damp; - Vector2 j = n * v_damp * n_mass; - - if (dynamic_A) { - A->apply_impulse(-j, rA); - } - if (dynamic_B) { - B->apply_impulse(j, rB); - } -} - -void DampedSpringJoint2DSW::set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value) { - switch (p_param) { - case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: { - rest_length = p_value; - } break; - case PhysicsServer2D::DAMPED_SPRING_DAMPING: { - damping = p_value; - } break; - case PhysicsServer2D::DAMPED_SPRING_STIFFNESS: { - stiffness = p_value; - } break; - } -} - -real_t DampedSpringJoint2DSW::get_param(PhysicsServer2D::DampedSpringParam p_param) const { - switch (p_param) { - case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: { - return rest_length; - } break; - case PhysicsServer2D::DAMPED_SPRING_DAMPING: { - return damping; - } break; - case PhysicsServer2D::DAMPED_SPRING_STIFFNESS: { - return stiffness; - } break; - } - - ERR_FAIL_V(0); -} - -DampedSpringJoint2DSW::DampedSpringJoint2DSW(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, Body2DSW *p_body_a, Body2DSW *p_body_b) : - Joint2DSW(_arr, 2) { - A = p_body_a; - B = p_body_b; - anchor_A = A->get_inv_transform().xform(p_anchor_a); - anchor_B = B->get_inv_transform().xform(p_anchor_b); - - rest_length = p_anchor_a.distance_to(p_anchor_b); - - A->add_constraint(this, 0); - B->add_constraint(this, 1); -} diff --git a/servers/physics_2d/joints_2d_sw.h b/servers/physics_2d/joints_2d_sw.h deleted file mode 100644 index e2a7c0c91e..0000000000 --- a/servers/physics_2d/joints_2d_sw.h +++ /dev/null @@ -1,178 +0,0 @@ -/*************************************************************************/ -/* joints_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 JOINTS_2D_SW_H -#define JOINTS_2D_SW_H - -#include "body_2d_sw.h" -#include "constraint_2d_sw.h" - -class Joint2DSW : public Constraint2DSW { - real_t bias = 0; - real_t max_bias = 3.40282e+38; - real_t max_force = 3.40282e+38; - -protected: - bool dynamic_A = false; - bool dynamic_B = false; - -public: - _FORCE_INLINE_ void set_max_force(real_t p_force) { max_force = p_force; } - _FORCE_INLINE_ real_t get_max_force() const { return max_force; } - - _FORCE_INLINE_ void set_bias(real_t p_bias) { bias = p_bias; } - _FORCE_INLINE_ real_t get_bias() const { return bias; } - - _FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias = p_bias; } - _FORCE_INLINE_ real_t get_max_bias() const { return max_bias; } - - virtual bool setup(real_t p_step) override { return false; } - virtual bool pre_solve(real_t p_step) override { return false; } - virtual void solve(real_t p_step) override {} - - void copy_settings_from(Joint2DSW *p_joint); - - virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_MAX; } - Joint2DSW(Body2DSW **p_body_ptr = nullptr, int p_body_count = 0) : - Constraint2DSW(p_body_ptr, p_body_count) {} - - virtual ~Joint2DSW() { - for (int i = 0; i < get_body_count(); i++) { - Body2DSW *body = get_body_ptr()[i]; - if (body) { - body->remove_constraint(this, i); - } - } - }; -}; - -class PinJoint2DSW : public Joint2DSW { - union { - struct { - Body2DSW *A; - Body2DSW *B; - }; - - Body2DSW *_arr[2] = { nullptr, nullptr }; - }; - - Transform2D M; - Vector2 rA, rB; - Vector2 anchor_A; - Vector2 anchor_B; - Vector2 bias; - Vector2 P; - real_t softness = 0.0; - -public: - virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_PIN; } - - virtual bool setup(real_t p_step) override; - virtual bool pre_solve(real_t p_step) override; - virtual void solve(real_t p_step) override; - - void set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value); - real_t get_param(PhysicsServer2D::PinJointParam p_param) const; - - PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p_body_b = nullptr); -}; - -class GrooveJoint2DSW : public Joint2DSW { - union { - struct { - Body2DSW *A; - Body2DSW *B; - }; - - Body2DSW *_arr[2] = { nullptr, nullptr }; - }; - - Vector2 A_groove_1; - Vector2 A_groove_2; - Vector2 A_groove_normal; - Vector2 B_anchor; - Vector2 jn_acc; - Vector2 gbias; - real_t jn_max = 0.0; - real_t clamp = 0.0; - Vector2 xf_normal; - Vector2 rA, rB; - Vector2 k1, k2; - - bool correct = false; - -public: - virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_GROOVE; } - - virtual bool setup(real_t p_step) override; - virtual bool pre_solve(real_t p_step) override; - virtual void solve(real_t p_step) override; - - GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, Body2DSW *p_body_a, Body2DSW *p_body_b); -}; - -class DampedSpringJoint2DSW : public Joint2DSW { - union { - struct { - Body2DSW *A; - Body2DSW *B; - }; - - Body2DSW *_arr[2] = { nullptr, nullptr }; - }; - - Vector2 anchor_A; - Vector2 anchor_B; - - real_t rest_length = 0.0; - real_t damping = 1.5; - real_t stiffness = 20.0; - - Vector2 rA, rB; - Vector2 n; - Vector2 j; - real_t n_mass = 0.0; - real_t target_vrn = 0.0; - real_t v_coef = 0.0; - -public: - virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_DAMPED_SPRING; } - - virtual bool setup(real_t p_step) override; - virtual bool pre_solve(real_t p_step) override; - virtual void solve(real_t p_step) override; - - void set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value); - real_t get_param(PhysicsServer2D::DampedSpringParam p_param) const; - - DampedSpringJoint2DSW(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, Body2DSW *p_body_a, Body2DSW *p_body_b); -}; - -#endif // JOINTS_2D_SW_H diff --git a/servers/physics_2d/physics_server_2d_sw.cpp b/servers/physics_2d/physics_server_2d_sw.cpp deleted file mode 100644 index c2205e33b0..0000000000 --- a/servers/physics_2d/physics_server_2d_sw.cpp +++ /dev/null @@ -1,1349 +0,0 @@ -/*************************************************************************/ -/* physics_server_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "physics_server_2d_sw.h" - -#include "body_direct_state_2d_sw.h" -#include "broad_phase_2d_bvh.h" -#include "collision_solver_2d_sw.h" -#include "core/config/project_settings.h" -#include "core/debugger/engine_debugger.h" -#include "core/os/os.h" - -#define FLUSH_QUERY_CHECK(m_object) \ - ERR_FAIL_COND_MSG(m_object->get_space() && flushing_queries, "Can't change this state while flushing queries. Use call_deferred() or set_deferred() to change monitoring state instead."); - -RID PhysicsServer2DSW::_shape_create(ShapeType p_shape) { - Shape2DSW *shape = nullptr; - switch (p_shape) { - case SHAPE_WORLD_BOUNDARY: { - shape = memnew(WorldBoundaryShape2DSW); - } break; - case SHAPE_SEPARATION_RAY: { - shape = memnew(SeparationRayShape2DSW); - } break; - case SHAPE_SEGMENT: { - shape = memnew(SegmentShape2DSW); - } break; - case SHAPE_CIRCLE: { - shape = memnew(CircleShape2DSW); - } break; - case SHAPE_RECTANGLE: { - shape = memnew(RectangleShape2DSW); - } break; - case SHAPE_CAPSULE: { - shape = memnew(CapsuleShape2DSW); - } break; - case SHAPE_CONVEX_POLYGON: { - shape = memnew(ConvexPolygonShape2DSW); - } break; - case SHAPE_CONCAVE_POLYGON: { - shape = memnew(ConcavePolygonShape2DSW); - } break; - case SHAPE_CUSTOM: { - ERR_FAIL_V(RID()); - - } break; - } - - RID id = shape_owner.make_rid(shape); - shape->set_self(id); - - return id; -} - -RID PhysicsServer2DSW::world_boundary_shape_create() { - return _shape_create(SHAPE_WORLD_BOUNDARY); -} - -RID PhysicsServer2DSW::separation_ray_shape_create() { - return _shape_create(SHAPE_SEPARATION_RAY); -} - -RID PhysicsServer2DSW::segment_shape_create() { - return _shape_create(SHAPE_SEGMENT); -} - -RID PhysicsServer2DSW::circle_shape_create() { - return _shape_create(SHAPE_CIRCLE); -} - -RID PhysicsServer2DSW::rectangle_shape_create() { - return _shape_create(SHAPE_RECTANGLE); -} - -RID PhysicsServer2DSW::capsule_shape_create() { - return _shape_create(SHAPE_CAPSULE); -} - -RID PhysicsServer2DSW::convex_polygon_shape_create() { - return _shape_create(SHAPE_CONVEX_POLYGON); -} - -RID PhysicsServer2DSW::concave_polygon_shape_create() { - return _shape_create(SHAPE_CONCAVE_POLYGON); -} - -void PhysicsServer2DSW::shape_set_data(RID p_shape, const Variant &p_data) { - Shape2DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND(!shape); - shape->set_data(p_data); -}; - -void PhysicsServer2DSW::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) { - Shape2DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND(!shape); - shape->set_custom_bias(p_bias); -} - -PhysicsServer2D::ShapeType PhysicsServer2DSW::shape_get_type(RID p_shape) const { - const Shape2DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND_V(!shape, SHAPE_CUSTOM); - return shape->get_type(); -}; - -Variant PhysicsServer2DSW::shape_get_data(RID p_shape) const { - const Shape2DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND_V(!shape, Variant()); - ERR_FAIL_COND_V(!shape->is_configured(), Variant()); - return shape->get_data(); -}; - -real_t PhysicsServer2DSW::shape_get_custom_solver_bias(RID p_shape) const { - const Shape2DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND_V(!shape, 0); - return shape->get_custom_bias(); -} - -void PhysicsServer2DSW::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) { - CollCbkData *cbk = (CollCbkData *)p_userdata; - - if (cbk->max == 0) { - return; - } - - Vector2 rel_dir = (p_point_A - p_point_B); - real_t rel_length2 = rel_dir.length_squared(); - if (cbk->valid_dir != Vector2()) { - if (cbk->valid_depth < 10e20) { - if (rel_length2 > cbk->valid_depth * cbk->valid_depth || - (rel_length2 > CMP_EPSILON && cbk->valid_dir.dot(rel_dir.normalized()) < CMP_EPSILON)) { - cbk->invalid_by_dir++; - return; - } - } else { - if (rel_length2 > 0 && cbk->valid_dir.dot(rel_dir.normalized()) < CMP_EPSILON) { - return; - } - } - } - - if (cbk->amount == cbk->max) { - //find least deep - real_t min_depth = 1e20; - int min_depth_idx = 0; - for (int i = 0; i < cbk->amount; i++) { - real_t d = cbk->ptr[i * 2 + 0].distance_squared_to(cbk->ptr[i * 2 + 1]); - if (d < min_depth) { - min_depth = d; - min_depth_idx = i; - } - } - - if (rel_length2 < min_depth) { - return; - } - cbk->ptr[min_depth_idx * 2 + 0] = p_point_A; - cbk->ptr[min_depth_idx * 2 + 1] = p_point_B; - cbk->passed++; - - } else { - cbk->ptr[cbk->amount * 2 + 0] = p_point_A; - cbk->ptr[cbk->amount * 2 + 1] = p_point_B; - cbk->amount++; - cbk->passed++; - } -} - -bool PhysicsServer2DSW::shape_collide(RID p_shape_A, const Transform2D &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Transform2D &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) { - Shape2DSW *shape_A = shape_owner.get_or_null(p_shape_A); - ERR_FAIL_COND_V(!shape_A, false); - Shape2DSW *shape_B = shape_owner.get_or_null(p_shape_B); - ERR_FAIL_COND_V(!shape_B, false); - - if (p_result_max == 0) { - return CollisionSolver2DSW::solve(shape_A, p_xform_A, p_motion_A, shape_B, p_xform_B, p_motion_B, nullptr, nullptr); - } - - CollCbkData cbk; - cbk.max = p_result_max; - cbk.amount = 0; - cbk.passed = 0; - cbk.ptr = r_results; - - bool res = CollisionSolver2DSW::solve(shape_A, p_xform_A, p_motion_A, shape_B, p_xform_B, p_motion_B, _shape_col_cbk, &cbk); - r_result_count = cbk.amount; - return res; -} - -RID PhysicsServer2DSW::space_create() { - Space2DSW *space = memnew(Space2DSW); - RID id = space_owner.make_rid(space); - space->set_self(id); - RID area_id = area_create(); - Area2DSW *area = area_owner.get_or_null(area_id); - ERR_FAIL_COND_V(!area, RID()); - space->set_default_area(area); - area->set_space(space); - area->set_priority(-1); - - return id; -}; - -void PhysicsServer2DSW::space_set_active(RID p_space, bool p_active) { - Space2DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND(!space); - if (p_active) { - active_spaces.insert(space); - } else { - active_spaces.erase(space); - } -} - -bool PhysicsServer2DSW::space_is_active(RID p_space) const { - const Space2DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND_V(!space, false); - - return active_spaces.has(space); -} - -void PhysicsServer2DSW::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) { - Space2DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND(!space); - - space->set_param(p_param, p_value); -} - -real_t PhysicsServer2DSW::space_get_param(RID p_space, SpaceParameter p_param) const { - const Space2DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND_V(!space, 0); - return space->get_param(p_param); -} - -void PhysicsServer2DSW::space_set_debug_contacts(RID p_space, int p_max_contacts) { - Space2DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND(!space); - space->set_debug_contacts(p_max_contacts); -} - -Vector PhysicsServer2DSW::space_get_contacts(RID p_space) const { - Space2DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND_V(!space, Vector()); - return space->get_debug_contacts(); -} - -int PhysicsServer2DSW::space_get_contact_count(RID p_space) const { - Space2DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND_V(!space, 0); - return space->get_debug_contact_count(); -} - -PhysicsDirectSpaceState2D *PhysicsServer2DSW::space_get_direct_state(RID p_space) { - Space2DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND_V(!space, nullptr); - ERR_FAIL_COND_V_MSG((using_threads && !doing_sync) || space->is_locked(), nullptr, "Space state is inaccessible right now, wait for iteration or physics process notification."); - - return space->get_direct_state(); -} - -RID PhysicsServer2DSW::area_create() { - Area2DSW *area = memnew(Area2DSW); - RID rid = area_owner.make_rid(area); - area->set_self(rid); - return rid; -}; - -void PhysicsServer2DSW::area_set_space(RID p_area, RID p_space) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - Space2DSW *space = nullptr; - if (p_space.is_valid()) { - space = space_owner.get_or_null(p_space); - ERR_FAIL_COND(!space); - } - - if (area->get_space() == space) { - return; //pointless - } - - area->clear_constraints(); - area->set_space(space); -}; - -RID PhysicsServer2DSW::area_get_space(RID p_area) const { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, RID()); - - Space2DSW *space = area->get_space(); - if (!space) { - return RID(); - } - return space->get_self(); -}; - -void PhysicsServer2DSW::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->set_space_override_mode(p_mode); -} - -PhysicsServer2D::AreaSpaceOverrideMode PhysicsServer2DSW::area_get_space_override_mode(RID p_area) const { - const Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, AREA_SPACE_OVERRIDE_DISABLED); - - return area->get_space_override_mode(); -} - -void PhysicsServer2DSW::area_add_shape(RID p_area, RID p_shape, const Transform2D &p_transform, bool p_disabled) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - Shape2DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND(!shape); - - area->add_shape(shape, p_transform, p_disabled); -} - -void PhysicsServer2DSW::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - Shape2DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND(!shape); - ERR_FAIL_COND(!shape->is_configured()); - - area->set_shape(p_shape_idx, shape); -} - -void PhysicsServer2DSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform2D &p_transform) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->set_shape_transform(p_shape_idx, p_transform); -} - -void PhysicsServer2DSW::area_set_shape_disabled(RID p_area, int p_shape, bool p_disabled) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - ERR_FAIL_INDEX(p_shape, area->get_shape_count()); - FLUSH_QUERY_CHECK(area); - - area->set_shape_disabled(p_shape, p_disabled); -} - -int PhysicsServer2DSW::area_get_shape_count(RID p_area) const { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, -1); - - return area->get_shape_count(); -} - -RID PhysicsServer2DSW::area_get_shape(RID p_area, int p_shape_idx) const { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, RID()); - - Shape2DSW *shape = area->get_shape(p_shape_idx); - ERR_FAIL_COND_V(!shape, RID()); - - return shape->get_self(); -} - -Transform2D PhysicsServer2DSW::area_get_shape_transform(RID p_area, int p_shape_idx) const { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, Transform2D()); - - return area->get_shape_transform(p_shape_idx); -} - -void PhysicsServer2DSW::area_remove_shape(RID p_area, int p_shape_idx) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->remove_shape(p_shape_idx); -} - -void PhysicsServer2DSW::area_clear_shapes(RID p_area) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - while (area->get_shape_count()) { - area->remove_shape(0); - } -} - -void PhysicsServer2DSW::area_attach_object_instance_id(RID p_area, ObjectID p_id) { - if (space_owner.owns(p_area)) { - Space2DSW *space = space_owner.get_or_null(p_area); - p_area = space->get_default_area()->get_self(); - } - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - area->set_instance_id(p_id); -} - -ObjectID PhysicsServer2DSW::area_get_object_instance_id(RID p_area) const { - if (space_owner.owns(p_area)) { - Space2DSW *space = space_owner.get_or_null(p_area); - p_area = space->get_default_area()->get_self(); - } - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, ObjectID()); - return area->get_instance_id(); -} - -void PhysicsServer2DSW::area_attach_canvas_instance_id(RID p_area, ObjectID p_id) { - if (space_owner.owns(p_area)) { - Space2DSW *space = space_owner.get_or_null(p_area); - p_area = space->get_default_area()->get_self(); - } - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - area->set_canvas_instance_id(p_id); -} - -ObjectID PhysicsServer2DSW::area_get_canvas_instance_id(RID p_area) const { - if (space_owner.owns(p_area)) { - Space2DSW *space = space_owner.get_or_null(p_area); - p_area = space->get_default_area()->get_self(); - } - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, ObjectID()); - return area->get_canvas_instance_id(); -} - -void PhysicsServer2DSW::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) { - if (space_owner.owns(p_area)) { - Space2DSW *space = space_owner.get_or_null(p_area); - p_area = space->get_default_area()->get_self(); - } - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - area->set_param(p_param, p_value); -}; - -void PhysicsServer2DSW::area_set_transform(RID p_area, const Transform2D &p_transform) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - area->set_transform(p_transform); -}; - -Variant PhysicsServer2DSW::area_get_param(RID p_area, AreaParameter p_param) const { - if (space_owner.owns(p_area)) { - Space2DSW *space = space_owner.get_or_null(p_area); - p_area = space->get_default_area()->get_self(); - } - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, Variant()); - - return area->get_param(p_param); -}; - -Transform2D PhysicsServer2DSW::area_get_transform(RID p_area) const { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, Transform2D()); - - return area->get_transform(); -}; - -void PhysicsServer2DSW::area_set_pickable(RID p_area, bool p_pickable) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - area->set_pickable(p_pickable); -} - -void PhysicsServer2DSW::area_set_monitorable(RID p_area, bool p_monitorable) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - FLUSH_QUERY_CHECK(area); - - area->set_monitorable(p_monitorable); -} - -void PhysicsServer2DSW::area_set_collision_mask(RID p_area, uint32_t p_mask) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->set_collision_mask(p_mask); -} - -void PhysicsServer2DSW::area_set_collision_layer(RID p_area, uint32_t p_layer) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->set_collision_layer(p_layer); -} - -void PhysicsServer2DSW::area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->set_monitor_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method); -} - -void PhysicsServer2DSW::area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { - Area2DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->set_area_monitor_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method); -} - -/* BODY API */ - -RID PhysicsServer2DSW::body_create() { - Body2DSW *body = memnew(Body2DSW); - RID rid = body_owner.make_rid(body); - body->set_self(rid); - return rid; -} - -void PhysicsServer2DSW::body_set_space(RID p_body, RID p_space) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - Space2DSW *space = nullptr; - if (p_space.is_valid()) { - space = space_owner.get_or_null(p_space); - ERR_FAIL_COND(!space); - } - - if (body->get_space() == space) { - return; //pointless - } - - body->clear_constraint_list(); - body->set_space(space); -}; - -RID PhysicsServer2DSW::body_get_space(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, RID()); - - Space2DSW *space = body->get_space(); - if (!space) { - return RID(); - } - return space->get_self(); -}; - -void PhysicsServer2DSW::body_set_mode(RID p_body, BodyMode p_mode) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - FLUSH_QUERY_CHECK(body); - - body->set_mode(p_mode); -}; - -PhysicsServer2D::BodyMode PhysicsServer2DSW::body_get_mode(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, BODY_MODE_STATIC); - - return body->get_mode(); -}; - -void PhysicsServer2DSW::body_add_shape(RID p_body, RID p_shape, const Transform2D &p_transform, bool p_disabled) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - Shape2DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND(!shape); - - body->add_shape(shape, p_transform, p_disabled); -} - -void PhysicsServer2DSW::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - Shape2DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND(!shape); - ERR_FAIL_COND(!shape->is_configured()); - - body->set_shape(p_shape_idx, shape); -} - -void PhysicsServer2DSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform2D &p_transform) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_shape_transform(p_shape_idx, p_transform); -} - -int PhysicsServer2DSW::body_get_shape_count(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, -1); - - return body->get_shape_count(); -} - -RID PhysicsServer2DSW::body_get_shape(RID p_body, int p_shape_idx) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, RID()); - - Shape2DSW *shape = body->get_shape(p_shape_idx); - ERR_FAIL_COND_V(!shape, RID()); - - return shape->get_self(); -} - -Transform2D PhysicsServer2DSW::body_get_shape_transform(RID p_body, int p_shape_idx) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, Transform2D()); - - return body->get_shape_transform(p_shape_idx); -} - -void PhysicsServer2DSW::body_remove_shape(RID p_body, int p_shape_idx) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->remove_shape(p_shape_idx); -} - -void PhysicsServer2DSW::body_clear_shapes(RID p_body) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - while (body->get_shape_count()) { - body->remove_shape(0); - } -} - -void PhysicsServer2DSW::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count()); - FLUSH_QUERY_CHECK(body); - - body->set_shape_disabled(p_shape_idx, p_disabled); -} - -void PhysicsServer2DSW::body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, real_t p_margin) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count()); - FLUSH_QUERY_CHECK(body); - - body->set_shape_as_one_way_collision(p_shape_idx, p_enable, p_margin); -} - -void PhysicsServer2DSW::body_set_continuous_collision_detection_mode(RID p_body, CCDMode p_mode) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - body->set_continuous_collision_detection_mode(p_mode); -} - -PhysicsServer2DSW::CCDMode PhysicsServer2DSW::body_get_continuous_collision_detection_mode(RID p_body) const { - const Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, CCD_MODE_DISABLED); - - return body->get_continuous_collision_detection_mode(); -} - -void PhysicsServer2DSW::body_attach_object_instance_id(RID p_body, ObjectID p_id) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_instance_id(p_id); -}; - -ObjectID PhysicsServer2DSW::body_get_object_instance_id(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, ObjectID()); - - return body->get_instance_id(); -}; - -void PhysicsServer2DSW::body_attach_canvas_instance_id(RID p_body, ObjectID p_id) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_canvas_instance_id(p_id); -}; - -ObjectID PhysicsServer2DSW::body_get_canvas_instance_id(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, ObjectID()); - - return body->get_canvas_instance_id(); -}; - -void PhysicsServer2DSW::body_set_collision_layer(RID p_body, uint32_t p_layer) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - body->set_collision_layer(p_layer); -}; - -uint32_t PhysicsServer2DSW::body_get_collision_layer(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, 0); - - return body->get_collision_layer(); -}; - -void PhysicsServer2DSW::body_set_collision_mask(RID p_body, uint32_t p_mask) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - body->set_collision_mask(p_mask); -}; - -uint32_t PhysicsServer2DSW::body_get_collision_mask(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, 0); - - return body->get_collision_mask(); -}; - -void PhysicsServer2DSW::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_param(p_param, p_value); -}; - -Variant PhysicsServer2DSW::body_get_param(RID p_body, BodyParameter p_param) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, 0); - - return body->get_param(p_param); -}; - -void PhysicsServer2DSW::body_reset_mass_properties(RID p_body) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - return body->reset_mass_properties(); -} - -void PhysicsServer2DSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_state(p_state, p_variant); -}; - -Variant PhysicsServer2DSW::body_get_state(RID p_body, BodyState p_state) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, Variant()); - - return body->get_state(p_state); -}; - -void PhysicsServer2DSW::body_set_applied_force(RID p_body, const Vector2 &p_force) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_applied_force(p_force); - body->wakeup(); -}; - -Vector2 PhysicsServer2DSW::body_get_applied_force(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, Vector2()); - return body->get_applied_force(); -}; - -void PhysicsServer2DSW::body_set_applied_torque(RID p_body, real_t p_torque) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_applied_torque(p_torque); - body->wakeup(); -}; - -real_t PhysicsServer2DSW::body_get_applied_torque(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, 0); - - return body->get_applied_torque(); -}; - -void PhysicsServer2DSW::body_apply_central_impulse(RID p_body, const Vector2 &p_impulse) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->apply_central_impulse(p_impulse); - body->wakeup(); -} - -void PhysicsServer2DSW::body_apply_torque_impulse(RID p_body, real_t p_torque) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - _update_shapes(); - - body->apply_torque_impulse(p_torque); - body->wakeup(); -} - -void PhysicsServer2DSW::body_apply_impulse(RID p_body, const Vector2 &p_impulse, const Vector2 &p_position) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - _update_shapes(); - - body->apply_impulse(p_impulse, p_position); - body->wakeup(); -}; - -void PhysicsServer2DSW::body_add_central_force(RID p_body, const Vector2 &p_force) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->add_central_force(p_force); - body->wakeup(); -}; - -void PhysicsServer2DSW::body_add_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->add_force(p_force, p_position); - body->wakeup(); -}; - -void PhysicsServer2DSW::body_add_torque(RID p_body, real_t p_torque) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->add_torque(p_torque); - body->wakeup(); -}; - -void PhysicsServer2DSW::body_set_axis_velocity(RID p_body, const Vector2 &p_axis_velocity) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - _update_shapes(); - - Vector2 v = body->get_linear_velocity(); - Vector2 axis = p_axis_velocity.normalized(); - v -= axis * axis.dot(v); - v += p_axis_velocity; - body->set_linear_velocity(v); - body->wakeup(); -}; - -void PhysicsServer2DSW::body_add_collision_exception(RID p_body, RID p_body_b) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->add_exception(p_body_b); - body->wakeup(); -}; - -void PhysicsServer2DSW::body_remove_collision_exception(RID p_body, RID p_body_b) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->remove_exception(p_body_b); - body->wakeup(); -}; - -void PhysicsServer2DSW::body_get_collision_exceptions(RID p_body, List *p_exceptions) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - for (int i = 0; i < body->get_exceptions().size(); i++) { - p_exceptions->push_back(body->get_exceptions()[i]); - } -}; - -void PhysicsServer2DSW::body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); -}; - -real_t PhysicsServer2DSW::body_get_contacts_reported_depth_threshold(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, 0); - return 0; -}; - -void PhysicsServer2DSW::body_set_omit_force_integration(RID p_body, bool p_omit) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_omit_force_integration(p_omit); -}; - -bool PhysicsServer2DSW::body_is_omitting_force_integration(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, false); - return body->get_omit_force_integration(); -}; - -void PhysicsServer2DSW::body_set_max_contacts_reported(RID p_body, int p_contacts) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - body->set_max_contacts_reported(p_contacts); -} - -int PhysicsServer2DSW::body_get_max_contacts_reported(RID p_body) const { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, -1); - return body->get_max_contacts_reported(); -} - -void PhysicsServer2DSW::body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - body->set_state_sync_callback(p_instance, p_callback); -} - -void PhysicsServer2DSW::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - body->set_force_integration_callback(p_callable, p_udata); -} - -bool PhysicsServer2DSW::body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, false); - ERR_FAIL_INDEX_V(p_body_shape, body->get_shape_count(), false); - - return shape_collide(body->get_shape(p_body_shape)->get_self(), body->get_transform() * body->get_shape_transform(p_body_shape), Vector2(), p_shape, p_shape_xform, p_motion, r_results, p_result_max, r_result_count); -} - -void PhysicsServer2DSW::body_set_pickable(RID p_body, bool p_pickable) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - body->set_pickable(p_pickable); -} - -bool PhysicsServer2DSW::body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result) { - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, false); - ERR_FAIL_COND_V(!body->get_space(), false); - ERR_FAIL_COND_V(body->get_space()->is_locked(), false); - - _update_shapes(); - - return body->get_space()->test_body_motion(body, p_parameters, r_result); -} - -PhysicsDirectBodyState2D *PhysicsServer2DSW::body_get_direct_state(RID p_body) { - ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); - - Body2DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, nullptr); - - ERR_FAIL_COND_V(!body->get_space(), nullptr); - ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); - - return body->get_direct_state(); -} - -/* JOINT API */ - -RID PhysicsServer2DSW::joint_create() { - Joint2DSW *joint = memnew(Joint2DSW); - RID joint_rid = joint_owner.make_rid(joint); - joint->set_self(joint_rid); - return joint_rid; -} - -void PhysicsServer2DSW::joint_clear(RID p_joint) { - Joint2DSW *joint = joint_owner.get_or_null(p_joint); - if (joint->get_type() != JOINT_TYPE_MAX) { - Joint2DSW *empty_joint = memnew(Joint2DSW); - empty_joint->copy_settings_from(joint); - - joint_owner.replace(p_joint, empty_joint); - memdelete(joint); - } -} - -void PhysicsServer2DSW::joint_set_param(RID p_joint, JointParam p_param, real_t p_value) { - Joint2DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(!joint); - - switch (p_param) { - case JOINT_PARAM_BIAS: - joint->set_bias(p_value); - break; - case JOINT_PARAM_MAX_BIAS: - joint->set_max_bias(p_value); - break; - case JOINT_PARAM_MAX_FORCE: - joint->set_max_force(p_value); - break; - } -} - -real_t PhysicsServer2DSW::joint_get_param(RID p_joint, JointParam p_param) const { - const Joint2DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!joint, -1); - - switch (p_param) { - case JOINT_PARAM_BIAS: - return joint->get_bias(); - break; - case JOINT_PARAM_MAX_BIAS: - return joint->get_max_bias(); - break; - case JOINT_PARAM_MAX_FORCE: - return joint->get_max_force(); - break; - } - - return 0; -} - -void PhysicsServer2DSW::joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) { - Joint2DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(!joint); - - joint->disable_collisions_between_bodies(p_disable); - - if (2 == joint->get_body_count()) { - Body2DSW *body_a = *joint->get_body_ptr(); - Body2DSW *body_b = *(joint->get_body_ptr() + 1); - - if (p_disable) { - body_add_collision_exception(body_a->get_self(), body_b->get_self()); - body_add_collision_exception(body_b->get_self(), body_a->get_self()); - } else { - body_remove_collision_exception(body_a->get_self(), body_b->get_self()); - body_remove_collision_exception(body_b->get_self(), body_a->get_self()); - } - } -} - -bool PhysicsServer2DSW::joint_is_disabled_collisions_between_bodies(RID p_joint) const { - const Joint2DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!joint, true); - - return joint->is_disabled_collisions_between_bodies(); -} - -void PhysicsServer2DSW::joint_make_pin(RID p_joint, const Vector2 &p_pos, RID p_body_a, RID p_body_b) { - Body2DSW *A = body_owner.get_or_null(p_body_a); - ERR_FAIL_COND(!A); - Body2DSW *B = nullptr; - if (body_owner.owns(p_body_b)) { - B = body_owner.get_or_null(p_body_b); - ERR_FAIL_COND(!B); - } - - Joint2DSW *prev_joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(prev_joint == nullptr); - - Joint2DSW *joint = memnew(PinJoint2DSW(p_pos, A, B)); - - joint_owner.replace(p_joint, joint); - joint->copy_settings_from(prev_joint); - memdelete(prev_joint); -} - -void PhysicsServer2DSW::joint_make_groove(RID p_joint, const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) { - Body2DSW *A = body_owner.get_or_null(p_body_a); - ERR_FAIL_COND(!A); - - Body2DSW *B = body_owner.get_or_null(p_body_b); - ERR_FAIL_COND(!B); - - Joint2DSW *prev_joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(prev_joint == nullptr); - - Joint2DSW *joint = memnew(GrooveJoint2DSW(p_a_groove1, p_a_groove2, p_b_anchor, A, B)); - - joint_owner.replace(p_joint, joint); - joint->copy_settings_from(prev_joint); - memdelete(prev_joint); -} - -void PhysicsServer2DSW::joint_make_damped_spring(RID p_joint, const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b) { - Body2DSW *A = body_owner.get_or_null(p_body_a); - ERR_FAIL_COND(!A); - - Body2DSW *B = body_owner.get_or_null(p_body_b); - ERR_FAIL_COND(!B); - - Joint2DSW *prev_joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(prev_joint == nullptr); - - Joint2DSW *joint = memnew(DampedSpringJoint2DSW(p_anchor_a, p_anchor_b, A, B)); - - joint_owner.replace(p_joint, joint); - joint->copy_settings_from(prev_joint); - memdelete(prev_joint); -} - -void PhysicsServer2DSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) { - Joint2DSW *j = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(!j); - ERR_FAIL_COND(j->get_type() != JOINT_TYPE_PIN); - - PinJoint2DSW *pin_joint = static_cast(j); - pin_joint->set_param(p_param, p_value); -} - -real_t PhysicsServer2DSW::pin_joint_get_param(RID p_joint, PinJointParam p_param) const { - Joint2DSW *j = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!j, 0); - ERR_FAIL_COND_V(j->get_type() != JOINT_TYPE_PIN, 0); - - PinJoint2DSW *pin_joint = static_cast(j); - return pin_joint->get_param(p_param); -} - -void PhysicsServer2DSW::damped_spring_joint_set_param(RID p_joint, DampedSpringParam p_param, real_t p_value) { - Joint2DSW *j = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(!j); - ERR_FAIL_COND(j->get_type() != JOINT_TYPE_DAMPED_SPRING); - - DampedSpringJoint2DSW *dsj = static_cast(j); - dsj->set_param(p_param, p_value); -} - -real_t PhysicsServer2DSW::damped_spring_joint_get_param(RID p_joint, DampedSpringParam p_param) const { - Joint2DSW *j = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!j, 0); - ERR_FAIL_COND_V(j->get_type() != JOINT_TYPE_DAMPED_SPRING, 0); - - DampedSpringJoint2DSW *dsj = static_cast(j); - return dsj->get_param(p_param); -} - -PhysicsServer2D::JointType PhysicsServer2DSW::joint_get_type(RID p_joint) const { - Joint2DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!joint, JOINT_TYPE_PIN); - - return joint->get_type(); -} - -void PhysicsServer2DSW::free(RID p_rid) { - _update_shapes(); // just in case - - if (shape_owner.owns(p_rid)) { - Shape2DSW *shape = shape_owner.get_or_null(p_rid); - - while (shape->get_owners().size()) { - ShapeOwner2DSW *so = shape->get_owners().front()->key(); - so->remove_shape(shape); - } - - shape_owner.free(p_rid); - memdelete(shape); - } else if (body_owner.owns(p_rid)) { - Body2DSW *body = body_owner.get_or_null(p_rid); - - /* - if (body->get_state_query()) - _clear_query(body->get_state_query()); - - if (body->get_direct_state_query()) - _clear_query(body->get_direct_state_query()); - */ - - body_set_space(p_rid, RID()); - - while (body->get_shape_count()) { - body->remove_shape(0); - } - - body_owner.free(p_rid); - memdelete(body); - - } else if (area_owner.owns(p_rid)) { - Area2DSW *area = area_owner.get_or_null(p_rid); - - /* - if (area->get_monitor_query()) - _clear_query(area->get_monitor_query()); - */ - - area->set_space(nullptr); - - while (area->get_shape_count()) { - area->remove_shape(0); - } - - area_owner.free(p_rid); - memdelete(area); - } else if (space_owner.owns(p_rid)) { - Space2DSW *space = space_owner.get_or_null(p_rid); - - while (space->get_objects().size()) { - CollisionObject2DSW *co = (CollisionObject2DSW *)space->get_objects().front()->get(); - co->set_space(nullptr); - } - - active_spaces.erase(space); - free(space->get_default_area()->get_self()); - space_owner.free(p_rid); - memdelete(space); - } else if (joint_owner.owns(p_rid)) { - Joint2DSW *joint = joint_owner.get_or_null(p_rid); - - joint_owner.free(p_rid); - memdelete(joint); - - } else { - ERR_FAIL_MSG("Invalid ID."); - } -}; - -void PhysicsServer2DSW::set_active(bool p_active) { - active = p_active; -}; - -void PhysicsServer2DSW::set_collision_iterations(int p_iterations) { - iterations = p_iterations; -}; - -void PhysicsServer2DSW::init() { - doing_sync = false; - iterations = 8; // 8? - stepper = memnew(Step2DSW); -}; - -void PhysicsServer2DSW::step(real_t p_step) { - if (!active) { - return; - } - - _update_shapes(); - - island_count = 0; - active_objects = 0; - collision_pairs = 0; - for (Set::Element *E = active_spaces.front(); E; E = E->next()) { - stepper->step((Space2DSW *)E->get(), p_step, iterations); - island_count += E->get()->get_island_count(); - active_objects += E->get()->get_active_objects(); - collision_pairs += E->get()->get_collision_pairs(); - } -}; - -void PhysicsServer2DSW::sync() { - doing_sync = true; -}; - -void PhysicsServer2DSW::flush_queries() { - if (!active) { - return; - } - - flushing_queries = true; - - uint64_t time_beg = OS::get_singleton()->get_ticks_usec(); - - for (Set::Element *E = active_spaces.front(); E; E = E->next()) { - Space2DSW *space = (Space2DSW *)E->get(); - space->call_queries(); - } - - flushing_queries = false; - - if (EngineDebugger::is_profiling("servers")) { - uint64_t total_time[Space2DSW::ELAPSED_TIME_MAX]; - static const char *time_name[Space2DSW::ELAPSED_TIME_MAX] = { - "integrate_forces", - "generate_islands", - "setup_constraints", - "solve_constraints", - "integrate_velocities" - }; - - for (int i = 0; i < Space2DSW::ELAPSED_TIME_MAX; i++) { - total_time[i] = 0; - } - - for (Set::Element *E = active_spaces.front(); E; E = E->next()) { - for (int i = 0; i < Space2DSW::ELAPSED_TIME_MAX; i++) { - total_time[i] += E->get()->get_elapsed_time(Space2DSW::ElapsedTime(i)); - } - } - - Array values; - values.resize(Space2DSW::ELAPSED_TIME_MAX * 2); - for (int i = 0; i < Space2DSW::ELAPSED_TIME_MAX; i++) { - values[i * 2 + 0] = time_name[i]; - values[i * 2 + 1] = USEC_TO_SEC(total_time[i]); - } - values.push_back("flush_queries"); - values.push_back(USEC_TO_SEC(OS::get_singleton()->get_ticks_usec() - time_beg)); - - values.push_front("physics_2d"); - EngineDebugger::profiler_add_frame_data("servers", values); - } -} - -void PhysicsServer2DSW::end_sync() { - doing_sync = false; -} - -void PhysicsServer2DSW::finish() { - memdelete(stepper); -}; - -void PhysicsServer2DSW::_update_shapes() { - while (pending_shape_update_list.first()) { - pending_shape_update_list.first()->self()->_shape_changed(); - pending_shape_update_list.remove(pending_shape_update_list.first()); - } -} - -int PhysicsServer2DSW::get_process_info(ProcessInfo p_info) { - switch (p_info) { - case INFO_ACTIVE_OBJECTS: { - return active_objects; - } break; - case INFO_COLLISION_PAIRS: { - return collision_pairs; - } break; - case INFO_ISLAND_COUNT: { - return island_count; - } break; - } - - return 0; -} - -PhysicsServer2DSW *PhysicsServer2DSW::singletonsw = nullptr; - -PhysicsServer2DSW::PhysicsServer2DSW(bool p_using_threads) { - singletonsw = this; - BroadPhase2DSW::create_func = BroadPhase2DBVH::_create; - - using_threads = p_using_threads; -}; diff --git a/servers/physics_2d/physics_server_2d_sw.h b/servers/physics_2d/physics_server_2d_sw.h deleted file mode 100644 index b8e375a087..0000000000 --- a/servers/physics_2d/physics_server_2d_sw.h +++ /dev/null @@ -1,299 +0,0 @@ -/*************************************************************************/ -/* physics_server_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 PHYSICS_2D_SERVER_SW -#define PHYSICS_2D_SERVER_SW - -#include "core/templates/rid_owner.h" -#include "joints_2d_sw.h" -#include "servers/physics_server_2d.h" -#include "shape_2d_sw.h" -#include "space_2d_sw.h" -#include "step_2d_sw.h" - -class PhysicsServer2DSW : public PhysicsServer2D { - GDCLASS(PhysicsServer2DSW, PhysicsServer2D); - - friend class PhysicsDirectSpaceState2DSW; - friend class PhysicsDirectBodyState2DSW; - bool active = true; - int iterations = 0; - bool doing_sync = false; - - int island_count = 0; - int active_objects = 0; - int collision_pairs = 0; - - bool using_threads = false; - - bool flushing_queries = false; - - Step2DSW *stepper = nullptr; - Set active_spaces; - - mutable RID_PtrOwner shape_owner; - mutable RID_PtrOwner space_owner; - mutable RID_PtrOwner area_owner; - mutable RID_PtrOwner body_owner; - mutable RID_PtrOwner joint_owner; - - static PhysicsServer2DSW *singletonsw; - - //void _clear_query(Query2DSW *p_query); - friend class CollisionObject2DSW; - SelfList::List pending_shape_update_list; - void _update_shapes(); - - RID _shape_create(ShapeType p_shape); - -public: - struct CollCbkData { - Vector2 valid_dir; - real_t valid_depth = 0.0; - int max = 0; - int amount = 0; - int passed = 0; - int invalid_by_dir = 0; - Vector2 *ptr = nullptr; - }; - - virtual RID world_boundary_shape_create() override; - virtual RID separation_ray_shape_create() override; - virtual RID segment_shape_create() override; - virtual RID circle_shape_create() override; - virtual RID rectangle_shape_create() override; - virtual RID capsule_shape_create() override; - virtual RID convex_polygon_shape_create() override; - virtual RID concave_polygon_shape_create() override; - - static void _shape_col_cbk(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata); - - virtual void shape_set_data(RID p_shape, const Variant &p_data) override; - virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias) override; - - virtual ShapeType shape_get_type(RID p_shape) const override; - virtual Variant shape_get_data(RID p_shape) const override; - virtual real_t shape_get_custom_solver_bias(RID p_shape) const override; - - virtual bool shape_collide(RID p_shape_A, const Transform2D &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Transform2D &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) override; - - /* SPACE API */ - - virtual RID space_create() override; - virtual void space_set_active(RID p_space, bool p_active) override; - virtual bool space_is_active(RID p_space) const override; - - virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) override; - virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const override; - - virtual void space_set_debug_contacts(RID p_space, int p_max_contacts) override; - virtual Vector space_get_contacts(RID p_space) const override; - virtual int space_get_contact_count(RID p_space) const override; - - // this function only works on physics process, errors and returns null otherwise - virtual PhysicsDirectSpaceState2D *space_get_direct_state(RID p_space) override; - - /* AREA API */ - - virtual RID area_create() override; - - virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) override; - virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const 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 Transform2D &p_transform = Transform2D(), bool p_disabled = false) override; - virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape) override; - virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform2D &p_transform) override; - - virtual int area_get_shape_count(RID p_area) const override; - virtual RID area_get_shape(RID p_area, int p_shape_idx) const override; - virtual Transform2D 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, bool p_disabled) 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_attach_canvas_instance_id(RID p_area, ObjectID p_id) override; - virtual ObjectID area_get_canvas_instance_id(RID p_area) const override; - - virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) override; - virtual void area_set_transform(RID p_area, const Transform2D &p_transform) override; - - virtual Variant area_get_param(RID p_area, AreaParameter p_param) const override; - virtual Transform2D area_get_transform(RID p_area) const override; - virtual void area_set_monitorable(RID p_area, bool p_monitorable) override; - virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) override; - virtual void area_set_collision_layer(RID p_area, uint32_t p_layer) override; - - virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) override; - virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) override; - - virtual void area_set_pickable(RID p_area, bool p_pickable) override; - - /* BODY API */ - - // create a body of a given type - virtual RID body_create() override; - - virtual void body_set_space(RID p_body, RID p_space) override; - virtual RID body_get_space(RID p_body) const override; - - virtual void body_set_mode(RID p_body, BodyMode p_mode) override; - virtual BodyMode body_get_mode(RID p_body) const override; - - virtual void body_add_shape(RID p_body, RID p_shape, const Transform2D &p_transform = Transform2D(), bool p_disabled = false) override; - virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape) override; - virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform2D &p_transform) override; - - virtual int body_get_shape_count(RID p_body) const override; - virtual RID body_get_shape(RID p_body, int p_shape_idx) const override; - virtual Transform2D body_get_shape_transform(RID p_body, int p_shape_idx) 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_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) override; - virtual void body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, real_t p_margin) 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_attach_canvas_instance_id(RID p_body, ObjectID p_id) override; - virtual ObjectID body_get_canvas_instance_id(RID p_body) const override; - - virtual void body_set_continuous_collision_detection_mode(RID p_body, CCDMode p_mode) override; - virtual CCDMode body_get_continuous_collision_detection_mode(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_param(RID p_body, BodyParameter p_param, const Variant &p_value) override; - virtual Variant body_get_param(RID p_body, BodyParameter p_param) const override; - - virtual void body_reset_mass_properties(RID p_body) override; - - virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override; - virtual Variant body_get_state(RID p_body, BodyState p_state) const override; - - virtual void body_set_applied_force(RID p_body, const Vector2 &p_force) override; - virtual Vector2 body_get_applied_force(RID p_body) const override; - - virtual void body_set_applied_torque(RID p_body, real_t p_torque) override; - virtual real_t body_get_applied_torque(RID p_body) const override; - - virtual void body_add_central_force(RID p_body, const Vector2 &p_force) override; - virtual void body_add_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position = Vector2()) override; - virtual void body_add_torque(RID p_body, real_t p_torque) override; - - virtual void body_apply_central_impulse(RID p_body, const Vector2 &p_impulse) override; - virtual void body_apply_torque_impulse(RID p_body, real_t p_torque) override; - virtual void body_apply_impulse(RID p_body, const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override; - virtual void body_set_axis_velocity(RID p_body, const Vector2 &p_axis_velocity) override; - - virtual void body_add_collision_exception(RID p_body, RID p_body_b) override; - virtual void body_remove_collision_exception(RID p_body, RID p_body_b) override; - virtual void body_get_collision_exceptions(RID p_body, List *p_exceptions) override; - - virtual void body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) override; - virtual real_t body_get_contacts_reported_depth_threshold(RID p_body) const override; - - virtual void body_set_omit_force_integration(RID p_body, bool p_omit) override; - virtual bool body_is_omitting_force_integration(RID p_body) const override; - - virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) override; - virtual int body_get_max_contacts_reported(RID p_body) const override; - - virtual void body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) override; - virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override; - - virtual bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override; - - virtual void body_set_pickable(RID p_body, bool p_pickable) override; - - virtual bool body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result = nullptr) override; - - // this function only works on physics process, errors and returns null otherwise - virtual PhysicsDirectBodyState2D *body_get_direct_state(RID p_body) override; - - /* JOINT API */ - - virtual RID joint_create() override; - - virtual void joint_clear(RID p_joint) override; - - virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value) override; - virtual real_t joint_get_param(RID p_joint, JointParam p_param) const override; - - virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disabled) override; - virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const override; - - virtual void joint_make_pin(RID p_joint, const Vector2 &p_anchor, RID p_body_a, RID p_body_b = RID()) override; - virtual void joint_make_groove(RID p_joint, const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) override; - virtual void joint_make_damped_spring(RID p_joint, const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b = RID()) override; - - virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) override; - virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const override; - virtual void damped_spring_joint_set_param(RID p_joint, DampedSpringParam p_param, real_t p_value) override; - virtual real_t damped_spring_joint_get_param(RID p_joint, DampedSpringParam p_param) const override; - - virtual JointType joint_get_type(RID p_joint) const override; - - /* MISC */ - - virtual void free(RID p_rid) override; - - virtual void set_active(bool p_active) override; - virtual void init() override; - virtual void step(real_t p_step) override; - virtual void sync() override; - virtual void flush_queries() override; - virtual void end_sync() override; - virtual void finish() override; - - virtual void set_collision_iterations(int p_iterations) override; - - virtual bool is_flushing_queries() const override { return flushing_queries; } - - int get_process_info(ProcessInfo p_info) override; - - PhysicsServer2DSW(bool p_using_threads = false); - ~PhysicsServer2DSW() {} -}; - -#endif diff --git a/servers/physics_2d/physics_server_2d_wrap_mt.cpp b/servers/physics_2d/physics_server_2d_wrap_mt.cpp deleted file mode 100644 index 33070bf42d..0000000000 --- a/servers/physics_2d/physics_server_2d_wrap_mt.cpp +++ /dev/null @@ -1,137 +0,0 @@ -/*************************************************************************/ -/* physics_server_2d_wrap_mt.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "physics_server_2d_wrap_mt.h" - -#include "core/os/os.h" - -void PhysicsServer2DWrapMT::thread_exit() { - exit.set(); -} - -void PhysicsServer2DWrapMT::thread_step(real_t p_delta) { - physics_2d_server->step(p_delta); - step_sem.post(); -} - -void PhysicsServer2DWrapMT::_thread_callback(void *_instance) { - PhysicsServer2DWrapMT *vsmt = reinterpret_cast(_instance); - - vsmt->thread_loop(); -} - -void PhysicsServer2DWrapMT::thread_loop() { - server_thread = Thread::get_caller_id(); - - physics_2d_server->init(); - - exit.clear(); - step_thread_up.set(); - while (!exit.is_set()) { - // flush commands one by one, until exit is requested - command_queue.wait_and_flush(); - } - - command_queue.flush_all(); // flush all - - physics_2d_server->finish(); -} - -/* EVENT QUEUING */ - -void PhysicsServer2DWrapMT::step(real_t p_step) { - if (create_thread) { - command_queue.push(this, &PhysicsServer2DWrapMT::thread_step, p_step); - } else { - command_queue.flush_all(); //flush all pending from other threads - physics_2d_server->step(p_step); - } -} - -void PhysicsServer2DWrapMT::sync() { - if (create_thread) { - if (first_frame) { - first_frame = false; - } else { - step_sem.wait(); //must not wait if a step was not issued - } - } - physics_2d_server->sync(); -} - -void PhysicsServer2DWrapMT::flush_queries() { - physics_2d_server->flush_queries(); -} - -void PhysicsServer2DWrapMT::end_sync() { - physics_2d_server->end_sync(); -} - -void PhysicsServer2DWrapMT::init() { - if (create_thread) { - //OS::get_singleton()->release_rendering_thread(); - thread.start(_thread_callback, this); - while (!step_thread_up.is_set()) { - OS::get_singleton()->delay_usec(1000); - } - } else { - physics_2d_server->init(); - } -} - -void PhysicsServer2DWrapMT::finish() { - if (thread.is_started()) { - command_queue.push(this, &PhysicsServer2DWrapMT::thread_exit); - thread.wait_to_finish(); - } else { - physics_2d_server->finish(); - } -} - -PhysicsServer2DWrapMT::PhysicsServer2DWrapMT(PhysicsServer2D *p_contained, bool p_create_thread) : - command_queue(p_create_thread) { - physics_2d_server = p_contained; - create_thread = p_create_thread; - - pool_max_size = GLOBAL_GET("memory/limits/multithreaded_server/rid_pool_prealloc"); - - if (!p_create_thread) { - server_thread = Thread::get_caller_id(); - } else { - server_thread = 0; - } - - main_thread = Thread::get_caller_id(); -} - -PhysicsServer2DWrapMT::~PhysicsServer2DWrapMT() { - memdelete(physics_2d_server); - //finish(); -} diff --git a/servers/physics_2d/physics_server_2d_wrap_mt.h b/servers/physics_2d/physics_server_2d_wrap_mt.h deleted file mode 100644 index 8d9e366661..0000000000 --- a/servers/physics_2d/physics_server_2d_wrap_mt.h +++ /dev/null @@ -1,333 +0,0 @@ -/*************************************************************************/ -/* physics_server_2d_wrap_mt.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 PHYSICS2DSERVERWRAPMT_H -#define PHYSICS2DSERVERWRAPMT_H - -#include "core/config/project_settings.h" -#include "core/os/thread.h" -#include "core/templates/command_queue_mt.h" -#include "core/templates/safe_refcount.h" -#include "servers/physics_server_2d.h" - -#ifdef DEBUG_SYNC -#define SYNC_DEBUG print_line("sync on: " + String(__FUNCTION__)); -#else -#define SYNC_DEBUG -#endif - -class PhysicsServer2DWrapMT : public PhysicsServer2D { - mutable PhysicsServer2D *physics_2d_server; - - mutable CommandQueueMT command_queue; - - static void _thread_callback(void *_instance); - void thread_loop(); - - Thread::ID server_thread; - Thread::ID main_thread; - SafeFlag exit; - Thread thread; - SafeFlag step_thread_up; - bool create_thread = false; - - Semaphore step_sem; - int step_pending = 0; - void thread_step(real_t p_delta); - void thread_flush(); - - void thread_exit(); - - bool first_frame = true; - - Mutex alloc_mutex; - int pool_max_size = 0; - -public: -#define ServerName PhysicsServer2D -#define ServerNameWrapMT PhysicsServer2DWrapMT -#define server_name physics_2d_server -#define WRITE_ACTION - -#include "servers/server_wrap_mt_common.h" - - //FUNC1RID(shape,ShapeType); todo fix - FUNCRID(world_boundary_shape) - FUNCRID(separation_ray_shape) - FUNCRID(segment_shape) - FUNCRID(circle_shape) - FUNCRID(rectangle_shape) - FUNCRID(capsule_shape) - FUNCRID(convex_polygon_shape) - FUNCRID(concave_polygon_shape) - - FUNC2(shape_set_data, RID, const Variant &); - FUNC2(shape_set_custom_solver_bias, RID, real_t); - - FUNC1RC(ShapeType, shape_get_type, RID); - FUNC1RC(Variant, shape_get_data, RID); - FUNC1RC(real_t, shape_get_custom_solver_bias, RID); - - //these work well, but should be used from the main thread only - bool shape_collide(RID p_shape_A, const Transform2D &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Transform2D &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) override { - ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); - return physics_2d_server->shape_collide(p_shape_A, p_xform_A, p_motion_A, p_shape_B, p_xform_B, p_motion_B, r_results, p_result_max, r_result_count); - } - - /* SPACE API */ - - FUNCRID(space); - FUNC2(space_set_active, RID, bool); - FUNC1RC(bool, space_is_active, RID); - - FUNC3(space_set_param, RID, SpaceParameter, real_t); - FUNC2RC(real_t, space_get_param, RID, SpaceParameter); - - // this function only works on physics process, errors and returns null otherwise - PhysicsDirectSpaceState2D *space_get_direct_state(RID p_space) override { - ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr); - return physics_2d_server->space_get_direct_state(p_space); - } - - FUNC2(space_set_debug_contacts, RID, int); - virtual Vector space_get_contacts(RID p_space) const override { - ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), Vector()); - return physics_2d_server->space_get_contacts(p_space); - } - - virtual int space_get_contact_count(RID p_space) const override { - ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), 0); - return physics_2d_server->space_get_contact_count(p_space); - } - - /* AREA API */ - - //FUNC0RID(area); - FUNCRID(area); - - FUNC2(area_set_space, RID, RID); - FUNC1RC(RID, area_get_space, RID); - - FUNC2(area_set_space_override_mode, RID, AreaSpaceOverrideMode); - FUNC1RC(AreaSpaceOverrideMode, area_get_space_override_mode, RID); - - FUNC4(area_add_shape, RID, RID, const Transform2D &, bool); - FUNC3(area_set_shape, RID, int, RID); - FUNC3(area_set_shape_transform, RID, int, const Transform2D &); - FUNC3(area_set_shape_disabled, RID, int, bool); - - FUNC1RC(int, area_get_shape_count, RID); - FUNC2RC(RID, area_get_shape, RID, int); - FUNC2RC(Transform2D, area_get_shape_transform, RID, int); - FUNC2(area_remove_shape, RID, int); - FUNC1(area_clear_shapes, RID); - - FUNC2(area_attach_object_instance_id, RID, ObjectID); - FUNC1RC(ObjectID, area_get_object_instance_id, RID); - - FUNC2(area_attach_canvas_instance_id, RID, ObjectID); - FUNC1RC(ObjectID, area_get_canvas_instance_id, RID); - - FUNC3(area_set_param, RID, AreaParameter, const Variant &); - FUNC2(area_set_transform, RID, const Transform2D &); - - FUNC2RC(Variant, area_get_param, RID, AreaParameter); - FUNC1RC(Transform2D, area_get_transform, RID); - - FUNC2(area_set_collision_mask, RID, uint32_t); - FUNC2(area_set_collision_layer, RID, uint32_t); - - FUNC2(area_set_monitorable, RID, bool); - FUNC2(area_set_pickable, RID, bool); - - FUNC3(area_set_monitor_callback, RID, Object *, const StringName &); - FUNC3(area_set_area_monitor_callback, RID, Object *, const StringName &); - - /* BODY API */ - - //FUNC2RID(body,BodyMode,bool); - FUNCRID(body) - - FUNC2(body_set_space, RID, RID); - FUNC1RC(RID, body_get_space, RID); - - FUNC2(body_set_mode, RID, BodyMode); - FUNC1RC(BodyMode, body_get_mode, RID); - - FUNC4(body_add_shape, RID, RID, const Transform2D &, bool); - FUNC3(body_set_shape, RID, int, RID); - FUNC3(body_set_shape_transform, RID, int, const Transform2D &); - - FUNC1RC(int, body_get_shape_count, RID); - FUNC2RC(Transform2D, body_get_shape_transform, RID, int); - FUNC2RC(RID, body_get_shape, RID, int); - - FUNC3(body_set_shape_disabled, RID, int, bool); - FUNC4(body_set_shape_as_one_way_collision, RID, int, bool, real_t); - - FUNC2(body_remove_shape, RID, int); - FUNC1(body_clear_shapes, RID); - - FUNC2(body_attach_object_instance_id, RID, ObjectID); - FUNC1RC(ObjectID, body_get_object_instance_id, RID); - - FUNC2(body_attach_canvas_instance_id, RID, ObjectID); - FUNC1RC(ObjectID, body_get_canvas_instance_id, RID); - - FUNC2(body_set_continuous_collision_detection_mode, RID, CCDMode); - FUNC1RC(CCDMode, body_get_continuous_collision_detection_mode, RID); - - FUNC2(body_set_collision_layer, RID, uint32_t); - FUNC1RC(uint32_t, body_get_collision_layer, RID); - - FUNC2(body_set_collision_mask, RID, uint32_t); - FUNC1RC(uint32_t, body_get_collision_mask, RID); - - FUNC3(body_set_param, RID, BodyParameter, const Variant &); - FUNC2RC(Variant, body_get_param, RID, BodyParameter); - - FUNC1(body_reset_mass_properties, RID); - - FUNC3(body_set_state, RID, BodyState, const Variant &); - FUNC2RC(Variant, body_get_state, RID, BodyState); - - FUNC2(body_set_applied_force, RID, const Vector2 &); - FUNC1RC(Vector2, body_get_applied_force, RID); - - FUNC2(body_set_applied_torque, RID, real_t); - FUNC1RC(real_t, body_get_applied_torque, RID); - - FUNC2(body_add_central_force, RID, const Vector2 &); - FUNC3(body_add_force, RID, const Vector2 &, const Vector2 &); - FUNC2(body_add_torque, RID, real_t); - FUNC2(body_apply_central_impulse, RID, const Vector2 &); - FUNC2(body_apply_torque_impulse, RID, real_t); - FUNC3(body_apply_impulse, RID, const Vector2 &, const Vector2 &); - FUNC2(body_set_axis_velocity, RID, const Vector2 &); - - FUNC2(body_add_collision_exception, RID, RID); - FUNC2(body_remove_collision_exception, RID, RID); - FUNC2S(body_get_collision_exceptions, RID, List *); - - FUNC2(body_set_max_contacts_reported, RID, int); - FUNC1RC(int, body_get_max_contacts_reported, RID); - - FUNC2(body_set_contacts_reported_depth_threshold, RID, real_t); - FUNC1RC(real_t, body_get_contacts_reported_depth_threshold, RID); - - FUNC2(body_set_omit_force_integration, RID, bool); - FUNC1RC(bool, body_is_omitting_force_integration, RID); - - FUNC3(body_set_state_sync_callback, RID, void *, BodyStateCallback); - FUNC3(body_set_force_integration_callback, RID, const Callable &, const Variant &); - - bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override { - return physics_2d_server->body_collide_shape(p_body, p_body_shape, p_shape, p_shape_xform, p_motion, r_results, p_result_max, r_result_count); - } - - FUNC2(body_set_pickable, RID, bool); - - bool body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result = nullptr) override { - ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); - return physics_2d_server->body_test_motion(p_body, p_parameters, r_result); - } - - // this function only works on physics process, errors and returns null otherwise - PhysicsDirectBodyState2D *body_get_direct_state(RID p_body) override { - ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr); - return physics_2d_server->body_get_direct_state(p_body); - } - - /* JOINT API */ - - FUNCRID(joint) - - FUNC1(joint_clear, RID) - - FUNC3(joint_set_param, RID, JointParam, real_t); - FUNC2RC(real_t, joint_get_param, RID, JointParam); - - FUNC2(joint_disable_collisions_between_bodies, RID, const bool); - FUNC1RC(bool, joint_is_disabled_collisions_between_bodies, RID); - - ///FUNC3RID(pin_joint,const Vector2&,RID,RID); - ///FUNC5RID(groove_joint,const Vector2&,const Vector2&,const Vector2&,RID,RID); - ///FUNC4RID(damped_spring_joint,const Vector2&,const Vector2&,RID,RID); - - //TODO need to convert this to FUNCRID, but it's a hassle.. - - FUNC4(joint_make_pin, RID, const Vector2 &, RID, RID); - FUNC6(joint_make_groove, RID, const Vector2 &, const Vector2 &, const Vector2 &, RID, RID); - FUNC5(joint_make_damped_spring, RID, const Vector2 &, const Vector2 &, RID, RID); - - FUNC3(pin_joint_set_param, RID, PinJointParam, real_t); - FUNC2RC(real_t, pin_joint_get_param, RID, PinJointParam); - - FUNC3(damped_spring_joint_set_param, RID, DampedSpringParam, real_t); - FUNC2RC(real_t, damped_spring_joint_get_param, RID, DampedSpringParam); - - FUNC1RC(JointType, joint_get_type, RID); - - /* MISC */ - - FUNC1(free, RID); - FUNC1(set_active, bool); - FUNC1(set_collision_iterations, int); - - virtual void init() override; - virtual void step(real_t p_step) override; - virtual void sync() override; - virtual void end_sync() override; - virtual void flush_queries() override; - virtual void finish() override; - - virtual bool is_flushing_queries() const override { - return physics_2d_server->is_flushing_queries(); - } - - int get_process_info(ProcessInfo p_info) override { - return physics_2d_server->get_process_info(p_info); - } - - PhysicsServer2DWrapMT(PhysicsServer2D *p_contained, bool p_create_thread); - ~PhysicsServer2DWrapMT(); - -#undef ServerNameWrapMT -#undef ServerName -#undef server_name -#undef WRITE_ACTION -}; - -#ifdef DEBUG_SYNC -#undef DEBUG_SYNC -#endif -#undef SYNC_DEBUG - -#endif // PHYSICS2DSERVERWRAPMT_H diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp deleted file mode 100644 index bde882ac24..0000000000 --- a/servers/physics_2d/shape_2d_sw.cpp +++ /dev/null @@ -1,995 +0,0 @@ -/*************************************************************************/ -/* shape_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "shape_2d_sw.h" - -#include "core/math/geometry_2d.h" -#include "core/templates/sort_array.h" - -void Shape2DSW::configure(const Rect2 &p_aabb) { - aabb = p_aabb; - configured = true; - for (const KeyValue &E : owners) { - ShapeOwner2DSW *co = (ShapeOwner2DSW *)E.key; - co->_shape_changed(); - } -} - -Vector2 Shape2DSW::get_support(const Vector2 &p_normal) const { - Vector2 res[2]; - int amnt; - get_supports(p_normal, res, amnt); - return res[0]; -} - -void Shape2DSW::add_owner(ShapeOwner2DSW *p_owner) { - Map::Element *E = owners.find(p_owner); - if (E) { - E->get()++; - } else { - owners[p_owner] = 1; - } -} - -void Shape2DSW::remove_owner(ShapeOwner2DSW *p_owner) { - Map::Element *E = owners.find(p_owner); - ERR_FAIL_COND(!E); - E->get()--; - if (E->get() == 0) { - owners.erase(E); - } -} - -bool Shape2DSW::is_owner(ShapeOwner2DSW *p_owner) const { - return owners.has(p_owner); -} - -const Map &Shape2DSW::get_owners() const { - return owners; -} - -Shape2DSW::~Shape2DSW() { - ERR_FAIL_COND(owners.size()); -} - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - -void WorldBoundaryShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - r_amount = 0; -} - -bool WorldBoundaryShape2DSW::contains_point(const Vector2 &p_point) const { - return normal.dot(p_point) < d; -} - -bool WorldBoundaryShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { - Vector2 segment = p_begin - p_end; - real_t den = normal.dot(segment); - - //printf("den is %i\n",den); - if (Math::abs(den) <= CMP_EPSILON) { - return false; - } - - real_t dist = (normal.dot(p_begin) - d) / den; - //printf("dist is %i\n",dist); - - if (dist < -CMP_EPSILON || dist > (1.0 + CMP_EPSILON)) { - return false; - } - - r_point = p_begin + segment * -dist; - r_normal = normal; - - return true; -} - -real_t WorldBoundaryShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - return 0; -} - -void WorldBoundaryShape2DSW::set_data(const Variant &p_data) { - ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY); - Array arr = p_data; - ERR_FAIL_COND(arr.size() != 2); - normal = arr[0]; - d = arr[1]; - configure(Rect2(Vector2(-1e4, -1e4), Vector2(1e4 * 2, 1e4 * 2))); -} - -Variant WorldBoundaryShape2DSW::get_data() const { - Array arr; - arr.resize(2); - arr[0] = normal; - arr[1] = d; - return arr; -} - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - -void SeparationRayShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - r_amount = 1; - - if (p_normal.y > 0) { - *r_supports = Vector2(0, length); - } else { - *r_supports = Vector2(); - } -} - -bool SeparationRayShape2DSW::contains_point(const Vector2 &p_point) const { - return false; -} - -bool SeparationRayShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { - return false; //rays can't be intersected -} - -real_t SeparationRayShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - return 0; //rays are mass-less -} - -void SeparationRayShape2DSW::set_data(const Variant &p_data) { - Dictionary d = p_data; - length = d["length"]; - slide_on_slope = d["slide_on_slope"]; - configure(Rect2(0, 0, 0.001, length)); -} - -Variant SeparationRayShape2DSW::get_data() const { - Dictionary d; - d["length"] = length; - d["slide_on_slope"] = slide_on_slope; - return d; -} - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - -void SegmentShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - if (Math::abs(p_normal.dot(n)) > _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) { - r_supports[0] = a; - r_supports[1] = b; - r_amount = 2; - return; - } - - real_t dp = p_normal.dot(b - a); - if (dp > 0) { - *r_supports = b; - } else { - *r_supports = a; - } - r_amount = 1; -} - -bool SegmentShape2DSW::contains_point(const Vector2 &p_point) const { - return false; -} - -bool SegmentShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { - if (!Geometry2D::segment_intersects_segment(p_begin, p_end, a, b, &r_point)) { - return false; - } - - if (n.dot(p_begin) > n.dot(a)) { - r_normal = n; - } else { - r_normal = -n; - } - - return true; -} - -real_t SegmentShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - return p_mass * ((a * p_scale).distance_squared_to(b * p_scale)) / 12; -} - -void SegmentShape2DSW::set_data(const Variant &p_data) { - ERR_FAIL_COND(p_data.get_type() != Variant::RECT2); - - Rect2 r = p_data; - a = r.position; - b = r.size; - n = (b - a).orthogonal(); - - Rect2 aabb; - aabb.position = a; - aabb.expand_to(b); - if (aabb.size.x == 0) { - aabb.size.x = 0.001; - } - if (aabb.size.y == 0) { - aabb.size.y = 0.001; - } - configure(aabb); -} - -Variant SegmentShape2DSW::get_data() const { - Rect2 r; - r.position = a; - r.size = b; - return r; -} - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - -void CircleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - r_amount = 1; - *r_supports = p_normal * radius; -} - -bool CircleShape2DSW::contains_point(const Vector2 &p_point) const { - return p_point.length_squared() < radius * radius; -} - -bool CircleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { - Vector2 line_vec = p_end - p_begin; - - real_t a, b, c; - - a = line_vec.dot(line_vec); - b = 2 * p_begin.dot(line_vec); - c = p_begin.dot(p_begin) - radius * radius; - - real_t sqrtterm = b * b - 4 * a * c; - - if (sqrtterm < 0) { - return false; - } - sqrtterm = Math::sqrt(sqrtterm); - real_t res = (-b - sqrtterm) / (2 * a); - - if (res < 0 || res > 1 + CMP_EPSILON) { - return false; - } - - r_point = p_begin + line_vec * res; - r_normal = r_point.normalized(); - return true; -} - -real_t CircleShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - real_t a = radius * p_scale.x; - real_t b = radius * p_scale.y; - return p_mass * (a * a + b * b) / 4; -} - -void CircleShape2DSW::set_data(const Variant &p_data) { - ERR_FAIL_COND(!p_data.is_num()); - radius = p_data; - configure(Rect2(-radius, -radius, radius * 2, radius * 2)); -} - -Variant CircleShape2DSW::get_data() const { - return radius; -} - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - -void RectangleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - for (int i = 0; i < 2; i++) { - Vector2 ag; - ag[i] = 1.0; - real_t dp = ag.dot(p_normal); - if (Math::abs(dp) < _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) { - continue; - } - - real_t sgn = dp > 0 ? 1.0 : -1.0; - - r_amount = 2; - - r_supports[0][i] = half_extents[i] * sgn; - r_supports[0][i ^ 1] = half_extents[i ^ 1]; - - r_supports[1][i] = half_extents[i] * sgn; - r_supports[1][i ^ 1] = -half_extents[i ^ 1]; - - return; - } - - /* USE POINT */ - - r_amount = 1; - r_supports[0] = Vector2( - (p_normal.x < 0) ? -half_extents.x : half_extents.x, - (p_normal.y < 0) ? -half_extents.y : half_extents.y); -} - -bool RectangleShape2DSW::contains_point(const Vector2 &p_point) const { - real_t x = p_point.x; - real_t y = p_point.y; - real_t edge_x = half_extents.x; - real_t edge_y = half_extents.y; - return (x >= -edge_x) && (x < edge_x) && (y >= -edge_y) && (y < edge_y); -} - -bool RectangleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { - return get_aabb().intersects_segment(p_begin, p_end, &r_point, &r_normal); -} - -real_t RectangleShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - Vector2 he2 = half_extents * 2 * p_scale; - return p_mass * he2.dot(he2) / 12.0; -} - -void RectangleShape2DSW::set_data(const Variant &p_data) { - ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR2); - - half_extents = p_data; - configure(Rect2(-half_extents, half_extents * 2.0)); -} - -Variant RectangleShape2DSW::get_data() const { - return half_extents; -} - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - -void CapsuleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - Vector2 n = p_normal; - - real_t d = n.y; - - if (Math::abs(d) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_THRESHOLD)) { - // make it flat - n.y = 0.0; - n.normalize(); - n *= radius; - - r_amount = 2; - r_supports[0] = n; - r_supports[0].y += height * 0.5 - radius; - r_supports[1] = n; - r_supports[1].y -= height * 0.5 - radius; - - } else { - real_t h = height * 0.5 - radius; - - n *= radius; - n.y += (d > 0) ? h : -h; - r_amount = 1; - *r_supports = n; - } -} - -bool CapsuleShape2DSW::contains_point(const Vector2 &p_point) const { - Vector2 p = p_point; - p.y = Math::abs(p.y); - p.y -= height * 0.5 - radius; - if (p.y < 0) { - p.y = 0; - } - - return p.length_squared() < radius * radius; -} - -bool CapsuleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { - real_t d = 1e10; - Vector2 n = (p_end - p_begin).normalized(); - bool collided = false; - - //try spheres - for (int i = 0; i < 2; i++) { - Vector2 begin = p_begin; - Vector2 end = p_end; - real_t ofs = (i == 0) ? -height * 0.5 + radius : height * 0.5 - radius; - begin.y += ofs; - end.y += ofs; - - Vector2 line_vec = end - begin; - - real_t a, b, c; - - a = line_vec.dot(line_vec); - b = 2 * begin.dot(line_vec); - c = begin.dot(begin) - radius * radius; - - real_t sqrtterm = b * b - 4 * a * c; - - if (sqrtterm < 0) { - continue; - } - - sqrtterm = Math::sqrt(sqrtterm); - real_t res = (-b - sqrtterm) / (2 * a); - - if (res < 0 || res > 1 + CMP_EPSILON) { - continue; - } - - Vector2 point = begin + line_vec * res; - Vector2 pointf(point.x, point.y - ofs); - real_t pd = n.dot(pointf); - if (pd < d) { - r_point = pointf; - r_normal = point.normalized(); - d = pd; - collided = true; - } - } - - Vector2 rpos, rnorm; - if (Rect2(Point2(-radius, -height * 0.5 + radius), Size2(radius * 2.0, height - radius * 2)).intersects_segment(p_begin, p_end, &rpos, &rnorm)) { - real_t pd = n.dot(rpos); - if (pd < d) { - r_point = rpos; - r_normal = rnorm; - d = pd; - collided = true; - } - } - - //return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal); - return collided; //todo -} - -real_t CapsuleShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - Vector2 he2 = Vector2(radius * 2, height) * p_scale; - return p_mass * he2.dot(he2) / 12.0; -} - -void CapsuleShape2DSW::set_data(const Variant &p_data) { - ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY && p_data.get_type() != Variant::VECTOR2); - - if (p_data.get_type() == Variant::ARRAY) { - Array arr = p_data; - ERR_FAIL_COND(arr.size() != 2); - height = arr[0]; - radius = arr[1]; - } else { - Point2 p = p_data; - radius = p.x; - height = p.y; - } - - Point2 he(radius, height * 0.5); - configure(Rect2(-he, he * 2)); -} - -Variant CapsuleShape2DSW::get_data() const { - return Point2(height, radius); -} - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - -void ConvexPolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - int support_idx = -1; - real_t d = -1e10; - r_amount = 0; - - for (int i = 0; i < point_count; i++) { - //test point - real_t ld = p_normal.dot(points[i].pos); - if (ld > d) { - support_idx = i; - d = ld; - } - - //test segment - if (points[i].normal.dot(p_normal) > _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) { - r_amount = 2; - r_supports[0] = points[i].pos; - r_supports[1] = points[(i + 1) % point_count].pos; - return; - } - } - - ERR_FAIL_COND_MSG(support_idx == -1, "Convex polygon shape support not found."); - - r_amount = 1; - r_supports[0] = points[support_idx].pos; -} - -bool ConvexPolygonShape2DSW::contains_point(const Vector2 &p_point) const { - bool out = false; - bool in = false; - - for (int i = 0; i < point_count; i++) { - real_t d = points[i].normal.dot(p_point) - points[i].normal.dot(points[i].pos); - if (d > 0) { - out = true; - } else { - in = true; - } - } - - return in != out; -} - -bool ConvexPolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { - Vector2 n = (p_end - p_begin).normalized(); - real_t d = 1e10; - bool inters = false; - - for (int i = 0; i < point_count; i++) { - //hmm.. no can do.. - /* - if (d.dot(points[i].normal)>=0) - continue; - */ - - Vector2 res; - - if (!Geometry2D::segment_intersects_segment(p_begin, p_end, points[i].pos, points[(i + 1) % point_count].pos, &res)) { - continue; - } - - real_t nd = n.dot(res); - if (nd < d) { - d = nd; - r_point = res; - r_normal = points[i].normal; - inters = true; - } - } - - return inters; -} - -real_t ConvexPolygonShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - ERR_FAIL_COND_V_MSG(point_count == 0, 0, "Convex polygon shape has no points."); - Rect2 aabb; - aabb.position = points[0].pos * p_scale; - for (int i = 0; i < point_count; i++) { - aabb.expand_to(points[i].pos * p_scale); - } - - return p_mass * aabb.size.dot(aabb.size) / 12.0; -} - -void ConvexPolygonShape2DSW::set_data(const Variant &p_data) { -#ifdef REAL_T_IS_DOUBLE - ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY); -#else - ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY); -#endif - - if (points) { - memdelete_arr(points); - } - points = nullptr; - point_count = 0; - - if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) { - Vector arr = p_data; - ERR_FAIL_COND(arr.size() == 0); - point_count = arr.size(); - points = memnew_arr(Point, point_count); - const Vector2 *r = arr.ptr(); - - for (int i = 0; i < point_count; i++) { - points[i].pos = r[i]; - } - - for (int i = 0; i < point_count; i++) { - Vector2 p = points[i].pos; - Vector2 pn = points[(i + 1) % point_count].pos; - points[i].normal = (pn - p).orthogonal().normalized(); - } - } else { - Vector dvr = p_data; - point_count = dvr.size() / 4; - ERR_FAIL_COND(point_count == 0); - - points = memnew_arr(Point, point_count); - const real_t *r = dvr.ptr(); - - for (int i = 0; i < point_count; i++) { - int idx = i << 2; - points[i].pos.x = r[idx + 0]; - points[i].pos.y = r[idx + 1]; - points[i].normal.x = r[idx + 2]; - points[i].normal.y = r[idx + 3]; - } - } - - ERR_FAIL_COND(point_count == 0); - Rect2 aabb; - aabb.position = points[0].pos; - for (int i = 1; i < point_count; i++) { - aabb.expand_to(points[i].pos); - } - - configure(aabb); -} - -Variant ConvexPolygonShape2DSW::get_data() const { - Vector dvr; - - dvr.resize(point_count); - - for (int i = 0; i < point_count; i++) { - dvr.set(i, points[i].pos); - } - - return dvr; -} - -ConvexPolygonShape2DSW::~ConvexPolygonShape2DSW() { - if (points) { - memdelete_arr(points); - } -} - -////////////////////////////////////////////////// - -void ConcavePolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - real_t d = -1e10; - int idx = -1; - for (int i = 0; i < points.size(); i++) { - real_t ld = p_normal.dot(points[i]); - if (ld > d) { - d = ld; - idx = i; - } - } - - r_amount = 1; - ERR_FAIL_COND(idx == -1); - *r_supports = points[idx]; -} - -bool ConcavePolygonShape2DSW::contains_point(const Vector2 &p_point) const { - return false; //sorry -} - -bool ConcavePolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { - if (segments.size() == 0 || points.size() == 0) { - return false; - } - - uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth); - - enum { - TEST_AABB_BIT = 0, - VISIT_LEFT_BIT = 1, - VISIT_RIGHT_BIT = 2, - VISIT_DONE_BIT = 3, - VISITED_BIT_SHIFT = 29, - NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1, - VISITED_BIT_MASK = ~NODE_IDX_MASK, - - }; - - Vector2 n = (p_end - p_begin).normalized(); - real_t d = 1e10; - bool inters = false; - - /* - for(int i=0;i> VISITED_BIT_SHIFT) { - case TEST_AABB_BIT: { - bool valid = bvh.aabb.intersects_segment(p_begin, p_end); - if (!valid) { - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - - } else { - if (bvh.left < 0) { - const Segment &s = segmentptr[bvh.right]; - Vector2 a = pointptr[s.points[0]]; - Vector2 b = pointptr[s.points[1]]; - - Vector2 res; - - if (Geometry2D::segment_intersects_segment(p_begin, p_end, a, b, &res)) { - real_t nd = n.dot(res); - if (nd < d) { - d = nd; - r_point = res; - r_normal = (b - a).orthogonal().normalized(); - inters = true; - } - } - - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - - } else { - stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; - } - } - } - continue; - case VISIT_LEFT_BIT: { - stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = bvh.left | TEST_AABB_BIT; - level++; - } - continue; - case VISIT_RIGHT_BIT: { - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = bvh.right | TEST_AABB_BIT; - level++; - } - continue; - case VISIT_DONE_BIT: { - if (level == 0) { - done = true; - break; - } else { - level--; - } - } - continue; - } - - if (done) { - break; - } - } - - if (inters) { - if (n.dot(r_normal) > 0) { - r_normal = -r_normal; - } - } - - return inters; -} - -int ConcavePolygonShape2DSW::_generate_bvh(BVH *p_bvh, int p_len, int p_depth) { - if (p_len == 1) { - bvh_depth = MAX(p_depth, bvh_depth); - bvh.push_back(*p_bvh); - return bvh.size() - 1; - } - - //else sort best - - Rect2 global_aabb = p_bvh[0].aabb; - for (int i = 1; i < p_len; i++) { - global_aabb = global_aabb.merge(p_bvh[i].aabb); - } - - if (global_aabb.size.x > global_aabb.size.y) { - SortArray sort; - sort.sort(p_bvh, p_len); - - } else { - SortArray sort; - sort.sort(p_bvh, p_len); - } - - int median = p_len / 2; - - BVH node; - node.aabb = global_aabb; - int node_idx = bvh.size(); - bvh.push_back(node); - - int l = _generate_bvh(p_bvh, median, p_depth + 1); - int r = _generate_bvh(&p_bvh[median], p_len - median, p_depth + 1); - bvh.write[node_idx].left = l; - bvh.write[node_idx].right = r; - - return node_idx; -} - -void ConcavePolygonShape2DSW::set_data(const Variant &p_data) { -#ifdef REAL_T_IS_DOUBLE - ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY); -#else - ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY); -#endif - - Rect2 aabb; - - if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) { - Vector p2arr = p_data; - int len = p2arr.size(); - ERR_FAIL_COND(len % 2); - - segments.clear(); - points.clear(); - bvh.clear(); - bvh_depth = 1; - - if (len == 0) { - configure(aabb); - return; - } - - const Vector2 *arr = p2arr.ptr(); - - Map pointmap; - for (int i = 0; i < len; i += 2) { - Point2 p1 = arr[i]; - Point2 p2 = arr[i + 1]; - int idx_p1, idx_p2; - - if (pointmap.has(p1)) { - idx_p1 = pointmap[p1]; - } else { - idx_p1 = pointmap.size(); - pointmap[p1] = idx_p1; - } - - if (pointmap.has(p2)) { - idx_p2 = pointmap[p2]; - } else { - idx_p2 = pointmap.size(); - pointmap[p2] = idx_p2; - } - - Segment s; - s.points[0] = idx_p1; - s.points[1] = idx_p2; - segments.push_back(s); - } - - points.resize(pointmap.size()); - aabb.position = pointmap.front()->key(); - for (const KeyValue &E : pointmap) { - aabb.expand_to(E.key); - points.write[E.value] = E.key; - } - - Vector main_vbh; - main_vbh.resize(segments.size()); - for (int i = 0; i < main_vbh.size(); i++) { - main_vbh.write[i].aabb.position = points[segments[i].points[0]]; - main_vbh.write[i].aabb.expand_to(points[segments[i].points[1]]); - main_vbh.write[i].left = -1; - main_vbh.write[i].right = i; - } - - _generate_bvh(main_vbh.ptrw(), main_vbh.size(), 1); - - } else { - //dictionary with arrays - } - - configure(aabb); -} - -Variant ConcavePolygonShape2DSW::get_data() const { - Vector rsegments; - int len = segments.size(); - rsegments.resize(len * 2); - Vector2 *w = rsegments.ptrw(); - for (int i = 0; i < len; i++) { - w[(i << 1) + 0] = points[segments[i].points[0]]; - w[(i << 1) + 1] = points[segments[i].points[1]]; - } - - return rsegments; -} - -void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const { - uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth); - - enum { - TEST_AABB_BIT = 0, - VISIT_LEFT_BIT = 1, - VISIT_RIGHT_BIT = 2, - VISIT_DONE_BIT = 3, - VISITED_BIT_SHIFT = 29, - NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1, - VISITED_BIT_MASK = ~NODE_IDX_MASK, - - }; - - /* - for(int i=0;i> VISITED_BIT_SHIFT) { - case TEST_AABB_BIT: { - bool valid = p_local_aabb.intersects(bvh.aabb); - if (!valid) { - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - - } else { - if (bvh.left < 0) { - const Segment &s = segmentptr[bvh.right]; - Vector2 a = pointptr[s.points[0]]; - Vector2 b = pointptr[s.points[1]]; - - SegmentShape2DSW ss(a, b, (b - a).orthogonal().normalized()); - - if (p_callback(p_userdata, &ss)) { - return; - } - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - - } else { - stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; - } - } - } - continue; - case VISIT_LEFT_BIT: { - stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = bvh.left | TEST_AABB_BIT; - level++; - } - continue; - case VISIT_RIGHT_BIT: { - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = bvh.right | TEST_AABB_BIT; - level++; - } - continue; - case VISIT_DONE_BIT: { - if (level == 0) { - return; - } else { - level--; - } - } - continue; - } - } -} diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h deleted file mode 100644 index c118826284..0000000000 --- a/servers/physics_2d/shape_2d_sw.h +++ /dev/null @@ -1,536 +0,0 @@ -/*************************************************************************/ -/* shape_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 SHAPE_2D_2DSW_H -#define SHAPE_2D_2DSW_H - -#include "servers/physics_server_2d.h" -#define _SEGMENT_IS_VALID_SUPPORT_THRESHOLD 0.99998 - -class Shape2DSW; - -class ShapeOwner2DSW { -public: - virtual void _shape_changed() = 0; - virtual void remove_shape(Shape2DSW *p_shape) = 0; - - virtual ~ShapeOwner2DSW() {} -}; - -class Shape2DSW { - RID self; - Rect2 aabb; - bool configured = false; - real_t custom_bias = 0.0; - - Map owners; - -protected: - void configure(const Rect2 &p_aabb); - -public: - _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } - _FORCE_INLINE_ RID get_self() const { return self; } - - virtual PhysicsServer2D::ShapeType get_type() const = 0; - - _FORCE_INLINE_ Rect2 get_aabb() const { return aabb; } - _FORCE_INLINE_ bool is_configured() const { return configured; } - - virtual bool allows_one_way_collision() const { return true; } - - virtual bool is_concave() const { return false; } - - virtual bool contains_point(const Vector2 &p_point) const = 0; - - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const = 0; - virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const = 0; - virtual Vector2 get_support(const Vector2 &p_normal) const; - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const = 0; - - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const = 0; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const = 0; - virtual void set_data(const Variant &p_data) = 0; - virtual Variant get_data() const = 0; - - _FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias = p_bias; } - _FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; } - - void add_owner(ShapeOwner2DSW *p_owner); - void remove_owner(ShapeOwner2DSW *p_owner); - bool is_owner(ShapeOwner2DSW *p_owner) const; - const Map &get_owners() const; - - _FORCE_INLINE_ void get_supports_transformed_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_xform, Vector2 *r_supports, int &r_amount) const { - get_supports(p_xform.basis_xform_inv(p_normal).normalized(), r_supports, r_amount); - for (int i = 0; i < r_amount; i++) { - r_supports[i] = p_xform.xform(r_supports[i]); - } - - if (r_amount == 1) { - if (Math::abs(p_normal.dot(p_cast.normalized())) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_THRESHOLD)) { - //make line because they are parallel - r_amount = 2; - r_supports[1] = r_supports[0] + p_cast; - } else if (p_cast.dot(p_normal) > 0) { - //normal points towards cast, add cast - r_supports[0] += p_cast; - } - - } else { - if (Math::abs(p_normal.dot(p_cast.normalized())) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_THRESHOLD)) { - //optimize line and make it larger because they are parallel - if ((r_supports[1] - r_supports[0]).dot(p_cast) > 0) { - //larger towards 1 - r_supports[1] += p_cast; - } else { - //larger towards 0 - r_supports[0] += p_cast; - } - } else if (p_cast.dot(p_normal) > 0) { - //normal points towards cast, add cast - r_supports[0] += p_cast; - r_supports[1] += p_cast; - } - } - } - Shape2DSW() {} - virtual ~Shape2DSW(); -}; - -//let the optimizer do the magic -#define DEFAULT_PROJECT_RANGE_CAST \ - virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { \ - project_range_cast(p_cast, p_normal, p_transform, r_min, r_max); \ - } \ - _FORCE_INLINE_ void project_range_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { \ - real_t mina, maxa; \ - real_t minb, maxb; \ - Transform2D ofsb = p_transform; \ - ofsb.elements[2] += p_cast; \ - project_range(p_normal, p_transform, mina, maxa); \ - project_range(p_normal, ofsb, minb, maxb); \ - r_min = MIN(mina, minb); \ - r_max = MAX(maxa, maxb); \ - } - -class WorldBoundaryShape2DSW : public Shape2DSW { - Vector2 normal; - real_t d = 0.0; - -public: - _FORCE_INLINE_ Vector2 get_normal() const { return normal; } - _FORCE_INLINE_ real_t get_d() const { return d; } - - virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_WORLD_BOUNDARY; } - - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - - virtual bool contains_point(const Vector2 &p_point) const override; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { - //real large - r_min = -1e10; - r_max = 1e10; - } - - virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { - project_range_cast(p_cast, p_normal, p_transform, r_min, r_max); - } - - _FORCE_INLINE_ void project_range_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { - //real large - r_min = -1e10; - r_max = 1e10; - } -}; - -class SeparationRayShape2DSW : public Shape2DSW { - real_t length = 0.0; - bool slide_on_slope = false; - -public: - _FORCE_INLINE_ real_t get_length() const { return length; } - _FORCE_INLINE_ bool get_slide_on_slope() const { return slide_on_slope; } - - virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_SEPARATION_RAY; } - - virtual bool allows_one_way_collision() const override { return false; } - - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - - virtual bool contains_point(const Vector2 &p_point) const override; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { - //real large - r_max = p_normal.dot(p_transform.get_origin()); - r_min = p_normal.dot(p_transform.xform(Vector2(0, length))); - if (r_max < r_min) { - SWAP(r_max, r_min); - } - } - - DEFAULT_PROJECT_RANGE_CAST - - _FORCE_INLINE_ SeparationRayShape2DSW() {} - _FORCE_INLINE_ SeparationRayShape2DSW(real_t p_length) { length = p_length; } -}; - -class SegmentShape2DSW : public Shape2DSW { - Vector2 a; - Vector2 b; - Vector2 n; - -public: - _FORCE_INLINE_ const Vector2 &get_a() const { return a; } - _FORCE_INLINE_ const Vector2 &get_b() const { return b; } - _FORCE_INLINE_ const Vector2 &get_normal() const { return n; } - - virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_SEGMENT; } - - _FORCE_INLINE_ Vector2 get_xformed_normal(const Transform2D &p_xform) const { - return (p_xform.xform(b) - p_xform.xform(a)).normalized().orthogonal(); - } - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - - virtual bool contains_point(const Vector2 &p_point) const override; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { - //real large - r_max = p_normal.dot(p_transform.xform(a)); - r_min = p_normal.dot(p_transform.xform(b)); - if (r_max < r_min) { - SWAP(r_max, r_min); - } - } - - DEFAULT_PROJECT_RANGE_CAST - - _FORCE_INLINE_ SegmentShape2DSW() {} - _FORCE_INLINE_ SegmentShape2DSW(const Vector2 &p_a, const Vector2 &p_b, const Vector2 &p_n) { - a = p_a; - b = p_b; - n = p_n; - } -}; - -class CircleShape2DSW : public Shape2DSW { - real_t radius; - -public: - _FORCE_INLINE_ const real_t &get_radius() const { return radius; } - - virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CIRCLE; } - - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - - virtual bool contains_point(const Vector2 &p_point) const override; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { - //real large - real_t d = p_normal.dot(p_transform.get_origin()); - - // figure out scale at point - Vector2 local_normal = p_transform.basis_xform_inv(p_normal); - real_t scale = local_normal.length(); - - r_min = d - (radius)*scale; - r_max = d + (radius)*scale; - } - - DEFAULT_PROJECT_RANGE_CAST -}; - -class RectangleShape2DSW : public Shape2DSW { - Vector2 half_extents; - -public: - _FORCE_INLINE_ const Vector2 &get_half_extents() const { return half_extents; } - - virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_RECTANGLE; } - - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - - virtual bool contains_point(const Vector2 &p_point) const override; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { - // no matter the angle, the box is mirrored anyway - r_max = -1e20; - r_min = 1e20; - for (int i = 0; i < 4; i++) { - real_t d = p_normal.dot(p_transform.xform(Vector2(((i & 1) * 2 - 1) * half_extents.x, ((i >> 1) * 2 - 1) * half_extents.y))); - - if (d > r_max) { - r_max = d; - } - if (d < r_min) { - r_min = d; - } - } - } - - _FORCE_INLINE_ Vector2 get_circle_axis(const Transform2D &p_xform, const Transform2D &p_xform_inv, const Vector2 &p_circle) const { - Vector2 local_v = p_xform_inv.xform(p_circle); - - Vector2 he( - (local_v.x < 0) ? -half_extents.x : half_extents.x, - (local_v.y < 0) ? -half_extents.y : half_extents.y); - - return (p_xform.xform(he) - p_circle).normalized(); - } - - _FORCE_INLINE_ Vector2 get_box_axis(const Transform2D &p_xform, const Transform2D &p_xform_inv, const RectangleShape2DSW *p_B, const Transform2D &p_B_xform, const Transform2D &p_B_xform_inv) const { - Vector2 a, b; - - { - Vector2 local_v = p_xform_inv.xform(p_B_xform.get_origin()); - - Vector2 he( - (local_v.x < 0) ? -half_extents.x : half_extents.x, - (local_v.y < 0) ? -half_extents.y : half_extents.y); - - a = p_xform.xform(he); - } - { - Vector2 local_v = p_B_xform_inv.xform(p_xform.get_origin()); - - Vector2 he( - (local_v.x < 0) ? -p_B->half_extents.x : p_B->half_extents.x, - (local_v.y < 0) ? -p_B->half_extents.y : p_B->half_extents.y); - - b = p_B_xform.xform(he); - } - - return (a - b).normalized(); - } - - DEFAULT_PROJECT_RANGE_CAST -}; - -class CapsuleShape2DSW : public Shape2DSW { - real_t radius = 0.0; - real_t height = 0.0; - -public: - _FORCE_INLINE_ const real_t &get_radius() const { return radius; } - _FORCE_INLINE_ const real_t &get_height() const { return height; } - - virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CAPSULE; } - - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - - virtual bool contains_point(const Vector2 &p_point) const override; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { - // no matter the angle, the box is mirrored anyway - Vector2 n = p_transform.basis_xform_inv(p_normal).normalized(); - real_t h = height * 0.5 - radius; - - n *= radius; - n.y += (n.y > 0) ? h : -h; - - r_max = p_normal.dot(p_transform.xform(n)); - r_min = p_normal.dot(p_transform.xform(-n)); - - if (r_max < r_min) { - SWAP(r_max, r_min); - } - - //ERR_FAIL_COND( r_max < r_min ); - } - - DEFAULT_PROJECT_RANGE_CAST -}; - -class ConvexPolygonShape2DSW : public Shape2DSW { - struct Point { - Vector2 pos; - Vector2 normal; //normal to next segment - }; - - Point *points = nullptr; - int point_count = 0; - -public: - _FORCE_INLINE_ int get_point_count() const { return point_count; } - _FORCE_INLINE_ const Vector2 &get_point(int p_idx) const { return points[p_idx].pos; } - _FORCE_INLINE_ const Vector2 &get_segment_normal(int p_idx) const { return points[p_idx].normal; } - _FORCE_INLINE_ Vector2 get_xformed_segment_normal(const Transform2D &p_xform, int p_idx) const { - Vector2 a = points[p_idx].pos; - p_idx++; - Vector2 b = points[p_idx == point_count ? 0 : p_idx].pos; - return (p_xform.xform(b) - p_xform.xform(a)).normalized().orthogonal(); - } - - virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CONVEX_POLYGON; } - - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - - virtual bool contains_point(const Vector2 &p_point) const override; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { - if (!points || point_count <= 0) { - r_min = r_max = 0; - return; - } - - r_min = r_max = p_normal.dot(p_transform.xform(points[0].pos)); - for (int i = 1; i < point_count; i++) { - real_t d = p_normal.dot(p_transform.xform(points[i].pos)); - if (d > r_max) { - r_max = d; - } - if (d < r_min) { - r_min = d; - } - } - } - - DEFAULT_PROJECT_RANGE_CAST - - ConvexPolygonShape2DSW() {} - ~ConvexPolygonShape2DSW(); -}; - -class ConcaveShape2DSW : public Shape2DSW { -public: - virtual bool is_concave() const override { return true; } - - // Returns true to stop the query. - typedef bool (*QueryCallback)(void *p_userdata, Shape2DSW *p_convex); - - virtual void cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0; -}; - -class ConcavePolygonShape2DSW : public ConcaveShape2DSW { - struct Segment { - int points[2] = {}; - }; - - Vector segments; - Vector points; - - struct BVH { - Rect2 aabb; - int left = 0, right = 0; - }; - - Vector bvh; - int bvh_depth = 0; - - struct BVH_CompareX { - _FORCE_INLINE_ bool operator()(const BVH &a, const BVH &b) const { - return (a.aabb.position.x + a.aabb.size.x * 0.5) < (b.aabb.position.x + b.aabb.size.x * 0.5); - } - }; - - struct BVH_CompareY { - _FORCE_INLINE_ bool operator()(const BVH &a, const BVH &b) const { - return (a.aabb.position.y + a.aabb.size.y * 0.5) < (b.aabb.position.y + b.aabb.size.y * 0.5); - } - }; - - int _generate_bvh(BVH *p_bvh, int p_len, int p_depth); - -public: - virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CONCAVE_POLYGON; } - - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { - r_min = 0; - r_max = 0; - ERR_FAIL_MSG("Unsupported call to project_rangev in ConcavePolygonShape2DSW"); - } - - void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { - r_min = 0; - r_max = 0; - ERR_FAIL_MSG("Unsupported call to project_range in ConcavePolygonShape2DSW"); - } - - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - - virtual bool contains_point(const Vector2 &p_point) const override; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; - - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override { return 0; } - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - virtual void cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override; - - DEFAULT_PROJECT_RANGE_CAST -}; - -#undef DEFAULT_PROJECT_RANGE_CAST - -#endif // SHAPE_2D_2DSW_H diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp deleted file mode 100644 index dd0780b5ff..0000000000 --- a/servers/physics_2d/space_2d_sw.cpp +++ /dev/null @@ -1,1212 +0,0 @@ -/*************************************************************************/ -/* space_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "space_2d_sw.h" - -#include "collision_solver_2d_sw.h" -#include "core/os/os.h" -#include "core/templates/pair.h" -#include "physics_server_2d_sw.h" - -#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05 - -_FORCE_INLINE_ static bool _can_collide_with(CollisionObject2DSW *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - if (!(p_object->get_collision_layer() & p_collision_mask)) { - return false; - } - - if (p_object->get_type() == CollisionObject2DSW::TYPE_AREA && !p_collide_with_areas) { - return false; - } - - if (p_object->get_type() == CollisionObject2DSW::TYPE_BODY && !p_collide_with_bodies) { - return false; - } - - return true; -} - -int PhysicsDirectSpaceState2DSW::_intersect_point_impl(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point, bool p_filter_by_canvas, ObjectID p_canvas_instance_id) { - if (p_result_max <= 0) { - return 0; - } - - Rect2 aabb; - aabb.position = p_point - Vector2(0.00001, 0.00001); - aabb.size = Vector2(0.00002, 0.00002); - - int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - int cc = 0; - - for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { - continue; - } - - if (p_exclude.has(space->intersection_query_results[i]->get_self())) { - continue; - } - - const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; - - if (p_pick_point && !col_obj->is_pickable()) { - continue; - } - - if (p_filter_by_canvas && col_obj->get_canvas_instance_id() != p_canvas_instance_id) { - continue; - } - - int shape_idx = space->intersection_query_subindex_results[i]; - - Shape2DSW *shape = col_obj->get_shape(shape_idx); - - Vector2 local_point = (col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).affine_inverse().xform(p_point); - - if (!shape->contains_point(local_point)) { - continue; - } - - if (cc >= p_result_max) { - continue; - } - - r_results[cc].collider_id = col_obj->get_instance_id(); - if (r_results[cc].collider_id.is_valid()) { - r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id); - } - r_results[cc].rid = col_obj->get_self(); - r_results[cc].shape = shape_idx; - - cc++; - } - - return cc; -} - -int PhysicsDirectSpaceState2DSW::intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point) { - return _intersect_point_impl(p_point, r_results, p_result_max, p_exclude, p_collision_mask, p_collide_with_bodies, p_collide_with_areas, p_pick_point); -} - -int PhysicsDirectSpaceState2DSW::intersect_point_on_canvas(const Vector2 &p_point, ObjectID p_canvas_instance_id, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point) { - return _intersect_point_impl(p_point, r_results, p_result_max, p_exclude, p_collision_mask, p_collide_with_bodies, p_collide_with_areas, p_pick_point, true, p_canvas_instance_id); -} - -bool PhysicsDirectSpaceState2DSW::intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - ERR_FAIL_COND_V(space->locked, false); - - Vector2 begin, end; - Vector2 normal; - begin = p_from; - end = p_to; - normal = (end - begin).normalized(); - - int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - //todo, create another array that references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision - - bool collided = false; - Vector2 res_point, res_normal; - int res_shape; - const CollisionObject2DSW *res_obj; - real_t min_d = 1e10; - - for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { - continue; - } - - if (p_exclude.has(space->intersection_query_results[i]->get_self())) { - continue; - } - - const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; - - int shape_idx = space->intersection_query_subindex_results[i]; - Transform2D inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform(); - - Vector2 local_from = inv_xform.xform(begin); - Vector2 local_to = inv_xform.xform(end); - - /*local_from = col_obj->get_inv_transform().xform(begin); - local_from = col_obj->get_shape_inv_transform(shape_idx).xform(local_from); - - local_to = col_obj->get_inv_transform().xform(end); - local_to = col_obj->get_shape_inv_transform(shape_idx).xform(local_to);*/ - - const Shape2DSW *shape = col_obj->get_shape(shape_idx); - - Vector2 shape_point, shape_normal; - - if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) { - Transform2D xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - shape_point = xform.xform(shape_point); - - real_t ld = normal.dot(shape_point); - - if (ld < min_d) { - min_d = ld; - res_point = shape_point; - res_normal = inv_xform.basis_xform_inv(shape_normal).normalized(); - res_shape = shape_idx; - res_obj = col_obj; - collided = true; - } - } - } - - if (!collided) { - return false; - } - - r_result.collider_id = res_obj->get_instance_id(); - if (r_result.collider_id.is_valid()) { - r_result.collider = ObjectDB::get_instance(r_result.collider_id); - } - r_result.normal = res_normal; - r_result.position = res_point; - r_result.rid = res_obj->get_self(); - r_result.shape = res_shape; - - return true; -} - -int PhysicsDirectSpaceState2DSW::intersect_shape(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - if (p_result_max <= 0) { - return 0; - } - - Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.get_or_null(p_shape); - ERR_FAIL_COND_V(!shape, 0); - - Rect2 aabb = p_xform.xform(shape->get_aabb()); - aabb = aabb.grow(p_margin); - - int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - int cc = 0; - - for (int i = 0; i < amount; i++) { - if (cc >= p_result_max) { - break; - } - - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { - continue; - } - - if (p_exclude.has(space->intersection_query_results[i]->get_self())) { - continue; - } - - const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; - int shape_idx = space->intersection_query_subindex_results[i]; - - if (!CollisionSolver2DSW::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), nullptr, nullptr, nullptr, p_margin)) { - continue; - } - - r_results[cc].collider_id = col_obj->get_instance_id(); - if (r_results[cc].collider_id.is_valid()) { - r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id); - } - r_results[cc].rid = col_obj->get_self(); - r_results[cc].shape = shape_idx; - - cc++; - } - - return cc; -} - -bool PhysicsDirectSpaceState2DSW::cast_motion(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.get_or_null(p_shape); - ERR_FAIL_COND_V(!shape, false); - - Rect2 aabb = p_xform.xform(shape->get_aabb()); - aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion - aabb = aabb.grow(p_margin); - - int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - real_t best_safe = 1; - real_t best_unsafe = 1; - - for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { - continue; - } - - if (p_exclude.has(space->intersection_query_results[i]->get_self())) { - continue; //ignore excluded - } - - const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; - int shape_idx = space->intersection_query_subindex_results[i]; - - Transform2D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - //test initial overlap, does it collide if going all the way? - if (!CollisionSolver2DSW::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_margin)) { - continue; - } - - //test initial overlap, ignore objects it's inside of. - if (CollisionSolver2DSW::solve(shape, p_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_margin)) { - continue; - } - - Vector2 mnormal = p_motion.normalized(); - - //just do kinematic solving - real_t low = 0.0; - real_t hi = 1.0; - real_t fraction_coeff = 0.5; - for (int j = 0; j < 8; j++) { //steps should be customizable.. - real_t fraction = low + (hi - low) * fraction_coeff; - - Vector2 sep = mnormal; //important optimization for this to work fast enough - bool collided = CollisionSolver2DSW::solve(shape, p_xform, p_motion * fraction, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, &sep, p_margin); - - if (collided) { - hi = fraction; - if ((j == 0) || (low > 0.0)) { // Did it not collide before? - // When alternating or first iteration, use dichotomy. - fraction_coeff = 0.5; - } else { - // When colliding again, converge faster towards low fraction - // for more accurate results with long motions that collide near the start. - fraction_coeff = 0.25; - } - } else { - low = fraction; - if ((j == 0) || (hi < 1.0)) { // Did it collide before? - // When alternating or first iteration, use dichotomy. - fraction_coeff = 0.5; - } else { - // When not colliding again, converge faster towards high fraction - // for more accurate results with long motions that collide near the end. - fraction_coeff = 0.75; - } - } - } - - if (low < best_safe) { - best_safe = low; - best_unsafe = hi; - } - } - - p_closest_safe = best_safe; - p_closest_unsafe = best_unsafe; - - return true; -} - -bool PhysicsDirectSpaceState2DSW::collide_shape(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - if (p_result_max <= 0) { - return false; - } - - Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.get_or_null(p_shape); - ERR_FAIL_COND_V(!shape, 0); - - Rect2 aabb = p_shape_xform.xform(shape->get_aabb()); - aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion - aabb = aabb.grow(p_margin); - - int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - bool collided = false; - r_result_count = 0; - - PhysicsServer2DSW::CollCbkData cbk; - cbk.max = p_result_max; - cbk.amount = 0; - cbk.passed = 0; - cbk.ptr = r_results; - CollisionSolver2DSW::CallbackResult cbkres = PhysicsServer2DSW::_shape_col_cbk; - - PhysicsServer2DSW::CollCbkData *cbkptr = &cbk; - - for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { - continue; - } - - const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; - - if (p_exclude.has(col_obj->get_self())) { - continue; - } - - int shape_idx = space->intersection_query_subindex_results[i]; - - cbk.valid_dir = Vector2(); - cbk.valid_depth = 0; - - if (CollisionSolver2DSW::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, nullptr, p_margin)) { - collided = cbk.amount > 0; - } - } - - r_result_count = cbk.amount; - - return collided; -} - -struct _RestCallbackData2D { - const CollisionObject2DSW *object = nullptr; - const CollisionObject2DSW *best_object = nullptr; - int local_shape = 0; - int best_local_shape = 0; - int shape = 0; - int best_shape = 0; - Vector2 best_contact; - Vector2 best_normal; - real_t best_len = 0.0; - Vector2 valid_dir; - real_t valid_depth = 0.0; - real_t min_allowed_depth = 0.0; -}; - -static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) { - _RestCallbackData2D *rd = (_RestCallbackData2D *)p_userdata; - - Vector2 contact_rel = p_point_B - p_point_A; - real_t len = contact_rel.length(); - - if (len < rd->min_allowed_depth) { - return; - } - - if (len <= rd->best_len) { - return; - } - - Vector2 normal = contact_rel / len; - - if (rd->valid_dir != Vector2()) { - if (len > rd->valid_depth) { - return; - } - - if (rd->valid_dir.dot(normal) > -CMP_EPSILON) { - return; - } - } - - rd->best_len = len; - rd->best_contact = p_point_B; - rd->best_normal = normal; - rd->best_object = rd->object; - rd->best_shape = rd->shape; - rd->best_local_shape = rd->local_shape; -} - -bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, ShapeRestInfo *r_info, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.get_or_null(p_shape); - ERR_FAIL_COND_V(!shape, 0); - - real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; - - Rect2 aabb = p_shape_xform.xform(shape->get_aabb()); - aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion - aabb = aabb.grow(p_margin); - - int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - _RestCallbackData2D rcd; - rcd.best_len = 0; - rcd.best_object = nullptr; - rcd.best_shape = 0; - rcd.min_allowed_depth = min_contact_depth; - - for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { - continue; - } - - const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; - - if (p_exclude.has(col_obj->get_self())) { - continue; - } - - int shape_idx = space->intersection_query_subindex_results[i]; - - rcd.valid_dir = Vector2(); - rcd.object = col_obj; - rcd.shape = shape_idx; - rcd.local_shape = 0; - bool sc = CollisionSolver2DSW::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, nullptr, p_margin); - if (!sc) { - continue; - } - } - - if (rcd.best_len == 0 || !rcd.best_object) { - return false; - } - - r_info->collider_id = rcd.best_object->get_instance_id(); - r_info->shape = rcd.best_shape; - r_info->normal = rcd.best_normal; - r_info->point = rcd.best_contact; - r_info->rid = rcd.best_object->get_self(); - if (rcd.best_object->get_type() == CollisionObject2DSW::TYPE_BODY) { - const Body2DSW *body = static_cast(rcd.best_object); - Vector2 rel_vec = r_info->point - (body->get_transform().get_origin() + body->get_center_of_mass()); - r_info->linear_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); - - } else { - r_info->linear_velocity = Vector2(); - } - - return true; -} - -//////////////////////////////////////////////////////////////////////////////////////////////////////////// - -int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb) { - int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results); - - for (int i = 0; i < amount; i++) { - bool keep = true; - - if (intersection_query_results[i] == p_body) { - keep = false; - } else if (intersection_query_results[i]->get_type() == CollisionObject2DSW::TYPE_AREA) { - keep = false; - } else if (!p_body->collides_with(static_cast(intersection_query_results[i]))) { - keep = false; - } else if (static_cast(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) { - keep = false; - } - - if (!keep) { - if (i < amount - 1) { - SWAP(intersection_query_results[i], intersection_query_results[amount - 1]); - SWAP(intersection_query_subindex_results[i], intersection_query_subindex_results[amount - 1]); - } - - amount--; - i--; - } - } - - return amount; -} - -bool Space2DSW::test_body_motion(Body2DSW *p_body, const PhysicsServer2D::MotionParameters &p_parameters, PhysicsServer2D::MotionResult *r_result) { - //give me back regular physics engine logic - //this is madness - //and most people using this function will think - //what it does is simpler than using physics - //this took about a week to get right.. - //but is it right? who knows at this point.. - - if (r_result) { - r_result->collider_id = ObjectID(); - r_result->collider_shape = 0; - } - Rect2 body_aabb; - - bool shapes_found = false; - - for (int i = 0; i < p_body->get_shape_count(); i++) { - if (p_body->is_shape_disabled(i)) { - continue; - } - - if (!shapes_found) { - body_aabb = p_body->get_shape_aabb(i); - shapes_found = true; - } else { - body_aabb = body_aabb.merge(p_body->get_shape_aabb(i)); - } - } - - if (!shapes_found) { - if (r_result) { - *r_result = PhysicsServer2D::MotionResult(); - r_result->travel = p_parameters.motion; - } - return false; - } - - // Undo the currently transform the physics server is aware of and apply the provided one - body_aabb = p_parameters.from.xform(p_body->get_inv_transform().xform(body_aabb)); - body_aabb = body_aabb.grow(p_parameters.margin); - - static const int max_excluded_shape_pairs = 32; - ExcludedShapeSW excluded_shape_pairs[max_excluded_shape_pairs]; - int excluded_shape_pair_count = 0; - - real_t min_contact_depth = p_parameters.margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; - - real_t motion_length = p_parameters.motion.length(); - Vector2 motion_normal = p_parameters.motion / motion_length; - - Transform2D body_transform = p_parameters.from; - - bool recovered = false; - - { - //STEP 1, FREE BODY IF STUCK - - const int max_results = 32; - int recover_attempts = 4; - Vector2 sr[max_results * 2]; - - do { - PhysicsServer2DSW::CollCbkData cbk; - cbk.max = max_results; - cbk.amount = 0; - cbk.passed = 0; - cbk.ptr = sr; - cbk.invalid_by_dir = 0; - excluded_shape_pair_count = 0; //last step is the one valid - - PhysicsServer2DSW::CollCbkData *cbkptr = &cbk; - CollisionSolver2DSW::CallbackResult cbkres = PhysicsServer2DSW::_shape_col_cbk; - - bool collided = false; - - int amount = _cull_aabb_for_body(p_body, body_aabb); - - for (int j = 0; j < p_body->get_shape_count(); j++) { - if (p_body->is_shape_disabled(j)) { - continue; - } - - Shape2DSW *body_shape = p_body->get_shape(j); - Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); - - for (int i = 0; i < amount; i++) { - const CollisionObject2DSW *col_obj = intersection_query_results[i]; - if (p_parameters.exclude_bodies.has(col_obj->get_self())) { - continue; - } - if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { - continue; - } - - int shape_idx = intersection_query_subindex_results[i]; - - Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - - if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) { - cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); - - real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); - cbk.valid_depth = MAX(owc_margin, p_parameters.margin); //user specified, but never less than actual margin or it won't work - cbk.invalid_by_dir = 0; - - if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { - const Body2DSW *b = static_cast(col_obj); - if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_DYNAMIC) { - //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction - Vector2 lv = b->get_linear_velocity(); - //compute displacement from linear velocity - Vector2 motion = lv * last_step; - real_t motion_len = motion.length(); - motion.normalize(); - cbk.valid_depth += motion_len * MAX(motion.dot(-cbk.valid_dir), 0.0); - } - } - } else { - cbk.valid_dir = Vector2(); - cbk.valid_depth = 0; - cbk.invalid_by_dir = 0; - } - - int current_passed = cbk.passed; //save how many points passed collision - bool did_collide = false; - - Shape2DSW *against_shape = col_obj->get_shape(shape_idx); - if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, p_parameters.margin)) { - did_collide = cbk.passed > current_passed; //more passed, so collision actually existed - } - - if (!did_collide && cbk.invalid_by_dir > 0) { - //this shape must be excluded - if (excluded_shape_pair_count < max_excluded_shape_pairs) { - ExcludedShapeSW esp; - esp.local_shape = body_shape; - esp.against_object = col_obj; - esp.against_shape_index = shape_idx; - excluded_shape_pairs[excluded_shape_pair_count++] = esp; - } - } - - if (did_collide) { - collided = true; - } - } - } - - if (!collided) { - break; - } - - recovered = true; - - Vector2 recover_motion; - for (int i = 0; i < cbk.amount; i++) { - Vector2 a = sr[i * 2 + 0]; - Vector2 b = sr[i * 2 + 1]; - - // Compute plane on b towards a. - Vector2 n = (a - b).normalized(); - real_t d = n.dot(b); - - // Compute depth on recovered motion. - real_t depth = n.dot(a + recover_motion) - d; - if (depth > min_contact_depth + CMP_EPSILON) { - // Only recover if there is penetration. - recover_motion -= n * (depth - min_contact_depth) * 0.4; - } - } - - if (recover_motion == Vector2()) { - collided = false; - break; - } - - body_transform.elements[2] += recover_motion; - body_aabb.position += recover_motion; - - recover_attempts--; - - } while (recover_attempts); - } - - real_t safe = 1.0; - real_t unsafe = 1.0; - int best_shape = -1; - - { - // STEP 2 ATTEMPT MOTION - - Rect2 motion_aabb = body_aabb; - motion_aabb.position += p_parameters.motion; - motion_aabb = motion_aabb.merge(body_aabb); - - int amount = _cull_aabb_for_body(p_body, motion_aabb); - - for (int body_shape_idx = 0; body_shape_idx < p_body->get_shape_count(); body_shape_idx++) { - if (p_body->is_shape_disabled(body_shape_idx)) { - continue; - } - - Shape2DSW *body_shape = p_body->get_shape(body_shape_idx); - - // Colliding separation rays allows to properly snap to the ground, - // otherwise it's not needed in regular motion. - if (!p_parameters.collide_separation_ray && (body_shape->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY)) { - // When slide on slope is on, separation ray shape acts like a regular shape. - if (!static_cast(body_shape)->get_slide_on_slope()) { - continue; - } - } - - Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(body_shape_idx); - - bool stuck = false; - - real_t best_safe = 1; - real_t best_unsafe = 1; - - for (int i = 0; i < amount; i++) { - const CollisionObject2DSW *col_obj = intersection_query_results[i]; - if (p_parameters.exclude_bodies.has(col_obj->get_self())) { - continue; - } - if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { - continue; - } - - int col_shape_idx = intersection_query_subindex_results[i]; - Shape2DSW *against_shape = col_obj->get_shape(col_shape_idx); - - bool excluded = false; - - for (int k = 0; k < excluded_shape_pair_count; k++) { - if (excluded_shape_pairs[k].local_shape == body_shape && excluded_shape_pairs[k].against_object == col_obj && excluded_shape_pairs[k].against_shape_index == col_shape_idx) { - excluded = true; - break; - } - } - - if (excluded) { - continue; - } - - Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(col_shape_idx); - //test initial overlap, does it collide if going all the way? - if (!CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_parameters.motion, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) { - continue; - } - - //test initial overlap - if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) { - if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) { - Vector2 direction = col_obj_shape_xform.get_axis(1).normalized(); - if (motion_normal.dot(direction) < 0) { - continue; - } - } - - stuck = true; - break; - } - - //just do kinematic solving - real_t low = 0.0; - real_t hi = 1.0; - real_t fraction_coeff = 0.5; - for (int k = 0; k < 8; k++) { //steps should be customizable.. - real_t fraction = low + (hi - low) * fraction_coeff; - - Vector2 sep = motion_normal; //important optimization for this to work fast enough - bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_parameters.motion * fraction, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0); - - if (collided) { - hi = fraction; - if ((k == 0) || (low > 0.0)) { // Did it not collide before? - // When alternating or first iteration, use dichotomy. - fraction_coeff = 0.5; - } else { - // When colliding again, converge faster towards low fraction - // for more accurate results with long motions that collide near the start. - fraction_coeff = 0.25; - } - } else { - low = fraction; - if ((k == 0) || (hi < 1.0)) { // Did it collide before? - // When alternating or first iteration, use dichotomy. - fraction_coeff = 0.5; - } else { - // When not colliding again, converge faster towards high fraction - // for more accurate results with long motions that collide near the end. - fraction_coeff = 0.75; - } - } - } - - if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) { - Vector2 cd[2]; - PhysicsServer2DSW::CollCbkData cbk; - cbk.max = 1; - cbk.amount = 0; - cbk.passed = 0; - cbk.ptr = cd; - cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); - - cbk.valid_depth = 10e20; - - Vector2 sep = motion_normal; //important optimization for this to work fast enough - bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_parameters.motion * (hi + contact_max_allowed_penetration), col_obj->get_shape(col_shape_idx), col_obj_shape_xform, Vector2(), PhysicsServer2DSW::_shape_col_cbk, &cbk, &sep, 0); - if (!collided || cbk.amount == 0) { - continue; - } - } - - if (low < best_safe) { - best_safe = low; - best_unsafe = hi; - } - } - - if (stuck) { - safe = 0; - unsafe = 0; - best_shape = body_shape_idx; //sadly it's the best - break; - } - if (best_safe == 1.0) { - continue; - } - if (best_safe < safe) { - safe = best_safe; - unsafe = best_unsafe; - best_shape = body_shape_idx; - } - } - } - - bool collided = false; - - if (recovered || (safe < 1)) { - if (safe >= 1) { - best_shape = -1; //no best shape with cast, reset to -1 - } - - //it collided, let's get the rest info in unsafe advance - Transform2D ugt = body_transform; - ugt.elements[2] += p_parameters.motion * unsafe; - - _RestCallbackData2D rcd; - rcd.best_len = 0; - rcd.best_object = nullptr; - rcd.best_shape = 0; - - // Allowed depth can't be lower than motion length, in order to handle contacts at low speed. - rcd.min_allowed_depth = MIN(motion_length, min_contact_depth); - - int from_shape = best_shape != -1 ? best_shape : 0; - int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count(); - - for (int j = from_shape; j < to_shape; j++) { - if (p_body->is_shape_disabled(j)) { - continue; - } - - Transform2D body_shape_xform = ugt * p_body->get_shape_transform(j); - Shape2DSW *body_shape = p_body->get_shape(j); - - body_aabb.position += p_parameters.motion * unsafe; - - int amount = _cull_aabb_for_body(p_body, body_aabb); - - for (int i = 0; i < amount; i++) { - const CollisionObject2DSW *col_obj = intersection_query_results[i]; - if (p_parameters.exclude_bodies.has(col_obj->get_self())) { - continue; - } - if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { - continue; - } - - int shape_idx = intersection_query_subindex_results[i]; - - Shape2DSW *against_shape = col_obj->get_shape(shape_idx); - - bool excluded = false; - for (int k = 0; k < excluded_shape_pair_count; k++) { - if (excluded_shape_pairs[k].local_shape == body_shape && excluded_shape_pairs[k].against_object == col_obj && excluded_shape_pairs[k].against_shape_index == shape_idx) { - excluded = true; - break; - } - } - if (excluded) { - continue; - } - - Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - - if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) { - rcd.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); - - real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); - rcd.valid_depth = MAX(owc_margin, p_parameters.margin); //user specified, but never less than actual margin or it won't work - - if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { - const Body2DSW *b = static_cast(col_obj); - if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_DYNAMIC) { - //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction - Vector2 lv = b->get_linear_velocity(); - //compute displacement from linear velocity - Vector2 motion = lv * last_step; - real_t motion_len = motion.length(); - motion.normalize(); - rcd.valid_depth += motion_len * MAX(motion.dot(-rcd.valid_dir), 0.0); - } - } - } else { - rcd.valid_dir = Vector2(); - rcd.valid_depth = 0; - } - - rcd.object = col_obj; - rcd.shape = shape_idx; - rcd.local_shape = j; - bool sc = CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), _rest_cbk_result, &rcd, nullptr, p_parameters.margin); - if (!sc) { - continue; - } - } - } - - if (rcd.best_len != 0) { - if (r_result) { - r_result->collider = rcd.best_object->get_self(); - r_result->collider_id = rcd.best_object->get_instance_id(); - r_result->collider_shape = rcd.best_shape; - r_result->collision_local_shape = rcd.best_local_shape; - r_result->collision_normal = rcd.best_normal; - r_result->collision_point = rcd.best_contact; - r_result->collision_depth = rcd.best_len; - r_result->collision_safe_fraction = safe; - r_result->collision_unsafe_fraction = unsafe; - - const Body2DSW *body = static_cast(rcd.best_object); - Vector2 rel_vec = r_result->collision_point - (body->get_transform().get_origin() + body->get_center_of_mass()); - r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); - - r_result->travel = safe * p_parameters.motion; - r_result->remainder = p_parameters.motion - safe * p_parameters.motion; - r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin()); - } - - collided = true; - } - } - - if (!collided && r_result) { - r_result->travel = p_parameters.motion; - r_result->remainder = Vector2(); - r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin()); - } - - return collided; -} - -void *Space2DSW::_broadphase_pair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_self) { - if (!A->interacts_with(B)) { - return nullptr; - } - - CollisionObject2DSW::Type type_A = A->get_type(); - CollisionObject2DSW::Type type_B = B->get_type(); - if (type_A > type_B) { - SWAP(A, B); - SWAP(p_subindex_A, p_subindex_B); - SWAP(type_A, type_B); - } - - Space2DSW *self = (Space2DSW *)p_self; - self->collision_pairs++; - - if (type_A == CollisionObject2DSW::TYPE_AREA) { - Area2DSW *area = static_cast(A); - if (type_B == CollisionObject2DSW::TYPE_AREA) { - Area2DSW *area_b = static_cast(B); - Area2Pair2DSW *area2_pair = memnew(Area2Pair2DSW(area_b, p_subindex_B, area, p_subindex_A)); - return area2_pair; - } else { - Body2DSW *body = static_cast(B); - AreaPair2DSW *area_pair = memnew(AreaPair2DSW(body, p_subindex_B, area, p_subindex_A)); - return area_pair; - } - - } else { - BodyPair2DSW *b = memnew(BodyPair2DSW((Body2DSW *)A, p_subindex_A, (Body2DSW *)B, p_subindex_B)); - return b; - } - - return nullptr; -} - -void Space2DSW::_broadphase_unpair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, void *p_self) { - if (!p_data) { - return; - } - - Space2DSW *self = (Space2DSW *)p_self; - self->collision_pairs--; - Constraint2DSW *c = (Constraint2DSW *)p_data; - memdelete(c); -} - -const SelfList::List &Space2DSW::get_active_body_list() const { - return active_list; -} - -void Space2DSW::body_add_to_active_list(SelfList *p_body) { - active_list.add(p_body); -} - -void Space2DSW::body_remove_from_active_list(SelfList *p_body) { - active_list.remove(p_body); -} - -void Space2DSW::body_add_to_mass_properties_update_list(SelfList *p_body) { - mass_properties_update_list.add(p_body); -} - -void Space2DSW::body_remove_from_mass_properties_update_list(SelfList *p_body) { - mass_properties_update_list.remove(p_body); -} - -BroadPhase2DSW *Space2DSW::get_broadphase() { - return broadphase; -} - -void Space2DSW::add_object(CollisionObject2DSW *p_object) { - ERR_FAIL_COND(objects.has(p_object)); - objects.insert(p_object); -} - -void Space2DSW::remove_object(CollisionObject2DSW *p_object) { - ERR_FAIL_COND(!objects.has(p_object)); - objects.erase(p_object); -} - -const Set &Space2DSW::get_objects() const { - return objects; -} - -void Space2DSW::body_add_to_state_query_list(SelfList *p_body) { - state_query_list.add(p_body); -} - -void Space2DSW::body_remove_from_state_query_list(SelfList *p_body) { - state_query_list.remove(p_body); -} - -void Space2DSW::area_add_to_monitor_query_list(SelfList *p_area) { - monitor_query_list.add(p_area); -} - -void Space2DSW::area_remove_from_monitor_query_list(SelfList *p_area) { - monitor_query_list.remove(p_area); -} - -void Space2DSW::area_add_to_moved_list(SelfList *p_area) { - area_moved_list.add(p_area); -} - -void Space2DSW::area_remove_from_moved_list(SelfList *p_area) { - area_moved_list.remove(p_area); -} - -const SelfList::List &Space2DSW::get_moved_area_list() const { - return area_moved_list; -} - -void Space2DSW::call_queries() { - while (state_query_list.first()) { - Body2DSW *b = state_query_list.first()->self(); - state_query_list.remove(state_query_list.first()); - b->call_queries(); - } - - while (monitor_query_list.first()) { - Area2DSW *a = monitor_query_list.first()->self(); - monitor_query_list.remove(monitor_query_list.first()); - a->call_queries(); - } -} - -void Space2DSW::setup() { - contact_debug_count = 0; - - while (mass_properties_update_list.first()) { - mass_properties_update_list.first()->self()->update_mass_properties(); - mass_properties_update_list.remove(mass_properties_update_list.first()); - } -} - -void Space2DSW::update() { - broadphase->update(); -} - -void Space2DSW::set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_value) { - switch (p_param) { - case PhysicsServer2D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: - contact_recycle_radius = p_value; - break; - case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_SEPARATION: - contact_max_separation = p_value; - break; - case PhysicsServer2D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: - contact_max_allowed_penetration = p_value; - break; - case PhysicsServer2D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: - body_linear_velocity_sleep_threshold = p_value; - break; - case PhysicsServer2D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: - body_angular_velocity_sleep_threshold = p_value; - break; - case PhysicsServer2D::SPACE_PARAM_BODY_TIME_TO_SLEEP: - body_time_to_sleep = p_value; - break; - case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: - constraint_bias = p_value; - break; - } -} - -real_t Space2DSW::get_param(PhysicsServer2D::SpaceParameter p_param) const { - switch (p_param) { - case PhysicsServer2D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: - return contact_recycle_radius; - case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_SEPARATION: - return contact_max_separation; - case PhysicsServer2D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: - return contact_max_allowed_penetration; - case PhysicsServer2D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: - return body_linear_velocity_sleep_threshold; - case PhysicsServer2D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: - return body_angular_velocity_sleep_threshold; - case PhysicsServer2D::SPACE_PARAM_BODY_TIME_TO_SLEEP: - return body_time_to_sleep; - case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: - return constraint_bias; - } - return 0; -} - -void Space2DSW::lock() { - locked = true; -} - -void Space2DSW::unlock() { - locked = false; -} - -bool Space2DSW::is_locked() const { - return locked; -} - -PhysicsDirectSpaceState2DSW *Space2DSW::get_direct_state() { - return direct_access; -} - -Space2DSW::Space2DSW() { - body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_linear", 2.0); - body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_angular", Math::deg2rad(8.0)); - body_time_to_sleep = GLOBAL_DEF("physics/2d/time_before_sleep", 0.5); - ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/time_before_sleep", PropertyInfo(Variant::FLOAT, "physics/2d/time_before_sleep", PROPERTY_HINT_RANGE, "0,5,0.01,or_greater")); - - broadphase = BroadPhase2DSW::create_func(); - broadphase->set_pair_callback(_broadphase_pair, this); - broadphase->set_unpair_callback(_broadphase_unpair, this); - - direct_access = memnew(PhysicsDirectSpaceState2DSW); - direct_access->space = this; -} - -Space2DSW::~Space2DSW() { - memdelete(broadphase); - memdelete(direct_access); -} diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h deleted file mode 100644 index 746b5c6c9a..0000000000 --- a/servers/physics_2d/space_2d_sw.h +++ /dev/null @@ -1,211 +0,0 @@ -/*************************************************************************/ -/* space_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 SPACE_2D_SW_H -#define SPACE_2D_SW_H - -#include "area_2d_sw.h" -#include "area_pair_2d_sw.h" -#include "body_2d_sw.h" -#include "body_pair_2d_sw.h" -#include "broad_phase_2d_sw.h" -#include "collision_object_2d_sw.h" -#include "core/config/project_settings.h" -#include "core/templates/hash_map.h" -#include "core/typedefs.h" - -class PhysicsDirectSpaceState2DSW : public PhysicsDirectSpaceState2D { - GDCLASS(PhysicsDirectSpaceState2DSW, PhysicsDirectSpaceState2D); - - int _intersect_point_impl(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point, bool p_filter_by_canvas = false, ObjectID p_canvas_instance_id = ObjectID()); - -public: - Space2DSW *space = nullptr; - - virtual int intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_point = false) override; - virtual int intersect_point_on_canvas(const Vector2 &p_point, ObjectID p_canvas_instance_id, ShapeResult *r_results, int p_result_max, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_point = false) override; - virtual bool intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual int intersect_shape(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual bool cast_motion(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual bool collide_shape(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual bool rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, ShapeRestInfo *r_info, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - - PhysicsDirectSpaceState2DSW() {} -}; - -class Space2DSW { -public: - enum ElapsedTime { - ELAPSED_TIME_INTEGRATE_FORCES, - ELAPSED_TIME_GENERATE_ISLANDS, - ELAPSED_TIME_SETUP_CONSTRAINTS, - ELAPSED_TIME_SOLVE_CONSTRAINTS, - ELAPSED_TIME_INTEGRATE_VELOCITIES, - ELAPSED_TIME_MAX - - }; - -private: - struct ExcludedShapeSW { - Shape2DSW *local_shape = nullptr; - const CollisionObject2DSW *against_object = nullptr; - int against_shape_index = 0; - }; - - uint64_t elapsed_time[ELAPSED_TIME_MAX] = {}; - - PhysicsDirectSpaceState2DSW *direct_access = nullptr; - RID self; - - BroadPhase2DSW *broadphase; - SelfList::List active_list; - SelfList::List mass_properties_update_list; - SelfList::List state_query_list; - SelfList::List monitor_query_list; - SelfList::List area_moved_list; - - static void *_broadphase_pair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_self); - static void _broadphase_unpair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, void *p_self); - - Set objects; - - Area2DSW *area = nullptr; - - real_t contact_recycle_radius = 1.0; - real_t contact_max_separation = 1.5; - real_t contact_max_allowed_penetration = 0.3; - real_t constraint_bias = 0.2; - - enum { - INTERSECTION_QUERY_MAX = 2048 - }; - - CollisionObject2DSW *intersection_query_results[INTERSECTION_QUERY_MAX]; - int intersection_query_subindex_results[INTERSECTION_QUERY_MAX]; - - real_t body_linear_velocity_sleep_threshold = 0.0; - real_t body_angular_velocity_sleep_threshold = 0.0; - real_t body_time_to_sleep = 0.0; - - bool locked = false; - - real_t last_step = 0.001; - - int island_count = 0; - int active_objects = 0; - int collision_pairs = 0; - - int _cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb); - - Vector contact_debug; - int contact_debug_count = 0; - - friend class PhysicsDirectSpaceState2DSW; - -public: - _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } - _FORCE_INLINE_ RID get_self() const { return self; } - - void set_default_area(Area2DSW *p_area) { area = p_area; } - Area2DSW *get_default_area() const { return area; } - - const SelfList::List &get_active_body_list() const; - void body_add_to_active_list(SelfList *p_body); - void body_remove_from_active_list(SelfList *p_body); - void body_add_to_mass_properties_update_list(SelfList *p_body); - void body_remove_from_mass_properties_update_list(SelfList *p_body); - void area_add_to_moved_list(SelfList *p_area); - void area_remove_from_moved_list(SelfList *p_area); - const SelfList::List &get_moved_area_list() const; - - void body_add_to_state_query_list(SelfList *p_body); - void body_remove_from_state_query_list(SelfList *p_body); - - void area_add_to_monitor_query_list(SelfList *p_area); - void area_remove_from_monitor_query_list(SelfList *p_area); - - BroadPhase2DSW *get_broadphase(); - - void add_object(CollisionObject2DSW *p_object); - void remove_object(CollisionObject2DSW *p_object); - const Set &get_objects() const; - - _FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; } - _FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; } - _FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; } - _FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; } - _FORCE_INLINE_ real_t get_body_linear_velocity_sleep_threshold() const { return body_linear_velocity_sleep_threshold; } - _FORCE_INLINE_ real_t get_body_angular_velocity_sleep_threshold() const { return body_angular_velocity_sleep_threshold; } - _FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; } - - void update(); - void setup(); - void call_queries(); - - bool is_locked() const; - void lock(); - void unlock(); - - real_t get_last_step() const { return last_step; } - void set_last_step(real_t p_step) { last_step = p_step; } - - void set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_value); - real_t get_param(PhysicsServer2D::SpaceParameter p_param) const; - - void set_island_count(int p_island_count) { island_count = p_island_count; } - int get_island_count() const { return island_count; } - - void set_active_objects(int p_active_objects) { active_objects = p_active_objects; } - int get_active_objects() const { return active_objects; } - - int get_collision_pairs() const { return collision_pairs; } - - bool test_body_motion(Body2DSW *p_body, const PhysicsServer2D::MotionParameters &p_parameters, PhysicsServer2D::MotionResult *r_result); - - void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); } - _FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.is_empty(); } - _FORCE_INLINE_ void add_debug_contact(const Vector2 &p_contact) { - if (contact_debug_count < contact_debug.size()) { - contact_debug.write[contact_debug_count++] = p_contact; - } - } - _FORCE_INLINE_ Vector get_debug_contacts() { return contact_debug; } - _FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; } - - PhysicsDirectSpaceState2DSW *get_direct_state(); - - void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; } - uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; } - - Space2DSW(); - ~Space2DSW(); -}; - -#endif // SPACE_2D_SW_H diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp deleted file mode 100644 index a03e30f850..0000000000 --- a/servers/physics_2d/step_2d_sw.cpp +++ /dev/null @@ -1,308 +0,0 @@ -/*************************************************************************/ -/* step_2d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "step_2d_sw.h" - -#include "core/os/os.h" - -#define BODY_ISLAND_COUNT_RESERVE 128 -#define BODY_ISLAND_SIZE_RESERVE 512 -#define ISLAND_COUNT_RESERVE 128 -#define ISLAND_SIZE_RESERVE 512 -#define CONSTRAINT_COUNT_RESERVE 1024 - -void Step2DSW::_populate_island(Body2DSW *p_body, LocalVector &p_body_island, LocalVector &p_constraint_island) { - p_body->set_island_step(_step); - - if (p_body->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) { - // Only dynamic bodies are tested for activation. - p_body_island.push_back(p_body); - } - - for (const Pair &E : p_body->get_constraint_list()) { - Constraint2DSW *constraint = (Constraint2DSW *)E.first; - if (constraint->get_island_step() == _step) { - continue; // Already processed. - } - constraint->set_island_step(_step); - p_constraint_island.push_back(constraint); - all_constraints.push_back(constraint); - - for (int i = 0; i < constraint->get_body_count(); i++) { - if (i == E.second) { - continue; - } - Body2DSW *other_body = constraint->get_body_ptr()[i]; - if (other_body->get_island_step() == _step) { - continue; // Already processed. - } - if (other_body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC) { - continue; // Static bodies don't connect islands. - } - _populate_island(other_body, p_body_island, p_constraint_island); - } - } -} - -void Step2DSW::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) { - Constraint2DSW *constraint = all_constraints[p_constraint_index]; - constraint->setup(delta); -} - -void Step2DSW::_pre_solve_island(LocalVector &p_constraint_island) const { - uint32_t constraint_count = p_constraint_island.size(); - uint32_t valid_constraint_count = 0; - for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { - Constraint2DSW *constraint = p_constraint_island[constraint_index]; - if (p_constraint_island[constraint_index]->pre_solve(delta)) { - // Keep this constraint for solving. - p_constraint_island[valid_constraint_count++] = constraint; - } - } - p_constraint_island.resize(valid_constraint_count); -} - -void Step2DSW::_solve_island(uint32_t p_island_index, void *p_userdata) const { - const LocalVector &constraint_island = constraint_islands[p_island_index]; - - for (int i = 0; i < iterations; i++) { - uint32_t constraint_count = constraint_island.size(); - for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { - constraint_island[constraint_index]->solve(delta); - } - } -} - -void Step2DSW::_check_suspend(LocalVector &p_body_island) const { - bool can_sleep = true; - - uint32_t body_count = p_body_island.size(); - for (uint32_t body_index = 0; body_index < body_count; ++body_index) { - Body2DSW *body = p_body_island[body_index]; - - if (!body->sleep_test(delta)) { - can_sleep = false; - } - } - - // Put all to sleep or wake up everyone. - for (uint32_t body_index = 0; body_index < body_count; ++body_index) { - Body2DSW *body = p_body_island[body_index]; - - bool active = body->is_active(); - - if (active == can_sleep) { - body->set_active(!can_sleep); - } - } -} - -void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { - p_space->lock(); // can't access space during this - - p_space->setup(); //update inertias, etc - - p_space->set_last_step(p_delta); - - iterations = p_iterations; - delta = p_delta; - - const SelfList::List *body_list = &p_space->get_active_body_list(); - - /* INTEGRATE FORCES */ - - uint64_t profile_begtime = OS::get_singleton()->get_ticks_usec(); - uint64_t profile_endtime = 0; - - int active_count = 0; - - const SelfList *b = body_list->first(); - while (b) { - b->self()->integrate_forces(p_delta); - b = b->next(); - active_count++; - } - - p_space->set_active_objects(active_count); - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime); - profile_begtime = profile_endtime; - } - - /* GENERATE CONSTRAINT ISLANDS FOR MOVING AREAS */ - - uint32_t island_count = 0; - - const SelfList::List &aml = p_space->get_moved_area_list(); - - while (aml.first()) { - for (const Set::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { - Constraint2DSW *constraint = E->get(); - if (constraint->get_island_step() == _step) { - continue; - } - constraint->set_island_step(_step); - - // Each constraint can be on a separate island for areas as there's no solving phase. - ++island_count; - if (constraint_islands.size() < island_count) { - constraint_islands.resize(island_count); - } - LocalVector &constraint_island = constraint_islands[island_count - 1]; - constraint_island.clear(); - - all_constraints.push_back(constraint); - constraint_island.push_back(constraint); - } - p_space->area_remove_from_moved_list((SelfList *)aml.first()); //faster to remove here - } - - /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */ - - b = body_list->first(); - - uint32_t body_island_count = 0; - - while (b) { - Body2DSW *body = b->self(); - - if (body->get_island_step() != _step) { - ++body_island_count; - if (body_islands.size() < body_island_count) { - body_islands.resize(body_island_count); - } - LocalVector &body_island = body_islands[body_island_count - 1]; - body_island.clear(); - body_island.reserve(BODY_ISLAND_SIZE_RESERVE); - - ++island_count; - if (constraint_islands.size() < island_count) { - constraint_islands.resize(island_count); - } - LocalVector &constraint_island = constraint_islands[island_count - 1]; - constraint_island.clear(); - constraint_island.reserve(ISLAND_SIZE_RESERVE); - - _populate_island(body, body_island, constraint_island); - - if (body_island.is_empty()) { - --body_island_count; - } - - if (constraint_island.is_empty()) { - --island_count; - } - } - b = b->next(); - } - - p_space->set_island_count((int)island_count); - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); - profile_begtime = profile_endtime; - } - - /* SETUP CONSTRAINTS / PROCESS COLLISIONS */ - - uint32_t total_contraint_count = all_constraints.size(); - work_pool.do_work(total_contraint_count, this, &Step2DSW::_setup_contraint, nullptr); - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime); - profile_begtime = profile_endtime; - } - - /* PRE-SOLVE CONSTRAINT ISLANDS */ - - // Warning: This doesn't run on threads, because it involves thread-unsafe processing. - for (uint32_t island_index = 0; island_index < island_count; ++island_index) { - _pre_solve_island(constraint_islands[island_index]); - } - - /* SOLVE CONSTRAINT ISLANDS */ - - // Warning: _solve_island modifies the constraint islands for optimization purpose, - // their content is not reliable after these calls and shouldn't be used anymore. - if (island_count > 1) { - work_pool.do_work(island_count, this, &Step2DSW::_solve_island, nullptr); - } else if (island_count > 0) { - _solve_island(0); - } - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime); - profile_begtime = profile_endtime; - } - - /* INTEGRATE VELOCITIES */ - - b = body_list->first(); - while (b) { - const SelfList *n = b->next(); - b->self()->integrate_velocities(p_delta); - b = n; // in case it shuts itself down - } - - /* SLEEP / WAKE UP ISLANDS */ - - for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) { - _check_suspend(body_islands[island_index]); - } - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime); - //profile_begtime=profile_endtime; - } - - all_constraints.clear(); - - p_space->update(); - p_space->unlock(); - _step++; -} - -Step2DSW::Step2DSW() { - body_islands.reserve(BODY_ISLAND_COUNT_RESERVE); - constraint_islands.reserve(ISLAND_COUNT_RESERVE); - all_constraints.reserve(CONSTRAINT_COUNT_RESERVE); - - work_pool.init(); -} - -Step2DSW::~Step2DSW() { - work_pool.finish(); -} diff --git a/servers/physics_2d/step_2d_sw.h b/servers/physics_2d/step_2d_sw.h deleted file mode 100644 index de8e76cc99..0000000000 --- a/servers/physics_2d/step_2d_sw.h +++ /dev/null @@ -1,63 +0,0 @@ -/*************************************************************************/ -/* step_2d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 STEP_2D_SW_H -#define STEP_2D_SW_H - -#include "space_2d_sw.h" - -#include "core/templates/local_vector.h" -#include "core/templates/thread_work_pool.h" - -class Step2DSW { - uint64_t _step = 1; - - int iterations = 0; - real_t delta = 0.0; - - ThreadWorkPool work_pool; - - LocalVector> body_islands; - LocalVector> constraint_islands; - LocalVector all_constraints; - - void _populate_island(Body2DSW *p_body, LocalVector &p_body_island, LocalVector &p_constraint_island); - void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr); - void _pre_solve_island(LocalVector &p_constraint_island) const; - void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr) const; - void _check_suspend(LocalVector &p_body_island) const; - -public: - void step(Space2DSW *p_space, real_t p_delta, int p_iterations); - Step2DSW(); - ~Step2DSW(); -}; - -#endif // STEP_2D_SW_H diff --git a/servers/physics_3d/area_3d_sw.cpp b/servers/physics_3d/area_3d_sw.cpp deleted file mode 100644 index 630ab7e229..0000000000 --- a/servers/physics_3d/area_3d_sw.cpp +++ /dev/null @@ -1,336 +0,0 @@ -/*************************************************************************/ -/* area_3d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "area_3d_sw.h" -#include "body_3d_sw.h" -#include "soft_body_3d_sw.h" -#include "space_3d_sw.h" - -Area3DSW::BodyKey::BodyKey(SoftBody3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { - rid = p_body->get_self(); - instance_id = p_body->get_instance_id(); - body_shape = p_body_shape; - area_shape = p_area_shape; -} - -Area3DSW::BodyKey::BodyKey(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { - rid = p_body->get_self(); - instance_id = p_body->get_instance_id(); - body_shape = p_body_shape; - area_shape = p_area_shape; -} - -Area3DSW::BodyKey::BodyKey(Area3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { - rid = p_body->get_self(); - instance_id = p_body->get_instance_id(); - body_shape = p_body_shape; - area_shape = p_area_shape; -} - -void Area3DSW::_shapes_changed() { - if (!moved_list.in_list() && get_space()) { - get_space()->area_add_to_moved_list(&moved_list); - } -} - -void Area3DSW::set_transform(const Transform3D &p_transform) { - if (!moved_list.in_list() && get_space()) { - get_space()->area_add_to_moved_list(&moved_list); - } - - _set_transform(p_transform); - _set_inv_transform(p_transform.affine_inverse()); -} - -void Area3DSW::set_space(Space3DSW *p_space) { - if (get_space()) { - if (monitor_query_list.in_list()) { - get_space()->area_remove_from_monitor_query_list(&monitor_query_list); - } - if (moved_list.in_list()) { - get_space()->area_remove_from_moved_list(&moved_list); - } - } - - monitored_bodies.clear(); - monitored_areas.clear(); - - _set_space(p_space); -} - -void Area3DSW::set_monitor_callback(ObjectID p_id, const StringName &p_method) { - if (p_id == monitor_callback_id) { - monitor_callback_method = p_method; - return; - } - - _unregister_shapes(); - - monitor_callback_id = p_id; - monitor_callback_method = p_method; - - monitored_bodies.clear(); - monitored_areas.clear(); - - _shape_changed(); - - if (!moved_list.in_list() && get_space()) { - get_space()->area_add_to_moved_list(&moved_list); - } -} - -void Area3DSW::set_area_monitor_callback(ObjectID p_id, const StringName &p_method) { - if (p_id == area_monitor_callback_id) { - area_monitor_callback_method = p_method; - return; - } - - _unregister_shapes(); - - area_monitor_callback_id = p_id; - area_monitor_callback_method = p_method; - - monitored_bodies.clear(); - monitored_areas.clear(); - - _shape_changed(); - - if (!moved_list.in_list() && get_space()) { - get_space()->area_add_to_moved_list(&moved_list); - } -} - -void Area3DSW::set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode p_mode) { - bool do_override = p_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; - if (do_override == (space_override_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED)) { - return; - } - _unregister_shapes(); - space_override_mode = p_mode; - _shape_changed(); -} - -void Area3DSW::set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value) { - switch (p_param) { - case PhysicsServer3D::AREA_PARAM_GRAVITY: - gravity = p_value; - break; - case PhysicsServer3D::AREA_PARAM_GRAVITY_VECTOR: - gravity_vector = p_value; - break; - case PhysicsServer3D::AREA_PARAM_GRAVITY_IS_POINT: - gravity_is_point = p_value; - break; - case PhysicsServer3D::AREA_PARAM_GRAVITY_DISTANCE_SCALE: - gravity_distance_scale = p_value; - break; - case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_ATTENUATION: - point_attenuation = p_value; - break; - case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP: - linear_damp = p_value; - break; - case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP: - angular_damp = p_value; - break; - case PhysicsServer3D::AREA_PARAM_PRIORITY: - priority = p_value; - break; - case PhysicsServer3D::AREA_PARAM_WIND_FORCE_MAGNITUDE: - ERR_FAIL_COND_MSG(wind_force_magnitude < 0, "Wind force magnitude must be a non-negative real number, but a negative number was specified."); - wind_force_magnitude = p_value; - break; - case PhysicsServer3D::AREA_PARAM_WIND_SOURCE: - wind_source = p_value; - break; - case PhysicsServer3D::AREA_PARAM_WIND_DIRECTION: - wind_direction = p_value; - break; - case PhysicsServer3D::AREA_PARAM_WIND_ATTENUATION_FACTOR: - ERR_FAIL_COND_MSG(wind_attenuation_factor < 0, "Wind attenuation factor must be a non-negative real number, but a negative number was specified."); - wind_attenuation_factor = p_value; - break; - } -} - -Variant Area3DSW::get_param(PhysicsServer3D::AreaParameter p_param) const { - switch (p_param) { - case PhysicsServer3D::AREA_PARAM_GRAVITY: - return gravity; - case PhysicsServer3D::AREA_PARAM_GRAVITY_VECTOR: - return gravity_vector; - case PhysicsServer3D::AREA_PARAM_GRAVITY_IS_POINT: - return gravity_is_point; - case PhysicsServer3D::AREA_PARAM_GRAVITY_DISTANCE_SCALE: - return gravity_distance_scale; - case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_ATTENUATION: - return point_attenuation; - case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP: - return linear_damp; - case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP: - return angular_damp; - case PhysicsServer3D::AREA_PARAM_PRIORITY: - return priority; - case PhysicsServer3D::AREA_PARAM_WIND_FORCE_MAGNITUDE: - return wind_force_magnitude; - case PhysicsServer3D::AREA_PARAM_WIND_SOURCE: - return wind_source; - case PhysicsServer3D::AREA_PARAM_WIND_DIRECTION: - return wind_direction; - case PhysicsServer3D::AREA_PARAM_WIND_ATTENUATION_FACTOR: - return wind_attenuation_factor; - } - - return Variant(); -} - -void Area3DSW::_queue_monitor_update() { - ERR_FAIL_COND(!get_space()); - - if (!monitor_query_list.in_list()) { - get_space()->area_add_to_monitor_query_list(&monitor_query_list); - } -} - -void Area3DSW::set_monitorable(bool p_monitorable) { - if (monitorable == p_monitorable) { - return; - } - - monitorable = p_monitorable; - _set_static(!monitorable); -} - -void Area3DSW::call_queries() { - if (monitor_callback_id.is_valid() && !monitored_bodies.is_empty()) { - Variant res[5]; - Variant *resptr[5]; - for (int i = 0; i < 5; i++) { - resptr[i] = &res[i]; - } - - Object *obj = ObjectDB::get_instance(monitor_callback_id); - if (!obj) { - monitored_bodies.clear(); - monitor_callback_id = ObjectID(); - return; - } - - for (Map::Element *E = monitored_bodies.front(); E;) { - if (E->get().state == 0) { // Nothing happened - Map::Element *next = E->next(); - monitored_bodies.erase(E); - E = next; - continue; - } - - res[0] = E->get().state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED; - res[1] = E->key().rid; - res[2] = E->key().instance_id; - res[3] = E->key().body_shape; - res[4] = E->key().area_shape; - - Map::Element *next = E->next(); - monitored_bodies.erase(E); - E = next; - - Callable::CallError ce; - obj->call(monitor_callback_method, (const Variant **)resptr, 5, ce); - } - } - - if (area_monitor_callback_id.is_valid() && !monitored_areas.is_empty()) { - Variant res[5]; - Variant *resptr[5]; - for (int i = 0; i < 5; i++) { - resptr[i] = &res[i]; - } - - Object *obj = ObjectDB::get_instance(area_monitor_callback_id); - if (!obj) { - monitored_areas.clear(); - area_monitor_callback_id = ObjectID(); - return; - } - - for (Map::Element *E = monitored_areas.front(); E;) { - if (E->get().state == 0) { // Nothing happened - Map::Element *next = E->next(); - monitored_areas.erase(E); - E = next; - continue; - } - - res[0] = E->get().state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED; - res[1] = E->key().rid; - res[2] = E->key().instance_id; - res[3] = E->key().body_shape; - res[4] = E->key().area_shape; - - Map::Element *next = E->next(); - monitored_areas.erase(E); - E = next; - - Callable::CallError ce; - obj->call(area_monitor_callback_method, (const Variant **)resptr, 5, ce); - } - } -} - -void Area3DSW::compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const { - if (is_gravity_point()) { - const real_t gravity_distance_scale = get_gravity_distance_scale(); - Vector3 v = get_transform().xform(get_gravity_vector()) - p_position; - if (gravity_distance_scale > 0) { - const real_t v_length = v.length(); - if (v_length > 0) { - const real_t v_scaled = v_length * gravity_distance_scale; - r_gravity = (v.normalized() * (get_gravity() / (v_scaled * v_scaled))); - } else { - r_gravity = Vector3(); - } - } else { - r_gravity = v.normalized() * get_gravity(); - } - } else { - r_gravity = get_gravity_vector() * get_gravity(); - } -} - -Area3DSW::Area3DSW() : - CollisionObject3DSW(TYPE_AREA), - monitor_query_list(this), - moved_list(this) { - _set_static(true); //areas are never active - set_ray_pickable(false); -} - -Area3DSW::~Area3DSW() { -} diff --git a/servers/physics_3d/area_3d_sw.h b/servers/physics_3d/area_3d_sw.h deleted file mode 100644 index af5c23949c..0000000000 --- a/servers/physics_3d/area_3d_sw.h +++ /dev/null @@ -1,245 +0,0 @@ -/*************************************************************************/ -/* area_3d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 AREA_SW_H -#define AREA_SW_H - -#include "collision_object_3d_sw.h" -#include "core/templates/self_list.h" -#include "servers/physics_server_3d.h" - -class Space3DSW; -class Body3DSW; -class SoftBody3DSW; -class Constraint3DSW; - -class Area3DSW : public CollisionObject3DSW { - PhysicsServer3D::AreaSpaceOverrideMode space_override_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; - real_t gravity = 9.80665; - Vector3 gravity_vector = Vector3(0, -1, 0); - bool gravity_is_point = false; - real_t gravity_distance_scale = 0.0; - real_t point_attenuation = 1.0; - real_t linear_damp = 0.1; - real_t angular_damp = 0.1; - real_t wind_force_magnitude = 0.0; - real_t wind_attenuation_factor = 0.0; - Vector3 wind_source; - Vector3 wind_direction; - int priority = 0; - bool monitorable = false; - - ObjectID monitor_callback_id; - StringName monitor_callback_method; - - ObjectID area_monitor_callback_id; - StringName area_monitor_callback_method; - - SelfList monitor_query_list; - SelfList moved_list; - - struct BodyKey { - RID rid; - ObjectID instance_id; - uint32_t body_shape = 0; - uint32_t area_shape = 0; - - _FORCE_INLINE_ bool operator<(const BodyKey &p_key) const { - if (rid == p_key.rid) { - if (body_shape == p_key.body_shape) { - return area_shape < p_key.area_shape; - } else { - return body_shape < p_key.body_shape; - } - } else { - return rid < p_key.rid; - } - } - - _FORCE_INLINE_ BodyKey() {} - BodyKey(SoftBody3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); - BodyKey(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); - BodyKey(Area3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); - }; - - struct BodyState { - int state = 0; - _FORCE_INLINE_ void inc() { state++; } - _FORCE_INLINE_ void dec() { state--; } - }; - - Map monitored_soft_bodies; - Map monitored_bodies; - Map monitored_areas; - - Set constraints; - - virtual void _shapes_changed(); - void _queue_monitor_update(); - -public: - void set_monitor_callback(ObjectID p_id, const StringName &p_method); - _FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id.is_valid(); } - - void set_area_monitor_callback(ObjectID p_id, const StringName &p_method); - _FORCE_INLINE_ bool has_area_monitor_callback() const { return area_monitor_callback_id.is_valid(); } - - _FORCE_INLINE_ void add_body_to_query(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); - _FORCE_INLINE_ void remove_body_from_query(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); - - _FORCE_INLINE_ void add_soft_body_to_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape); - _FORCE_INLINE_ void remove_soft_body_from_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape); - - _FORCE_INLINE_ void add_area_to_query(Area3DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape); - _FORCE_INLINE_ void remove_area_from_query(Area3DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape); - - void set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value); - Variant get_param(PhysicsServer3D::AreaParameter p_param) const; - - void set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode p_mode); - PhysicsServer3D::AreaSpaceOverrideMode get_space_override_mode() const { return space_override_mode; } - - _FORCE_INLINE_ void set_gravity(real_t p_gravity) { gravity = p_gravity; } - _FORCE_INLINE_ real_t get_gravity() const { return gravity; } - - _FORCE_INLINE_ void set_gravity_vector(const Vector3 &p_gravity) { gravity_vector = p_gravity; } - _FORCE_INLINE_ Vector3 get_gravity_vector() const { return gravity_vector; } - - _FORCE_INLINE_ void set_gravity_as_point(bool p_enable) { gravity_is_point = p_enable; } - _FORCE_INLINE_ bool is_gravity_point() const { return gravity_is_point; } - - _FORCE_INLINE_ void set_gravity_distance_scale(real_t scale) { gravity_distance_scale = scale; } - _FORCE_INLINE_ real_t get_gravity_distance_scale() const { return gravity_distance_scale; } - - _FORCE_INLINE_ void set_point_attenuation(real_t p_point_attenuation) { point_attenuation = p_point_attenuation; } - _FORCE_INLINE_ real_t get_point_attenuation() const { return point_attenuation; } - - _FORCE_INLINE_ void set_linear_damp(real_t p_linear_damp) { linear_damp = p_linear_damp; } - _FORCE_INLINE_ real_t get_linear_damp() const { return linear_damp; } - - _FORCE_INLINE_ void set_angular_damp(real_t p_angular_damp) { angular_damp = p_angular_damp; } - _FORCE_INLINE_ real_t get_angular_damp() const { return angular_damp; } - - _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } - _FORCE_INLINE_ int get_priority() const { return priority; } - - _FORCE_INLINE_ void set_wind_force_magnitude(real_t p_wind_force_magnitude) { wind_force_magnitude = p_wind_force_magnitude; } - _FORCE_INLINE_ real_t get_wind_force_magnitude() const { return wind_force_magnitude; } - - _FORCE_INLINE_ void set_wind_attenuation_factor(real_t p_wind_attenuation_factor) { wind_attenuation_factor = p_wind_attenuation_factor; } - _FORCE_INLINE_ real_t get_wind_attenuation_factor() const { return wind_attenuation_factor; } - - _FORCE_INLINE_ void set_wind_source(const Vector3 &p_wind_source) { wind_source = p_wind_source; } - _FORCE_INLINE_ const Vector3 &get_wind_source() const { return wind_source; } - - _FORCE_INLINE_ void set_wind_direction(const Vector3 &p_wind_direction) { wind_direction = p_wind_direction; } - _FORCE_INLINE_ const Vector3 &get_wind_direction() const { return wind_direction; } - - _FORCE_INLINE_ void add_constraint(Constraint3DSW *p_constraint) { constraints.insert(p_constraint); } - _FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraints.erase(p_constraint); } - _FORCE_INLINE_ const Set &get_constraints() const { return constraints; } - _FORCE_INLINE_ void clear_constraints() { constraints.clear(); } - - void set_monitorable(bool p_monitorable); - _FORCE_INLINE_ bool is_monitorable() const { return monitorable; } - - void set_transform(const Transform3D &p_transform); - - void set_space(Space3DSW *p_space); - - void call_queries(); - - void compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const; - - Area3DSW(); - ~Area3DSW(); -}; - -void Area3DSW::add_soft_body_to_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape) { - BodyKey bk(p_soft_body, p_soft_body_shape, p_area_shape); - monitored_soft_bodies[bk].inc(); - if (!monitor_query_list.in_list()) { - _queue_monitor_update(); - } -} - -void Area3DSW::remove_soft_body_from_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape) { - BodyKey bk(p_soft_body, p_soft_body_shape, p_area_shape); - monitored_soft_bodies[bk].dec(); - if (!monitor_query_list.in_list()) { - _queue_monitor_update(); - } -} - -void Area3DSW::add_body_to_query(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { - BodyKey bk(p_body, p_body_shape, p_area_shape); - monitored_bodies[bk].inc(); - if (!monitor_query_list.in_list()) { - _queue_monitor_update(); - } -} - -void Area3DSW::remove_body_from_query(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { - BodyKey bk(p_body, p_body_shape, p_area_shape); - monitored_bodies[bk].dec(); - if (!monitor_query_list.in_list()) { - _queue_monitor_update(); - } -} - -void Area3DSW::add_area_to_query(Area3DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { - BodyKey bk(p_area, p_area_shape, p_self_shape); - monitored_areas[bk].inc(); - if (!monitor_query_list.in_list()) { - _queue_monitor_update(); - } -} - -void Area3DSW::remove_area_from_query(Area3DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { - BodyKey bk(p_area, p_area_shape, p_self_shape); - monitored_areas[bk].dec(); - if (!monitor_query_list.in_list()) { - _queue_monitor_update(); - } -} - -struct AreaCMP { - Area3DSW *area = nullptr; - int refCount = 0; - _FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); } - _FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); } - _FORCE_INLINE_ AreaCMP() {} - _FORCE_INLINE_ AreaCMP(Area3DSW *p_area) { - area = p_area; - refCount = 1; - } -}; - -#endif // AREA__SW_H diff --git a/servers/physics_3d/area_pair_3d_sw.cpp b/servers/physics_3d/area_pair_3d_sw.cpp deleted file mode 100644 index bf4f0035b4..0000000000 --- a/servers/physics_3d/area_pair_3d_sw.cpp +++ /dev/null @@ -1,272 +0,0 @@ -/*************************************************************************/ -/* area_pair_3d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "area_pair_3d_sw.h" -#include "collision_solver_3d_sw.h" - -bool AreaPair3DSW::setup(real_t p_step) { - bool result = false; - if (area->collides_with(body) && CollisionSolver3DSW::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), nullptr, this)) { - result = true; - } - - process_collision = false; - if (result != colliding) { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { - process_collision = true; - } else if (area->has_monitor_callback()) { - process_collision = true; - } - - colliding = result; - } - - return process_collision; -} - -bool AreaPair3DSW::pre_solve(real_t p_step) { - if (!process_collision) { - return false; - } - - if (colliding) { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { - body->add_area(area); - } - - if (area->has_monitor_callback()) { - area->add_body_to_query(body, body_shape, area_shape); - } - } else { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { - body->remove_area(area); - } - - if (area->has_monitor_callback()) { - area->remove_body_from_query(body, body_shape, area_shape); - } - } - - return false; // Never do any post solving. -} - -void AreaPair3DSW::solve(real_t p_step) { - // Nothing to do. -} - -AreaPair3DSW::AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, int p_area_shape) { - body = p_body; - area = p_area; - body_shape = p_body_shape; - area_shape = p_area_shape; - body->add_constraint(this, 0); - area->add_constraint(this); - if (p_body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { - p_body->set_active(true); - } -} - -AreaPair3DSW::~AreaPair3DSW() { - if (colliding) { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { - body->remove_area(area); - } - if (area->has_monitor_callback()) { - area->remove_body_from_query(body, body_shape, area_shape); - } - } - body->remove_constraint(this); - area->remove_constraint(this); -} - -//////////////////////////////////////////////////// - -bool Area2Pair3DSW::setup(real_t p_step) { - bool result_a = area_a->collides_with(area_b); - bool result_b = area_b->collides_with(area_a); - if ((result_a || result_b) && !CollisionSolver3DSW::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), nullptr, this)) { - result_a = false; - result_b = false; - } - - bool process_collision = false; - - process_collision_a = false; - if (result_a != colliding_a) { - if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { - process_collision_a = true; - process_collision = true; - } - colliding_a = result_a; - } - - process_collision_b = false; - if (result_b != colliding_b) { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { - process_collision_b = true; - process_collision = true; - } - colliding_b = result_b; - } - - return process_collision; -} - -bool Area2Pair3DSW::pre_solve(real_t p_step) { - if (process_collision_a) { - if (colliding_a) { - area_a->add_area_to_query(area_b, shape_b, shape_a); - } else { - area_a->remove_area_from_query(area_b, shape_b, shape_a); - } - } - - if (process_collision_b) { - if (colliding_b) { - area_b->add_area_to_query(area_a, shape_a, shape_b); - } else { - area_b->remove_area_from_query(area_a, shape_a, shape_b); - } - } - - return false; // Never do any post solving. -} - -void Area2Pair3DSW::solve(real_t p_step) { - // Nothing to do. -} - -Area2Pair3DSW::Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area_b, int p_shape_b) { - area_a = p_area_a; - area_b = p_area_b; - shape_a = p_shape_a; - shape_b = p_shape_b; - area_a->add_constraint(this); - area_b->add_constraint(this); -} - -Area2Pair3DSW::~Area2Pair3DSW() { - if (colliding_a) { - if (area_a->has_area_monitor_callback()) { - area_a->remove_area_from_query(area_b, shape_b, shape_a); - } - } - - if (colliding_b) { - if (area_b->has_area_monitor_callback()) { - area_b->remove_area_from_query(area_a, shape_a, shape_b); - } - } - - area_a->remove_constraint(this); - area_b->remove_constraint(this); -} - -//////////////////////////////////////////////////// - -bool AreaSoftBodyPair3DSW::setup(real_t p_step) { - bool result = false; - if ( - area->collides_with(soft_body) && - CollisionSolver3DSW::solve_static( - soft_body->get_shape(soft_body_shape), - soft_body->get_transform() * soft_body->get_shape_transform(soft_body_shape), - area->get_shape(area_shape), - area->get_transform() * area->get_shape_transform(area_shape), - nullptr, - this)) { - result = true; - } - - process_collision = false; - if (result != colliding) { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { - process_collision = true; - } else if (area->has_monitor_callback()) { - process_collision = true; - } - - colliding = result; - } - - return process_collision; -} - -bool AreaSoftBodyPair3DSW::pre_solve(real_t p_step) { - if (!process_collision) { - return false; - } - - if (colliding) { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { - soft_body->add_area(area); - } - - if (area->has_monitor_callback()) { - area->add_soft_body_to_query(soft_body, soft_body_shape, area_shape); - } - } else { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { - soft_body->remove_area(area); - } - - if (area->has_monitor_callback()) { - area->remove_soft_body_from_query(soft_body, soft_body_shape, area_shape); - } - } - - return false; // Never do any post solving. -} - -void AreaSoftBodyPair3DSW::solve(real_t p_step) { - // Nothing to do. -} - -AreaSoftBodyPair3DSW::AreaSoftBodyPair3DSW(SoftBody3DSW *p_soft_body, int p_soft_body_shape, Area3DSW *p_area, int p_area_shape) { - soft_body = p_soft_body; - area = p_area; - soft_body_shape = p_soft_body_shape; - area_shape = p_area_shape; - soft_body->add_constraint(this); - area->add_constraint(this); -} - -AreaSoftBodyPair3DSW::~AreaSoftBodyPair3DSW() { - if (colliding) { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { - soft_body->remove_area(area); - } - if (area->has_monitor_callback()) { - area->remove_soft_body_from_query(soft_body, soft_body_shape, area_shape); - } - } - soft_body->remove_constraint(this); - area->remove_constraint(this); -} diff --git a/servers/physics_3d/area_pair_3d_sw.h b/servers/physics_3d/area_pair_3d_sw.h deleted file mode 100644 index 4572dcbb23..0000000000 --- a/servers/physics_3d/area_pair_3d_sw.h +++ /dev/null @@ -1,92 +0,0 @@ -/*************************************************************************/ -/* area_pair_3d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 AREA_PAIR_SW_H -#define AREA_PAIR_SW_H - -#include "area_3d_sw.h" -#include "body_3d_sw.h" -#include "constraint_3d_sw.h" -#include "soft_body_3d_sw.h" - -class AreaPair3DSW : public Constraint3DSW { - Body3DSW *body; - Area3DSW *area; - int body_shape; - int area_shape; - bool colliding = false; - bool process_collision = false; - -public: - virtual bool setup(real_t p_step) override; - virtual bool pre_solve(real_t p_step) override; - virtual void solve(real_t p_step) override; - - AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, int p_area_shape); - ~AreaPair3DSW(); -}; - -class Area2Pair3DSW : public Constraint3DSW { - Area3DSW *area_a; - Area3DSW *area_b; - int shape_a; - int shape_b; - bool colliding_a = false; - bool colliding_b = false; - bool process_collision_a = false; - bool process_collision_b = false; - -public: - virtual bool setup(real_t p_step) override; - virtual bool pre_solve(real_t p_step) override; - virtual void solve(real_t p_step) override; - - Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area_b, int p_shape_b); - ~Area2Pair3DSW(); -}; - -class AreaSoftBodyPair3DSW : public Constraint3DSW { - SoftBody3DSW *soft_body; - Area3DSW *area; - int soft_body_shape; - int area_shape; - bool colliding = false; - bool process_collision = false; - -public: - virtual bool setup(real_t p_step) override; - virtual bool pre_solve(real_t p_step) override; - virtual void solve(real_t p_step) override; - - AreaSoftBodyPair3DSW(SoftBody3DSW *p_sof_body, int p_soft_body_shape, Area3DSW *p_area, int p_area_shape); - ~AreaSoftBodyPair3DSW(); -}; - -#endif // AREA_PAIR__SW_H diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp deleted file mode 100644 index 069374d122..0000000000 --- a/servers/physics_3d/body_3d_sw.cpp +++ /dev/null @@ -1,790 +0,0 @@ -/*************************************************************************/ -/* body_3d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "body_3d_sw.h" - -#include "area_3d_sw.h" -#include "body_direct_state_3d_sw.h" -#include "space_3d_sw.h" - -void Body3DSW::_mass_properties_changed() { - if (get_space() && !mass_properties_update_list.in_list() && (calculate_inertia || calculate_center_of_mass)) { - get_space()->body_add_to_mass_properties_update_list(&mass_properties_update_list); - } -} - -void Body3DSW::_update_transform_dependant() { - center_of_mass = get_transform().basis.xform(center_of_mass_local); - principal_inertia_axes = get_transform().basis * principal_inertia_axes_local; - - // Update inertia tensor. - Basis tb = principal_inertia_axes; - Basis tbt = tb.transposed(); - Basis diag; - diag.scale(_inv_inertia); - _inv_inertia_tensor = tb * diag * tbt; -} - -void Body3DSW::update_mass_properties() { - // Update shapes and motions. - - switch (mode) { - case PhysicsServer3D::BODY_MODE_DYNAMIC: { - real_t total_area = 0; - for (int i = 0; i < get_shape_count(); i++) { - if (is_shape_disabled(i)) { - continue; - } - - total_area += get_shape_area(i); - } - - if (calculate_center_of_mass) { - // We have to recompute the center of mass. - center_of_mass_local.zero(); - - if (total_area != 0.0) { - for (int i = 0; i < get_shape_count(); i++) { - if (is_shape_disabled(i)) { - continue; - } - - real_t area = get_shape_area(i); - - real_t mass = area * this->mass / total_area; - - // NOTE: we assume that the shape origin is also its center of mass. - center_of_mass_local += mass * get_shape_transform(i).origin; - } - - center_of_mass_local /= mass; - } - } - - if (calculate_inertia) { - // Recompute the inertia tensor. - Basis inertia_tensor; - inertia_tensor.set_zero(); - bool inertia_set = false; - - for (int i = 0; i < get_shape_count(); i++) { - if (is_shape_disabled(i)) { - continue; - } - - real_t area = get_shape_area(i); - if (area == 0.0) { - continue; - } - - inertia_set = true; - - const Shape3DSW *shape = get_shape(i); - - real_t mass = area * this->mass / total_area; - - Basis shape_inertia_tensor = Basis::from_scale(shape->get_moment_of_inertia(mass)); - Transform3D shape_transform = get_shape_transform(i); - Basis shape_basis = shape_transform.basis.orthonormalized(); - - // NOTE: we don't take the scale of collision shapes into account when computing the inertia tensor! - shape_inertia_tensor = shape_basis * shape_inertia_tensor * shape_basis.transposed(); - - Vector3 shape_origin = shape_transform.origin - center_of_mass_local; - inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass; - } - - // Set the inertia to a valid value when there are no valid shapes. - if (!inertia_set) { - inertia_tensor = Basis(); - } - - // Handle partial custom inertia. - if (inertia.x > 0.0) { - inertia_tensor[0][0] = inertia.x; - } - if (inertia.y > 0.0) { - inertia_tensor[1][1] = inertia.y; - } - if (inertia.z > 0.0) { - inertia_tensor[2][2] = inertia.z; - } - - // Compute the principal axes of inertia. - principal_inertia_axes_local = inertia_tensor.diagonalize().transposed(); - _inv_inertia = inertia_tensor.get_main_diagonal().inverse(); - } - - if (mass) { - _inv_mass = 1.0 / mass; - } else { - _inv_mass = 0; - } - - } break; - case PhysicsServer3D::BODY_MODE_KINEMATIC: - case PhysicsServer3D::BODY_MODE_STATIC: { - _inv_inertia = Vector3(); - _inv_mass = 0; - } break; - case PhysicsServer3D::BODY_MODE_DYNAMIC_LINEAR: { - _inv_inertia_tensor.set_zero(); - _inv_mass = 1.0 / mass; - - } break; - } - - _update_transform_dependant(); -} - -void Body3DSW::reset_mass_properties() { - calculate_inertia = true; - calculate_center_of_mass = true; - _mass_properties_changed(); -} - -void Body3DSW::set_active(bool p_active) { - if (active == p_active) { - return; - } - - active = p_active; - - if (active) { - if (mode == PhysicsServer3D::BODY_MODE_STATIC) { - // Static bodies can't be active. - active = false; - } else if (get_space()) { - get_space()->body_add_to_active_list(&active_list); - } - } else if (get_space()) { - get_space()->body_remove_from_active_list(&active_list); - } -} - -void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, const Variant &p_value) { - switch (p_param) { - case PhysicsServer3D::BODY_PARAM_BOUNCE: { - bounce = p_value; - } break; - case PhysicsServer3D::BODY_PARAM_FRICTION: { - friction = p_value; - } break; - case PhysicsServer3D::BODY_PARAM_MASS: { - real_t mass_value = p_value; - ERR_FAIL_COND(mass_value <= 0); - mass = mass_value; - if (mode >= PhysicsServer3D::BODY_MODE_DYNAMIC) { - _mass_properties_changed(); - } - } break; - case PhysicsServer3D::BODY_PARAM_INERTIA: { - inertia = p_value; - if ((inertia.x <= 0.0) || (inertia.y <= 0.0) || (inertia.z <= 0.0)) { - calculate_inertia = true; - if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC) { - _mass_properties_changed(); - } - } else { - calculate_inertia = false; - if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC) { - principal_inertia_axes_local = Basis(); - _inv_inertia = inertia.inverse(); - _update_transform_dependant(); - } - } - } break; - case PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS: { - calculate_center_of_mass = false; - center_of_mass_local = p_value; - _update_transform_dependant(); - } break; - case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE: { - gravity_scale = p_value; - } break; - case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP: { - linear_damp = p_value; - } break; - case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP: { - angular_damp = p_value; - } break; - default: { - } - } -} - -Variant Body3DSW::get_param(PhysicsServer3D::BodyParameter p_param) const { - switch (p_param) { - case PhysicsServer3D::BODY_PARAM_BOUNCE: { - return bounce; - } break; - case PhysicsServer3D::BODY_PARAM_FRICTION: { - return friction; - } break; - case PhysicsServer3D::BODY_PARAM_MASS: { - return mass; - } break; - case PhysicsServer3D::BODY_PARAM_INERTIA: { - if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC) { - return _inv_inertia.inverse(); - } else { - return Vector3(); - } - } break; - case PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS: { - return center_of_mass; - } break; - case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE: { - return gravity_scale; - } break; - case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP: { - return linear_damp; - } break; - case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP: { - return angular_damp; - } break; - - default: { - } - } - - return 0; -} - -void Body3DSW::set_mode(PhysicsServer3D::BodyMode p_mode) { - PhysicsServer3D::BodyMode prev = mode; - mode = p_mode; - - switch (p_mode) { - case PhysicsServer3D::BODY_MODE_STATIC: - case PhysicsServer3D::BODY_MODE_KINEMATIC: { - _set_inv_transform(get_transform().affine_inverse()); - _inv_mass = 0; - _inv_inertia = Vector3(); - _set_static(p_mode == PhysicsServer3D::BODY_MODE_STATIC); - set_active(p_mode == PhysicsServer3D::BODY_MODE_KINEMATIC && contacts.size()); - linear_velocity = Vector3(); - angular_velocity = Vector3(); - if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC && prev != mode) { - first_time_kinematic = true; - } - _update_transform_dependant(); - - } break; - case PhysicsServer3D::BODY_MODE_DYNAMIC: { - _inv_mass = mass > 0 ? (1.0 / mass) : 0; - if (!calculate_inertia) { - principal_inertia_axes_local = Basis(); - _inv_inertia = inertia.inverse(); - _update_transform_dependant(); - } - _mass_properties_changed(); - _set_static(false); - set_active(true); - - } break; - case PhysicsServer3D::BODY_MODE_DYNAMIC_LINEAR: { - _inv_mass = mass > 0 ? (1.0 / mass) : 0; - _inv_inertia = Vector3(); - angular_velocity = Vector3(); - _update_transform_dependant(); - _set_static(false); - set_active(true); - } - } -} - -PhysicsServer3D::BodyMode Body3DSW::get_mode() const { - return mode; -} - -void Body3DSW::_shapes_changed() { - _mass_properties_changed(); -} - -void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) { - switch (p_state) { - case PhysicsServer3D::BODY_STATE_TRANSFORM: { - if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { - new_transform = p_variant; - //wakeup_neighbours(); - set_active(true); - if (first_time_kinematic) { - _set_transform(p_variant); - _set_inv_transform(get_transform().affine_inverse()); - first_time_kinematic = false; - } - - } else if (mode == PhysicsServer3D::BODY_MODE_STATIC) { - _set_transform(p_variant); - _set_inv_transform(get_transform().affine_inverse()); - wakeup_neighbours(); - } else { - Transform3D t = p_variant; - t.orthonormalize(); - new_transform = get_transform(); //used as old to compute motion - if (new_transform == t) { - break; - } - _set_transform(t); - _set_inv_transform(get_transform().inverse()); - } - wakeup(); - - } break; - case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { - linear_velocity = p_variant; - constant_linear_velocity = linear_velocity; - wakeup(); - } break; - case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { - angular_velocity = p_variant; - constant_angular_velocity = angular_velocity; - wakeup(); - - } break; - case PhysicsServer3D::BODY_STATE_SLEEPING: { - if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { - break; - } - bool do_sleep = p_variant; - if (do_sleep) { - linear_velocity = Vector3(); - //biased_linear_velocity=Vector3(); - angular_velocity = Vector3(); - //biased_angular_velocity=Vector3(); - set_active(false); - } else { - set_active(true); - } - } break; - case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { - can_sleep = p_variant; - if (mode >= PhysicsServer3D::BODY_MODE_DYNAMIC && !active && !can_sleep) { - set_active(true); - } - - } break; - } -} - -Variant Body3DSW::get_state(PhysicsServer3D::BodyState p_state) const { - switch (p_state) { - case PhysicsServer3D::BODY_STATE_TRANSFORM: { - return get_transform(); - } break; - case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { - return linear_velocity; - } break; - case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { - return angular_velocity; - } break; - case PhysicsServer3D::BODY_STATE_SLEEPING: { - return !is_active(); - } break; - case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { - return can_sleep; - } break; - } - - return Variant(); -} - -void Body3DSW::set_space(Space3DSW *p_space) { - if (get_space()) { - if (mass_properties_update_list.in_list()) { - get_space()->body_remove_from_mass_properties_update_list(&mass_properties_update_list); - } - if (active_list.in_list()) { - get_space()->body_remove_from_active_list(&active_list); - } - if (direct_state_query_list.in_list()) { - get_space()->body_remove_from_state_query_list(&direct_state_query_list); - } - } - - _set_space(p_space); - - if (get_space()) { - _mass_properties_changed(); - if (active) { - get_space()->body_add_to_active_list(&active_list); - } - } -} - -void Body3DSW::_compute_area_gravity_and_damping(const Area3DSW *p_area) { - Vector3 area_gravity; - p_area->compute_gravity(get_transform().get_origin(), area_gravity); - gravity += area_gravity; - - area_linear_damp += p_area->get_linear_damp(); - area_angular_damp += p_area->get_angular_damp(); -} - -void Body3DSW::set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool lock) { - if (lock) { - locked_axis |= p_axis; - } else { - locked_axis &= ~p_axis; - } -} - -bool Body3DSW::is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const { - return locked_axis & p_axis; -} - -void Body3DSW::integrate_forces(real_t p_step) { - if (mode == PhysicsServer3D::BODY_MODE_STATIC) { - return; - } - - Area3DSW *def_area = get_space()->get_default_area(); - // AreaSW *damp_area = def_area; - - ERR_FAIL_COND(!def_area); - - int ac = areas.size(); - bool stopped = false; - gravity = Vector3(0, 0, 0); - area_linear_damp = 0; - area_angular_damp = 0; - if (ac) { - areas.sort(); - const AreaCMP *aa = &areas[0]; - // damp_area = aa[ac-1].area; - for (int i = ac - 1; i >= 0 && !stopped; i--) { - PhysicsServer3D::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode(); - switch (mode) { - case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: - case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { - _compute_area_gravity_and_damping(aa[i].area); - stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; - } break; - case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: - case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { - gravity = Vector3(0, 0, 0); - area_angular_damp = 0; - area_linear_damp = 0; - _compute_area_gravity_and_damping(aa[i].area); - stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE; - } break; - default: { - } - } - } - } - - if (!stopped) { - _compute_area_gravity_and_damping(def_area); - } - - gravity *= gravity_scale; - - // If less than 0, override dampenings with that of the Body - if (angular_damp >= 0) { - area_angular_damp = angular_damp; - } - /* - else - area_angular_damp=damp_area->get_angular_damp(); - */ - - if (linear_damp >= 0) { - area_linear_damp = linear_damp; - } - /* - else - area_linear_damp=damp_area->get_linear_damp(); - */ - - Vector3 motion; - bool do_motion = false; - - if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { - //compute motion, angular and etc. velocities from prev transform - motion = new_transform.origin - get_transform().origin; - do_motion = true; - linear_velocity = constant_linear_velocity + motion / p_step; - - //compute a FAKE angular velocity, not so easy - Basis rot = new_transform.basis.orthonormalized() * get_transform().basis.orthonormalized().transposed(); - Vector3 axis; - real_t angle; - - rot.get_axis_angle(axis, angle); - axis.normalize(); - angular_velocity = constant_angular_velocity + axis * (angle / p_step); - } else { - if (!omit_force_integration) { - //overridden by direct state query - - Vector3 force = gravity * mass; - force += applied_force; - Vector3 torque = applied_torque; - - real_t damp = 1.0 - p_step * area_linear_damp; - - if (damp < 0) { // reached zero in the given time - damp = 0; - } - - real_t angular_damp = 1.0 - p_step * area_angular_damp; - - if (angular_damp < 0) { // reached zero in the given time - angular_damp = 0; - } - - linear_velocity *= damp; - angular_velocity *= angular_damp; - - linear_velocity += _inv_mass * force * p_step; - angular_velocity += _inv_inertia_tensor.xform(torque) * p_step; - } - - if (continuous_cd) { - motion = linear_velocity * p_step; - do_motion = true; - } - } - - applied_force = Vector3(); - applied_torque = Vector3(); - - //motion=linear_velocity*p_step; - - biased_angular_velocity = Vector3(); - biased_linear_velocity = Vector3(); - - if (do_motion) { //shapes temporarily extend for raycast - _update_shapes_with_motion(motion); - } - - def_area = nullptr; // clear the area, so it is set in the next frame - contact_count = 0; -} - -void Body3DSW::integrate_velocities(real_t p_step) { - if (mode == PhysicsServer3D::BODY_MODE_STATIC) { - return; - } - - if (fi_callback_data || body_state_callback) { - get_space()->body_add_to_state_query_list(&direct_state_query_list); - } - - //apply axis lock linear - for (int i = 0; i < 3; i++) { - if (is_axis_locked((PhysicsServer3D::BodyAxis)(1 << i))) { - linear_velocity[i] = 0; - biased_linear_velocity[i] = 0; - new_transform.origin[i] = get_transform().origin[i]; - } - } - //apply axis lock angular - for (int i = 0; i < 3; i++) { - if (is_axis_locked((PhysicsServer3D::BodyAxis)(1 << (i + 3)))) { - angular_velocity[i] = 0; - biased_angular_velocity[i] = 0; - } - } - - if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { - _set_transform(new_transform, false); - _set_inv_transform(new_transform.affine_inverse()); - if (contacts.size() == 0 && linear_velocity == Vector3() && angular_velocity == Vector3()) { - set_active(false); //stopped moving, deactivate - } - - return; - } - - Vector3 total_angular_velocity = angular_velocity + biased_angular_velocity; - - real_t ang_vel = total_angular_velocity.length(); - Transform3D transform = get_transform(); - - if (!Math::is_zero_approx(ang_vel)) { - Vector3 ang_vel_axis = total_angular_velocity / ang_vel; - Basis rot(ang_vel_axis, ang_vel * p_step); - Basis identity3(1, 0, 0, 0, 1, 0, 0, 0, 1); - transform.origin += ((identity3 - rot) * transform.basis).xform(center_of_mass_local); - transform.basis = rot * transform.basis; - transform.orthonormalize(); - } - - Vector3 total_linear_velocity = linear_velocity + biased_linear_velocity; - /*for(int i=0;i<3;i++) { - if (axis_lock&(1<body_add_to_state_query_list(&direct_state_query_list); - simulated_motion=true; - _set_transform(p_xform); -} - -*/ - -void Body3DSW::wakeup_neighbours() { - for (const KeyValue &E : constraint_map) { - const Constraint3DSW *c = E.key; - Body3DSW **n = c->get_body_ptr(); - int bc = c->get_body_count(); - - for (int i = 0; i < bc; i++) { - if (i == E.value) { - continue; - } - Body3DSW *b = n[i]; - if (b->mode < PhysicsServer3D::BODY_MODE_DYNAMIC) { - continue; - } - - if (!b->is_active()) { - b->set_active(true); - } - } - } -} - -void Body3DSW::call_queries() { - if (fi_callback_data) { - if (!fi_callback_data->callable.get_object()) { - set_force_integration_callback(Callable()); - } else { - Variant direct_state_variant = get_direct_state(); - const Variant *vp[2] = { &direct_state_variant, &fi_callback_data->udata }; - - Callable::CallError ce; - int argc = (fi_callback_data->udata.get_type() == Variant::NIL) ? 1 : 2; - Variant rv; - fi_callback_data->callable.call(vp, argc, rv, ce); - } - } - - if (body_state_callback_instance) { - (body_state_callback)(body_state_callback_instance, get_direct_state()); - } -} - -bool Body3DSW::sleep_test(real_t p_step) { - if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { - return true; - } else if (!can_sleep) { - return false; - } - - if (Math::abs(angular_velocity.length()) < get_space()->get_body_angular_velocity_sleep_threshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_threshold() * get_space()->get_body_linear_velocity_sleep_threshold()) { - still_time += p_step; - - return still_time > get_space()->get_body_time_to_sleep(); - } else { - still_time = 0; //maybe this should be set to 0 on set_active? - return false; - } -} - -void Body3DSW::set_state_sync_callback(void *p_instance, PhysicsServer3D::BodyStateCallback p_callback) { - body_state_callback_instance = p_instance; - body_state_callback = p_callback; -} - -void Body3DSW::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) { - if (p_callable.get_object()) { - if (!fi_callback_data) { - fi_callback_data = memnew(ForceIntegrationCallbackData); - } - fi_callback_data->callable = p_callable; - fi_callback_data->udata = p_udata; - } else if (fi_callback_data) { - memdelete(fi_callback_data); - fi_callback_data = nullptr; - } -} - -PhysicsDirectBodyState3DSW *Body3DSW::get_direct_state() { - if (!direct_state) { - direct_state = memnew(PhysicsDirectBodyState3DSW); - direct_state->body = this; - } - return direct_state; -} - -Body3DSW::Body3DSW() : - CollisionObject3DSW(TYPE_BODY), - active_list(this), - mass_properties_update_list(this), - direct_state_query_list(this) { - _set_static(false); -} - -Body3DSW::~Body3DSW() { - if (fi_callback_data) { - memdelete(fi_callback_data); - } - if (direct_state) { - memdelete(direct_state); - } -} diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h deleted file mode 100644 index fc47040389..0000000000 --- a/servers/physics_3d/body_3d_sw.h +++ /dev/null @@ -1,368 +0,0 @@ -/*************************************************************************/ -/* body_3d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 BODY_3D_SW_H -#define BODY_3D_SW_H - -#include "area_3d_sw.h" -#include "collision_object_3d_sw.h" -#include "core/templates/vset.h" - -class Constraint3DSW; -class PhysicsDirectBodyState3DSW; - -class Body3DSW : public CollisionObject3DSW { - PhysicsServer3D::BodyMode mode = PhysicsServer3D::BODY_MODE_DYNAMIC; - - Vector3 linear_velocity; - Vector3 angular_velocity; - - Vector3 constant_linear_velocity; - Vector3 constant_angular_velocity; - - Vector3 biased_linear_velocity; - Vector3 biased_angular_velocity; - real_t mass = 1.0; - real_t bounce = 0.0; - real_t friction = 1.0; - Vector3 inertia; - - real_t linear_damp = -1.0; - real_t angular_damp = -1.0; - real_t gravity_scale = 1.0; - - uint16_t locked_axis = 0; - - real_t _inv_mass = 1.0; - Vector3 _inv_inertia; // Relative to the principal axes of inertia - - // Relative to the local frame of reference - Basis principal_inertia_axes_local; - Vector3 center_of_mass_local; - - // In world orientation with local origin - Basis _inv_inertia_tensor; - Basis principal_inertia_axes; - Vector3 center_of_mass; - - bool calculate_inertia = true; - bool calculate_center_of_mass = true; - - Vector3 gravity; - - real_t still_time = 0.0; - - Vector3 applied_force; - Vector3 applied_torque; - - real_t area_angular_damp = 0.0; - real_t area_linear_damp = 0.0; - - SelfList active_list; - SelfList mass_properties_update_list; - SelfList direct_state_query_list; - - VSet exceptions; - bool omit_force_integration = false; - bool active = true; - - bool continuous_cd = false; - bool can_sleep = true; - bool first_time_kinematic = false; - - void _mass_properties_changed(); - virtual void _shapes_changed(); - Transform3D new_transform; - - Map constraint_map; - - Vector areas; - - struct Contact { - Vector3 local_pos; - Vector3 local_normal; - real_t depth = 0.0; - int local_shape = 0; - Vector3 collider_pos; - int collider_shape = 0; - ObjectID collider_instance_id; - RID collider; - Vector3 collider_velocity_at_pos; - }; - - Vector contacts; //no contacts by default - int contact_count = 0; - - void *body_state_callback_instance = nullptr; - PhysicsServer3D::BodyStateCallback body_state_callback = nullptr; - - struct ForceIntegrationCallbackData { - Callable callable; - Variant udata; - }; - - ForceIntegrationCallbackData *fi_callback_data = nullptr; - - PhysicsDirectBodyState3DSW *direct_state = nullptr; - - uint64_t island_step = 0; - - _FORCE_INLINE_ void _compute_area_gravity_and_damping(const Area3DSW *p_area); - - _FORCE_INLINE_ void _update_transform_dependant(); - - friend class PhysicsDirectBodyState3DSW; // i give up, too many functions to expose - -public: - void set_state_sync_callback(void *p_instance, PhysicsServer3D::BodyStateCallback p_callback); - void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant()); - - PhysicsDirectBodyState3DSW *get_direct_state(); - - _FORCE_INLINE_ void add_area(Area3DSW *p_area) { - int index = areas.find(AreaCMP(p_area)); - if (index > -1) { - areas.write[index].refCount += 1; - } else { - areas.ordered_insert(AreaCMP(p_area)); - } - } - - _FORCE_INLINE_ void remove_area(Area3DSW *p_area) { - int index = areas.find(AreaCMP(p_area)); - if (index > -1) { - areas.write[index].refCount -= 1; - if (areas[index].refCount < 1) { - areas.remove(index); - } - } - } - - _FORCE_INLINE_ void set_max_contacts_reported(int p_size) { - contacts.resize(p_size); - contact_count = 0; - if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC && p_size) { - set_active(true); - } - } - _FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); } - - _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.is_empty(); } - _FORCE_INLINE_ void add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos); - - _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); } - _FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); } - _FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); } - _FORCE_INLINE_ const VSet &get_exceptions() const { return exceptions; } - - _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } - _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } - - _FORCE_INLINE_ void add_constraint(Constraint3DSW *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; } - _FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraint_map.erase(p_constraint); } - const Map &get_constraint_map() const { return constraint_map; } - _FORCE_INLINE_ void clear_constraint_map() { constraint_map.clear(); } - - _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; } - _FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; } - - _FORCE_INLINE_ Basis get_principal_inertia_axes() const { return principal_inertia_axes; } - _FORCE_INLINE_ Vector3 get_center_of_mass() const { return center_of_mass; } - _FORCE_INLINE_ Vector3 xform_local_to_principal(const Vector3 &p_pos) const { return principal_inertia_axes_local.xform(p_pos - center_of_mass_local); } - - _FORCE_INLINE_ void set_linear_velocity(const Vector3 &p_velocity) { linear_velocity = p_velocity; } - _FORCE_INLINE_ Vector3 get_linear_velocity() const { return linear_velocity; } - - _FORCE_INLINE_ void set_angular_velocity(const Vector3 &p_velocity) { angular_velocity = p_velocity; } - _FORCE_INLINE_ Vector3 get_angular_velocity() const { return angular_velocity; } - - _FORCE_INLINE_ const Vector3 &get_biased_linear_velocity() const { return biased_linear_velocity; } - _FORCE_INLINE_ const Vector3 &get_biased_angular_velocity() const { return biased_angular_velocity; } - - _FORCE_INLINE_ void apply_central_impulse(const Vector3 &p_impulse) { - linear_velocity += p_impulse * _inv_mass; - } - - _FORCE_INLINE_ void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) { - linear_velocity += p_impulse * _inv_mass; - angular_velocity += _inv_inertia_tensor.xform((p_position - center_of_mass).cross(p_impulse)); - } - - _FORCE_INLINE_ void apply_torque_impulse(const Vector3 &p_impulse) { - angular_velocity += _inv_inertia_tensor.xform(p_impulse); - } - - _FORCE_INLINE_ void apply_bias_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3(), real_t p_max_delta_av = -1.0) { - biased_linear_velocity += p_impulse * _inv_mass; - if (p_max_delta_av != 0.0) { - Vector3 delta_av = _inv_inertia_tensor.xform((p_position - center_of_mass).cross(p_impulse)); - if (p_max_delta_av > 0 && delta_av.length() > p_max_delta_av) { - delta_av = delta_av.normalized() * p_max_delta_av; - } - biased_angular_velocity += delta_av; - } - } - - _FORCE_INLINE_ void apply_bias_torque_impulse(const Vector3 &p_impulse) { - biased_angular_velocity += _inv_inertia_tensor.xform(p_impulse); - } - - _FORCE_INLINE_ void add_central_force(const Vector3 &p_force) { - applied_force += p_force; - } - - _FORCE_INLINE_ void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) { - applied_force += p_force; - applied_torque += (p_position - center_of_mass).cross(p_force); - } - - _FORCE_INLINE_ void add_torque(const Vector3 &p_torque) { - applied_torque += p_torque; - } - - void set_active(bool p_active); - _FORCE_INLINE_ bool is_active() const { return active; } - - _FORCE_INLINE_ void wakeup() { - if ((!get_space()) || mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { - return; - } - set_active(true); - } - - void set_param(PhysicsServer3D::BodyParameter p_param, const Variant &p_value); - Variant get_param(PhysicsServer3D::BodyParameter p_param) const; - - void set_mode(PhysicsServer3D::BodyMode p_mode); - PhysicsServer3D::BodyMode get_mode() const; - - void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant); - Variant get_state(PhysicsServer3D::BodyState p_state) const; - - void set_applied_force(const Vector3 &p_force) { applied_force = p_force; } - Vector3 get_applied_force() const { return applied_force; } - - void set_applied_torque(const Vector3 &p_torque) { applied_torque = p_torque; } - Vector3 get_applied_torque() const { return applied_torque; } - - _FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd = p_enable; } - _FORCE_INLINE_ bool is_continuous_collision_detection_enabled() const { return continuous_cd; } - - void set_space(Space3DSW *p_space); - - void update_mass_properties(); - void reset_mass_properties(); - - _FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; } - _FORCE_INLINE_ const Vector3 &get_inv_inertia() const { return _inv_inertia; } - _FORCE_INLINE_ const Basis &get_inv_inertia_tensor() const { return _inv_inertia_tensor; } - _FORCE_INLINE_ real_t get_friction() const { return friction; } - _FORCE_INLINE_ const Vector3 &get_gravity() const { return gravity; } - _FORCE_INLINE_ real_t get_bounce() const { return bounce; } - - void set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool lock); - bool is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const; - - void integrate_forces(real_t p_step); - void integrate_velocities(real_t p_step); - - _FORCE_INLINE_ Vector3 get_velocity_in_local_point(const Vector3 &rel_pos) const { - return linear_velocity + angular_velocity.cross(rel_pos - center_of_mass); - } - - _FORCE_INLINE_ real_t compute_impulse_denominator(const Vector3 &p_pos, const Vector3 &p_normal) const { - Vector3 r0 = p_pos - get_transform().origin - center_of_mass; - - Vector3 c0 = (r0).cross(p_normal); - - Vector3 vec = (_inv_inertia_tensor.xform_inv(c0)).cross(r0); - - return _inv_mass + p_normal.dot(vec); - } - - _FORCE_INLINE_ real_t compute_angular_impulse_denominator(const Vector3 &p_axis) const { - return p_axis.dot(_inv_inertia_tensor.xform_inv(p_axis)); - } - - //void simulate_motion(const Transform3D& p_xform,real_t p_step); - void call_queries(); - void wakeup_neighbours(); - - bool sleep_test(real_t p_step); - - Body3DSW(); - ~Body3DSW(); -}; - -//add contact inline - -void Body3DSW::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos) { - int c_max = contacts.size(); - - if (c_max == 0) { - return; - } - - Contact *c = contacts.ptrw(); - - int idx = -1; - - if (contact_count < c_max) { - idx = contact_count++; - } else { - real_t least_depth = 1e20; - int least_deep = -1; - for (int i = 0; i < c_max; i++) { - if (i == 0 || c[i].depth < least_depth) { - least_deep = i; - least_depth = c[i].depth; - } - } - - if (least_deep >= 0 && least_depth < p_depth) { - idx = least_deep; - } - if (idx == -1) { - return; //none least deepe than this - } - } - - c[idx].local_pos = p_local_pos; - c[idx].local_normal = p_local_normal; - c[idx].depth = p_depth; - c[idx].local_shape = p_local_shape; - c[idx].collider_pos = p_collider_pos; - c[idx].collider_shape = p_collider_shape; - c[idx].collider_instance_id = p_collider_instance_id; - c[idx].collider = p_collider; - c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos; -} - -#endif // BODY_3D_SW_H diff --git a/servers/physics_3d/body_direct_state_3d_sw.cpp b/servers/physics_3d/body_direct_state_3d_sw.cpp deleted file mode 100644 index d61a6ac8e4..0000000000 --- a/servers/physics_3d/body_direct_state_3d_sw.cpp +++ /dev/null @@ -1,190 +0,0 @@ -/*************************************************************************/ -/* body_direct_state_3d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "body_direct_state_3d_sw.h" - -#include "body_3d_sw.h" -#include "space_3d_sw.h" - -Vector3 PhysicsDirectBodyState3DSW::get_total_gravity() const { - return body->gravity; -} - -real_t PhysicsDirectBodyState3DSW::get_total_angular_damp() const { - return body->area_angular_damp; -} - -real_t PhysicsDirectBodyState3DSW::get_total_linear_damp() const { - return body->area_linear_damp; -} - -Vector3 PhysicsDirectBodyState3DSW::get_center_of_mass() const { - return body->get_center_of_mass(); -} - -Basis PhysicsDirectBodyState3DSW::get_principal_inertia_axes() const { - return body->get_principal_inertia_axes(); -} - -real_t PhysicsDirectBodyState3DSW::get_inverse_mass() const { - return body->get_inv_mass(); -} - -Vector3 PhysicsDirectBodyState3DSW::get_inverse_inertia() const { - return body->get_inv_inertia(); -} - -Basis PhysicsDirectBodyState3DSW::get_inverse_inertia_tensor() const { - return body->get_inv_inertia_tensor(); -} - -void PhysicsDirectBodyState3DSW::set_linear_velocity(const Vector3 &p_velocity) { - body->wakeup(); - body->set_linear_velocity(p_velocity); -} - -Vector3 PhysicsDirectBodyState3DSW::get_linear_velocity() const { - return body->get_linear_velocity(); -} - -void PhysicsDirectBodyState3DSW::set_angular_velocity(const Vector3 &p_velocity) { - body->wakeup(); - body->set_angular_velocity(p_velocity); -} - -Vector3 PhysicsDirectBodyState3DSW::get_angular_velocity() const { - return body->get_angular_velocity(); -} - -void PhysicsDirectBodyState3DSW::set_transform(const Transform3D &p_transform) { - body->set_state(PhysicsServer3D::BODY_STATE_TRANSFORM, p_transform); -} - -Transform3D PhysicsDirectBodyState3DSW::get_transform() const { - return body->get_transform(); -} - -Vector3 PhysicsDirectBodyState3DSW::get_velocity_at_local_position(const Vector3 &p_position) const { - return body->get_velocity_in_local_point(p_position); -} - -void PhysicsDirectBodyState3DSW::add_central_force(const Vector3 &p_force) { - body->wakeup(); - body->add_central_force(p_force); -} - -void PhysicsDirectBodyState3DSW::add_force(const Vector3 &p_force, const Vector3 &p_position) { - body->wakeup(); - body->add_force(p_force, p_position); -} - -void PhysicsDirectBodyState3DSW::add_torque(const Vector3 &p_torque) { - body->wakeup(); - body->add_torque(p_torque); -} - -void PhysicsDirectBodyState3DSW::apply_central_impulse(const Vector3 &p_impulse) { - body->wakeup(); - body->apply_central_impulse(p_impulse); -} - -void PhysicsDirectBodyState3DSW::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) { - body->wakeup(); - body->apply_impulse(p_impulse, p_position); -} - -void PhysicsDirectBodyState3DSW::apply_torque_impulse(const Vector3 &p_impulse) { - body->wakeup(); - body->apply_torque_impulse(p_impulse); -} - -void PhysicsDirectBodyState3DSW::set_sleep_state(bool p_sleep) { - body->set_active(!p_sleep); -} - -bool PhysicsDirectBodyState3DSW::is_sleeping() const { - return !body->is_active(); -} - -int PhysicsDirectBodyState3DSW::get_contact_count() const { - return body->contact_count; -} - -Vector3 PhysicsDirectBodyState3DSW::get_contact_local_position(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); - return body->contacts[p_contact_idx].local_pos; -} - -Vector3 PhysicsDirectBodyState3DSW::get_contact_local_normal(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); - return body->contacts[p_contact_idx].local_normal; -} - -real_t PhysicsDirectBodyState3DSW::get_contact_impulse(int p_contact_idx) const { - return 0.0f; // Only implemented for bullet -} - -int PhysicsDirectBodyState3DSW::get_contact_local_shape(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1); - return body->contacts[p_contact_idx].local_shape; -} - -RID PhysicsDirectBodyState3DSW::get_contact_collider(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID()); - return body->contacts[p_contact_idx].collider; -} - -Vector3 PhysicsDirectBodyState3DSW::get_contact_collider_position(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); - return body->contacts[p_contact_idx].collider_pos; -} - -ObjectID PhysicsDirectBodyState3DSW::get_contact_collider_id(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID()); - return body->contacts[p_contact_idx].collider_instance_id; -} - -int PhysicsDirectBodyState3DSW::get_contact_collider_shape(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0); - return body->contacts[p_contact_idx].collider_shape; -} - -Vector3 PhysicsDirectBodyState3DSW::get_contact_collider_velocity_at_position(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); - return body->contacts[p_contact_idx].collider_velocity_at_pos; -} - -PhysicsDirectSpaceState3D *PhysicsDirectBodyState3DSW::get_space_state() { - return body->get_space()->get_direct_state(); -} - -real_t PhysicsDirectBodyState3DSW::get_step() const { - return body->get_space()->get_last_step(); -} diff --git a/servers/physics_3d/body_direct_state_3d_sw.h b/servers/physics_3d/body_direct_state_3d_sw.h deleted file mode 100644 index 5132376715..0000000000 --- a/servers/physics_3d/body_direct_state_3d_sw.h +++ /dev/null @@ -1,94 +0,0 @@ -/*************************************************************************/ -/* body_direct_state_3d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 BODY_DIRECT_STATE_3D_SW_H -#define BODY_DIRECT_STATE_3D_SW_H - -#include "servers/physics_server_3d.h" - -class Body3DSW; - -class PhysicsDirectBodyState3DSW : public PhysicsDirectBodyState3D { - GDCLASS(PhysicsDirectBodyState3DSW, PhysicsDirectBodyState3D); - -public: - Body3DSW *body = nullptr; - - virtual Vector3 get_total_gravity() const override; - virtual real_t get_total_angular_damp() const override; - virtual real_t get_total_linear_damp() const override; - - virtual Vector3 get_center_of_mass() const override; - virtual Basis get_principal_inertia_axes() const override; - - virtual real_t get_inverse_mass() const override; - virtual Vector3 get_inverse_inertia() const override; - virtual Basis get_inverse_inertia_tensor() const override; - - virtual void set_linear_velocity(const Vector3 &p_velocity) override; - virtual Vector3 get_linear_velocity() const override; - - virtual void set_angular_velocity(const Vector3 &p_velocity) override; - virtual Vector3 get_angular_velocity() const override; - - virtual void set_transform(const Transform3D &p_transform) override; - virtual Transform3D get_transform() const override; - - virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const override; - - virtual void add_central_force(const Vector3 &p_force) override; - virtual void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override; - virtual void add_torque(const Vector3 &p_torque) override; - virtual void apply_central_impulse(const Vector3 &p_impulse) override; - virtual void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) override; - virtual void apply_torque_impulse(const Vector3 &p_impulse) override; - - virtual void set_sleep_state(bool p_sleep) override; - virtual bool is_sleeping() const override; - - virtual int get_contact_count() const override; - - virtual Vector3 get_contact_local_position(int p_contact_idx) const override; - virtual Vector3 get_contact_local_normal(int p_contact_idx) const override; - virtual real_t get_contact_impulse(int p_contact_idx) const override; - virtual int get_contact_local_shape(int p_contact_idx) const override; - - virtual RID get_contact_collider(int p_contact_idx) const override; - virtual Vector3 get_contact_collider_position(int p_contact_idx) const override; - virtual ObjectID get_contact_collider_id(int p_contact_idx) const override; - virtual int get_contact_collider_shape(int p_contact_idx) const override; - virtual Vector3 get_contact_collider_velocity_at_position(int p_contact_idx) const override; - - virtual PhysicsDirectSpaceState3D *get_space_state() override; - - virtual real_t get_step() const override; -}; - -#endif // BODY_DIRECT_STATE_3D_SW_H diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp deleted file mode 100644 index c27a2ecced..0000000000 --- a/servers/physics_3d/body_pair_3d_sw.cpp +++ /dev/null @@ -1,907 +0,0 @@ -/*************************************************************************/ -/* body_pair_3d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "body_pair_3d_sw.h" - -#include "collision_solver_3d_sw.h" -#include "core/os/os.h" -#include "space_3d_sw.h" - -/* -#define NO_ACCUMULATE_IMPULSES -#define NO_SPLIT_IMPULSES - -#define NO_FRICTION -*/ - -#define NO_TANGENTIALS -/* BODY PAIR */ - -//#define ALLOWED_PENETRATION 0.01 -#define RELAXATION_TIMESTEPS 3 -#define MIN_VELOCITY 0.0001 -#define MAX_BIAS_ROTATION (Math_PI / 8) - -void BodyPair3DSW::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { - BodyPair3DSW *pair = (BodyPair3DSW *)p_userdata; - pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B); -} - -void BodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B) { - // check if we already have the contact - - //Vector3 local_A = A->get_inv_transform().xform(p_point_A); - //Vector3 local_B = B->get_inv_transform().xform(p_point_B); - - Vector3 local_A = A->get_inv_transform().basis.xform(p_point_A); - Vector3 local_B = B->get_inv_transform().basis.xform(p_point_B - offset_B); - - int new_index = contact_count; - - ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1)); - - Contact contact; - - contact.acc_normal_impulse = 0; - contact.acc_bias_impulse = 0; - contact.acc_bias_impulse_center_of_mass = 0; - contact.acc_tangent_impulse = Vector3(); - contact.index_A = p_index_A; - contact.index_B = p_index_B; - contact.local_A = local_A; - contact.local_B = local_B; - contact.normal = (p_point_A - p_point_B).normalized(); - contact.mass_normal = 0; // will be computed in setup() - - // attempt to determine if the contact will be reused - real_t contact_recycle_radius = space->get_contact_recycle_radius(); - - for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; - if (c.local_A.distance_squared_to(local_A) < (contact_recycle_radius * contact_recycle_radius) && - c.local_B.distance_squared_to(local_B) < (contact_recycle_radius * contact_recycle_radius)) { - contact.acc_normal_impulse = c.acc_normal_impulse; - contact.acc_bias_impulse = c.acc_bias_impulse; - contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass; - contact.acc_tangent_impulse = c.acc_tangent_impulse; - new_index = i; - break; - } - } - - // figure out if the contact amount must be reduced to fit the new contact - - if (new_index == MAX_CONTACTS) { - // remove the contact with the minimum depth - - int least_deep = -1; - real_t min_depth = 1e10; - - for (int i = 0; i <= contact_count; i++) { - Contact &c = (i == contact_count) ? contact : contacts[i]; - Vector3 global_A = A->get_transform().basis.xform(c.local_A); - Vector3 global_B = B->get_transform().basis.xform(c.local_B) + offset_B; - - Vector3 axis = global_A - global_B; - real_t depth = axis.dot(c.normal); - - if (depth < min_depth) { - min_depth = depth; - least_deep = i; - } - } - - ERR_FAIL_COND(least_deep == -1); - - if (least_deep < contact_count) { //replace the last deep contact by the new one - - contacts[least_deep] = contact; - } - - return; - } - - contacts[new_index] = contact; - - if (new_index == contact_count) { - contact_count++; - } -} - -void BodyPair3DSW::validate_contacts() { - //make sure to erase contacts that are no longer valid - - real_t contact_max_separation = space->get_contact_max_separation(); - for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; - - Vector3 global_A = A->get_transform().basis.xform(c.local_A); - Vector3 global_B = B->get_transform().basis.xform(c.local_B) + offset_B; - Vector3 axis = global_A - global_B; - real_t depth = axis.dot(c.normal); - - if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) { - // contact no longer needed, remove - - if ((i + 1) < contact_count) { - // swap with the last one - SWAP(contacts[i], contacts[contact_count - 1]); - } - - i--; - contact_count--; - } - } -} - -bool BodyPair3DSW::_test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Transform3D &p_xform_A, Body3DSW *p_B, int p_shape_B, const Transform3D &p_xform_B) { - Vector3 motion = p_A->get_linear_velocity() * p_step; - real_t mlen = motion.length(); - if (mlen < CMP_EPSILON) { - return false; - } - - Vector3 mnormal = motion / mlen; - - real_t min, max; - p_A->get_shape(p_shape_A)->project_range(mnormal, p_xform_A, min, max); - bool fast_object = mlen > (max - min) * 0.3; //going too fast in that direction - - if (!fast_object) { //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis - return false; - } - - //cast a segment from support in motion normal, in the same direction of motion by motion length - //support is the worst case collision point, so real collision happened before - Vector3 s = p_A->get_shape(p_shape_A)->get_support(p_xform_A.basis.xform(mnormal).normalized()); - Vector3 from = p_xform_A.xform(s); - Vector3 to = from + motion; - - Transform3D from_inv = p_xform_B.affine_inverse(); - - Vector3 local_from = from_inv.xform(from - mnormal * mlen * 0.1); //start from a little inside the bounding box - Vector3 local_to = from_inv.xform(to); - - Vector3 rpos, rnorm; - if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) { - return false; - } - - //shorten the linear velocity so it does not hit, but gets close enough, next frame will hit softly or soft enough - Vector3 hitpos = p_xform_B.xform(rpos); - - real_t newlen = hitpos.distance_to(from) - (max - min) * 0.01; - p_A->set_linear_velocity((mnormal * newlen) / p_step); - - return true; -} - -real_t combine_bounce(Body3DSW *A, Body3DSW *B) { - return CLAMP(A->get_bounce() + B->get_bounce(), 0, 1); -} - -real_t combine_friction(Body3DSW *A, Body3DSW *B) { - return ABS(MIN(A->get_friction(), B->get_friction())); -} - -bool BodyPair3DSW::setup(real_t p_step) { - if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) { - collided = false; - return false; - } - - collide_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) && A->collides_with(B); - collide_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) && B->collides_with(A); - - report_contacts_only = false; - if (!collide_A && !collide_B) { - if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) { - report_contacts_only = true; - } else { - collided = false; - return false; - } - } - - offset_B = B->get_transform().get_origin() - A->get_transform().get_origin(); - - validate_contacts(); - - const Vector3 &offset_A = A->get_transform().get_origin(); - Transform3D xform_Au = Transform3D(A->get_transform().basis, Vector3()); - Transform3D xform_A = xform_Au * A->get_shape_transform(shape_A); - - Transform3D xform_Bu = B->get_transform(); - xform_Bu.origin -= offset_A; - Transform3D xform_B = xform_Bu * B->get_shape_transform(shape_B); - - Shape3DSW *shape_A_ptr = A->get_shape(shape_A); - Shape3DSW *shape_B_ptr = B->get_shape(shape_B); - - collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); - - if (!collided) { - //test ccd (currently just a raycast) - - if (A->is_continuous_collision_detection_enabled() && collide_A) { - _test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B); - } - - if (B->is_continuous_collision_detection_enabled() && collide_B) { - _test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A); - } - - return false; - } - - return true; -} - -bool BodyPair3DSW::pre_solve(real_t p_step) { - if (!collided) { - return false; - } - - real_t max_penetration = space->get_contact_max_allowed_penetration(); - - real_t bias = (real_t)0.3; - - Shape3DSW *shape_A_ptr = A->get_shape(shape_A); - Shape3DSW *shape_B_ptr = B->get_shape(shape_B); - - if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) { - if (shape_A_ptr->get_custom_bias() == 0) { - bias = shape_B_ptr->get_custom_bias(); - } else if (shape_B_ptr->get_custom_bias() == 0) { - bias = shape_A_ptr->get_custom_bias(); - } else { - bias = (shape_B_ptr->get_custom_bias() + shape_A_ptr->get_custom_bias()) * 0.5; - } - } - - real_t inv_dt = 1.0 / p_step; - - bool do_process = false; - - const Basis &basis_A = A->get_transform().basis; - const Basis &basis_B = B->get_transform().basis; - - Basis zero_basis; - zero_basis.set_zero(); - - const Basis &inv_inertia_tensor_A = collide_A ? A->get_inv_inertia_tensor() : zero_basis; - const Basis &inv_inertia_tensor_B = collide_B ? B->get_inv_inertia_tensor() : zero_basis; - - real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0; - real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0; - - for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; - c.active = false; - - Vector3 global_A = basis_A.xform(c.local_A); - Vector3 global_B = basis_B.xform(c.local_B) + offset_B; - - Vector3 axis = global_A - global_B; - real_t depth = axis.dot(c.normal); - - if (depth <= 0.0) { - continue; - } - -#ifdef DEBUG_ENABLED - if (space->is_debugging_contacts()) { - const Vector3 &offset_A = A->get_transform().get_origin(); - space->add_debug_contact(global_A + offset_A); - space->add_debug_contact(global_B + offset_A); - } -#endif - - c.rA = global_A - A->get_center_of_mass(); - c.rB = global_B - B->get_center_of_mass() - offset_B; - - // contact query reporting... - - if (A->can_report_contacts()) { - Vector3 crA = A->get_angular_velocity().cross(c.rA) + A->get_linear_velocity(); - A->add_contact(global_A, -c.normal, depth, shape_A, global_B, shape_B, B->get_instance_id(), B->get_self(), crA); - } - - if (B->can_report_contacts()) { - Vector3 crB = B->get_angular_velocity().cross(c.rB) + B->get_linear_velocity(); - B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crB); - } - - if (report_contacts_only) { - collided = false; - continue; - } - - c.active = true; - do_process = true; - - // Precompute normal mass, tangent mass, and bias. - Vector3 inertia_A = inv_inertia_tensor_A.xform(c.rA.cross(c.normal)); - Vector3 inertia_B = inv_inertia_tensor_B.xform(c.rB.cross(c.normal)); - real_t kNormal = inv_mass_A + inv_mass_B; - kNormal += c.normal.dot(inertia_A.cross(c.rA)) + c.normal.dot(inertia_B.cross(c.rB)); - c.mass_normal = 1.0f / kNormal; - - c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration); - c.depth = depth; - - Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse; - if (collide_A) { - A->apply_impulse(-j_vec, c.rA + A->get_center_of_mass()); - } - if (collide_B) { - B->apply_impulse(j_vec, c.rB + B->get_center_of_mass()); - } - c.acc_bias_impulse = 0; - c.acc_bias_impulse_center_of_mass = 0; - - c.bounce = combine_bounce(A, B); - if (c.bounce) { - Vector3 crA = A->get_angular_velocity().cross(c.rA); - Vector3 crB = B->get_angular_velocity().cross(c.rB); - Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; - //normal impule - c.bounce = c.bounce * dv.dot(c.normal); - } - } - - return do_process; -} - -void BodyPair3DSW::solve(real_t p_step) { - if (!collided) { - return; - } - - const real_t max_bias_av = MAX_BIAS_ROTATION / p_step; - - Basis zero_basis; - zero_basis.set_zero(); - - const Basis &inv_inertia_tensor_A = collide_A ? A->get_inv_inertia_tensor() : zero_basis; - const Basis &inv_inertia_tensor_B = collide_B ? B->get_inv_inertia_tensor() : zero_basis; - - real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0; - real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0; - - for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; - if (!c.active) { - continue; - } - - c.active = false; //try to deactivate, will activate itself if still needed - - //bias impulse - - Vector3 crbA = A->get_biased_angular_velocity().cross(c.rA); - Vector3 crbB = B->get_biased_angular_velocity().cross(c.rB); - Vector3 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA; - - real_t vbn = dbv.dot(c.normal); - - if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { - real_t jbn = (-vbn + c.bias) * c.mass_normal; - real_t jbnOld = c.acc_bias_impulse; - c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f); - - Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); - - if (collide_A) { - A->apply_bias_impulse(-jb, c.rA + A->get_center_of_mass(), max_bias_av); - } - if (collide_B) { - B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), max_bias_av); - } - - crbA = A->get_biased_angular_velocity().cross(c.rA); - crbB = B->get_biased_angular_velocity().cross(c.rB); - dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA; - - vbn = dbv.dot(c.normal); - - if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { - real_t jbn_com = (-vbn + c.bias) / (inv_mass_A + inv_mass_B); - real_t jbnOld_com = c.acc_bias_impulse_center_of_mass; - c.acc_bias_impulse_center_of_mass = MAX(jbnOld_com + jbn_com, 0.0f); - - Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com); - - if (collide_A) { - A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f); - } - if (collide_B) { - B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f); - } - } - - c.active = true; - } - - Vector3 crA = A->get_angular_velocity().cross(c.rA); - Vector3 crB = B->get_angular_velocity().cross(c.rB); - Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; - - //normal impulse - real_t vn = dv.dot(c.normal); - - if (Math::abs(vn) > MIN_VELOCITY) { - real_t jn = -(c.bounce + vn) * c.mass_normal; - real_t jnOld = c.acc_normal_impulse; - c.acc_normal_impulse = MAX(jnOld + jn, 0.0f); - - Vector3 j = c.normal * (c.acc_normal_impulse - jnOld); - - if (collide_A) { - A->apply_impulse(-j, c.rA + A->get_center_of_mass()); - } - if (collide_B) { - B->apply_impulse(j, c.rB + B->get_center_of_mass()); - } - - c.active = true; - } - - //friction impulse - - real_t friction = combine_friction(A, B); - - Vector3 lvA = A->get_linear_velocity() + A->get_angular_velocity().cross(c.rA); - Vector3 lvB = B->get_linear_velocity() + B->get_angular_velocity().cross(c.rB); - - Vector3 dtv = lvB - lvA; - real_t tn = c.normal.dot(dtv); - - // tangential velocity - Vector3 tv = dtv - c.normal * tn; - real_t tvl = tv.length(); - - if (tvl > MIN_VELOCITY) { - tv /= tvl; - - Vector3 temp1 = inv_inertia_tensor_A.xform(c.rA.cross(tv)); - Vector3 temp2 = inv_inertia_tensor_B.xform(c.rB.cross(tv)); - - real_t t = -tvl / - (inv_mass_A + inv_mass_B + tv.dot(temp1.cross(c.rA) + temp2.cross(c.rB))); - - Vector3 jt = t * tv; - - Vector3 jtOld = c.acc_tangent_impulse; - c.acc_tangent_impulse += jt; - - real_t fi_len = c.acc_tangent_impulse.length(); - real_t jtMax = c.acc_normal_impulse * friction; - - if (fi_len > CMP_EPSILON && fi_len > jtMax) { - c.acc_tangent_impulse *= jtMax / fi_len; - } - - jt = c.acc_tangent_impulse - jtOld; - - if (collide_A) { - A->apply_impulse(-jt, c.rA + A->get_center_of_mass()); - } - if (collide_B) { - B->apply_impulse(jt, c.rB + B->get_center_of_mass()); - } - - c.active = true; - } - } -} - -BodyPair3DSW::BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_shape_B) : - BodyContact3DSW(_arr, 2) { - A = p_A; - B = p_B; - shape_A = p_shape_A; - shape_B = p_shape_B; - space = A->get_space(); - A->add_constraint(this, 0); - B->add_constraint(this, 1); -} - -BodyPair3DSW::~BodyPair3DSW() { - A->remove_constraint(this); - B->remove_constraint(this); -} - -void BodySoftBodyPair3DSW::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { - BodySoftBodyPair3DSW *pair = (BodySoftBodyPair3DSW *)p_userdata; - pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B); -} - -void BodySoftBodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B) { - Vector3 local_A = body->get_inv_transform().xform(p_point_A); - Vector3 local_B = p_point_B - soft_body->get_node_position(p_index_B); - - Contact contact; - contact.index_A = p_index_A; - contact.index_B = p_index_B; - contact.acc_normal_impulse = 0; - contact.acc_bias_impulse = 0; - contact.acc_bias_impulse_center_of_mass = 0; - contact.acc_tangent_impulse = Vector3(); - contact.local_A = local_A; - contact.local_B = local_B; - contact.normal = (p_point_A - p_point_B).normalized(); - contact.mass_normal = 0; - - // Attempt to determine if the contact will be reused. - real_t contact_recycle_radius = space->get_contact_recycle_radius(); - - uint32_t contact_count = contacts.size(); - for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { - Contact &c = contacts[contact_index]; - if (c.index_B == p_index_B) { - if (c.local_A.distance_squared_to(local_A) < (contact_recycle_radius * contact_recycle_radius) && - c.local_B.distance_squared_to(local_B) < (contact_recycle_radius * contact_recycle_radius)) { - contact.acc_normal_impulse = c.acc_normal_impulse; - contact.acc_bias_impulse = c.acc_bias_impulse; - contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass; - contact.acc_tangent_impulse = c.acc_tangent_impulse; - } - c = contact; - return; - } - } - - contacts.push_back(contact); -} - -void BodySoftBodyPair3DSW::validate_contacts() { - // Make sure to erase contacts that are no longer valid. - const Transform3D &transform_A = body->get_transform(); - - real_t contact_max_separation = space->get_contact_max_separation(); - - uint32_t contact_count = contacts.size(); - for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { - Contact &c = contacts[contact_index]; - - Vector3 global_A = transform_A.xform(c.local_A); - Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B; - Vector3 axis = global_A - global_B; - real_t depth = axis.dot(c.normal); - - if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) { - // Contact no longer needed, remove. - if ((contact_index + 1) < contact_count) { - // Swap with the last one. - SWAP(c, contacts[contact_count - 1]); - } - - contact_index--; - contact_count--; - } - } - - contacts.resize(contact_count); -} - -bool BodySoftBodyPair3DSW::setup(real_t p_step) { - if (!body->interacts_with(soft_body) || body->has_exception(soft_body->get_self()) || soft_body->has_exception(body->get_self())) { - collided = false; - return false; - } - - body_collides = (body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) && body->collides_with(soft_body); - soft_body_collides = soft_body->collides_with(body); - - if (!body_collides && !soft_body_collides) { - if (body->get_max_contacts_reported() > 0) { - report_contacts_only = true; - } else { - collided = false; - return false; - } - } - - const Transform3D &xform_Au = body->get_transform(); - Transform3D xform_A = xform_Au * body->get_shape_transform(body_shape); - - Transform3D xform_Bu = soft_body->get_transform(); - Transform3D xform_B = xform_Bu * soft_body->get_shape_transform(0); - - validate_contacts(); - - Shape3DSW *shape_A_ptr = body->get_shape(body_shape); - Shape3DSW *shape_B_ptr = soft_body->get_shape(0); - - collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); - - return collided; -} - -bool BodySoftBodyPair3DSW::pre_solve(real_t p_step) { - if (!collided) { - return false; - } - - real_t max_penetration = space->get_contact_max_allowed_penetration(); - - real_t bias = (real_t)0.3; - - Shape3DSW *shape_A_ptr = body->get_shape(body_shape); - - if (shape_A_ptr->get_custom_bias()) { - bias = shape_A_ptr->get_custom_bias(); - } - - real_t inv_dt = 1.0 / p_step; - - bool do_process = false; - - const Transform3D &transform_A = body->get_transform(); - - Basis zero_basis; - zero_basis.set_zero(); - - const Basis &body_inv_inertia_tensor = body_collides ? body->get_inv_inertia_tensor() : zero_basis; - - real_t body_inv_mass = body_collides ? body->get_inv_mass() : 0.0; - - uint32_t contact_count = contacts.size(); - for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { - Contact &c = contacts[contact_index]; - c.active = false; - - real_t node_inv_mass = soft_body_collides ? soft_body->get_node_inv_mass(c.index_B) : 0.0; - if ((node_inv_mass == 0.0) && (body_inv_mass == 0.0)) { - continue; - } - - Vector3 global_A = transform_A.xform(c.local_A); - Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B; - Vector3 axis = global_A - global_B; - real_t depth = axis.dot(c.normal); - - if (depth <= 0.0) { - continue; - } - -#ifdef DEBUG_ENABLED - if (space->is_debugging_contacts()) { - space->add_debug_contact(global_A); - space->add_debug_contact(global_B); - } -#endif - - c.rA = global_A - transform_A.origin - body->get_center_of_mass(); - c.rB = global_B; - - if (body->can_report_contacts()) { - Vector3 crA = body->get_angular_velocity().cross(c.rA) + body->get_linear_velocity(); - body->add_contact(global_A, -c.normal, depth, body_shape, global_B, 0, soft_body->get_instance_id(), soft_body->get_self(), crA); - } - - if (report_contacts_only) { - collided = false; - continue; - } - - c.active = true; - do_process = true; - - if (body_collides) { - body->set_active(true); - } - - // Precompute normal mass, tangent mass, and bias. - Vector3 inertia_A = body_inv_inertia_tensor.xform(c.rA.cross(c.normal)); - real_t kNormal = body_inv_mass + node_inv_mass; - kNormal += c.normal.dot(inertia_A.cross(c.rA)); - c.mass_normal = 1.0f / kNormal; - - c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration); - c.depth = depth; - - Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse; - if (body_collides) { - body->apply_impulse(-j_vec, c.rA + body->get_center_of_mass()); - } - if (soft_body_collides) { - soft_body->apply_node_impulse(c.index_B, j_vec); - } - c.acc_bias_impulse = 0; - c.acc_bias_impulse_center_of_mass = 0; - - c.bounce = body->get_bounce(); - - if (c.bounce) { - Vector3 crA = body->get_angular_velocity().cross(c.rA); - Vector3 dv = soft_body->get_node_velocity(c.index_B) - body->get_linear_velocity() - crA; - - // Normal impulse. - c.bounce = c.bounce * dv.dot(c.normal); - } - } - - return do_process; -} - -void BodySoftBodyPair3DSW::solve(real_t p_step) { - if (!collided) { - return; - } - - const real_t max_bias_av = MAX_BIAS_ROTATION / p_step; - - Basis zero_basis; - zero_basis.set_zero(); - - const Basis &body_inv_inertia_tensor = body_collides ? body->get_inv_inertia_tensor() : zero_basis; - - real_t body_inv_mass = body_collides ? body->get_inv_mass() : 0.0; - - uint32_t contact_count = contacts.size(); - for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { - Contact &c = contacts[contact_index]; - if (!c.active) { - continue; - } - - c.active = false; - - real_t node_inv_mass = soft_body_collides ? soft_body->get_node_inv_mass(c.index_B) : 0.0; - - // Bias impulse. - Vector3 crbA = body->get_biased_angular_velocity().cross(c.rA); - Vector3 dbv = soft_body->get_node_biased_velocity(c.index_B) - body->get_biased_linear_velocity() - crbA; - - real_t vbn = dbv.dot(c.normal); - - if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { - real_t jbn = (-vbn + c.bias) * c.mass_normal; - real_t jbnOld = c.acc_bias_impulse; - c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f); - - Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); - - if (body_collides) { - body->apply_bias_impulse(-jb, c.rA + body->get_center_of_mass(), max_bias_av); - } - if (soft_body_collides) { - soft_body->apply_node_bias_impulse(c.index_B, jb); - } - - crbA = body->get_biased_angular_velocity().cross(c.rA); - dbv = soft_body->get_node_biased_velocity(c.index_B) - body->get_biased_linear_velocity() - crbA; - - vbn = dbv.dot(c.normal); - - if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { - real_t jbn_com = (-vbn + c.bias) / (body_inv_mass + node_inv_mass); - real_t jbnOld_com = c.acc_bias_impulse_center_of_mass; - c.acc_bias_impulse_center_of_mass = MAX(jbnOld_com + jbn_com, 0.0f); - - Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com); - - if (body_collides) { - body->apply_bias_impulse(-jb_com, body->get_center_of_mass(), 0.0f); - } - if (soft_body_collides) { - soft_body->apply_node_bias_impulse(c.index_B, jb_com); - } - } - - c.active = true; - } - - Vector3 crA = body->get_angular_velocity().cross(c.rA); - Vector3 dv = soft_body->get_node_velocity(c.index_B) - body->get_linear_velocity() - crA; - - // Normal impulse. - real_t vn = dv.dot(c.normal); - - if (Math::abs(vn) > MIN_VELOCITY) { - real_t jn = -(c.bounce + vn) * c.mass_normal; - real_t jnOld = c.acc_normal_impulse; - c.acc_normal_impulse = MAX(jnOld + jn, 0.0f); - - Vector3 j = c.normal * (c.acc_normal_impulse - jnOld); - - if (body_collides) { - body->apply_impulse(-j, c.rA + body->get_center_of_mass()); - } - if (soft_body_collides) { - soft_body->apply_node_impulse(c.index_B, j); - } - - c.active = true; - } - - // Friction impulse. - real_t friction = body->get_friction(); - - Vector3 lvA = body->get_linear_velocity() + body->get_angular_velocity().cross(c.rA); - Vector3 lvB = soft_body->get_node_velocity(c.index_B); - Vector3 dtv = lvB - lvA; - - real_t tn = c.normal.dot(dtv); - - // Tangential velocity. - Vector3 tv = dtv - c.normal * tn; - real_t tvl = tv.length(); - - if (tvl > MIN_VELOCITY) { - tv /= tvl; - - Vector3 temp1 = body_inv_inertia_tensor.xform(c.rA.cross(tv)); - - real_t t = -tvl / - (body_inv_mass + node_inv_mass + tv.dot(temp1.cross(c.rA))); - - Vector3 jt = t * tv; - - Vector3 jtOld = c.acc_tangent_impulse; - c.acc_tangent_impulse += jt; - - real_t fi_len = c.acc_tangent_impulse.length(); - real_t jtMax = c.acc_normal_impulse * friction; - - if (fi_len > CMP_EPSILON && fi_len > jtMax) { - c.acc_tangent_impulse *= jtMax / fi_len; - } - - jt = c.acc_tangent_impulse - jtOld; - - if (body_collides) { - body->apply_impulse(-jt, c.rA + body->get_center_of_mass()); - } - if (soft_body_collides) { - soft_body->apply_node_impulse(c.index_B, jt); - } - - c.active = true; - } - } -} - -BodySoftBodyPair3DSW::BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B) : - BodyContact3DSW(&body, 1) { - body = p_A; - soft_body = p_B; - body_shape = p_shape_A; - space = p_A->get_space(); - body->add_constraint(this, 0); - soft_body->add_constraint(this); -} - -BodySoftBodyPair3DSW::~BodySoftBodyPair3DSW() { - body->remove_constraint(this); - soft_body->remove_constraint(this); -} diff --git a/servers/physics_3d/body_pair_3d_sw.h b/servers/physics_3d/body_pair_3d_sw.h deleted file mode 100644 index 01afb07e13..0000000000 --- a/servers/physics_3d/body_pair_3d_sw.h +++ /dev/null @@ -1,143 +0,0 @@ -/*************************************************************************/ -/* body_pair_3d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 BODY_PAIR_3D_SW_H -#define BODY_PAIR_3D_SW_H - -#include "body_3d_sw.h" -#include "constraint_3d_sw.h" -#include "core/templates/local_vector.h" -#include "soft_body_3d_sw.h" - -class BodyContact3DSW : public Constraint3DSW { -protected: - struct Contact { - Vector3 position; - Vector3 normal; - int index_A = 0, index_B = 0; - Vector3 local_A, local_B; - real_t acc_normal_impulse = 0.0; // accumulated normal impulse (Pn) - Vector3 acc_tangent_impulse; // accumulated tangent impulse (Pt) - real_t acc_bias_impulse = 0.0; // accumulated normal impulse for position bias (Pnb) - real_t acc_bias_impulse_center_of_mass = 0.0; // accumulated normal impulse for position bias applied to com - real_t mass_normal = 0.0; - real_t bias = 0.0; - real_t bounce = 0.0; - - real_t depth = 0.0; - bool active = false; - Vector3 rA, rB; // Offset in world orientation with respect to center of mass - }; - - Vector3 sep_axis; - bool collided = false; - - Space3DSW *space = nullptr; - - BodyContact3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) : - Constraint3DSW(p_body_ptr, p_body_count) { - } -}; - -class BodyPair3DSW : public BodyContact3DSW { - enum { - MAX_CONTACTS = 4 - }; - - union { - struct { - Body3DSW *A; - Body3DSW *B; - }; - - Body3DSW *_arr[2] = { nullptr, nullptr }; - }; - - int shape_A = 0; - int shape_B = 0; - - bool collide_A = false; - bool collide_B = false; - - bool report_contacts_only = false; - - Vector3 offset_B; //use local A coordinates to avoid numerical issues on collision detection - - Contact contacts[MAX_CONTACTS]; - int contact_count = 0; - - static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); - - void contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B); - - void validate_contacts(); - bool _test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Transform3D &p_xform_A, Body3DSW *p_B, int p_shape_B, const Transform3D &p_xform_B); - -public: - virtual bool setup(real_t p_step) override; - virtual bool pre_solve(real_t p_step) override; - virtual void solve(real_t p_step) override; - - BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_shape_B); - ~BodyPair3DSW(); -}; - -class BodySoftBodyPair3DSW : public BodyContact3DSW { - Body3DSW *body = nullptr; - SoftBody3DSW *soft_body = nullptr; - - int body_shape = 0; - - bool body_collides = false; - bool soft_body_collides = false; - - bool report_contacts_only = false; - - LocalVector contacts; - - static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); - - void contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B); - - void validate_contacts(); - -public: - virtual bool setup(real_t p_step) override; - virtual bool pre_solve(real_t p_step) override; - virtual void solve(real_t p_step) override; - - virtual SoftBody3DSW *get_soft_body_ptr(int p_index) const override { return soft_body; } - virtual int get_soft_body_count() const override { return 1; } - - BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B); - ~BodySoftBodyPair3DSW(); -}; - -#endif // BODY_PAIR_3D_SW_H diff --git a/servers/physics_3d/broad_phase_3d_bvh.cpp b/servers/physics_3d/broad_phase_3d_bvh.cpp deleted file mode 100644 index d89e0e1f6d..0000000000 --- a/servers/physics_3d/broad_phase_3d_bvh.cpp +++ /dev/null @@ -1,117 +0,0 @@ -/*************************************************************************/ -/* broad_phase_3d_bvh.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "broad_phase_3d_bvh.h" -#include "collision_object_3d_sw.h" - -BroadPhase3DBVH::ID BroadPhase3DBVH::create(CollisionObject3DSW *p_object, int p_subindex, const AABB &p_aabb, bool p_static) { - ID oid = bvh.create(p_object, true, p_aabb, p_subindex, !p_static, 1 << p_object->get_type(), p_static ? 0 : 0xFFFFF); // Pair everything, don't care? - return oid + 1; -} - -void BroadPhase3DBVH::move(ID p_id, const AABB &p_aabb) { - bvh.move(p_id - 1, p_aabb); -} - -void BroadPhase3DBVH::set_static(ID p_id, bool p_static) { - CollisionObject3DSW *it = bvh.get(p_id - 1); - bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF, false); // Pair everything, don't care? -} - -void BroadPhase3DBVH::remove(ID p_id) { - bvh.erase(p_id - 1); -} - -CollisionObject3DSW *BroadPhase3DBVH::get_object(ID p_id) const { - CollisionObject3DSW *it = bvh.get(p_id - 1); - ERR_FAIL_COND_V(!it, nullptr); - return it; -} - -bool BroadPhase3DBVH::is_static(ID p_id) const { - return !bvh.is_pairable(p_id - 1); -} - -int BroadPhase3DBVH::get_subindex(ID p_id) const { - return bvh.get_subindex(p_id - 1); -} - -int BroadPhase3DBVH::cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) { - return bvh.cull_point(p_point, p_results, p_max_results, p_result_indices); -} - -int BroadPhase3DBVH::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) { - return bvh.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices); -} - -int BroadPhase3DBVH::cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) { - return bvh.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices); -} - -void *BroadPhase3DBVH::_pair_callback(void *self, uint32_t p_A, CollisionObject3DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject3DSW *p_object_B, int subindex_B) { - BroadPhase3DBVH *bpo = (BroadPhase3DBVH *)(self); - if (!bpo->pair_callback) { - return nullptr; - } - - return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata); -} - -void BroadPhase3DBVH::_unpair_callback(void *self, uint32_t p_A, CollisionObject3DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject3DSW *p_object_B, int subindex_B, void *pairdata) { - BroadPhase3DBVH *bpo = (BroadPhase3DBVH *)(self); - if (!bpo->unpair_callback) { - return; - } - - bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata); -} - -void BroadPhase3DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { - pair_callback = p_pair_callback; - pair_userdata = p_userdata; -} - -void BroadPhase3DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { - unpair_callback = p_unpair_callback; - unpair_userdata = p_userdata; -} - -void BroadPhase3DBVH::update() { - bvh.update(); -} - -BroadPhase3DSW *BroadPhase3DBVH::_create() { - return memnew(BroadPhase3DBVH); -} - -BroadPhase3DBVH::BroadPhase3DBVH() { - bvh.set_pair_callback(_pair_callback, this); - bvh.set_unpair_callback(_unpair_callback, this); -} diff --git a/servers/physics_3d/broad_phase_3d_bvh.h b/servers/physics_3d/broad_phase_3d_bvh.h deleted file mode 100644 index 03131c9db2..0000000000 --- a/servers/physics_3d/broad_phase_3d_bvh.h +++ /dev/null @@ -1,72 +0,0 @@ -/*************************************************************************/ -/* broad_phase_3d_bvh.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 BROAD_PHASE_3D_BVH_H -#define BROAD_PHASE_3D_BVH_H - -#include "broad_phase_3d_sw.h" -#include "core/math/bvh.h" - -class BroadPhase3DBVH : public BroadPhase3DSW { - BVH_Manager bvh; - - static void *_pair_callback(void *, uint32_t, CollisionObject3DSW *, int, uint32_t, CollisionObject3DSW *, int); - static void _unpair_callback(void *, uint32_t, CollisionObject3DSW *, int, uint32_t, CollisionObject3DSW *, int, void *); - - PairCallback pair_callback = nullptr; - void *pair_userdata = nullptr; - UnpairCallback unpair_callback = nullptr; - void *unpair_userdata = nullptr; - -public: - // 0 is an invalid ID - virtual ID create(CollisionObject3DSW *p_object, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false); - virtual void move(ID p_id, const AABB &p_aabb); - virtual void set_static(ID p_id, bool p_static); - virtual void remove(ID p_id); - - virtual CollisionObject3DSW *get_object(ID p_id) const; - virtual bool is_static(ID p_id) const; - virtual int get_subindex(ID p_id) const; - - virtual int cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr); - virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr); - virtual int cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr); - - virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata); - virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata); - - virtual void update(); - - static BroadPhase3DSW *_create(); - BroadPhase3DBVH(); -}; - -#endif // BROAD_PHASE_3D_BVH_H diff --git a/servers/physics_3d/broad_phase_3d_sw.cpp b/servers/physics_3d/broad_phase_3d_sw.cpp deleted file mode 100644 index 8aa64034ec..0000000000 --- a/servers/physics_3d/broad_phase_3d_sw.cpp +++ /dev/null @@ -1,36 +0,0 @@ -/*************************************************************************/ -/* broad_phase_3d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "broad_phase_3d_sw.h" - -BroadPhase3DSW::CreateFunction BroadPhase3DSW::create_func = nullptr; - -BroadPhase3DSW::~BroadPhase3DSW() { -} diff --git a/servers/physics_3d/broad_phase_3d_sw.h b/servers/physics_3d/broad_phase_3d_sw.h deleted file mode 100644 index 98313cb216..0000000000 --- a/servers/physics_3d/broad_phase_3d_sw.h +++ /dev/null @@ -1,72 +0,0 @@ -/*************************************************************************/ -/* broad_phase_3d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 BROAD_PHASE_SW_H -#define BROAD_PHASE_SW_H - -#include "core/math/aabb.h" -#include "core/math/math_funcs.h" - -class CollisionObject3DSW; - -class BroadPhase3DSW { -public: - typedef BroadPhase3DSW *(*CreateFunction)(); - - static CreateFunction create_func; - - typedef uint32_t ID; - - typedef void *(*PairCallback)(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_userdata); - typedef void (*UnpairCallback)(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_data, void *p_userdata); - - // 0 is an invalid ID - virtual ID create(CollisionObject3DSW *p_object_, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false) = 0; - virtual void move(ID p_id, const AABB &p_aabb) = 0; - virtual void set_static(ID p_id, bool p_static) = 0; - virtual void remove(ID p_id) = 0; - - virtual CollisionObject3DSW *get_object(ID p_id) const = 0; - virtual bool is_static(ID p_id) const = 0; - virtual int get_subindex(ID p_id) const = 0; - - virtual int cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; - virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; - virtual int cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; - - virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata) = 0; - virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) = 0; - - virtual void update() = 0; - - virtual ~BroadPhase3DSW(); -}; - -#endif // BROAD_PHASE__SW_H diff --git a/servers/physics_3d/collision_object_3d_sw.cpp b/servers/physics_3d/collision_object_3d_sw.cpp deleted file mode 100644 index 098f627d11..0000000000 --- a/servers/physics_3d/collision_object_3d_sw.cpp +++ /dev/null @@ -1,240 +0,0 @@ -/*************************************************************************/ -/* collision_object_3d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "collision_object_3d_sw.h" -#include "servers/physics_3d/physics_server_3d_sw.h" -#include "space_3d_sw.h" - -void CollisionObject3DSW::add_shape(Shape3DSW *p_shape, const Transform3D &p_transform, bool p_disabled) { - Shape s; - s.shape = p_shape; - s.xform = p_transform; - s.xform_inv = s.xform.affine_inverse(); - s.bpid = 0; //needs update - s.disabled = p_disabled; - shapes.push_back(s); - p_shape->add_owner(this); - - if (!pending_shape_update_list.in_list()) { - PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); - } -} - -void CollisionObject3DSW::set_shape(int p_index, Shape3DSW *p_shape) { - ERR_FAIL_INDEX(p_index, shapes.size()); - shapes[p_index].shape->remove_owner(this); - shapes.write[p_index].shape = p_shape; - - p_shape->add_owner(this); - if (!pending_shape_update_list.in_list()) { - PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); - } -} - -void CollisionObject3DSW::set_shape_transform(int p_index, const Transform3D &p_transform) { - ERR_FAIL_INDEX(p_index, shapes.size()); - - shapes.write[p_index].xform = p_transform; - shapes.write[p_index].xform_inv = p_transform.affine_inverse(); - if (!pending_shape_update_list.in_list()) { - PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); - } -} - -void CollisionObject3DSW::set_shape_disabled(int p_idx, bool p_disabled) { - ERR_FAIL_INDEX(p_idx, shapes.size()); - - CollisionObject3DSW::Shape &shape = shapes.write[p_idx]; - if (shape.disabled == p_disabled) { - return; - } - - shape.disabled = p_disabled; - - if (!space) { - return; - } - - if (p_disabled && shape.bpid != 0) { - space->get_broadphase()->remove(shape.bpid); - shape.bpid = 0; - if (!pending_shape_update_list.in_list()) { - PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); - } - } else if (!p_disabled && shape.bpid == 0) { - if (!pending_shape_update_list.in_list()) { - PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); - } - } -} - -void CollisionObject3DSW::remove_shape(Shape3DSW *p_shape) { - //remove a shape, all the times it appears - for (int i = 0; i < shapes.size(); i++) { - if (shapes[i].shape == p_shape) { - remove_shape(i); - i--; - } - } -} - -void CollisionObject3DSW::remove_shape(int p_index) { - //remove anything from shape to be erased to end, so subindices don't change - ERR_FAIL_INDEX(p_index, shapes.size()); - for (int i = p_index; i < shapes.size(); i++) { - if (shapes[i].bpid == 0) { - continue; - } - //should never get here with a null owner - space->get_broadphase()->remove(shapes[i].bpid); - shapes.write[i].bpid = 0; - } - shapes[p_index].shape->remove_owner(this); - shapes.remove(p_index); - - if (!pending_shape_update_list.in_list()) { - PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); - } -} - -void CollisionObject3DSW::_set_static(bool p_static) { - if (_static == p_static) { - return; - } - _static = p_static; - - if (!space) { - return; - } - for (int i = 0; i < get_shape_count(); i++) { - const Shape &s = shapes[i]; - if (s.bpid > 0) { - space->get_broadphase()->set_static(s.bpid, _static); - } - } -} - -void CollisionObject3DSW::_unregister_shapes() { - for (int i = 0; i < shapes.size(); i++) { - Shape &s = shapes.write[i]; - if (s.bpid > 0) { - space->get_broadphase()->remove(s.bpid); - s.bpid = 0; - } - } -} - -void CollisionObject3DSW::_update_shapes() { - if (!space) { - return; - } - - for (int i = 0; i < shapes.size(); i++) { - Shape &s = shapes.write[i]; - if (s.disabled) { - continue; - } - - //not quite correct, should compute the next matrix.. - AABB shape_aabb = s.shape->get_aabb(); - Transform3D xform = transform * s.xform; - shape_aabb = xform.xform(shape_aabb); - shape_aabb.grow_by((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05); - s.aabb_cache = shape_aabb; - - Vector3 scale = xform.get_basis().get_scale(); - s.area_cache = s.shape->get_area() * scale.x * scale.y * scale.z; - - if (s.bpid == 0) { - s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); - space->get_broadphase()->set_static(s.bpid, _static); - } - - space->get_broadphase()->move(s.bpid, shape_aabb); - } -} - -void CollisionObject3DSW::_update_shapes_with_motion(const Vector3 &p_motion) { - if (!space) { - return; - } - - for (int i = 0; i < shapes.size(); i++) { - Shape &s = shapes.write[i]; - if (s.disabled) { - continue; - } - - //not quite correct, should compute the next matrix.. - AABB shape_aabb = s.shape->get_aabb(); - Transform3D xform = transform * s.xform; - shape_aabb = xform.xform(shape_aabb); - shape_aabb.merge_with(AABB(shape_aabb.position + p_motion, shape_aabb.size)); //use motion - s.aabb_cache = shape_aabb; - - if (s.bpid == 0) { - s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); - space->get_broadphase()->set_static(s.bpid, _static); - } - - space->get_broadphase()->move(s.bpid, shape_aabb); - } -} - -void CollisionObject3DSW::_set_space(Space3DSW *p_space) { - if (space) { - space->remove_object(this); - - for (int i = 0; i < shapes.size(); i++) { - Shape &s = shapes.write[i]; - if (s.bpid) { - space->get_broadphase()->remove(s.bpid); - s.bpid = 0; - } - } - } - - space = p_space; - - if (space) { - space->add_object(this); - _update_shapes(); - } -} - -void CollisionObject3DSW::_shape_changed() { - _update_shapes(); - _shapes_changed(); -} - -CollisionObject3DSW::CollisionObject3DSW(Type p_type) : - pending_shape_update_list(this) { - type = p_type; -} diff --git a/servers/physics_3d/collision_object_3d_sw.h b/servers/physics_3d/collision_object_3d_sw.h deleted file mode 100644 index 3aa48946b7..0000000000 --- a/servers/physics_3d/collision_object_3d_sw.h +++ /dev/null @@ -1,185 +0,0 @@ -/*************************************************************************/ -/* collision_object_3d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 COLLISION_OBJECT_SW_H -#define COLLISION_OBJECT_SW_H - -#include "broad_phase_3d_sw.h" -#include "core/templates/self_list.h" -#include "servers/physics_server_3d.h" -#include "shape_3d_sw.h" - -#ifdef DEBUG_ENABLED -#define MAX_OBJECT_DISTANCE 3.1622776601683791e+18 - -#define MAX_OBJECT_DISTANCE_X2 (MAX_OBJECT_DISTANCE * MAX_OBJECT_DISTANCE) -#endif - -class Space3DSW; - -class CollisionObject3DSW : public ShapeOwner3DSW { -public: - enum Type { - TYPE_AREA, - TYPE_BODY, - TYPE_SOFT_BODY, - }; - -private: - Type type; - RID self; - ObjectID instance_id; - uint32_t collision_layer = 1; - uint32_t collision_mask = 1; - - struct Shape { - Transform3D xform; - Transform3D xform_inv; - BroadPhase3DSW::ID bpid; - AABB aabb_cache; //for rayqueries - real_t area_cache = 0.0; - Shape3DSW *shape = nullptr; - bool disabled = false; - }; - - Vector shapes; - Space3DSW *space = nullptr; - Transform3D transform; - Transform3D inv_transform; - bool _static = true; - - SelfList pending_shape_update_list; - - void _update_shapes(); - -protected: - void _update_shapes_with_motion(const Vector3 &p_motion); - void _unregister_shapes(); - - _FORCE_INLINE_ void _set_transform(const Transform3D &p_transform, bool p_update_shapes = true) { -#ifdef DEBUG_ENABLED - - ERR_FAIL_COND_MSG(p_transform.origin.length_squared() > MAX_OBJECT_DISTANCE_X2, "Object went too far away (more than '" + itos(MAX_OBJECT_DISTANCE) + "' units from origin)."); -#endif - - transform = p_transform; - if (p_update_shapes) { - _update_shapes(); - } - } - _FORCE_INLINE_ void _set_inv_transform(const Transform3D &p_transform) { inv_transform = p_transform; } - void _set_static(bool p_static); - - virtual void _shapes_changed() = 0; - void _set_space(Space3DSW *p_space); - - bool ray_pickable = true; - - CollisionObject3DSW(Type p_type); - -public: - _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } - _FORCE_INLINE_ RID get_self() const { return self; } - - _FORCE_INLINE_ void set_instance_id(const ObjectID &p_instance_id) { instance_id = p_instance_id; } - _FORCE_INLINE_ ObjectID get_instance_id() const { return instance_id; } - - void _shape_changed(); - - _FORCE_INLINE_ Type get_type() const { return type; } - void add_shape(Shape3DSW *p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false); - void set_shape(int p_index, Shape3DSW *p_shape); - void set_shape_transform(int p_index, const Transform3D &p_transform); - _FORCE_INLINE_ int get_shape_count() const { return shapes.size(); } - _FORCE_INLINE_ Shape3DSW *get_shape(int p_index) const { - CRASH_BAD_INDEX(p_index, shapes.size()); - return shapes[p_index].shape; - } - _FORCE_INLINE_ const Transform3D &get_shape_transform(int p_index) const { - CRASH_BAD_INDEX(p_index, shapes.size()); - return shapes[p_index].xform; - } - _FORCE_INLINE_ const Transform3D &get_shape_inv_transform(int p_index) const { - CRASH_BAD_INDEX(p_index, shapes.size()); - return shapes[p_index].xform_inv; - } - _FORCE_INLINE_ const AABB &get_shape_aabb(int p_index) const { - CRASH_BAD_INDEX(p_index, shapes.size()); - return shapes[p_index].aabb_cache; - } - _FORCE_INLINE_ real_t get_shape_area(int p_index) const { - CRASH_BAD_INDEX(p_index, shapes.size()); - return shapes[p_index].area_cache; - } - - _FORCE_INLINE_ const Transform3D &get_transform() const { return transform; } - _FORCE_INLINE_ const Transform3D &get_inv_transform() const { return inv_transform; } - _FORCE_INLINE_ Space3DSW *get_space() const { return space; } - - _FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; } - _FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; } - - void set_shape_disabled(int p_idx, bool p_disabled); - _FORCE_INLINE_ bool is_shape_disabled(int p_idx) const { - ERR_FAIL_INDEX_V(p_idx, shapes.size(), false); - return shapes[p_idx].disabled; - } - - _FORCE_INLINE_ void set_collision_layer(uint32_t p_layer) { - collision_layer = p_layer; - _shape_changed(); - } - _FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; } - - _FORCE_INLINE_ void set_collision_mask(uint32_t p_mask) { - collision_mask = p_mask; - _shape_changed(); - } - _FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; } - - _FORCE_INLINE_ bool collides_with(CollisionObject3DSW *p_other) const { - return p_other->collision_layer & collision_mask; - } - - _FORCE_INLINE_ bool interacts_with(CollisionObject3DSW *p_other) const { - return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask; - } - - void remove_shape(Shape3DSW *p_shape); - void remove_shape(int p_index); - - virtual void set_space(Space3DSW *p_space) = 0; - - _FORCE_INLINE_ bool is_static() const { return _static; } - - virtual ~CollisionObject3DSW() {} -}; - -#endif // COLLISION_OBJECT_SW_H diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp deleted file mode 100644 index 86150f41a0..0000000000 --- a/servers/physics_3d/collision_solver_3d_sat.cpp +++ /dev/null @@ -1,2396 +0,0 @@ -/*************************************************************************/ -/* collision_solver_3d_sat.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "collision_solver_3d_sat.h" -#include "core/math/geometry_3d.h" - -#include "gjk_epa.h" - -#define fallback_collision_solver gjk_epa_calculate_penetration - -// Cylinder SAT analytic methods and face-circle contact points for cylinder-trimesh and cylinder-box collision are based on ODE colliders. - -/* - * Cylinder-trimesh and Cylinder-box colliders by Alen Ladavac - * Ported to ODE by Nguyen Binh - */ - -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -struct _CollectorCallback { - CollisionSolver3DSW::CallbackResult callback; - void *userdata = nullptr; - bool swap = false; - bool collided = false; - Vector3 normal; - Vector3 *prev_axis = nullptr; - - _FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B) { - if (swap) { - callback(p_point_B, 0, p_point_A, 0, userdata); - } else { - callback(p_point_A, 0, p_point_B, 0, userdata); - } - } -}; - -typedef void (*GenerateContactsFunc)(const Vector3 *, int, const Vector3 *, int, _CollectorCallback *); - -static void _generate_contacts_point_point(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { -#ifdef DEBUG_ENABLED - ERR_FAIL_COND(p_point_count_A != 1); - ERR_FAIL_COND(p_point_count_B != 1); -#endif - - p_callback->call(*p_points_A, *p_points_B); -} - -static void _generate_contacts_point_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { -#ifdef DEBUG_ENABLED - ERR_FAIL_COND(p_point_count_A != 1); - ERR_FAIL_COND(p_point_count_B != 2); -#endif - - Vector3 closest_B = Geometry3D::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B); - p_callback->call(*p_points_A, closest_B); -} - -static void _generate_contacts_point_face(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { -#ifdef DEBUG_ENABLED - ERR_FAIL_COND(p_point_count_A != 1); - ERR_FAIL_COND(p_point_count_B < 3); -#endif - - Vector3 closest_B = Plane(p_points_B[0], p_points_B[1], p_points_B[2]).project(*p_points_A); - - p_callback->call(*p_points_A, closest_B); -} - -static void _generate_contacts_point_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { -#ifdef DEBUG_ENABLED - ERR_FAIL_COND(p_point_count_A != 1); - ERR_FAIL_COND(p_point_count_B != 3); -#endif - - Vector3 closest_B = Plane(p_points_B[0], p_points_B[1], p_points_B[2]).project(*p_points_A); - - p_callback->call(*p_points_A, closest_B); -} - -static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { -#ifdef DEBUG_ENABLED - ERR_FAIL_COND(p_point_count_A != 2); - ERR_FAIL_COND(p_point_count_B != 2); // circle is actually a 4x3 matrix -#endif - - Vector3 rel_A = p_points_A[1] - p_points_A[0]; - Vector3 rel_B = p_points_B[1] - p_points_B[0]; - - Vector3 c = rel_A.cross(rel_B).cross(rel_B); - - if (Math::is_zero_approx(rel_A.dot(c))) { - // should handle somehow.. - //ERR_PRINT("TODO FIX"); - //return; - - Vector3 axis = rel_A.normalized(); //make an axis - Vector3 base_A = p_points_A[0] - axis * axis.dot(p_points_A[0]); - Vector3 base_B = p_points_B[0] - axis * axis.dot(p_points_B[0]); - - //sort all 4 points in axis - real_t dvec[4] = { axis.dot(p_points_A[0]), axis.dot(p_points_A[1]), axis.dot(p_points_B[0]), axis.dot(p_points_B[1]) }; - - SortArray sa; - sa.sort(dvec, 4); - - //use the middle ones as contacts - p_callback->call(base_A + axis * dvec[1], base_B + axis * dvec[1]); - p_callback->call(base_A + axis * dvec[2], base_B + axis * dvec[2]); - - return; - } - - real_t d = (c.dot(p_points_B[0]) - p_points_A[0].dot(c)) / rel_A.dot(c); - - if (d < 0.0) { - d = 0.0; - } else if (d > 1.0) { - d = 1.0; - } - - Vector3 closest_A = p_points_A[0] + rel_A * d; - Vector3 closest_B = Geometry3D::get_closest_point_to_segment_uncapped(closest_A, p_points_B); - p_callback->call(closest_A, closest_B); -} - -static void _generate_contacts_edge_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { -#ifdef DEBUG_ENABLED - ERR_FAIL_COND(p_point_count_A != 2); - ERR_FAIL_COND(p_point_count_B != 3); -#endif - - const Vector3 &circle_B_pos = p_points_B[0]; - Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos; - Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos; - - real_t circle_B_radius = circle_B_line_1.length(); - Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized(); - - Plane circle_plane(circle_B_normal, circle_B_pos); - - static const int max_clip = 2; - Vector3 contact_points[max_clip]; - int num_points = 0; - - // Project edge point in circle plane. - const Vector3 &edge_A_1 = p_points_A[0]; - Vector3 proj_point_1 = circle_plane.project(edge_A_1); - - Vector3 dist_vec = proj_point_1 - circle_B_pos; - real_t dist_sq = dist_vec.length_squared(); - - // Point 1 is inside disk, add as contact point. - if (dist_sq <= circle_B_radius * circle_B_radius) { - contact_points[num_points] = edge_A_1; - ++num_points; - } - - const Vector3 &edge_A_2 = p_points_A[1]; - Vector3 proj_point_2 = circle_plane.project(edge_A_2); - - Vector3 dist_vec_2 = proj_point_2 - circle_B_pos; - real_t dist_sq_2 = dist_vec_2.length_squared(); - - // Point 2 is inside disk, add as contact point. - if (dist_sq_2 <= circle_B_radius * circle_B_radius) { - contact_points[num_points] = edge_A_2; - ++num_points; - } - - if (num_points < 2) { - Vector3 line_vec = proj_point_2 - proj_point_1; - real_t line_length_sq = line_vec.length_squared(); - - // Create a quadratic formula of the form ax^2 + bx + c = 0 - real_t a, b, c; - - a = line_length_sq; - b = 2.0 * dist_vec.dot(line_vec); - c = dist_sq - circle_B_radius * circle_B_radius; - - // Solve for t. - real_t sqrtterm = b * b - 4.0 * a * c; - - // If the term we intend to square root is less than 0 then the answer won't be real, - // so the line doesn't intersect. - if (sqrtterm >= 0) { - sqrtterm = Math::sqrt(sqrtterm); - - Vector3 edge_dir = edge_A_2 - edge_A_1; - - real_t fraction_1 = (-b - sqrtterm) / (2.0 * a); - if ((fraction_1 > 0.0) && (fraction_1 < 1.0)) { - Vector3 face_point_1 = edge_A_1 + fraction_1 * edge_dir; - ERR_FAIL_COND(num_points >= max_clip); - contact_points[num_points] = face_point_1; - ++num_points; - } - - real_t fraction_2 = (-b + sqrtterm) / (2.0 * a); - if ((fraction_2 > 0.0) && (fraction_2 < 1.0) && !Math::is_equal_approx(fraction_1, fraction_2)) { - Vector3 face_point_2 = edge_A_1 + fraction_2 * edge_dir; - ERR_FAIL_COND(num_points >= max_clip); - contact_points[num_points] = face_point_2; - ++num_points; - } - } - } - - // Generate contact points. - for (int i = 0; i < num_points; i++) { - const Vector3 &contact_point_A = contact_points[i]; - - real_t d = circle_plane.distance_to(contact_point_A); - Vector3 closest_B = contact_point_A - circle_plane.normal * d; - - if (p_callback->normal.dot(contact_point_A) >= p_callback->normal.dot(closest_B)) { - continue; - } - - p_callback->call(contact_point_A, closest_B); - } -} - -static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { -#ifdef DEBUG_ENABLED - ERR_FAIL_COND(p_point_count_A < 2); - ERR_FAIL_COND(p_point_count_B < 3); -#endif - - static const int max_clip = 32; - - Vector3 _clipbuf1[max_clip]; - Vector3 _clipbuf2[max_clip]; - Vector3 *clipbuf_src = _clipbuf1; - Vector3 *clipbuf_dst = _clipbuf2; - int clipbuf_len = p_point_count_A; - - // copy A points to clipbuf_src - for (int i = 0; i < p_point_count_A; i++) { - clipbuf_src[i] = p_points_A[i]; - } - - Plane plane_B(p_points_B[0], p_points_B[1], p_points_B[2]); - - // go through all of B points - for (int i = 0; i < p_point_count_B; i++) { - int i_n = (i + 1) % p_point_count_B; - - Vector3 edge0_B = p_points_B[i]; - Vector3 edge1_B = p_points_B[i_n]; - - Vector3 clip_normal = (edge0_B - edge1_B).cross(plane_B.normal).normalized(); - // make a clip plane - - Plane clip(clip_normal, edge0_B); - // avoid double clip if A is edge - int dst_idx = 0; - bool edge = clipbuf_len == 2; - for (int j = 0; j < clipbuf_len; j++) { - int j_n = (j + 1) % clipbuf_len; - - Vector3 edge0_A = clipbuf_src[j]; - Vector3 edge1_A = clipbuf_src[j_n]; - - real_t dist0 = clip.distance_to(edge0_A); - real_t dist1 = clip.distance_to(edge1_A); - - if (dist0 <= 0) { // behind plane - - ERR_FAIL_COND(dst_idx >= max_clip); - clipbuf_dst[dst_idx++] = clipbuf_src[j]; - } - - // check for different sides and non coplanar - //if ( (dist0*dist1) < -CMP_EPSILON && !(edge && j)) { - if ((dist0 * dist1) < 0 && !(edge && j)) { - // calculate intersection - Vector3 rel = edge1_A - edge0_A; - real_t den = clip.normal.dot(rel); - real_t dist = -(clip.normal.dot(edge0_A) - clip.d) / den; - Vector3 inters = edge0_A + rel * dist; - - ERR_FAIL_COND(dst_idx >= max_clip); - clipbuf_dst[dst_idx] = inters; - dst_idx++; - } - } - - clipbuf_len = dst_idx; - SWAP(clipbuf_src, clipbuf_dst); - } - - // generate contacts - //Plane plane_A(p_points_A[0],p_points_A[1],p_points_A[2]); - - for (int i = 0; i < clipbuf_len; i++) { - real_t d = plane_B.distance_to(clipbuf_src[i]); - /* - if (d>CMP_EPSILON) - continue; - */ - - Vector3 closest_B = clipbuf_src[i] - plane_B.normal * d; - - if (p_callback->normal.dot(clipbuf_src[i]) >= p_callback->normal.dot(closest_B)) { - continue; - } - - p_callback->call(clipbuf_src[i], closest_B); - } -} - -static void _generate_contacts_face_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { -#ifdef DEBUG_ENABLED - ERR_FAIL_COND(p_point_count_A < 3); - ERR_FAIL_COND(p_point_count_B != 3); -#endif - - const Vector3 &circle_B_pos = p_points_B[0]; - Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos; - Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos; - - // Clip face with circle segments. - static const int circle_segments = 8; - Vector3 circle_points[circle_segments]; - - real_t angle_delta = 2.0 * Math_PI / circle_segments; - - for (int i = 0; i < circle_segments; ++i) { - Vector3 point_pos = circle_B_pos; - point_pos += circle_B_line_1 * Math::cos(i * angle_delta); - point_pos += circle_B_line_2 * Math::sin(i * angle_delta); - circle_points[i] = point_pos; - } - - _generate_contacts_face_face(p_points_A, p_point_count_A, circle_points, circle_segments, p_callback); - - // Clip face with circle plane. - Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized(); - - Plane circle_plane(circle_B_normal, circle_B_pos); - - static const int max_clip = 32; - Vector3 contact_points[max_clip]; - int num_points = 0; - - for (int i = 0; i < p_point_count_A; i++) { - int i_n = (i + 1) % p_point_count_A; - - const Vector3 &edge0_A = p_points_A[i]; - const Vector3 &edge1_A = p_points_A[i_n]; - - real_t dist0 = circle_plane.distance_to(edge0_A); - real_t dist1 = circle_plane.distance_to(edge1_A); - - // First point in front of plane, generate contact point. - if (dist0 * circle_plane.d >= 0) { - ERR_FAIL_COND(num_points >= max_clip); - contact_points[num_points] = edge0_A; - ++num_points; - } - - // Points on different sides, generate contact point. - if (dist0 * dist1 < 0) { - // calculate intersection - Vector3 rel = edge1_A - edge0_A; - real_t den = circle_plane.normal.dot(rel); - real_t dist = -(circle_plane.normal.dot(edge0_A) - circle_plane.d) / den; - Vector3 inters = edge0_A + rel * dist; - - ERR_FAIL_COND(num_points >= max_clip); - contact_points[num_points] = inters; - ++num_points; - } - } - - // Generate contact points. - for (int i = 0; i < num_points; i++) { - const Vector3 &contact_point_A = contact_points[i]; - - real_t d = circle_plane.distance_to(contact_point_A); - Vector3 closest_B = contact_point_A - circle_plane.normal * d; - - if (p_callback->normal.dot(contact_point_A) >= p_callback->normal.dot(closest_B)) { - continue; - } - - p_callback->call(contact_point_A, closest_B); - } -} - -static void _generate_contacts_circle_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { -#ifdef DEBUG_ENABLED - ERR_FAIL_COND(p_point_count_A != 3); - ERR_FAIL_COND(p_point_count_B != 3); -#endif - - const Vector3 &circle_A_pos = p_points_A[0]; - Vector3 circle_A_line_1 = p_points_A[1] - circle_A_pos; - Vector3 circle_A_line_2 = p_points_A[2] - circle_A_pos; - - real_t circle_A_radius = circle_A_line_1.length(); - Vector3 circle_A_normal = circle_A_line_1.cross(circle_A_line_2).normalized(); - - const Vector3 &circle_B_pos = p_points_B[0]; - Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos; - Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos; - - real_t circle_B_radius = circle_B_line_1.length(); - Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized(); - - static const int max_clip = 4; - Vector3 contact_points[max_clip]; - int num_points = 0; - - Vector3 centers_diff = circle_B_pos - circle_A_pos; - Vector3 norm_proj = circle_A_normal.dot(centers_diff) * circle_A_normal; - Vector3 comp_proj = centers_diff - norm_proj; - real_t proj_dist = comp_proj.length(); - if (!Math::is_zero_approx(proj_dist)) { - comp_proj /= proj_dist; - if ((proj_dist > circle_A_radius - circle_B_radius) && (proj_dist > circle_B_radius - circle_A_radius)) { - // Circles are overlapping, use the 2 points of intersection as contacts. - real_t radius_a_sqr = circle_A_radius * circle_A_radius; - real_t radius_b_sqr = circle_B_radius * circle_B_radius; - real_t d_sqr = proj_dist * proj_dist; - real_t s = (1.0 + (radius_a_sqr - radius_b_sqr) / d_sqr) * 0.5; - real_t h = Math::sqrt(MAX(radius_a_sqr - d_sqr * s * s, 0.0)); - Vector3 midpoint = circle_A_pos + s * comp_proj * proj_dist; - Vector3 h_vec = h * circle_A_normal.cross(comp_proj); - - Vector3 point_A = midpoint + h_vec; - contact_points[num_points] = point_A; - ++num_points; - - point_A = midpoint - h_vec; - contact_points[num_points] = point_A; - ++num_points; - - // Add 2 points from circle A and B along the line between the centers. - point_A = circle_A_pos + comp_proj * circle_A_radius; - contact_points[num_points] = point_A; - ++num_points; - - point_A = circle_B_pos - comp_proj * circle_B_radius - norm_proj; - contact_points[num_points] = point_A; - ++num_points; - } // Otherwise one circle is inside the other one, use 3 arbitrary equidistant points. - } // Otherwise circles are concentric, use 3 arbitrary equidistant points. - - if (num_points == 0) { - // Generate equidistant points. - if (circle_A_radius < circle_B_radius) { - // Circle A inside circle B. - for (int i = 0; i < 3; ++i) { - Vector3 circle_A_point = circle_A_pos; - circle_A_point += circle_A_line_1 * Math::cos(2.0 * Math_PI * i / 3.0); - circle_A_point += circle_A_line_2 * Math::sin(2.0 * Math_PI * i / 3.0); - - contact_points[num_points] = circle_A_point; - ++num_points; - } - } else { - // Circle B inside circle A. - for (int i = 0; i < 3; ++i) { - Vector3 circle_B_point = circle_B_pos; - circle_B_point += circle_B_line_1 * Math::cos(2.0 * Math_PI * i / 3.0); - circle_B_point += circle_B_line_2 * Math::sin(2.0 * Math_PI * i / 3.0); - - Vector3 circle_A_point = circle_B_point - norm_proj; - - contact_points[num_points] = circle_A_point; - ++num_points; - } - } - } - - Plane circle_B_plane(circle_B_normal, circle_B_pos); - - // Generate contact points. - for (int i = 0; i < num_points; i++) { - const Vector3 &contact_point_A = contact_points[i]; - - real_t d = circle_B_plane.distance_to(contact_point_A); - Vector3 closest_B = contact_point_A - circle_B_plane.normal * d; - - if (p_callback->normal.dot(contact_point_A) >= p_callback->normal.dot(closest_B)) { - continue; - } - - p_callback->call(contact_point_A, closest_B); - } -} - -static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_point_count_A, Shape3DSW::FeatureType p_feature_type_A, const Vector3 *p_points_B, int p_point_count_B, Shape3DSW::FeatureType p_feature_type_B, _CollectorCallback *p_callback) { -#ifdef DEBUG_ENABLED - ERR_FAIL_COND(p_point_count_A < 1); - ERR_FAIL_COND(p_point_count_B < 1); -#endif - - static const GenerateContactsFunc generate_contacts_func_table[4][4] = { - { - _generate_contacts_point_point, - _generate_contacts_point_edge, - _generate_contacts_point_face, - _generate_contacts_point_circle, - }, - { - nullptr, - _generate_contacts_edge_edge, - _generate_contacts_face_face, - _generate_contacts_edge_circle, - }, - { - nullptr, - nullptr, - _generate_contacts_face_face, - _generate_contacts_face_circle, - }, - { - nullptr, - nullptr, - nullptr, - _generate_contacts_circle_circle, - }, - }; - - int pointcount_B; - int pointcount_A; - const Vector3 *points_A; - const Vector3 *points_B; - int version_A; - int version_B; - - if (p_feature_type_A > p_feature_type_B) { - //swap - p_callback->swap = !p_callback->swap; - p_callback->normal = -p_callback->normal; - - pointcount_B = p_point_count_A; - pointcount_A = p_point_count_B; - points_A = p_points_B; - points_B = p_points_A; - version_A = p_feature_type_B; - version_B = p_feature_type_A; - } else { - pointcount_B = p_point_count_B; - pointcount_A = p_point_count_A; - points_A = p_points_A; - points_B = p_points_B; - version_A = p_feature_type_A; - version_B = p_feature_type_B; - } - - GenerateContactsFunc contacts_func = generate_contacts_func_table[version_A][version_B]; - ERR_FAIL_COND(!contacts_func); - contacts_func(points_A, pointcount_A, points_B, pointcount_B, p_callback); -} - -template -class SeparatorAxisTest { - const ShapeA *shape_A = nullptr; - const ShapeB *shape_B = nullptr; - const Transform3D *transform_A = nullptr; - const Transform3D *transform_B = nullptr; - real_t best_depth = 1e15; - Vector3 best_axis; - _CollectorCallback *callback = nullptr; - real_t margin_A = 0.0; - real_t margin_B = 0.0; - Vector3 separator_axis; - -public: - _FORCE_INLINE_ bool test_previous_axis() { - if (callback && callback->prev_axis && *callback->prev_axis != Vector3()) { - return test_axis(*callback->prev_axis); - } else { - return true; - } - } - - _FORCE_INLINE_ bool test_axis(const Vector3 &p_axis, bool p_directional = false) { - Vector3 axis = p_axis; - - if (axis.is_equal_approx(Vector3())) { - // strange case, try an upwards separator - axis = Vector3(0.0, 1.0, 0.0); - } - - real_t min_A, max_A, min_B, max_B; - - shape_A->project_range(axis, *transform_A, min_A, max_A); - shape_B->project_range(axis, *transform_B, min_B, max_B); - - if (withMargin) { - min_A -= margin_A; - max_A += margin_A; - min_B -= margin_B; - max_B += margin_B; - } - - min_B -= (max_A - min_A) * 0.5; - max_B += (max_A - min_A) * 0.5; - - min_B -= (min_A + max_A) * 0.5; - max_B -= (min_A + max_A) * 0.5; - - if (min_B > 0.0 || max_B < 0.0) { - separator_axis = axis; - return false; // doesn't contain 0 - } - - //use the smallest depth - - if (min_B < 0.0) { // could be +0.0, we don't want it to become -0.0 - if (p_directional) { - min_B = max_B; - axis = -axis; - } else { - min_B = -min_B; - } - } - - if (max_B < min_B) { - if (max_B < best_depth) { - best_depth = max_B; - best_axis = axis; - } - } else { - if (min_B < best_depth) { - best_depth = min_B; - best_axis = -axis; // keep it as A axis - } - } - - return true; - } - - static _FORCE_INLINE_ void test_contact_points(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { - SeparatorAxisTest *separator = (SeparatorAxisTest *)p_userdata; - Vector3 axis = (p_point_B - p_point_A); - real_t depth = axis.length(); - - // Filter out bogus directions with a threshold and re-testing axis. - if (separator->best_depth - depth > 0.001) { - separator->test_axis(axis / depth); - } - } - - _FORCE_INLINE_ void generate_contacts() { - // nothing to do, don't generate - if (best_axis == Vector3(0.0, 0.0, 0.0)) { - return; - } - - if (!callback->callback) { - //just was checking intersection? - callback->collided = true; - if (callback->prev_axis) { - *callback->prev_axis = best_axis; - } - return; - } - - static const int max_supports = 16; - - Vector3 supports_A[max_supports]; - int support_count_A; - Shape3DSW::FeatureType support_type_A; - shape_A->get_supports(transform_A->basis.xform_inv(-best_axis).normalized(), max_supports, supports_A, support_count_A, support_type_A); - for (int i = 0; i < support_count_A; i++) { - supports_A[i] = transform_A->xform(supports_A[i]); - } - - if (withMargin) { - for (int i = 0; i < support_count_A; i++) { - supports_A[i] += -best_axis * margin_A; - } - } - - Vector3 supports_B[max_supports]; - int support_count_B; - Shape3DSW::FeatureType support_type_B; - shape_B->get_supports(transform_B->basis.xform_inv(best_axis).normalized(), max_supports, supports_B, support_count_B, support_type_B); - for (int i = 0; i < support_count_B; i++) { - supports_B[i] = transform_B->xform(supports_B[i]); - } - - if (withMargin) { - for (int i = 0; i < support_count_B; i++) { - supports_B[i] += best_axis * margin_B; - } - } - - callback->normal = best_axis; - if (callback->prev_axis) { - *callback->prev_axis = best_axis; - } - _generate_contacts_from_supports(supports_A, support_count_A, support_type_A, supports_B, support_count_B, support_type_B, callback); - - callback->collided = true; - } - - _FORCE_INLINE_ SeparatorAxisTest(const ShapeA *p_shape_A, const Transform3D &p_transform_A, const ShapeB *p_shape_B, const Transform3D &p_transform_B, _CollectorCallback *p_callback, real_t p_margin_A = 0, real_t p_margin_B = 0) { - shape_A = p_shape_A; - shape_B = p_shape_B; - transform_A = &p_transform_A; - transform_B = &p_transform_B; - callback = p_callback; - margin_A = p_margin_A; - margin_B = p_margin_B; - } -}; - -/****** SAT TESTS *******/ - -typedef void (*CollisionFunc)(const Shape3DSW *, const Transform3D &, const Shape3DSW *, const Transform3D &, _CollectorCallback *p_callback, real_t, real_t); - -template -static void _collision_sphere_sphere(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShape3DSW *sphere_A = static_cast(p_a); - const SphereShape3DSW *sphere_B = static_cast(p_b); - - SeparatorAxisTest separator(sphere_A, p_transform_a, sphere_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - // previous axis - - if (!separator.test_previous_axis()) { - return; - } - - if (!separator.test_axis((p_transform_a.origin - p_transform_b.origin).normalized())) { - return; - } - - separator.generate_contacts(); -} - -template -static void _collision_sphere_box(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShape3DSW *sphere_A = static_cast(p_a); - const BoxShape3DSW *box_B = static_cast(p_b); - - SeparatorAxisTest separator(sphere_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - if (!separator.test_previous_axis()) { - return; - } - - // test faces - - for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_b.basis.get_axis(i).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - - // calculate closest point to sphere - - Vector3 cnormal = p_transform_b.xform_inv(p_transform_a.origin); - - Vector3 cpoint = p_transform_b.xform(Vector3( - - (cnormal.x < 0) ? -box_B->get_half_extents().x : box_B->get_half_extents().x, - (cnormal.y < 0) ? -box_B->get_half_extents().y : box_B->get_half_extents().y, - (cnormal.z < 0) ? -box_B->get_half_extents().z : box_B->get_half_extents().z)); - - // use point to test axis - Vector3 point_axis = (p_transform_a.origin - cpoint).normalized(); - - if (!separator.test_axis(point_axis)) { - return; - } - - // test edges - - for (int i = 0; i < 3; i++) { - Vector3 axis = point_axis.cross(p_transform_b.basis.get_axis(i)).cross(p_transform_b.basis.get_axis(i)).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - - separator.generate_contacts(); -} - -template -static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShape3DSW *sphere_A = static_cast(p_a); - const CapsuleShape3DSW *capsule_B = static_cast(p_b); - - SeparatorAxisTest separator(sphere_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - if (!separator.test_previous_axis()) { - return; - } - - //capsule sphere 1, sphere - - Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); - - Vector3 capsule_ball_1 = p_transform_b.origin + capsule_axis; - - if (!separator.test_axis((capsule_ball_1 - p_transform_a.origin).normalized())) { - return; - } - - //capsule sphere 2, sphere - - Vector3 capsule_ball_2 = p_transform_b.origin - capsule_axis; - - if (!separator.test_axis((capsule_ball_2 - p_transform_a.origin).normalized())) { - return; - } - - //capsule edge, sphere - - Vector3 b2a = p_transform_a.origin - p_transform_b.origin; - - Vector3 axis = b2a.cross(capsule_axis).cross(capsule_axis).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - - separator.generate_contacts(); -} - -template -static void _collision_sphere_cylinder(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShape3DSW *sphere_A = static_cast(p_a); - const CylinderShape3DSW *cylinder_B = static_cast(p_b); - - SeparatorAxisTest separator(sphere_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - if (!separator.test_previous_axis()) { - return; - } - - // Cylinder B end caps. - Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1).normalized(); - if (!separator.test_axis(cylinder_B_axis)) { - return; - } - - Vector3 cylinder_diff = p_transform_b.origin - p_transform_a.origin; - - // Cylinder B lateral surface. - if (!separator.test_axis(cylinder_B_axis.cross(cylinder_diff).cross(cylinder_B_axis).normalized())) { - return; - } - - // Closest point to cylinder caps. - const Vector3 &sphere_center = p_transform_a.origin; - Vector3 cyl_axis = p_transform_b.basis.get_axis(1); - Vector3 cap_axis = p_transform_b.basis.get_axis(0); - real_t height_scale = cyl_axis.length(); - real_t cap_dist = cylinder_B->get_height() * 0.5 * height_scale; - cyl_axis /= height_scale; - real_t radius_scale = cap_axis.length(); - real_t cap_radius = cylinder_B->get_radius() * radius_scale; - - for (int i = 0; i < 2; i++) { - Vector3 cap_dir = ((i == 0) ? cyl_axis : -cyl_axis); - Vector3 cap_pos = p_transform_b.origin + cap_dir * cap_dist; - - Vector3 closest_point; - - Vector3 diff = sphere_center - cap_pos; - Vector3 proj = diff - cap_dir.dot(diff) * cap_dir; - - real_t proj_len = proj.length(); - if (Math::is_zero_approx(proj_len)) { - // Point is equidistant to all circle points. - continue; - } - - closest_point = cap_pos + (cap_radius / proj_len) * proj; - - if (!separator.test_axis((closest_point - sphere_center).normalized())) { - return; - } - } - - separator.generate_contacts(); -} - -template -static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShape3DSW *sphere_A = static_cast(p_a); - const ConvexPolygonShape3DSW *convex_polygon_B = static_cast(p_b); - - SeparatorAxisTest separator(sphere_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - if (!separator.test_previous_axis()) { - return; - } - - const Geometry3D::MeshData &mesh = convex_polygon_B->get_mesh(); - - const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); - int face_count = mesh.faces.size(); - const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); - int edge_count = mesh.edges.size(); - const Vector3 *vertices = mesh.vertices.ptr(); - int vertex_count = mesh.vertices.size(); - - // Precalculating this makes the transforms faster. - Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); - - // faces of B - for (int i = 0; i < face_count; i++) { - Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - - // edges of B - for (int i = 0; i < edge_count; i++) { - Vector3 v1 = p_transform_b.xform(vertices[edges[i].a]); - Vector3 v2 = p_transform_b.xform(vertices[edges[i].b]); - Vector3 v3 = p_transform_a.origin; - - Vector3 n1 = v2 - v1; - Vector3 n2 = v2 - v3; - - Vector3 axis = n1.cross(n2).cross(n1).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - - // vertices of B - for (int i = 0; i < vertex_count; i++) { - Vector3 v1 = p_transform_b.xform(vertices[i]); - Vector3 v2 = p_transform_a.origin; - - Vector3 axis = (v2 - v1).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - - separator.generate_contacts(); -} - -template -static void _collision_sphere_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShape3DSW *sphere_A = static_cast(p_a); - const FaceShape3DSW *face_B = static_cast(p_b); - - SeparatorAxisTest separator(sphere_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - Vector3 vertex[3] = { - p_transform_b.xform(face_B->vertex[0]), - p_transform_b.xform(face_B->vertex[1]), - p_transform_b.xform(face_B->vertex[2]), - }; - - Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); - - if (!separator.test_axis(normal, !face_B->backface_collision)) { - return; - } - - // edges and points of B - for (int i = 0; i < 3; i++) { - Vector3 n1 = vertex[i] - p_transform_a.origin; - if (n1.dot(normal) < 0.0) { - n1 *= -1.0; - } - - if (!separator.test_axis(n1.normalized())) { - return; - } - - Vector3 n2 = vertex[(i + 1) % 3] - vertex[i]; - - Vector3 axis = n1.cross(n2).cross(n2).normalized(); - if (axis.dot(normal) < 0.0) { - axis *= -1.0; - } - - if (!separator.test_axis(axis)) { - return; - } - } - - separator.generate_contacts(); -} - -template -static void _collision_box_box(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const BoxShape3DSW *box_A = static_cast(p_a); - const BoxShape3DSW *box_B = static_cast(p_b); - - SeparatorAxisTest separator(box_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - if (!separator.test_previous_axis()) { - return; - } - - // test faces of A - - for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - - // test faces of B - - for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_b.basis.get_axis(i).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - - // test combined edges - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - Vector3 axis = p_transform_a.basis.get_axis(i).cross(p_transform_b.basis.get_axis(j)); - - if (Math::is_zero_approx(axis.length_squared())) { - continue; - } - axis.normalize(); - - if (!separator.test_axis(axis)) { - return; - } - } - } - - if (withMargin) { - //add endpoint test between closest vertices and edges - - // calculate closest point to sphere - - Vector3 ab_vec = p_transform_b.origin - p_transform_a.origin; - - Vector3 cnormal_a = p_transform_a.basis.xform_inv(ab_vec); - - Vector3 support_a = p_transform_a.xform(Vector3( - - (cnormal_a.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, - (cnormal_a.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, - (cnormal_a.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z)); - - Vector3 cnormal_b = p_transform_b.basis.xform_inv(-ab_vec); - - Vector3 support_b = p_transform_b.xform(Vector3( - - (cnormal_b.x < 0) ? -box_B->get_half_extents().x : box_B->get_half_extents().x, - (cnormal_b.y < 0) ? -box_B->get_half_extents().y : box_B->get_half_extents().y, - (cnormal_b.z < 0) ? -box_B->get_half_extents().z : box_B->get_half_extents().z)); - - Vector3 axis_ab = (support_a - support_b); - - if (!separator.test_axis(axis_ab.normalized())) { - return; - } - - //now try edges, which become cylinders! - - for (int i = 0; i < 3; i++) { - //a ->b - Vector3 axis_a = p_transform_a.basis.get_axis(i); - - if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) { - return; - } - - //b ->a - Vector3 axis_b = p_transform_b.basis.get_axis(i); - - if (!separator.test_axis(axis_ab.cross(axis_b).cross(axis_b).normalized())) { - return; - } - } - } - - separator.generate_contacts(); -} - -template -static void _collision_box_capsule(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const BoxShape3DSW *box_A = static_cast(p_a); - const CapsuleShape3DSW *capsule_B = static_cast(p_b); - - SeparatorAxisTest separator(box_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - if (!separator.test_previous_axis()) { - return; - } - - // faces of A - for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - - Vector3 cyl_axis = p_transform_b.basis.get_axis(1).normalized(); - - // edges of A, capsule cylinder - - for (int i = 0; i < 3; i++) { - // cylinder - Vector3 box_axis = p_transform_a.basis.get_axis(i); - Vector3 axis = box_axis.cross(cyl_axis); - if (Math::is_zero_approx(axis.length_squared())) { - continue; - } - - if (!separator.test_axis(axis.normalized())) { - return; - } - } - - // points of A, capsule cylinder - // this sure could be made faster somehow.. - - for (int i = 0; i < 2; i++) { - for (int j = 0; j < 2; j++) { - for (int k = 0; k < 2; k++) { - Vector3 he = box_A->get_half_extents(); - he.x *= (i * 2 - 1); - he.y *= (j * 2 - 1); - he.z *= (k * 2 - 1); - Vector3 point = p_transform_a.origin; - for (int l = 0; l < 3; l++) { - point += p_transform_a.basis.get_axis(l) * he[l]; - } - - //Vector3 axis = (point - cyl_axis * cyl_axis.dot(point)).normalized(); - Vector3 axis = Plane(cyl_axis).project(point).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - } - } - - // capsule balls, edges of A - - for (int i = 0; i < 2; i++) { - Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); - - Vector3 sphere_pos = p_transform_b.origin + ((i == 0) ? capsule_axis : -capsule_axis); - - Vector3 cnormal = p_transform_a.xform_inv(sphere_pos); - - Vector3 cpoint = p_transform_a.xform(Vector3( - - (cnormal.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, - (cnormal.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, - (cnormal.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z)); - - // use point to test axis - Vector3 point_axis = (sphere_pos - cpoint).normalized(); - - if (!separator.test_axis(point_axis)) { - return; - } - - // test edges of A - - for (int j = 0; j < 3; j++) { - Vector3 axis = point_axis.cross(p_transform_a.basis.get_axis(j)).cross(p_transform_a.basis.get_axis(j)).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - } - - separator.generate_contacts(); -} - -template -static void _collision_box_cylinder(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const BoxShape3DSW *box_A = static_cast(p_a); - const CylinderShape3DSW *cylinder_B = static_cast(p_b); - - SeparatorAxisTest separator(box_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - if (!separator.test_previous_axis()) { - return; - } - - // Faces of A. - for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - - Vector3 cyl_axis = p_transform_b.basis.get_axis(1).normalized(); - - // Cylinder end caps. - { - if (!separator.test_axis(cyl_axis)) { - return; - } - } - - // Edges of A, cylinder lateral surface. - for (int i = 0; i < 3; i++) { - Vector3 box_axis = p_transform_a.basis.get_axis(i); - Vector3 axis = box_axis.cross(cyl_axis); - if (Math::is_zero_approx(axis.length_squared())) { - continue; - } - - if (!separator.test_axis(axis.normalized())) { - return; - } - } - - // Gather points of A. - Vector3 vertices_A[8]; - Vector3 box_extent = box_A->get_half_extents(); - for (int i = 0; i < 2; i++) { - for (int j = 0; j < 2; j++) { - for (int k = 0; k < 2; k++) { - Vector3 extent = box_extent; - extent.x *= (i * 2 - 1); - extent.y *= (j * 2 - 1); - extent.z *= (k * 2 - 1); - Vector3 &point = vertices_A[i * 2 * 2 + j * 2 + k]; - point = p_transform_a.origin; - for (int l = 0; l < 3; l++) { - point += p_transform_a.basis.get_axis(l) * extent[l]; - } - } - } - } - - // Points of A, cylinder lateral surface. - for (int i = 0; i < 8; i++) { - const Vector3 &point = vertices_A[i]; - Vector3 axis = Plane(cyl_axis).project(point).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - - // Edges of A, cylinder end caps rim. - int edges_start_A[12] = { 0, 2, 4, 6, 0, 1, 4, 5, 0, 1, 2, 3 }; - int edges_end_A[12] = { 1, 3, 5, 7, 2, 3, 6, 7, 4, 5, 6, 7 }; - - Vector3 cap_axis = cyl_axis * (cylinder_B->get_height() * 0.5); - - for (int i = 0; i < 2; i++) { - Vector3 cap_pos = p_transform_b.origin + ((i == 0) ? cap_axis : -cap_axis); - - for (int e = 0; e < 12; e++) { - const Vector3 &edge_start = vertices_A[edges_start_A[e]]; - const Vector3 &edge_end = vertices_A[edges_end_A[e]]; - - Vector3 edge_dir = (edge_end - edge_start); - edge_dir.normalize(); - - real_t edge_dot = edge_dir.dot(cyl_axis); - if (Math::is_zero_approx(edge_dot)) { - // Edge is perpendicular to cylinder axis. - continue; - } - - // Calculate intersection between edge and circle plane. - Vector3 edge_diff = cap_pos - edge_start; - real_t diff_dot = edge_diff.dot(cyl_axis); - Vector3 intersection = edge_start + edge_dir * diff_dot / edge_dot; - - // Calculate tangent that touches intersection. - Vector3 tangent = (cap_pos - intersection).cross(cyl_axis); - - // Axis is orthogonal both to tangent and edge direction. - Vector3 axis = tangent.cross(edge_dir); - - if (!separator.test_axis(axis.normalized())) { - return; - } - } - } - - separator.generate_contacts(); -} - -template -static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const BoxShape3DSW *box_A = static_cast(p_a); - const ConvexPolygonShape3DSW *convex_polygon_B = static_cast(p_b); - - SeparatorAxisTest separator(box_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - if (!separator.test_previous_axis()) { - return; - } - - const Geometry3D::MeshData &mesh = convex_polygon_B->get_mesh(); - - const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); - int face_count = mesh.faces.size(); - const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); - int edge_count = mesh.edges.size(); - const Vector3 *vertices = mesh.vertices.ptr(); - int vertex_count = mesh.vertices.size(); - - // faces of A - for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - - // Precalculating this makes the transforms faster. - Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); - - // faces of B - for (int i = 0; i < face_count; i++) { - Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - - // A<->B edges - for (int i = 0; i < 3; i++) { - Vector3 e1 = p_transform_a.basis.get_axis(i); - - for (int j = 0; j < edge_count; j++) { - Vector3 e2 = p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]); - - Vector3 axis = e1.cross(e2).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - } - - if (withMargin) { - // calculate closest points between vertices and box edges - for (int v = 0; v < vertex_count; v++) { - Vector3 vtxb = p_transform_b.xform(vertices[v]); - Vector3 ab_vec = vtxb - p_transform_a.origin; - - Vector3 cnormal_a = p_transform_a.basis.xform_inv(ab_vec); - - Vector3 support_a = p_transform_a.xform(Vector3( - - (cnormal_a.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, - (cnormal_a.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, - (cnormal_a.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z)); - - Vector3 axis_ab = support_a - vtxb; - - if (!separator.test_axis(axis_ab.normalized())) { - return; - } - - //now try edges, which become cylinders! - - for (int i = 0; i < 3; i++) { - //a ->b - Vector3 axis_a = p_transform_a.basis.get_axis(i); - - if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) { - return; - } - } - } - - //convex edges and box points - for (int i = 0; i < 2; i++) { - for (int j = 0; j < 2; j++) { - for (int k = 0; k < 2; k++) { - Vector3 he = box_A->get_half_extents(); - he.x *= (i * 2 - 1); - he.y *= (j * 2 - 1); - he.z *= (k * 2 - 1); - Vector3 point = p_transform_a.origin; - for (int l = 0; l < 3; l++) { - point += p_transform_a.basis.get_axis(l) * he[l]; - } - - for (int e = 0; e < edge_count; e++) { - Vector3 p1 = p_transform_b.xform(vertices[edges[e].a]); - Vector3 p2 = p_transform_b.xform(vertices[edges[e].b]); - Vector3 n = (p2 - p1); - - if (!separator.test_axis((point - p2).cross(n).cross(n).normalized())) { - return; - } - } - } - } - } - } - - separator.generate_contacts(); -} - -template -static void _collision_box_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const BoxShape3DSW *box_A = static_cast(p_a); - const FaceShape3DSW *face_B = static_cast(p_b); - - SeparatorAxisTest separator(box_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - Vector3 vertex[3] = { - p_transform_b.xform(face_B->vertex[0]), - p_transform_b.xform(face_B->vertex[1]), - p_transform_b.xform(face_B->vertex[2]), - }; - - Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); - - if (!separator.test_axis(normal, !face_B->backface_collision)) { - return; - } - - // faces of A - for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); - if (axis.dot(normal) < 0.0) { - axis *= -1.0; - } - - if (!separator.test_axis(axis)) { - return; - } - } - - // combined edges - - for (int i = 0; i < 3; i++) { - Vector3 e = vertex[i] - vertex[(i + 1) % 3]; - - for (int j = 0; j < 3; j++) { - Vector3 axis = e.cross(p_transform_a.basis.get_axis(j)).normalized(); - if (axis.dot(normal) < 0.0) { - axis *= -1.0; - } - - if (!separator.test_axis(axis)) { - return; - } - } - } - - if (withMargin) { - // calculate closest points between vertices and box edges - for (int v = 0; v < 3; v++) { - Vector3 ab_vec = vertex[v] - p_transform_a.origin; - - Vector3 cnormal_a = p_transform_a.basis.xform_inv(ab_vec); - - Vector3 support_a = p_transform_a.xform(Vector3( - - (cnormal_a.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, - (cnormal_a.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, - (cnormal_a.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z)); - - Vector3 axis_ab = support_a - vertex[v]; - if (axis_ab.dot(normal) < 0.0) { - axis_ab *= -1.0; - } - - if (!separator.test_axis(axis_ab.normalized())) { - return; - } - - //now try edges, which become cylinders! - - for (int i = 0; i < 3; i++) { - //a ->b - Vector3 axis_a = p_transform_a.basis.get_axis(i); - - Vector3 axis = axis_ab.cross(axis_a).cross(axis_a).normalized(); - if (axis.dot(normal) < 0.0) { - axis *= -1.0; - } - - if (!separator.test_axis(axis)) { - return; - } - } - } - - //convex edges and box points, there has to be a way to speed up this (get closest point?) - for (int i = 0; i < 2; i++) { - for (int j = 0; j < 2; j++) { - for (int k = 0; k < 2; k++) { - Vector3 he = box_A->get_half_extents(); - he.x *= (i * 2 - 1); - he.y *= (j * 2 - 1); - he.z *= (k * 2 - 1); - Vector3 point = p_transform_a.origin; - for (int l = 0; l < 3; l++) { - point += p_transform_a.basis.get_axis(l) * he[l]; - } - - for (int e = 0; e < 3; e++) { - Vector3 p1 = vertex[e]; - Vector3 p2 = vertex[(e + 1) % 3]; - - Vector3 n = (p2 - p1); - - Vector3 axis = (point - p2).cross(n).cross(n).normalized(); - if (axis.dot(normal) < 0.0) { - axis *= -1.0; - } - - if (!separator.test_axis(axis)) { - return; - } - } - } - } - } - } - - separator.generate_contacts(); -} - -template -static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const CapsuleShape3DSW *capsule_A = static_cast(p_a); - const CapsuleShape3DSW *capsule_B = static_cast(p_b); - - SeparatorAxisTest separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - if (!separator.test_previous_axis()) { - return; - } - - // some values - - Vector3 capsule_A_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); - Vector3 capsule_B_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); - - Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis; - Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis; - Vector3 capsule_B_ball_1 = p_transform_b.origin + capsule_B_axis; - Vector3 capsule_B_ball_2 = p_transform_b.origin - capsule_B_axis; - - //balls-balls - - if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_1).normalized())) { - return; - } - if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_2).normalized())) { - return; - } - - if (!separator.test_axis((capsule_A_ball_2 - capsule_B_ball_1).normalized())) { - return; - } - if (!separator.test_axis((capsule_A_ball_2 - capsule_B_ball_2).normalized())) { - return; - } - - // edges-balls - - if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_1).cross(capsule_A_axis).cross(capsule_A_axis).normalized())) { - return; - } - - if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_2).cross(capsule_A_axis).cross(capsule_A_axis).normalized())) { - return; - } - - if (!separator.test_axis((capsule_B_ball_1 - capsule_A_ball_1).cross(capsule_B_axis).cross(capsule_B_axis).normalized())) { - return; - } - - if (!separator.test_axis((capsule_B_ball_1 - capsule_A_ball_2).cross(capsule_B_axis).cross(capsule_B_axis).normalized())) { - return; - } - - // edges - - if (!separator.test_axis(capsule_A_axis.cross(capsule_B_axis).normalized())) { - return; - } - - separator.generate_contacts(); -} - -template -static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const CapsuleShape3DSW *capsule_A = static_cast(p_a); - const CylinderShape3DSW *cylinder_B = static_cast(p_b); - - SeparatorAxisTest separator(capsule_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - if (!separator.test_previous_axis()) { - return; - } - - // Cylinder B end caps. - Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1).normalized(); - if (!separator.test_axis(cylinder_B_axis)) { - return; - } - - // Cylinder edge against capsule balls. - - Vector3 capsule_A_axis = p_transform_a.basis.get_axis(1); - - Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); - Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); - - if (!separator.test_axis((p_transform_b.origin - capsule_A_ball_1).cross(cylinder_B_axis).cross(cylinder_B_axis).normalized())) { - return; - } - - if (!separator.test_axis((p_transform_b.origin - capsule_A_ball_2).cross(cylinder_B_axis).cross(cylinder_B_axis).normalized())) { - return; - } - - // Cylinder edge against capsule edge. - - Vector3 center_diff = p_transform_b.origin - p_transform_a.origin; - - if (!separator.test_axis(capsule_A_axis.cross(center_diff).cross(capsule_A_axis).normalized())) { - return; - } - - if (!separator.test_axis(cylinder_B_axis.cross(center_diff).cross(cylinder_B_axis).normalized())) { - return; - } - - real_t proj = capsule_A_axis.cross(cylinder_B_axis).cross(cylinder_B_axis).dot(capsule_A_axis); - if (Math::is_zero_approx(proj)) { - // Parallel capsule and cylinder axes, handle with specific axes only. - // Note: GJKEPA with no margin can lead to degenerate cases in this situation. - separator.generate_contacts(); - return; - } - - CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest::test_contact_points; - - // Fallback to generic algorithm to find the best separating axis. - if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) { - return; - } - - separator.generate_contacts(); -} - -template -static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const CapsuleShape3DSW *capsule_A = static_cast(p_a); - const ConvexPolygonShape3DSW *convex_polygon_B = static_cast(p_b); - - SeparatorAxisTest separator(capsule_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - if (!separator.test_previous_axis()) { - return; - } - - const Geometry3D::MeshData &mesh = convex_polygon_B->get_mesh(); - - const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); - int face_count = mesh.faces.size(); - const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); - int edge_count = mesh.edges.size(); - const Vector3 *vertices = mesh.vertices.ptr(); - - // Precalculating this makes the transforms faster. - Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); - - // faces of B - for (int i = 0; i < face_count; i++) { - Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - - // edges of B, capsule cylinder - - for (int i = 0; i < edge_count; i++) { - // cylinder - Vector3 edge_axis = p_transform_b.basis.xform(vertices[edges[i].a]) - p_transform_b.basis.xform(vertices[edges[i].b]); - Vector3 axis = edge_axis.cross(p_transform_a.basis.get_axis(1)).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - - // capsule balls, edges of B - - for (int i = 0; i < 2; i++) { - // edges of B, capsule cylinder - - Vector3 capsule_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); - - Vector3 sphere_pos = p_transform_a.origin + ((i == 0) ? capsule_axis : -capsule_axis); - - for (int j = 0; j < edge_count; j++) { - Vector3 n1 = sphere_pos - p_transform_b.xform(vertices[edges[j].a]); - Vector3 n2 = p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]); - - Vector3 axis = n1.cross(n2).cross(n2).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - } - - separator.generate_contacts(); -} - -template -static void _collision_capsule_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const CapsuleShape3DSW *capsule_A = static_cast(p_a); - const FaceShape3DSW *face_B = static_cast(p_b); - - SeparatorAxisTest separator(capsule_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - Vector3 vertex[3] = { - p_transform_b.xform(face_B->vertex[0]), - p_transform_b.xform(face_B->vertex[1]), - p_transform_b.xform(face_B->vertex[2]), - }; - - Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); - - if (!separator.test_axis(normal, !face_B->backface_collision)) { - return; - } - - // edges of B, capsule cylinder - - Vector3 capsule_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); - - for (int i = 0; i < 3; i++) { - // edge-cylinder - Vector3 edge_axis = vertex[i] - vertex[(i + 1) % 3]; - - Vector3 axis = edge_axis.cross(capsule_axis).normalized(); - if (axis.dot(normal) < 0.0) { - axis *= -1.0; - } - - if (!separator.test_axis(axis)) { - return; - } - - Vector3 dir_axis = (p_transform_a.origin - vertex[i]).cross(capsule_axis).cross(capsule_axis).normalized(); - if (dir_axis.dot(normal) < 0.0) { - dir_axis *= -1.0; - } - - if (!separator.test_axis(dir_axis)) { - return; - } - - for (int j = 0; j < 2; j++) { - // point-spheres - Vector3 sphere_pos = p_transform_a.origin + ((j == 0) ? capsule_axis : -capsule_axis); - - Vector3 n1 = sphere_pos - vertex[i]; - if (n1.dot(normal) < 0.0) { - n1 *= -1.0; - } - - if (!separator.test_axis(n1.normalized())) { - return; - } - - Vector3 n2 = edge_axis; - - axis = n1.cross(n2).cross(n2); - if (axis.dot(normal) < 0.0) { - axis *= -1.0; - } - - if (!separator.test_axis(axis.normalized())) { - return; - } - } - } - - separator.generate_contacts(); -} - -template -static void _collision_cylinder_cylinder(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const CylinderShape3DSW *cylinder_A = static_cast(p_a); - const CylinderShape3DSW *cylinder_B = static_cast(p_b); - - SeparatorAxisTest separator(cylinder_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - Vector3 cylinder_A_axis = p_transform_a.basis.get_axis(1); - Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1); - - if (!separator.test_previous_axis()) { - return; - } - - // Cylinder A end caps. - if (!separator.test_axis(cylinder_A_axis.normalized())) { - return; - } - - // Cylinder B end caps. - if (!separator.test_axis(cylinder_A_axis.normalized())) { - return; - } - - Vector3 cylinder_diff = p_transform_b.origin - p_transform_a.origin; - - // Cylinder A lateral surface. - if (!separator.test_axis(cylinder_A_axis.cross(cylinder_diff).cross(cylinder_A_axis).normalized())) { - return; - } - - // Cylinder B lateral surface. - if (!separator.test_axis(cylinder_B_axis.cross(cylinder_diff).cross(cylinder_B_axis).normalized())) { - return; - } - - real_t proj = cylinder_A_axis.cross(cylinder_B_axis).cross(cylinder_B_axis).dot(cylinder_A_axis); - if (Math::is_zero_approx(proj)) { - // Parallel cylinders, handle with specific axes only. - // Note: GJKEPA with no margin can lead to degenerate cases in this situation. - separator.generate_contacts(); - return; - } - - CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest::test_contact_points; - - // Fallback to generic algorithm to find the best separating axis. - if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) { - return; - } - - separator.generate_contacts(); -} - -template -static void _collision_cylinder_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const CylinderShape3DSW *cylinder_A = static_cast(p_a); - const ConvexPolygonShape3DSW *convex_polygon_B = static_cast(p_b); - - SeparatorAxisTest separator(cylinder_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest::test_contact_points; - - // Fallback to generic algorithm to find the best separating axis. - if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) { - return; - } - - separator.generate_contacts(); -} - -template -static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const CylinderShape3DSW *cylinder_A = static_cast(p_a); - const FaceShape3DSW *face_B = static_cast(p_b); - - SeparatorAxisTest separator(cylinder_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - if (!separator.test_previous_axis()) { - return; - } - - Vector3 vertex[3] = { - p_transform_b.xform(face_B->vertex[0]), - p_transform_b.xform(face_B->vertex[1]), - p_transform_b.xform(face_B->vertex[2]), - }; - - Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); - - // Face B normal. - if (!separator.test_axis(normal, !face_B->backface_collision)) { - return; - } - - Vector3 cyl_axis = p_transform_a.basis.get_axis(1).normalized(); - if (cyl_axis.dot(normal) < 0.0) { - cyl_axis *= -1.0; - } - - // Cylinder end caps. - if (!separator.test_axis(cyl_axis)) { - return; - } - - // Edges of B, cylinder lateral surface. - for (int i = 0; i < 3; i++) { - Vector3 edge_axis = vertex[i] - vertex[(i + 1) % 3]; - Vector3 axis = edge_axis.cross(cyl_axis); - if (Math::is_zero_approx(axis.length_squared())) { - continue; - } - - if (axis.dot(normal) < 0.0) { - axis *= -1.0; - } - - if (!separator.test_axis(axis.normalized())) { - return; - } - } - - // Points of B, cylinder lateral surface. - for (int i = 0; i < 3; i++) { - const Vector3 &point = vertex[i]; - Vector3 axis = Plane(cyl_axis).project(point).normalized(); - if (axis.dot(normal) < 0.0) { - axis *= -1.0; - } - - if (!separator.test_axis(axis)) { - return; - } - } - - // Edges of B, cylinder end caps rim. - Vector3 cap_axis = cyl_axis * (cylinder_A->get_height() * 0.5); - - for (int i = 0; i < 2; i++) { - Vector3 cap_pos = p_transform_a.origin + ((i == 0) ? cap_axis : -cap_axis); - - for (int j = 0; j < 3; j++) { - const Vector3 &edge_start = vertex[j]; - const Vector3 &edge_end = vertex[(j + 1) % 3]; - Vector3 edge_dir = edge_end - edge_start; - edge_dir.normalize(); - - real_t edge_dot = edge_dir.dot(cyl_axis); - if (Math::is_zero_approx(edge_dot)) { - // Edge is perpendicular to cylinder axis. - continue; - } - - // Calculate intersection between edge and circle plane. - Vector3 edge_diff = cap_pos - edge_start; - real_t diff_dot = edge_diff.dot(cyl_axis); - Vector3 intersection = edge_start + edge_dir * diff_dot / edge_dot; - - // Calculate tangent that touches intersection. - Vector3 tangent = (cap_pos - intersection).cross(cyl_axis); - - // Axis is orthogonal both to tangent and edge direction. - Vector3 axis = tangent.cross(edge_dir); - if (axis.dot(normal) < 0.0) { - axis *= -1.0; - } - - if (!separator.test_axis(axis.normalized())) { - return; - } - } - } - - separator.generate_contacts(); -} - -template -static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const ConvexPolygonShape3DSW *convex_polygon_A = static_cast(p_a); - const ConvexPolygonShape3DSW *convex_polygon_B = static_cast(p_b); - - SeparatorAxisTest separator(convex_polygon_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - if (!separator.test_previous_axis()) { - return; - } - - const Geometry3D::MeshData &mesh_A = convex_polygon_A->get_mesh(); - - const Geometry3D::MeshData::Face *faces_A = mesh_A.faces.ptr(); - int face_count_A = mesh_A.faces.size(); - const Geometry3D::MeshData::Edge *edges_A = mesh_A.edges.ptr(); - int edge_count_A = mesh_A.edges.size(); - const Vector3 *vertices_A = mesh_A.vertices.ptr(); - int vertex_count_A = mesh_A.vertices.size(); - - const Geometry3D::MeshData &mesh_B = convex_polygon_B->get_mesh(); - - const Geometry3D::MeshData::Face *faces_B = mesh_B.faces.ptr(); - int face_count_B = mesh_B.faces.size(); - const Geometry3D::MeshData::Edge *edges_B = mesh_B.edges.ptr(); - int edge_count_B = mesh_B.edges.size(); - const Vector3 *vertices_B = mesh_B.vertices.ptr(); - int vertex_count_B = mesh_B.vertices.size(); - - // Precalculating this makes the transforms faster. - Basis a_xform_normal = p_transform_b.basis.inverse().transposed(); - - // faces of A - for (int i = 0; i < face_count_A; i++) { - Vector3 axis = a_xform_normal.xform(faces_A[i].plane.normal).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - - // Precalculating this makes the transforms faster. - Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); - - // faces of B - for (int i = 0; i < face_count_B; i++) { - Vector3 axis = b_xform_normal.xform(faces_B[i].plane.normal).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - - // A<->B edges - for (int i = 0; i < edge_count_A; i++) { - Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].a]) - p_transform_a.basis.xform(vertices_A[edges_A[i].b]); - - for (int j = 0; j < edge_count_B; j++) { - Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[j].a]) - p_transform_b.basis.xform(vertices_B[edges_B[j].b]); - - Vector3 axis = e1.cross(e2).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - } - - if (withMargin) { - //vertex-vertex - for (int i = 0; i < vertex_count_A; i++) { - Vector3 va = p_transform_a.xform(vertices_A[i]); - - for (int j = 0; j < vertex_count_B; j++) { - if (!separator.test_axis((va - p_transform_b.xform(vertices_B[j])).normalized())) { - return; - } - } - } - //edge-vertex (shell) - - for (int i = 0; i < edge_count_A; i++) { - Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].a]); - Vector3 e2 = p_transform_a.basis.xform(vertices_A[edges_A[i].b]); - Vector3 n = (e2 - e1); - - for (int j = 0; j < vertex_count_B; j++) { - Vector3 e3 = p_transform_b.xform(vertices_B[j]); - - if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) { - return; - } - } - } - - for (int i = 0; i < edge_count_B; i++) { - Vector3 e1 = p_transform_b.basis.xform(vertices_B[edges_B[i].a]); - Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[i].b]); - Vector3 n = (e2 - e1); - - for (int j = 0; j < vertex_count_A; j++) { - Vector3 e3 = p_transform_a.xform(vertices_A[j]); - - if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) { - return; - } - } - } - } - - separator.generate_contacts(); -} - -template -static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const ConvexPolygonShape3DSW *convex_polygon_A = static_cast(p_a); - const FaceShape3DSW *face_B = static_cast(p_b); - - SeparatorAxisTest separator(convex_polygon_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - - const Geometry3D::MeshData &mesh = convex_polygon_A->get_mesh(); - - const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); - int face_count = mesh.faces.size(); - const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); - int edge_count = mesh.edges.size(); - const Vector3 *vertices = mesh.vertices.ptr(); - int vertex_count = mesh.vertices.size(); - - Vector3 vertex[3] = { - p_transform_b.xform(face_B->vertex[0]), - p_transform_b.xform(face_B->vertex[1]), - p_transform_b.xform(face_B->vertex[2]), - }; - - Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); - - if (!separator.test_axis(normal, !face_B->backface_collision)) { - return; - } - - // faces of A - for (int i = 0; i < face_count; i++) { - //Vector3 axis = p_transform_a.xform( faces[i].plane ).normal; - Vector3 axis = p_transform_a.basis.xform(faces[i].plane.normal).normalized(); - if (axis.dot(normal) < 0.0) { - axis *= -1.0; - } - - if (!separator.test_axis(axis)) { - return; - } - } - - // A<->B edges - for (int i = 0; i < edge_count; i++) { - Vector3 e1 = p_transform_a.xform(vertices[edges[i].a]) - p_transform_a.xform(vertices[edges[i].b]); - - for (int j = 0; j < 3; j++) { - Vector3 e2 = vertex[j] - vertex[(j + 1) % 3]; - - Vector3 axis = e1.cross(e2).normalized(); - if (axis.dot(normal) < 0.0) { - axis *= -1.0; - } - - if (!separator.test_axis(axis)) { - return; - } - } - } - - if (withMargin) { - //vertex-vertex - for (int i = 0; i < vertex_count; i++) { - Vector3 va = p_transform_a.xform(vertices[i]); - - for (int j = 0; j < 3; j++) { - Vector3 axis = (va - vertex[j]).normalized(); - if (axis.dot(normal) < 0.0) { - axis *= -1.0; - } - - if (!separator.test_axis(axis)) { - return; - } - } - } - //edge-vertex (shell) - - for (int i = 0; i < edge_count; i++) { - Vector3 e1 = p_transform_a.basis.xform(vertices[edges[i].a]); - Vector3 e2 = p_transform_a.basis.xform(vertices[edges[i].b]); - Vector3 n = (e2 - e1); - - for (int j = 0; j < 3; j++) { - Vector3 e3 = vertex[j]; - - Vector3 axis = (e1 - e3).cross(n).cross(n).normalized(); - if (axis.dot(normal) < 0.0) { - axis *= -1.0; - } - - if (!separator.test_axis(axis)) { - return; - } - } - } - - for (int i = 0; i < 3; i++) { - Vector3 e1 = vertex[i]; - Vector3 e2 = vertex[(i + 1) % 3]; - Vector3 n = (e2 - e1); - - for (int j = 0; j < vertex_count; j++) { - Vector3 e3 = p_transform_a.xform(vertices[j]); - - Vector3 axis = (e1 - e3).cross(n).cross(n).normalized(); - if (axis.dot(normal) < 0.0) { - axis *= -1.0; - } - - if (!separator.test_axis(axis)) { - return; - } - } - } - } - - separator.generate_contacts(); -} - -bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector3 *r_prev_axis, real_t p_margin_a, real_t p_margin_b) { - PhysicsServer3D::ShapeType type_A = p_shape_A->get_type(); - - ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_WORLD_BOUNDARY, false); - ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_SEPARATION_RAY, false); - ERR_FAIL_COND_V(p_shape_A->is_concave(), false); - - PhysicsServer3D::ShapeType type_B = p_shape_B->get_type(); - - ERR_FAIL_COND_V(type_B == PhysicsServer3D::SHAPE_WORLD_BOUNDARY, false); - ERR_FAIL_COND_V(type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY, false); - ERR_FAIL_COND_V(p_shape_B->is_concave(), false); - - static const CollisionFunc collision_table[6][6] = { - { _collision_sphere_sphere, - _collision_sphere_box, - _collision_sphere_capsule, - _collision_sphere_cylinder, - _collision_sphere_convex_polygon, - _collision_sphere_face }, - { nullptr, - _collision_box_box, - _collision_box_capsule, - _collision_box_cylinder, - _collision_box_convex_polygon, - _collision_box_face }, - { nullptr, - nullptr, - _collision_capsule_capsule, - _collision_capsule_cylinder, - _collision_capsule_convex_polygon, - _collision_capsule_face }, - { nullptr, - nullptr, - nullptr, - _collision_cylinder_cylinder, - _collision_cylinder_convex_polygon, - _collision_cylinder_face }, - { nullptr, - nullptr, - nullptr, - nullptr, - _collision_convex_polygon_convex_polygon, - _collision_convex_polygon_face }, - { nullptr, - nullptr, - nullptr, - nullptr, - nullptr, - nullptr }, - }; - - static const CollisionFunc collision_table_margin[6][6] = { - { _collision_sphere_sphere, - _collision_sphere_box, - _collision_sphere_capsule, - _collision_sphere_cylinder, - _collision_sphere_convex_polygon, - _collision_sphere_face }, - { nullptr, - _collision_box_box, - _collision_box_capsule, - _collision_box_cylinder, - _collision_box_convex_polygon, - _collision_box_face }, - { nullptr, - nullptr, - _collision_capsule_capsule, - _collision_capsule_cylinder, - _collision_capsule_convex_polygon, - _collision_capsule_face }, - { nullptr, - nullptr, - nullptr, - _collision_cylinder_cylinder, - _collision_cylinder_convex_polygon, - _collision_cylinder_face }, - { nullptr, - nullptr, - nullptr, - nullptr, - _collision_convex_polygon_convex_polygon, - _collision_convex_polygon_face }, - { nullptr, - nullptr, - nullptr, - nullptr, - nullptr, - nullptr }, - }; - - _CollectorCallback callback; - callback.callback = p_result_callback; - callback.swap = p_swap; - callback.userdata = p_userdata; - callback.collided = false; - callback.prev_axis = r_prev_axis; - - const Shape3DSW *A = p_shape_A; - const Shape3DSW *B = p_shape_B; - const Transform3D *transform_A = &p_transform_A; - const Transform3D *transform_B = &p_transform_B; - real_t margin_A = p_margin_a; - real_t margin_B = p_margin_b; - - if (type_A > type_B) { - SWAP(A, B); - SWAP(transform_A, transform_B); - SWAP(type_A, type_B); - SWAP(margin_A, margin_B); - callback.swap = !callback.swap; - } - - CollisionFunc collision_func; - if (margin_A != 0.0 || margin_B != 0.0) { - collision_func = collision_table_margin[type_A - 2][type_B - 2]; - - } else { - collision_func = collision_table[type_A - 2][type_B - 2]; - } - ERR_FAIL_COND_V(!collision_func, false); - - collision_func(A, *transform_A, B, *transform_B, &callback, margin_A, margin_B); - - return callback.collided; -} diff --git a/servers/physics_3d/collision_solver_3d_sat.h b/servers/physics_3d/collision_solver_3d_sat.h deleted file mode 100644 index e50da7b101..0000000000 --- a/servers/physics_3d/collision_solver_3d_sat.h +++ /dev/null @@ -1,38 +0,0 @@ -/*************************************************************************/ -/* collision_solver_3d_sat.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 COLLISION_SOLVER_SAT_H -#define COLLISION_SOLVER_SAT_H - -#include "collision_solver_3d_sw.h" - -bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector3 *r_prev_axis = nullptr, real_t p_margin_a = 0, real_t p_margin_b = 0); - -#endif // COLLISION_SOLVER_SAT_H diff --git a/servers/physics_3d/collision_solver_3d_sw.cpp b/servers/physics_3d/collision_solver_3d_sw.cpp deleted file mode 100644 index 96f1936668..0000000000 --- a/servers/physics_3d/collision_solver_3d_sw.cpp +++ /dev/null @@ -1,572 +0,0 @@ -/*************************************************************************/ -/* collision_solver_3d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "collision_solver_3d_sw.h" -#include "collision_solver_3d_sat.h" -#include "soft_body_3d_sw.h" - -#include "gjk_epa.h" - -#define collision_solver sat_calculate_penetration -//#define collision_solver gjk_epa_calculate_penetration - -bool CollisionSolver3DSW::solve_static_world_boundary(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { - const WorldBoundaryShape3DSW *world_boundary = static_cast(p_shape_A); - if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { - return false; - } - Plane p = p_transform_A.xform(world_boundary->get_plane()); - - static const int max_supports = 16; - Vector3 supports[max_supports]; - int support_count; - Shape3DSW::FeatureType support_type; - p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type); - - if (support_type == Shape3DSW::FEATURE_CIRCLE) { - ERR_FAIL_COND_V(support_count != 3, false); - - Vector3 circle_pos = supports[0]; - Vector3 circle_axis_1 = supports[1] - circle_pos; - Vector3 circle_axis_2 = supports[2] - circle_pos; - - // Use 3 equidistant points on the circle. - for (int i = 0; i < 3; ++i) { - Vector3 vertex_pos = circle_pos; - vertex_pos += circle_axis_1 * Math::cos(2.0 * Math_PI * i / 3.0); - vertex_pos += circle_axis_2 * Math::sin(2.0 * Math_PI * i / 3.0); - supports[i] = vertex_pos; - } - } - - bool found = false; - - for (int i = 0; i < support_count; i++) { - supports[i] = p_transform_B.xform(supports[i]); - if (p.distance_to(supports[i]) >= 0) { - continue; - } - found = true; - - Vector3 support_A = p.project(supports[i]); - - if (p_result_callback) { - if (p_swap_result) { - p_result_callback(supports[i], 0, support_A, 0, p_userdata); - } else { - p_result_callback(support_A, 0, supports[i], 0, p_userdata); - } - } - } - - return found; -} - -bool CollisionSolver3DSW::solve_separation_ray(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) { - const SeparationRayShape3DSW *ray = static_cast(p_shape_A); - - Vector3 from = p_transform_A.origin; - Vector3 to = from + p_transform_A.basis.get_axis(2) * (ray->get_length() + p_margin); - Vector3 support_A = to; - - Transform3D ai = p_transform_B.affine_inverse(); - - from = ai.xform(from); - to = ai.xform(to); - - Vector3 p, n; - if (!p_shape_B->intersect_segment(from, to, p, n)) { - return false; - } - - // Discard contacts when the ray is fully contained inside the shape. - if (n == Vector3()) { - return false; - } - - // Discard contacts in the wrong direction. - if (n.dot(from - to) < CMP_EPSILON) { - return false; - } - - Vector3 support_B = p_transform_B.xform(p); - if (ray->get_slide_on_slope()) { - Vector3 global_n = ai.basis.xform_inv(n).normalized(); - support_B = support_A + (support_B - support_A).length() * global_n; - } - - if (p_result_callback) { - if (p_swap_result) { - p_result_callback(support_B, 0, support_A, 0, p_userdata); - } else { - p_result_callback(support_A, 0, support_B, 0, p_userdata); - } - } - return true; -} - -struct _SoftBodyContactCollisionInfo { - int node_index = 0; - CollisionSolver3DSW::CallbackResult result_callback = nullptr; - void *userdata = nullptr; - bool swap_result = false; - int contact_count = 0; -}; - -void CollisionSolver3DSW::soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { - _SoftBodyContactCollisionInfo &cinfo = *(_SoftBodyContactCollisionInfo *)(p_userdata); - - ++cinfo.contact_count; - - if (!cinfo.result_callback) { - return; - } - - if (cinfo.swap_result) { - cinfo.result_callback(p_point_B, cinfo.node_index, p_point_A, p_index_A, cinfo.userdata); - } else { - cinfo.result_callback(p_point_A, p_index_A, p_point_B, cinfo.node_index, cinfo.userdata); - } -} - -struct _SoftBodyQueryInfo { - SoftBody3DSW *soft_body = nullptr; - const Shape3DSW *shape_A = nullptr; - const Shape3DSW *shape_B = nullptr; - Transform3D transform_A; - Transform3D node_transform; - _SoftBodyContactCollisionInfo contact_info; -#ifdef DEBUG_ENABLED - int node_query_count = 0; - int convex_query_count = 0; -#endif -}; - -bool CollisionSolver3DSW::soft_body_query_callback(uint32_t p_node_index, void *p_userdata) { - _SoftBodyQueryInfo &query_cinfo = *(_SoftBodyQueryInfo *)(p_userdata); - - Vector3 node_position = query_cinfo.soft_body->get_node_position(p_node_index); - - Transform3D transform_B; - transform_B.origin = query_cinfo.node_transform.xform(node_position); - - query_cinfo.contact_info.node_index = p_node_index; - bool collided = solve_static(query_cinfo.shape_A, query_cinfo.transform_A, query_cinfo.shape_B, transform_B, soft_body_contact_callback, &query_cinfo.contact_info); - -#ifdef DEBUG_ENABLED - ++query_cinfo.node_query_count; -#endif - - // Stop at first collision if contacts are not needed. - return (collided && !query_cinfo.contact_info.result_callback); -} - -bool CollisionSolver3DSW::soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex) { - _SoftBodyQueryInfo &query_cinfo = *(_SoftBodyQueryInfo *)(p_userdata); - - query_cinfo.shape_A = p_convex; - - // Calculate AABB for internal soft body query (in world space). - AABB shape_aabb; - for (int i = 0; i < 3; i++) { - Vector3 axis; - axis[i] = 1.0; - - real_t smin, smax; - p_convex->project_range(axis, query_cinfo.transform_A, smin, smax); - - shape_aabb.position[i] = smin; - shape_aabb.size[i] = smax - smin; - } - - shape_aabb.grow_by(query_cinfo.soft_body->get_collision_margin()); - - query_cinfo.soft_body->query_aabb(shape_aabb, soft_body_query_callback, &query_cinfo); - - bool collided = (query_cinfo.contact_info.contact_count > 0); - -#ifdef DEBUG_ENABLED - ++query_cinfo.convex_query_count; -#endif - - // Stop at first collision if contacts are not needed. - return (collided && !query_cinfo.contact_info.result_callback); -} - -bool CollisionSolver3DSW::solve_soft_body(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { - const SoftBodyShape3DSW *soft_body_shape_B = static_cast(p_shape_B); - - SoftBody3DSW *soft_body = soft_body_shape_B->get_soft_body(); - const Transform3D &world_to_local = soft_body->get_inv_transform(); - - const real_t collision_margin = soft_body->get_collision_margin(); - - SphereShape3DSW sphere_shape; - sphere_shape.set_data(collision_margin); - - _SoftBodyQueryInfo query_cinfo; - query_cinfo.contact_info.result_callback = p_result_callback; - query_cinfo.contact_info.userdata = p_userdata; - query_cinfo.contact_info.swap_result = p_swap_result; - query_cinfo.soft_body = soft_body; - query_cinfo.node_transform = p_transform_B * world_to_local; - query_cinfo.shape_A = p_shape_A; - query_cinfo.transform_A = p_transform_A; - query_cinfo.shape_B = &sphere_shape; - - if (p_shape_A->is_concave()) { - // In case of concave shape, query convex shapes first. - const ConcaveShape3DSW *concave_shape_A = static_cast(p_shape_A); - - AABB soft_body_aabb = soft_body->get_bounds(); - soft_body_aabb.grow_by(collision_margin); - - // Calculate AABB for internal concave shape query (in local space). - AABB local_aabb; - for (int i = 0; i < 3; i++) { - Vector3 axis(p_transform_A.basis.get_axis(i)); - real_t axis_scale = 1.0 / axis.length(); - - real_t smin = soft_body_aabb.position[i]; - real_t smax = smin + soft_body_aabb.size[i]; - - smin *= axis_scale; - smax *= axis_scale; - - local_aabb.position[i] = smin; - local_aabb.size[i] = smax - smin; - } - - concave_shape_A->cull(local_aabb, soft_body_concave_callback, &query_cinfo); - } else { - AABB shape_aabb = p_transform_A.xform(p_shape_A->get_aabb()); - shape_aabb.grow_by(collision_margin); - - soft_body->query_aabb(shape_aabb, soft_body_query_callback, &query_cinfo); - } - - return (query_cinfo.contact_info.contact_count > 0); -} - -struct _ConcaveCollisionInfo { - const Transform3D *transform_A; - const Shape3DSW *shape_A; - const Transform3D *transform_B; - CollisionSolver3DSW::CallbackResult result_callback; - void *userdata; - bool swap_result; - bool collided; - int aabb_tests; - int collisions; - bool tested; - real_t margin_A; - real_t margin_B; - Vector3 close_A, close_B; -}; - -bool CollisionSolver3DSW::concave_callback(void *p_userdata, Shape3DSW *p_convex) { - _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata); - cinfo.aabb_tests++; - - bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, nullptr, cinfo.margin_A, cinfo.margin_B); - if (!collided) { - return false; - } - - cinfo.collided = true; - cinfo.collisions++; - - // Stop at first collision if contacts are not needed. - return !cinfo.result_callback; -} - -bool CollisionSolver3DSW::solve_concave(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A, real_t p_margin_B) { - const ConcaveShape3DSW *concave_B = static_cast(p_shape_B); - - _ConcaveCollisionInfo cinfo; - cinfo.transform_A = &p_transform_A; - cinfo.shape_A = p_shape_A; - cinfo.transform_B = &p_transform_B; - cinfo.result_callback = p_result_callback; - cinfo.userdata = p_userdata; - cinfo.swap_result = p_swap_result; - cinfo.collided = false; - cinfo.collisions = 0; - cinfo.margin_A = p_margin_A; - cinfo.margin_B = p_margin_B; - - cinfo.aabb_tests = 0; - - Transform3D rel_transform = p_transform_A; - rel_transform.origin -= p_transform_B.origin; - - //quickly compute a local AABB - - AABB local_aabb; - for (int i = 0; i < 3; i++) { - Vector3 axis(p_transform_B.basis.get_axis(i)); - real_t axis_scale = 1.0 / axis.length(); - axis *= axis_scale; - - real_t smin, smax; - p_shape_A->project_range(axis, rel_transform, smin, smax); - smin -= p_margin_A; - smax += p_margin_A; - smin *= axis_scale; - smax *= axis_scale; - - local_aabb.position[i] = smin; - local_aabb.size[i] = smax - smin; - } - - concave_B->cull(local_aabb, concave_callback, &cinfo); - - return cinfo.collided; -} - -bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { - PhysicsServer3D::ShapeType type_A = p_shape_A->get_type(); - PhysicsServer3D::ShapeType type_B = p_shape_B->get_type(); - bool concave_A = p_shape_A->is_concave(); - bool concave_B = p_shape_B->is_concave(); - - bool swap = false; - - if (type_A > type_B) { - SWAP(type_A, type_B); - SWAP(concave_A, concave_B); - swap = true; - } - - if (type_A == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { - if (type_B == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { - return false; - } - if (type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY) { - return false; - } - if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) { - return false; - } - - if (swap) { - return solve_static_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); - } else { - return solve_static_world_boundary(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); - } - - } else if (type_A == PhysicsServer3D::SHAPE_SEPARATION_RAY) { - if (type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY) { - return false; - } - - if (swap) { - return solve_separation_ray(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, p_margin_B); - } else { - return solve_separation_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, p_margin_A); - } - - } else if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) { - if (type_A == PhysicsServer3D::SHAPE_SOFT_BODY) { - // Soft Body / Soft Body not supported. - return false; - } - - if (swap) { - return solve_soft_body(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); - } else { - return solve_soft_body(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); - } - - } else if (concave_B) { - if (concave_A) { - return false; - } - - if (!swap) { - return solve_concave(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, p_margin_A, p_margin_B); - } else { - return solve_concave(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, p_margin_A, p_margin_B); - } - - } else { - return collision_solver(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, r_sep_axis, p_margin_A, p_margin_B); - } -} - -bool CollisionSolver3DSW::concave_distance_callback(void *p_userdata, Shape3DSW *p_convex) { - _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata); - cinfo.aabb_tests++; - - Vector3 close_A, close_B; - cinfo.collided = !gjk_epa_calculate_distance(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, close_A, close_B); - - if (cinfo.collided) { - // No need to process any more result. - return true; - } - - if (!cinfo.tested || close_A.distance_squared_to(close_B) < cinfo.close_A.distance_squared_to(cinfo.close_B)) { - cinfo.close_A = close_A; - cinfo.close_B = close_B; - cinfo.tested = true; - } - - cinfo.collisions++; - return false; -} - -bool CollisionSolver3DSW::solve_distance_world_boundary(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) { - const WorldBoundaryShape3DSW *world_boundary = static_cast(p_shape_A); - if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { - return false; - } - Plane p = p_transform_A.xform(world_boundary->get_plane()); - - static const int max_supports = 16; - Vector3 supports[max_supports]; - int support_count; - Shape3DSW::FeatureType support_type; - - p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type); - - if (support_type == Shape3DSW::FEATURE_CIRCLE) { - ERR_FAIL_COND_V(support_count != 3, false); - - Vector3 circle_pos = supports[0]; - Vector3 circle_axis_1 = supports[1] - circle_pos; - Vector3 circle_axis_2 = supports[2] - circle_pos; - - // Use 3 equidistant points on the circle. - for (int i = 0; i < 3; ++i) { - Vector3 vertex_pos = circle_pos; - vertex_pos += circle_axis_1 * Math::cos(2.0 * Math_PI * i / 3.0); - vertex_pos += circle_axis_2 * Math::sin(2.0 * Math_PI * i / 3.0); - supports[i] = vertex_pos; - } - } - - bool collided = false; - Vector3 closest; - real_t closest_d = 0; - - for (int i = 0; i < support_count; i++) { - supports[i] = p_transform_B.xform(supports[i]); - real_t d = p.distance_to(supports[i]); - if (i == 0 || d < closest_d) { - closest = supports[i]; - closest_d = d; - if (d <= 0) { - collided = true; - } - } - } - - r_point_A = p.project(closest); - r_point_B = closest; - - return collided; -} - -bool CollisionSolver3DSW::solve_distance(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis) { - if (p_shape_A->is_concave()) { - return false; - } - - if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { - Vector3 a, b; - bool col = solve_distance_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, a, b); - r_point_A = b; - r_point_B = a; - return !col; - - } else if (p_shape_B->is_concave()) { - if (p_shape_A->is_concave()) { - return false; - } - - const ConcaveShape3DSW *concave_B = static_cast(p_shape_B); - - _ConcaveCollisionInfo cinfo; - cinfo.transform_A = &p_transform_A; - cinfo.shape_A = p_shape_A; - cinfo.transform_B = &p_transform_B; - cinfo.result_callback = nullptr; - cinfo.userdata = nullptr; - cinfo.swap_result = false; - cinfo.collided = false; - cinfo.collisions = 0; - cinfo.aabb_tests = 0; - cinfo.tested = false; - - Transform3D rel_transform = p_transform_A; - rel_transform.origin -= p_transform_B.origin; - - //quickly compute a local AABB - - bool use_cc_hint = p_concave_hint != AABB(); - AABB cc_hint_aabb; - if (use_cc_hint) { - cc_hint_aabb = p_concave_hint; - cc_hint_aabb.position -= p_transform_B.origin; - } - - AABB local_aabb; - for (int i = 0; i < 3; i++) { - Vector3 axis(p_transform_B.basis.get_axis(i)); - real_t axis_scale = ((real_t)1.0) / axis.length(); - axis *= axis_scale; - - real_t smin, smax; - - if (use_cc_hint) { - cc_hint_aabb.project_range_in_plane(Plane(axis), smin, smax); - } else { - p_shape_A->project_range(axis, rel_transform, smin, smax); - } - - smin *= axis_scale; - smax *= axis_scale; - - local_aabb.position[i] = smin; - local_aabb.size[i] = smax - smin; - } - - concave_B->cull(local_aabb, concave_distance_callback, &cinfo); - if (!cinfo.collided) { - r_point_A = cinfo.close_A; - r_point_B = cinfo.close_B; - } - - return !cinfo.collided; - } else { - return gjk_epa_calculate_distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, r_point_A, r_point_B); //should pass sepaxis.. - } -} diff --git a/servers/physics_3d/collision_solver_3d_sw.h b/servers/physics_3d/collision_solver_3d_sw.h deleted file mode 100644 index 0a9ea7c0eb..0000000000 --- a/servers/physics_3d/collision_solver_3d_sw.h +++ /dev/null @@ -1,57 +0,0 @@ -/*************************************************************************/ -/* collision_solver_3d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 COLLISION_SOLVER_SW_H -#define COLLISION_SOLVER_SW_H - -#include "shape_3d_sw.h" - -class CollisionSolver3DSW { -public: - typedef void (*CallbackResult)(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); - -private: - static bool soft_body_query_callback(uint32_t p_node_index, void *p_userdata); - static void soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); - static bool soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex); - static bool concave_callback(void *p_userdata, Shape3DSW *p_convex); - static bool solve_static_world_boundary(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); - static bool solve_separation_ray(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0); - static bool solve_soft_body(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); - static bool solve_concave(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A = 0, real_t p_margin_B = 0); - static bool concave_distance_callback(void *p_userdata, Shape3DSW *p_convex); - static bool solve_distance_world_boundary(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B); - -public: - static bool solve_static(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); - static bool solve_distance(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis = nullptr); -}; - -#endif // COLLISION_SOLVER__SW_H diff --git a/servers/physics_3d/constraint_3d_sw.h b/servers/physics_3d/constraint_3d_sw.h deleted file mode 100644 index 7b44726ef5..0000000000 --- a/servers/physics_3d/constraint_3d_sw.h +++ /dev/null @@ -1,81 +0,0 @@ -/*************************************************************************/ -/* constraint_3d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 CONSTRAINT_SW_H -#define CONSTRAINT_SW_H - -class Body3DSW; -class SoftBody3DSW; - -class Constraint3DSW { - Body3DSW **_body_ptr; - int _body_count; - uint64_t island_step; - int priority; - bool disabled_collisions_between_bodies; - - RID self; - -protected: - Constraint3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) { - _body_ptr = p_body_ptr; - _body_count = p_body_count; - island_step = 0; - priority = 1; - disabled_collisions_between_bodies = true; - } - -public: - _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } - _FORCE_INLINE_ RID get_self() const { return self; } - - _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } - _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } - - _FORCE_INLINE_ Body3DSW **get_body_ptr() const { return _body_ptr; } - _FORCE_INLINE_ int get_body_count() const { return _body_count; } - - virtual SoftBody3DSW *get_soft_body_ptr(int p_index) const { return nullptr; } - virtual int get_soft_body_count() const { return 0; } - - _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } - _FORCE_INLINE_ int get_priority() const { return priority; } - - _FORCE_INLINE_ void disable_collisions_between_bodies(const bool p_disabled) { disabled_collisions_between_bodies = p_disabled; } - _FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; } - - virtual bool setup(real_t p_step) = 0; - virtual bool pre_solve(real_t p_step) = 0; - virtual void solve(real_t p_step) = 0; - - virtual ~Constraint3DSW() {} -}; - -#endif // CONSTRAINT__SW_H diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp index a1dbdd0a70..ef6535a878 100644 --- a/servers/physics_3d/gjk_epa.cpp +++ b/servers/physics_3d/gjk_epa.cpp @@ -105,7 +105,7 @@ typedef unsigned char U1; // MinkowskiDiff struct MinkowskiDiff { - const Shape3DSW* m_shapes[2]; + const GodotShape3D* m_shapes[2]; Transform3D transform_A; Transform3D transform_B; @@ -113,10 +113,10 @@ struct MinkowskiDiff { real_t margin_A = 0.0; real_t margin_B = 0.0; - Vector3 (*get_support)(const Shape3DSW*, const Vector3&, real_t); + Vector3 (*get_support)(const GodotShape3D*, const Vector3&, real_t); - void Initialize(const Shape3DSW* shape0, const Transform3D& wtrs0, const real_t margin0, - const Shape3DSW* shape1, const Transform3D& wtrs1, const real_t margin1) { + void Initialize(const GodotShape3D* shape0, const Transform3D& wtrs0, const real_t margin0, + const GodotShape3D* shape1, const Transform3D& wtrs1, const real_t margin1) { m_shapes[0] = shape0; m_shapes[1] = shape1; transform_A = wtrs0; @@ -131,11 +131,11 @@ struct MinkowskiDiff { } } - static Vector3 get_support_without_margin(const Shape3DSW* p_shape, const Vector3& p_dir, real_t p_margin) { + static Vector3 get_support_without_margin(const GodotShape3D* p_shape, const Vector3& p_dir, real_t p_margin) { return p_shape->get_support(p_dir.normalized()); } - static Vector3 get_support_with_margin(const Shape3DSW* p_shape, const Vector3& p_dir, real_t p_margin) { + static Vector3 get_support_with_margin(const GodotShape3D* p_shape, const Vector3& p_dir, real_t p_margin) { Vector3 local_dir_norm = p_dir; if (local_dir_norm.length_squared() < CMP_EPSILON2) { local_dir_norm = Vector3(-1.0, -1.0, -1.0); @@ -862,8 +862,8 @@ struct GJK }; // - static void Initialize( const Shape3DSW* shape0, const Transform3D& wtrs0, real_t margin0, - const Shape3DSW* shape1, const Transform3D& wtrs1, real_t margin1, + static void Initialize( const GodotShape3D* shape0, const Transform3D& wtrs0, real_t margin0, + const GodotShape3D* shape1, const Transform3D& wtrs1, real_t margin1, sResults& results, tShape& shape) { @@ -884,10 +884,10 @@ struct GJK // // -bool Distance( const Shape3DSW* shape0, +bool Distance( const GodotShape3D* shape0, const Transform3D& wtrs0, real_t margin0, - const Shape3DSW* shape1, + const GodotShape3D* shape1, const Transform3D& wtrs1, real_t margin1, const Vector3& guess, @@ -925,10 +925,10 @@ bool Distance( const Shape3DSW* shape0, // -bool Penetration( const Shape3DSW* shape0, +bool Penetration( const GodotShape3D* shape0, const Transform3D& wtrs0, real_t margin0, - const Shape3DSW* shape1, + const GodotShape3D* shape1, const Transform3D& wtrs1, real_t margin1, const Vector3& guess, @@ -993,7 +993,7 @@ bool Penetration( const Shape3DSW* shape0, /* clang-format on */ -bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B) { +bool gjk_epa_calculate_distance(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B) { GjkEpa2::sResults res; if (GjkEpa2::Distance(p_shape_A, p_transform_A, 0.0, p_shape_B, p_transform_B, 0.0, p_transform_B.origin - p_transform_A.origin, res)) { @@ -1005,7 +1005,7 @@ bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform3D &p return false; } -bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, real_t p_margin_A, real_t p_margin_B) { +bool gjk_epa_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap, real_t p_margin_A, real_t p_margin_B) { GjkEpa2::sResults res; if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_margin_A, p_shape_B, p_transform_B, p_margin_B, p_transform_B.origin - p_transform_A.origin, res)) { diff --git a/servers/physics_3d/gjk_epa.h b/servers/physics_3d/gjk_epa.h index 69e85d2bc0..39a7d03435 100644 --- a/servers/physics_3d/gjk_epa.h +++ b/servers/physics_3d/gjk_epa.h @@ -31,10 +31,10 @@ #ifndef GJK_EPA_H #define GJK_EPA_H -#include "collision_solver_3d_sw.h" -#include "shape_3d_sw.h" +#include "godot_collision_solver_3d.h" +#include "godot_shape_3d.h" -bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, real_t p_margin_A = 0.0, real_t p_margin_B = 0.0); -bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B); +bool gjk_epa_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, real_t p_margin_A = 0.0, real_t p_margin_B = 0.0); +bool gjk_epa_calculate_distance(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B); #endif diff --git a/servers/physics_3d/godot_area_3d.cpp b/servers/physics_3d/godot_area_3d.cpp new file mode 100644 index 0000000000..e115e17061 --- /dev/null +++ b/servers/physics_3d/godot_area_3d.cpp @@ -0,0 +1,337 @@ +/*************************************************************************/ +/* godot_area_3d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_area_3d.h" + +#include "godot_body_3d.h" +#include "godot_soft_body_3d.h" +#include "godot_space_3d.h" + +GodotArea3D::BodyKey::BodyKey(GodotSoftBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + rid = p_body->get_self(); + instance_id = p_body->get_instance_id(); + body_shape = p_body_shape; + area_shape = p_area_shape; +} + +GodotArea3D::BodyKey::BodyKey(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + rid = p_body->get_self(); + instance_id = p_body->get_instance_id(); + body_shape = p_body_shape; + area_shape = p_area_shape; +} + +GodotArea3D::BodyKey::BodyKey(GodotArea3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + rid = p_body->get_self(); + instance_id = p_body->get_instance_id(); + body_shape = p_body_shape; + area_shape = p_area_shape; +} + +void GodotArea3D::_shapes_changed() { + if (!moved_list.in_list() && get_space()) { + get_space()->area_add_to_moved_list(&moved_list); + } +} + +void GodotArea3D::set_transform(const Transform3D &p_transform) { + if (!moved_list.in_list() && get_space()) { + get_space()->area_add_to_moved_list(&moved_list); + } + + _set_transform(p_transform); + _set_inv_transform(p_transform.affine_inverse()); +} + +void GodotArea3D::set_space(GodotSpace3D *p_space) { + if (get_space()) { + if (monitor_query_list.in_list()) { + get_space()->area_remove_from_monitor_query_list(&monitor_query_list); + } + if (moved_list.in_list()) { + get_space()->area_remove_from_moved_list(&moved_list); + } + } + + monitored_bodies.clear(); + monitored_areas.clear(); + + _set_space(p_space); +} + +void GodotArea3D::set_monitor_callback(ObjectID p_id, const StringName &p_method) { + if (p_id == monitor_callback_id) { + monitor_callback_method = p_method; + return; + } + + _unregister_shapes(); + + monitor_callback_id = p_id; + monitor_callback_method = p_method; + + monitored_bodies.clear(); + monitored_areas.clear(); + + _shape_changed(); + + if (!moved_list.in_list() && get_space()) { + get_space()->area_add_to_moved_list(&moved_list); + } +} + +void GodotArea3D::set_area_monitor_callback(ObjectID p_id, const StringName &p_method) { + if (p_id == area_monitor_callback_id) { + area_monitor_callback_method = p_method; + return; + } + + _unregister_shapes(); + + area_monitor_callback_id = p_id; + area_monitor_callback_method = p_method; + + monitored_bodies.clear(); + monitored_areas.clear(); + + _shape_changed(); + + if (!moved_list.in_list() && get_space()) { + get_space()->area_add_to_moved_list(&moved_list); + } +} + +void GodotArea3D::set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode p_mode) { + bool do_override = p_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; + if (do_override == (space_override_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED)) { + return; + } + _unregister_shapes(); + space_override_mode = p_mode; + _shape_changed(); +} + +void GodotArea3D::set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value) { + switch (p_param) { + case PhysicsServer3D::AREA_PARAM_GRAVITY: + gravity = p_value; + break; + case PhysicsServer3D::AREA_PARAM_GRAVITY_VECTOR: + gravity_vector = p_value; + break; + case PhysicsServer3D::AREA_PARAM_GRAVITY_IS_POINT: + gravity_is_point = p_value; + break; + case PhysicsServer3D::AREA_PARAM_GRAVITY_DISTANCE_SCALE: + gravity_distance_scale = p_value; + break; + case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_ATTENUATION: + point_attenuation = p_value; + break; + case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP: + linear_damp = p_value; + break; + case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP: + angular_damp = p_value; + break; + case PhysicsServer3D::AREA_PARAM_PRIORITY: + priority = p_value; + break; + case PhysicsServer3D::AREA_PARAM_WIND_FORCE_MAGNITUDE: + ERR_FAIL_COND_MSG(wind_force_magnitude < 0, "Wind force magnitude must be a non-negative real number, but a negative number was specified."); + wind_force_magnitude = p_value; + break; + case PhysicsServer3D::AREA_PARAM_WIND_SOURCE: + wind_source = p_value; + break; + case PhysicsServer3D::AREA_PARAM_WIND_DIRECTION: + wind_direction = p_value; + break; + case PhysicsServer3D::AREA_PARAM_WIND_ATTENUATION_FACTOR: + ERR_FAIL_COND_MSG(wind_attenuation_factor < 0, "Wind attenuation factor must be a non-negative real number, but a negative number was specified."); + wind_attenuation_factor = p_value; + break; + } +} + +Variant GodotArea3D::get_param(PhysicsServer3D::AreaParameter p_param) const { + switch (p_param) { + case PhysicsServer3D::AREA_PARAM_GRAVITY: + return gravity; + case PhysicsServer3D::AREA_PARAM_GRAVITY_VECTOR: + return gravity_vector; + case PhysicsServer3D::AREA_PARAM_GRAVITY_IS_POINT: + return gravity_is_point; + case PhysicsServer3D::AREA_PARAM_GRAVITY_DISTANCE_SCALE: + return gravity_distance_scale; + case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_ATTENUATION: + return point_attenuation; + case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP: + return linear_damp; + case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP: + return angular_damp; + case PhysicsServer3D::AREA_PARAM_PRIORITY: + return priority; + case PhysicsServer3D::AREA_PARAM_WIND_FORCE_MAGNITUDE: + return wind_force_magnitude; + case PhysicsServer3D::AREA_PARAM_WIND_SOURCE: + return wind_source; + case PhysicsServer3D::AREA_PARAM_WIND_DIRECTION: + return wind_direction; + case PhysicsServer3D::AREA_PARAM_WIND_ATTENUATION_FACTOR: + return wind_attenuation_factor; + } + + return Variant(); +} + +void GodotArea3D::_queue_monitor_update() { + ERR_FAIL_COND(!get_space()); + + if (!monitor_query_list.in_list()) { + get_space()->area_add_to_monitor_query_list(&monitor_query_list); + } +} + +void GodotArea3D::set_monitorable(bool p_monitorable) { + if (monitorable == p_monitorable) { + return; + } + + monitorable = p_monitorable; + _set_static(!monitorable); +} + +void GodotArea3D::call_queries() { + if (monitor_callback_id.is_valid() && !monitored_bodies.is_empty()) { + Variant res[5]; + Variant *resptr[5]; + for (int i = 0; i < 5; i++) { + resptr[i] = &res[i]; + } + + Object *obj = ObjectDB::get_instance(monitor_callback_id); + if (!obj) { + monitored_bodies.clear(); + monitor_callback_id = ObjectID(); + return; + } + + for (Map::Element *E = monitored_bodies.front(); E;) { + if (E->get().state == 0) { // Nothing happened + Map::Element *next = E->next(); + monitored_bodies.erase(E); + E = next; + continue; + } + + res[0] = E->get().state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED; + res[1] = E->key().rid; + res[2] = E->key().instance_id; + res[3] = E->key().body_shape; + res[4] = E->key().area_shape; + + Map::Element *next = E->next(); + monitored_bodies.erase(E); + E = next; + + Callable::CallError ce; + obj->call(monitor_callback_method, (const Variant **)resptr, 5, ce); + } + } + + if (area_monitor_callback_id.is_valid() && !monitored_areas.is_empty()) { + Variant res[5]; + Variant *resptr[5]; + for (int i = 0; i < 5; i++) { + resptr[i] = &res[i]; + } + + Object *obj = ObjectDB::get_instance(area_monitor_callback_id); + if (!obj) { + monitored_areas.clear(); + area_monitor_callback_id = ObjectID(); + return; + } + + for (Map::Element *E = monitored_areas.front(); E;) { + if (E->get().state == 0) { // Nothing happened + Map::Element *next = E->next(); + monitored_areas.erase(E); + E = next; + continue; + } + + res[0] = E->get().state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED; + res[1] = E->key().rid; + res[2] = E->key().instance_id; + res[3] = E->key().body_shape; + res[4] = E->key().area_shape; + + Map::Element *next = E->next(); + monitored_areas.erase(E); + E = next; + + Callable::CallError ce; + obj->call(area_monitor_callback_method, (const Variant **)resptr, 5, ce); + } + } +} + +void GodotArea3D::compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const { + if (is_gravity_point()) { + const real_t gravity_distance_scale = get_gravity_distance_scale(); + Vector3 v = get_transform().xform(get_gravity_vector()) - p_position; + if (gravity_distance_scale > 0) { + const real_t v_length = v.length(); + if (v_length > 0) { + const real_t v_scaled = v_length * gravity_distance_scale; + r_gravity = (v.normalized() * (get_gravity() / (v_scaled * v_scaled))); + } else { + r_gravity = Vector3(); + } + } else { + r_gravity = v.normalized() * get_gravity(); + } + } else { + r_gravity = get_gravity_vector() * get_gravity(); + } +} + +GodotArea3D::GodotArea3D() : + GodotCollisionObject3D(TYPE_AREA), + monitor_query_list(this), + moved_list(this) { + _set_static(true); //areas are never active + set_ray_pickable(false); +} + +GodotArea3D::~GodotArea3D() { +} diff --git a/servers/physics_3d/godot_area_3d.h b/servers/physics_3d/godot_area_3d.h new file mode 100644 index 0000000000..e8caa9221b --- /dev/null +++ b/servers/physics_3d/godot_area_3d.h @@ -0,0 +1,246 @@ +/*************************************************************************/ +/* godot_area_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_AREA_3D_H +#define GODOT_AREA_3D_H + +#include "godot_collision_object_3d.h" + +#include "core/templates/self_list.h" +#include "servers/physics_server_3d.h" + +class GodotSpace3D; +class GodotBody3D; +class GodotSoftBody3D; +class GodotConstraint3D; + +class GodotArea3D : public GodotCollisionObject3D { + PhysicsServer3D::AreaSpaceOverrideMode space_override_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; + real_t gravity = 9.80665; + Vector3 gravity_vector = Vector3(0, -1, 0); + bool gravity_is_point = false; + real_t gravity_distance_scale = 0.0; + real_t point_attenuation = 1.0; + real_t linear_damp = 0.1; + real_t angular_damp = 0.1; + real_t wind_force_magnitude = 0.0; + real_t wind_attenuation_factor = 0.0; + Vector3 wind_source; + Vector3 wind_direction; + int priority = 0; + bool monitorable = false; + + ObjectID monitor_callback_id; + StringName monitor_callback_method; + + ObjectID area_monitor_callback_id; + StringName area_monitor_callback_method; + + SelfList monitor_query_list; + SelfList moved_list; + + struct BodyKey { + RID rid; + ObjectID instance_id; + uint32_t body_shape = 0; + uint32_t area_shape = 0; + + _FORCE_INLINE_ bool operator<(const BodyKey &p_key) const { + if (rid == p_key.rid) { + if (body_shape == p_key.body_shape) { + return area_shape < p_key.area_shape; + } else { + return body_shape < p_key.body_shape; + } + } else { + return rid < p_key.rid; + } + } + + _FORCE_INLINE_ BodyKey() {} + BodyKey(GodotSoftBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + BodyKey(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + BodyKey(GodotArea3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + }; + + struct BodyState { + int state = 0; + _FORCE_INLINE_ void inc() { state++; } + _FORCE_INLINE_ void dec() { state--; } + }; + + Map monitored_soft_bodies; + Map monitored_bodies; + Map monitored_areas; + + Set constraints; + + virtual void _shapes_changed(); + void _queue_monitor_update(); + +public: + void set_monitor_callback(ObjectID p_id, const StringName &p_method); + _FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id.is_valid(); } + + void set_area_monitor_callback(ObjectID p_id, const StringName &p_method); + _FORCE_INLINE_ bool has_area_monitor_callback() const { return area_monitor_callback_id.is_valid(); } + + _FORCE_INLINE_ void add_body_to_query(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + _FORCE_INLINE_ void remove_body_from_query(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + + _FORCE_INLINE_ void add_soft_body_to_query(GodotSoftBody3D *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape); + _FORCE_INLINE_ void remove_soft_body_from_query(GodotSoftBody3D *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape); + + _FORCE_INLINE_ void add_area_to_query(GodotArea3D *p_area, uint32_t p_area_shape, uint32_t p_self_shape); + _FORCE_INLINE_ void remove_area_from_query(GodotArea3D *p_area, uint32_t p_area_shape, uint32_t p_self_shape); + + void set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value); + Variant get_param(PhysicsServer3D::AreaParameter p_param) const; + + void set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode p_mode); + PhysicsServer3D::AreaSpaceOverrideMode get_space_override_mode() const { return space_override_mode; } + + _FORCE_INLINE_ void set_gravity(real_t p_gravity) { gravity = p_gravity; } + _FORCE_INLINE_ real_t get_gravity() const { return gravity; } + + _FORCE_INLINE_ void set_gravity_vector(const Vector3 &p_gravity) { gravity_vector = p_gravity; } + _FORCE_INLINE_ Vector3 get_gravity_vector() const { return gravity_vector; } + + _FORCE_INLINE_ void set_gravity_as_point(bool p_enable) { gravity_is_point = p_enable; } + _FORCE_INLINE_ bool is_gravity_point() const { return gravity_is_point; } + + _FORCE_INLINE_ void set_gravity_distance_scale(real_t scale) { gravity_distance_scale = scale; } + _FORCE_INLINE_ real_t get_gravity_distance_scale() const { return gravity_distance_scale; } + + _FORCE_INLINE_ void set_point_attenuation(real_t p_point_attenuation) { point_attenuation = p_point_attenuation; } + _FORCE_INLINE_ real_t get_point_attenuation() const { return point_attenuation; } + + _FORCE_INLINE_ void set_linear_damp(real_t p_linear_damp) { linear_damp = p_linear_damp; } + _FORCE_INLINE_ real_t get_linear_damp() const { return linear_damp; } + + _FORCE_INLINE_ void set_angular_damp(real_t p_angular_damp) { angular_damp = p_angular_damp; } + _FORCE_INLINE_ real_t get_angular_damp() const { return angular_damp; } + + _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } + _FORCE_INLINE_ int get_priority() const { return priority; } + + _FORCE_INLINE_ void set_wind_force_magnitude(real_t p_wind_force_magnitude) { wind_force_magnitude = p_wind_force_magnitude; } + _FORCE_INLINE_ real_t get_wind_force_magnitude() const { return wind_force_magnitude; } + + _FORCE_INLINE_ void set_wind_attenuation_factor(real_t p_wind_attenuation_factor) { wind_attenuation_factor = p_wind_attenuation_factor; } + _FORCE_INLINE_ real_t get_wind_attenuation_factor() const { return wind_attenuation_factor; } + + _FORCE_INLINE_ void set_wind_source(const Vector3 &p_wind_source) { wind_source = p_wind_source; } + _FORCE_INLINE_ const Vector3 &get_wind_source() const { return wind_source; } + + _FORCE_INLINE_ void set_wind_direction(const Vector3 &p_wind_direction) { wind_direction = p_wind_direction; } + _FORCE_INLINE_ const Vector3 &get_wind_direction() const { return wind_direction; } + + _FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint) { constraints.insert(p_constraint); } + _FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraints.erase(p_constraint); } + _FORCE_INLINE_ const Set &get_constraints() const { return constraints; } + _FORCE_INLINE_ void clear_constraints() { constraints.clear(); } + + void set_monitorable(bool p_monitorable); + _FORCE_INLINE_ bool is_monitorable() const { return monitorable; } + + void set_transform(const Transform3D &p_transform); + + void set_space(GodotSpace3D *p_space); + + void call_queries(); + + void compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const; + + GodotArea3D(); + ~GodotArea3D(); +}; + +void GodotArea3D::add_soft_body_to_query(GodotSoftBody3D *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape) { + BodyKey bk(p_soft_body, p_soft_body_shape, p_area_shape); + monitored_soft_bodies[bk].inc(); + if (!monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +void GodotArea3D::remove_soft_body_from_query(GodotSoftBody3D *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape) { + BodyKey bk(p_soft_body, p_soft_body_shape, p_area_shape); + monitored_soft_bodies[bk].dec(); + if (!monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +void GodotArea3D::add_body_to_query(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + BodyKey bk(p_body, p_body_shape, p_area_shape); + monitored_bodies[bk].inc(); + if (!monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +void GodotArea3D::remove_body_from_query(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + BodyKey bk(p_body, p_body_shape, p_area_shape); + monitored_bodies[bk].dec(); + if (!monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +void GodotArea3D::add_area_to_query(GodotArea3D *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { + BodyKey bk(p_area, p_area_shape, p_self_shape); + monitored_areas[bk].inc(); + if (!monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +void GodotArea3D::remove_area_from_query(GodotArea3D *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { + BodyKey bk(p_area, p_area_shape, p_self_shape); + monitored_areas[bk].dec(); + if (!monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +struct AreaCMP { + GodotArea3D *area = nullptr; + int refCount = 0; + _FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); } + _FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); } + _FORCE_INLINE_ AreaCMP() {} + _FORCE_INLINE_ AreaCMP(GodotArea3D *p_area) { + area = p_area; + refCount = 1; + } +}; + +#endif // GODOT_AREA_3D_H diff --git a/servers/physics_3d/godot_area_pair_3d.cpp b/servers/physics_3d/godot_area_pair_3d.cpp new file mode 100644 index 0000000000..7453153de6 --- /dev/null +++ b/servers/physics_3d/godot_area_pair_3d.cpp @@ -0,0 +1,273 @@ +/*************************************************************************/ +/* godot_area_pair_3d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_area_pair_3d.h" + +#include "godot_collision_solver_3d.h" + +bool GodotAreaPair3D::setup(real_t p_step) { + bool result = false; + if (area->collides_with(body) && GodotCollisionSolver3D::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), nullptr, this)) { + result = true; + } + + process_collision = false; + if (result != colliding) { + if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + process_collision = true; + } else if (area->has_monitor_callback()) { + process_collision = true; + } + + colliding = result; + } + + return process_collision; +} + +bool GodotAreaPair3D::pre_solve(real_t p_step) { + if (!process_collision) { + return false; + } + + if (colliding) { + if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + body->add_area(area); + } + + if (area->has_monitor_callback()) { + area->add_body_to_query(body, body_shape, area_shape); + } + } else { + if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + body->remove_area(area); + } + + if (area->has_monitor_callback()) { + area->remove_body_from_query(body, body_shape, area_shape); + } + } + + return false; // Never do any post solving. +} + +void GodotAreaPair3D::solve(real_t p_step) { + // Nothing to do. +} + +GodotAreaPair3D::GodotAreaPair3D(GodotBody3D *p_body, int p_body_shape, GodotArea3D *p_area, int p_area_shape) { + body = p_body; + area = p_area; + body_shape = p_body_shape; + area_shape = p_area_shape; + body->add_constraint(this, 0); + area->add_constraint(this); + if (p_body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { + p_body->set_active(true); + } +} + +GodotAreaPair3D::~GodotAreaPair3D() { + if (colliding) { + if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + body->remove_area(area); + } + if (area->has_monitor_callback()) { + area->remove_body_from_query(body, body_shape, area_shape); + } + } + body->remove_constraint(this); + area->remove_constraint(this); +} + +//////////////////////////////////////////////////// + +bool GodotArea2Pair3D::setup(real_t p_step) { + bool result_a = area_a->collides_with(area_b); + bool result_b = area_b->collides_with(area_a); + if ((result_a || result_b) && !GodotCollisionSolver3D::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), nullptr, this)) { + result_a = false; + result_b = false; + } + + bool process_collision = false; + + process_collision_a = false; + if (result_a != colliding_a) { + if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { + process_collision_a = true; + process_collision = true; + } + colliding_a = result_a; + } + + process_collision_b = false; + if (result_b != colliding_b) { + if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { + process_collision_b = true; + process_collision = true; + } + colliding_b = result_b; + } + + return process_collision; +} + +bool GodotArea2Pair3D::pre_solve(real_t p_step) { + if (process_collision_a) { + if (colliding_a) { + area_a->add_area_to_query(area_b, shape_b, shape_a); + } else { + area_a->remove_area_from_query(area_b, shape_b, shape_a); + } + } + + if (process_collision_b) { + if (colliding_b) { + area_b->add_area_to_query(area_a, shape_a, shape_b); + } else { + area_b->remove_area_from_query(area_a, shape_a, shape_b); + } + } + + return false; // Never do any post solving. +} + +void GodotArea2Pair3D::solve(real_t p_step) { + // Nothing to do. +} + +GodotArea2Pair3D::GodotArea2Pair3D(GodotArea3D *p_area_a, int p_shape_a, GodotArea3D *p_area_b, int p_shape_b) { + area_a = p_area_a; + area_b = p_area_b; + shape_a = p_shape_a; + shape_b = p_shape_b; + area_a->add_constraint(this); + area_b->add_constraint(this); +} + +GodotArea2Pair3D::~GodotArea2Pair3D() { + if (colliding_a) { + if (area_a->has_area_monitor_callback()) { + area_a->remove_area_from_query(area_b, shape_b, shape_a); + } + } + + if (colliding_b) { + if (area_b->has_area_monitor_callback()) { + area_b->remove_area_from_query(area_a, shape_a, shape_b); + } + } + + area_a->remove_constraint(this); + area_b->remove_constraint(this); +} + +//////////////////////////////////////////////////// + +bool GodotAreaSoftBodyPair3D::setup(real_t p_step) { + bool result = false; + if ( + area->collides_with(soft_body) && + GodotCollisionSolver3D::solve_static( + soft_body->get_shape(soft_body_shape), + soft_body->get_transform() * soft_body->get_shape_transform(soft_body_shape), + area->get_shape(area_shape), + area->get_transform() * area->get_shape_transform(area_shape), + nullptr, + this)) { + result = true; + } + + process_collision = false; + if (result != colliding) { + if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + process_collision = true; + } else if (area->has_monitor_callback()) { + process_collision = true; + } + + colliding = result; + } + + return process_collision; +} + +bool GodotAreaSoftBodyPair3D::pre_solve(real_t p_step) { + if (!process_collision) { + return false; + } + + if (colliding) { + if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + soft_body->add_area(area); + } + + if (area->has_monitor_callback()) { + area->add_soft_body_to_query(soft_body, soft_body_shape, area_shape); + } + } else { + if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + soft_body->remove_area(area); + } + + if (area->has_monitor_callback()) { + area->remove_soft_body_from_query(soft_body, soft_body_shape, area_shape); + } + } + + return false; // Never do any post solving. +} + +void GodotAreaSoftBodyPair3D::solve(real_t p_step) { + // Nothing to do. +} + +GodotAreaSoftBodyPair3D::GodotAreaSoftBodyPair3D(GodotSoftBody3D *p_soft_body, int p_soft_body_shape, GodotArea3D *p_area, int p_area_shape) { + soft_body = p_soft_body; + area = p_area; + soft_body_shape = p_soft_body_shape; + area_shape = p_area_shape; + soft_body->add_constraint(this); + area->add_constraint(this); +} + +GodotAreaSoftBodyPair3D::~GodotAreaSoftBodyPair3D() { + if (colliding) { + if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + soft_body->remove_area(area); + } + if (area->has_monitor_callback()) { + area->remove_soft_body_from_query(soft_body, soft_body_shape, area_shape); + } + } + soft_body->remove_constraint(this); + area->remove_constraint(this); +} diff --git a/servers/physics_3d/godot_area_pair_3d.h b/servers/physics_3d/godot_area_pair_3d.h new file mode 100644 index 0000000000..f55c03be03 --- /dev/null +++ b/servers/physics_3d/godot_area_pair_3d.h @@ -0,0 +1,92 @@ +/*************************************************************************/ +/* godot_area_pair_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_AREA_PAIR_3D_H +#define GODOT_AREA_PAIR_3D_H + +#include "godot_area_3d.h" +#include "godot_body_3d.h" +#include "godot_constraint_3d.h" +#include "godot_soft_body_3d.h" + +class GodotAreaPair3D : public GodotConstraint3D { + GodotBody3D *body; + GodotArea3D *area; + int body_shape; + int area_shape; + bool colliding = false; + bool process_collision = false; + +public: + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + GodotAreaPair3D(GodotBody3D *p_body, int p_body_shape, GodotArea3D *p_area, int p_area_shape); + ~GodotAreaPair3D(); +}; + +class GodotArea2Pair3D : public GodotConstraint3D { + GodotArea3D *area_a; + GodotArea3D *area_b; + int shape_a; + int shape_b; + bool colliding_a = false; + bool colliding_b = false; + bool process_collision_a = false; + bool process_collision_b = false; + +public: + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + GodotArea2Pair3D(GodotArea3D *p_area_a, int p_shape_a, GodotArea3D *p_area_b, int p_shape_b); + ~GodotArea2Pair3D(); +}; + +class GodotAreaSoftBodyPair3D : public GodotConstraint3D { + GodotSoftBody3D *soft_body; + GodotArea3D *area; + int soft_body_shape; + int area_shape; + bool colliding = false; + bool process_collision = false; + +public: + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + GodotAreaSoftBodyPair3D(GodotSoftBody3D *p_sof_body, int p_soft_body_shape, GodotArea3D *p_area, int p_area_shape); + ~GodotAreaSoftBodyPair3D(); +}; + +#endif // GODOT_AREA_PAIR_3D_H diff --git a/servers/physics_3d/godot_body_3d.cpp b/servers/physics_3d/godot_body_3d.cpp new file mode 100644 index 0000000000..6df6a0c45b --- /dev/null +++ b/servers/physics_3d/godot_body_3d.cpp @@ -0,0 +1,757 @@ +/*************************************************************************/ +/* godot_body_3d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_body_3d.h" + +#include "godot_area_3d.h" +#include "godot_body_direct_state_3d.h" +#include "godot_space_3d.h" + +void GodotBody3D::_mass_properties_changed() { + if (get_space() && !mass_properties_update_list.in_list() && (calculate_inertia || calculate_center_of_mass)) { + get_space()->body_add_to_mass_properties_update_list(&mass_properties_update_list); + } +} + +void GodotBody3D::_update_transform_dependant() { + center_of_mass = get_transform().basis.xform(center_of_mass_local); + principal_inertia_axes = get_transform().basis * principal_inertia_axes_local; + + // Update inertia tensor. + Basis tb = principal_inertia_axes; + Basis tbt = tb.transposed(); + Basis diag; + diag.scale(_inv_inertia); + _inv_inertia_tensor = tb * diag * tbt; +} + +void GodotBody3D::update_mass_properties() { + // Update shapes and motions. + + switch (mode) { + case PhysicsServer3D::BODY_MODE_DYNAMIC: { + real_t total_area = 0; + for (int i = 0; i < get_shape_count(); i++) { + if (is_shape_disabled(i)) { + continue; + } + + total_area += get_shape_area(i); + } + + if (calculate_center_of_mass) { + // We have to recompute the center of mass. + center_of_mass_local.zero(); + + if (total_area != 0.0) { + for (int i = 0; i < get_shape_count(); i++) { + if (is_shape_disabled(i)) { + continue; + } + + real_t area = get_shape_area(i); + + real_t mass = area * this->mass / total_area; + + // NOTE: we assume that the shape origin is also its center of mass. + center_of_mass_local += mass * get_shape_transform(i).origin; + } + + center_of_mass_local /= mass; + } + } + + if (calculate_inertia) { + // Recompute the inertia tensor. + Basis inertia_tensor; + inertia_tensor.set_zero(); + bool inertia_set = false; + + for (int i = 0; i < get_shape_count(); i++) { + if (is_shape_disabled(i)) { + continue; + } + + real_t area = get_shape_area(i); + if (area == 0.0) { + continue; + } + + inertia_set = true; + + const GodotShape3D *shape = get_shape(i); + + real_t mass = area * this->mass / total_area; + + Basis shape_inertia_tensor = Basis::from_scale(shape->get_moment_of_inertia(mass)); + Transform3D shape_transform = get_shape_transform(i); + Basis shape_basis = shape_transform.basis.orthonormalized(); + + // NOTE: we don't take the scale of collision shapes into account when computing the inertia tensor! + shape_inertia_tensor = shape_basis * shape_inertia_tensor * shape_basis.transposed(); + + Vector3 shape_origin = shape_transform.origin - center_of_mass_local; + inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass; + } + + // Set the inertia to a valid value when there are no valid shapes. + if (!inertia_set) { + inertia_tensor = Basis(); + } + + // Handle partial custom inertia. + if (inertia.x > 0.0) { + inertia_tensor[0][0] = inertia.x; + } + if (inertia.y > 0.0) { + inertia_tensor[1][1] = inertia.y; + } + if (inertia.z > 0.0) { + inertia_tensor[2][2] = inertia.z; + } + + // Compute the principal axes of inertia. + principal_inertia_axes_local = inertia_tensor.diagonalize().transposed(); + _inv_inertia = inertia_tensor.get_main_diagonal().inverse(); + } + + if (mass) { + _inv_mass = 1.0 / mass; + } else { + _inv_mass = 0; + } + + } break; + case PhysicsServer3D::BODY_MODE_KINEMATIC: + case PhysicsServer3D::BODY_MODE_STATIC: { + _inv_inertia = Vector3(); + _inv_mass = 0; + } break; + case PhysicsServer3D::BODY_MODE_DYNAMIC_LINEAR: { + _inv_inertia_tensor.set_zero(); + _inv_mass = 1.0 / mass; + + } break; + } + + _update_transform_dependant(); +} + +void GodotBody3D::reset_mass_properties() { + calculate_inertia = true; + calculate_center_of_mass = true; + _mass_properties_changed(); +} + +void GodotBody3D::set_active(bool p_active) { + if (active == p_active) { + return; + } + + active = p_active; + + if (active) { + if (mode == PhysicsServer3D::BODY_MODE_STATIC) { + // Static bodies can't be active. + active = false; + } else if (get_space()) { + get_space()->body_add_to_active_list(&active_list); + } + } else if (get_space()) { + get_space()->body_remove_from_active_list(&active_list); + } +} + +void GodotBody3D::set_param(PhysicsServer3D::BodyParameter p_param, const Variant &p_value) { + switch (p_param) { + case PhysicsServer3D::BODY_PARAM_BOUNCE: { + bounce = p_value; + } break; + case PhysicsServer3D::BODY_PARAM_FRICTION: { + friction = p_value; + } break; + case PhysicsServer3D::BODY_PARAM_MASS: { + real_t mass_value = p_value; + ERR_FAIL_COND(mass_value <= 0); + mass = mass_value; + if (mode >= PhysicsServer3D::BODY_MODE_DYNAMIC) { + _mass_properties_changed(); + } + } break; + case PhysicsServer3D::BODY_PARAM_INERTIA: { + inertia = p_value; + if ((inertia.x <= 0.0) || (inertia.y <= 0.0) || (inertia.z <= 0.0)) { + calculate_inertia = true; + if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC) { + _mass_properties_changed(); + } + } else { + calculate_inertia = false; + if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC) { + principal_inertia_axes_local = Basis(); + _inv_inertia = inertia.inverse(); + _update_transform_dependant(); + } + } + } break; + case PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS: { + calculate_center_of_mass = false; + center_of_mass_local = p_value; + _update_transform_dependant(); + } break; + case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE: { + gravity_scale = p_value; + } break; + case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP: { + linear_damp = p_value; + } break; + case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP: { + angular_damp = p_value; + } break; + default: { + } + } +} + +Variant GodotBody3D::get_param(PhysicsServer3D::BodyParameter p_param) const { + switch (p_param) { + case PhysicsServer3D::BODY_PARAM_BOUNCE: { + return bounce; + } break; + case PhysicsServer3D::BODY_PARAM_FRICTION: { + return friction; + } break; + case PhysicsServer3D::BODY_PARAM_MASS: { + return mass; + } break; + case PhysicsServer3D::BODY_PARAM_INERTIA: { + if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC) { + return _inv_inertia.inverse(); + } else { + return Vector3(); + } + } break; + case PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS: { + return center_of_mass; + } break; + case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE: { + return gravity_scale; + } break; + case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP: { + return linear_damp; + } break; + case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP: { + return angular_damp; + } break; + + default: { + } + } + + return 0; +} + +void GodotBody3D::set_mode(PhysicsServer3D::BodyMode p_mode) { + PhysicsServer3D::BodyMode prev = mode; + mode = p_mode; + + switch (p_mode) { + case PhysicsServer3D::BODY_MODE_STATIC: + case PhysicsServer3D::BODY_MODE_KINEMATIC: { + _set_inv_transform(get_transform().affine_inverse()); + _inv_mass = 0; + _inv_inertia = Vector3(); + _set_static(p_mode == PhysicsServer3D::BODY_MODE_STATIC); + set_active(p_mode == PhysicsServer3D::BODY_MODE_KINEMATIC && contacts.size()); + linear_velocity = Vector3(); + angular_velocity = Vector3(); + if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC && prev != mode) { + first_time_kinematic = true; + } + _update_transform_dependant(); + + } break; + case PhysicsServer3D::BODY_MODE_DYNAMIC: { + _inv_mass = mass > 0 ? (1.0 / mass) : 0; + if (!calculate_inertia) { + principal_inertia_axes_local = Basis(); + _inv_inertia = inertia.inverse(); + _update_transform_dependant(); + } + _mass_properties_changed(); + _set_static(false); + set_active(true); + + } break; + case PhysicsServer3D::BODY_MODE_DYNAMIC_LINEAR: { + _inv_mass = mass > 0 ? (1.0 / mass) : 0; + _inv_inertia = Vector3(); + angular_velocity = Vector3(); + _update_transform_dependant(); + _set_static(false); + set_active(true); + } + } +} + +PhysicsServer3D::BodyMode GodotBody3D::get_mode() const { + return mode; +} + +void GodotBody3D::_shapes_changed() { + _mass_properties_changed(); +} + +void GodotBody3D::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) { + switch (p_state) { + case PhysicsServer3D::BODY_STATE_TRANSFORM: { + if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { + new_transform = p_variant; + //wakeup_neighbours(); + set_active(true); + if (first_time_kinematic) { + _set_transform(p_variant); + _set_inv_transform(get_transform().affine_inverse()); + first_time_kinematic = false; + } + + } else if (mode == PhysicsServer3D::BODY_MODE_STATIC) { + _set_transform(p_variant); + _set_inv_transform(get_transform().affine_inverse()); + wakeup_neighbours(); + } else { + Transform3D t = p_variant; + t.orthonormalize(); + new_transform = get_transform(); //used as old to compute motion + if (new_transform == t) { + break; + } + _set_transform(t); + _set_inv_transform(get_transform().inverse()); + } + wakeup(); + + } break; + case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { + linear_velocity = p_variant; + constant_linear_velocity = linear_velocity; + wakeup(); + } break; + case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { + angular_velocity = p_variant; + constant_angular_velocity = angular_velocity; + wakeup(); + + } break; + case PhysicsServer3D::BODY_STATE_SLEEPING: { + if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { + break; + } + bool do_sleep = p_variant; + if (do_sleep) { + linear_velocity = Vector3(); + //biased_linear_velocity=Vector3(); + angular_velocity = Vector3(); + //biased_angular_velocity=Vector3(); + set_active(false); + } else { + set_active(true); + } + } break; + case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { + can_sleep = p_variant; + if (mode >= PhysicsServer3D::BODY_MODE_DYNAMIC && !active && !can_sleep) { + set_active(true); + } + + } break; + } +} + +Variant GodotBody3D::get_state(PhysicsServer3D::BodyState p_state) const { + switch (p_state) { + case PhysicsServer3D::BODY_STATE_TRANSFORM: { + return get_transform(); + } break; + case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { + return linear_velocity; + } break; + case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { + return angular_velocity; + } break; + case PhysicsServer3D::BODY_STATE_SLEEPING: { + return !is_active(); + } break; + case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { + return can_sleep; + } break; + } + + return Variant(); +} + +void GodotBody3D::set_space(GodotSpace3D *p_space) { + if (get_space()) { + if (mass_properties_update_list.in_list()) { + get_space()->body_remove_from_mass_properties_update_list(&mass_properties_update_list); + } + if (active_list.in_list()) { + get_space()->body_remove_from_active_list(&active_list); + } + if (direct_state_query_list.in_list()) { + get_space()->body_remove_from_state_query_list(&direct_state_query_list); + } + } + + _set_space(p_space); + + if (get_space()) { + _mass_properties_changed(); + if (active) { + get_space()->body_add_to_active_list(&active_list); + } + } +} + +void GodotBody3D::_compute_area_gravity_and_damping(const GodotArea3D *p_area) { + Vector3 area_gravity; + p_area->compute_gravity(get_transform().get_origin(), area_gravity); + gravity += area_gravity; + + area_linear_damp += p_area->get_linear_damp(); + area_angular_damp += p_area->get_angular_damp(); +} + +void GodotBody3D::set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool lock) { + if (lock) { + locked_axis |= p_axis; + } else { + locked_axis &= ~p_axis; + } +} + +bool GodotBody3D::is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const { + return locked_axis & p_axis; +} + +void GodotBody3D::integrate_forces(real_t p_step) { + if (mode == PhysicsServer3D::BODY_MODE_STATIC) { + return; + } + + GodotArea3D *def_area = get_space()->get_default_area(); + + ERR_FAIL_COND(!def_area); + + int ac = areas.size(); + bool stopped = false; + gravity = Vector3(0, 0, 0); + area_linear_damp = 0; + area_angular_damp = 0; + if (ac) { + areas.sort(); + const AreaCMP *aa = &areas[0]; + // damp_area = aa[ac-1].area; + for (int i = ac - 1; i >= 0 && !stopped; i--) { + PhysicsServer3D::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode(); + switch (mode) { + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + _compute_area_gravity_and_damping(aa[i].area); + stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + } break; + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { + gravity = Vector3(0, 0, 0); + area_angular_damp = 0; + area_linear_damp = 0; + _compute_area_gravity_and_damping(aa[i].area); + stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE; + } break; + default: { + } + } + } + } + + if (!stopped) { + _compute_area_gravity_and_damping(def_area); + } + + gravity *= gravity_scale; + + // If less than 0, override dampenings with that of the Body + if (angular_damp >= 0) { + area_angular_damp = angular_damp; + } + /* + else + area_angular_damp=damp_area->get_angular_damp(); + */ + + if (linear_damp >= 0) { + area_linear_damp = linear_damp; + } + /* + else + area_linear_damp=damp_area->get_linear_damp(); + */ + + Vector3 motion; + bool do_motion = false; + + if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { + //compute motion, angular and etc. velocities from prev transform + motion = new_transform.origin - get_transform().origin; + do_motion = true; + linear_velocity = constant_linear_velocity + motion / p_step; + + //compute a FAKE angular velocity, not so easy + Basis rot = new_transform.basis.orthonormalized() * get_transform().basis.orthonormalized().transposed(); + Vector3 axis; + real_t angle; + + rot.get_axis_angle(axis, angle); + axis.normalize(); + angular_velocity = constant_angular_velocity + axis * (angle / p_step); + } else { + if (!omit_force_integration) { + //overridden by direct state query + + Vector3 force = gravity * mass; + force += applied_force; + Vector3 torque = applied_torque; + + real_t damp = 1.0 - p_step * area_linear_damp; + + if (damp < 0) { // reached zero in the given time + damp = 0; + } + + real_t angular_damp = 1.0 - p_step * area_angular_damp; + + if (angular_damp < 0) { // reached zero in the given time + angular_damp = 0; + } + + linear_velocity *= damp; + angular_velocity *= angular_damp; + + linear_velocity += _inv_mass * force * p_step; + angular_velocity += _inv_inertia_tensor.xform(torque) * p_step; + } + + if (continuous_cd) { + motion = linear_velocity * p_step; + do_motion = true; + } + } + + applied_force = Vector3(); + applied_torque = Vector3(); + + //motion=linear_velocity*p_step; + + biased_angular_velocity = Vector3(); + biased_linear_velocity = Vector3(); + + if (do_motion) { //shapes temporarily extend for raycast + _update_shapes_with_motion(motion); + } + + def_area = nullptr; // clear the area, so it is set in the next frame + contact_count = 0; +} + +void GodotBody3D::integrate_velocities(real_t p_step) { + if (mode == PhysicsServer3D::BODY_MODE_STATIC) { + return; + } + + if (fi_callback_data || body_state_callback) { + get_space()->body_add_to_state_query_list(&direct_state_query_list); + } + + //apply axis lock linear + for (int i = 0; i < 3; i++) { + if (is_axis_locked((PhysicsServer3D::BodyAxis)(1 << i))) { + linear_velocity[i] = 0; + biased_linear_velocity[i] = 0; + new_transform.origin[i] = get_transform().origin[i]; + } + } + //apply axis lock angular + for (int i = 0; i < 3; i++) { + if (is_axis_locked((PhysicsServer3D::BodyAxis)(1 << (i + 3)))) { + angular_velocity[i] = 0; + biased_angular_velocity[i] = 0; + } + } + + if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { + _set_transform(new_transform, false); + _set_inv_transform(new_transform.affine_inverse()); + if (contacts.size() == 0 && linear_velocity == Vector3() && angular_velocity == Vector3()) { + set_active(false); //stopped moving, deactivate + } + + return; + } + + Vector3 total_angular_velocity = angular_velocity + biased_angular_velocity; + + real_t ang_vel = total_angular_velocity.length(); + Transform3D transform = get_transform(); + + if (!Math::is_zero_approx(ang_vel)) { + Vector3 ang_vel_axis = total_angular_velocity / ang_vel; + Basis rot(ang_vel_axis, ang_vel * p_step); + Basis identity3(1, 0, 0, 0, 1, 0, 0, 0, 1); + transform.origin += ((identity3 - rot) * transform.basis).xform(center_of_mass_local); + transform.basis = rot * transform.basis; + transform.orthonormalize(); + } + + Vector3 total_linear_velocity = linear_velocity + biased_linear_velocity; + /*for(int i=0;i<3;i++) { + if (axis_lock&(1< &E : constraint_map) { + const GodotConstraint3D *c = E.key; + GodotBody3D **n = c->get_body_ptr(); + int bc = c->get_body_count(); + + for (int i = 0; i < bc; i++) { + if (i == E.value) { + continue; + } + GodotBody3D *b = n[i]; + if (b->mode < PhysicsServer3D::BODY_MODE_DYNAMIC) { + continue; + } + + if (!b->is_active()) { + b->set_active(true); + } + } + } +} + +void GodotBody3D::call_queries() { + if (fi_callback_data) { + if (!fi_callback_data->callable.get_object()) { + set_force_integration_callback(Callable()); + } else { + Variant direct_state_variant = get_direct_state(); + const Variant *vp[2] = { &direct_state_variant, &fi_callback_data->udata }; + + Callable::CallError ce; + int argc = (fi_callback_data->udata.get_type() == Variant::NIL) ? 1 : 2; + Variant rv; + fi_callback_data->callable.call(vp, argc, rv, ce); + } + } + + if (body_state_callback_instance) { + (body_state_callback)(body_state_callback_instance, get_direct_state()); + } +} + +bool GodotBody3D::sleep_test(real_t p_step) { + if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { + return true; + } else if (!can_sleep) { + return false; + } + + if (Math::abs(angular_velocity.length()) < get_space()->get_body_angular_velocity_sleep_threshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_threshold() * get_space()->get_body_linear_velocity_sleep_threshold()) { + still_time += p_step; + + return still_time > get_space()->get_body_time_to_sleep(); + } else { + still_time = 0; //maybe this should be set to 0 on set_active? + return false; + } +} + +void GodotBody3D::set_state_sync_callback(void *p_instance, PhysicsServer3D::BodyStateCallback p_callback) { + body_state_callback_instance = p_instance; + body_state_callback = p_callback; +} + +void GodotBody3D::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) { + if (p_callable.get_object()) { + if (!fi_callback_data) { + fi_callback_data = memnew(ForceIntegrationCallbackData); + } + fi_callback_data->callable = p_callable; + fi_callback_data->udata = p_udata; + } else if (fi_callback_data) { + memdelete(fi_callback_data); + fi_callback_data = nullptr; + } +} + +GodotPhysicsDirectBodyState3D *GodotBody3D::get_direct_state() { + if (!direct_state) { + direct_state = memnew(GodotPhysicsDirectBodyState3D); + direct_state->body = this; + } + return direct_state; +} + +GodotBody3D::GodotBody3D() : + GodotCollisionObject3D(TYPE_BODY), + active_list(this), + mass_properties_update_list(this), + direct_state_query_list(this) { + _set_static(false); +} + +GodotBody3D::~GodotBody3D() { + if (fi_callback_data) { + memdelete(fi_callback_data); + } + if (direct_state) { + memdelete(direct_state); + } +} diff --git a/servers/physics_3d/godot_body_3d.h b/servers/physics_3d/godot_body_3d.h new file mode 100644 index 0000000000..1151a22c96 --- /dev/null +++ b/servers/physics_3d/godot_body_3d.h @@ -0,0 +1,369 @@ +/*************************************************************************/ +/* godot_body_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_BODY_3D_H +#define GODOT_BODY_3D_H + +#include "godot_area_3d.h" +#include "godot_collision_object_3d.h" + +#include "core/templates/vset.h" + +class GodotConstraint3D; +class GodotPhysicsDirectBodyState3D; + +class GodotBody3D : public GodotCollisionObject3D { + PhysicsServer3D::BodyMode mode = PhysicsServer3D::BODY_MODE_DYNAMIC; + + Vector3 linear_velocity; + Vector3 angular_velocity; + + Vector3 constant_linear_velocity; + Vector3 constant_angular_velocity; + + Vector3 biased_linear_velocity; + Vector3 biased_angular_velocity; + real_t mass = 1.0; + real_t bounce = 0.0; + real_t friction = 1.0; + Vector3 inertia; + + real_t linear_damp = -1.0; + real_t angular_damp = -1.0; + real_t gravity_scale = 1.0; + + uint16_t locked_axis = 0; + + real_t _inv_mass = 1.0; + Vector3 _inv_inertia; // Relative to the principal axes of inertia + + // Relative to the local frame of reference + Basis principal_inertia_axes_local; + Vector3 center_of_mass_local; + + // In world orientation with local origin + Basis _inv_inertia_tensor; + Basis principal_inertia_axes; + Vector3 center_of_mass; + + bool calculate_inertia = true; + bool calculate_center_of_mass = true; + + Vector3 gravity; + + real_t still_time = 0.0; + + Vector3 applied_force; + Vector3 applied_torque; + + real_t area_angular_damp = 0.0; + real_t area_linear_damp = 0.0; + + SelfList active_list; + SelfList mass_properties_update_list; + SelfList direct_state_query_list; + + VSet exceptions; + bool omit_force_integration = false; + bool active = true; + + bool continuous_cd = false; + bool can_sleep = true; + bool first_time_kinematic = false; + + void _mass_properties_changed(); + virtual void _shapes_changed(); + Transform3D new_transform; + + Map constraint_map; + + Vector areas; + + struct Contact { + Vector3 local_pos; + Vector3 local_normal; + real_t depth = 0.0; + int local_shape = 0; + Vector3 collider_pos; + int collider_shape = 0; + ObjectID collider_instance_id; + RID collider; + Vector3 collider_velocity_at_pos; + }; + + Vector contacts; //no contacts by default + int contact_count = 0; + + void *body_state_callback_instance = nullptr; + PhysicsServer3D::BodyStateCallback body_state_callback = nullptr; + + struct ForceIntegrationCallbackData { + Callable callable; + Variant udata; + }; + + ForceIntegrationCallbackData *fi_callback_data = nullptr; + + GodotPhysicsDirectBodyState3D *direct_state = nullptr; + + uint64_t island_step = 0; + + _FORCE_INLINE_ void _compute_area_gravity_and_damping(const GodotArea3D *p_area); + + _FORCE_INLINE_ void _update_transform_dependant(); + + friend class GodotPhysicsDirectBodyState3D; // i give up, too many functions to expose + +public: + void set_state_sync_callback(void *p_instance, PhysicsServer3D::BodyStateCallback p_callback); + void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant()); + + GodotPhysicsDirectBodyState3D *get_direct_state(); + + _FORCE_INLINE_ void add_area(GodotArea3D *p_area) { + int index = areas.find(AreaCMP(p_area)); + if (index > -1) { + areas.write[index].refCount += 1; + } else { + areas.ordered_insert(AreaCMP(p_area)); + } + } + + _FORCE_INLINE_ void remove_area(GodotArea3D *p_area) { + int index = areas.find(AreaCMP(p_area)); + if (index > -1) { + areas.write[index].refCount -= 1; + if (areas[index].refCount < 1) { + areas.remove(index); + } + } + } + + _FORCE_INLINE_ void set_max_contacts_reported(int p_size) { + contacts.resize(p_size); + contact_count = 0; + if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC && p_size) { + set_active(true); + } + } + _FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); } + + _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.is_empty(); } + _FORCE_INLINE_ void add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos); + + _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); } + _FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); } + _FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); } + _FORCE_INLINE_ const VSet &get_exceptions() const { return exceptions; } + + _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } + _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } + + _FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; } + _FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraint_map.erase(p_constraint); } + const Map &get_constraint_map() const { return constraint_map; } + _FORCE_INLINE_ void clear_constraint_map() { constraint_map.clear(); } + + _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; } + _FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; } + + _FORCE_INLINE_ Basis get_principal_inertia_axes() const { return principal_inertia_axes; } + _FORCE_INLINE_ Vector3 get_center_of_mass() const { return center_of_mass; } + _FORCE_INLINE_ Vector3 xform_local_to_principal(const Vector3 &p_pos) const { return principal_inertia_axes_local.xform(p_pos - center_of_mass_local); } + + _FORCE_INLINE_ void set_linear_velocity(const Vector3 &p_velocity) { linear_velocity = p_velocity; } + _FORCE_INLINE_ Vector3 get_linear_velocity() const { return linear_velocity; } + + _FORCE_INLINE_ void set_angular_velocity(const Vector3 &p_velocity) { angular_velocity = p_velocity; } + _FORCE_INLINE_ Vector3 get_angular_velocity() const { return angular_velocity; } + + _FORCE_INLINE_ const Vector3 &get_biased_linear_velocity() const { return biased_linear_velocity; } + _FORCE_INLINE_ const Vector3 &get_biased_angular_velocity() const { return biased_angular_velocity; } + + _FORCE_INLINE_ void apply_central_impulse(const Vector3 &p_impulse) { + linear_velocity += p_impulse * _inv_mass; + } + + _FORCE_INLINE_ void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) { + linear_velocity += p_impulse * _inv_mass; + angular_velocity += _inv_inertia_tensor.xform((p_position - center_of_mass).cross(p_impulse)); + } + + _FORCE_INLINE_ void apply_torque_impulse(const Vector3 &p_impulse) { + angular_velocity += _inv_inertia_tensor.xform(p_impulse); + } + + _FORCE_INLINE_ void apply_bias_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3(), real_t p_max_delta_av = -1.0) { + biased_linear_velocity += p_impulse * _inv_mass; + if (p_max_delta_av != 0.0) { + Vector3 delta_av = _inv_inertia_tensor.xform((p_position - center_of_mass).cross(p_impulse)); + if (p_max_delta_av > 0 && delta_av.length() > p_max_delta_av) { + delta_av = delta_av.normalized() * p_max_delta_av; + } + biased_angular_velocity += delta_av; + } + } + + _FORCE_INLINE_ void apply_bias_torque_impulse(const Vector3 &p_impulse) { + biased_angular_velocity += _inv_inertia_tensor.xform(p_impulse); + } + + _FORCE_INLINE_ void add_central_force(const Vector3 &p_force) { + applied_force += p_force; + } + + _FORCE_INLINE_ void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) { + applied_force += p_force; + applied_torque += (p_position - center_of_mass).cross(p_force); + } + + _FORCE_INLINE_ void add_torque(const Vector3 &p_torque) { + applied_torque += p_torque; + } + + void set_active(bool p_active); + _FORCE_INLINE_ bool is_active() const { return active; } + + _FORCE_INLINE_ void wakeup() { + if ((!get_space()) || mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { + return; + } + set_active(true); + } + + void set_param(PhysicsServer3D::BodyParameter p_param, const Variant &p_value); + Variant get_param(PhysicsServer3D::BodyParameter p_param) const; + + void set_mode(PhysicsServer3D::BodyMode p_mode); + PhysicsServer3D::BodyMode get_mode() const; + + void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant); + Variant get_state(PhysicsServer3D::BodyState p_state) const; + + void set_applied_force(const Vector3 &p_force) { applied_force = p_force; } + Vector3 get_applied_force() const { return applied_force; } + + void set_applied_torque(const Vector3 &p_torque) { applied_torque = p_torque; } + Vector3 get_applied_torque() const { return applied_torque; } + + _FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd = p_enable; } + _FORCE_INLINE_ bool is_continuous_collision_detection_enabled() const { return continuous_cd; } + + void set_space(GodotSpace3D *p_space); + + void update_mass_properties(); + void reset_mass_properties(); + + _FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; } + _FORCE_INLINE_ const Vector3 &get_inv_inertia() const { return _inv_inertia; } + _FORCE_INLINE_ const Basis &get_inv_inertia_tensor() const { return _inv_inertia_tensor; } + _FORCE_INLINE_ real_t get_friction() const { return friction; } + _FORCE_INLINE_ const Vector3 &get_gravity() const { return gravity; } + _FORCE_INLINE_ real_t get_bounce() const { return bounce; } + + void set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool lock); + bool is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const; + + void integrate_forces(real_t p_step); + void integrate_velocities(real_t p_step); + + _FORCE_INLINE_ Vector3 get_velocity_in_local_point(const Vector3 &rel_pos) const { + return linear_velocity + angular_velocity.cross(rel_pos - center_of_mass); + } + + _FORCE_INLINE_ real_t compute_impulse_denominator(const Vector3 &p_pos, const Vector3 &p_normal) const { + Vector3 r0 = p_pos - get_transform().origin - center_of_mass; + + Vector3 c0 = (r0).cross(p_normal); + + Vector3 vec = (_inv_inertia_tensor.xform_inv(c0)).cross(r0); + + return _inv_mass + p_normal.dot(vec); + } + + _FORCE_INLINE_ real_t compute_angular_impulse_denominator(const Vector3 &p_axis) const { + return p_axis.dot(_inv_inertia_tensor.xform_inv(p_axis)); + } + + //void simulate_motion(const Transform3D& p_xform,real_t p_step); + void call_queries(); + void wakeup_neighbours(); + + bool sleep_test(real_t p_step); + + GodotBody3D(); + ~GodotBody3D(); +}; + +//add contact inline + +void GodotBody3D::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos) { + int c_max = contacts.size(); + + if (c_max == 0) { + return; + } + + Contact *c = contacts.ptrw(); + + int idx = -1; + + if (contact_count < c_max) { + idx = contact_count++; + } else { + real_t least_depth = 1e20; + int least_deep = -1; + for (int i = 0; i < c_max; i++) { + if (i == 0 || c[i].depth < least_depth) { + least_deep = i; + least_depth = c[i].depth; + } + } + + if (least_deep >= 0 && least_depth < p_depth) { + idx = least_deep; + } + if (idx == -1) { + return; //none least deepe than this + } + } + + c[idx].local_pos = p_local_pos; + c[idx].local_normal = p_local_normal; + c[idx].depth = p_depth; + c[idx].local_shape = p_local_shape; + c[idx].collider_pos = p_collider_pos; + c[idx].collider_shape = p_collider_shape; + c[idx].collider_instance_id = p_collider_instance_id; + c[idx].collider = p_collider; + c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos; +} + +#endif // GODOT_BODY_3D_H diff --git a/servers/physics_3d/godot_body_direct_state_3d.cpp b/servers/physics_3d/godot_body_direct_state_3d.cpp new file mode 100644 index 0000000000..db09657f8a --- /dev/null +++ b/servers/physics_3d/godot_body_direct_state_3d.cpp @@ -0,0 +1,190 @@ +/*************************************************************************/ +/* godot_body_direct_state_3d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_body_direct_state_3d.h" + +#include "godot_body_3d.h" +#include "godot_space_3d.h" + +Vector3 GodotPhysicsDirectBodyState3D::get_total_gravity() const { + return body->gravity; +} + +real_t GodotPhysicsDirectBodyState3D::get_total_angular_damp() const { + return body->area_angular_damp; +} + +real_t GodotPhysicsDirectBodyState3D::get_total_linear_damp() const { + return body->area_linear_damp; +} + +Vector3 GodotPhysicsDirectBodyState3D::get_center_of_mass() const { + return body->get_center_of_mass(); +} + +Basis GodotPhysicsDirectBodyState3D::get_principal_inertia_axes() const { + return body->get_principal_inertia_axes(); +} + +real_t GodotPhysicsDirectBodyState3D::get_inverse_mass() const { + return body->get_inv_mass(); +} + +Vector3 GodotPhysicsDirectBodyState3D::get_inverse_inertia() const { + return body->get_inv_inertia(); +} + +Basis GodotPhysicsDirectBodyState3D::get_inverse_inertia_tensor() const { + return body->get_inv_inertia_tensor(); +} + +void GodotPhysicsDirectBodyState3D::set_linear_velocity(const Vector3 &p_velocity) { + body->wakeup(); + body->set_linear_velocity(p_velocity); +} + +Vector3 GodotPhysicsDirectBodyState3D::get_linear_velocity() const { + return body->get_linear_velocity(); +} + +void GodotPhysicsDirectBodyState3D::set_angular_velocity(const Vector3 &p_velocity) { + body->wakeup(); + body->set_angular_velocity(p_velocity); +} + +Vector3 GodotPhysicsDirectBodyState3D::get_angular_velocity() const { + return body->get_angular_velocity(); +} + +void GodotPhysicsDirectBodyState3D::set_transform(const Transform3D &p_transform) { + body->set_state(PhysicsServer3D::BODY_STATE_TRANSFORM, p_transform); +} + +Transform3D GodotPhysicsDirectBodyState3D::get_transform() const { + return body->get_transform(); +} + +Vector3 GodotPhysicsDirectBodyState3D::get_velocity_at_local_position(const Vector3 &p_position) const { + return body->get_velocity_in_local_point(p_position); +} + +void GodotPhysicsDirectBodyState3D::add_central_force(const Vector3 &p_force) { + body->wakeup(); + body->add_central_force(p_force); +} + +void GodotPhysicsDirectBodyState3D::add_force(const Vector3 &p_force, const Vector3 &p_position) { + body->wakeup(); + body->add_force(p_force, p_position); +} + +void GodotPhysicsDirectBodyState3D::add_torque(const Vector3 &p_torque) { + body->wakeup(); + body->add_torque(p_torque); +} + +void GodotPhysicsDirectBodyState3D::apply_central_impulse(const Vector3 &p_impulse) { + body->wakeup(); + body->apply_central_impulse(p_impulse); +} + +void GodotPhysicsDirectBodyState3D::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) { + body->wakeup(); + body->apply_impulse(p_impulse, p_position); +} + +void GodotPhysicsDirectBodyState3D::apply_torque_impulse(const Vector3 &p_impulse) { + body->wakeup(); + body->apply_torque_impulse(p_impulse); +} + +void GodotPhysicsDirectBodyState3D::set_sleep_state(bool p_sleep) { + body->set_active(!p_sleep); +} + +bool GodotPhysicsDirectBodyState3D::is_sleeping() const { + return !body->is_active(); +} + +int GodotPhysicsDirectBodyState3D::get_contact_count() const { + return body->contact_count; +} + +Vector3 GodotPhysicsDirectBodyState3D::get_contact_local_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); + return body->contacts[p_contact_idx].local_pos; +} + +Vector3 GodotPhysicsDirectBodyState3D::get_contact_local_normal(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); + return body->contacts[p_contact_idx].local_normal; +} + +real_t GodotPhysicsDirectBodyState3D::get_contact_impulse(int p_contact_idx) const { + return 0.0f; // Only implemented for bullet +} + +int GodotPhysicsDirectBodyState3D::get_contact_local_shape(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1); + return body->contacts[p_contact_idx].local_shape; +} + +RID GodotPhysicsDirectBodyState3D::get_contact_collider(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID()); + return body->contacts[p_contact_idx].collider; +} + +Vector3 GodotPhysicsDirectBodyState3D::get_contact_collider_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); + return body->contacts[p_contact_idx].collider_pos; +} + +ObjectID GodotPhysicsDirectBodyState3D::get_contact_collider_id(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID()); + return body->contacts[p_contact_idx].collider_instance_id; +} + +int GodotPhysicsDirectBodyState3D::get_contact_collider_shape(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0); + return body->contacts[p_contact_idx].collider_shape; +} + +Vector3 GodotPhysicsDirectBodyState3D::get_contact_collider_velocity_at_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); + return body->contacts[p_contact_idx].collider_velocity_at_pos; +} + +PhysicsDirectSpaceState3D *GodotPhysicsDirectBodyState3D::get_space_state() { + return body->get_space()->get_direct_state(); +} + +real_t GodotPhysicsDirectBodyState3D::get_step() const { + return body->get_space()->get_last_step(); +} diff --git a/servers/physics_3d/godot_body_direct_state_3d.h b/servers/physics_3d/godot_body_direct_state_3d.h new file mode 100644 index 0000000000..6c584a2634 --- /dev/null +++ b/servers/physics_3d/godot_body_direct_state_3d.h @@ -0,0 +1,94 @@ +/*************************************************************************/ +/* godot_body_direct_state_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_BODY_DIRECT_STATE_3D_H +#define GODOT_BODY_DIRECT_STATE_3D_H + +#include "servers/physics_server_3d.h" + +class GodotBody3D; + +class GodotPhysicsDirectBodyState3D : public PhysicsDirectBodyState3D { + GDCLASS(GodotPhysicsDirectBodyState3D, PhysicsDirectBodyState3D); + +public: + GodotBody3D *body = nullptr; + + virtual Vector3 get_total_gravity() const override; + virtual real_t get_total_angular_damp() const override; + virtual real_t get_total_linear_damp() const override; + + virtual Vector3 get_center_of_mass() const override; + virtual Basis get_principal_inertia_axes() const override; + + virtual real_t get_inverse_mass() const override; + virtual Vector3 get_inverse_inertia() const override; + virtual Basis get_inverse_inertia_tensor() const override; + + virtual void set_linear_velocity(const Vector3 &p_velocity) override; + virtual Vector3 get_linear_velocity() const override; + + virtual void set_angular_velocity(const Vector3 &p_velocity) override; + virtual Vector3 get_angular_velocity() const override; + + virtual void set_transform(const Transform3D &p_transform) override; + virtual Transform3D get_transform() const override; + + virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const override; + + virtual void add_central_force(const Vector3 &p_force) override; + virtual void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override; + virtual void add_torque(const Vector3 &p_torque) override; + virtual void apply_central_impulse(const Vector3 &p_impulse) override; + virtual void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) override; + virtual void apply_torque_impulse(const Vector3 &p_impulse) override; + + virtual void set_sleep_state(bool p_sleep) override; + virtual bool is_sleeping() const override; + + virtual int get_contact_count() const override; + + virtual Vector3 get_contact_local_position(int p_contact_idx) const override; + virtual Vector3 get_contact_local_normal(int p_contact_idx) const override; + virtual real_t get_contact_impulse(int p_contact_idx) const override; + virtual int get_contact_local_shape(int p_contact_idx) const override; + + virtual RID get_contact_collider(int p_contact_idx) const override; + virtual Vector3 get_contact_collider_position(int p_contact_idx) const override; + virtual ObjectID get_contact_collider_id(int p_contact_idx) const override; + virtual int get_contact_collider_shape(int p_contact_idx) const override; + virtual Vector3 get_contact_collider_velocity_at_position(int p_contact_idx) const override; + + virtual PhysicsDirectSpaceState3D *get_space_state() override; + + virtual real_t get_step() const override; +}; + +#endif // GODOT_BODY_DIRECT_STATE_3D_H diff --git a/servers/physics_3d/godot_body_pair_3d.cpp b/servers/physics_3d/godot_body_pair_3d.cpp new file mode 100644 index 0000000000..457abfb71a --- /dev/null +++ b/servers/physics_3d/godot_body_pair_3d.cpp @@ -0,0 +1,908 @@ +/*************************************************************************/ +/* godot_body_pair_3d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_body_pair_3d.h" + +#include "godot_collision_solver_3d.h" +#include "godot_space_3d.h" + +#include "core/os/os.h" + +/* +#define NO_ACCUMULATE_IMPULSES +#define NO_SPLIT_IMPULSES + +#define NO_FRICTION +*/ + +#define NO_TANGENTIALS +/* BODY PAIR */ + +//#define ALLOWED_PENETRATION 0.01 +#define RELAXATION_TIMESTEPS 3 +#define MIN_VELOCITY 0.0001 +#define MAX_BIAS_ROTATION (Math_PI / 8) + +void GodotBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { + GodotBodyPair3D *pair = (GodotBodyPair3D *)p_userdata; + pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B); +} + +void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B) { + // check if we already have the contact + + //Vector3 local_A = A->get_inv_transform().xform(p_point_A); + //Vector3 local_B = B->get_inv_transform().xform(p_point_B); + + Vector3 local_A = A->get_inv_transform().basis.xform(p_point_A); + Vector3 local_B = B->get_inv_transform().basis.xform(p_point_B - offset_B); + + int new_index = contact_count; + + ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1)); + + Contact contact; + + contact.acc_normal_impulse = 0; + contact.acc_bias_impulse = 0; + contact.acc_bias_impulse_center_of_mass = 0; + contact.acc_tangent_impulse = Vector3(); + contact.index_A = p_index_A; + contact.index_B = p_index_B; + contact.local_A = local_A; + contact.local_B = local_B; + contact.normal = (p_point_A - p_point_B).normalized(); + contact.mass_normal = 0; // will be computed in setup() + + // attempt to determine if the contact will be reused + real_t contact_recycle_radius = space->get_contact_recycle_radius(); + + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + if (c.local_A.distance_squared_to(local_A) < (contact_recycle_radius * contact_recycle_radius) && + c.local_B.distance_squared_to(local_B) < (contact_recycle_radius * contact_recycle_radius)) { + contact.acc_normal_impulse = c.acc_normal_impulse; + contact.acc_bias_impulse = c.acc_bias_impulse; + contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass; + contact.acc_tangent_impulse = c.acc_tangent_impulse; + new_index = i; + break; + } + } + + // figure out if the contact amount must be reduced to fit the new contact + + if (new_index == MAX_CONTACTS) { + // remove the contact with the minimum depth + + int least_deep = -1; + real_t min_depth = 1e10; + + for (int i = 0; i <= contact_count; i++) { + Contact &c = (i == contact_count) ? contact : contacts[i]; + Vector3 global_A = A->get_transform().basis.xform(c.local_A); + Vector3 global_B = B->get_transform().basis.xform(c.local_B) + offset_B; + + Vector3 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + + if (depth < min_depth) { + min_depth = depth; + least_deep = i; + } + } + + ERR_FAIL_COND(least_deep == -1); + + if (least_deep < contact_count) { //replace the last deep contact by the new one + + contacts[least_deep] = contact; + } + + return; + } + + contacts[new_index] = contact; + + if (new_index == contact_count) { + contact_count++; + } +} + +void GodotBodyPair3D::validate_contacts() { + //make sure to erase contacts that are no longer valid + + real_t contact_max_separation = space->get_contact_max_separation(); + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + + Vector3 global_A = A->get_transform().basis.xform(c.local_A); + Vector3 global_B = B->get_transform().basis.xform(c.local_B) + offset_B; + Vector3 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + + if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) { + // contact no longer needed, remove + + if ((i + 1) < contact_count) { + // swap with the last one + SWAP(contacts[i], contacts[contact_count - 1]); + } + + i--; + contact_count--; + } + } +} + +bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A, const Transform3D &p_xform_A, GodotBody3D *p_B, int p_shape_B, const Transform3D &p_xform_B) { + Vector3 motion = p_A->get_linear_velocity() * p_step; + real_t mlen = motion.length(); + if (mlen < CMP_EPSILON) { + return false; + } + + Vector3 mnormal = motion / mlen; + + real_t min, max; + p_A->get_shape(p_shape_A)->project_range(mnormal, p_xform_A, min, max); + bool fast_object = mlen > (max - min) * 0.3; //going too fast in that direction + + if (!fast_object) { //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis + return false; + } + + //cast a segment from support in motion normal, in the same direction of motion by motion length + //support is the worst case collision point, so real collision happened before + Vector3 s = p_A->get_shape(p_shape_A)->get_support(p_xform_A.basis.xform(mnormal).normalized()); + Vector3 from = p_xform_A.xform(s); + Vector3 to = from + motion; + + Transform3D from_inv = p_xform_B.affine_inverse(); + + Vector3 local_from = from_inv.xform(from - mnormal * mlen * 0.1); //start from a little inside the bounding box + Vector3 local_to = from_inv.xform(to); + + Vector3 rpos, rnorm; + if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) { + return false; + } + + //shorten the linear velocity so it does not hit, but gets close enough, next frame will hit softly or soft enough + Vector3 hitpos = p_xform_B.xform(rpos); + + real_t newlen = hitpos.distance_to(from) - (max - min) * 0.01; + p_A->set_linear_velocity((mnormal * newlen) / p_step); + + return true; +} + +real_t combine_bounce(GodotBody3D *A, GodotBody3D *B) { + return CLAMP(A->get_bounce() + B->get_bounce(), 0, 1); +} + +real_t combine_friction(GodotBody3D *A, GodotBody3D *B) { + return ABS(MIN(A->get_friction(), B->get_friction())); +} + +bool GodotBodyPair3D::setup(real_t p_step) { + if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) { + collided = false; + return false; + } + + collide_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) && A->collides_with(B); + collide_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) && B->collides_with(A); + + report_contacts_only = false; + if (!collide_A && !collide_B) { + if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) { + report_contacts_only = true; + } else { + collided = false; + return false; + } + } + + offset_B = B->get_transform().get_origin() - A->get_transform().get_origin(); + + validate_contacts(); + + const Vector3 &offset_A = A->get_transform().get_origin(); + Transform3D xform_Au = Transform3D(A->get_transform().basis, Vector3()); + Transform3D xform_A = xform_Au * A->get_shape_transform(shape_A); + + Transform3D xform_Bu = B->get_transform(); + xform_Bu.origin -= offset_A; + Transform3D xform_B = xform_Bu * B->get_shape_transform(shape_B); + + GodotShape3D *shape_A_ptr = A->get_shape(shape_A); + GodotShape3D *shape_B_ptr = B->get_shape(shape_B); + + collided = GodotCollisionSolver3D::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); + + if (!collided) { + //test ccd (currently just a raycast) + + if (A->is_continuous_collision_detection_enabled() && collide_A) { + _test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B); + } + + if (B->is_continuous_collision_detection_enabled() && collide_B) { + _test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A); + } + + return false; + } + + return true; +} + +bool GodotBodyPair3D::pre_solve(real_t p_step) { + if (!collided) { + return false; + } + + real_t max_penetration = space->get_contact_max_allowed_penetration(); + + real_t bias = (real_t)0.3; + + GodotShape3D *shape_A_ptr = A->get_shape(shape_A); + GodotShape3D *shape_B_ptr = B->get_shape(shape_B); + + if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) { + if (shape_A_ptr->get_custom_bias() == 0) { + bias = shape_B_ptr->get_custom_bias(); + } else if (shape_B_ptr->get_custom_bias() == 0) { + bias = shape_A_ptr->get_custom_bias(); + } else { + bias = (shape_B_ptr->get_custom_bias() + shape_A_ptr->get_custom_bias()) * 0.5; + } + } + + real_t inv_dt = 1.0 / p_step; + + bool do_process = false; + + const Basis &basis_A = A->get_transform().basis; + const Basis &basis_B = B->get_transform().basis; + + Basis zero_basis; + zero_basis.set_zero(); + + const Basis &inv_inertia_tensor_A = collide_A ? A->get_inv_inertia_tensor() : zero_basis; + const Basis &inv_inertia_tensor_B = collide_B ? B->get_inv_inertia_tensor() : zero_basis; + + real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0; + real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0; + + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + c.active = false; + + Vector3 global_A = basis_A.xform(c.local_A); + Vector3 global_B = basis_B.xform(c.local_B) + offset_B; + + Vector3 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + + if (depth <= 0.0) { + continue; + } + +#ifdef DEBUG_ENABLED + if (space->is_debugging_contacts()) { + const Vector3 &offset_A = A->get_transform().get_origin(); + space->add_debug_contact(global_A + offset_A); + space->add_debug_contact(global_B + offset_A); + } +#endif + + c.rA = global_A - A->get_center_of_mass(); + c.rB = global_B - B->get_center_of_mass() - offset_B; + + // contact query reporting... + + if (A->can_report_contacts()) { + Vector3 crA = A->get_angular_velocity().cross(c.rA) + A->get_linear_velocity(); + A->add_contact(global_A, -c.normal, depth, shape_A, global_B, shape_B, B->get_instance_id(), B->get_self(), crA); + } + + if (B->can_report_contacts()) { + Vector3 crB = B->get_angular_velocity().cross(c.rB) + B->get_linear_velocity(); + B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crB); + } + + if (report_contacts_only) { + collided = false; + continue; + } + + c.active = true; + do_process = true; + + // Precompute normal mass, tangent mass, and bias. + Vector3 inertia_A = inv_inertia_tensor_A.xform(c.rA.cross(c.normal)); + Vector3 inertia_B = inv_inertia_tensor_B.xform(c.rB.cross(c.normal)); + real_t kNormal = inv_mass_A + inv_mass_B; + kNormal += c.normal.dot(inertia_A.cross(c.rA)) + c.normal.dot(inertia_B.cross(c.rB)); + c.mass_normal = 1.0f / kNormal; + + c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration); + c.depth = depth; + + Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse; + if (collide_A) { + A->apply_impulse(-j_vec, c.rA + A->get_center_of_mass()); + } + if (collide_B) { + B->apply_impulse(j_vec, c.rB + B->get_center_of_mass()); + } + c.acc_bias_impulse = 0; + c.acc_bias_impulse_center_of_mass = 0; + + c.bounce = combine_bounce(A, B); + if (c.bounce) { + Vector3 crA = A->get_angular_velocity().cross(c.rA); + Vector3 crB = B->get_angular_velocity().cross(c.rB); + Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; + //normal impule + c.bounce = c.bounce * dv.dot(c.normal); + } + } + + return do_process; +} + +void GodotBodyPair3D::solve(real_t p_step) { + if (!collided) { + return; + } + + const real_t max_bias_av = MAX_BIAS_ROTATION / p_step; + + Basis zero_basis; + zero_basis.set_zero(); + + const Basis &inv_inertia_tensor_A = collide_A ? A->get_inv_inertia_tensor() : zero_basis; + const Basis &inv_inertia_tensor_B = collide_B ? B->get_inv_inertia_tensor() : zero_basis; + + real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0; + real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0; + + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + if (!c.active) { + continue; + } + + c.active = false; //try to deactivate, will activate itself if still needed + + //bias impulse + + Vector3 crbA = A->get_biased_angular_velocity().cross(c.rA); + Vector3 crbB = B->get_biased_angular_velocity().cross(c.rB); + Vector3 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA; + + real_t vbn = dbv.dot(c.normal); + + if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { + real_t jbn = (-vbn + c.bias) * c.mass_normal; + real_t jbnOld = c.acc_bias_impulse; + c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f); + + Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); + + if (collide_A) { + A->apply_bias_impulse(-jb, c.rA + A->get_center_of_mass(), max_bias_av); + } + if (collide_B) { + B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), max_bias_av); + } + + crbA = A->get_biased_angular_velocity().cross(c.rA); + crbB = B->get_biased_angular_velocity().cross(c.rB); + dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA; + + vbn = dbv.dot(c.normal); + + if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { + real_t jbn_com = (-vbn + c.bias) / (inv_mass_A + inv_mass_B); + real_t jbnOld_com = c.acc_bias_impulse_center_of_mass; + c.acc_bias_impulse_center_of_mass = MAX(jbnOld_com + jbn_com, 0.0f); + + Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com); + + if (collide_A) { + A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f); + } + if (collide_B) { + B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f); + } + } + + c.active = true; + } + + Vector3 crA = A->get_angular_velocity().cross(c.rA); + Vector3 crB = B->get_angular_velocity().cross(c.rB); + Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; + + //normal impulse + real_t vn = dv.dot(c.normal); + + if (Math::abs(vn) > MIN_VELOCITY) { + real_t jn = -(c.bounce + vn) * c.mass_normal; + real_t jnOld = c.acc_normal_impulse; + c.acc_normal_impulse = MAX(jnOld + jn, 0.0f); + + Vector3 j = c.normal * (c.acc_normal_impulse - jnOld); + + if (collide_A) { + A->apply_impulse(-j, c.rA + A->get_center_of_mass()); + } + if (collide_B) { + B->apply_impulse(j, c.rB + B->get_center_of_mass()); + } + + c.active = true; + } + + //friction impulse + + real_t friction = combine_friction(A, B); + + Vector3 lvA = A->get_linear_velocity() + A->get_angular_velocity().cross(c.rA); + Vector3 lvB = B->get_linear_velocity() + B->get_angular_velocity().cross(c.rB); + + Vector3 dtv = lvB - lvA; + real_t tn = c.normal.dot(dtv); + + // tangential velocity + Vector3 tv = dtv - c.normal * tn; + real_t tvl = tv.length(); + + if (tvl > MIN_VELOCITY) { + tv /= tvl; + + Vector3 temp1 = inv_inertia_tensor_A.xform(c.rA.cross(tv)); + Vector3 temp2 = inv_inertia_tensor_B.xform(c.rB.cross(tv)); + + real_t t = -tvl / + (inv_mass_A + inv_mass_B + tv.dot(temp1.cross(c.rA) + temp2.cross(c.rB))); + + Vector3 jt = t * tv; + + Vector3 jtOld = c.acc_tangent_impulse; + c.acc_tangent_impulse += jt; + + real_t fi_len = c.acc_tangent_impulse.length(); + real_t jtMax = c.acc_normal_impulse * friction; + + if (fi_len > CMP_EPSILON && fi_len > jtMax) { + c.acc_tangent_impulse *= jtMax / fi_len; + } + + jt = c.acc_tangent_impulse - jtOld; + + if (collide_A) { + A->apply_impulse(-jt, c.rA + A->get_center_of_mass()); + } + if (collide_B) { + B->apply_impulse(jt, c.rB + B->get_center_of_mass()); + } + + c.active = true; + } + } +} + +GodotBodyPair3D::GodotBodyPair3D(GodotBody3D *p_A, int p_shape_A, GodotBody3D *p_B, int p_shape_B) : + GodotBodyContact3D(_arr, 2) { + A = p_A; + B = p_B; + shape_A = p_shape_A; + shape_B = p_shape_B; + space = A->get_space(); + A->add_constraint(this, 0); + B->add_constraint(this, 1); +} + +GodotBodyPair3D::~GodotBodyPair3D() { + A->remove_constraint(this); + B->remove_constraint(this); +} + +void GodotBodySoftBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { + GodotBodySoftBodyPair3D *pair = (GodotBodySoftBodyPair3D *)p_userdata; + pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B); +} + +void GodotBodySoftBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B) { + Vector3 local_A = body->get_inv_transform().xform(p_point_A); + Vector3 local_B = p_point_B - soft_body->get_node_position(p_index_B); + + Contact contact; + contact.index_A = p_index_A; + contact.index_B = p_index_B; + contact.acc_normal_impulse = 0; + contact.acc_bias_impulse = 0; + contact.acc_bias_impulse_center_of_mass = 0; + contact.acc_tangent_impulse = Vector3(); + contact.local_A = local_A; + contact.local_B = local_B; + contact.normal = (p_point_A - p_point_B).normalized(); + contact.mass_normal = 0; + + // Attempt to determine if the contact will be reused. + real_t contact_recycle_radius = space->get_contact_recycle_radius(); + + uint32_t contact_count = contacts.size(); + for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { + Contact &c = contacts[contact_index]; + if (c.index_B == p_index_B) { + if (c.local_A.distance_squared_to(local_A) < (contact_recycle_radius * contact_recycle_radius) && + c.local_B.distance_squared_to(local_B) < (contact_recycle_radius * contact_recycle_radius)) { + contact.acc_normal_impulse = c.acc_normal_impulse; + contact.acc_bias_impulse = c.acc_bias_impulse; + contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass; + contact.acc_tangent_impulse = c.acc_tangent_impulse; + } + c = contact; + return; + } + } + + contacts.push_back(contact); +} + +void GodotBodySoftBodyPair3D::validate_contacts() { + // Make sure to erase contacts that are no longer valid. + const Transform3D &transform_A = body->get_transform(); + + real_t contact_max_separation = space->get_contact_max_separation(); + + uint32_t contact_count = contacts.size(); + for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { + Contact &c = contacts[contact_index]; + + Vector3 global_A = transform_A.xform(c.local_A); + Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B; + Vector3 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + + if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) { + // Contact no longer needed, remove. + if ((contact_index + 1) < contact_count) { + // Swap with the last one. + SWAP(c, contacts[contact_count - 1]); + } + + contact_index--; + contact_count--; + } + } + + contacts.resize(contact_count); +} + +bool GodotBodySoftBodyPair3D::setup(real_t p_step) { + if (!body->interacts_with(soft_body) || body->has_exception(soft_body->get_self()) || soft_body->has_exception(body->get_self())) { + collided = false; + return false; + } + + body_collides = (body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) && body->collides_with(soft_body); + soft_body_collides = soft_body->collides_with(body); + + if (!body_collides && !soft_body_collides) { + if (body->get_max_contacts_reported() > 0) { + report_contacts_only = true; + } else { + collided = false; + return false; + } + } + + const Transform3D &xform_Au = body->get_transform(); + Transform3D xform_A = xform_Au * body->get_shape_transform(body_shape); + + Transform3D xform_Bu = soft_body->get_transform(); + Transform3D xform_B = xform_Bu * soft_body->get_shape_transform(0); + + validate_contacts(); + + GodotShape3D *shape_A_ptr = body->get_shape(body_shape); + GodotShape3D *shape_B_ptr = soft_body->get_shape(0); + + collided = GodotCollisionSolver3D::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); + + return collided; +} + +bool GodotBodySoftBodyPair3D::pre_solve(real_t p_step) { + if (!collided) { + return false; + } + + real_t max_penetration = space->get_contact_max_allowed_penetration(); + + real_t bias = (real_t)0.3; + + GodotShape3D *shape_A_ptr = body->get_shape(body_shape); + + if (shape_A_ptr->get_custom_bias()) { + bias = shape_A_ptr->get_custom_bias(); + } + + real_t inv_dt = 1.0 / p_step; + + bool do_process = false; + + const Transform3D &transform_A = body->get_transform(); + + Basis zero_basis; + zero_basis.set_zero(); + + const Basis &body_inv_inertia_tensor = body_collides ? body->get_inv_inertia_tensor() : zero_basis; + + real_t body_inv_mass = body_collides ? body->get_inv_mass() : 0.0; + + uint32_t contact_count = contacts.size(); + for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { + Contact &c = contacts[contact_index]; + c.active = false; + + real_t node_inv_mass = soft_body_collides ? soft_body->get_node_inv_mass(c.index_B) : 0.0; + if ((node_inv_mass == 0.0) && (body_inv_mass == 0.0)) { + continue; + } + + Vector3 global_A = transform_A.xform(c.local_A); + Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B; + Vector3 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + + if (depth <= 0.0) { + continue; + } + +#ifdef DEBUG_ENABLED + if (space->is_debugging_contacts()) { + space->add_debug_contact(global_A); + space->add_debug_contact(global_B); + } +#endif + + c.rA = global_A - transform_A.origin - body->get_center_of_mass(); + c.rB = global_B; + + if (body->can_report_contacts()) { + Vector3 crA = body->get_angular_velocity().cross(c.rA) + body->get_linear_velocity(); + body->add_contact(global_A, -c.normal, depth, body_shape, global_B, 0, soft_body->get_instance_id(), soft_body->get_self(), crA); + } + + if (report_contacts_only) { + collided = false; + continue; + } + + c.active = true; + do_process = true; + + if (body_collides) { + body->set_active(true); + } + + // Precompute normal mass, tangent mass, and bias. + Vector3 inertia_A = body_inv_inertia_tensor.xform(c.rA.cross(c.normal)); + real_t kNormal = body_inv_mass + node_inv_mass; + kNormal += c.normal.dot(inertia_A.cross(c.rA)); + c.mass_normal = 1.0f / kNormal; + + c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration); + c.depth = depth; + + Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse; + if (body_collides) { + body->apply_impulse(-j_vec, c.rA + body->get_center_of_mass()); + } + if (soft_body_collides) { + soft_body->apply_node_impulse(c.index_B, j_vec); + } + c.acc_bias_impulse = 0; + c.acc_bias_impulse_center_of_mass = 0; + + c.bounce = body->get_bounce(); + + if (c.bounce) { + Vector3 crA = body->get_angular_velocity().cross(c.rA); + Vector3 dv = soft_body->get_node_velocity(c.index_B) - body->get_linear_velocity() - crA; + + // Normal impulse. + c.bounce = c.bounce * dv.dot(c.normal); + } + } + + return do_process; +} + +void GodotBodySoftBodyPair3D::solve(real_t p_step) { + if (!collided) { + return; + } + + const real_t max_bias_av = MAX_BIAS_ROTATION / p_step; + + Basis zero_basis; + zero_basis.set_zero(); + + const Basis &body_inv_inertia_tensor = body_collides ? body->get_inv_inertia_tensor() : zero_basis; + + real_t body_inv_mass = body_collides ? body->get_inv_mass() : 0.0; + + uint32_t contact_count = contacts.size(); + for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { + Contact &c = contacts[contact_index]; + if (!c.active) { + continue; + } + + c.active = false; + + real_t node_inv_mass = soft_body_collides ? soft_body->get_node_inv_mass(c.index_B) : 0.0; + + // Bias impulse. + Vector3 crbA = body->get_biased_angular_velocity().cross(c.rA); + Vector3 dbv = soft_body->get_node_biased_velocity(c.index_B) - body->get_biased_linear_velocity() - crbA; + + real_t vbn = dbv.dot(c.normal); + + if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { + real_t jbn = (-vbn + c.bias) * c.mass_normal; + real_t jbnOld = c.acc_bias_impulse; + c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f); + + Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); + + if (body_collides) { + body->apply_bias_impulse(-jb, c.rA + body->get_center_of_mass(), max_bias_av); + } + if (soft_body_collides) { + soft_body->apply_node_bias_impulse(c.index_B, jb); + } + + crbA = body->get_biased_angular_velocity().cross(c.rA); + dbv = soft_body->get_node_biased_velocity(c.index_B) - body->get_biased_linear_velocity() - crbA; + + vbn = dbv.dot(c.normal); + + if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { + real_t jbn_com = (-vbn + c.bias) / (body_inv_mass + node_inv_mass); + real_t jbnOld_com = c.acc_bias_impulse_center_of_mass; + c.acc_bias_impulse_center_of_mass = MAX(jbnOld_com + jbn_com, 0.0f); + + Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com); + + if (body_collides) { + body->apply_bias_impulse(-jb_com, body->get_center_of_mass(), 0.0f); + } + if (soft_body_collides) { + soft_body->apply_node_bias_impulse(c.index_B, jb_com); + } + } + + c.active = true; + } + + Vector3 crA = body->get_angular_velocity().cross(c.rA); + Vector3 dv = soft_body->get_node_velocity(c.index_B) - body->get_linear_velocity() - crA; + + // Normal impulse. + real_t vn = dv.dot(c.normal); + + if (Math::abs(vn) > MIN_VELOCITY) { + real_t jn = -(c.bounce + vn) * c.mass_normal; + real_t jnOld = c.acc_normal_impulse; + c.acc_normal_impulse = MAX(jnOld + jn, 0.0f); + + Vector3 j = c.normal * (c.acc_normal_impulse - jnOld); + + if (body_collides) { + body->apply_impulse(-j, c.rA + body->get_center_of_mass()); + } + if (soft_body_collides) { + soft_body->apply_node_impulse(c.index_B, j); + } + + c.active = true; + } + + // Friction impulse. + real_t friction = body->get_friction(); + + Vector3 lvA = body->get_linear_velocity() + body->get_angular_velocity().cross(c.rA); + Vector3 lvB = soft_body->get_node_velocity(c.index_B); + Vector3 dtv = lvB - lvA; + + real_t tn = c.normal.dot(dtv); + + // Tangential velocity. + Vector3 tv = dtv - c.normal * tn; + real_t tvl = tv.length(); + + if (tvl > MIN_VELOCITY) { + tv /= tvl; + + Vector3 temp1 = body_inv_inertia_tensor.xform(c.rA.cross(tv)); + + real_t t = -tvl / + (body_inv_mass + node_inv_mass + tv.dot(temp1.cross(c.rA))); + + Vector3 jt = t * tv; + + Vector3 jtOld = c.acc_tangent_impulse; + c.acc_tangent_impulse += jt; + + real_t fi_len = c.acc_tangent_impulse.length(); + real_t jtMax = c.acc_normal_impulse * friction; + + if (fi_len > CMP_EPSILON && fi_len > jtMax) { + c.acc_tangent_impulse *= jtMax / fi_len; + } + + jt = c.acc_tangent_impulse - jtOld; + + if (body_collides) { + body->apply_impulse(-jt, c.rA + body->get_center_of_mass()); + } + if (soft_body_collides) { + soft_body->apply_node_impulse(c.index_B, jt); + } + + c.active = true; + } + } +} + +GodotBodySoftBodyPair3D::GodotBodySoftBodyPair3D(GodotBody3D *p_A, int p_shape_A, GodotSoftBody3D *p_B) : + GodotBodyContact3D(&body, 1) { + body = p_A; + soft_body = p_B; + body_shape = p_shape_A; + space = p_A->get_space(); + body->add_constraint(this, 0); + soft_body->add_constraint(this); +} + +GodotBodySoftBodyPair3D::~GodotBodySoftBodyPair3D() { + body->remove_constraint(this); + soft_body->remove_constraint(this); +} diff --git a/servers/physics_3d/godot_body_pair_3d.h b/servers/physics_3d/godot_body_pair_3d.h new file mode 100644 index 0000000000..c0a2424e05 --- /dev/null +++ b/servers/physics_3d/godot_body_pair_3d.h @@ -0,0 +1,144 @@ +/*************************************************************************/ +/* godot_body_pair_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_BODY_PAIR_3D_H +#define GODOT_BODY_PAIR_3D_H + +#include "godot_body_3d.h" +#include "godot_constraint_3d.h" +#include "godot_soft_body_3d.h" + +#include "core/templates/local_vector.h" + +class GodotBodyContact3D : public GodotConstraint3D { +protected: + struct Contact { + Vector3 position; + Vector3 normal; + int index_A = 0, index_B = 0; + Vector3 local_A, local_B; + real_t acc_normal_impulse = 0.0; // accumulated normal impulse (Pn) + Vector3 acc_tangent_impulse; // accumulated tangent impulse (Pt) + real_t acc_bias_impulse = 0.0; // accumulated normal impulse for position bias (Pnb) + real_t acc_bias_impulse_center_of_mass = 0.0; // accumulated normal impulse for position bias applied to com + real_t mass_normal = 0.0; + real_t bias = 0.0; + real_t bounce = 0.0; + + real_t depth = 0.0; + bool active = false; + Vector3 rA, rB; // Offset in world orientation with respect to center of mass + }; + + Vector3 sep_axis; + bool collided = false; + + GodotSpace3D *space = nullptr; + + GodotBodyContact3D(GodotBody3D **p_body_ptr = nullptr, int p_body_count = 0) : + GodotConstraint3D(p_body_ptr, p_body_count) { + } +}; + +class GodotBodyPair3D : public GodotBodyContact3D { + enum { + MAX_CONTACTS = 4 + }; + + union { + struct { + GodotBody3D *A; + GodotBody3D *B; + }; + + GodotBody3D *_arr[2] = { nullptr, nullptr }; + }; + + int shape_A = 0; + int shape_B = 0; + + bool collide_A = false; + bool collide_B = false; + + bool report_contacts_only = false; + + Vector3 offset_B; //use local A coordinates to avoid numerical issues on collision detection + + Contact contacts[MAX_CONTACTS]; + int contact_count = 0; + + static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); + + void contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B); + + void validate_contacts(); + bool _test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A, const Transform3D &p_xform_A, GodotBody3D *p_B, int p_shape_B, const Transform3D &p_xform_B); + +public: + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + GodotBodyPair3D(GodotBody3D *p_A, int p_shape_A, GodotBody3D *p_B, int p_shape_B); + ~GodotBodyPair3D(); +}; + +class GodotBodySoftBodyPair3D : public GodotBodyContact3D { + GodotBody3D *body = nullptr; + GodotSoftBody3D *soft_body = nullptr; + + int body_shape = 0; + + bool body_collides = false; + bool soft_body_collides = false; + + bool report_contacts_only = false; + + LocalVector contacts; + + static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); + + void contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B); + + void validate_contacts(); + +public: + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + virtual GodotSoftBody3D *get_soft_body_ptr(int p_index) const override { return soft_body; } + virtual int get_soft_body_count() const override { return 1; } + + GodotBodySoftBodyPair3D(GodotBody3D *p_A, int p_shape_A, GodotSoftBody3D *p_B); + ~GodotBodySoftBodyPair3D(); +}; + +#endif // GODOT_BODY_PAIR_3D_H diff --git a/servers/physics_3d/godot_broad_phase_3d.cpp b/servers/physics_3d/godot_broad_phase_3d.cpp new file mode 100644 index 0000000000..db51dfb2b6 --- /dev/null +++ b/servers/physics_3d/godot_broad_phase_3d.cpp @@ -0,0 +1,36 @@ +/*************************************************************************/ +/* godot_broad_phase_3d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_broad_phase_3d.h" + +GodotBroadPhase3D::CreateFunction GodotBroadPhase3D::create_func = nullptr; + +GodotBroadPhase3D::~GodotBroadPhase3D() { +} diff --git a/servers/physics_3d/godot_broad_phase_3d.h b/servers/physics_3d/godot_broad_phase_3d.h new file mode 100644 index 0000000000..65423f293c --- /dev/null +++ b/servers/physics_3d/godot_broad_phase_3d.h @@ -0,0 +1,72 @@ +/*************************************************************************/ +/* godot_broad_phase_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_BROAD_PHASE_3D_H +#define GODOT_BROAD_PHASE_3D_H + +#include "core/math/aabb.h" +#include "core/math/math_funcs.h" + +class GodotCollisionObject3D; + +class GodotBroadPhase3D { +public: + typedef GodotBroadPhase3D *(*CreateFunction)(); + + static CreateFunction create_func; + + typedef uint32_t ID; + + typedef void *(*PairCallback)(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_userdata); + typedef void (*UnpairCallback)(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_data, void *p_userdata); + + // 0 is an invalid ID + virtual ID create(GodotCollisionObject3D *p_object_, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false) = 0; + virtual void move(ID p_id, const AABB &p_aabb) = 0; + virtual void set_static(ID p_id, bool p_static) = 0; + virtual void remove(ID p_id) = 0; + + virtual GodotCollisionObject3D *get_object(ID p_id) const = 0; + virtual bool is_static(ID p_id) const = 0; + virtual int get_subindex(ID p_id) const = 0; + + virtual int cull_point(const Vector3 &p_point, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; + virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; + virtual int cull_aabb(const AABB &p_aabb, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; + + virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata) = 0; + virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) = 0; + + virtual void update() = 0; + + virtual ~GodotBroadPhase3D(); +}; + +#endif // GODOT_BROAD_PHASE_3D_H diff --git a/servers/physics_3d/godot_broad_phase_3d_bvh.cpp b/servers/physics_3d/godot_broad_phase_3d_bvh.cpp new file mode 100644 index 0000000000..0f2061a1ea --- /dev/null +++ b/servers/physics_3d/godot_broad_phase_3d_bvh.cpp @@ -0,0 +1,118 @@ +/*************************************************************************/ +/* godot_broad_phase_3d_bvh.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_broad_phase_3d_bvh.h" + +#include "godot_collision_object_3d.h" + +GodotBroadPhase3DBVH::ID GodotBroadPhase3DBVH::create(GodotCollisionObject3D *p_object, int p_subindex, const AABB &p_aabb, bool p_static) { + ID oid = bvh.create(p_object, true, p_aabb, p_subindex, !p_static, 1 << p_object->get_type(), p_static ? 0 : 0xFFFFF); // Pair everything, don't care? + return oid + 1; +} + +void GodotBroadPhase3DBVH::move(ID p_id, const AABB &p_aabb) { + bvh.move(p_id - 1, p_aabb); +} + +void GodotBroadPhase3DBVH::set_static(ID p_id, bool p_static) { + GodotCollisionObject3D *it = bvh.get(p_id - 1); + bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF, false); // Pair everything, don't care? +} + +void GodotBroadPhase3DBVH::remove(ID p_id) { + bvh.erase(p_id - 1); +} + +GodotCollisionObject3D *GodotBroadPhase3DBVH::get_object(ID p_id) const { + GodotCollisionObject3D *it = bvh.get(p_id - 1); + ERR_FAIL_COND_V(!it, nullptr); + return it; +} + +bool GodotBroadPhase3DBVH::is_static(ID p_id) const { + return !bvh.is_pairable(p_id - 1); +} + +int GodotBroadPhase3DBVH::get_subindex(ID p_id) const { + return bvh.get_subindex(p_id - 1); +} + +int GodotBroadPhase3DBVH::cull_point(const Vector3 &p_point, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices) { + return bvh.cull_point(p_point, p_results, p_max_results, p_result_indices); +} + +int GodotBroadPhase3DBVH::cull_segment(const Vector3 &p_from, const Vector3 &p_to, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices) { + return bvh.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices); +} + +int GodotBroadPhase3DBVH::cull_aabb(const AABB &p_aabb, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices) { + return bvh.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices); +} + +void *GodotBroadPhase3DBVH::_pair_callback(void *self, uint32_t p_A, GodotCollisionObject3D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject3D *p_object_B, int subindex_B) { + GodotBroadPhase3DBVH *bpo = (GodotBroadPhase3DBVH *)(self); + if (!bpo->pair_callback) { + return nullptr; + } + + return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata); +} + +void GodotBroadPhase3DBVH::_unpair_callback(void *self, uint32_t p_A, GodotCollisionObject3D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject3D *p_object_B, int subindex_B, void *pairdata) { + GodotBroadPhase3DBVH *bpo = (GodotBroadPhase3DBVH *)(self); + if (!bpo->unpair_callback) { + return; + } + + bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata); +} + +void GodotBroadPhase3DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { + pair_callback = p_pair_callback; + pair_userdata = p_userdata; +} + +void GodotBroadPhase3DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { + unpair_callback = p_unpair_callback; + unpair_userdata = p_userdata; +} + +void GodotBroadPhase3DBVH::update() { + bvh.update(); +} + +GodotBroadPhase3D *GodotBroadPhase3DBVH::_create() { + return memnew(GodotBroadPhase3DBVH); +} + +GodotBroadPhase3DBVH::GodotBroadPhase3DBVH() { + bvh.set_pair_callback(_pair_callback, this); + bvh.set_unpair_callback(_unpair_callback, this); +} diff --git a/servers/physics_3d/godot_broad_phase_3d_bvh.h b/servers/physics_3d/godot_broad_phase_3d_bvh.h new file mode 100644 index 0000000000..61127e52c1 --- /dev/null +++ b/servers/physics_3d/godot_broad_phase_3d_bvh.h @@ -0,0 +1,73 @@ +/*************************************************************************/ +/* godot_broad_phase_3d_bvh.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_BROAD_PHASE_3D_BVH_H +#define GODOT_BROAD_PHASE_3D_BVH_H + +#include "godot_broad_phase_3d.h" + +#include "core/math/bvh.h" + +class GodotBroadPhase3DBVH : public GodotBroadPhase3D { + BVH_Manager bvh; + + static void *_pair_callback(void *, uint32_t, GodotCollisionObject3D *, int, uint32_t, GodotCollisionObject3D *, int); + static void _unpair_callback(void *, uint32_t, GodotCollisionObject3D *, int, uint32_t, GodotCollisionObject3D *, int, void *); + + PairCallback pair_callback = nullptr; + void *pair_userdata = nullptr; + UnpairCallback unpair_callback = nullptr; + void *unpair_userdata = nullptr; + +public: + // 0 is an invalid ID + virtual ID create(GodotCollisionObject3D *p_object, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false); + virtual void move(ID p_id, const AABB &p_aabb); + virtual void set_static(ID p_id, bool p_static); + virtual void remove(ID p_id); + + virtual GodotCollisionObject3D *get_object(ID p_id) const; + virtual bool is_static(ID p_id) const; + virtual int get_subindex(ID p_id) const; + + virtual int cull_point(const Vector3 &p_point, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr); + virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr); + virtual int cull_aabb(const AABB &p_aabb, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr); + + virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata); + virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata); + + virtual void update(); + + static GodotBroadPhase3D *_create(); + GodotBroadPhase3DBVH(); +}; + +#endif // GODOT_BROAD_PHASE_3D_BVH_H diff --git a/servers/physics_3d/godot_collision_object_3d.cpp b/servers/physics_3d/godot_collision_object_3d.cpp new file mode 100644 index 0000000000..80a3d18ce0 --- /dev/null +++ b/servers/physics_3d/godot_collision_object_3d.cpp @@ -0,0 +1,241 @@ +/*************************************************************************/ +/* godot_collision_object_3d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_collision_object_3d.h" + +#include "godot_physics_server_3d.h" +#include "godot_space_3d.h" + +void GodotCollisionObject3D::add_shape(GodotShape3D *p_shape, const Transform3D &p_transform, bool p_disabled) { + Shape s; + s.shape = p_shape; + s.xform = p_transform; + s.xform_inv = s.xform.affine_inverse(); + s.bpid = 0; //needs update + s.disabled = p_disabled; + shapes.push_back(s); + p_shape->add_owner(this); + + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } +} + +void GodotCollisionObject3D::set_shape(int p_index, GodotShape3D *p_shape) { + ERR_FAIL_INDEX(p_index, shapes.size()); + shapes[p_index].shape->remove_owner(this); + shapes.write[p_index].shape = p_shape; + + p_shape->add_owner(this); + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } +} + +void GodotCollisionObject3D::set_shape_transform(int p_index, const Transform3D &p_transform) { + ERR_FAIL_INDEX(p_index, shapes.size()); + + shapes.write[p_index].xform = p_transform; + shapes.write[p_index].xform_inv = p_transform.affine_inverse(); + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } +} + +void GodotCollisionObject3D::set_shape_disabled(int p_idx, bool p_disabled) { + ERR_FAIL_INDEX(p_idx, shapes.size()); + + GodotCollisionObject3D::Shape &shape = shapes.write[p_idx]; + if (shape.disabled == p_disabled) { + return; + } + + shape.disabled = p_disabled; + + if (!space) { + return; + } + + if (p_disabled && shape.bpid != 0) { + space->get_broadphase()->remove(shape.bpid); + shape.bpid = 0; + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } + } else if (!p_disabled && shape.bpid == 0) { + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } + } +} + +void GodotCollisionObject3D::remove_shape(GodotShape3D *p_shape) { + //remove a shape, all the times it appears + for (int i = 0; i < shapes.size(); i++) { + if (shapes[i].shape == p_shape) { + remove_shape(i); + i--; + } + } +} + +void GodotCollisionObject3D::remove_shape(int p_index) { + //remove anything from shape to be erased to end, so subindices don't change + ERR_FAIL_INDEX(p_index, shapes.size()); + for (int i = p_index; i < shapes.size(); i++) { + if (shapes[i].bpid == 0) { + continue; + } + //should never get here with a null owner + space->get_broadphase()->remove(shapes[i].bpid); + shapes.write[i].bpid = 0; + } + shapes[p_index].shape->remove_owner(this); + shapes.remove(p_index); + + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } +} + +void GodotCollisionObject3D::_set_static(bool p_static) { + if (_static == p_static) { + return; + } + _static = p_static; + + if (!space) { + return; + } + for (int i = 0; i < get_shape_count(); i++) { + const Shape &s = shapes[i]; + if (s.bpid > 0) { + space->get_broadphase()->set_static(s.bpid, _static); + } + } +} + +void GodotCollisionObject3D::_unregister_shapes() { + for (int i = 0; i < shapes.size(); i++) { + Shape &s = shapes.write[i]; + if (s.bpid > 0) { + space->get_broadphase()->remove(s.bpid); + s.bpid = 0; + } + } +} + +void GodotCollisionObject3D::_update_shapes() { + if (!space) { + return; + } + + for (int i = 0; i < shapes.size(); i++) { + Shape &s = shapes.write[i]; + if (s.disabled) { + continue; + } + + //not quite correct, should compute the next matrix.. + AABB shape_aabb = s.shape->get_aabb(); + Transform3D xform = transform * s.xform; + shape_aabb = xform.xform(shape_aabb); + shape_aabb.grow_by((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05); + s.aabb_cache = shape_aabb; + + Vector3 scale = xform.get_basis().get_scale(); + s.area_cache = s.shape->get_area() * scale.x * scale.y * scale.z; + + if (s.bpid == 0) { + s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); + space->get_broadphase()->set_static(s.bpid, _static); + } + + space->get_broadphase()->move(s.bpid, shape_aabb); + } +} + +void GodotCollisionObject3D::_update_shapes_with_motion(const Vector3 &p_motion) { + if (!space) { + return; + } + + for (int i = 0; i < shapes.size(); i++) { + Shape &s = shapes.write[i]; + if (s.disabled) { + continue; + } + + //not quite correct, should compute the next matrix.. + AABB shape_aabb = s.shape->get_aabb(); + Transform3D xform = transform * s.xform; + shape_aabb = xform.xform(shape_aabb); + shape_aabb.merge_with(AABB(shape_aabb.position + p_motion, shape_aabb.size)); //use motion + s.aabb_cache = shape_aabb; + + if (s.bpid == 0) { + s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); + space->get_broadphase()->set_static(s.bpid, _static); + } + + space->get_broadphase()->move(s.bpid, shape_aabb); + } +} + +void GodotCollisionObject3D::_set_space(GodotSpace3D *p_space) { + if (space) { + space->remove_object(this); + + for (int i = 0; i < shapes.size(); i++) { + Shape &s = shapes.write[i]; + if (s.bpid) { + space->get_broadphase()->remove(s.bpid); + s.bpid = 0; + } + } + } + + space = p_space; + + if (space) { + space->add_object(this); + _update_shapes(); + } +} + +void GodotCollisionObject3D::_shape_changed() { + _update_shapes(); + _shapes_changed(); +} + +GodotCollisionObject3D::GodotCollisionObject3D(Type p_type) : + pending_shape_update_list(this) { + type = p_type; +} diff --git a/servers/physics_3d/godot_collision_object_3d.h b/servers/physics_3d/godot_collision_object_3d.h new file mode 100644 index 0000000000..43558034e0 --- /dev/null +++ b/servers/physics_3d/godot_collision_object_3d.h @@ -0,0 +1,186 @@ +/*************************************************************************/ +/* godot_collision_object_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_COLLISION_OBJECT_3D_H +#define GODOT_COLLISION_OBJECT_3D_H + +#include "godot_broad_phase_3d.h" +#include "godot_shape_3d.h" + +#include "core/templates/self_list.h" +#include "servers/physics_server_3d.h" + +#ifdef DEBUG_ENABLED +#define MAX_OBJECT_DISTANCE 3.1622776601683791e+18 + +#define MAX_OBJECT_DISTANCE_X2 (MAX_OBJECT_DISTANCE * MAX_OBJECT_DISTANCE) +#endif + +class GodotSpace3D; + +class GodotCollisionObject3D : public GodotShapeOwner3D { +public: + enum Type { + TYPE_AREA, + TYPE_BODY, + TYPE_SOFT_BODY, + }; + +private: + Type type; + RID self; + ObjectID instance_id; + uint32_t collision_layer = 1; + uint32_t collision_mask = 1; + + struct Shape { + Transform3D xform; + Transform3D xform_inv; + GodotBroadPhase3D::ID bpid; + AABB aabb_cache; //for rayqueries + real_t area_cache = 0.0; + GodotShape3D *shape = nullptr; + bool disabled = false; + }; + + Vector shapes; + GodotSpace3D *space = nullptr; + Transform3D transform; + Transform3D inv_transform; + bool _static = true; + + SelfList pending_shape_update_list; + + void _update_shapes(); + +protected: + void _update_shapes_with_motion(const Vector3 &p_motion); + void _unregister_shapes(); + + _FORCE_INLINE_ void _set_transform(const Transform3D &p_transform, bool p_update_shapes = true) { +#ifdef DEBUG_ENABLED + + ERR_FAIL_COND_MSG(p_transform.origin.length_squared() > MAX_OBJECT_DISTANCE_X2, "Object went too far away (more than '" + itos(MAX_OBJECT_DISTANCE) + "' units from origin)."); +#endif + + transform = p_transform; + if (p_update_shapes) { + _update_shapes(); + } + } + _FORCE_INLINE_ void _set_inv_transform(const Transform3D &p_transform) { inv_transform = p_transform; } + void _set_static(bool p_static); + + virtual void _shapes_changed() = 0; + void _set_space(GodotSpace3D *p_space); + + bool ray_pickable = true; + + GodotCollisionObject3D(Type p_type); + +public: + _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + _FORCE_INLINE_ void set_instance_id(const ObjectID &p_instance_id) { instance_id = p_instance_id; } + _FORCE_INLINE_ ObjectID get_instance_id() const { return instance_id; } + + void _shape_changed(); + + _FORCE_INLINE_ Type get_type() const { return type; } + void add_shape(GodotShape3D *p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false); + void set_shape(int p_index, GodotShape3D *p_shape); + void set_shape_transform(int p_index, const Transform3D &p_transform); + _FORCE_INLINE_ int get_shape_count() const { return shapes.size(); } + _FORCE_INLINE_ GodotShape3D *get_shape(int p_index) const { + CRASH_BAD_INDEX(p_index, shapes.size()); + return shapes[p_index].shape; + } + _FORCE_INLINE_ const Transform3D &get_shape_transform(int p_index) const { + CRASH_BAD_INDEX(p_index, shapes.size()); + return shapes[p_index].xform; + } + _FORCE_INLINE_ const Transform3D &get_shape_inv_transform(int p_index) const { + CRASH_BAD_INDEX(p_index, shapes.size()); + return shapes[p_index].xform_inv; + } + _FORCE_INLINE_ const AABB &get_shape_aabb(int p_index) const { + CRASH_BAD_INDEX(p_index, shapes.size()); + return shapes[p_index].aabb_cache; + } + _FORCE_INLINE_ real_t get_shape_area(int p_index) const { + CRASH_BAD_INDEX(p_index, shapes.size()); + return shapes[p_index].area_cache; + } + + _FORCE_INLINE_ const Transform3D &get_transform() const { return transform; } + _FORCE_INLINE_ const Transform3D &get_inv_transform() const { return inv_transform; } + _FORCE_INLINE_ GodotSpace3D *get_space() const { return space; } + + _FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; } + _FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; } + + void set_shape_disabled(int p_idx, bool p_disabled); + _FORCE_INLINE_ bool is_shape_disabled(int p_idx) const { + ERR_FAIL_INDEX_V(p_idx, shapes.size(), false); + return shapes[p_idx].disabled; + } + + _FORCE_INLINE_ void set_collision_layer(uint32_t p_layer) { + collision_layer = p_layer; + _shape_changed(); + } + _FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; } + + _FORCE_INLINE_ void set_collision_mask(uint32_t p_mask) { + collision_mask = p_mask; + _shape_changed(); + } + _FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; } + + _FORCE_INLINE_ bool collides_with(GodotCollisionObject3D *p_other) const { + return p_other->collision_layer & collision_mask; + } + + _FORCE_INLINE_ bool interacts_with(GodotCollisionObject3D *p_other) const { + return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask; + } + + void remove_shape(GodotShape3D *p_shape); + void remove_shape(int p_index); + + virtual void set_space(GodotSpace3D *p_space) = 0; + + _FORCE_INLINE_ bool is_static() const { return _static; } + + virtual ~GodotCollisionObject3D() {} +}; + +#endif // GODOT_COLLISION_OBJECT_3D_H diff --git a/servers/physics_3d/godot_collision_solver_3d.cpp b/servers/physics_3d/godot_collision_solver_3d.cpp new file mode 100644 index 0000000000..b9f2f7506b --- /dev/null +++ b/servers/physics_3d/godot_collision_solver_3d.cpp @@ -0,0 +1,572 @@ +/*************************************************************************/ +/* godot_collision_solver_3d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_collision_solver_3d.h" +#include "godot_collision_solver_3d_sat.h" +#include "godot_soft_body_3d.h" + +#include "gjk_epa.h" + +#define collision_solver sat_calculate_penetration +//#define collision_solver gjk_epa_calculate_penetration + +bool GodotCollisionSolver3D::solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { + const GodotWorldBoundaryShape3D *world_boundary = static_cast(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { + return false; + } + Plane p = p_transform_A.xform(world_boundary->get_plane()); + + static const int max_supports = 16; + Vector3 supports[max_supports]; + int support_count; + GodotShape3D::FeatureType support_type; + p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type); + + if (support_type == GodotShape3D::FEATURE_CIRCLE) { + ERR_FAIL_COND_V(support_count != 3, false); + + Vector3 circle_pos = supports[0]; + Vector3 circle_axis_1 = supports[1] - circle_pos; + Vector3 circle_axis_2 = supports[2] - circle_pos; + + // Use 3 equidistant points on the circle. + for (int i = 0; i < 3; ++i) { + Vector3 vertex_pos = circle_pos; + vertex_pos += circle_axis_1 * Math::cos(2.0 * Math_PI * i / 3.0); + vertex_pos += circle_axis_2 * Math::sin(2.0 * Math_PI * i / 3.0); + supports[i] = vertex_pos; + } + } + + bool found = false; + + for (int i = 0; i < support_count; i++) { + supports[i] = p_transform_B.xform(supports[i]); + if (p.distance_to(supports[i]) >= 0) { + continue; + } + found = true; + + Vector3 support_A = p.project(supports[i]); + + if (p_result_callback) { + if (p_swap_result) { + p_result_callback(supports[i], 0, support_A, 0, p_userdata); + } else { + p_result_callback(support_A, 0, supports[i], 0, p_userdata); + } + } + } + + return found; +} + +bool GodotCollisionSolver3D::solve_separation_ray(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) { + const GodotSeparationRayShape3D *ray = static_cast(p_shape_A); + + Vector3 from = p_transform_A.origin; + Vector3 to = from + p_transform_A.basis.get_axis(2) * (ray->get_length() + p_margin); + Vector3 support_A = to; + + Transform3D ai = p_transform_B.affine_inverse(); + + from = ai.xform(from); + to = ai.xform(to); + + Vector3 p, n; + if (!p_shape_B->intersect_segment(from, to, p, n)) { + return false; + } + + // Discard contacts when the ray is fully contained inside the shape. + if (n == Vector3()) { + return false; + } + + // Discard contacts in the wrong direction. + if (n.dot(from - to) < CMP_EPSILON) { + return false; + } + + Vector3 support_B = p_transform_B.xform(p); + if (ray->get_slide_on_slope()) { + Vector3 global_n = ai.basis.xform_inv(n).normalized(); + support_B = support_A + (support_B - support_A).length() * global_n; + } + + if (p_result_callback) { + if (p_swap_result) { + p_result_callback(support_B, 0, support_A, 0, p_userdata); + } else { + p_result_callback(support_A, 0, support_B, 0, p_userdata); + } + } + return true; +} + +struct _SoftBodyContactCollisionInfo { + int node_index = 0; + GodotCollisionSolver3D::CallbackResult result_callback = nullptr; + void *userdata = nullptr; + bool swap_result = false; + int contact_count = 0; +}; + +void GodotCollisionSolver3D::soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { + _SoftBodyContactCollisionInfo &cinfo = *(_SoftBodyContactCollisionInfo *)(p_userdata); + + ++cinfo.contact_count; + + if (!cinfo.result_callback) { + return; + } + + if (cinfo.swap_result) { + cinfo.result_callback(p_point_B, cinfo.node_index, p_point_A, p_index_A, cinfo.userdata); + } else { + cinfo.result_callback(p_point_A, p_index_A, p_point_B, cinfo.node_index, cinfo.userdata); + } +} + +struct _SoftBodyQueryInfo { + GodotSoftBody3D *soft_body = nullptr; + const GodotShape3D *shape_A = nullptr; + const GodotShape3D *shape_B = nullptr; + Transform3D transform_A; + Transform3D node_transform; + _SoftBodyContactCollisionInfo contact_info; +#ifdef DEBUG_ENABLED + int node_query_count = 0; + int convex_query_count = 0; +#endif +}; + +bool GodotCollisionSolver3D::soft_body_query_callback(uint32_t p_node_index, void *p_userdata) { + _SoftBodyQueryInfo &query_cinfo = *(_SoftBodyQueryInfo *)(p_userdata); + + Vector3 node_position = query_cinfo.soft_body->get_node_position(p_node_index); + + Transform3D transform_B; + transform_B.origin = query_cinfo.node_transform.xform(node_position); + + query_cinfo.contact_info.node_index = p_node_index; + bool collided = solve_static(query_cinfo.shape_A, query_cinfo.transform_A, query_cinfo.shape_B, transform_B, soft_body_contact_callback, &query_cinfo.contact_info); + +#ifdef DEBUG_ENABLED + ++query_cinfo.node_query_count; +#endif + + // Stop at first collision if contacts are not needed. + return (collided && !query_cinfo.contact_info.result_callback); +} + +bool GodotCollisionSolver3D::soft_body_concave_callback(void *p_userdata, GodotShape3D *p_convex) { + _SoftBodyQueryInfo &query_cinfo = *(_SoftBodyQueryInfo *)(p_userdata); + + query_cinfo.shape_A = p_convex; + + // Calculate AABB for internal soft body query (in world space). + AABB shape_aabb; + for (int i = 0; i < 3; i++) { + Vector3 axis; + axis[i] = 1.0; + + real_t smin, smax; + p_convex->project_range(axis, query_cinfo.transform_A, smin, smax); + + shape_aabb.position[i] = smin; + shape_aabb.size[i] = smax - smin; + } + + shape_aabb.grow_by(query_cinfo.soft_body->get_collision_margin()); + + query_cinfo.soft_body->query_aabb(shape_aabb, soft_body_query_callback, &query_cinfo); + + bool collided = (query_cinfo.contact_info.contact_count > 0); + +#ifdef DEBUG_ENABLED + ++query_cinfo.convex_query_count; +#endif + + // Stop at first collision if contacts are not needed. + return (collided && !query_cinfo.contact_info.result_callback); +} + +bool GodotCollisionSolver3D::solve_soft_body(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { + const GodotSoftBodyShape3D *soft_body_shape_B = static_cast(p_shape_B); + + GodotSoftBody3D *soft_body = soft_body_shape_B->get_soft_body(); + const Transform3D &world_to_local = soft_body->get_inv_transform(); + + const real_t collision_margin = soft_body->get_collision_margin(); + + GodotSphereShape3D sphere_shape; + sphere_shape.set_data(collision_margin); + + _SoftBodyQueryInfo query_cinfo; + query_cinfo.contact_info.result_callback = p_result_callback; + query_cinfo.contact_info.userdata = p_userdata; + query_cinfo.contact_info.swap_result = p_swap_result; + query_cinfo.soft_body = soft_body; + query_cinfo.node_transform = p_transform_B * world_to_local; + query_cinfo.shape_A = p_shape_A; + query_cinfo.transform_A = p_transform_A; + query_cinfo.shape_B = &sphere_shape; + + if (p_shape_A->is_concave()) { + // In case of concave shape, query convex shapes first. + const GodotConcaveShape3D *concave_shape_A = static_cast(p_shape_A); + + AABB soft_body_aabb = soft_body->get_bounds(); + soft_body_aabb.grow_by(collision_margin); + + // Calculate AABB for internal concave shape query (in local space). + AABB local_aabb; + for (int i = 0; i < 3; i++) { + Vector3 axis(p_transform_A.basis.get_axis(i)); + real_t axis_scale = 1.0 / axis.length(); + + real_t smin = soft_body_aabb.position[i]; + real_t smax = smin + soft_body_aabb.size[i]; + + smin *= axis_scale; + smax *= axis_scale; + + local_aabb.position[i] = smin; + local_aabb.size[i] = smax - smin; + } + + concave_shape_A->cull(local_aabb, soft_body_concave_callback, &query_cinfo); + } else { + AABB shape_aabb = p_transform_A.xform(p_shape_A->get_aabb()); + shape_aabb.grow_by(collision_margin); + + soft_body->query_aabb(shape_aabb, soft_body_query_callback, &query_cinfo); + } + + return (query_cinfo.contact_info.contact_count > 0); +} + +struct _ConcaveCollisionInfo { + const Transform3D *transform_A; + const GodotShape3D *shape_A; + const Transform3D *transform_B; + GodotCollisionSolver3D::CallbackResult result_callback; + void *userdata; + bool swap_result; + bool collided; + int aabb_tests; + int collisions; + bool tested; + real_t margin_A; + real_t margin_B; + Vector3 close_A, close_B; +}; + +bool GodotCollisionSolver3D::concave_callback(void *p_userdata, GodotShape3D *p_convex) { + _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata); + cinfo.aabb_tests++; + + bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, nullptr, cinfo.margin_A, cinfo.margin_B); + if (!collided) { + return false; + } + + cinfo.collided = true; + cinfo.collisions++; + + // Stop at first collision if contacts are not needed. + return !cinfo.result_callback; +} + +bool GodotCollisionSolver3D::solve_concave(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A, real_t p_margin_B) { + const GodotConcaveShape3D *concave_B = static_cast(p_shape_B); + + _ConcaveCollisionInfo cinfo; + cinfo.transform_A = &p_transform_A; + cinfo.shape_A = p_shape_A; + cinfo.transform_B = &p_transform_B; + cinfo.result_callback = p_result_callback; + cinfo.userdata = p_userdata; + cinfo.swap_result = p_swap_result; + cinfo.collided = false; + cinfo.collisions = 0; + cinfo.margin_A = p_margin_A; + cinfo.margin_B = p_margin_B; + + cinfo.aabb_tests = 0; + + Transform3D rel_transform = p_transform_A; + rel_transform.origin -= p_transform_B.origin; + + //quickly compute a local AABB + + AABB local_aabb; + for (int i = 0; i < 3; i++) { + Vector3 axis(p_transform_B.basis.get_axis(i)); + real_t axis_scale = 1.0 / axis.length(); + axis *= axis_scale; + + real_t smin, smax; + p_shape_A->project_range(axis, rel_transform, smin, smax); + smin -= p_margin_A; + smax += p_margin_A; + smin *= axis_scale; + smax *= axis_scale; + + local_aabb.position[i] = smin; + local_aabb.size[i] = smax - smin; + } + + concave_B->cull(local_aabb, concave_callback, &cinfo); + + return cinfo.collided; +} + +bool GodotCollisionSolver3D::solve_static(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { + PhysicsServer3D::ShapeType type_A = p_shape_A->get_type(); + PhysicsServer3D::ShapeType type_B = p_shape_B->get_type(); + bool concave_A = p_shape_A->is_concave(); + bool concave_B = p_shape_B->is_concave(); + + bool swap = false; + + if (type_A > type_B) { + SWAP(type_A, type_B); + SWAP(concave_A, concave_B); + swap = true; + } + + if (type_A == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { + if (type_B == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { + return false; + } + if (type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY) { + return false; + } + if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) { + return false; + } + + if (swap) { + return solve_static_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); + } else { + return solve_static_world_boundary(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); + } + + } else if (type_A == PhysicsServer3D::SHAPE_SEPARATION_RAY) { + if (type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY) { + return false; + } + + if (swap) { + return solve_separation_ray(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, p_margin_B); + } else { + return solve_separation_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, p_margin_A); + } + + } else if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) { + if (type_A == PhysicsServer3D::SHAPE_SOFT_BODY) { + // Soft Body / Soft Body not supported. + return false; + } + + if (swap) { + return solve_soft_body(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); + } else { + return solve_soft_body(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); + } + + } else if (concave_B) { + if (concave_A) { + return false; + } + + if (!swap) { + return solve_concave(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, p_margin_A, p_margin_B); + } else { + return solve_concave(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, p_margin_A, p_margin_B); + } + + } else { + return collision_solver(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, r_sep_axis, p_margin_A, p_margin_B); + } +} + +bool GodotCollisionSolver3D::concave_distance_callback(void *p_userdata, GodotShape3D *p_convex) { + _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata); + cinfo.aabb_tests++; + + Vector3 close_A, close_B; + cinfo.collided = !gjk_epa_calculate_distance(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, close_A, close_B); + + if (cinfo.collided) { + // No need to process any more result. + return true; + } + + if (!cinfo.tested || close_A.distance_squared_to(close_B) < cinfo.close_A.distance_squared_to(cinfo.close_B)) { + cinfo.close_A = close_A; + cinfo.close_B = close_B; + cinfo.tested = true; + } + + cinfo.collisions++; + return false; +} + +bool GodotCollisionSolver3D::solve_distance_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) { + const GodotWorldBoundaryShape3D *world_boundary = static_cast(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { + return false; + } + Plane p = p_transform_A.xform(world_boundary->get_plane()); + + static const int max_supports = 16; + Vector3 supports[max_supports]; + int support_count; + GodotShape3D::FeatureType support_type; + + p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type); + + if (support_type == GodotShape3D::FEATURE_CIRCLE) { + ERR_FAIL_COND_V(support_count != 3, false); + + Vector3 circle_pos = supports[0]; + Vector3 circle_axis_1 = supports[1] - circle_pos; + Vector3 circle_axis_2 = supports[2] - circle_pos; + + // Use 3 equidistant points on the circle. + for (int i = 0; i < 3; ++i) { + Vector3 vertex_pos = circle_pos; + vertex_pos += circle_axis_1 * Math::cos(2.0 * Math_PI * i / 3.0); + vertex_pos += circle_axis_2 * Math::sin(2.0 * Math_PI * i / 3.0); + supports[i] = vertex_pos; + } + } + + bool collided = false; + Vector3 closest; + real_t closest_d = 0; + + for (int i = 0; i < support_count; i++) { + supports[i] = p_transform_B.xform(supports[i]); + real_t d = p.distance_to(supports[i]); + if (i == 0 || d < closest_d) { + closest = supports[i]; + closest_d = d; + if (d <= 0) { + collided = true; + } + } + } + + r_point_A = p.project(closest); + r_point_B = closest; + + return collided; +} + +bool GodotCollisionSolver3D::solve_distance(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis) { + if (p_shape_A->is_concave()) { + return false; + } + + if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { + Vector3 a, b; + bool col = solve_distance_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, a, b); + r_point_A = b; + r_point_B = a; + return !col; + + } else if (p_shape_B->is_concave()) { + if (p_shape_A->is_concave()) { + return false; + } + + const GodotConcaveShape3D *concave_B = static_cast(p_shape_B); + + _ConcaveCollisionInfo cinfo; + cinfo.transform_A = &p_transform_A; + cinfo.shape_A = p_shape_A; + cinfo.transform_B = &p_transform_B; + cinfo.result_callback = nullptr; + cinfo.userdata = nullptr; + cinfo.swap_result = false; + cinfo.collided = false; + cinfo.collisions = 0; + cinfo.aabb_tests = 0; + cinfo.tested = false; + + Transform3D rel_transform = p_transform_A; + rel_transform.origin -= p_transform_B.origin; + + //quickly compute a local AABB + + bool use_cc_hint = p_concave_hint != AABB(); + AABB cc_hint_aabb; + if (use_cc_hint) { + cc_hint_aabb = p_concave_hint; + cc_hint_aabb.position -= p_transform_B.origin; + } + + AABB local_aabb; + for (int i = 0; i < 3; i++) { + Vector3 axis(p_transform_B.basis.get_axis(i)); + real_t axis_scale = ((real_t)1.0) / axis.length(); + axis *= axis_scale; + + real_t smin, smax; + + if (use_cc_hint) { + cc_hint_aabb.project_range_in_plane(Plane(axis), smin, smax); + } else { + p_shape_A->project_range(axis, rel_transform, smin, smax); + } + + smin *= axis_scale; + smax *= axis_scale; + + local_aabb.position[i] = smin; + local_aabb.size[i] = smax - smin; + } + + concave_B->cull(local_aabb, concave_distance_callback, &cinfo); + if (!cinfo.collided) { + r_point_A = cinfo.close_A; + r_point_B = cinfo.close_B; + } + + return !cinfo.collided; + } else { + return gjk_epa_calculate_distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, r_point_A, r_point_B); //should pass sepaxis.. + } +} diff --git a/servers/physics_3d/godot_collision_solver_3d.h b/servers/physics_3d/godot_collision_solver_3d.h new file mode 100644 index 0000000000..133635ca7e --- /dev/null +++ b/servers/physics_3d/godot_collision_solver_3d.h @@ -0,0 +1,57 @@ +/*************************************************************************/ +/* godot_collision_solver_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_COLLISION_SOLVER_3D_H +#define GODOT_COLLISION_SOLVER_3D_H + +#include "godot_shape_3d.h" + +class GodotCollisionSolver3D { +public: + typedef void (*CallbackResult)(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); + +private: + static bool soft_body_query_callback(uint32_t p_node_index, void *p_userdata); + static void soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); + static bool soft_body_concave_callback(void *p_userdata, GodotShape3D *p_convex); + static bool concave_callback(void *p_userdata, GodotShape3D *p_convex); + static bool solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool solve_separation_ray(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0); + static bool solve_soft_body(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool solve_concave(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A = 0, real_t p_margin_B = 0); + static bool concave_distance_callback(void *p_userdata, GodotShape3D *p_convex); + static bool solve_distance_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B); + +public: + static bool solve_static(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); + static bool solve_distance(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis = nullptr); +}; + +#endif // GODOT_COLLISION_SOLVER_3D_H diff --git a/servers/physics_3d/godot_collision_solver_3d_sat.cpp b/servers/physics_3d/godot_collision_solver_3d_sat.cpp new file mode 100644 index 0000000000..0790333f65 --- /dev/null +++ b/servers/physics_3d/godot_collision_solver_3d_sat.cpp @@ -0,0 +1,2397 @@ +/*************************************************************************/ +/* godot_collision_solver_3d_sat.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_collision_solver_3d_sat.h" + +#include "gjk_epa.h" + +#include "core/math/geometry_3d.h" + +#define fallback_collision_solver gjk_epa_calculate_penetration + +// Cylinder SAT analytic methods and face-circle contact points for cylinder-trimesh and cylinder-box collision are based on ODE colliders. + +/* + * Cylinder-trimesh and Cylinder-box colliders by Alen Ladavac + * Ported to ODE by Nguyen Binh + */ + +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +struct _CollectorCallback { + GodotCollisionSolver3D::CallbackResult callback; + void *userdata = nullptr; + bool swap = false; + bool collided = false; + Vector3 normal; + Vector3 *prev_axis = nullptr; + + _FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B) { + if (swap) { + callback(p_point_B, 0, p_point_A, 0, userdata); + } else { + callback(p_point_A, 0, p_point_B, 0, userdata); + } + } +}; + +typedef void (*GenerateContactsFunc)(const Vector3 *, int, const Vector3 *, int, _CollectorCallback *); + +static void _generate_contacts_point_point(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 1); + ERR_FAIL_COND(p_point_count_B != 1); +#endif + + p_callback->call(*p_points_A, *p_points_B); +} + +static void _generate_contacts_point_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 1); + ERR_FAIL_COND(p_point_count_B != 2); +#endif + + Vector3 closest_B = Geometry3D::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B); + p_callback->call(*p_points_A, closest_B); +} + +static void _generate_contacts_point_face(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 1); + ERR_FAIL_COND(p_point_count_B < 3); +#endif + + Vector3 closest_B = Plane(p_points_B[0], p_points_B[1], p_points_B[2]).project(*p_points_A); + + p_callback->call(*p_points_A, closest_B); +} + +static void _generate_contacts_point_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 1); + ERR_FAIL_COND(p_point_count_B != 3); +#endif + + Vector3 closest_B = Plane(p_points_B[0], p_points_B[1], p_points_B[2]).project(*p_points_A); + + p_callback->call(*p_points_A, closest_B); +} + +static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 2); + ERR_FAIL_COND(p_point_count_B != 2); // circle is actually a 4x3 matrix +#endif + + Vector3 rel_A = p_points_A[1] - p_points_A[0]; + Vector3 rel_B = p_points_B[1] - p_points_B[0]; + + Vector3 c = rel_A.cross(rel_B).cross(rel_B); + + if (Math::is_zero_approx(rel_A.dot(c))) { + // should handle somehow.. + //ERR_PRINT("TODO FIX"); + //return; + + Vector3 axis = rel_A.normalized(); //make an axis + Vector3 base_A = p_points_A[0] - axis * axis.dot(p_points_A[0]); + Vector3 base_B = p_points_B[0] - axis * axis.dot(p_points_B[0]); + + //sort all 4 points in axis + real_t dvec[4] = { axis.dot(p_points_A[0]), axis.dot(p_points_A[1]), axis.dot(p_points_B[0]), axis.dot(p_points_B[1]) }; + + SortArray sa; + sa.sort(dvec, 4); + + //use the middle ones as contacts + p_callback->call(base_A + axis * dvec[1], base_B + axis * dvec[1]); + p_callback->call(base_A + axis * dvec[2], base_B + axis * dvec[2]); + + return; + } + + real_t d = (c.dot(p_points_B[0]) - p_points_A[0].dot(c)) / rel_A.dot(c); + + if (d < 0.0) { + d = 0.0; + } else if (d > 1.0) { + d = 1.0; + } + + Vector3 closest_A = p_points_A[0] + rel_A * d; + Vector3 closest_B = Geometry3D::get_closest_point_to_segment_uncapped(closest_A, p_points_B); + p_callback->call(closest_A, closest_B); +} + +static void _generate_contacts_edge_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 2); + ERR_FAIL_COND(p_point_count_B != 3); +#endif + + const Vector3 &circle_B_pos = p_points_B[0]; + Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos; + Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos; + + real_t circle_B_radius = circle_B_line_1.length(); + Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized(); + + Plane circle_plane(circle_B_normal, circle_B_pos); + + static const int max_clip = 2; + Vector3 contact_points[max_clip]; + int num_points = 0; + + // Project edge point in circle plane. + const Vector3 &edge_A_1 = p_points_A[0]; + Vector3 proj_point_1 = circle_plane.project(edge_A_1); + + Vector3 dist_vec = proj_point_1 - circle_B_pos; + real_t dist_sq = dist_vec.length_squared(); + + // Point 1 is inside disk, add as contact point. + if (dist_sq <= circle_B_radius * circle_B_radius) { + contact_points[num_points] = edge_A_1; + ++num_points; + } + + const Vector3 &edge_A_2 = p_points_A[1]; + Vector3 proj_point_2 = circle_plane.project(edge_A_2); + + Vector3 dist_vec_2 = proj_point_2 - circle_B_pos; + real_t dist_sq_2 = dist_vec_2.length_squared(); + + // Point 2 is inside disk, add as contact point. + if (dist_sq_2 <= circle_B_radius * circle_B_radius) { + contact_points[num_points] = edge_A_2; + ++num_points; + } + + if (num_points < 2) { + Vector3 line_vec = proj_point_2 - proj_point_1; + real_t line_length_sq = line_vec.length_squared(); + + // Create a quadratic formula of the form ax^2 + bx + c = 0 + real_t a, b, c; + + a = line_length_sq; + b = 2.0 * dist_vec.dot(line_vec); + c = dist_sq - circle_B_radius * circle_B_radius; + + // Solve for t. + real_t sqrtterm = b * b - 4.0 * a * c; + + // If the term we intend to square root is less than 0 then the answer won't be real, + // so the line doesn't intersect. + if (sqrtterm >= 0) { + sqrtterm = Math::sqrt(sqrtterm); + + Vector3 edge_dir = edge_A_2 - edge_A_1; + + real_t fraction_1 = (-b - sqrtterm) / (2.0 * a); + if ((fraction_1 > 0.0) && (fraction_1 < 1.0)) { + Vector3 face_point_1 = edge_A_1 + fraction_1 * edge_dir; + ERR_FAIL_COND(num_points >= max_clip); + contact_points[num_points] = face_point_1; + ++num_points; + } + + real_t fraction_2 = (-b + sqrtterm) / (2.0 * a); + if ((fraction_2 > 0.0) && (fraction_2 < 1.0) && !Math::is_equal_approx(fraction_1, fraction_2)) { + Vector3 face_point_2 = edge_A_1 + fraction_2 * edge_dir; + ERR_FAIL_COND(num_points >= max_clip); + contact_points[num_points] = face_point_2; + ++num_points; + } + } + } + + // Generate contact points. + for (int i = 0; i < num_points; i++) { + const Vector3 &contact_point_A = contact_points[i]; + + real_t d = circle_plane.distance_to(contact_point_A); + Vector3 closest_B = contact_point_A - circle_plane.normal * d; + + if (p_callback->normal.dot(contact_point_A) >= p_callback->normal.dot(closest_B)) { + continue; + } + + p_callback->call(contact_point_A, closest_B); + } +} + +static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A < 2); + ERR_FAIL_COND(p_point_count_B < 3); +#endif + + static const int max_clip = 32; + + Vector3 _clipbuf1[max_clip]; + Vector3 _clipbuf2[max_clip]; + Vector3 *clipbuf_src = _clipbuf1; + Vector3 *clipbuf_dst = _clipbuf2; + int clipbuf_len = p_point_count_A; + + // copy A points to clipbuf_src + for (int i = 0; i < p_point_count_A; i++) { + clipbuf_src[i] = p_points_A[i]; + } + + Plane plane_B(p_points_B[0], p_points_B[1], p_points_B[2]); + + // go through all of B points + for (int i = 0; i < p_point_count_B; i++) { + int i_n = (i + 1) % p_point_count_B; + + Vector3 edge0_B = p_points_B[i]; + Vector3 edge1_B = p_points_B[i_n]; + + Vector3 clip_normal = (edge0_B - edge1_B).cross(plane_B.normal).normalized(); + // make a clip plane + + Plane clip(clip_normal, edge0_B); + // avoid double clip if A is edge + int dst_idx = 0; + bool edge = clipbuf_len == 2; + for (int j = 0; j < clipbuf_len; j++) { + int j_n = (j + 1) % clipbuf_len; + + Vector3 edge0_A = clipbuf_src[j]; + Vector3 edge1_A = clipbuf_src[j_n]; + + real_t dist0 = clip.distance_to(edge0_A); + real_t dist1 = clip.distance_to(edge1_A); + + if (dist0 <= 0) { // behind plane + + ERR_FAIL_COND(dst_idx >= max_clip); + clipbuf_dst[dst_idx++] = clipbuf_src[j]; + } + + // check for different sides and non coplanar + //if ( (dist0*dist1) < -CMP_EPSILON && !(edge && j)) { + if ((dist0 * dist1) < 0 && !(edge && j)) { + // calculate intersection + Vector3 rel = edge1_A - edge0_A; + real_t den = clip.normal.dot(rel); + real_t dist = -(clip.normal.dot(edge0_A) - clip.d) / den; + Vector3 inters = edge0_A + rel * dist; + + ERR_FAIL_COND(dst_idx >= max_clip); + clipbuf_dst[dst_idx] = inters; + dst_idx++; + } + } + + clipbuf_len = dst_idx; + SWAP(clipbuf_src, clipbuf_dst); + } + + // generate contacts + //Plane plane_A(p_points_A[0],p_points_A[1],p_points_A[2]); + + for (int i = 0; i < clipbuf_len; i++) { + real_t d = plane_B.distance_to(clipbuf_src[i]); + /* + if (d>CMP_EPSILON) + continue; + */ + + Vector3 closest_B = clipbuf_src[i] - plane_B.normal * d; + + if (p_callback->normal.dot(clipbuf_src[i]) >= p_callback->normal.dot(closest_B)) { + continue; + } + + p_callback->call(clipbuf_src[i], closest_B); + } +} + +static void _generate_contacts_face_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A < 3); + ERR_FAIL_COND(p_point_count_B != 3); +#endif + + const Vector3 &circle_B_pos = p_points_B[0]; + Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos; + Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos; + + // Clip face with circle segments. + static const int circle_segments = 8; + Vector3 circle_points[circle_segments]; + + real_t angle_delta = 2.0 * Math_PI / circle_segments; + + for (int i = 0; i < circle_segments; ++i) { + Vector3 point_pos = circle_B_pos; + point_pos += circle_B_line_1 * Math::cos(i * angle_delta); + point_pos += circle_B_line_2 * Math::sin(i * angle_delta); + circle_points[i] = point_pos; + } + + _generate_contacts_face_face(p_points_A, p_point_count_A, circle_points, circle_segments, p_callback); + + // Clip face with circle plane. + Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized(); + + Plane circle_plane(circle_B_normal, circle_B_pos); + + static const int max_clip = 32; + Vector3 contact_points[max_clip]; + int num_points = 0; + + for (int i = 0; i < p_point_count_A; i++) { + int i_n = (i + 1) % p_point_count_A; + + const Vector3 &edge0_A = p_points_A[i]; + const Vector3 &edge1_A = p_points_A[i_n]; + + real_t dist0 = circle_plane.distance_to(edge0_A); + real_t dist1 = circle_plane.distance_to(edge1_A); + + // First point in front of plane, generate contact point. + if (dist0 * circle_plane.d >= 0) { + ERR_FAIL_COND(num_points >= max_clip); + contact_points[num_points] = edge0_A; + ++num_points; + } + + // Points on different sides, generate contact point. + if (dist0 * dist1 < 0) { + // calculate intersection + Vector3 rel = edge1_A - edge0_A; + real_t den = circle_plane.normal.dot(rel); + real_t dist = -(circle_plane.normal.dot(edge0_A) - circle_plane.d) / den; + Vector3 inters = edge0_A + rel * dist; + + ERR_FAIL_COND(num_points >= max_clip); + contact_points[num_points] = inters; + ++num_points; + } + } + + // Generate contact points. + for (int i = 0; i < num_points; i++) { + const Vector3 &contact_point_A = contact_points[i]; + + real_t d = circle_plane.distance_to(contact_point_A); + Vector3 closest_B = contact_point_A - circle_plane.normal * d; + + if (p_callback->normal.dot(contact_point_A) >= p_callback->normal.dot(closest_B)) { + continue; + } + + p_callback->call(contact_point_A, closest_B); + } +} + +static void _generate_contacts_circle_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 3); + ERR_FAIL_COND(p_point_count_B != 3); +#endif + + const Vector3 &circle_A_pos = p_points_A[0]; + Vector3 circle_A_line_1 = p_points_A[1] - circle_A_pos; + Vector3 circle_A_line_2 = p_points_A[2] - circle_A_pos; + + real_t circle_A_radius = circle_A_line_1.length(); + Vector3 circle_A_normal = circle_A_line_1.cross(circle_A_line_2).normalized(); + + const Vector3 &circle_B_pos = p_points_B[0]; + Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos; + Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos; + + real_t circle_B_radius = circle_B_line_1.length(); + Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized(); + + static const int max_clip = 4; + Vector3 contact_points[max_clip]; + int num_points = 0; + + Vector3 centers_diff = circle_B_pos - circle_A_pos; + Vector3 norm_proj = circle_A_normal.dot(centers_diff) * circle_A_normal; + Vector3 comp_proj = centers_diff - norm_proj; + real_t proj_dist = comp_proj.length(); + if (!Math::is_zero_approx(proj_dist)) { + comp_proj /= proj_dist; + if ((proj_dist > circle_A_radius - circle_B_radius) && (proj_dist > circle_B_radius - circle_A_radius)) { + // Circles are overlapping, use the 2 points of intersection as contacts. + real_t radius_a_sqr = circle_A_radius * circle_A_radius; + real_t radius_b_sqr = circle_B_radius * circle_B_radius; + real_t d_sqr = proj_dist * proj_dist; + real_t s = (1.0 + (radius_a_sqr - radius_b_sqr) / d_sqr) * 0.5; + real_t h = Math::sqrt(MAX(radius_a_sqr - d_sqr * s * s, 0.0)); + Vector3 midpoint = circle_A_pos + s * comp_proj * proj_dist; + Vector3 h_vec = h * circle_A_normal.cross(comp_proj); + + Vector3 point_A = midpoint + h_vec; + contact_points[num_points] = point_A; + ++num_points; + + point_A = midpoint - h_vec; + contact_points[num_points] = point_A; + ++num_points; + + // Add 2 points from circle A and B along the line between the centers. + point_A = circle_A_pos + comp_proj * circle_A_radius; + contact_points[num_points] = point_A; + ++num_points; + + point_A = circle_B_pos - comp_proj * circle_B_radius - norm_proj; + contact_points[num_points] = point_A; + ++num_points; + } // Otherwise one circle is inside the other one, use 3 arbitrary equidistant points. + } // Otherwise circles are concentric, use 3 arbitrary equidistant points. + + if (num_points == 0) { + // Generate equidistant points. + if (circle_A_radius < circle_B_radius) { + // Circle A inside circle B. + for (int i = 0; i < 3; ++i) { + Vector3 circle_A_point = circle_A_pos; + circle_A_point += circle_A_line_1 * Math::cos(2.0 * Math_PI * i / 3.0); + circle_A_point += circle_A_line_2 * Math::sin(2.0 * Math_PI * i / 3.0); + + contact_points[num_points] = circle_A_point; + ++num_points; + } + } else { + // Circle B inside circle A. + for (int i = 0; i < 3; ++i) { + Vector3 circle_B_point = circle_B_pos; + circle_B_point += circle_B_line_1 * Math::cos(2.0 * Math_PI * i / 3.0); + circle_B_point += circle_B_line_2 * Math::sin(2.0 * Math_PI * i / 3.0); + + Vector3 circle_A_point = circle_B_point - norm_proj; + + contact_points[num_points] = circle_A_point; + ++num_points; + } + } + } + + Plane circle_B_plane(circle_B_normal, circle_B_pos); + + // Generate contact points. + for (int i = 0; i < num_points; i++) { + const Vector3 &contact_point_A = contact_points[i]; + + real_t d = circle_B_plane.distance_to(contact_point_A); + Vector3 closest_B = contact_point_A - circle_B_plane.normal * d; + + if (p_callback->normal.dot(contact_point_A) >= p_callback->normal.dot(closest_B)) { + continue; + } + + p_callback->call(contact_point_A, closest_B); + } +} + +static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_point_count_A, GodotShape3D::FeatureType p_feature_type_A, const Vector3 *p_points_B, int p_point_count_B, GodotShape3D::FeatureType p_feature_type_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A < 1); + ERR_FAIL_COND(p_point_count_B < 1); +#endif + + static const GenerateContactsFunc generate_contacts_func_table[4][4] = { + { + _generate_contacts_point_point, + _generate_contacts_point_edge, + _generate_contacts_point_face, + _generate_contacts_point_circle, + }, + { + nullptr, + _generate_contacts_edge_edge, + _generate_contacts_face_face, + _generate_contacts_edge_circle, + }, + { + nullptr, + nullptr, + _generate_contacts_face_face, + _generate_contacts_face_circle, + }, + { + nullptr, + nullptr, + nullptr, + _generate_contacts_circle_circle, + }, + }; + + int pointcount_B; + int pointcount_A; + const Vector3 *points_A; + const Vector3 *points_B; + int version_A; + int version_B; + + if (p_feature_type_A > p_feature_type_B) { + //swap + p_callback->swap = !p_callback->swap; + p_callback->normal = -p_callback->normal; + + pointcount_B = p_point_count_A; + pointcount_A = p_point_count_B; + points_A = p_points_B; + points_B = p_points_A; + version_A = p_feature_type_B; + version_B = p_feature_type_A; + } else { + pointcount_B = p_point_count_B; + pointcount_A = p_point_count_A; + points_A = p_points_A; + points_B = p_points_B; + version_A = p_feature_type_A; + version_B = p_feature_type_B; + } + + GenerateContactsFunc contacts_func = generate_contacts_func_table[version_A][version_B]; + ERR_FAIL_COND(!contacts_func); + contacts_func(points_A, pointcount_A, points_B, pointcount_B, p_callback); +} + +template +class SeparatorAxisTest { + const ShapeA *shape_A = nullptr; + const ShapeB *shape_B = nullptr; + const Transform3D *transform_A = nullptr; + const Transform3D *transform_B = nullptr; + real_t best_depth = 1e15; + Vector3 best_axis; + _CollectorCallback *callback = nullptr; + real_t margin_A = 0.0; + real_t margin_B = 0.0; + Vector3 separator_axis; + +public: + _FORCE_INLINE_ bool test_previous_axis() { + if (callback && callback->prev_axis && *callback->prev_axis != Vector3()) { + return test_axis(*callback->prev_axis); + } else { + return true; + } + } + + _FORCE_INLINE_ bool test_axis(const Vector3 &p_axis, bool p_directional = false) { + Vector3 axis = p_axis; + + if (axis.is_equal_approx(Vector3())) { + // strange case, try an upwards separator + axis = Vector3(0.0, 1.0, 0.0); + } + + real_t min_A, max_A, min_B, max_B; + + shape_A->project_range(axis, *transform_A, min_A, max_A); + shape_B->project_range(axis, *transform_B, min_B, max_B); + + if (withMargin) { + min_A -= margin_A; + max_A += margin_A; + min_B -= margin_B; + max_B += margin_B; + } + + min_B -= (max_A - min_A) * 0.5; + max_B += (max_A - min_A) * 0.5; + + min_B -= (min_A + max_A) * 0.5; + max_B -= (min_A + max_A) * 0.5; + + if (min_B > 0.0 || max_B < 0.0) { + separator_axis = axis; + return false; // doesn't contain 0 + } + + //use the smallest depth + + if (min_B < 0.0) { // could be +0.0, we don't want it to become -0.0 + if (p_directional) { + min_B = max_B; + axis = -axis; + } else { + min_B = -min_B; + } + } + + if (max_B < min_B) { + if (max_B < best_depth) { + best_depth = max_B; + best_axis = axis; + } + } else { + if (min_B < best_depth) { + best_depth = min_B; + best_axis = -axis; // keep it as A axis + } + } + + return true; + } + + static _FORCE_INLINE_ void test_contact_points(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { + SeparatorAxisTest *separator = (SeparatorAxisTest *)p_userdata; + Vector3 axis = (p_point_B - p_point_A); + real_t depth = axis.length(); + + // Filter out bogus directions with a threshold and re-testing axis. + if (separator->best_depth - depth > 0.001) { + separator->test_axis(axis / depth); + } + } + + _FORCE_INLINE_ void generate_contacts() { + // nothing to do, don't generate + if (best_axis == Vector3(0.0, 0.0, 0.0)) { + return; + } + + if (!callback->callback) { + //just was checking intersection? + callback->collided = true; + if (callback->prev_axis) { + *callback->prev_axis = best_axis; + } + return; + } + + static const int max_supports = 16; + + Vector3 supports_A[max_supports]; + int support_count_A; + GodotShape3D::FeatureType support_type_A; + shape_A->get_supports(transform_A->basis.xform_inv(-best_axis).normalized(), max_supports, supports_A, support_count_A, support_type_A); + for (int i = 0; i < support_count_A; i++) { + supports_A[i] = transform_A->xform(supports_A[i]); + } + + if (withMargin) { + for (int i = 0; i < support_count_A; i++) { + supports_A[i] += -best_axis * margin_A; + } + } + + Vector3 supports_B[max_supports]; + int support_count_B; + GodotShape3D::FeatureType support_type_B; + shape_B->get_supports(transform_B->basis.xform_inv(best_axis).normalized(), max_supports, supports_B, support_count_B, support_type_B); + for (int i = 0; i < support_count_B; i++) { + supports_B[i] = transform_B->xform(supports_B[i]); + } + + if (withMargin) { + for (int i = 0; i < support_count_B; i++) { + supports_B[i] += best_axis * margin_B; + } + } + + callback->normal = best_axis; + if (callback->prev_axis) { + *callback->prev_axis = best_axis; + } + _generate_contacts_from_supports(supports_A, support_count_A, support_type_A, supports_B, support_count_B, support_type_B, callback); + + callback->collided = true; + } + + _FORCE_INLINE_ SeparatorAxisTest(const ShapeA *p_shape_A, const Transform3D &p_transform_A, const ShapeB *p_shape_B, const Transform3D &p_transform_B, _CollectorCallback *p_callback, real_t p_margin_A = 0, real_t p_margin_B = 0) { + shape_A = p_shape_A; + shape_B = p_shape_B; + transform_A = &p_transform_A; + transform_B = &p_transform_B; + callback = p_callback; + margin_A = p_margin_A; + margin_B = p_margin_B; + } +}; + +/****** SAT TESTS *******/ + +typedef void (*CollisionFunc)(const GodotShape3D *, const Transform3D &, const GodotShape3D *, const Transform3D &, _CollectorCallback *p_callback, real_t, real_t); + +template +static void _collision_sphere_sphere(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotSphereShape3D *sphere_A = static_cast(p_a); + const GodotSphereShape3D *sphere_B = static_cast(p_b); + + SeparatorAxisTest separator(sphere_A, p_transform_a, sphere_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + // previous axis + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_axis((p_transform_a.origin - p_transform_b.origin).normalized())) { + return; + } + + separator.generate_contacts(); +} + +template +static void _collision_sphere_box(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotSphereShape3D *sphere_A = static_cast(p_a); + const GodotBoxShape3D *box_B = static_cast(p_b); + + SeparatorAxisTest separator(sphere_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + // test faces + + for (int i = 0; i < 3; i++) { + Vector3 axis = p_transform_b.basis.get_axis(i).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // calculate closest point to sphere + + Vector3 cnormal = p_transform_b.xform_inv(p_transform_a.origin); + + Vector3 cpoint = p_transform_b.xform(Vector3( + + (cnormal.x < 0) ? -box_B->get_half_extents().x : box_B->get_half_extents().x, + (cnormal.y < 0) ? -box_B->get_half_extents().y : box_B->get_half_extents().y, + (cnormal.z < 0) ? -box_B->get_half_extents().z : box_B->get_half_extents().z)); + + // use point to test axis + Vector3 point_axis = (p_transform_a.origin - cpoint).normalized(); + + if (!separator.test_axis(point_axis)) { + return; + } + + // test edges + + for (int i = 0; i < 3; i++) { + Vector3 axis = point_axis.cross(p_transform_b.basis.get_axis(i)).cross(p_transform_b.basis.get_axis(i)).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + separator.generate_contacts(); +} + +template +static void _collision_sphere_capsule(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotSphereShape3D *sphere_A = static_cast(p_a); + const GodotCapsuleShape3D *capsule_B = static_cast(p_b); + + SeparatorAxisTest separator(sphere_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + //capsule sphere 1, sphere + + Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); + + Vector3 capsule_ball_1 = p_transform_b.origin + capsule_axis; + + if (!separator.test_axis((capsule_ball_1 - p_transform_a.origin).normalized())) { + return; + } + + //capsule sphere 2, sphere + + Vector3 capsule_ball_2 = p_transform_b.origin - capsule_axis; + + if (!separator.test_axis((capsule_ball_2 - p_transform_a.origin).normalized())) { + return; + } + + //capsule edge, sphere + + Vector3 b2a = p_transform_a.origin - p_transform_b.origin; + + Vector3 axis = b2a.cross(capsule_axis).cross(capsule_axis).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + + separator.generate_contacts(); +} + +template +static void _collision_sphere_cylinder(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotSphereShape3D *sphere_A = static_cast(p_a); + const GodotCylinderShape3D *cylinder_B = static_cast(p_b); + + SeparatorAxisTest separator(sphere_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + // Cylinder B end caps. + Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1).normalized(); + if (!separator.test_axis(cylinder_B_axis)) { + return; + } + + Vector3 cylinder_diff = p_transform_b.origin - p_transform_a.origin; + + // Cylinder B lateral surface. + if (!separator.test_axis(cylinder_B_axis.cross(cylinder_diff).cross(cylinder_B_axis).normalized())) { + return; + } + + // Closest point to cylinder caps. + const Vector3 &sphere_center = p_transform_a.origin; + Vector3 cyl_axis = p_transform_b.basis.get_axis(1); + Vector3 cap_axis = p_transform_b.basis.get_axis(0); + real_t height_scale = cyl_axis.length(); + real_t cap_dist = cylinder_B->get_height() * 0.5 * height_scale; + cyl_axis /= height_scale; + real_t radius_scale = cap_axis.length(); + real_t cap_radius = cylinder_B->get_radius() * radius_scale; + + for (int i = 0; i < 2; i++) { + Vector3 cap_dir = ((i == 0) ? cyl_axis : -cyl_axis); + Vector3 cap_pos = p_transform_b.origin + cap_dir * cap_dist; + + Vector3 closest_point; + + Vector3 diff = sphere_center - cap_pos; + Vector3 proj = diff - cap_dir.dot(diff) * cap_dir; + + real_t proj_len = proj.length(); + if (Math::is_zero_approx(proj_len)) { + // Point is equidistant to all circle points. + continue; + } + + closest_point = cap_pos + (cap_radius / proj_len) * proj; + + if (!separator.test_axis((closest_point - sphere_center).normalized())) { + return; + } + } + + separator.generate_contacts(); +} + +template +static void _collision_sphere_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotSphereShape3D *sphere_A = static_cast(p_a); + const GodotConvexPolygonShape3D *convex_polygon_B = static_cast(p_b); + + SeparatorAxisTest separator(sphere_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + const Geometry3D::MeshData &mesh = convex_polygon_B->get_mesh(); + + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); + int face_count = mesh.faces.size(); + const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); + int edge_count = mesh.edges.size(); + const Vector3 *vertices = mesh.vertices.ptr(); + int vertex_count = mesh.vertices.size(); + + // Precalculating this makes the transforms faster. + Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); + + // faces of B + for (int i = 0; i < face_count; i++) { + Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // edges of B + for (int i = 0; i < edge_count; i++) { + Vector3 v1 = p_transform_b.xform(vertices[edges[i].a]); + Vector3 v2 = p_transform_b.xform(vertices[edges[i].b]); + Vector3 v3 = p_transform_a.origin; + + Vector3 n1 = v2 - v1; + Vector3 n2 = v2 - v3; + + Vector3 axis = n1.cross(n2).cross(n1).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // vertices of B + for (int i = 0; i < vertex_count; i++) { + Vector3 v1 = p_transform_b.xform(vertices[i]); + Vector3 v2 = p_transform_a.origin; + + Vector3 axis = (v2 - v1).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + separator.generate_contacts(); +} + +template +static void _collision_sphere_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotSphereShape3D *sphere_A = static_cast(p_a); + const GodotFaceShape3D *face_B = static_cast(p_b); + + SeparatorAxisTest separator(sphere_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + Vector3 vertex[3] = { + p_transform_b.xform(face_B->vertex[0]), + p_transform_b.xform(face_B->vertex[1]), + p_transform_b.xform(face_B->vertex[2]), + }; + + Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); + + if (!separator.test_axis(normal, !face_B->backface_collision)) { + return; + } + + // edges and points of B + for (int i = 0; i < 3; i++) { + Vector3 n1 = vertex[i] - p_transform_a.origin; + if (n1.dot(normal) < 0.0) { + n1 *= -1.0; + } + + if (!separator.test_axis(n1.normalized())) { + return; + } + + Vector3 n2 = vertex[(i + 1) % 3] - vertex[i]; + + Vector3 axis = n1.cross(n2).cross(n2).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + } + + separator.generate_contacts(); +} + +template +static void _collision_box_box(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotBoxShape3D *box_A = static_cast(p_a); + const GodotBoxShape3D *box_B = static_cast(p_b); + + SeparatorAxisTest separator(box_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + // test faces of A + + for (int i = 0; i < 3; i++) { + Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // test faces of B + + for (int i = 0; i < 3; i++) { + Vector3 axis = p_transform_b.basis.get_axis(i).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // test combined edges + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + Vector3 axis = p_transform_a.basis.get_axis(i).cross(p_transform_b.basis.get_axis(j)); + + if (Math::is_zero_approx(axis.length_squared())) { + continue; + } + axis.normalize(); + + if (!separator.test_axis(axis)) { + return; + } + } + } + + if (withMargin) { + //add endpoint test between closest vertices and edges + + // calculate closest point to sphere + + Vector3 ab_vec = p_transform_b.origin - p_transform_a.origin; + + Vector3 cnormal_a = p_transform_a.basis.xform_inv(ab_vec); + + Vector3 support_a = p_transform_a.xform(Vector3( + + (cnormal_a.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, + (cnormal_a.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, + (cnormal_a.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z)); + + Vector3 cnormal_b = p_transform_b.basis.xform_inv(-ab_vec); + + Vector3 support_b = p_transform_b.xform(Vector3( + + (cnormal_b.x < 0) ? -box_B->get_half_extents().x : box_B->get_half_extents().x, + (cnormal_b.y < 0) ? -box_B->get_half_extents().y : box_B->get_half_extents().y, + (cnormal_b.z < 0) ? -box_B->get_half_extents().z : box_B->get_half_extents().z)); + + Vector3 axis_ab = (support_a - support_b); + + if (!separator.test_axis(axis_ab.normalized())) { + return; + } + + //now try edges, which become cylinders! + + for (int i = 0; i < 3; i++) { + //a ->b + Vector3 axis_a = p_transform_a.basis.get_axis(i); + + if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) { + return; + } + + //b ->a + Vector3 axis_b = p_transform_b.basis.get_axis(i); + + if (!separator.test_axis(axis_ab.cross(axis_b).cross(axis_b).normalized())) { + return; + } + } + } + + separator.generate_contacts(); +} + +template +static void _collision_box_capsule(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotBoxShape3D *box_A = static_cast(p_a); + const GodotCapsuleShape3D *capsule_B = static_cast(p_b); + + SeparatorAxisTest separator(box_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + // faces of A + for (int i = 0; i < 3; i++) { + Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + Vector3 cyl_axis = p_transform_b.basis.get_axis(1).normalized(); + + // edges of A, capsule cylinder + + for (int i = 0; i < 3; i++) { + // cylinder + Vector3 box_axis = p_transform_a.basis.get_axis(i); + Vector3 axis = box_axis.cross(cyl_axis); + if (Math::is_zero_approx(axis.length_squared())) { + continue; + } + + if (!separator.test_axis(axis.normalized())) { + return; + } + } + + // points of A, capsule cylinder + // this sure could be made faster somehow.. + + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 2; j++) { + for (int k = 0; k < 2; k++) { + Vector3 he = box_A->get_half_extents(); + he.x *= (i * 2 - 1); + he.y *= (j * 2 - 1); + he.z *= (k * 2 - 1); + Vector3 point = p_transform_a.origin; + for (int l = 0; l < 3; l++) { + point += p_transform_a.basis.get_axis(l) * he[l]; + } + + //Vector3 axis = (point - cyl_axis * cyl_axis.dot(point)).normalized(); + Vector3 axis = Plane(cyl_axis).project(point).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + } + } + + // capsule balls, edges of A + + for (int i = 0; i < 2; i++) { + Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); + + Vector3 sphere_pos = p_transform_b.origin + ((i == 0) ? capsule_axis : -capsule_axis); + + Vector3 cnormal = p_transform_a.xform_inv(sphere_pos); + + Vector3 cpoint = p_transform_a.xform(Vector3( + + (cnormal.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, + (cnormal.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, + (cnormal.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z)); + + // use point to test axis + Vector3 point_axis = (sphere_pos - cpoint).normalized(); + + if (!separator.test_axis(point_axis)) { + return; + } + + // test edges of A + + for (int j = 0; j < 3; j++) { + Vector3 axis = point_axis.cross(p_transform_a.basis.get_axis(j)).cross(p_transform_a.basis.get_axis(j)).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + } + + separator.generate_contacts(); +} + +template +static void _collision_box_cylinder(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotBoxShape3D *box_A = static_cast(p_a); + const GodotCylinderShape3D *cylinder_B = static_cast(p_b); + + SeparatorAxisTest separator(box_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + // Faces of A. + for (int i = 0; i < 3; i++) { + Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + Vector3 cyl_axis = p_transform_b.basis.get_axis(1).normalized(); + + // Cylinder end caps. + { + if (!separator.test_axis(cyl_axis)) { + return; + } + } + + // Edges of A, cylinder lateral surface. + for (int i = 0; i < 3; i++) { + Vector3 box_axis = p_transform_a.basis.get_axis(i); + Vector3 axis = box_axis.cross(cyl_axis); + if (Math::is_zero_approx(axis.length_squared())) { + continue; + } + + if (!separator.test_axis(axis.normalized())) { + return; + } + } + + // Gather points of A. + Vector3 vertices_A[8]; + Vector3 box_extent = box_A->get_half_extents(); + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 2; j++) { + for (int k = 0; k < 2; k++) { + Vector3 extent = box_extent; + extent.x *= (i * 2 - 1); + extent.y *= (j * 2 - 1); + extent.z *= (k * 2 - 1); + Vector3 &point = vertices_A[i * 2 * 2 + j * 2 + k]; + point = p_transform_a.origin; + for (int l = 0; l < 3; l++) { + point += p_transform_a.basis.get_axis(l) * extent[l]; + } + } + } + } + + // Points of A, cylinder lateral surface. + for (int i = 0; i < 8; i++) { + const Vector3 &point = vertices_A[i]; + Vector3 axis = Plane(cyl_axis).project(point).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // Edges of A, cylinder end caps rim. + int edges_start_A[12] = { 0, 2, 4, 6, 0, 1, 4, 5, 0, 1, 2, 3 }; + int edges_end_A[12] = { 1, 3, 5, 7, 2, 3, 6, 7, 4, 5, 6, 7 }; + + Vector3 cap_axis = cyl_axis * (cylinder_B->get_height() * 0.5); + + for (int i = 0; i < 2; i++) { + Vector3 cap_pos = p_transform_b.origin + ((i == 0) ? cap_axis : -cap_axis); + + for (int e = 0; e < 12; e++) { + const Vector3 &edge_start = vertices_A[edges_start_A[e]]; + const Vector3 &edge_end = vertices_A[edges_end_A[e]]; + + Vector3 edge_dir = (edge_end - edge_start); + edge_dir.normalize(); + + real_t edge_dot = edge_dir.dot(cyl_axis); + if (Math::is_zero_approx(edge_dot)) { + // Edge is perpendicular to cylinder axis. + continue; + } + + // Calculate intersection between edge and circle plane. + Vector3 edge_diff = cap_pos - edge_start; + real_t diff_dot = edge_diff.dot(cyl_axis); + Vector3 intersection = edge_start + edge_dir * diff_dot / edge_dot; + + // Calculate tangent that touches intersection. + Vector3 tangent = (cap_pos - intersection).cross(cyl_axis); + + // Axis is orthogonal both to tangent and edge direction. + Vector3 axis = tangent.cross(edge_dir); + + if (!separator.test_axis(axis.normalized())) { + return; + } + } + } + + separator.generate_contacts(); +} + +template +static void _collision_box_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotBoxShape3D *box_A = static_cast(p_a); + const GodotConvexPolygonShape3D *convex_polygon_B = static_cast(p_b); + + SeparatorAxisTest separator(box_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + const Geometry3D::MeshData &mesh = convex_polygon_B->get_mesh(); + + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); + int face_count = mesh.faces.size(); + const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); + int edge_count = mesh.edges.size(); + const Vector3 *vertices = mesh.vertices.ptr(); + int vertex_count = mesh.vertices.size(); + + // faces of A + for (int i = 0; i < 3; i++) { + Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // Precalculating this makes the transforms faster. + Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); + + // faces of B + for (int i = 0; i < face_count; i++) { + Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // A<->B edges + for (int i = 0; i < 3; i++) { + Vector3 e1 = p_transform_a.basis.get_axis(i); + + for (int j = 0; j < edge_count; j++) { + Vector3 e2 = p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]); + + Vector3 axis = e1.cross(e2).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + } + + if (withMargin) { + // calculate closest points between vertices and box edges + for (int v = 0; v < vertex_count; v++) { + Vector3 vtxb = p_transform_b.xform(vertices[v]); + Vector3 ab_vec = vtxb - p_transform_a.origin; + + Vector3 cnormal_a = p_transform_a.basis.xform_inv(ab_vec); + + Vector3 support_a = p_transform_a.xform(Vector3( + + (cnormal_a.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, + (cnormal_a.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, + (cnormal_a.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z)); + + Vector3 axis_ab = support_a - vtxb; + + if (!separator.test_axis(axis_ab.normalized())) { + return; + } + + //now try edges, which become cylinders! + + for (int i = 0; i < 3; i++) { + //a ->b + Vector3 axis_a = p_transform_a.basis.get_axis(i); + + if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) { + return; + } + } + } + + //convex edges and box points + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 2; j++) { + for (int k = 0; k < 2; k++) { + Vector3 he = box_A->get_half_extents(); + he.x *= (i * 2 - 1); + he.y *= (j * 2 - 1); + he.z *= (k * 2 - 1); + Vector3 point = p_transform_a.origin; + for (int l = 0; l < 3; l++) { + point += p_transform_a.basis.get_axis(l) * he[l]; + } + + for (int e = 0; e < edge_count; e++) { + Vector3 p1 = p_transform_b.xform(vertices[edges[e].a]); + Vector3 p2 = p_transform_b.xform(vertices[edges[e].b]); + Vector3 n = (p2 - p1); + + if (!separator.test_axis((point - p2).cross(n).cross(n).normalized())) { + return; + } + } + } + } + } + } + + separator.generate_contacts(); +} + +template +static void _collision_box_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotBoxShape3D *box_A = static_cast(p_a); + const GodotFaceShape3D *face_B = static_cast(p_b); + + SeparatorAxisTest separator(box_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + Vector3 vertex[3] = { + p_transform_b.xform(face_B->vertex[0]), + p_transform_b.xform(face_B->vertex[1]), + p_transform_b.xform(face_B->vertex[2]), + }; + + Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); + + if (!separator.test_axis(normal, !face_B->backface_collision)) { + return; + } + + // faces of A + for (int i = 0; i < 3; i++) { + Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + } + + // combined edges + + for (int i = 0; i < 3; i++) { + Vector3 e = vertex[i] - vertex[(i + 1) % 3]; + + for (int j = 0; j < 3; j++) { + Vector3 axis = e.cross(p_transform_a.basis.get_axis(j)).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + } + } + + if (withMargin) { + // calculate closest points between vertices and box edges + for (int v = 0; v < 3; v++) { + Vector3 ab_vec = vertex[v] - p_transform_a.origin; + + Vector3 cnormal_a = p_transform_a.basis.xform_inv(ab_vec); + + Vector3 support_a = p_transform_a.xform(Vector3( + + (cnormal_a.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, + (cnormal_a.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, + (cnormal_a.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z)); + + Vector3 axis_ab = support_a - vertex[v]; + if (axis_ab.dot(normal) < 0.0) { + axis_ab *= -1.0; + } + + if (!separator.test_axis(axis_ab.normalized())) { + return; + } + + //now try edges, which become cylinders! + + for (int i = 0; i < 3; i++) { + //a ->b + Vector3 axis_a = p_transform_a.basis.get_axis(i); + + Vector3 axis = axis_ab.cross(axis_a).cross(axis_a).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + } + } + + //convex edges and box points, there has to be a way to speed up this (get closest point?) + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 2; j++) { + for (int k = 0; k < 2; k++) { + Vector3 he = box_A->get_half_extents(); + he.x *= (i * 2 - 1); + he.y *= (j * 2 - 1); + he.z *= (k * 2 - 1); + Vector3 point = p_transform_a.origin; + for (int l = 0; l < 3; l++) { + point += p_transform_a.basis.get_axis(l) * he[l]; + } + + for (int e = 0; e < 3; e++) { + Vector3 p1 = vertex[e]; + Vector3 p2 = vertex[(e + 1) % 3]; + + Vector3 n = (p2 - p1); + + Vector3 axis = (point - p2).cross(n).cross(n).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + } + } + } + } + } + + separator.generate_contacts(); +} + +template +static void _collision_capsule_capsule(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotCapsuleShape3D *capsule_A = static_cast(p_a); + const GodotCapsuleShape3D *capsule_B = static_cast(p_b); + + SeparatorAxisTest separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + // some values + + Vector3 capsule_A_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); + Vector3 capsule_B_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); + + Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis; + Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis; + Vector3 capsule_B_ball_1 = p_transform_b.origin + capsule_B_axis; + Vector3 capsule_B_ball_2 = p_transform_b.origin - capsule_B_axis; + + //balls-balls + + if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_1).normalized())) { + return; + } + if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_2).normalized())) { + return; + } + + if (!separator.test_axis((capsule_A_ball_2 - capsule_B_ball_1).normalized())) { + return; + } + if (!separator.test_axis((capsule_A_ball_2 - capsule_B_ball_2).normalized())) { + return; + } + + // edges-balls + + if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_1).cross(capsule_A_axis).cross(capsule_A_axis).normalized())) { + return; + } + + if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_2).cross(capsule_A_axis).cross(capsule_A_axis).normalized())) { + return; + } + + if (!separator.test_axis((capsule_B_ball_1 - capsule_A_ball_1).cross(capsule_B_axis).cross(capsule_B_axis).normalized())) { + return; + } + + if (!separator.test_axis((capsule_B_ball_1 - capsule_A_ball_2).cross(capsule_B_axis).cross(capsule_B_axis).normalized())) { + return; + } + + // edges + + if (!separator.test_axis(capsule_A_axis.cross(capsule_B_axis).normalized())) { + return; + } + + separator.generate_contacts(); +} + +template +static void _collision_capsule_cylinder(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotCapsuleShape3D *capsule_A = static_cast(p_a); + const GodotCylinderShape3D *cylinder_B = static_cast(p_b); + + SeparatorAxisTest separator(capsule_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + // Cylinder B end caps. + Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1).normalized(); + if (!separator.test_axis(cylinder_B_axis)) { + return; + } + + // Cylinder edge against capsule balls. + + Vector3 capsule_A_axis = p_transform_a.basis.get_axis(1); + + Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); + Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); + + if (!separator.test_axis((p_transform_b.origin - capsule_A_ball_1).cross(cylinder_B_axis).cross(cylinder_B_axis).normalized())) { + return; + } + + if (!separator.test_axis((p_transform_b.origin - capsule_A_ball_2).cross(cylinder_B_axis).cross(cylinder_B_axis).normalized())) { + return; + } + + // Cylinder edge against capsule edge. + + Vector3 center_diff = p_transform_b.origin - p_transform_a.origin; + + if (!separator.test_axis(capsule_A_axis.cross(center_diff).cross(capsule_A_axis).normalized())) { + return; + } + + if (!separator.test_axis(cylinder_B_axis.cross(center_diff).cross(cylinder_B_axis).normalized())) { + return; + } + + real_t proj = capsule_A_axis.cross(cylinder_B_axis).cross(cylinder_B_axis).dot(capsule_A_axis); + if (Math::is_zero_approx(proj)) { + // Parallel capsule and cylinder axes, handle with specific axes only. + // Note: GJKEPA with no margin can lead to degenerate cases in this situation. + separator.generate_contacts(); + return; + } + + GodotCollisionSolver3D::CallbackResult callback = SeparatorAxisTest::test_contact_points; + + // Fallback to generic algorithm to find the best separating axis. + if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) { + return; + } + + separator.generate_contacts(); +} + +template +static void _collision_capsule_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotCapsuleShape3D *capsule_A = static_cast(p_a); + const GodotConvexPolygonShape3D *convex_polygon_B = static_cast(p_b); + + SeparatorAxisTest separator(capsule_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + const Geometry3D::MeshData &mesh = convex_polygon_B->get_mesh(); + + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); + int face_count = mesh.faces.size(); + const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); + int edge_count = mesh.edges.size(); + const Vector3 *vertices = mesh.vertices.ptr(); + + // Precalculating this makes the transforms faster. + Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); + + // faces of B + for (int i = 0; i < face_count; i++) { + Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // edges of B, capsule cylinder + + for (int i = 0; i < edge_count; i++) { + // cylinder + Vector3 edge_axis = p_transform_b.basis.xform(vertices[edges[i].a]) - p_transform_b.basis.xform(vertices[edges[i].b]); + Vector3 axis = edge_axis.cross(p_transform_a.basis.get_axis(1)).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // capsule balls, edges of B + + for (int i = 0; i < 2; i++) { + // edges of B, capsule cylinder + + Vector3 capsule_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); + + Vector3 sphere_pos = p_transform_a.origin + ((i == 0) ? capsule_axis : -capsule_axis); + + for (int j = 0; j < edge_count; j++) { + Vector3 n1 = sphere_pos - p_transform_b.xform(vertices[edges[j].a]); + Vector3 n2 = p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]); + + Vector3 axis = n1.cross(n2).cross(n2).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + } + + separator.generate_contacts(); +} + +template +static void _collision_capsule_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotCapsuleShape3D *capsule_A = static_cast(p_a); + const GodotFaceShape3D *face_B = static_cast(p_b); + + SeparatorAxisTest separator(capsule_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + Vector3 vertex[3] = { + p_transform_b.xform(face_B->vertex[0]), + p_transform_b.xform(face_B->vertex[1]), + p_transform_b.xform(face_B->vertex[2]), + }; + + Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); + + if (!separator.test_axis(normal, !face_B->backface_collision)) { + return; + } + + // edges of B, capsule cylinder + + Vector3 capsule_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); + + for (int i = 0; i < 3; i++) { + // edge-cylinder + Vector3 edge_axis = vertex[i] - vertex[(i + 1) % 3]; + + Vector3 axis = edge_axis.cross(capsule_axis).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + + Vector3 dir_axis = (p_transform_a.origin - vertex[i]).cross(capsule_axis).cross(capsule_axis).normalized(); + if (dir_axis.dot(normal) < 0.0) { + dir_axis *= -1.0; + } + + if (!separator.test_axis(dir_axis)) { + return; + } + + for (int j = 0; j < 2; j++) { + // point-spheres + Vector3 sphere_pos = p_transform_a.origin + ((j == 0) ? capsule_axis : -capsule_axis); + + Vector3 n1 = sphere_pos - vertex[i]; + if (n1.dot(normal) < 0.0) { + n1 *= -1.0; + } + + if (!separator.test_axis(n1.normalized())) { + return; + } + + Vector3 n2 = edge_axis; + + axis = n1.cross(n2).cross(n2); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis.normalized())) { + return; + } + } + } + + separator.generate_contacts(); +} + +template +static void _collision_cylinder_cylinder(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotCylinderShape3D *cylinder_A = static_cast(p_a); + const GodotCylinderShape3D *cylinder_B = static_cast(p_b); + + SeparatorAxisTest separator(cylinder_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + Vector3 cylinder_A_axis = p_transform_a.basis.get_axis(1); + Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1); + + if (!separator.test_previous_axis()) { + return; + } + + // Cylinder A end caps. + if (!separator.test_axis(cylinder_A_axis.normalized())) { + return; + } + + // Cylinder B end caps. + if (!separator.test_axis(cylinder_A_axis.normalized())) { + return; + } + + Vector3 cylinder_diff = p_transform_b.origin - p_transform_a.origin; + + // Cylinder A lateral surface. + if (!separator.test_axis(cylinder_A_axis.cross(cylinder_diff).cross(cylinder_A_axis).normalized())) { + return; + } + + // Cylinder B lateral surface. + if (!separator.test_axis(cylinder_B_axis.cross(cylinder_diff).cross(cylinder_B_axis).normalized())) { + return; + } + + real_t proj = cylinder_A_axis.cross(cylinder_B_axis).cross(cylinder_B_axis).dot(cylinder_A_axis); + if (Math::is_zero_approx(proj)) { + // Parallel cylinders, handle with specific axes only. + // Note: GJKEPA with no margin can lead to degenerate cases in this situation. + separator.generate_contacts(); + return; + } + + GodotCollisionSolver3D::CallbackResult callback = SeparatorAxisTest::test_contact_points; + + // Fallback to generic algorithm to find the best separating axis. + if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) { + return; + } + + separator.generate_contacts(); +} + +template +static void _collision_cylinder_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotCylinderShape3D *cylinder_A = static_cast(p_a); + const GodotConvexPolygonShape3D *convex_polygon_B = static_cast(p_b); + + SeparatorAxisTest separator(cylinder_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + GodotCollisionSolver3D::CallbackResult callback = SeparatorAxisTest::test_contact_points; + + // Fallback to generic algorithm to find the best separating axis. + if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) { + return; + } + + separator.generate_contacts(); +} + +template +static void _collision_cylinder_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotCylinderShape3D *cylinder_A = static_cast(p_a); + const GodotFaceShape3D *face_B = static_cast(p_b); + + SeparatorAxisTest separator(cylinder_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + Vector3 vertex[3] = { + p_transform_b.xform(face_B->vertex[0]), + p_transform_b.xform(face_B->vertex[1]), + p_transform_b.xform(face_B->vertex[2]), + }; + + Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); + + // Face B normal. + if (!separator.test_axis(normal, !face_B->backface_collision)) { + return; + } + + Vector3 cyl_axis = p_transform_a.basis.get_axis(1).normalized(); + if (cyl_axis.dot(normal) < 0.0) { + cyl_axis *= -1.0; + } + + // Cylinder end caps. + if (!separator.test_axis(cyl_axis)) { + return; + } + + // Edges of B, cylinder lateral surface. + for (int i = 0; i < 3; i++) { + Vector3 edge_axis = vertex[i] - vertex[(i + 1) % 3]; + Vector3 axis = edge_axis.cross(cyl_axis); + if (Math::is_zero_approx(axis.length_squared())) { + continue; + } + + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis.normalized())) { + return; + } + } + + // Points of B, cylinder lateral surface. + for (int i = 0; i < 3; i++) { + const Vector3 &point = vertex[i]; + Vector3 axis = Plane(cyl_axis).project(point).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + } + + // Edges of B, cylinder end caps rim. + Vector3 cap_axis = cyl_axis * (cylinder_A->get_height() * 0.5); + + for (int i = 0; i < 2; i++) { + Vector3 cap_pos = p_transform_a.origin + ((i == 0) ? cap_axis : -cap_axis); + + for (int j = 0; j < 3; j++) { + const Vector3 &edge_start = vertex[j]; + const Vector3 &edge_end = vertex[(j + 1) % 3]; + Vector3 edge_dir = edge_end - edge_start; + edge_dir.normalize(); + + real_t edge_dot = edge_dir.dot(cyl_axis); + if (Math::is_zero_approx(edge_dot)) { + // Edge is perpendicular to cylinder axis. + continue; + } + + // Calculate intersection between edge and circle plane. + Vector3 edge_diff = cap_pos - edge_start; + real_t diff_dot = edge_diff.dot(cyl_axis); + Vector3 intersection = edge_start + edge_dir * diff_dot / edge_dot; + + // Calculate tangent that touches intersection. + Vector3 tangent = (cap_pos - intersection).cross(cyl_axis); + + // Axis is orthogonal both to tangent and edge direction. + Vector3 axis = tangent.cross(edge_dir); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis.normalized())) { + return; + } + } + } + + separator.generate_contacts(); +} + +template +static void _collision_convex_polygon_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotConvexPolygonShape3D *convex_polygon_A = static_cast(p_a); + const GodotConvexPolygonShape3D *convex_polygon_B = static_cast(p_b); + + SeparatorAxisTest separator(convex_polygon_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + const Geometry3D::MeshData &mesh_A = convex_polygon_A->get_mesh(); + + const Geometry3D::MeshData::Face *faces_A = mesh_A.faces.ptr(); + int face_count_A = mesh_A.faces.size(); + const Geometry3D::MeshData::Edge *edges_A = mesh_A.edges.ptr(); + int edge_count_A = mesh_A.edges.size(); + const Vector3 *vertices_A = mesh_A.vertices.ptr(); + int vertex_count_A = mesh_A.vertices.size(); + + const Geometry3D::MeshData &mesh_B = convex_polygon_B->get_mesh(); + + const Geometry3D::MeshData::Face *faces_B = mesh_B.faces.ptr(); + int face_count_B = mesh_B.faces.size(); + const Geometry3D::MeshData::Edge *edges_B = mesh_B.edges.ptr(); + int edge_count_B = mesh_B.edges.size(); + const Vector3 *vertices_B = mesh_B.vertices.ptr(); + int vertex_count_B = mesh_B.vertices.size(); + + // Precalculating this makes the transforms faster. + Basis a_xform_normal = p_transform_b.basis.inverse().transposed(); + + // faces of A + for (int i = 0; i < face_count_A; i++) { + Vector3 axis = a_xform_normal.xform(faces_A[i].plane.normal).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // Precalculating this makes the transforms faster. + Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); + + // faces of B + for (int i = 0; i < face_count_B; i++) { + Vector3 axis = b_xform_normal.xform(faces_B[i].plane.normal).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // A<->B edges + for (int i = 0; i < edge_count_A; i++) { + Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].a]) - p_transform_a.basis.xform(vertices_A[edges_A[i].b]); + + for (int j = 0; j < edge_count_B; j++) { + Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[j].a]) - p_transform_b.basis.xform(vertices_B[edges_B[j].b]); + + Vector3 axis = e1.cross(e2).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + } + + if (withMargin) { + //vertex-vertex + for (int i = 0; i < vertex_count_A; i++) { + Vector3 va = p_transform_a.xform(vertices_A[i]); + + for (int j = 0; j < vertex_count_B; j++) { + if (!separator.test_axis((va - p_transform_b.xform(vertices_B[j])).normalized())) { + return; + } + } + } + //edge-vertex (shell) + + for (int i = 0; i < edge_count_A; i++) { + Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].a]); + Vector3 e2 = p_transform_a.basis.xform(vertices_A[edges_A[i].b]); + Vector3 n = (e2 - e1); + + for (int j = 0; j < vertex_count_B; j++) { + Vector3 e3 = p_transform_b.xform(vertices_B[j]); + + if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) { + return; + } + } + } + + for (int i = 0; i < edge_count_B; i++) { + Vector3 e1 = p_transform_b.basis.xform(vertices_B[edges_B[i].a]); + Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[i].b]); + Vector3 n = (e2 - e1); + + for (int j = 0; j < vertex_count_A; j++) { + Vector3 e3 = p_transform_a.xform(vertices_A[j]); + + if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) { + return; + } + } + } + } + + separator.generate_contacts(); +} + +template +static void _collision_convex_polygon_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotConvexPolygonShape3D *convex_polygon_A = static_cast(p_a); + const GodotFaceShape3D *face_B = static_cast(p_b); + + SeparatorAxisTest separator(convex_polygon_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + const Geometry3D::MeshData &mesh = convex_polygon_A->get_mesh(); + + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); + int face_count = mesh.faces.size(); + const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); + int edge_count = mesh.edges.size(); + const Vector3 *vertices = mesh.vertices.ptr(); + int vertex_count = mesh.vertices.size(); + + Vector3 vertex[3] = { + p_transform_b.xform(face_B->vertex[0]), + p_transform_b.xform(face_B->vertex[1]), + p_transform_b.xform(face_B->vertex[2]), + }; + + Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); + + if (!separator.test_axis(normal, !face_B->backface_collision)) { + return; + } + + // faces of A + for (int i = 0; i < face_count; i++) { + //Vector3 axis = p_transform_a.xform( faces[i].plane ).normal; + Vector3 axis = p_transform_a.basis.xform(faces[i].plane.normal).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + } + + // A<->B edges + for (int i = 0; i < edge_count; i++) { + Vector3 e1 = p_transform_a.xform(vertices[edges[i].a]) - p_transform_a.xform(vertices[edges[i].b]); + + for (int j = 0; j < 3; j++) { + Vector3 e2 = vertex[j] - vertex[(j + 1) % 3]; + + Vector3 axis = e1.cross(e2).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + } + } + + if (withMargin) { + //vertex-vertex + for (int i = 0; i < vertex_count; i++) { + Vector3 va = p_transform_a.xform(vertices[i]); + + for (int j = 0; j < 3; j++) { + Vector3 axis = (va - vertex[j]).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + } + } + //edge-vertex (shell) + + for (int i = 0; i < edge_count; i++) { + Vector3 e1 = p_transform_a.basis.xform(vertices[edges[i].a]); + Vector3 e2 = p_transform_a.basis.xform(vertices[edges[i].b]); + Vector3 n = (e2 - e1); + + for (int j = 0; j < 3; j++) { + Vector3 e3 = vertex[j]; + + Vector3 axis = (e1 - e3).cross(n).cross(n).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + } + } + + for (int i = 0; i < 3; i++) { + Vector3 e1 = vertex[i]; + Vector3 e2 = vertex[(i + 1) % 3]; + Vector3 n = (e2 - e1); + + for (int j = 0; j < vertex_count; j++) { + Vector3 e3 = p_transform_a.xform(vertices[j]); + + Vector3 axis = (e1 - e3).cross(n).cross(n).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + } + } + } + + separator.generate_contacts(); +} + +bool sat_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector3 *r_prev_axis, real_t p_margin_a, real_t p_margin_b) { + PhysicsServer3D::ShapeType type_A = p_shape_A->get_type(); + + ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_WORLD_BOUNDARY, false); + ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_SEPARATION_RAY, false); + ERR_FAIL_COND_V(p_shape_A->is_concave(), false); + + PhysicsServer3D::ShapeType type_B = p_shape_B->get_type(); + + ERR_FAIL_COND_V(type_B == PhysicsServer3D::SHAPE_WORLD_BOUNDARY, false); + ERR_FAIL_COND_V(type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY, false); + ERR_FAIL_COND_V(p_shape_B->is_concave(), false); + + static const CollisionFunc collision_table[6][6] = { + { _collision_sphere_sphere, + _collision_sphere_box, + _collision_sphere_capsule, + _collision_sphere_cylinder, + _collision_sphere_convex_polygon, + _collision_sphere_face }, + { nullptr, + _collision_box_box, + _collision_box_capsule, + _collision_box_cylinder, + _collision_box_convex_polygon, + _collision_box_face }, + { nullptr, + nullptr, + _collision_capsule_capsule, + _collision_capsule_cylinder, + _collision_capsule_convex_polygon, + _collision_capsule_face }, + { nullptr, + nullptr, + nullptr, + _collision_cylinder_cylinder, + _collision_cylinder_convex_polygon, + _collision_cylinder_face }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon, + _collision_convex_polygon_face }, + { nullptr, + nullptr, + nullptr, + nullptr, + nullptr, + nullptr }, + }; + + static const CollisionFunc collision_table_margin[6][6] = { + { _collision_sphere_sphere, + _collision_sphere_box, + _collision_sphere_capsule, + _collision_sphere_cylinder, + _collision_sphere_convex_polygon, + _collision_sphere_face }, + { nullptr, + _collision_box_box, + _collision_box_capsule, + _collision_box_cylinder, + _collision_box_convex_polygon, + _collision_box_face }, + { nullptr, + nullptr, + _collision_capsule_capsule, + _collision_capsule_cylinder, + _collision_capsule_convex_polygon, + _collision_capsule_face }, + { nullptr, + nullptr, + nullptr, + _collision_cylinder_cylinder, + _collision_cylinder_convex_polygon, + _collision_cylinder_face }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon, + _collision_convex_polygon_face }, + { nullptr, + nullptr, + nullptr, + nullptr, + nullptr, + nullptr }, + }; + + _CollectorCallback callback; + callback.callback = p_result_callback; + callback.swap = p_swap; + callback.userdata = p_userdata; + callback.collided = false; + callback.prev_axis = r_prev_axis; + + const GodotShape3D *A = p_shape_A; + const GodotShape3D *B = p_shape_B; + const Transform3D *transform_A = &p_transform_A; + const Transform3D *transform_B = &p_transform_B; + real_t margin_A = p_margin_a; + real_t margin_B = p_margin_b; + + if (type_A > type_B) { + SWAP(A, B); + SWAP(transform_A, transform_B); + SWAP(type_A, type_B); + SWAP(margin_A, margin_B); + callback.swap = !callback.swap; + } + + CollisionFunc collision_func; + if (margin_A != 0.0 || margin_B != 0.0) { + collision_func = collision_table_margin[type_A - 2][type_B - 2]; + + } else { + collision_func = collision_table[type_A - 2][type_B - 2]; + } + ERR_FAIL_COND_V(!collision_func, false); + + collision_func(A, *transform_A, B, *transform_B, &callback, margin_A, margin_B); + + return callback.collided; +} diff --git a/servers/physics_3d/godot_collision_solver_3d_sat.h b/servers/physics_3d/godot_collision_solver_3d_sat.h new file mode 100644 index 0000000000..069a701cba --- /dev/null +++ b/servers/physics_3d/godot_collision_solver_3d_sat.h @@ -0,0 +1,38 @@ +/*************************************************************************/ +/* godot_collision_solver_3d_sat.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_COLLISION_SOLVER_SAT_H +#define GODOT_COLLISION_SOLVER_SAT_H + +#include "godot_collision_solver_3d.h" + +bool sat_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector3 *r_prev_axis = nullptr, real_t p_margin_a = 0, real_t p_margin_b = 0); + +#endif // GODOT_COLLISION_SOLVER_SAT_H diff --git a/servers/physics_3d/godot_constraint_3d.h b/servers/physics_3d/godot_constraint_3d.h new file mode 100644 index 0000000000..840c81716c --- /dev/null +++ b/servers/physics_3d/godot_constraint_3d.h @@ -0,0 +1,81 @@ +/*************************************************************************/ +/* godot_constraint_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_CONSTRAINT_3D_H +#define GODOT_CONSTRAINT_3D_H + +class GodotBody3D; +class GodotSoftBody3D; + +class GodotConstraint3D { + GodotBody3D **_body_ptr; + int _body_count; + uint64_t island_step; + int priority; + bool disabled_collisions_between_bodies; + + RID self; + +protected: + GodotConstraint3D(GodotBody3D **p_body_ptr = nullptr, int p_body_count = 0) { + _body_ptr = p_body_ptr; + _body_count = p_body_count; + island_step = 0; + priority = 1; + disabled_collisions_between_bodies = true; + } + +public: + _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } + _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } + + _FORCE_INLINE_ GodotBody3D **get_body_ptr() const { return _body_ptr; } + _FORCE_INLINE_ int get_body_count() const { return _body_count; } + + virtual GodotSoftBody3D *get_soft_body_ptr(int p_index) const { return nullptr; } + virtual int get_soft_body_count() const { return 0; } + + _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } + _FORCE_INLINE_ int get_priority() const { return priority; } + + _FORCE_INLINE_ void disable_collisions_between_bodies(const bool p_disabled) { disabled_collisions_between_bodies = p_disabled; } + _FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; } + + virtual bool setup(real_t p_step) = 0; + virtual bool pre_solve(real_t p_step) = 0; + virtual void solve(real_t p_step) = 0; + + virtual ~GodotConstraint3D() {} +}; + +#endif // GODOT_CONSTRAINT_3D_H diff --git a/servers/physics_3d/godot_joint_3d.h b/servers/physics_3d/godot_joint_3d.h new file mode 100644 index 0000000000..4086bb53e1 --- /dev/null +++ b/servers/physics_3d/godot_joint_3d.h @@ -0,0 +1,68 @@ +/*************************************************************************/ +/* godot_joint_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_JOINT_3D_H +#define GODOT_JOINT_3D_H + +#include "godot_body_3d.h" +#include "godot_constraint_3d.h" + +class GodotJoint3D : public GodotConstraint3D { +protected: + bool dynamic_A = false; + bool dynamic_B = false; + +public: + virtual bool setup(real_t p_step) override { return false; } + virtual bool pre_solve(real_t p_step) override { return true; } + virtual void solve(real_t p_step) override {} + + void copy_settings_from(GodotJoint3D *p_joint) { + set_self(p_joint->get_self()); + set_priority(p_joint->get_priority()); + disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies()); + } + + virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_MAX; } + _FORCE_INLINE_ GodotJoint3D(GodotBody3D **p_body_ptr = nullptr, int p_body_count = 0) : + GodotConstraint3D(p_body_ptr, p_body_count) { + } + + virtual ~GodotJoint3D() { + for (int i = 0; i < get_body_count(); i++) { + GodotBody3D *body = get_body_ptr()[i]; + if (body) { + body->remove_constraint(this); + } + } + } +}; + +#endif // GODOT_JOINT_3D_H diff --git a/servers/physics_3d/godot_physics_server_3d.cpp b/servers/physics_3d/godot_physics_server_3d.cpp new file mode 100644 index 0000000000..34b56e733e --- /dev/null +++ b/servers/physics_3d/godot_physics_server_3d.cpp @@ -0,0 +1,1749 @@ +/*************************************************************************/ +/* godot_physics_server_3d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_physics_server_3d.h" + +#include "godot_body_direct_state_3d.h" +#include "godot_broad_phase_3d_bvh.h" +#include "joints/godot_cone_twist_joint_3d.h" +#include "joints/godot_generic_6dof_joint_3d.h" +#include "joints/godot_hinge_joint_3d.h" +#include "joints/godot_pin_joint_3d.h" +#include "joints/godot_slider_joint_3d.h" + +#include "core/debugger/engine_debugger.h" +#include "core/os/os.h" + +#define FLUSH_QUERY_CHECK(m_object) \ + ERR_FAIL_COND_MSG(m_object->get_space() && flushing_queries, "Can't change this state while flushing queries. Use call_deferred() or set_deferred() to change monitoring state instead."); + +RID GodotPhysicsServer3D::world_boundary_shape_create() { + GodotShape3D *shape = memnew(GodotWorldBoundaryShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID GodotPhysicsServer3D::separation_ray_shape_create() { + GodotShape3D *shape = memnew(GodotSeparationRayShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID GodotPhysicsServer3D::sphere_shape_create() { + GodotShape3D *shape = memnew(GodotSphereShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID GodotPhysicsServer3D::box_shape_create() { + GodotShape3D *shape = memnew(GodotBoxShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID GodotPhysicsServer3D::capsule_shape_create() { + GodotShape3D *shape = memnew(GodotCapsuleShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID GodotPhysicsServer3D::cylinder_shape_create() { + GodotShape3D *shape = memnew(GodotCylinderShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID GodotPhysicsServer3D::convex_polygon_shape_create() { + GodotShape3D *shape = memnew(GodotConvexPolygonShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID GodotPhysicsServer3D::concave_polygon_shape_create() { + GodotShape3D *shape = memnew(GodotConcavePolygonShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID GodotPhysicsServer3D::heightmap_shape_create() { + GodotShape3D *shape = memnew(GodotHeightMapShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID GodotPhysicsServer3D::custom_shape_create() { + ERR_FAIL_V(RID()); +} + +void GodotPhysicsServer3D::shape_set_data(RID p_shape, const Variant &p_data) { + GodotShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND(!shape); + shape->set_data(p_data); +}; + +void GodotPhysicsServer3D::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) { + GodotShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND(!shape); + shape->set_custom_bias(p_bias); +} + +PhysicsServer3D::ShapeType GodotPhysicsServer3D::shape_get_type(RID p_shape) const { + const GodotShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND_V(!shape, SHAPE_CUSTOM); + return shape->get_type(); +}; + +Variant GodotPhysicsServer3D::shape_get_data(RID p_shape) const { + const GodotShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND_V(!shape, Variant()); + ERR_FAIL_COND_V(!shape->is_configured(), Variant()); + return shape->get_data(); +}; + +void GodotPhysicsServer3D::shape_set_margin(RID p_shape, real_t p_margin) { +} + +real_t GodotPhysicsServer3D::shape_get_margin(RID p_shape) const { + return 0.0; +} + +real_t GodotPhysicsServer3D::shape_get_custom_solver_bias(RID p_shape) const { + const GodotShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND_V(!shape, 0); + return shape->get_custom_bias(); +} + +RID GodotPhysicsServer3D::space_create() { + GodotSpace3D *space = memnew(GodotSpace3D); + RID id = space_owner.make_rid(space); + space->set_self(id); + RID area_id = area_create(); + GodotArea3D *area = area_owner.get_or_null(area_id); + ERR_FAIL_COND_V(!area, RID()); + space->set_default_area(area); + area->set_space(space); + area->set_priority(-1); + RID sgb = body_create(); + body_set_space(sgb, id); + body_set_mode(sgb, BODY_MODE_STATIC); + space->set_static_global_body(sgb); + + return id; +}; + +void GodotPhysicsServer3D::space_set_active(RID p_space, bool p_active) { + GodotSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND(!space); + if (p_active) { + active_spaces.insert(space); + } else { + active_spaces.erase(space); + } +} + +bool GodotPhysicsServer3D::space_is_active(RID p_space) const { + const GodotSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND_V(!space, false); + + return active_spaces.has(space); +} + +void GodotPhysicsServer3D::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) { + GodotSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND(!space); + + space->set_param(p_param, p_value); +} + +real_t GodotPhysicsServer3D::space_get_param(RID p_space, SpaceParameter p_param) const { + const GodotSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND_V(!space, 0); + return space->get_param(p_param); +} + +PhysicsDirectSpaceState3D *GodotPhysicsServer3D::space_get_direct_state(RID p_space) { + GodotSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND_V(!space, nullptr); + ERR_FAIL_COND_V_MSG((using_threads && !doing_sync) || space->is_locked(), nullptr, "Space state is inaccessible right now, wait for iteration or physics process notification."); + + return space->get_direct_state(); +} + +void GodotPhysicsServer3D::space_set_debug_contacts(RID p_space, int p_max_contacts) { + GodotSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND(!space); + space->set_debug_contacts(p_max_contacts); +} + +Vector GodotPhysicsServer3D::space_get_contacts(RID p_space) const { + GodotSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND_V(!space, Vector()); + return space->get_debug_contacts(); +} + +int GodotPhysicsServer3D::space_get_contact_count(RID p_space) const { + GodotSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_COND_V(!space, 0); + return space->get_debug_contact_count(); +} + +RID GodotPhysicsServer3D::area_create() { + GodotArea3D *area = memnew(GodotArea3D); + RID rid = area_owner.make_rid(area); + area->set_self(rid); + return rid; +}; + +void GodotPhysicsServer3D::area_set_space(RID p_area, RID p_space) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + GodotSpace3D *space = nullptr; + if (p_space.is_valid()) { + space = space_owner.get_or_null(p_space); + ERR_FAIL_COND(!space); + } + + if (area->get_space() == space) { + return; //pointless + } + + area->clear_constraints(); + area->set_space(space); +}; + +RID GodotPhysicsServer3D::area_get_space(RID p_area) const { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, RID()); + + GodotSpace3D *space = area->get_space(); + if (!space) { + return RID(); + } + return space->get_self(); +}; + +void GodotPhysicsServer3D::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + area->set_space_override_mode(p_mode); +} + +PhysicsServer3D::AreaSpaceOverrideMode GodotPhysicsServer3D::area_get_space_override_mode(RID p_area) const { + const GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, AREA_SPACE_OVERRIDE_DISABLED); + + return area->get_space_override_mode(); +} + +void GodotPhysicsServer3D::area_add_shape(RID p_area, RID p_shape, const Transform3D &p_transform, bool p_disabled) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + GodotShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND(!shape); + + area->add_shape(shape, p_transform, p_disabled); +} + +void GodotPhysicsServer3D::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + GodotShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND(!shape); + ERR_FAIL_COND(!shape->is_configured()); + + area->set_shape(p_shape_idx, shape); +} + +void GodotPhysicsServer3D::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform3D &p_transform) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + area->set_shape_transform(p_shape_idx, p_transform); +} + +int GodotPhysicsServer3D::area_get_shape_count(RID p_area) const { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, -1); + + return area->get_shape_count(); +} + +RID GodotPhysicsServer3D::area_get_shape(RID p_area, int p_shape_idx) const { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, RID()); + + GodotShape3D *shape = area->get_shape(p_shape_idx); + ERR_FAIL_COND_V(!shape, RID()); + + return shape->get_self(); +} + +Transform3D GodotPhysicsServer3D::area_get_shape_transform(RID p_area, int p_shape_idx) const { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, Transform3D()); + + return area->get_shape_transform(p_shape_idx); +} + +void GodotPhysicsServer3D::area_remove_shape(RID p_area, int p_shape_idx) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + area->remove_shape(p_shape_idx); +} + +void GodotPhysicsServer3D::area_clear_shapes(RID p_area) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + while (area->get_shape_count()) { + area->remove_shape(0); + } +} + +void GodotPhysicsServer3D::area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + ERR_FAIL_INDEX(p_shape_idx, area->get_shape_count()); + FLUSH_QUERY_CHECK(area); + area->set_shape_disabled(p_shape_idx, p_disabled); +} + +void GodotPhysicsServer3D::area_attach_object_instance_id(RID p_area, ObjectID p_id) { + if (space_owner.owns(p_area)) { + GodotSpace3D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + area->set_instance_id(p_id); +} + +ObjectID GodotPhysicsServer3D::area_get_object_instance_id(RID p_area) const { + if (space_owner.owns(p_area)) { + GodotSpace3D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, ObjectID()); + return area->get_instance_id(); +} + +void GodotPhysicsServer3D::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) { + if (space_owner.owns(p_area)) { + GodotSpace3D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + area->set_param(p_param, p_value); +}; + +void GodotPhysicsServer3D::area_set_transform(RID p_area, const Transform3D &p_transform) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + area->set_transform(p_transform); +}; + +Variant GodotPhysicsServer3D::area_get_param(RID p_area, AreaParameter p_param) const { + if (space_owner.owns(p_area)) { + GodotSpace3D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, Variant()); + + return area->get_param(p_param); +}; + +Transform3D GodotPhysicsServer3D::area_get_transform(RID p_area) const { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, Transform3D()); + + return area->get_transform(); +}; + +void GodotPhysicsServer3D::area_set_collision_layer(RID p_area, uint32_t p_layer) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + area->set_collision_layer(p_layer); +} + +void GodotPhysicsServer3D::area_set_collision_mask(RID p_area, uint32_t p_mask) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + area->set_collision_mask(p_mask); +} + +void GodotPhysicsServer3D::area_set_monitorable(RID p_area, bool p_monitorable) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + FLUSH_QUERY_CHECK(area); + + area->set_monitorable(p_monitorable); +} + +void GodotPhysicsServer3D::area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + area->set_monitor_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method); +} + +void GodotPhysicsServer3D::area_set_ray_pickable(RID p_area, bool p_enable) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + area->set_ray_pickable(p_enable); +} + +void GodotPhysicsServer3D::area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + area->set_area_monitor_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method); +} + +/* BODY API */ + +RID GodotPhysicsServer3D::body_create() { + GodotBody3D *body = memnew(GodotBody3D); + RID rid = body_owner.make_rid(body); + body->set_self(rid); + return rid; +}; + +void GodotPhysicsServer3D::body_set_space(RID p_body, RID p_space) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + GodotSpace3D *space = nullptr; + if (p_space.is_valid()) { + space = space_owner.get_or_null(p_space); + ERR_FAIL_COND(!space); + } + + if (body->get_space() == space) { + return; //pointless + } + + body->clear_constraint_map(); + body->set_space(space); +}; + +RID GodotPhysicsServer3D::body_get_space(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, RID()); + + GodotSpace3D *space = body->get_space(); + if (!space) { + return RID(); + } + return space->get_self(); +}; + +void GodotPhysicsServer3D::body_set_mode(RID p_body, BodyMode p_mode) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_mode(p_mode); +}; + +PhysicsServer3D::BodyMode GodotPhysicsServer3D::body_get_mode(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, BODY_MODE_STATIC); + + return body->get_mode(); +}; + +void GodotPhysicsServer3D::body_add_shape(RID p_body, RID p_shape, const Transform3D &p_transform, bool p_disabled) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + GodotShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND(!shape); + + body->add_shape(shape, p_transform, p_disabled); +} + +void GodotPhysicsServer3D::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + GodotShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_COND(!shape); + ERR_FAIL_COND(!shape->is_configured()); + + body->set_shape(p_shape_idx, shape); +} +void GodotPhysicsServer3D::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform3D &p_transform) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_shape_transform(p_shape_idx, p_transform); +} + +int GodotPhysicsServer3D::body_get_shape_count(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, -1); + + return body->get_shape_count(); +} + +RID GodotPhysicsServer3D::body_get_shape(RID p_body, int p_shape_idx) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, RID()); + + GodotShape3D *shape = body->get_shape(p_shape_idx); + ERR_FAIL_COND_V(!shape, RID()); + + return shape->get_self(); +} + +void GodotPhysicsServer3D::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count()); + FLUSH_QUERY_CHECK(body); + + body->set_shape_disabled(p_shape_idx, p_disabled); +} + +Transform3D GodotPhysicsServer3D::body_get_shape_transform(RID p_body, int p_shape_idx) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, Transform3D()); + + return body->get_shape_transform(p_shape_idx); +} + +void GodotPhysicsServer3D::body_remove_shape(RID p_body, int p_shape_idx) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->remove_shape(p_shape_idx); +} + +void GodotPhysicsServer3D::body_clear_shapes(RID p_body) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + while (body->get_shape_count()) { + body->remove_shape(0); + } +} + +void GodotPhysicsServer3D::body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_continuous_collision_detection(p_enable); +} + +bool GodotPhysicsServer3D::body_is_continuous_collision_detection_enabled(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, false); + + return body->is_continuous_collision_detection_enabled(); +} + +void GodotPhysicsServer3D::body_set_collision_layer(RID p_body, uint32_t p_layer) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_collision_layer(p_layer); + body->wakeup(); +} + +uint32_t GodotPhysicsServer3D::body_get_collision_layer(RID p_body) const { + const GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, 0); + + return body->get_collision_layer(); +} + +void GodotPhysicsServer3D::body_set_collision_mask(RID p_body, uint32_t p_mask) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_collision_mask(p_mask); + body->wakeup(); +} + +uint32_t GodotPhysicsServer3D::body_get_collision_mask(RID p_body) const { + const GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, 0); + + return body->get_collision_mask(); +} + +void GodotPhysicsServer3D::body_attach_object_instance_id(RID p_body, ObjectID p_id) { + GodotBody3D *body = body_owner.get_or_null(p_body); + if (body) { + body->set_instance_id(p_id); + return; + } + + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + if (soft_body) { + soft_body->set_instance_id(p_id); + return; + } + + ERR_FAIL_MSG("Invalid ID."); +}; + +ObjectID GodotPhysicsServer3D::body_get_object_instance_id(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, ObjectID()); + + return body->get_instance_id(); +}; + +void GodotPhysicsServer3D::body_set_user_flags(RID p_body, uint32_t p_flags) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); +}; + +uint32_t GodotPhysicsServer3D::body_get_user_flags(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, 0); + + return 0; +}; + +void GodotPhysicsServer3D::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_param(p_param, p_value); +}; + +Variant GodotPhysicsServer3D::body_get_param(RID p_body, BodyParameter p_param) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, 0); + + return body->get_param(p_param); +}; + +void GodotPhysicsServer3D::body_reset_mass_properties(RID p_body) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + return body->reset_mass_properties(); +} + +void GodotPhysicsServer3D::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_state(p_state, p_variant); +}; + +Variant GodotPhysicsServer3D::body_get_state(RID p_body, BodyState p_state) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, Variant()); + + return body->get_state(p_state); +}; + +void GodotPhysicsServer3D::body_set_applied_force(RID p_body, const Vector3 &p_force) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_applied_force(p_force); + body->wakeup(); +}; + +Vector3 GodotPhysicsServer3D::body_get_applied_force(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, Vector3()); + return body->get_applied_force(); +}; + +void GodotPhysicsServer3D::body_set_applied_torque(RID p_body, const Vector3 &p_torque) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_applied_torque(p_torque); + body->wakeup(); +}; + +Vector3 GodotPhysicsServer3D::body_get_applied_torque(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, Vector3()); + + return body->get_applied_torque(); +}; + +void GodotPhysicsServer3D::body_add_central_force(RID p_body, const Vector3 &p_force) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->add_central_force(p_force); + body->wakeup(); +} + +void GodotPhysicsServer3D::body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->add_force(p_force, p_position); + body->wakeup(); +}; + +void GodotPhysicsServer3D::body_add_torque(RID p_body, const Vector3 &p_torque) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->add_torque(p_torque); + body->wakeup(); +}; + +void GodotPhysicsServer3D::body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + _update_shapes(); + + body->apply_central_impulse(p_impulse); + body->wakeup(); +} + +void GodotPhysicsServer3D::body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + _update_shapes(); + + body->apply_impulse(p_impulse, p_position); + body->wakeup(); +}; + +void GodotPhysicsServer3D::body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + _update_shapes(); + + body->apply_torque_impulse(p_impulse); + body->wakeup(); +}; + +void GodotPhysicsServer3D::body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + _update_shapes(); + + Vector3 v = body->get_linear_velocity(); + Vector3 axis = p_axis_velocity.normalized(); + v -= axis * axis.dot(v); + v += p_axis_velocity; + body->set_linear_velocity(v); + body->wakeup(); +}; + +void GodotPhysicsServer3D::body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_axis_lock(p_axis, p_lock); + body->wakeup(); +} + +bool GodotPhysicsServer3D::body_is_axis_locked(RID p_body, BodyAxis p_axis) const { + const GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, 0); + return body->is_axis_locked(p_axis); +} + +void GodotPhysicsServer3D::body_add_collision_exception(RID p_body, RID p_body_b) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->add_exception(p_body_b); + body->wakeup(); +}; + +void GodotPhysicsServer3D::body_remove_collision_exception(RID p_body, RID p_body_b) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->remove_exception(p_body_b); + body->wakeup(); +}; + +void GodotPhysicsServer3D::body_get_collision_exceptions(RID p_body, List *p_exceptions) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + for (int i = 0; i < body->get_exceptions().size(); i++) { + p_exceptions->push_back(body->get_exceptions()[i]); + } +}; + +void GodotPhysicsServer3D::body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); +}; + +real_t GodotPhysicsServer3D::body_get_contacts_reported_depth_threshold(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, 0); + return 0; +}; + +void GodotPhysicsServer3D::body_set_omit_force_integration(RID p_body, bool p_omit) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_omit_force_integration(p_omit); +}; + +bool GodotPhysicsServer3D::body_is_omitting_force_integration(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, false); + return body->get_omit_force_integration(); +}; + +void GodotPhysicsServer3D::body_set_max_contacts_reported(RID p_body, int p_contacts) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + body->set_max_contacts_reported(p_contacts); +} + +int GodotPhysicsServer3D::body_get_max_contacts_reported(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, -1); + return body->get_max_contacts_reported(); +} + +void GodotPhysicsServer3D::body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + body->set_state_sync_callback(p_instance, p_callback); +} + +void GodotPhysicsServer3D::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + body->set_force_integration_callback(p_callable, p_udata); +} + +void GodotPhysicsServer3D::body_set_ray_pickable(RID p_body, bool p_enable) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + body->set_ray_pickable(p_enable); +} + +bool GodotPhysicsServer3D::body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, false); + ERR_FAIL_COND_V(!body->get_space(), false); + ERR_FAIL_COND_V(body->get_space()->is_locked(), false); + + _update_shapes(); + + return body->get_space()->test_body_motion(body, p_parameters, r_result); +} + +PhysicsDirectBodyState3D *GodotPhysicsServer3D::body_get_direct_state(RID p_body) { + ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); + + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, nullptr); + + ERR_FAIL_NULL_V(body->get_space(), nullptr); + ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); + + return body->get_direct_state(); +} + +/* SOFT BODY */ + +RID GodotPhysicsServer3D::soft_body_create() { + GodotSoftBody3D *soft_body = memnew(GodotSoftBody3D); + RID rid = soft_body_owner.make_rid(soft_body); + soft_body->set_self(rid); + return rid; +} + +void GodotPhysicsServer3D::soft_body_update_rendering_server(RID p_body, RenderingServerHandler *p_rendering_server_handler) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->update_rendering_server(p_rendering_server_handler); +} + +void GodotPhysicsServer3D::soft_body_set_space(RID p_body, RID p_space) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND(!soft_body); + + GodotSpace3D *space = nullptr; + if (p_space.is_valid()) { + space = space_owner.get_or_null(p_space); + ERR_FAIL_COND(!space); + } + + if (soft_body->get_space() == space) { + return; + } + + soft_body->set_space(space); +} + +RID GodotPhysicsServer3D::soft_body_get_space(RID p_body) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!soft_body, RID()); + + GodotSpace3D *space = soft_body->get_space(); + if (!space) { + return RID(); + } + return space->get_self(); +} + +void GodotPhysicsServer3D::soft_body_set_collision_layer(RID p_body, uint32_t p_layer) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_collision_layer(p_layer); +} + +uint32_t GodotPhysicsServer3D::soft_body_get_collision_layer(RID p_body) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!soft_body, 0); + + return soft_body->get_collision_layer(); +} + +void GodotPhysicsServer3D::soft_body_set_collision_mask(RID p_body, uint32_t p_mask) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_collision_mask(p_mask); +} + +uint32_t GodotPhysicsServer3D::soft_body_get_collision_mask(RID p_body) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!soft_body, 0); + + return soft_body->get_collision_mask(); +} + +void GodotPhysicsServer3D::soft_body_add_collision_exception(RID p_body, RID p_body_b) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->add_exception(p_body_b); +} + +void GodotPhysicsServer3D::soft_body_remove_collision_exception(RID p_body, RID p_body_b) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->remove_exception(p_body_b); +} + +void GodotPhysicsServer3D::soft_body_get_collision_exceptions(RID p_body, List *p_exceptions) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND(!soft_body); + + for (int i = 0; i < soft_body->get_exceptions().size(); i++) { + p_exceptions->push_back(soft_body->get_exceptions()[i]); + } +} + +void GodotPhysicsServer3D::soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_state(p_state, p_variant); +} + +Variant GodotPhysicsServer3D::soft_body_get_state(RID p_body, BodyState p_state) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!soft_body, Variant()); + + return soft_body->get_state(p_state); +} + +void GodotPhysicsServer3D::soft_body_set_transform(RID p_body, const Transform3D &p_transform) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_state(BODY_STATE_TRANSFORM, p_transform); +} + +void GodotPhysicsServer3D::soft_body_set_ray_pickable(RID p_body, bool p_enable) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_ray_pickable(p_enable); +} + +void GodotPhysicsServer3D::soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_iteration_count(p_simulation_precision); +} + +int GodotPhysicsServer3D::soft_body_get_simulation_precision(RID p_body) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!soft_body, 0.f); + + return soft_body->get_iteration_count(); +} + +void GodotPhysicsServer3D::soft_body_set_total_mass(RID p_body, real_t p_total_mass) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_total_mass(p_total_mass); +} + +real_t GodotPhysicsServer3D::soft_body_get_total_mass(RID p_body) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!soft_body, 0.f); + + return soft_body->get_total_mass(); +} + +void GodotPhysicsServer3D::soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_linear_stiffness(p_stiffness); +} + +real_t GodotPhysicsServer3D::soft_body_get_linear_stiffness(RID p_body) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!soft_body, 0.f); + + return soft_body->get_linear_stiffness(); +} + +void GodotPhysicsServer3D::soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_pressure_coefficient(p_pressure_coefficient); +} + +real_t GodotPhysicsServer3D::soft_body_get_pressure_coefficient(RID p_body) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!soft_body, 0.f); + + return soft_body->get_pressure_coefficient(); +} + +void GodotPhysicsServer3D::soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_damping_coefficient(p_damping_coefficient); +} + +real_t GodotPhysicsServer3D::soft_body_get_damping_coefficient(RID p_body) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!soft_body, 0.f); + + return soft_body->get_damping_coefficient(); +} + +void GodotPhysicsServer3D::soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_drag_coefficient(p_drag_coefficient); +} + +real_t GodotPhysicsServer3D::soft_body_get_drag_coefficient(RID p_body) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!soft_body, 0.f); + + return soft_body->get_drag_coefficient(); +} + +void GodotPhysicsServer3D::soft_body_set_mesh(RID p_body, RID p_mesh) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_mesh(p_mesh); +} + +AABB GodotPhysicsServer3D::soft_body_get_bounds(RID p_body) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!soft_body, AABB()); + + return soft_body->get_bounds(); +} + +void GodotPhysicsServer3D::soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->set_vertex_position(p_point_index, p_global_position); +} + +Vector3 GodotPhysicsServer3D::soft_body_get_point_global_position(RID p_body, int p_point_index) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!soft_body, Vector3()); + + return soft_body->get_vertex_position(p_point_index); +} + +void GodotPhysicsServer3D::soft_body_remove_all_pinned_points(RID p_body) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND(!soft_body); + + soft_body->unpin_all_vertices(); +} + +void GodotPhysicsServer3D::soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND(!soft_body); + + if (p_pin) { + soft_body->pin_vertex(p_point_index); + } else { + soft_body->unpin_vertex(p_point_index); + } +} + +bool GodotPhysicsServer3D::soft_body_is_point_pinned(RID p_body, int p_point_index) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!soft_body, false); + + return soft_body->is_vertex_pinned(p_point_index); +} + +/* JOINT API */ + +RID GodotPhysicsServer3D::joint_create() { + GodotJoint3D *joint = memnew(GodotJoint3D); + RID rid = joint_owner.make_rid(joint); + joint->set_self(rid); + return rid; +} + +void GodotPhysicsServer3D::joint_clear(RID p_joint) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + if (joint->get_type() != JOINT_TYPE_MAX) { + GodotJoint3D *empty_joint = memnew(GodotJoint3D); + empty_joint->copy_settings_from(joint); + + joint_owner.replace(p_joint, empty_joint); + memdelete(joint); + } +} + +void GodotPhysicsServer3D::joint_make_pin(RID p_joint, RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) { + GodotBody3D *body_A = body_owner.get_or_null(p_body_A); + ERR_FAIL_COND(!body_A); + + if (!p_body_B.is_valid()) { + ERR_FAIL_COND(!body_A->get_space()); + p_body_B = body_A->get_space()->get_static_global_body(); + } + + GodotBody3D *body_B = body_owner.get_or_null(p_body_B); + ERR_FAIL_COND(!body_B); + + ERR_FAIL_COND(body_A == body_B); + + GodotJoint3D *prev_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); + + GodotJoint3D *joint = memnew(GodotPinJoint3D(body_A, p_local_A, body_B, p_local_B)); + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); +} + +void GodotPhysicsServer3D::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); + GodotPinJoint3D *pin_joint = static_cast(joint); + pin_joint->set_param(p_param, p_value); +} + +real_t GodotPhysicsServer3D::pin_joint_get_param(RID p_joint, PinJointParam p_param) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, 0); + GodotPinJoint3D *pin_joint = static_cast(joint); + return pin_joint->get_param(p_param); +} + +void GodotPhysicsServer3D::pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); + GodotPinJoint3D *pin_joint = static_cast(joint); + pin_joint->set_pos_a(p_A); +} + +Vector3 GodotPhysicsServer3D::pin_joint_get_local_a(RID p_joint) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!joint, Vector3()); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, Vector3()); + GodotPinJoint3D *pin_joint = static_cast(joint); + return pin_joint->get_position_a(); +} + +void GodotPhysicsServer3D::pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); + GodotPinJoint3D *pin_joint = static_cast(joint); + pin_joint->set_pos_b(p_B); +} + +Vector3 GodotPhysicsServer3D::pin_joint_get_local_b(RID p_joint) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!joint, Vector3()); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, Vector3()); + GodotPinJoint3D *pin_joint = static_cast(joint); + return pin_joint->get_position_b(); +} + +void GodotPhysicsServer3D::joint_make_hinge(RID p_joint, RID p_body_A, const Transform3D &p_frame_A, RID p_body_B, const Transform3D &p_frame_B) { + GodotBody3D *body_A = body_owner.get_or_null(p_body_A); + ERR_FAIL_COND(!body_A); + + if (!p_body_B.is_valid()) { + ERR_FAIL_COND(!body_A->get_space()); + p_body_B = body_A->get_space()->get_static_global_body(); + } + + GodotBody3D *body_B = body_owner.get_or_null(p_body_B); + ERR_FAIL_COND(!body_B); + + ERR_FAIL_COND(body_A == body_B); + + GodotJoint3D *prev_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); + + GodotJoint3D *joint = memnew(GodotHingeJoint3D(body_A, body_B, p_frame_A, p_frame_B)); + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); +} + +void GodotPhysicsServer3D::joint_make_hinge_simple(RID p_joint, RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) { + GodotBody3D *body_A = body_owner.get_or_null(p_body_A); + ERR_FAIL_COND(!body_A); + + if (!p_body_B.is_valid()) { + ERR_FAIL_COND(!body_A->get_space()); + p_body_B = body_A->get_space()->get_static_global_body(); + } + + GodotBody3D *body_B = body_owner.get_or_null(p_body_B); + ERR_FAIL_COND(!body_B); + + ERR_FAIL_COND(body_A == body_B); + + GodotJoint3D *prev_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); + + GodotJoint3D *joint = memnew(GodotHingeJoint3D(body_A, body_B, p_pivot_A, p_pivot_B, p_axis_A, p_axis_B)); + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); +} + +void GodotPhysicsServer3D::hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE); + GodotHingeJoint3D *hinge_joint = static_cast(joint); + hinge_joint->set_param(p_param, p_value); +} + +real_t GodotPhysicsServer3D::hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, 0); + GodotHingeJoint3D *hinge_joint = static_cast(joint); + return hinge_joint->get_param(p_param); +} + +void GodotPhysicsServer3D::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE); + GodotHingeJoint3D *hinge_joint = static_cast(joint); + hinge_joint->set_flag(p_flag, p_value); +} + +bool GodotPhysicsServer3D::hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!joint, false); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, false); + GodotHingeJoint3D *hinge_joint = static_cast(joint); + return hinge_joint->get_flag(p_flag); +} + +void GodotPhysicsServer3D::joint_set_solver_priority(RID p_joint, int p_priority) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(!joint); + joint->set_priority(p_priority); +} + +int GodotPhysicsServer3D::joint_get_solver_priority(RID p_joint) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!joint, 0); + return joint->get_priority(); +} + +void GodotPhysicsServer3D::joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(!joint); + + joint->disable_collisions_between_bodies(p_disable); + + if (2 == joint->get_body_count()) { + GodotBody3D *body_a = *joint->get_body_ptr(); + GodotBody3D *body_b = *(joint->get_body_ptr() + 1); + + if (p_disable) { + body_add_collision_exception(body_a->get_self(), body_b->get_self()); + body_add_collision_exception(body_b->get_self(), body_a->get_self()); + } else { + body_remove_collision_exception(body_a->get_self(), body_b->get_self()); + body_remove_collision_exception(body_b->get_self(), body_a->get_self()); + } + } +} + +bool GodotPhysicsServer3D::joint_is_disabled_collisions_between_bodies(RID p_joint) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!joint, true); + + return joint->is_disabled_collisions_between_bodies(); +} + +GodotPhysicsServer3D::JointType GodotPhysicsServer3D::joint_get_type(RID p_joint) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!joint, JOINT_TYPE_PIN); + return joint->get_type(); +} + +void GodotPhysicsServer3D::joint_make_slider(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) { + GodotBody3D *body_A = body_owner.get_or_null(p_body_A); + ERR_FAIL_COND(!body_A); + + if (!p_body_B.is_valid()) { + ERR_FAIL_COND(!body_A->get_space()); + p_body_B = body_A->get_space()->get_static_global_body(); + } + + GodotBody3D *body_B = body_owner.get_or_null(p_body_B); + ERR_FAIL_COND(!body_B); + + ERR_FAIL_COND(body_A == body_B); + + GodotJoint3D *prev_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); + + GodotJoint3D *joint = memnew(GodotSliderJoint3D(body_A, body_B, p_local_frame_A, p_local_frame_B)); + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); +} + +void GodotPhysicsServer3D::slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_SLIDER); + GodotSliderJoint3D *slider_joint = static_cast(joint); + slider_joint->set_param(p_param, p_value); +} + +real_t GodotPhysicsServer3D::slider_joint_get_param(RID p_joint, SliderJointParam p_param) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0); + GodotSliderJoint3D *slider_joint = static_cast(joint); + return slider_joint->get_param(p_param); +} + +void GodotPhysicsServer3D::joint_make_cone_twist(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) { + GodotBody3D *body_A = body_owner.get_or_null(p_body_A); + ERR_FAIL_COND(!body_A); + + if (!p_body_B.is_valid()) { + ERR_FAIL_COND(!body_A->get_space()); + p_body_B = body_A->get_space()->get_static_global_body(); + } + + GodotBody3D *body_B = body_owner.get_or_null(p_body_B); + ERR_FAIL_COND(!body_B); + + ERR_FAIL_COND(body_A == body_B); + + GodotJoint3D *prev_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); + + GodotJoint3D *joint = memnew(GodotConeTwistJoint3D(body_A, body_B, p_local_frame_A, p_local_frame_B)); + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); +} + +void GodotPhysicsServer3D::cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_CONE_TWIST); + GodotConeTwistJoint3D *cone_twist_joint = static_cast(joint); + cone_twist_joint->set_param(p_param, p_value); +} + +real_t GodotPhysicsServer3D::cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0); + GodotConeTwistJoint3D *cone_twist_joint = static_cast(joint); + return cone_twist_joint->get_param(p_param); +} + +void GodotPhysicsServer3D::joint_make_generic_6dof(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) { + GodotBody3D *body_A = body_owner.get_or_null(p_body_A); + ERR_FAIL_COND(!body_A); + + if (!p_body_B.is_valid()) { + ERR_FAIL_COND(!body_A->get_space()); + p_body_B = body_A->get_space()->get_static_global_body(); + } + + GodotBody3D *body_B = body_owner.get_or_null(p_body_B); + ERR_FAIL_COND(!body_B); + + ERR_FAIL_COND(body_A == body_B); + + GodotJoint3D *prev_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); + + GodotJoint3D *joint = memnew(GodotGeneric6DOFJoint3D(body_A, body_B, p_local_frame_A, p_local_frame_B, true)); + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); +} + +void GodotPhysicsServer3D::generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param, real_t p_value) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF); + GodotGeneric6DOFJoint3D *generic_6dof_joint = static_cast(joint); + generic_6dof_joint->set_param(p_axis, p_param, p_value); +} + +real_t GodotPhysicsServer3D::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, 0); + GodotGeneric6DOFJoint3D *generic_6dof_joint = static_cast(joint); + return generic_6dof_joint->get_param(p_axis, p_param); +} + +void GodotPhysicsServer3D::generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag, bool p_enable) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF); + GodotGeneric6DOFJoint3D *generic_6dof_joint = static_cast(joint); + generic_6dof_joint->set_flag(p_axis, p_flag, p_enable); +} + +bool GodotPhysicsServer3D::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!joint, false); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, false); + GodotGeneric6DOFJoint3D *generic_6dof_joint = static_cast(joint); + return generic_6dof_joint->get_flag(p_axis, p_flag); +} + +void GodotPhysicsServer3D::free(RID p_rid) { + _update_shapes(); //just in case + + if (shape_owner.owns(p_rid)) { + GodotShape3D *shape = shape_owner.get_or_null(p_rid); + + while (shape->get_owners().size()) { + GodotShapeOwner3D *so = shape->get_owners().front()->key(); + so->remove_shape(shape); + } + + shape_owner.free(p_rid); + memdelete(shape); + } else if (body_owner.owns(p_rid)) { + GodotBody3D *body = body_owner.get_or_null(p_rid); + + /* + if (body->get_state_query()) + _clear_query(body->get_state_query()); + + if (body->get_direct_state_query()) + _clear_query(body->get_direct_state_query()); + */ + + body->set_space(nullptr); + + while (body->get_shape_count()) { + body->remove_shape(0); + } + + body_owner.free(p_rid); + memdelete(body); + } else if (soft_body_owner.owns(p_rid)) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_rid); + + soft_body->set_space(nullptr); + + soft_body_owner.free(p_rid); + memdelete(soft_body); + } else if (area_owner.owns(p_rid)) { + GodotArea3D *area = area_owner.get_or_null(p_rid); + + /* + if (area->get_monitor_query()) + _clear_query(area->get_monitor_query()); + */ + + area->set_space(nullptr); + + while (area->get_shape_count()) { + area->remove_shape(0); + } + + area_owner.free(p_rid); + memdelete(area); + } else if (space_owner.owns(p_rid)) { + GodotSpace3D *space = space_owner.get_or_null(p_rid); + + while (space->get_objects().size()) { + GodotCollisionObject3D *co = (GodotCollisionObject3D *)space->get_objects().front()->get(); + co->set_space(nullptr); + } + + active_spaces.erase(space); + free(space->get_default_area()->get_self()); + free(space->get_static_global_body()); + + space_owner.free(p_rid); + memdelete(space); + } else if (joint_owner.owns(p_rid)) { + GodotJoint3D *joint = joint_owner.get_or_null(p_rid); + + joint_owner.free(p_rid); + memdelete(joint); + + } else { + ERR_FAIL_MSG("Invalid ID."); + } +}; + +void GodotPhysicsServer3D::set_active(bool p_active) { + active = p_active; +}; + +void GodotPhysicsServer3D::set_collision_iterations(int p_iterations) { + iterations = p_iterations; +}; + +void GodotPhysicsServer3D::init() { + iterations = 8; // 8? + stepper = memnew(GodotStep3D); +}; + +void GodotPhysicsServer3D::step(real_t p_step) { +#ifndef _3D_DISABLED + + if (!active) { + return; + } + + _update_shapes(); + + island_count = 0; + active_objects = 0; + collision_pairs = 0; + for (Set::Element *E = active_spaces.front(); E; E = E->next()) { + stepper->step((GodotSpace3D *)E->get(), p_step, iterations); + island_count += E->get()->get_island_count(); + active_objects += E->get()->get_active_objects(); + collision_pairs += E->get()->get_collision_pairs(); + } +#endif +} + +void GodotPhysicsServer3D::sync() { + doing_sync = true; +}; + +void GodotPhysicsServer3D::flush_queries() { +#ifndef _3D_DISABLED + + if (!active) { + return; + } + + flushing_queries = true; + + uint64_t time_beg = OS::get_singleton()->get_ticks_usec(); + + for (Set::Element *E = active_spaces.front(); E; E = E->next()) { + GodotSpace3D *space = (GodotSpace3D *)E->get(); + space->call_queries(); + } + + flushing_queries = false; + + if (EngineDebugger::is_profiling("servers")) { + uint64_t total_time[GodotSpace3D::ELAPSED_TIME_MAX]; + static const char *time_name[GodotSpace3D::ELAPSED_TIME_MAX] = { + "integrate_forces", + "generate_islands", + "setup_constraints", + "solve_constraints", + "integrate_velocities" + }; + + for (int i = 0; i < GodotSpace3D::ELAPSED_TIME_MAX; i++) { + total_time[i] = 0; + } + + for (Set::Element *E = active_spaces.front(); E; E = E->next()) { + for (int i = 0; i < GodotSpace3D::ELAPSED_TIME_MAX; i++) { + total_time[i] += E->get()->get_elapsed_time(GodotSpace3D::ElapsedTime(i)); + } + } + + Array values; + values.resize(GodotSpace3D::ELAPSED_TIME_MAX * 2); + for (int i = 0; i < GodotSpace3D::ELAPSED_TIME_MAX; i++) { + values[i * 2 + 0] = time_name[i]; + values[i * 2 + 1] = USEC_TO_SEC(total_time[i]); + } + values.push_back("flush_queries"); + values.push_back(USEC_TO_SEC(OS::get_singleton()->get_ticks_usec() - time_beg)); + + values.push_front("physics"); + EngineDebugger::profiler_add_frame_data("servers", values); + } +#endif +}; + +void GodotPhysicsServer3D::end_sync() { + doing_sync = false; +}; + +void GodotPhysicsServer3D::finish() { + memdelete(stepper); +}; + +int GodotPhysicsServer3D::get_process_info(ProcessInfo p_info) { + switch (p_info) { + case INFO_ACTIVE_OBJECTS: { + return active_objects; + } break; + case INFO_COLLISION_PAIRS: { + return collision_pairs; + } break; + case INFO_ISLAND_COUNT: { + return island_count; + } break; + } + + return 0; +} + +void GodotPhysicsServer3D::_update_shapes() { + while (pending_shape_update_list.first()) { + pending_shape_update_list.first()->self()->_shape_changed(); + pending_shape_update_list.remove(pending_shape_update_list.first()); + } +} + +void GodotPhysicsServer3D::_shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { + CollCbkData *cbk = (CollCbkData *)p_userdata; + + if (cbk->max == 0) { + return; + } + + if (cbk->amount == cbk->max) { + //find least deep + real_t min_depth = 1e20; + int min_depth_idx = 0; + for (int i = 0; i < cbk->amount; i++) { + real_t d = cbk->ptr[i * 2 + 0].distance_squared_to(cbk->ptr[i * 2 + 1]); + if (d < min_depth) { + min_depth = d; + min_depth_idx = i; + } + } + + real_t d = p_point_A.distance_squared_to(p_point_B); + if (d < min_depth) { + return; + } + cbk->ptr[min_depth_idx * 2 + 0] = p_point_A; + cbk->ptr[min_depth_idx * 2 + 1] = p_point_B; + + } else { + cbk->ptr[cbk->amount * 2 + 0] = p_point_A; + cbk->ptr[cbk->amount * 2 + 1] = p_point_B; + cbk->amount++; + } +} + +GodotPhysicsServer3D *GodotPhysicsServer3D::godot_singleton = nullptr; +GodotPhysicsServer3D::GodotPhysicsServer3D(bool p_using_threads) { + godot_singleton = this; + GodotBroadPhase3D::create_func = GodotBroadPhase3DBVH::_create; + + using_threads = p_using_threads; +}; diff --git a/servers/physics_3d/godot_physics_server_3d.h b/servers/physics_3d/godot_physics_server_3d.h new file mode 100644 index 0000000000..3ed9e320dc --- /dev/null +++ b/servers/physics_3d/godot_physics_server_3d.h @@ -0,0 +1,380 @@ +/*************************************************************************/ +/* godot_physics_server_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_PHYSICS_SERVER_3D_H +#define GODOT_PHYSICS_SERVER_3D_H + +#include "godot_joint_3d.h" +#include "godot_shape_3d.h" +#include "godot_space_3d.h" +#include "godot_step_3d.h" + +#include "core/templates/rid_owner.h" +#include "servers/physics_server_3d.h" + +class GodotPhysicsServer3D : public PhysicsServer3D { + GDCLASS(GodotPhysicsServer3D, PhysicsServer3D); + + friend class GodotPhysicsDirectSpaceState3D; + bool active = true; + int iterations = 0; + + int island_count = 0; + int active_objects = 0; + int collision_pairs = 0; + + bool using_threads = false; + bool doing_sync = false; + bool flushing_queries = false; + + GodotStep3D *stepper = nullptr; + Set active_spaces; + + mutable RID_PtrOwner shape_owner; + mutable RID_PtrOwner space_owner; + mutable RID_PtrOwner area_owner; + mutable RID_PtrOwner body_owner; + mutable RID_PtrOwner soft_body_owner; + mutable RID_PtrOwner joint_owner; + + //void _clear_query(QuerySW *p_query); + friend class GodotCollisionObject3D; + SelfList::List pending_shape_update_list; + void _update_shapes(); + + static GodotPhysicsServer3D *godot_singleton; + +public: + struct CollCbkData { + int max; + int amount; + Vector3 *ptr; + }; + + static void _shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); + + virtual RID world_boundary_shape_create() override; + virtual RID separation_ray_shape_create() override; + virtual RID sphere_shape_create() override; + virtual RID box_shape_create() override; + virtual RID capsule_shape_create() override; + virtual RID cylinder_shape_create() override; + virtual RID convex_polygon_shape_create() override; + virtual RID concave_polygon_shape_create() override; + virtual RID heightmap_shape_create() override; + virtual RID custom_shape_create() override; + + virtual void shape_set_data(RID p_shape, const Variant &p_data) override; + virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias) override; + + virtual ShapeType shape_get_type(RID p_shape) const override; + virtual Variant shape_get_data(RID p_shape) const override; + + virtual void shape_set_margin(RID p_shape, real_t p_margin) override; + virtual real_t shape_get_margin(RID p_shape) const override; + + virtual real_t shape_get_custom_solver_bias(RID p_shape) const override; + + /* SPACE API */ + + virtual RID space_create() override; + virtual void space_set_active(RID p_space, bool p_active) override; + virtual bool space_is_active(RID p_space) const override; + + virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) override; + virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const override; + + // this function only works on physics process, errors and returns null otherwise + virtual PhysicsDirectSpaceState3D *space_get_direct_state(RID p_space) override; + + virtual void space_set_debug_contacts(RID p_space, int p_max_contacts) override; + virtual Vector space_get_contacts(RID p_space) const override; + virtual int space_get_contact_count(RID p_space) const override; + + /* AREA API */ + + virtual RID area_create() override; + + virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) override; + virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const override; + + virtual void area_set_space(RID p_area, RID p_space) override; + virtual RID area_get_space(RID p_area) const override; + + virtual void area_add_shape(RID p_area, RID p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false) override; + virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape) override; + virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform3D &p_transform) override; + + virtual int area_get_shape_count(RID p_area) const override; + virtual RID area_get_shape(RID p_area, int p_shape_idx) const override; + virtual Transform3D area_get_shape_transform(RID p_area, int p_shape_idx) const override; + + virtual void area_remove_shape(RID p_area, int p_shape_idx) override; + virtual void area_clear_shapes(RID p_area) override; + + virtual void area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) override; + + virtual void area_attach_object_instance_id(RID p_area, ObjectID p_id) override; + virtual ObjectID area_get_object_instance_id(RID p_area) const override; + + virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) override; + virtual void area_set_transform(RID p_area, const Transform3D &p_transform) override; + + virtual Variant area_get_param(RID p_area, AreaParameter p_param) const override; + virtual Transform3D area_get_transform(RID p_area) const override; + + virtual void area_set_ray_pickable(RID p_area, bool p_enable) override; + + virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) override; + virtual void area_set_collision_layer(RID p_area, uint32_t p_layer) override; + + virtual void area_set_monitorable(RID p_area, bool p_monitorable) override; + + virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) override; + virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) override; + + /* BODY API */ + + // create a body of a given type + virtual RID body_create() override; + + virtual void body_set_space(RID p_body, RID p_space) override; + virtual RID body_get_space(RID p_body) const override; + + virtual void body_set_mode(RID p_body, BodyMode p_mode) override; + virtual BodyMode body_get_mode(RID p_body) const override; + + virtual void body_add_shape(RID p_body, RID p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false) override; + virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape) override; + virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform3D &p_transform) override; + + virtual int body_get_shape_count(RID p_body) const override; + virtual RID body_get_shape(RID p_body, int p_shape_idx) const override; + virtual Transform3D body_get_shape_transform(RID p_body, int p_shape_idx) const override; + + virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) override; + + virtual void body_remove_shape(RID p_body, int p_shape_idx) override; + virtual void body_clear_shapes(RID p_body) override; + + virtual void body_attach_object_instance_id(RID p_body, ObjectID p_id) override; + virtual ObjectID body_get_object_instance_id(RID p_body) const override; + + virtual void body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) override; + virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const override; + + virtual void body_set_collision_layer(RID p_body, uint32_t p_layer) override; + virtual uint32_t body_get_collision_layer(RID p_body) const override; + + virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) override; + virtual uint32_t body_get_collision_mask(RID p_body) const override; + + virtual void body_set_user_flags(RID p_body, uint32_t p_flags) override; + virtual uint32_t body_get_user_flags(RID p_body) const override; + + virtual void body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) override; + virtual Variant body_get_param(RID p_body, BodyParameter p_param) const override; + + virtual void body_reset_mass_properties(RID p_body) override; + + virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override; + virtual Variant body_get_state(RID p_body, BodyState p_state) const override; + + virtual void body_set_applied_force(RID p_body, const Vector3 &p_force) override; + virtual Vector3 body_get_applied_force(RID p_body) const override; + + virtual void body_set_applied_torque(RID p_body, const Vector3 &p_torque) override; + virtual Vector3 body_get_applied_torque(RID p_body) const override; + + virtual void body_add_central_force(RID p_body, const Vector3 &p_force) override; + virtual void body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position = Vector3()) override; + virtual void body_add_torque(RID p_body, const Vector3 &p_torque) override; + + virtual void body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) override; + virtual void body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) override; + virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) override; + virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) override; + + virtual void body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) override; + virtual bool body_is_axis_locked(RID p_body, BodyAxis p_axis) const override; + + virtual void body_add_collision_exception(RID p_body, RID p_body_b) override; + virtual void body_remove_collision_exception(RID p_body, RID p_body_b) override; + virtual void body_get_collision_exceptions(RID p_body, List *p_exceptions) override; + + virtual void body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) override; + virtual real_t body_get_contacts_reported_depth_threshold(RID p_body) const override; + + virtual void body_set_omit_force_integration(RID p_body, bool p_omit) override; + virtual bool body_is_omitting_force_integration(RID p_body) const override; + + virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) override; + virtual int body_get_max_contacts_reported(RID p_body) const override; + + virtual void body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) override; + virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override; + + virtual void body_set_ray_pickable(RID p_body, bool p_enable) override; + + virtual bool body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result = nullptr) override; + + // this function only works on physics process, errors and returns null otherwise + virtual PhysicsDirectBodyState3D *body_get_direct_state(RID p_body) override; + + /* SOFT BODY */ + + virtual RID soft_body_create() override; + + virtual void soft_body_update_rendering_server(RID p_body, RenderingServerHandler *p_rendering_server_handler) override; + + virtual void soft_body_set_space(RID p_body, RID p_space) override; + virtual RID soft_body_get_space(RID p_body) const override; + + virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer) override; + virtual uint32_t soft_body_get_collision_layer(RID p_body) const override; + + virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask) override; + virtual uint32_t soft_body_get_collision_mask(RID p_body) const override; + + virtual void soft_body_add_collision_exception(RID p_body, RID p_body_b) override; + virtual void soft_body_remove_collision_exception(RID p_body, RID p_body_b) override; + virtual void soft_body_get_collision_exceptions(RID p_body, List *p_exceptions) override; + + virtual void soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override; + virtual Variant soft_body_get_state(RID p_body, BodyState p_state) const override; + + virtual void soft_body_set_transform(RID p_body, const Transform3D &p_transform) override; + + virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) override; + + virtual void soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) override; + virtual int soft_body_get_simulation_precision(RID p_body) const override; + + virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass) override; + virtual real_t soft_body_get_total_mass(RID p_body) const override; + + virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) override; + virtual real_t soft_body_get_linear_stiffness(RID p_body) const override; + + virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) override; + virtual real_t soft_body_get_pressure_coefficient(RID p_body) const override; + + virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) override; + virtual real_t soft_body_get_damping_coefficient(RID p_body) const override; + + virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) override; + virtual real_t soft_body_get_drag_coefficient(RID p_body) const override; + + virtual void soft_body_set_mesh(RID p_body, RID p_mesh) override; + + virtual AABB soft_body_get_bounds(RID p_body) const override; + + virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) override; + virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) const override; + + virtual void soft_body_remove_all_pinned_points(RID p_body) override; + virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) override; + virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) const override; + + /* JOINT API */ + + virtual RID joint_create() override; + + virtual void joint_clear(RID p_joint) override; //resets type + + virtual void joint_make_pin(RID p_joint, RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) override; + + virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) override; + virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const override; + + virtual void pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) override; + virtual Vector3 pin_joint_get_local_a(RID p_joint) const override; + + virtual void pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) override; + virtual Vector3 pin_joint_get_local_b(RID p_joint) const override; + + virtual void joint_make_hinge(RID p_joint, RID p_body_A, const Transform3D &p_frame_A, RID p_body_B, const Transform3D &p_frame_B) override; + virtual void joint_make_hinge_simple(RID p_joint, RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) override; + + virtual void hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) override; + virtual real_t hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const override; + + virtual void hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) override; + virtual bool hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const override; + + virtual void joint_make_slider(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) override; //reference frame is A + + virtual void slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) override; + virtual real_t slider_joint_get_param(RID p_joint, SliderJointParam p_param) const override; + + virtual void joint_make_cone_twist(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) override; //reference frame is A + + virtual void cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) override; + virtual real_t cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const override; + + virtual void joint_make_generic_6dof(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) override; //reference frame is A + + virtual void generic_6dof_joint_set_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param, real_t p_value) override; + virtual real_t generic_6dof_joint_get_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param) const override; + + virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag, bool p_enable) override; + virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag) const override; + + virtual JointType joint_get_type(RID p_joint) const override; + + virtual void joint_set_solver_priority(RID p_joint, int p_priority) override; + virtual int joint_get_solver_priority(RID p_joint) const override; + + virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) override; + virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const override; + + /* MISC */ + + virtual void free(RID p_rid) override; + + virtual void set_active(bool p_active) override; + virtual void init() override; + virtual void step(real_t p_step) override; + virtual void sync() override; + virtual void flush_queries() override; + virtual void end_sync() override; + virtual void finish() override; + + virtual void set_collision_iterations(int p_iterations) override; + + virtual bool is_flushing_queries() const override { return flushing_queries; } + + int get_process_info(ProcessInfo p_info) override; + + GodotPhysicsServer3D(bool p_using_threads = false); + ~GodotPhysicsServer3D() {} +}; + +#endif // GODOT_PHYSICS_SERVER_3D_H diff --git a/servers/physics_3d/godot_shape_3d.cpp b/servers/physics_3d/godot_shape_3d.cpp new file mode 100644 index 0000000000..4c12a5a948 --- /dev/null +++ b/servers/physics_3d/godot_shape_3d.cpp @@ -0,0 +1,2202 @@ +/*************************************************************************/ +/* godot_shape_3d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_shape_3d.h" + +#include "core/io/image.h" +#include "core/math/convex_hull.h" +#include "core/math/geometry_3d.h" +#include "core/templates/sort_array.h" + +// GodotHeightMapShape3D is based on Bullet btHeightfieldTerrainShape. + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.0002 +#define _FACE_IS_VALID_SUPPORT_THRESHOLD 0.9998 + +#define _CYLINDER_EDGE_IS_VALID_SUPPORT_THRESHOLD 0.002 +#define _CYLINDER_FACE_IS_VALID_SUPPORT_THRESHOLD 0.999 + +void GodotShape3D::configure(const AABB &p_aabb) { + aabb = p_aabb; + configured = true; + for (const KeyValue &E : owners) { + GodotShapeOwner3D *co = (GodotShapeOwner3D *)E.key; + co->_shape_changed(); + } +} + +Vector3 GodotShape3D::get_support(const Vector3 &p_normal) const { + Vector3 res; + int amnt; + FeatureType type; + get_supports(p_normal, 1, &res, amnt, type); + return res; +} + +void GodotShape3D::add_owner(GodotShapeOwner3D *p_owner) { + Map::Element *E = owners.find(p_owner); + if (E) { + E->get()++; + } else { + owners[p_owner] = 1; + } +} + +void GodotShape3D::remove_owner(GodotShapeOwner3D *p_owner) { + Map::Element *E = owners.find(p_owner); + ERR_FAIL_COND(!E); + E->get()--; + if (E->get() == 0) { + owners.erase(E); + } +} + +bool GodotShape3D::is_owner(GodotShapeOwner3D *p_owner) const { + return owners.has(p_owner); +} + +const Map &GodotShape3D::get_owners() const { + return owners; +} + +GodotShape3D::~GodotShape3D() { + ERR_FAIL_COND(owners.size()); +} + +Plane GodotWorldBoundaryShape3D::get_plane() const { + return plane; +} + +void GodotWorldBoundaryShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + // gibberish, a plane is infinity + r_min = -1e7; + r_max = 1e7; +} + +Vector3 GodotWorldBoundaryShape3D::get_support(const Vector3 &p_normal) const { + return p_normal * 1e15; +} + +bool GodotWorldBoundaryShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + bool inters = plane.intersects_segment(p_begin, p_end, &r_result); + if (inters) { + r_normal = plane.normal; + } + return inters; +} + +bool GodotWorldBoundaryShape3D::intersect_point(const Vector3 &p_point) const { + return plane.distance_to(p_point) < 0; +} + +Vector3 GodotWorldBoundaryShape3D::get_closest_point_to(const Vector3 &p_point) const { + if (plane.is_point_over(p_point)) { + return plane.project(p_point); + } else { + return p_point; + } +} + +Vector3 GodotWorldBoundaryShape3D::get_moment_of_inertia(real_t p_mass) const { + return Vector3(); // not applicable. +} + +void GodotWorldBoundaryShape3D::_setup(const Plane &p_plane) { + plane = p_plane; + configure(AABB(Vector3(-1e4, -1e4, -1e4), Vector3(1e4 * 2, 1e4 * 2, 1e4 * 2))); +} + +void GodotWorldBoundaryShape3D::set_data(const Variant &p_data) { + _setup(p_data); +} + +Variant GodotWorldBoundaryShape3D::get_data() const { + return plane; +} + +GodotWorldBoundaryShape3D::GodotWorldBoundaryShape3D() { +} + +// + +real_t GodotSeparationRayShape3D::get_length() const { + return length; +} + +bool GodotSeparationRayShape3D::get_slide_on_slope() const { + return slide_on_slope; +} + +void GodotSeparationRayShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + // don't think this will be even used + r_min = 0; + r_max = 1; +} + +Vector3 GodotSeparationRayShape3D::get_support(const Vector3 &p_normal) const { + if (p_normal.z > 0) { + return Vector3(0, 0, length); + } else { + return Vector3(0, 0, 0); + } +} + +void GodotSeparationRayShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { + r_amount = 2; + r_type = FEATURE_EDGE; + r_supports[0] = Vector3(0, 0, 0); + r_supports[1] = Vector3(0, 0, length); + } else if (p_normal.z > 0) { + r_amount = 1; + r_type = FEATURE_POINT; + *r_supports = Vector3(0, 0, length); + } else { + r_amount = 1; + r_type = FEATURE_POINT; + *r_supports = Vector3(0, 0, 0); + } +} + +bool GodotSeparationRayShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + return false; //simply not possible +} + +bool GodotSeparationRayShape3D::intersect_point(const Vector3 &p_point) const { + return false; //simply not possible +} + +Vector3 GodotSeparationRayShape3D::get_closest_point_to(const Vector3 &p_point) const { + Vector3 s[2] = { + Vector3(0, 0, 0), + Vector3(0, 0, length) + }; + + return Geometry3D::get_closest_point_to_segment(p_point, s); +} + +Vector3 GodotSeparationRayShape3D::get_moment_of_inertia(real_t p_mass) const { + return Vector3(); +} + +void GodotSeparationRayShape3D::_setup(real_t p_length, bool p_slide_on_slope) { + length = p_length; + slide_on_slope = p_slide_on_slope; + configure(AABB(Vector3(0, 0, 0), Vector3(0.1, 0.1, length))); +} + +void GodotSeparationRayShape3D::set_data(const Variant &p_data) { + Dictionary d = p_data; + _setup(d["length"], d["slide_on_slope"]); +} + +Variant GodotSeparationRayShape3D::get_data() const { + Dictionary d; + d["length"] = length; + d["slide_on_slope"] = slide_on_slope; + return d; +} + +GodotSeparationRayShape3D::GodotSeparationRayShape3D() {} + +/********** SPHERE *************/ + +real_t GodotSphereShape3D::get_radius() const { + return radius; +} + +void GodotSphereShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + real_t d = p_normal.dot(p_transform.origin); + + // figure out scale at point + Vector3 local_normal = p_transform.basis.xform_inv(p_normal); + real_t scale = local_normal.length(); + + r_min = d - (radius)*scale; + r_max = d + (radius)*scale; +} + +Vector3 GodotSphereShape3D::get_support(const Vector3 &p_normal) const { + return p_normal * radius; +} + +void GodotSphereShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + *r_supports = p_normal * radius; + r_amount = 1; + r_type = FEATURE_POINT; +} + +bool GodotSphereShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + return Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal); +} + +bool GodotSphereShape3D::intersect_point(const Vector3 &p_point) const { + return p_point.length() < radius; +} + +Vector3 GodotSphereShape3D::get_closest_point_to(const Vector3 &p_point) const { + Vector3 p = p_point; + real_t l = p.length(); + if (l < radius) { + return p_point; + } + return (p / l) * radius; +} + +Vector3 GodotSphereShape3D::get_moment_of_inertia(real_t p_mass) const { + real_t s = 0.4 * p_mass * radius * radius; + return Vector3(s, s, s); +} + +void GodotSphereShape3D::_setup(real_t p_radius) { + radius = p_radius; + configure(AABB(Vector3(-radius, -radius, -radius), Vector3(radius * 2.0, radius * 2.0, radius * 2.0))); +} + +void GodotSphereShape3D::set_data(const Variant &p_data) { + _setup(p_data); +} + +Variant GodotSphereShape3D::get_data() const { + return radius; +} + +GodotSphereShape3D::GodotSphereShape3D() {} + +/********** BOX *************/ + +void GodotBoxShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + // no matter the angle, the box is mirrored anyway + Vector3 local_normal = p_transform.basis.xform_inv(p_normal); + + real_t length = local_normal.abs().dot(half_extents); + real_t distance = p_normal.dot(p_transform.origin); + + r_min = distance - length; + r_max = distance + length; +} + +Vector3 GodotBoxShape3D::get_support(const Vector3 &p_normal) const { + Vector3 point( + (p_normal.x < 0) ? -half_extents.x : half_extents.x, + (p_normal.y < 0) ? -half_extents.y : half_extents.y, + (p_normal.z < 0) ? -half_extents.z : half_extents.z); + + return point; +} + +void GodotBoxShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + static const int next[3] = { 1, 2, 0 }; + static const int next2[3] = { 2, 0, 1 }; + + for (int i = 0; i < 3; i++) { + Vector3 axis; + axis[i] = 1.0; + real_t dot = p_normal.dot(axis); + if (Math::abs(dot) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { + //Vector3 axis_b; + + bool neg = dot < 0; + r_amount = 4; + r_type = FEATURE_FACE; + + Vector3 point; + point[i] = half_extents[i]; + + int i_n = next[i]; + int i_n2 = next2[i]; + + static const real_t sign[4][2] = { + { -1.0, 1.0 }, + { 1.0, 1.0 }, + { 1.0, -1.0 }, + { -1.0, -1.0 }, + }; + + for (int j = 0; j < 4; j++) { + point[i_n] = sign[j][0] * half_extents[i_n]; + point[i_n2] = sign[j][1] * half_extents[i_n2]; + r_supports[j] = neg ? -point : point; + } + + if (neg) { + SWAP(r_supports[1], r_supports[2]); + SWAP(r_supports[0], r_supports[3]); + } + + return; + } + + r_amount = 0; + } + + for (int i = 0; i < 3; i++) { + Vector3 axis; + axis[i] = 1.0; + + if (Math::abs(p_normal.dot(axis)) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { + r_amount = 2; + r_type = FEATURE_EDGE; + + int i_n = next[i]; + int i_n2 = next2[i]; + + Vector3 point = half_extents; + + if (p_normal[i_n] < 0) { + point[i_n] = -point[i_n]; + } + if (p_normal[i_n2] < 0) { + point[i_n2] = -point[i_n2]; + } + + r_supports[0] = point; + point[i] = -point[i]; + r_supports[1] = point; + return; + } + } + /* USE POINT */ + + Vector3 point( + (p_normal.x < 0) ? -half_extents.x : half_extents.x, + (p_normal.y < 0) ? -half_extents.y : half_extents.y, + (p_normal.z < 0) ? -half_extents.z : half_extents.z); + + r_amount = 1; + r_type = FEATURE_POINT; + r_supports[0] = point; +} + +bool GodotBoxShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + AABB aabb(-half_extents, half_extents * 2.0); + + return aabb.intersects_segment(p_begin, p_end, &r_result, &r_normal); +} + +bool GodotBoxShape3D::intersect_point(const Vector3 &p_point) const { + return (Math::abs(p_point.x) < half_extents.x && Math::abs(p_point.y) < half_extents.y && Math::abs(p_point.z) < half_extents.z); +} + +Vector3 GodotBoxShape3D::get_closest_point_to(const Vector3 &p_point) const { + int outside = 0; + Vector3 min_point; + + for (int i = 0; i < 3; i++) { + if (Math::abs(p_point[i]) > half_extents[i]) { + outside++; + if (outside == 1) { + //use plane if only one side matches + Vector3 n; + n[i] = SGN(p_point[i]); + + Plane p(n, half_extents[i]); + min_point = p.project(p_point); + } + } + } + + if (!outside) { + return p_point; //it's inside, don't do anything else + } + + if (outside == 1) { //if only above one plane, this plane clearly wins + return min_point; + } + + //check segments + real_t min_distance = 1e20; + Vector3 closest_vertex = half_extents * p_point.sign(); + Vector3 s[2] = { + closest_vertex, + closest_vertex + }; + + for (int i = 0; i < 3; i++) { + s[1] = closest_vertex; + s[1][i] = -s[1][i]; //edge + + Vector3 closest_edge = Geometry3D::get_closest_point_to_segment(p_point, s); + + real_t d = p_point.distance_to(closest_edge); + if (d < min_distance) { + min_point = closest_edge; + min_distance = d; + } + } + + return min_point; +} + +Vector3 GodotBoxShape3D::get_moment_of_inertia(real_t p_mass) const { + real_t lx = half_extents.x; + real_t ly = half_extents.y; + real_t lz = half_extents.z; + + return Vector3((p_mass / 3.0) * (ly * ly + lz * lz), (p_mass / 3.0) * (lx * lx + lz * lz), (p_mass / 3.0) * (lx * lx + ly * ly)); +} + +void GodotBoxShape3D::_setup(const Vector3 &p_half_extents) { + half_extents = p_half_extents.abs(); + + configure(AABB(-half_extents, half_extents * 2)); +} + +void GodotBoxShape3D::set_data(const Variant &p_data) { + _setup(p_data); +} + +Variant GodotBoxShape3D::get_data() const { + return half_extents; +} + +GodotBoxShape3D::GodotBoxShape3D() {} + +/********** CAPSULE *************/ + +void GodotCapsuleShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + Vector3 n = p_transform.basis.xform_inv(p_normal).normalized(); + real_t h = height * 0.5 - radius; + + n *= radius; + n.y += (n.y > 0) ? h : -h; + + r_max = p_normal.dot(p_transform.xform(n)); + r_min = p_normal.dot(p_transform.xform(-n)); +} + +Vector3 GodotCapsuleShape3D::get_support(const Vector3 &p_normal) const { + Vector3 n = p_normal; + + real_t h = height * 0.5 - radius; + + n *= radius; + n.y += (n.y > 0) ? h : -h; + return n; +} + +void GodotCapsuleShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + Vector3 n = p_normal; + + real_t d = n.y; + + if (Math::abs(d) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { + // make it flat + n.y = 0.0; + n.normalize(); + n *= radius; + + r_amount = 2; + r_type = FEATURE_EDGE; + r_supports[0] = n; + r_supports[0].y += height * 0.5 - radius; + r_supports[1] = n; + r_supports[1].y -= height * 0.5 - radius; + + } else { + real_t h = height * 0.5 - radius; + + n *= radius; + n.y += (d > 0) ? h : -h; + r_amount = 1; + r_type = FEATURE_POINT; + *r_supports = n; + } +} + +bool GodotCapsuleShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + Vector3 norm = (p_end - p_begin).normalized(); + real_t min_d = 1e20; + + Vector3 res, n; + bool collision = false; + + Vector3 auxres, auxn; + bool collided; + + // test against cylinder and spheres :-| + + collided = Geometry3D::segment_intersects_cylinder(p_begin, p_end, height - radius * 2.0, radius, &auxres, &auxn, 1); + + if (collided) { + real_t d = norm.dot(auxres); + if (d < min_d) { + min_d = d; + res = auxres; + n = auxn; + collision = true; + } + } + + collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * 0.5 - radius, 0), radius, &auxres, &auxn); + + if (collided) { + real_t d = norm.dot(auxres); + if (d < min_d) { + min_d = d; + res = auxres; + n = auxn; + collision = true; + } + } + + collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * -0.5 + radius, 0), radius, &auxres, &auxn); + + if (collided) { + real_t d = norm.dot(auxres); + + if (d < min_d) { + min_d = d; + res = auxres; + n = auxn; + collision = true; + } + } + + if (collision) { + r_result = res; + r_normal = n; + } + return collision; +} + +bool GodotCapsuleShape3D::intersect_point(const Vector3 &p_point) const { + if (Math::abs(p_point.y) < height * 0.5 - radius) { + return Vector3(p_point.x, 0, p_point.z).length() < radius; + } else { + Vector3 p = p_point; + p.y = Math::abs(p.y) - height * 0.5 + radius; + return p.length() < radius; + } +} + +Vector3 GodotCapsuleShape3D::get_closest_point_to(const Vector3 &p_point) const { + Vector3 s[2] = { + Vector3(0, -height * 0.5 + radius, 0), + Vector3(0, height * 0.5 - radius, 0), + }; + + Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, s); + + if (p.distance_to(p_point) < radius) { + return p_point; + } + + return p + (p_point - p).normalized() * radius; +} + +Vector3 GodotCapsuleShape3D::get_moment_of_inertia(real_t p_mass) const { + // use bad AABB approximation + Vector3 extents = get_aabb().size * 0.5; + + return Vector3( + (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); +} + +void GodotCapsuleShape3D::_setup(real_t p_height, real_t p_radius) { + height = p_height; + radius = p_radius; + configure(AABB(Vector3(-radius, -height * 0.5, -radius), Vector3(radius * 2, height, radius * 2))); +} + +void GodotCapsuleShape3D::set_data(const Variant &p_data) { + Dictionary d = p_data; + ERR_FAIL_COND(!d.has("radius")); + ERR_FAIL_COND(!d.has("height")); + _setup(d["height"], d["radius"]); +} + +Variant GodotCapsuleShape3D::get_data() const { + Dictionary d; + d["radius"] = radius; + d["height"] = height; + return d; +} + +GodotCapsuleShape3D::GodotCapsuleShape3D() {} + +/********** CYLINDER *************/ + +void GodotCylinderShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + Vector3 cylinder_axis = p_transform.basis.get_axis(1).normalized(); + real_t axis_dot = cylinder_axis.dot(p_normal); + + Vector3 local_normal = p_transform.basis.xform_inv(p_normal); + real_t scale = local_normal.length(); + real_t scaled_radius = radius * scale; + real_t scaled_height = height * scale; + + real_t length; + if (Math::abs(axis_dot) > 1.0) { + length = scaled_height * 0.5; + } else { + length = Math::abs(axis_dot * scaled_height * 0.5) + scaled_radius * Math::sqrt(1.0 - axis_dot * axis_dot); + } + + real_t distance = p_normal.dot(p_transform.origin); + + r_min = distance - length; + r_max = distance + length; +} + +Vector3 GodotCylinderShape3D::get_support(const Vector3 &p_normal) const { + Vector3 n = p_normal; + real_t h = (n.y > 0) ? height : -height; + real_t s = Math::sqrt(n.x * n.x + n.z * n.z); + if (Math::is_zero_approx(s)) { + n.x = radius; + n.y = h * 0.5; + n.z = 0.0; + } else { + real_t d = radius / s; + n.x = n.x * d; + n.y = h * 0.5; + n.z = n.z * d; + } + + return n; +} + +void GodotCylinderShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + real_t d = p_normal.y; + if (Math::abs(d) > _CYLINDER_FACE_IS_VALID_SUPPORT_THRESHOLD) { + real_t h = (d > 0) ? height : -height; + + Vector3 n = p_normal; + n.x = 0.0; + n.z = 0.0; + n.y = h * 0.5; + + r_amount = 3; + r_type = FEATURE_CIRCLE; + r_supports[0] = n; + r_supports[1] = n; + r_supports[1].x += radius; + r_supports[2] = n; + r_supports[2].z += radius; + } else if (Math::abs(d) < _CYLINDER_EDGE_IS_VALID_SUPPORT_THRESHOLD) { + // make it flat + Vector3 n = p_normal; + n.y = 0.0; + n.normalize(); + n *= radius; + + r_amount = 2; + r_type = FEATURE_EDGE; + r_supports[0] = n; + r_supports[0].y += height * 0.5; + r_supports[1] = n; + r_supports[1].y -= height * 0.5; + } else { + r_amount = 1; + r_type = FEATURE_POINT; + r_supports[0] = get_support(p_normal); + return; + + Vector3 n = p_normal; + real_t h = n.y * Math::sqrt(0.25 * height * height + radius * radius); + if (Math::abs(h) > 1.0) { + // Top or bottom surface. + n.y = (n.y > 0.0) ? height * 0.5 : -height * 0.5; + } else { + // Lateral surface. + n.y = height * 0.5 * h; + } + + real_t s = Math::sqrt(n.x * n.x + n.z * n.z); + if (Math::is_zero_approx(s)) { + n.x = 0.0; + n.z = 0.0; + } else { + real_t scaled_radius = radius / s; + n.x = n.x * scaled_radius; + n.z = n.z * scaled_radius; + } + + r_supports[0] = n; + } +} + +bool GodotCylinderShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + return Geometry3D::segment_intersects_cylinder(p_begin, p_end, height, radius, &r_result, &r_normal, 1); +} + +bool GodotCylinderShape3D::intersect_point(const Vector3 &p_point) const { + if (Math::abs(p_point.y) < height * 0.5) { + return Vector3(p_point.x, 0, p_point.z).length() < radius; + } + return false; +} + +Vector3 GodotCylinderShape3D::get_closest_point_to(const Vector3 &p_point) const { + if (Math::absf(p_point.y) > height * 0.5) { + // Project point to top disk. + real_t dir = p_point.y > 0.0 ? 1.0 : -1.0; + Vector3 circle_pos(0.0, dir * height * 0.5, 0.0); + Plane circle_plane(Vector3(0.0, dir, 0.0), circle_pos); + Vector3 proj_point = circle_plane.project(p_point); + + // Clip position. + Vector3 delta_point_1 = proj_point - circle_pos; + real_t dist_point_1 = delta_point_1.length_squared(); + if (!Math::is_zero_approx(dist_point_1)) { + dist_point_1 = Math::sqrt(dist_point_1); + proj_point = circle_pos + delta_point_1 * MIN(dist_point_1, radius) / dist_point_1; + } + + return proj_point; + } else { + Vector3 s[2] = { + Vector3(0, -height * 0.5, 0), + Vector3(0, height * 0.5, 0), + }; + + Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, s); + + if (p.distance_to(p_point) < radius) { + return p_point; + } + + return p + (p_point - p).normalized() * radius; + } +} + +Vector3 GodotCylinderShape3D::get_moment_of_inertia(real_t p_mass) const { + // use bad AABB approximation + Vector3 extents = get_aabb().size * 0.5; + + return Vector3( + (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); +} + +void GodotCylinderShape3D::_setup(real_t p_height, real_t p_radius) { + height = p_height; + radius = p_radius; + configure(AABB(Vector3(-radius, -height * 0.5, -radius), Vector3(radius * 2.0, height, radius * 2.0))); +} + +void GodotCylinderShape3D::set_data(const Variant &p_data) { + Dictionary d = p_data; + ERR_FAIL_COND(!d.has("radius")); + ERR_FAIL_COND(!d.has("height")); + _setup(d["height"], d["radius"]); +} + +Variant GodotCylinderShape3D::get_data() const { + Dictionary d; + d["radius"] = radius; + d["height"] = height; + return d; +} + +GodotCylinderShape3D::GodotCylinderShape3D() {} + +/********** CONVEX POLYGON *************/ + +void GodotConvexPolygonShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + int vertex_count = mesh.vertices.size(); + if (vertex_count == 0) { + return; + } + + const Vector3 *vrts = &mesh.vertices[0]; + + for (int i = 0; i < vertex_count; i++) { + real_t d = p_normal.dot(p_transform.xform(vrts[i])); + + if (i == 0 || d > r_max) { + r_max = d; + } + if (i == 0 || d < r_min) { + r_min = d; + } + } +} + +Vector3 GodotConvexPolygonShape3D::get_support(const Vector3 &p_normal) const { + Vector3 n = p_normal; + + int vert_support_idx = -1; + real_t support_max = 0; + + int vertex_count = mesh.vertices.size(); + if (vertex_count == 0) { + return Vector3(); + } + + const Vector3 *vrts = &mesh.vertices[0]; + + for (int i = 0; i < vertex_count; i++) { + real_t d = n.dot(vrts[i]); + + if (i == 0 || d > support_max) { + support_max = d; + vert_support_idx = i; + } + } + + return vrts[vert_support_idx]; +} + +void GodotConvexPolygonShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); + int fc = mesh.faces.size(); + + const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); + int ec = mesh.edges.size(); + + const Vector3 *vertices = mesh.vertices.ptr(); + int vc = mesh.vertices.size(); + + r_amount = 0; + ERR_FAIL_COND_MSG(vc == 0, "Convex polygon shape has no vertices."); + + //find vertex first + real_t max = 0; + int vtx = 0; + + for (int i = 0; i < vc; i++) { + real_t d = p_normal.dot(vertices[i]); + + if (i == 0 || d > max) { + max = d; + vtx = i; + } + } + + for (int i = 0; i < fc; i++) { + if (faces[i].plane.normal.dot(p_normal) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { + int ic = faces[i].indices.size(); + const int *ind = faces[i].indices.ptr(); + + bool valid = false; + for (int j = 0; j < ic; j++) { + if (ind[j] == vtx) { + valid = true; + break; + } + } + + if (!valid) { + continue; + } + + int m = MIN(p_max, ic); + for (int j = 0; j < m; j++) { + r_supports[j] = vertices[ind[j]]; + } + r_amount = m; + r_type = FEATURE_FACE; + return; + } + } + + for (int i = 0; i < ec; i++) { + real_t dot = (vertices[edges[i].a] - vertices[edges[i].b]).normalized().dot(p_normal); + dot = ABS(dot); + if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD && (edges[i].a == vtx || edges[i].b == vtx)) { + r_amount = 2; + r_type = FEATURE_EDGE; + r_supports[0] = vertices[edges[i].a]; + r_supports[1] = vertices[edges[i].b]; + return; + } + } + + r_supports[0] = vertices[vtx]; + r_amount = 1; + r_type = FEATURE_POINT; +} + +bool GodotConvexPolygonShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); + int fc = mesh.faces.size(); + + const Vector3 *vertices = mesh.vertices.ptr(); + + Vector3 n = p_end - p_begin; + real_t min = 1e20; + bool col = false; + + for (int i = 0; i < fc; i++) { + if (faces[i].plane.normal.dot(n) > 0) { + continue; //opposing face + } + + int ic = faces[i].indices.size(); + const int *ind = faces[i].indices.ptr(); + + for (int j = 1; j < ic - 1; j++) { + Face3 f(vertices[ind[0]], vertices[ind[j]], vertices[ind[j + 1]]); + Vector3 result; + if (f.intersects_segment(p_begin, p_end, &result)) { + real_t d = n.dot(result); + if (d < min) { + min = d; + r_result = result; + r_normal = faces[i].plane.normal; + col = true; + } + + break; + } + } + } + + return col; +} + +bool GodotConvexPolygonShape3D::intersect_point(const Vector3 &p_point) const { + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); + int fc = mesh.faces.size(); + + for (int i = 0; i < fc; i++) { + if (faces[i].plane.distance_to(p_point) >= 0) { + return false; + } + } + + return true; +} + +Vector3 GodotConvexPolygonShape3D::get_closest_point_to(const Vector3 &p_point) const { + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); + int fc = mesh.faces.size(); + const Vector3 *vertices = mesh.vertices.ptr(); + + bool all_inside = true; + for (int i = 0; i < fc; i++) { + if (!faces[i].plane.is_point_over(p_point)) { + continue; + } + + all_inside = false; + bool is_inside = true; + int ic = faces[i].indices.size(); + const int *indices = faces[i].indices.ptr(); + + for (int j = 0; j < ic; j++) { + Vector3 a = vertices[indices[j]]; + Vector3 b = vertices[indices[(j + 1) % ic]]; + Vector3 n = (a - b).cross(faces[i].plane.normal).normalized(); + if (Plane(n, a).is_point_over(p_point)) { + is_inside = false; + break; + } + } + + if (is_inside) { + return faces[i].plane.project(p_point); + } + } + + if (all_inside) { + return p_point; + } + + real_t min_distance = 1e20; + Vector3 min_point; + + //check edges + const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); + int ec = mesh.edges.size(); + for (int i = 0; i < ec; i++) { + Vector3 s[2] = { + vertices[edges[i].a], + vertices[edges[i].b] + }; + + Vector3 closest = Geometry3D::get_closest_point_to_segment(p_point, s); + real_t d = closest.distance_to(p_point); + if (d < min_distance) { + min_distance = d; + min_point = closest; + } + } + + return min_point; +} + +Vector3 GodotConvexPolygonShape3D::get_moment_of_inertia(real_t p_mass) const { + // use bad AABB approximation + Vector3 extents = get_aabb().size * 0.5; + + return Vector3( + (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); +} + +void GodotConvexPolygonShape3D::_setup(const Vector &p_vertices) { + Error err = ConvexHullComputer::convex_hull(p_vertices, mesh); + if (err != OK) { + ERR_PRINT("Failed to build convex hull"); + } + + AABB _aabb; + + for (int i = 0; i < mesh.vertices.size(); i++) { + if (i == 0) { + _aabb.position = mesh.vertices[i]; + } else { + _aabb.expand_to(mesh.vertices[i]); + } + } + + configure(_aabb); +} + +void GodotConvexPolygonShape3D::set_data(const Variant &p_data) { + _setup(p_data); +} + +Variant GodotConvexPolygonShape3D::get_data() const { + return mesh.vertices; +} + +GodotConvexPolygonShape3D::GodotConvexPolygonShape3D() { +} + +/********** FACE POLYGON *************/ + +void GodotFaceShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + for (int i = 0; i < 3; i++) { + Vector3 v = p_transform.xform(vertex[i]); + real_t d = p_normal.dot(v); + + if (i == 0 || d > r_max) { + r_max = d; + } + + if (i == 0 || d < r_min) { + r_min = d; + } + } +} + +Vector3 GodotFaceShape3D::get_support(const Vector3 &p_normal) const { + int vert_support_idx = -1; + real_t support_max = 0; + + for (int i = 0; i < 3; i++) { + real_t d = p_normal.dot(vertex[i]); + + if (i == 0 || d > support_max) { + support_max = d; + vert_support_idx = i; + } + } + + return vertex[vert_support_idx]; +} + +void GodotFaceShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + Vector3 n = p_normal; + + /** TEST FACE AS SUPPORT **/ + if (Math::abs(normal.dot(n)) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { + r_amount = 3; + r_type = FEATURE_FACE; + for (int i = 0; i < 3; i++) { + r_supports[i] = vertex[i]; + } + return; + } + + /** FIND SUPPORT VERTEX **/ + + int vert_support_idx = -1; + real_t support_max = 0; + + for (int i = 0; i < 3; i++) { + real_t d = n.dot(vertex[i]); + + if (i == 0 || d > support_max) { + support_max = d; + vert_support_idx = i; + } + } + + /** TEST EDGES AS SUPPORT **/ + + for (int i = 0; i < 3; i++) { + int nx = (i + 1) % 3; + if (i != vert_support_idx && nx != vert_support_idx) { + continue; + } + + // check if edge is valid as a support + real_t dot = (vertex[i] - vertex[nx]).normalized().dot(n); + dot = ABS(dot); + if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { + r_amount = 2; + r_type = FEATURE_EDGE; + r_supports[0] = vertex[i]; + r_supports[1] = vertex[nx]; + return; + } + } + + r_amount = 1; + r_type = FEATURE_POINT; + r_supports[0] = vertex[vert_support_idx]; +} + +bool GodotFaceShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + bool c = Geometry3D::segment_intersects_triangle(p_begin, p_end, vertex[0], vertex[1], vertex[2], &r_result); + if (c) { + r_normal = Plane(vertex[0], vertex[1], vertex[2]).normal; + if (r_normal.dot(p_end - p_begin) > 0) { + if (backface_collision) { + r_normal = -r_normal; + } else { + c = false; + } + } + } + + return c; +} + +bool GodotFaceShape3D::intersect_point(const Vector3 &p_point) const { + return false; //face is flat +} + +Vector3 GodotFaceShape3D::get_closest_point_to(const Vector3 &p_point) const { + return Face3(vertex[0], vertex[1], vertex[2]).get_closest_point_to(p_point); +} + +Vector3 GodotFaceShape3D::get_moment_of_inertia(real_t p_mass) const { + return Vector3(); // Sorry, but i don't think anyone cares, FaceShape! +} + +GodotFaceShape3D::GodotFaceShape3D() { + configure(AABB()); +} + +Vector GodotConcavePolygonShape3D::get_faces() const { + Vector rfaces; + rfaces.resize(faces.size() * 3); + + for (int i = 0; i < faces.size(); i++) { + Face f = faces.get(i); + + for (int j = 0; j < 3; j++) { + rfaces.set(i * 3 + j, vertices.get(f.indices[j])); + } + } + + return rfaces; +} + +void GodotConcavePolygonShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + int count = vertices.size(); + if (count == 0) { + r_min = 0; + r_max = 0; + return; + } + const Vector3 *vptr = vertices.ptr(); + + for (int i = 0; i < count; i++) { + real_t d = p_normal.dot(p_transform.xform(vptr[i])); + + if (i == 0 || d > r_max) { + r_max = d; + } + if (i == 0 || d < r_min) { + r_min = d; + } + } +} + +Vector3 GodotConcavePolygonShape3D::get_support(const Vector3 &p_normal) const { + int count = vertices.size(); + if (count == 0) { + return Vector3(); + } + + const Vector3 *vptr = vertices.ptr(); + + Vector3 n = p_normal; + + int vert_support_idx = -1; + real_t support_max = 0; + + for (int i = 0; i < count; i++) { + real_t d = n.dot(vptr[i]); + + if (i == 0 || d > support_max) { + support_max = d; + vert_support_idx = i; + } + } + + return vptr[vert_support_idx]; +} + +void GodotConcavePolygonShape3D::_cull_segment(int p_idx, _SegmentCullParams *p_params) const { + const BVH *bvh = &p_params->bvh[p_idx]; + + /* + if (p_params->dir.dot(bvh->aabb.get_support(-p_params->dir))>p_params->min_d) + return; //test against whole AABB, which isn't very costly + */ + + //printf("addr: %p\n",bvh); + if (!bvh->aabb.intersects_segment(p_params->from, p_params->to)) { + return; + } + + if (bvh->face_index >= 0) { + const Face *f = &p_params->faces[bvh->face_index]; + GodotFaceShape3D *face = p_params->face; + face->normal = f->normal; + face->vertex[0] = p_params->vertices[f->indices[0]]; + face->vertex[1] = p_params->vertices[f->indices[1]]; + face->vertex[2] = p_params->vertices[f->indices[2]]; + + Vector3 res; + Vector3 normal; + if (face->intersect_segment(p_params->from, p_params->to, res, normal)) { + real_t d = p_params->dir.dot(res) - p_params->dir.dot(p_params->from); + if ((d > 0) && (d < p_params->min_d)) { + p_params->min_d = d; + p_params->result = res; + p_params->normal = normal; + p_params->collisions++; + } + } + } else { + if (bvh->left >= 0) { + _cull_segment(bvh->left, p_params); + } + if (bvh->right >= 0) { + _cull_segment(bvh->right, p_params); + } + } +} + +bool GodotConcavePolygonShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + if (faces.size() == 0) { + return false; + } + + // unlock data + const Face *fr = faces.ptr(); + const Vector3 *vr = vertices.ptr(); + const BVH *br = bvh.ptr(); + + GodotFaceShape3D face; + face.backface_collision = backface_collision; + + _SegmentCullParams params; + params.from = p_begin; + params.to = p_end; + params.dir = (p_end - p_begin).normalized(); + + params.faces = fr; + params.vertices = vr; + params.bvh = br; + + params.face = &face; + + // cull + _cull_segment(0, ¶ms); + + if (params.collisions > 0) { + r_result = params.result; + r_normal = params.normal; + return true; + } else { + return false; + } +} + +bool GodotConcavePolygonShape3D::intersect_point(const Vector3 &p_point) const { + return false; //face is flat +} + +Vector3 GodotConcavePolygonShape3D::get_closest_point_to(const Vector3 &p_point) const { + return Vector3(); +} + +bool GodotConcavePolygonShape3D::_cull(int p_idx, _CullParams *p_params) const { + const BVH *bvh = &p_params->bvh[p_idx]; + + if (!p_params->aabb.intersects(bvh->aabb)) { + return false; + } + + if (bvh->face_index >= 0) { + const Face *f = &p_params->faces[bvh->face_index]; + GodotFaceShape3D *face = p_params->face; + face->normal = f->normal; + face->vertex[0] = p_params->vertices[f->indices[0]]; + face->vertex[1] = p_params->vertices[f->indices[1]]; + face->vertex[2] = p_params->vertices[f->indices[2]]; + if (p_params->callback(p_params->userdata, face)) { + return true; + } + } else { + if (bvh->left >= 0) { + if (_cull(bvh->left, p_params)) { + return true; + } + } + + if (bvh->right >= 0) { + if (_cull(bvh->right, p_params)) { + return true; + } + } + } + + return false; +} + +void GodotConcavePolygonShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const { + // make matrix local to concave + if (faces.size() == 0) { + return; + } + + AABB local_aabb = p_local_aabb; + + // unlock data + const Face *fr = faces.ptr(); + const Vector3 *vr = vertices.ptr(); + const BVH *br = bvh.ptr(); + + GodotFaceShape3D face; // use this to send in the callback + face.backface_collision = backface_collision; + + _CullParams params; + params.aabb = local_aabb; + params.face = &face; + params.faces = fr; + params.vertices = vr; + params.bvh = br; + params.callback = p_callback; + params.userdata = p_userdata; + + // cull + _cull(0, ¶ms); +} + +Vector3 GodotConcavePolygonShape3D::get_moment_of_inertia(real_t p_mass) const { + // use bad AABB approximation + Vector3 extents = get_aabb().size * 0.5; + + return Vector3( + (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); +} + +struct _Volume_BVH_Element { + AABB aabb; + Vector3 center; + int face_index; +}; + +struct _Volume_BVH_CompareX { + _FORCE_INLINE_ bool operator()(const _Volume_BVH_Element &a, const _Volume_BVH_Element &b) const { + return a.center.x < b.center.x; + } +}; + +struct _Volume_BVH_CompareY { + _FORCE_INLINE_ bool operator()(const _Volume_BVH_Element &a, const _Volume_BVH_Element &b) const { + return a.center.y < b.center.y; + } +}; + +struct _Volume_BVH_CompareZ { + _FORCE_INLINE_ bool operator()(const _Volume_BVH_Element &a, const _Volume_BVH_Element &b) const { + return a.center.z < b.center.z; + } +}; + +struct _Volume_BVH { + AABB aabb; + _Volume_BVH *left; + _Volume_BVH *right; + + int face_index; +}; + +_Volume_BVH *_volume_build_bvh(_Volume_BVH_Element *p_elements, int p_size, int &count) { + _Volume_BVH *bvh = memnew(_Volume_BVH); + + if (p_size == 1) { + //leaf + bvh->aabb = p_elements[0].aabb; + bvh->left = nullptr; + bvh->right = nullptr; + bvh->face_index = p_elements->face_index; + count++; + return bvh; + } else { + bvh->face_index = -1; + } + + AABB aabb; + for (int i = 0; i < p_size; i++) { + if (i == 0) { + aabb = p_elements[i].aabb; + } else { + aabb.merge_with(p_elements[i].aabb); + } + } + bvh->aabb = aabb; + switch (aabb.get_longest_axis_index()) { + case 0: { + SortArray<_Volume_BVH_Element, _Volume_BVH_CompareX> sort_x; + sort_x.sort(p_elements, p_size); + + } break; + case 1: { + SortArray<_Volume_BVH_Element, _Volume_BVH_CompareY> sort_y; + sort_y.sort(p_elements, p_size); + } break; + case 2: { + SortArray<_Volume_BVH_Element, _Volume_BVH_CompareZ> sort_z; + sort_z.sort(p_elements, p_size); + } break; + } + + int split = p_size / 2; + bvh->left = _volume_build_bvh(p_elements, split, count); + bvh->right = _volume_build_bvh(&p_elements[split], p_size - split, count); + + //printf("branch at %p - %i: %i\n",bvh,count,bvh->face_index); + count++; + return bvh; +} + +void GodotConcavePolygonShape3D::_fill_bvh(_Volume_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx) { + int idx = p_idx; + + p_bvh_array[idx].aabb = p_bvh_tree->aabb; + p_bvh_array[idx].face_index = p_bvh_tree->face_index; + //printf("%p - %i: %i(%p) -- %p:%p\n",%p_bvh_array[idx],p_idx,p_bvh_array[i]->face_index,&p_bvh_tree->face_index,p_bvh_tree->left,p_bvh_tree->right); + + if (p_bvh_tree->left) { + p_bvh_array[idx].left = ++p_idx; + _fill_bvh(p_bvh_tree->left, p_bvh_array, p_idx); + + } else { + p_bvh_array[p_idx].left = -1; + } + + if (p_bvh_tree->right) { + p_bvh_array[idx].right = ++p_idx; + _fill_bvh(p_bvh_tree->right, p_bvh_array, p_idx); + + } else { + p_bvh_array[p_idx].right = -1; + } + + memdelete(p_bvh_tree); +} + +void GodotConcavePolygonShape3D::_setup(const Vector &p_faces, bool p_backface_collision) { + int src_face_count = p_faces.size(); + if (src_face_count == 0) { + configure(AABB()); + return; + } + ERR_FAIL_COND(src_face_count % 3); + src_face_count /= 3; + + const Vector3 *facesr = p_faces.ptr(); + + Vector<_Volume_BVH_Element> bvh_array; + bvh_array.resize(src_face_count); + + _Volume_BVH_Element *bvh_arrayw = bvh_array.ptrw(); + + faces.resize(src_face_count); + Face *facesw = faces.ptrw(); + + vertices.resize(src_face_count * 3); + + Vector3 *verticesw = vertices.ptrw(); + + AABB _aabb; + + for (int i = 0; i < src_face_count; i++) { + Face3 face(facesr[i * 3 + 0], facesr[i * 3 + 1], facesr[i * 3 + 2]); + + bvh_arrayw[i].aabb = face.get_aabb(); + bvh_arrayw[i].center = bvh_arrayw[i].aabb.get_center(); + bvh_arrayw[i].face_index = i; + facesw[i].indices[0] = i * 3 + 0; + facesw[i].indices[1] = i * 3 + 1; + facesw[i].indices[2] = i * 3 + 2; + facesw[i].normal = face.get_plane().normal; + verticesw[i * 3 + 0] = face.vertex[0]; + verticesw[i * 3 + 1] = face.vertex[1]; + verticesw[i * 3 + 2] = face.vertex[2]; + if (i == 0) { + _aabb = bvh_arrayw[i].aabb; + } else { + _aabb.merge_with(bvh_arrayw[i].aabb); + } + } + + int count = 0; + _Volume_BVH *bvh_tree = _volume_build_bvh(bvh_arrayw, src_face_count, count); + + bvh.resize(count + 1); + + BVH *bvh_arrayw2 = bvh.ptrw(); + + int idx = 0; + _fill_bvh(bvh_tree, bvh_arrayw2, idx); + + backface_collision = p_backface_collision; + + configure(_aabb); // this type of shape has no margin +} + +void GodotConcavePolygonShape3D::set_data(const Variant &p_data) { + Dictionary d = p_data; + ERR_FAIL_COND(!d.has("faces")); + + _setup(d["faces"], d["backface_collision"]); +} + +Variant GodotConcavePolygonShape3D::get_data() const { + Dictionary d; + d["faces"] = get_faces(); + d["backface_collision"] = backface_collision; + + return d; +} + +GodotConcavePolygonShape3D::GodotConcavePolygonShape3D() { +} + +/* HEIGHT MAP SHAPE */ + +Vector GodotHeightMapShape3D::get_heights() const { + return heights; +} + +int GodotHeightMapShape3D::get_width() const { + return width; +} + +int GodotHeightMapShape3D::get_depth() const { + return depth; +} + +void GodotHeightMapShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + //not very useful, but not very used either + p_transform.xform(get_aabb()).project_range_in_plane(Plane(p_normal), r_min, r_max); +} + +Vector3 GodotHeightMapShape3D::get_support(const Vector3 &p_normal) const { + //not very useful, but not very used either + return get_aabb().get_support(p_normal); +} + +struct _HeightmapSegmentCullParams { + Vector3 from; + Vector3 to; + Vector3 dir; + + Vector3 result; + Vector3 normal; + + const GodotHeightMapShape3D *heightmap = nullptr; + GodotFaceShape3D *face = nullptr; +}; + +struct _HeightmapGridCullState { + real_t length = 0.0; + real_t length_flat = 0.0; + + real_t dist = 0.0; + real_t prev_dist = 0.0; + + int x = 0; + int z = 0; +}; + +_FORCE_INLINE_ bool _heightmap_face_cull_segment(_HeightmapSegmentCullParams &p_params) { + Vector3 res; + Vector3 normal; + if (p_params.face->intersect_segment(p_params.from, p_params.to, res, normal)) { + p_params.result = res; + p_params.normal = normal; + return true; + } + + return false; +} + +_FORCE_INLINE_ bool _heightmap_cell_cull_segment(_HeightmapSegmentCullParams &p_params, const _HeightmapGridCullState &p_state) { + // First triangle. + p_params.heightmap->_get_point(p_state.x, p_state.z, p_params.face->vertex[0]); + p_params.heightmap->_get_point(p_state.x + 1, p_state.z, p_params.face->vertex[1]); + p_params.heightmap->_get_point(p_state.x, p_state.z + 1, p_params.face->vertex[2]); + p_params.face->normal = Plane(p_params.face->vertex[0], p_params.face->vertex[1], p_params.face->vertex[2]).normal; + if (_heightmap_face_cull_segment(p_params)) { + return true; + } + + // Second triangle. + p_params.face->vertex[0] = p_params.face->vertex[1]; + p_params.heightmap->_get_point(p_state.x + 1, p_state.z + 1, p_params.face->vertex[1]); + p_params.face->normal = Plane(p_params.face->vertex[0], p_params.face->vertex[1], p_params.face->vertex[2]).normal; + if (_heightmap_face_cull_segment(p_params)) { + return true; + } + + return false; +} + +_FORCE_INLINE_ bool _heightmap_chunk_cull_segment(_HeightmapSegmentCullParams &p_params, const _HeightmapGridCullState &p_state) { + const GodotHeightMapShape3D::Range &chunk = p_params.heightmap->_get_bounds_chunk(p_state.x, p_state.z); + + Vector3 enter_pos; + Vector3 exit_pos; + + if (p_state.length_flat > CMP_EPSILON) { + real_t flat_to_3d = p_state.length / p_state.length_flat; + real_t enter_param = p_state.prev_dist * flat_to_3d; + real_t exit_param = p_state.dist * flat_to_3d; + enter_pos = p_params.from + p_params.dir * enter_param; + exit_pos = p_params.from + p_params.dir * exit_param; + } else { + // Consider the ray vertical. + // (though we shouldn't reach this often because there is an early check up-front) + enter_pos = p_params.from; + exit_pos = p_params.to; + } + + // Transform positions to heightmap space. + enter_pos *= GodotHeightMapShape3D::BOUNDS_CHUNK_SIZE; + exit_pos *= GodotHeightMapShape3D::BOUNDS_CHUNK_SIZE; + + // We did enter the flat projection of the AABB, + // but we have to check if we intersect it on the vertical axis. + if ((enter_pos.y > chunk.max) && (exit_pos.y > chunk.max)) { + return false; + } + if ((enter_pos.y < chunk.min) && (exit_pos.y < chunk.min)) { + return false; + } + + return p_params.heightmap->_intersect_grid_segment(_heightmap_cell_cull_segment, enter_pos, exit_pos, p_params.heightmap->width, p_params.heightmap->depth, p_params.heightmap->local_origin, p_params.result, p_params.normal); +} + +template +bool GodotHeightMapShape3D::_intersect_grid_segment(ProcessFunction &p_process, const Vector3 &p_begin, const Vector3 &p_end, int p_width, int p_depth, const Vector3 &offset, Vector3 &r_point, Vector3 &r_normal) const { + Vector3 delta = (p_end - p_begin); + real_t length = delta.length(); + + if (length < CMP_EPSILON) { + return false; + } + + Vector3 local_begin = p_begin + offset; + + GodotFaceShape3D face; + face.backface_collision = false; + + _HeightmapSegmentCullParams params; + params.from = p_begin; + params.to = p_end; + params.dir = delta / length; + params.heightmap = this; + params.face = &face; + + _HeightmapGridCullState state; + + // Perform grid query from projected ray. + Vector2 ray_dir_flat(delta.x, delta.z); + state.length = length; + state.length_flat = ray_dir_flat.length(); + + if (state.length_flat < CMP_EPSILON) { + ray_dir_flat = Vector2(); + } else { + ray_dir_flat /= state.length_flat; + } + + const int x_step = (ray_dir_flat.x > CMP_EPSILON) ? 1 : ((ray_dir_flat.x < -CMP_EPSILON) ? -1 : 0); + const int z_step = (ray_dir_flat.y > CMP_EPSILON) ? 1 : ((ray_dir_flat.y < -CMP_EPSILON) ? -1 : 0); + + const real_t infinite = 1e20; + const real_t delta_x = (x_step != 0) ? 1.f / Math::abs(ray_dir_flat.x) : infinite; + const real_t delta_z = (z_step != 0) ? 1.f / Math::abs(ray_dir_flat.y) : infinite; + + real_t cross_x; // At which value of `param` we will cross a x-axis lane? + real_t cross_z; // At which value of `param` we will cross a z-axis lane? + + // X initialization. + if (x_step != 0) { + if (x_step == 1) { + cross_x = (Math::ceil(local_begin.x) - local_begin.x) * delta_x; + } else { + cross_x = (local_begin.x - Math::floor(local_begin.x)) * delta_x; + } + } else { + cross_x = infinite; // Will never cross on X. + } + + // Z initialization. + if (z_step != 0) { + if (z_step == 1) { + cross_z = (Math::ceil(local_begin.z) - local_begin.z) * delta_z; + } else { + cross_z = (local_begin.z - Math::floor(local_begin.z)) * delta_z; + } + } else { + cross_z = infinite; // Will never cross on Z. + } + + int x = Math::floor(local_begin.x); + int z = Math::floor(local_begin.z); + + // Workaround cases where the ray starts at an integer position. + if (Math::is_zero_approx(cross_x)) { + cross_x += delta_x; + // If going backwards, we should ignore the position we would get by the above flooring, + // because the ray is not heading in that direction. + if (x_step == -1) { + x -= 1; + } + } + + if (Math::is_zero_approx(cross_z)) { + cross_z += delta_z; + if (z_step == -1) { + z -= 1; + } + } + + // Start inside the grid. + int x_start = MAX(MIN(x, p_width - 2), 0); + int z_start = MAX(MIN(z, p_depth - 2), 0); + + // Adjust initial cross values. + cross_x += delta_x * x_step * (x_start - x); + cross_z += delta_z * z_step * (z_start - z); + + x = x_start; + z = z_start; + + while (true) { + state.prev_dist = state.dist; + state.x = x; + state.z = z; + + if (cross_x < cross_z) { + // X lane. + x += x_step; + // Assign before advancing the param, + // to be in sync with the initialization step. + state.dist = cross_x; + cross_x += delta_x; + } else { + // Z lane. + z += z_step; + state.dist = cross_z; + cross_z += delta_z; + } + + if (state.dist > state.length_flat) { + state.dist = state.length_flat; + if (p_process(params, state)) { + r_point = params.result; + r_normal = params.normal; + return true; + } + break; + } + + if (p_process(params, state)) { + r_point = params.result; + r_normal = params.normal; + return true; + } + + // Stop when outside the grid. + if ((x < 0) || (z < 0) || (x >= p_width - 1) || (z >= p_depth - 1)) { + break; + } + } + + return false; +} + +bool GodotHeightMapShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const { + if (heights.is_empty()) { + return false; + } + + Vector3 local_begin = p_begin + local_origin; + Vector3 local_end = p_end + local_origin; + + // Quantize the ray begin/end. + int begin_x = Math::floor(local_begin.x); + int begin_z = Math::floor(local_begin.z); + int end_x = Math::floor(local_end.x); + int end_z = Math::floor(local_end.z); + + if ((begin_x == end_x) && (begin_z == end_z)) { + // Simple case for rays that don't traverse the grid horizontally. + // Just perform a test on the given cell. + GodotFaceShape3D face; + face.backface_collision = false; + + _HeightmapSegmentCullParams params; + params.from = p_begin; + params.to = p_end; + params.dir = (p_end - p_begin).normalized(); + + params.heightmap = this; + params.face = &face; + + _HeightmapGridCullState state; + state.x = MAX(MIN(begin_x, width - 2), 0); + state.z = MAX(MIN(begin_z, depth - 2), 0); + if (_heightmap_cell_cull_segment(params, state)) { + r_point = params.result; + r_normal = params.normal; + return true; + } + } else if (bounds_grid.is_empty()) { + // Process all cells intersecting the flat projection of the ray. + return _intersect_grid_segment(_heightmap_cell_cull_segment, p_begin, p_end, width, depth, local_origin, r_point, r_normal); + } else { + Vector3 ray_diff = (p_end - p_begin); + real_t length_flat_sqr = ray_diff.x * ray_diff.x + ray_diff.z * ray_diff.z; + if (length_flat_sqr < BOUNDS_CHUNK_SIZE * BOUNDS_CHUNK_SIZE) { + // Don't use chunks, the ray is too short in the plane. + return _intersect_grid_segment(_heightmap_cell_cull_segment, p_begin, p_end, width, depth, local_origin, r_point, r_normal); + } else { + // The ray is long, run raycast on a higher-level grid. + Vector3 bounds_from = p_begin / BOUNDS_CHUNK_SIZE; + Vector3 bounds_to = p_end / BOUNDS_CHUNK_SIZE; + Vector3 bounds_offset = local_origin / BOUNDS_CHUNK_SIZE; + return _intersect_grid_segment(_heightmap_chunk_cull_segment, bounds_from, bounds_to, bounds_grid_width, bounds_grid_depth, bounds_offset, r_point, r_normal); + } + } + + return false; +} + +bool GodotHeightMapShape3D::intersect_point(const Vector3 &p_point) const { + return false; +} + +Vector3 GodotHeightMapShape3D::get_closest_point_to(const Vector3 &p_point) const { + return Vector3(); +} + +void GodotHeightMapShape3D::_get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const { + const AABB &aabb = get_aabb(); + + Vector3 pos_local = aabb.position + local_origin; + + Vector3 clamped_point(p_point); + clamped_point.x = CLAMP(p_point.x, pos_local.x, pos_local.x + aabb.size.x); + clamped_point.y = CLAMP(p_point.y, pos_local.y, pos_local.y + aabb.size.y); + clamped_point.z = CLAMP(p_point.z, pos_local.z, pos_local.x + aabb.size.z); + + r_x = (clamped_point.x < 0.0) ? (clamped_point.x - 0.5) : (clamped_point.x + 0.5); + r_y = (clamped_point.y < 0.0) ? (clamped_point.y - 0.5) : (clamped_point.y + 0.5); + r_z = (clamped_point.z < 0.0) ? (clamped_point.z - 0.5) : (clamped_point.z + 0.5); +} + +void GodotHeightMapShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const { + if (heights.is_empty()) { + return; + } + + AABB local_aabb = p_local_aabb; + local_aabb.position += local_origin; + + // Quantize the aabb, and adjust the start/end ranges. + int aabb_min[3]; + int aabb_max[3]; + _get_cell(local_aabb.position, aabb_min[0], aabb_min[1], aabb_min[2]); + _get_cell(local_aabb.position + local_aabb.size, aabb_max[0], aabb_max[1], aabb_max[2]); + + // Expand the min/max quantized values. + // This is to catch the case where the input aabb falls between grid points. + for (int i = 0; i < 3; ++i) { + aabb_min[i]--; + aabb_max[i]++; + } + + int start_x = MAX(0, aabb_min[0]); + int end_x = MIN(width - 1, aabb_max[0]); + int start_z = MAX(0, aabb_min[2]); + int end_z = MIN(depth - 1, aabb_max[2]); + + GodotFaceShape3D face; + face.backface_collision = true; + + for (int z = start_z; z < end_z; z++) { + for (int x = start_x; x < end_x; x++) { + // First triangle. + _get_point(x, z, face.vertex[0]); + _get_point(x + 1, z, face.vertex[1]); + _get_point(x, z + 1, face.vertex[2]); + face.normal = Plane(face.vertex[0], face.vertex[1], face.vertex[2]).normal; + if (p_callback(p_userdata, &face)) { + return; + } + + // Second triangle. + face.vertex[0] = face.vertex[1]; + _get_point(x + 1, z + 1, face.vertex[1]); + face.normal = Plane(face.vertex[0], face.vertex[1], face.vertex[2]).normal; + if (p_callback(p_userdata, &face)) { + return; + } + } + } +} + +Vector3 GodotHeightMapShape3D::get_moment_of_inertia(real_t p_mass) const { + // use bad AABB approximation + Vector3 extents = get_aabb().size * 0.5; + + return Vector3( + (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); +} + +void GodotHeightMapShape3D::_build_accelerator() { + bounds_grid.clear(); + + bounds_grid_width = width / BOUNDS_CHUNK_SIZE; + bounds_grid_depth = depth / BOUNDS_CHUNK_SIZE; + + if (width % BOUNDS_CHUNK_SIZE > 0) { + ++bounds_grid_width; // In case terrain size isn't dividable by chunk size. + } + + if (depth % BOUNDS_CHUNK_SIZE > 0) { + ++bounds_grid_depth; + } + + uint32_t bound_grid_size = (uint32_t)(bounds_grid_width * bounds_grid_depth); + + if (bound_grid_size < 2) { + // Grid is empty or just one chunk. + return; + } + + bounds_grid.resize(bound_grid_size); + + // Compute min and max height for all chunks. + for (int cz = 0; cz < bounds_grid_depth; ++cz) { + int z0 = cz * BOUNDS_CHUNK_SIZE; + + for (int cx = 0; cx < bounds_grid_width; ++cx) { + int x0 = cx * BOUNDS_CHUNK_SIZE; + + Range r; + + r.min = _get_height(x0, z0); + r.max = r.min; + + // Compute min and max height for this chunk. + // We have to include one extra cell to account for neighbors. + // Here is why: + // Say we have a flat terrain, and a plateau that fits a chunk perfectly. + // + // Left Right + // 0---0---0---1---1---1 + // | | | | | | + // 0---0---0---1---1---1 + // | | | | | | + // 0---0---0---1---1---1 + // x + // + // If the AABB for the Left chunk did not share vertices with the Right, + // then we would fail collision tests at x due to a gap. + // + int z_max = MIN(z0 + BOUNDS_CHUNK_SIZE + 1, depth); + int x_max = MIN(x0 + BOUNDS_CHUNK_SIZE + 1, width); + for (int z = z0; z < z_max; ++z) { + for (int x = x0; x < x_max; ++x) { + real_t height = _get_height(x, z); + if (height < r.min) { + r.min = height; + } else if (height > r.max) { + r.max = height; + } + } + } + + bounds_grid[cx + cz * bounds_grid_width] = r; + } + } +} + +void GodotHeightMapShape3D::_setup(const Vector &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height) { + heights = p_heights; + width = p_width; + depth = p_depth; + + // Initialize aabb. + AABB aabb; + aabb.position = Vector3(0.0, p_min_height, 0.0); + aabb.size = Vector3(p_width - 1, p_max_height - p_min_height, p_depth - 1); + + // Initialize origin as the aabb center. + local_origin = aabb.position + 0.5 * aabb.size; + local_origin.y = 0.0; + + aabb.position -= local_origin; + + _build_accelerator(); + + configure(aabb); +} + +void GodotHeightMapShape3D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY); + + Dictionary d = p_data; + ERR_FAIL_COND(!d.has("width")); + ERR_FAIL_COND(!d.has("depth")); + ERR_FAIL_COND(!d.has("heights")); + + int width = d["width"]; + int depth = d["depth"]; + + ERR_FAIL_COND(width <= 0.0); + ERR_FAIL_COND(depth <= 0.0); + + Variant heights_variant = d["heights"]; + Vector heights_buffer; +#ifdef REAL_T_IS_DOUBLE + if (heights_variant.get_type() == Variant::PACKED_FLOAT64_ARRAY) { +#else + if (heights_variant.get_type() == Variant::PACKED_FLOAT32_ARRAY) { +#endif + // Ready-to-use heights can be passed. + heights_buffer = heights_variant; + } else if (heights_variant.get_type() == Variant::OBJECT) { + // If an image is passed, we have to convert it. + // This would be expensive to do with a script, so it's nice to have it here. + Ref image = heights_variant; + ERR_FAIL_COND(image.is_null()); + ERR_FAIL_COND(image->get_format() != Image::FORMAT_RF); + + PackedByteArray im_data = image->get_data(); + heights_buffer.resize(image->get_width() * image->get_height()); + + real_t *w = heights_buffer.ptrw(); + real_t *rp = (real_t *)im_data.ptr(); + for (int i = 0; i < heights_buffer.size(); ++i) { + w[i] = rp[i]; + } + } else { +#ifdef REAL_T_IS_DOUBLE + ERR_FAIL_MSG("Expected PackedFloat64Array or float Image."); +#else + ERR_FAIL_MSG("Expected PackedFloat32Array or float Image."); +#endif + } + + // Compute min and max heights or use precomputed values. + real_t min_height = 0.0; + real_t max_height = 0.0; + if (d.has("min_height") && d.has("max_height")) { + min_height = d["min_height"]; + max_height = d["max_height"]; + } else { + int heights_size = heights.size(); + for (int i = 0; i < heights_size; ++i) { + real_t h = heights[i]; + if (h < min_height) { + min_height = h; + } else if (h > max_height) { + max_height = h; + } + } + } + + ERR_FAIL_COND(min_height > max_height); + + ERR_FAIL_COND(heights_buffer.size() != (width * depth)); + + // If specified, min and max height will be used as precomputed values. + _setup(heights_buffer, width, depth, min_height, max_height); +} + +Variant GodotHeightMapShape3D::get_data() const { + Dictionary d; + d["width"] = width; + d["depth"] = depth; + + const AABB &aabb = get_aabb(); + d["min_height"] = aabb.position.y; + d["max_height"] = aabb.position.y + aabb.size.y; + + d["heights"] = heights; + + return d; +} + +GodotHeightMapShape3D::GodotHeightMapShape3D() { +} diff --git a/servers/physics_3d/godot_shape_3d.h b/servers/physics_3d/godot_shape_3d.h new file mode 100644 index 0000000000..8822d9487b --- /dev/null +++ b/servers/physics_3d/godot_shape_3d.h @@ -0,0 +1,510 @@ +/*************************************************************************/ +/* godot_shape_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_SHAPE_3D_H +#define GODOT_SHAPE_3D_H + +#include "core/math/geometry_3d.h" +#include "core/templates/local_vector.h" +#include "servers/physics_server_3d.h" + +class GodotShape3D; + +class GodotShapeOwner3D { +public: + virtual void _shape_changed() = 0; + virtual void remove_shape(GodotShape3D *p_shape) = 0; + + virtual ~GodotShapeOwner3D() {} +}; + +class GodotShape3D { + RID self; + AABB aabb; + bool configured = false; + real_t custom_bias = 0.0; + + Map owners; + +protected: + void configure(const AABB &p_aabb); + +public: + enum FeatureType { + FEATURE_POINT, + FEATURE_EDGE, + FEATURE_FACE, + FEATURE_CIRCLE, + }; + + virtual real_t get_area() const { return aabb.get_area(); } + + _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + virtual PhysicsServer3D::ShapeType get_type() const = 0; + + _FORCE_INLINE_ const AABB &get_aabb() const { return aabb; } + _FORCE_INLINE_ bool is_configured() const { return configured; } + + virtual bool is_concave() const { return false; } + + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const = 0; + virtual Vector3 get_support(const Vector3 &p_normal) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const = 0; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const = 0; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const = 0; + virtual bool intersect_point(const Vector3 &p_point) const = 0; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const = 0; + + virtual void set_data(const Variant &p_data) = 0; + virtual Variant get_data() const = 0; + + _FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias = p_bias; } + _FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; } + + void add_owner(GodotShapeOwner3D *p_owner); + void remove_owner(GodotShapeOwner3D *p_owner); + bool is_owner(GodotShapeOwner3D *p_owner) const; + const Map &get_owners() const; + + GodotShape3D() {} + virtual ~GodotShape3D(); +}; + +class GodotConcaveShape3D : public GodotShape3D { +public: + virtual bool is_concave() const override { return true; } + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; } + + // Returns true to stop the query. + typedef bool (*QueryCallback)(void *p_userdata, GodotShape3D *p_convex); + + virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0; + + GodotConcaveShape3D() {} +}; + +class GodotWorldBoundaryShape3D : public GodotShape3D { + Plane plane; + + void _setup(const Plane &p_plane); + +public: + Plane get_plane() const; + + virtual real_t get_area() const override { return INFINITY; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_WORLD_BOUNDARY; } + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; } + + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + GodotWorldBoundaryShape3D(); +}; + +class GodotSeparationRayShape3D : public GodotShape3D { + real_t length = 1.0; + bool slide_on_slope = false; + + void _setup(real_t p_length, bool p_slide_on_slope); + +public: + real_t get_length() const; + bool get_slide_on_slope() const; + + virtual real_t get_area() const override { return 0.0; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_SEPARATION_RAY; } + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + GodotSeparationRayShape3D(); +}; + +class GodotSphereShape3D : public GodotShape3D { + real_t radius = 0.0; + + void _setup(real_t p_radius); + +public: + real_t get_radius() const; + + virtual real_t get_area() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius; } + + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_SPHERE; } + + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + GodotSphereShape3D(); +}; + +class GodotBoxShape3D : public GodotShape3D { + Vector3 half_extents; + void _setup(const Vector3 &p_half_extents); + +public: + _FORCE_INLINE_ Vector3 get_half_extents() const { return half_extents; } + virtual real_t get_area() const override { return 8 * half_extents.x * half_extents.y * half_extents.z; } + + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_BOX; } + + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + GodotBoxShape3D(); +}; + +class GodotCapsuleShape3D : public GodotShape3D { + real_t height = 0.0; + real_t radius = 0.0; + + void _setup(real_t p_height, real_t p_radius); + +public: + _FORCE_INLINE_ real_t get_height() const { return height; } + _FORCE_INLINE_ real_t get_radius() const { return radius; } + + virtual real_t get_area() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius + (height - radius * 2.0) * Math_PI * radius * radius; } + + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CAPSULE; } + + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + GodotCapsuleShape3D(); +}; + +class GodotCylinderShape3D : public GodotShape3D { + real_t height = 0.0; + real_t radius = 0.0; + + void _setup(real_t p_height, real_t p_radius); + +public: + _FORCE_INLINE_ real_t get_height() const { return height; } + _FORCE_INLINE_ real_t get_radius() const { return radius; } + + virtual real_t get_area() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius + height * Math_PI * radius * radius; } + + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CYLINDER; } + + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + GodotCylinderShape3D(); +}; + +struct GodotConvexPolygonShape3D : public GodotShape3D { + Geometry3D::MeshData mesh; + + void _setup(const Vector &p_vertices); + +public: + const Geometry3D::MeshData &get_mesh() const { return mesh; } + + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; } + + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + GodotConvexPolygonShape3D(); +}; + +struct _Volume_BVH; +struct GodotFaceShape3D; + +struct GodotConcavePolygonShape3D : public GodotConcaveShape3D { + // always a trimesh + + struct Face { + Vector3 normal; + int indices[3] = {}; + }; + + Vector faces; + Vector vertices; + + struct BVH { + AABB aabb; + int left = 0; + int right = 0; + + int face_index = 0; + }; + + Vector bvh; + + struct _CullParams { + AABB aabb; + QueryCallback callback = nullptr; + void *userdata = nullptr; + const Face *faces = nullptr; + const Vector3 *vertices = nullptr; + const BVH *bvh = nullptr; + GodotFaceShape3D *face = nullptr; + }; + + struct _SegmentCullParams { + Vector3 from; + Vector3 to; + Vector3 dir; + const Face *faces = nullptr; + const Vector3 *vertices = nullptr; + const BVH *bvh = nullptr; + GodotFaceShape3D *face = nullptr; + + Vector3 result; + Vector3 normal; + real_t min_d = 1e20; + int collisions = 0; + }; + + bool backface_collision = false; + + void _cull_segment(int p_idx, _SegmentCullParams *p_params) const; + bool _cull(int p_idx, _CullParams *p_params) const; + + void _fill_bvh(_Volume_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx); + + void _setup(const Vector &p_faces, bool p_backface_collision); + +public: + Vector get_faces() const; + + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; } + + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + + virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override; + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + GodotConcavePolygonShape3D(); +}; + +struct GodotHeightMapShape3D : public GodotConcaveShape3D { + Vector heights; + int width = 0; + int depth = 0; + Vector3 local_origin; + + // Accelerator. + struct Range { + real_t min = 0.0; + real_t max = 0.0; + }; + LocalVector bounds_grid; + int bounds_grid_width = 0; + int bounds_grid_depth = 0; + + static const int BOUNDS_CHUNK_SIZE = 16; + + _FORCE_INLINE_ const Range &_get_bounds_chunk(int p_x, int p_z) const { + return bounds_grid[(p_z * bounds_grid_width) + p_x]; + } + + _FORCE_INLINE_ real_t _get_height(int p_x, int p_z) const { + return heights[(p_z * width) + p_x]; + } + + _FORCE_INLINE_ void _get_point(int p_x, int p_z, Vector3 &r_point) const { + r_point.x = p_x - 0.5 * (width - 1.0); + r_point.y = _get_height(p_x, p_z); + r_point.z = p_z - 0.5 * (depth - 1.0); + } + + void _get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const; + + void _build_accelerator(); + + template + bool _intersect_grid_segment(ProcessFunction &p_process, const Vector3 &p_begin, const Vector3 &p_end, int p_width, int p_depth, const Vector3 &offset, Vector3 &r_point, Vector3 &r_normal) const; + + void _setup(const Vector &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height); + +public: + Vector get_heights() const; + int get_width() const; + int get_depth() const; + + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_HEIGHTMAP; } + + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override; + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + GodotHeightMapShape3D(); +}; + +//used internally +struct GodotFaceShape3D : public GodotShape3D { + Vector3 normal; //cache + Vector3 vertex[3]; + bool backface_collision = false; + + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; } + + const Vector3 &get_vertex(int p_idx) const { return vertex[p_idx]; } + + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; + + virtual void set_data(const Variant &p_data) override {} + virtual Variant get_data() const override { return Variant(); } + + GodotFaceShape3D(); +}; + +struct GodotMotionShape3D : public GodotShape3D { + GodotShape3D *shape = nullptr; + Vector3 motion; + + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; } + + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override { + Vector3 cast = p_transform.basis.xform(motion); + real_t mina, maxa; + real_t minb, maxb; + Transform3D ofsb = p_transform; + ofsb.origin += cast; + shape->project_range(p_normal, p_transform, mina, maxa); + shape->project_range(p_normal, ofsb, minb, maxb); + r_min = MIN(mina, minb); + r_max = MAX(maxa, maxb); + } + + virtual Vector3 get_support(const Vector3 &p_normal) const override { + Vector3 support = shape->get_support(p_normal); + if (p_normal.dot(motion) > 0) { + support += motion; + } + return support; + } + + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; } + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override { return false; } + virtual bool intersect_point(const Vector3 &p_point) const override { return false; } + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override { return p_point; } + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override { return Vector3(); } + + virtual void set_data(const Variant &p_data) override {} + virtual Variant get_data() const override { return Variant(); } + + GodotMotionShape3D() { configure(AABB()); } +}; + +#endif // GODOT_SHAPE_3D_H diff --git a/servers/physics_3d/godot_soft_body_3d.cpp b/servers/physics_3d/godot_soft_body_3d.cpp new file mode 100644 index 0000000000..d15235d27c --- /dev/null +++ b/servers/physics_3d/godot_soft_body_3d.cpp @@ -0,0 +1,1323 @@ +/*************************************************************************/ +/* godot_soft_body_3d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_soft_body_3d.h" + +#include "godot_space_3d.h" + +#include "core/math/geometry_3d.h" +#include "core/templates/map.h" +#include "servers/rendering_server.h" + +// Based on Bullet soft body. + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///btSoftBody implementation by Nathanael Presson + +GodotSoftBody3D::GodotSoftBody3D() : + GodotCollisionObject3D(TYPE_SOFT_BODY), + active_list(this) { + _set_static(false); +} + +void GodotSoftBody3D::_shapes_changed() { +} + +void GodotSoftBody3D::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) { + switch (p_state) { + case PhysicsServer3D::BODY_STATE_TRANSFORM: { + _set_transform(p_variant); + _set_inv_transform(get_transform().inverse()); + + apply_nodes_transform(get_transform()); + + } break; + case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { + // Not supported. + ERR_FAIL_MSG("Linear velocity is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { + ERR_FAIL_MSG("Angular velocity is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_SLEEPING: { + ERR_FAIL_MSG("Sleeping state is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { + ERR_FAIL_MSG("Sleeping state is not supported for Soft bodies."); + } break; + } +} + +Variant GodotSoftBody3D::get_state(PhysicsServer3D::BodyState p_state) const { + switch (p_state) { + case PhysicsServer3D::BODY_STATE_TRANSFORM: { + return get_transform(); + } break; + case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { + ERR_FAIL_V_MSG(Vector3(), "Linear velocity is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { + ERR_FAIL_V_MSG(Vector3(), "Angular velocity is not supported for Soft bodies."); + return Vector3(); + } break; + case PhysicsServer3D::BODY_STATE_SLEEPING: { + ERR_FAIL_V_MSG(false, "Sleeping state is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { + ERR_FAIL_V_MSG(false, "Sleeping state is not supported for Soft bodies."); + } break; + } + + return Variant(); +} + +void GodotSoftBody3D::set_space(GodotSpace3D *p_space) { + if (get_space()) { + get_space()->soft_body_remove_from_active_list(&active_list); + + deinitialize_shape(); + } + + _set_space(p_space); + + if (get_space()) { + get_space()->soft_body_add_to_active_list(&active_list); + + if (bounds != AABB()) { + initialize_shape(true); + } + } +} + +void GodotSoftBody3D::set_mesh(RID p_mesh) { + destroy(); + + soft_mesh = p_mesh; + + if (soft_mesh.is_null()) { + return; + } + + Array arrays = RenderingServer::get_singleton()->mesh_surface_get_arrays(soft_mesh, 0); + + bool success = create_from_trimesh(arrays[RenderingServer::ARRAY_INDEX], arrays[RenderingServer::ARRAY_VERTEX]); + if (!success) { + destroy(); + } +} + +void GodotSoftBody3D::update_rendering_server(RenderingServerHandler *p_rendering_server_handler) { + if (soft_mesh.is_null()) { + return; + } + + const uint32_t vertex_count = map_visual_to_physics.size(); + for (uint32_t i = 0; i < vertex_count; ++i) { + const uint32_t node_index = map_visual_to_physics[i]; + const Node &node = nodes[node_index]; + const Vector3 &vertex_position = node.x; + const Vector3 &vertex_normal = node.n; + + p_rendering_server_handler->set_vertex(i, &vertex_position); + p_rendering_server_handler->set_normal(i, &vertex_normal); + } + + p_rendering_server_handler->set_aabb(bounds); +} + +void GodotSoftBody3D::update_normals_and_centroids() { + uint32_t i, ni; + + for (i = 0, ni = nodes.size(); i < ni; ++i) { + nodes[i].n = Vector3(); + } + + for (i = 0, ni = faces.size(); i < ni; ++i) { + Face &face = faces[i]; + const Vector3 n = vec3_cross(face.n[0]->x - face.n[2]->x, face.n[0]->x - face.n[1]->x); + face.n[0]->n += n; + face.n[1]->n += n; + face.n[2]->n += n; + face.normal = n; + face.normal.normalize(); + face.centroid = 0.33333333333 * (face.n[0]->x + face.n[1]->x + face.n[2]->x); + } + + for (i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + real_t len = node.n.length(); + if (len > CMP_EPSILON) { + node.n /= len; + } + } +} + +void GodotSoftBody3D::update_bounds() { + AABB prev_bounds = bounds; + prev_bounds.grow_by(collision_margin); + + bounds = AABB(); + + const uint32_t nodes_count = nodes.size(); + if (nodes_count == 0) { + deinitialize_shape(); + return; + } + + bool first = true; + bool moved = false; + for (uint32_t node_index = 0; node_index < nodes_count; ++node_index) { + const Node &node = nodes[node_index]; + if (!prev_bounds.has_point(node.x)) { + moved = true; + } + if (first) { + bounds.position = node.x; + first = false; + } else { + bounds.expand_to(node.x); + } + } + + if (get_space()) { + initialize_shape(moved); + } +} + +void GodotSoftBody3D::update_constants() { + reset_link_rest_lengths(); + update_link_constants(); + update_area(); +} + +void GodotSoftBody3D::update_area() { + int i, ni; + + // Face area. + for (i = 0, ni = faces.size(); i < ni; ++i) { + Face &face = faces[i]; + + const Vector3 &x0 = face.n[0]->x; + const Vector3 &x1 = face.n[1]->x; + const Vector3 &x2 = face.n[2]->x; + + const Vector3 a = x1 - x0; + const Vector3 b = x2 - x0; + const Vector3 cr = vec3_cross(a, b); + face.ra = cr.length(); + } + + // Node area. + LocalVector counts; + if (nodes.size() > 0) { + counts.resize(nodes.size()); + memset(counts.ptr(), 0, counts.size() * sizeof(int)); + } + + for (i = 0, ni = nodes.size(); i < ni; ++i) { + nodes[i].area = 0.0; + } + + for (i = 0, ni = faces.size(); i < ni; ++i) { + const Face &face = faces[i]; + for (int j = 0; j < 3; ++j) { + const int index = (int)(face.n[j] - &nodes[0]); + counts[index]++; + face.n[j]->area += Math::abs(face.ra); + } + } + + for (i = 0, ni = nodes.size(); i < ni; ++i) { + if (counts[i] > 0) { + nodes[i].area /= (real_t)counts[i]; + } else { + nodes[i].area = 0.0; + } + } +} + +void GodotSoftBody3D::reset_link_rest_lengths() { + for (uint32_t i = 0, ni = links.size(); i < ni; ++i) { + Link &link = links[i]; + link.rl = (link.n[0]->x - link.n[1]->x).length(); + link.c1 = link.rl * link.rl; + } +} + +void GodotSoftBody3D::update_link_constants() { + real_t inv_linear_stiffness = 1.0 / linear_stiffness; + for (uint32_t i = 0, ni = links.size(); i < ni; ++i) { + Link &link = links[i]; + link.c0 = (link.n[0]->im + link.n[1]->im) * inv_linear_stiffness; + } +} + +void GodotSoftBody3D::apply_nodes_transform(const Transform3D &p_transform) { + if (soft_mesh.is_null()) { + return; + } + + uint32_t node_count = nodes.size(); + Vector3 leaf_size = Vector3(collision_margin, collision_margin, collision_margin) * 2.0; + for (uint32_t node_index = 0; node_index < node_count; ++node_index) { + Node &node = nodes[node_index]; + + node.x = p_transform.xform(node.x); + node.q = node.x; + node.v = Vector3(); + node.bv = Vector3(); + + AABB node_aabb(node.x, leaf_size); + node_tree.update(node.leaf, node_aabb); + } + + face_tree.clear(); + + update_normals_and_centroids(); + update_bounds(); + update_constants(); +} + +Vector3 GodotSoftBody3D::get_vertex_position(int p_index) const { + ERR_FAIL_COND_V(p_index < 0, Vector3()); + + if (soft_mesh.is_null()) { + return Vector3(); + } + + ERR_FAIL_COND_V(p_index >= (int)map_visual_to_physics.size(), Vector3()); + uint32_t node_index = map_visual_to_physics[p_index]; + + ERR_FAIL_COND_V(node_index >= nodes.size(), Vector3()); + return nodes[node_index].x; +} + +void GodotSoftBody3D::set_vertex_position(int p_index, const Vector3 &p_position) { + ERR_FAIL_COND(p_index < 0); + + if (soft_mesh.is_null()) { + return; + } + + ERR_FAIL_COND(p_index >= (int)map_visual_to_physics.size()); + uint32_t node_index = map_visual_to_physics[p_index]; + + ERR_FAIL_COND(node_index >= nodes.size()); + Node &node = nodes[node_index]; + node.q = node.x; + node.x = p_position; +} + +void GodotSoftBody3D::pin_vertex(int p_index) { + ERR_FAIL_COND(p_index < 0); + + if (is_vertex_pinned(p_index)) { + return; + } + + pinned_vertices.push_back(p_index); + + if (!soft_mesh.is_null()) { + ERR_FAIL_COND(p_index >= (int)map_visual_to_physics.size()); + uint32_t node_index = map_visual_to_physics[p_index]; + + ERR_FAIL_COND(node_index >= nodes.size()); + Node &node = nodes[node_index]; + node.im = 0.0; + } +} + +void GodotSoftBody3D::unpin_vertex(int p_index) { + ERR_FAIL_COND(p_index < 0); + + uint32_t pinned_count = pinned_vertices.size(); + for (uint32_t i = 0; i < pinned_count; ++i) { + if (p_index == pinned_vertices[i]) { + pinned_vertices.remove(i); + + if (!soft_mesh.is_null()) { + ERR_FAIL_COND(p_index >= (int)map_visual_to_physics.size()); + uint32_t node_index = map_visual_to_physics[p_index]; + + ERR_FAIL_COND(node_index >= nodes.size()); + real_t inv_node_mass = nodes.size() * inv_total_mass; + + Node &node = nodes[node_index]; + node.im = inv_node_mass; + } + + return; + } + } +} + +void GodotSoftBody3D::unpin_all_vertices() { + if (!soft_mesh.is_null()) { + real_t inv_node_mass = nodes.size() * inv_total_mass; + uint32_t pinned_count = pinned_vertices.size(); + for (uint32_t i = 0; i < pinned_count; ++i) { + int pinned_vertex = pinned_vertices[i]; + + ERR_CONTINUE(pinned_vertex >= (int)map_visual_to_physics.size()); + uint32_t node_index = map_visual_to_physics[pinned_vertex]; + + ERR_CONTINUE(node_index >= nodes.size()); + Node &node = nodes[node_index]; + node.im = inv_node_mass; + } + } + + pinned_vertices.clear(); +} + +bool GodotSoftBody3D::is_vertex_pinned(int p_index) const { + ERR_FAIL_COND_V(p_index < 0, false); + + uint32_t pinned_count = pinned_vertices.size(); + for (uint32_t i = 0; i < pinned_count; ++i) { + if (p_index == pinned_vertices[i]) { + return true; + } + } + + return false; +} + +uint32_t GodotSoftBody3D::get_node_count() const { + return nodes.size(); +} + +real_t GodotSoftBody3D::get_node_inv_mass(uint32_t p_node_index) const { + ERR_FAIL_COND_V(p_node_index >= nodes.size(), 0.0); + return nodes[p_node_index].im; +} + +Vector3 GodotSoftBody3D::get_node_position(uint32_t p_node_index) const { + ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); + return nodes[p_node_index].x; +} + +Vector3 GodotSoftBody3D::get_node_velocity(uint32_t p_node_index) const { + ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); + return nodes[p_node_index].v; +} + +Vector3 GodotSoftBody3D::get_node_biased_velocity(uint32_t p_node_index) const { + ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); + return nodes[p_node_index].bv; +} + +void GodotSoftBody3D::apply_node_impulse(uint32_t p_node_index, const Vector3 &p_impulse) { + ERR_FAIL_COND(p_node_index >= nodes.size()); + Node &node = nodes[p_node_index]; + node.v += p_impulse * node.im; +} + +void GodotSoftBody3D::apply_node_bias_impulse(uint32_t p_node_index, const Vector3 &p_impulse) { + ERR_FAIL_COND(p_node_index >= nodes.size()); + Node &node = nodes[p_node_index]; + node.bv += p_impulse * node.im; +} + +uint32_t GodotSoftBody3D::get_face_count() const { + return faces.size(); +} + +void GodotSoftBody3D::get_face_points(uint32_t p_face_index, Vector3 &r_point_1, Vector3 &r_point_2, Vector3 &r_point_3) const { + ERR_FAIL_COND(p_face_index >= faces.size()); + const Face &face = faces[p_face_index]; + r_point_1 = face.n[0]->x; + r_point_2 = face.n[1]->x; + r_point_3 = face.n[2]->x; +} + +Vector3 GodotSoftBody3D::get_face_normal(uint32_t p_face_index) const { + ERR_FAIL_COND_V(p_face_index >= faces.size(), Vector3()); + return faces[p_face_index].normal; +} + +bool GodotSoftBody3D::create_from_trimesh(const Vector &p_indices, const Vector &p_vertices) { + ERR_FAIL_COND_V(p_indices.is_empty(), false); + ERR_FAIL_COND_V(p_vertices.is_empty(), false); + + uint32_t node_count = 0; + LocalVector vertices; + const int visual_vertex_count(p_vertices.size()); + + LocalVector triangles; + const uint32_t triangle_count(p_indices.size() / 3); + triangles.resize(triangle_count * 3); + + // Merge all overlapping vertices and create a map of physical vertices to visual vertices. + { + // Process vertices. + { + uint32_t vertex_count = 0; + Map unique_vertices; + + vertices.resize(visual_vertex_count); + map_visual_to_physics.resize(visual_vertex_count); + + for (int visual_vertex_index = 0; visual_vertex_index < visual_vertex_count; ++visual_vertex_index) { + const Vector3 &vertex = p_vertices[visual_vertex_index]; + + Map::Element *e = unique_vertices.find(vertex); + uint32_t vertex_id; + if (e) { + // Already existing. + vertex_id = e->value(); + } else { + // Create new one. + vertex_id = vertex_count++; + unique_vertices[vertex] = vertex_id; + vertices[vertex_id] = vertex; + } + + map_visual_to_physics[visual_vertex_index] = vertex_id; + } + + vertices.resize(vertex_count); + } + + // Process triangles. + { + for (uint32_t triangle_index = 0; triangle_index < triangle_count; ++triangle_index) { + for (int i = 0; i < 3; ++i) { + int visual_index = 3 * triangle_index + i; + int physics_index = map_visual_to_physics[p_indices[visual_index]]; + triangles[visual_index] = physics_index; + node_count = MAX((int)node_count, physics_index); + } + } + } + } + + ++node_count; + + // Create nodes from vertices. + nodes.resize(node_count); + real_t inv_node_mass = node_count * inv_total_mass; + Vector3 leaf_size = Vector3(collision_margin, collision_margin, collision_margin) * 2.0; + for (uint32_t i = 0; i < node_count; ++i) { + Node &node = nodes[i]; + node.s = vertices[i]; + node.x = node.s; + node.q = node.s; + node.im = inv_node_mass; + + AABB node_aabb(node.x, leaf_size); + node.leaf = node_tree.insert(node_aabb, &node); + + node.index = i; + } + + // Create links and faces from triangles. + LocalVector chks; + chks.resize(node_count * node_count); + memset(chks.ptr(), 0, chks.size() * sizeof(bool)); + + for (uint32_t i = 0; i < triangle_count * 3; i += 3) { + const int idx[] = { triangles[i], triangles[i + 1], triangles[i + 2] }; + + for (int j = 2, k = 0; k < 3; j = k++) { + int chk = idx[k] * node_count + idx[j]; + if (!chks[chk]) { + chks[chk] = true; + int inv_chk = idx[j] * node_count + idx[k]; + chks[inv_chk] = true; + + append_link(idx[j], idx[k]); + } + } + + append_face(idx[0], idx[1], idx[2]); + } + + // Set pinned nodes. + uint32_t pinned_count = pinned_vertices.size(); + for (uint32_t i = 0; i < pinned_count; ++i) { + int pinned_vertex = pinned_vertices[i]; + + ERR_CONTINUE(pinned_vertex >= visual_vertex_count); + uint32_t node_index = map_visual_to_physics[pinned_vertex]; + + ERR_CONTINUE(node_index >= node_count); + Node &node = nodes[node_index]; + node.im = 0.0; + } + + generate_bending_constraints(2); + reoptimize_link_order(); + + update_constants(); + update_normals_and_centroids(); + update_bounds(); + + return true; +} + +void GodotSoftBody3D::generate_bending_constraints(int p_distance) { + uint32_t i, j; + + if (p_distance > 1) { + // Build graph. + const uint32_t n = nodes.size(); + const unsigned inf = (~(unsigned)0) >> 1; + const uint32_t adj_size = n * n; + unsigned *adj = memnew_arr(unsigned, adj_size); + +#define IDX(_x_, _y_) ((_y_)*n + (_x_)) + for (j = 0; j < n; ++j) { + for (i = 0; i < n; ++i) { + int idx_ij = j * n + i; + int idx_ji = i * n + j; + if (i != j) { + adj[idx_ij] = adj[idx_ji] = inf; + } else { + adj[idx_ij] = adj[idx_ji] = 0; + } + } + } + for (i = 0; i < links.size(); ++i) { + const int ia = (int)(links[i].n[0] - &nodes[0]); + const int ib = (int)(links[i].n[1] - &nodes[0]); + int idx = ib * n + ia; + int idx_inv = ia * n + ib; + adj[idx] = 1; + adj[idx_inv] = 1; + } + + // Special optimized case for distance == 2. + if (p_distance == 2) { + LocalVector> node_links; + + // Build node links. + node_links.resize(nodes.size()); + + for (i = 0; i < links.size(); ++i) { + const int ia = (int)(links[i].n[0] - &nodes[0]); + const int ib = (int)(links[i].n[1] - &nodes[0]); + if (node_links[ia].find(ib) == -1) { + node_links[ia].push_back(ib); + } + + if (node_links[ib].find(ia) == -1) { + node_links[ib].push_back(ia); + } + } + for (uint32_t ii = 0; ii < node_links.size(); ii++) { + for (uint32_t jj = 0; jj < node_links[ii].size(); jj++) { + int k = node_links[ii][jj]; + for (uint32_t kk = 0; kk < node_links[k].size(); kk++) { + int l = node_links[k][kk]; + if ((int)ii != l) { + int idx_ik = k * n + ii; + int idx_kj = l * n + k; + const unsigned sum = adj[idx_ik] + adj[idx_kj]; + ERR_FAIL_COND(sum != 2); + int idx_ij = l * n + ii; + if (adj[idx_ij] > sum) { + int idx_ji = l * n + ii; + adj[idx_ij] = adj[idx_ji] = sum; + } + } + } + } + } + } else { + // Generic Floyd's algorithm. + for (uint32_t k = 0; k < n; ++k) { + for (j = 0; j < n; ++j) { + for (i = j + 1; i < n; ++i) { + int idx_ik = k * n + i; + int idx_kj = j * n + k; + const unsigned sum = adj[idx_ik] + adj[idx_kj]; + int idx_ij = j * n + i; + if (adj[idx_ij] > sum) { + int idx_ji = j * n + i; + adj[idx_ij] = adj[idx_ji] = sum; + } + } + } + } + } + + // Build links. + for (j = 0; j < n; ++j) { + for (i = j + 1; i < n; ++i) { + int idx_ij = j * n + i; + if (adj[idx_ij] == (unsigned)p_distance) { + append_link(i, j); + } + } + } + memdelete_arr(adj); + } +} + +//=================================================================== +// +// +// This function takes in a list of interdependent Links and tries +// to maximize the distance between calculation +// of dependent links. This increases the amount of parallelism that can +// be exploited by out-of-order instruction processors with large but +// (inevitably) finite instruction windows. +// +//=================================================================== + +// A small structure to track lists of dependent link calculations. +class LinkDeps { +public: + int value; // A link calculation that is dependent on this one + // Positive values = "input A" while negative values = "input B" + LinkDeps *next; // Next dependence in the list +}; +typedef LinkDeps *LinkDepsPtr; + +void GodotSoftBody3D::reoptimize_link_order() { + const int reop_not_dependent = -1; + const int reop_node_complete = -2; + + uint32_t i, link_count = links.size(), node_count = nodes.size(); + Link *lr; + int ar, br; + Node *node0 = &(nodes[0]); + Node *node1 = &(nodes[1]); + LinkDepsPtr link_dep; + int ready_list_head, ready_list_tail, link_num, link_dep_frees, dep_link; + + // Allocate temporary buffers. + int *node_written_at = memnew_arr(int, node_count + 1); // What link calculation produced this node's current values? + int *link_dep_A = memnew_arr(int, link_count); // Link calculation input is dependent upon prior calculation #N + int *link_dep_B = memnew_arr(int, link_count); + int *ready_list = memnew_arr(int, link_count); // List of ready-to-process link calculations (# of links, maximum) + LinkDeps *link_dep_free_list = memnew_arr(LinkDeps, 2 * link_count); // Dependent-on-me list elements (2x# of links, maximum) + LinkDepsPtr *link_dep_list_starts = memnew_arr(LinkDepsPtr, link_count); // Start nodes of dependent-on-me lists, one for each link + + // Copy the original, unsorted links to a side buffer. + Link *link_buffer = memnew_arr(Link, link_count); + memcpy(link_buffer, &(links[0]), sizeof(Link) * link_count); + + // Clear out the node setup and ready list. + for (i = 0; i < node_count + 1; i++) { + node_written_at[i] = reop_not_dependent; + } + for (i = 0; i < link_count; i++) { + link_dep_list_starts[i] = nullptr; + } + ready_list_head = ready_list_tail = link_dep_frees = 0; + + // Initial link analysis to set up data structures. + for (i = 0; i < link_count; i++) { + // Note which prior link calculations we are dependent upon & build up dependence lists. + lr = &(links[i]); + ar = (lr->n[0] - node0) / (node1 - node0); + br = (lr->n[1] - node0) / (node1 - node0); + if (node_written_at[ar] > reop_not_dependent) { + link_dep_A[i] = node_written_at[ar]; + link_dep = &link_dep_free_list[link_dep_frees++]; + link_dep->value = i; + link_dep->next = link_dep_list_starts[node_written_at[ar]]; + link_dep_list_starts[node_written_at[ar]] = link_dep; + } else { + link_dep_A[i] = reop_not_dependent; + } + if (node_written_at[br] > reop_not_dependent) { + link_dep_B[i] = node_written_at[br]; + link_dep = &link_dep_free_list[link_dep_frees++]; + link_dep->value = -(int)(i + 1); + link_dep->next = link_dep_list_starts[node_written_at[br]]; + link_dep_list_starts[node_written_at[br]] = link_dep; + } else { + link_dep_B[i] = reop_not_dependent; + } + + // Add this link to the initial ready list, if it is not dependent on any other links. + if ((link_dep_A[i] == reop_not_dependent) && (link_dep_B[i] == reop_not_dependent)) { + ready_list[ready_list_tail++] = i; + link_dep_A[i] = link_dep_B[i] = reop_node_complete; // Probably not needed now. + } + + // Update the nodes to mark which ones are calculated by this link. + node_written_at[ar] = node_written_at[br] = i; + } + + // Process the ready list and create the sorted list of links: + // -- By treating the ready list as a queue, we maximize the distance between any + // inter-dependent node calculations. + // -- All other (non-related) nodes in the ready list will automatically be inserted + // in between each set of inter-dependent link calculations by this loop. + i = 0; + while (ready_list_head != ready_list_tail) { + // Use ready list to select the next link to process. + link_num = ready_list[ready_list_head++]; + // Copy the next-to-calculate link back into the original link array. + links[i++] = link_buffer[link_num]; + + // Free up any link inputs that are dependent on this one. + link_dep = link_dep_list_starts[link_num]; + while (link_dep) { + dep_link = link_dep->value; + if (dep_link >= 0) { + link_dep_A[dep_link] = reop_not_dependent; + } else { + dep_link = -dep_link - 1; + link_dep_B[dep_link] = reop_not_dependent; + } + // Add this dependent link calculation to the ready list if *both* inputs are clear. + if ((link_dep_A[dep_link] == reop_not_dependent) && (link_dep_B[dep_link] == reop_not_dependent)) { + ready_list[ready_list_tail++] = dep_link; + link_dep_A[dep_link] = link_dep_B[dep_link] = reop_node_complete; // Probably not needed now. + } + link_dep = link_dep->next; + } + } + + // Delete the temporary buffers. + memdelete_arr(node_written_at); + memdelete_arr(link_dep_A); + memdelete_arr(link_dep_B); + memdelete_arr(ready_list); + memdelete_arr(link_dep_free_list); + memdelete_arr(link_dep_list_starts); + memdelete_arr(link_buffer); +} + +void GodotSoftBody3D::append_link(uint32_t p_node1, uint32_t p_node2) { + if (p_node1 == p_node2) { + return; + } + + Node *node1 = &nodes[p_node1]; + Node *node2 = &nodes[p_node2]; + + Link link; + link.n[0] = node1; + link.n[1] = node2; + link.rl = (node1->x - node2->x).length(); + + links.push_back(link); +} + +void GodotSoftBody3D::append_face(uint32_t p_node1, uint32_t p_node2, uint32_t p_node3) { + if (p_node1 == p_node2) { + return; + } + if (p_node1 == p_node3) { + return; + } + if (p_node2 == p_node3) { + return; + } + + Node *node1 = &nodes[p_node1]; + Node *node2 = &nodes[p_node2]; + Node *node3 = &nodes[p_node3]; + + Face face; + face.n[0] = node1; + face.n[1] = node2; + face.n[2] = node3; + + face.index = faces.size(); + + faces.push_back(face); +} + +void GodotSoftBody3D::set_iteration_count(int p_val) { + iteration_count = p_val; +} + +void GodotSoftBody3D::set_total_mass(real_t p_val) { + ERR_FAIL_COND(p_val < 0.0); + + inv_total_mass = 1.0 / p_val; + real_t mass_factor = total_mass * inv_total_mass; + total_mass = p_val; + + uint32_t node_count = nodes.size(); + for (uint32_t node_index = 0; node_index < node_count; ++node_index) { + Node &node = nodes[node_index]; + node.im *= mass_factor; + } + + update_constants(); +} + +void GodotSoftBody3D::set_collision_margin(real_t p_val) { + collision_margin = p_val; +} + +void GodotSoftBody3D::set_linear_stiffness(real_t p_val) { + linear_stiffness = p_val; +} + +void GodotSoftBody3D::set_pressure_coefficient(real_t p_val) { + pressure_coefficient = p_val; +} + +void GodotSoftBody3D::set_damping_coefficient(real_t p_val) { + damping_coefficient = p_val; +} + +void GodotSoftBody3D::set_drag_coefficient(real_t p_val) { + drag_coefficient = p_val; +} + +void GodotSoftBody3D::add_velocity(const Vector3 &p_velocity) { + for (uint32_t i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + if (node.im > 0) { + node.v += p_velocity; + } + } +} + +void GodotSoftBody3D::apply_forces(bool p_has_wind_forces) { + int ac = areas.size(); + + if (nodes.is_empty()) { + return; + } + + uint32_t i, ni; + int32_t j; + + real_t volume = 0.0; + const Vector3 &org = nodes[0].x; + + // Iterate over faces (try not to iterate elsewhere if possible). + for (i = 0, ni = faces.size(); i < ni; ++i) { + bool stopped = false; + const Face &face = faces[i]; + + Vector3 wind_force(0, 0, 0); + + // Compute volume. + volume += vec3_dot(face.n[0]->x - org, vec3_cross(face.n[1]->x - org, face.n[2]->x - org)); + + // Compute nodal forces from area winds. + if (ac && p_has_wind_forces) { + const AreaCMP *aa = &areas[0]; + for (j = ac - 1; j >= 0 && !stopped; j--) { + PhysicsServer3D::AreaSpaceOverrideMode mode = aa[j].area->get_space_override_mode(); + switch (mode) { + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + wind_force += _compute_area_windforce(aa[j].area, &face); + stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + } break; + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { + wind_force = _compute_area_windforce(aa[j].area, &face); + stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE; + } break; + default: { + } + } + } + + for (j = 0; j < 3; j++) { + Node *current_node = face.n[j]; + current_node->f += wind_force; + } + } + } + volume /= 6.0; + + // Apply nodal pressure forces. + if (pressure_coefficient > CMP_EPSILON) { + real_t ivolumetp = 1.0 / Math::abs(volume) * pressure_coefficient; + for (i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + if (node.im > 0) { + node.f += node.n * (node.area * ivolumetp); + } + } + } +} + +void GodotSoftBody3D::_compute_area_gravity(const GodotArea3D *p_area) { + Vector3 area_gravity; + p_area->compute_gravity(get_transform().get_origin(), area_gravity); + gravity += area_gravity; +} + +Vector3 GodotSoftBody3D::_compute_area_windforce(const GodotArea3D *p_area, const Face *p_face) { + real_t wfm = p_area->get_wind_force_magnitude(); + real_t waf = p_area->get_wind_attenuation_factor(); + const Vector3 &wd = p_area->get_wind_direction(); + const Vector3 &ws = p_area->get_wind_source(); + real_t projection_on_tri_normal = vec3_dot(p_face->normal, wd); + real_t projection_toward_centroid = vec3_dot(p_face->centroid - ws, wd); + real_t attenuation_over_distance = pow(projection_toward_centroid, -waf); + real_t nodal_force_magnitude = wfm * 0.33333333333 * p_face->ra * projection_on_tri_normal * attenuation_over_distance; + return nodal_force_magnitude * p_face->normal; +} + +void GodotSoftBody3D::predict_motion(real_t p_delta) { + const real_t inv_delta = 1.0 / p_delta; + + ERR_FAIL_COND(!get_space()); + + GodotArea3D *def_area = get_space()->get_default_area(); + ERR_FAIL_COND(!def_area); + gravity = def_area->get_gravity_vector() * def_area->get_gravity(); + + int ac = areas.size(); + bool stopped = false; + bool has_wind_forces = false; + + if (ac) { + areas.sort(); + const AreaCMP *aa = &areas[0]; + for (int i = ac - 1; i >= 0 && !stopped; i--) { + // Avoids unnecessary loop in apply_forces(). + has_wind_forces = has_wind_forces || aa[i].area->get_wind_force_magnitude() > CMP_EPSILON; + + PhysicsServer3D::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode(); + switch (mode) { + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + _compute_area_gravity(aa[i].area); + stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + } break; + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { + gravity = Vector3(0, 0, 0); + _compute_area_gravity(aa[i].area); + stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE; + } break; + default: { + } + } + } + } + + // Apply forces. + add_velocity(gravity * p_delta); + if (pressure_coefficient > CMP_EPSILON || has_wind_forces) { + apply_forces(has_wind_forces); + } + + // Avoid soft body from 'exploding' so use some upper threshold of maximum motion + // that a node can travel per frame. + const real_t max_displacement = 1000.0; + real_t clamp_delta_v = max_displacement * inv_delta; + + // Integrate. + uint32_t i, ni; + for (i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + node.q = node.x; + Vector3 delta_v = node.f * node.im * p_delta; + for (int c = 0; c < 3; c++) { + delta_v[c] = CLAMP(delta_v[c], -clamp_delta_v, clamp_delta_v); + } + node.v += delta_v; + node.x += node.v * p_delta; + node.f = Vector3(); + } + + // Bounds and tree update. + update_bounds(); + + // Node tree update. + for (i = 0, ni = nodes.size(); i < ni; ++i) { + const Node &node = nodes[i]; + + AABB node_aabb(node.x, Vector3()); + node_aabb.expand_to(node.x + node.v * p_delta); + node_aabb.grow_by(collision_margin); + + node_tree.update(node.leaf, node_aabb); + } + + // Face tree update. + if (!face_tree.is_empty()) { + update_face_tree(p_delta); + } + + // Optimize node tree. + node_tree.optimize_incremental(1); + face_tree.optimize_incremental(1); +} + +void GodotSoftBody3D::solve_constraints(real_t p_delta) { + const real_t inv_delta = 1.0 / p_delta; + + uint32_t i, ni; + + for (i = 0, ni = links.size(); i < ni; ++i) { + Link &link = links[i]; + link.c3 = link.n[1]->q - link.n[0]->q; + link.c2 = 1 / (link.c3.length_squared() * link.c0); + } + + // Solve velocities. + for (i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + node.x = node.q + node.v * p_delta; + } + + // Solve positions. + for (int isolve = 0; isolve < iteration_count; ++isolve) { + const real_t ti = isolve / (real_t)iteration_count; + solve_links(1.0, ti); + } + const real_t vc = (1.0 - damping_coefficient) * inv_delta; + for (i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + + node.x += node.bv * p_delta; + node.bv = Vector3(); + + node.v = (node.x - node.q) * vc; + + node.q = node.x; + } + + update_normals_and_centroids(); +} + +void GodotSoftBody3D::solve_links(real_t kst, real_t ti) { + for (uint32_t i = 0, ni = links.size(); i < ni; ++i) { + Link &link = links[i]; + if (link.c0 > 0) { + Node &node_a = *link.n[0]; + Node &node_b = *link.n[1]; + const Vector3 del = node_b.x - node_a.x; + const real_t len = del.length_squared(); + if (link.c1 + len > CMP_EPSILON) { + const real_t k = ((link.c1 - len) / (link.c0 * (link.c1 + len))) * kst; + node_a.x -= del * (k * node_a.im); + node_b.x += del * (k * node_b.im); + } + } + } +} + +struct AABBQueryResult { + const GodotSoftBody3D *soft_body = nullptr; + void *userdata = nullptr; + GodotSoftBody3D::QueryResultCallback result_callback = nullptr; + + _FORCE_INLINE_ bool operator()(void *p_data) { + return result_callback(soft_body->get_node_index(p_data), userdata); + }; +}; + +void GodotSoftBody3D::query_aabb(const AABB &p_aabb, GodotSoftBody3D::QueryResultCallback p_result_callback, void *p_userdata) { + AABBQueryResult query_result; + query_result.soft_body = this; + query_result.result_callback = p_result_callback; + query_result.userdata = p_userdata; + + node_tree.aabb_query(p_aabb, query_result); +} + +struct RayQueryResult { + const GodotSoftBody3D *soft_body = nullptr; + void *userdata = nullptr; + GodotSoftBody3D::QueryResultCallback result_callback = nullptr; + + _FORCE_INLINE_ bool operator()(void *p_data) { + return result_callback(soft_body->get_face_index(p_data), userdata); + }; +}; + +void GodotSoftBody3D::query_ray(const Vector3 &p_from, const Vector3 &p_to, GodotSoftBody3D::QueryResultCallback p_result_callback, void *p_userdata) { + if (face_tree.is_empty()) { + initialize_face_tree(); + } + + RayQueryResult query_result; + query_result.soft_body = this; + query_result.result_callback = p_result_callback; + query_result.userdata = p_userdata; + + face_tree.ray_query(p_from, p_to, query_result); +} + +void GodotSoftBody3D::initialize_face_tree() { + face_tree.clear(); + for (uint32_t i = 0; i < faces.size(); ++i) { + Face &face = faces[i]; + + AABB face_aabb; + + face_aabb.position = face.n[0]->x; + face_aabb.expand_to(face.n[1]->x); + face_aabb.expand_to(face.n[2]->x); + + face_aabb.grow_by(collision_margin); + + face.leaf = face_tree.insert(face_aabb, &face); + } +} + +void GodotSoftBody3D::update_face_tree(real_t p_delta) { + for (uint32_t i = 0; i < faces.size(); ++i) { + const Face &face = faces[i]; + + AABB face_aabb; + + const Node *node0 = face.n[0]; + face_aabb.position = node0->x; + face_aabb.expand_to(node0->x + node0->v * p_delta); + + const Node *node1 = face.n[1]; + face_aabb.expand_to(node1->x); + face_aabb.expand_to(node1->x + node1->v * p_delta); + + const Node *node2 = face.n[2]; + face_aabb.expand_to(node2->x); + face_aabb.expand_to(node2->x + node2->v * p_delta); + + face_aabb.grow_by(collision_margin); + + face_tree.update(face.leaf, face_aabb); + } +} + +void GodotSoftBody3D::initialize_shape(bool p_force_move) { + if (get_shape_count() == 0) { + GodotSoftBodyShape3D *soft_body_shape = memnew(GodotSoftBodyShape3D(this)); + add_shape(soft_body_shape); + } else if (p_force_move) { + GodotSoftBodyShape3D *soft_body_shape = static_cast(get_shape(0)); + soft_body_shape->update_bounds(); + } +} + +void GodotSoftBody3D::deinitialize_shape() { + if (get_shape_count() > 0) { + GodotShape3D *shape = get_shape(0); + remove_shape(shape); + memdelete(shape); + } +} + +void GodotSoftBody3D::destroy() { + soft_mesh = RID(); + + map_visual_to_physics.clear(); + + node_tree.clear(); + face_tree.clear(); + + nodes.clear(); + links.clear(); + faces.clear(); + + bounds = AABB(); + deinitialize_shape(); +} + +void GodotSoftBodyShape3D::update_bounds() { + ERR_FAIL_COND(!soft_body); + + AABB collision_aabb = soft_body->get_bounds(); + collision_aabb.grow_by(soft_body->get_collision_margin()); + configure(collision_aabb); +} + +GodotSoftBodyShape3D::GodotSoftBodyShape3D(GodotSoftBody3D *p_soft_body) { + soft_body = p_soft_body; + update_bounds(); +} + +struct _SoftBodyIntersectSegmentInfo { + const GodotSoftBody3D *soft_body = nullptr; + Vector3 from; + Vector3 dir; + Vector3 hit_position; + uint32_t hit_face_index = -1; + real_t hit_dist_sq = INFINITY; + + static bool process_hit(uint32_t p_face_index, void *p_userdata) { + _SoftBodyIntersectSegmentInfo &query_info = *(_SoftBodyIntersectSegmentInfo *)(p_userdata); + + Vector3 points[3]; + query_info.soft_body->get_face_points(p_face_index, points[0], points[1], points[2]); + + Vector3 result; + if (Geometry3D::ray_intersects_triangle(query_info.from, query_info.dir, points[0], points[1], points[2], &result)) { + real_t dist_sq = query_info.from.distance_squared_to(result); + if (dist_sq < query_info.hit_dist_sq) { + query_info.hit_dist_sq = dist_sq; + query_info.hit_position = result; + query_info.hit_face_index = p_face_index; + } + } + + // Continue with the query. + return false; + } +}; + +bool GodotSoftBodyShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + _SoftBodyIntersectSegmentInfo query_info; + query_info.soft_body = soft_body; + query_info.from = p_begin; + query_info.dir = (p_end - p_begin).normalized(); + + soft_body->query_ray(p_begin, p_end, _SoftBodyIntersectSegmentInfo::process_hit, &query_info); + + if (query_info.hit_dist_sq != INFINITY) { + r_result = query_info.hit_position; + r_normal = soft_body->get_face_normal(query_info.hit_face_index); + return true; + } + + return false; +} + +bool GodotSoftBodyShape3D::intersect_point(const Vector3 &p_point) const { + return false; +} + +Vector3 GodotSoftBodyShape3D::get_closest_point_to(const Vector3 &p_point) const { + ERR_FAIL_V_MSG(Vector3(), "Get closest point is not supported for soft bodies."); +} diff --git a/servers/physics_3d/godot_soft_body_3d.h b/servers/physics_3d/godot_soft_body_3d.h new file mode 100644 index 0000000000..008d5dddb8 --- /dev/null +++ b/servers/physics_3d/godot_soft_body_3d.h @@ -0,0 +1,279 @@ +/*************************************************************************/ +/* godot_soft_body_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_SOFT_BODY_3D_H +#define GODOT_SOFT_BODY_3D_H + +#include "godot_area_3d.h" +#include "godot_collision_object_3d.h" + +#include "core/math/aabb.h" +#include "core/math/dynamic_bvh.h" +#include "core/math/vector3.h" +#include "core/templates/local_vector.h" +#include "core/templates/set.h" +#include "core/templates/vset.h" + +class GodotConstraint3D; + +class GodotSoftBody3D : public GodotCollisionObject3D { + RID soft_mesh; + + struct Node { + Vector3 s; // Source position + Vector3 x; // Position + Vector3 q; // Previous step position/Test position + Vector3 f; // Force accumulator + Vector3 v; // Velocity + Vector3 bv; // Biased Velocity + Vector3 n; // Normal + real_t area = 0.0; // Area + real_t im = 0.0; // 1/mass + DynamicBVH::ID leaf; // Leaf data + uint32_t index = 0; + }; + + struct Link { + Vector3 c3; // gradient + Node *n[2] = { nullptr, nullptr }; // Node pointers + real_t rl = 0.0; // Rest length + real_t c0 = 0.0; // (ima+imb)*kLST + real_t c1 = 0.0; // rl^2 + real_t c2 = 0.0; // |gradient|^2/c0 + }; + + struct Face { + Vector3 centroid; + Node *n[3] = { nullptr, nullptr, nullptr }; // Node pointers + Vector3 normal; // Normal + real_t ra = 0.0; // Rest area + DynamicBVH::ID leaf; // Leaf data + uint32_t index = 0; + }; + + LocalVector nodes; + LocalVector links; + LocalVector faces; + + DynamicBVH node_tree; + DynamicBVH face_tree; + + LocalVector map_visual_to_physics; + + AABB bounds; + + real_t collision_margin = 0.05; + + real_t total_mass = 1.0; + real_t inv_total_mass = 1.0; + + int iteration_count = 5; + real_t linear_stiffness = 0.5; // [0,1] + real_t pressure_coefficient = 0.0; // [-inf,+inf] + real_t damping_coefficient = 0.01; // [0,1] + real_t drag_coefficient = 0.0; // [0,1] + LocalVector pinned_vertices; + + Vector3 gravity; + + SelfList active_list; + + Set constraints; + + Vector areas; + + VSet exceptions; + + uint64_t island_step = 0; + + _FORCE_INLINE_ void _compute_area_gravity(const GodotArea3D *p_area); + _FORCE_INLINE_ Vector3 _compute_area_windforce(const GodotArea3D *p_area, const Face *p_face); + +public: + GodotSoftBody3D(); + + const AABB &get_bounds() const { return bounds; } + + void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant); + Variant get_state(PhysicsServer3D::BodyState p_state) const; + + _FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint) { constraints.insert(p_constraint); } + _FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraints.erase(p_constraint); } + _FORCE_INLINE_ const Set &get_constraints() const { return constraints; } + _FORCE_INLINE_ void clear_constraints() { constraints.clear(); } + + _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); } + _FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); } + _FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); } + _FORCE_INLINE_ const VSet &get_exceptions() const { return exceptions; } + + _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } + _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } + + _FORCE_INLINE_ void add_area(GodotArea3D *p_area) { + int index = areas.find(AreaCMP(p_area)); + if (index > -1) { + areas.write[index].refCount += 1; + } else { + areas.ordered_insert(AreaCMP(p_area)); + } + } + + _FORCE_INLINE_ void remove_area(GodotArea3D *p_area) { + int index = areas.find(AreaCMP(p_area)); + if (index > -1) { + areas.write[index].refCount -= 1; + if (areas[index].refCount < 1) { + areas.remove(index); + } + } + } + + virtual void set_space(GodotSpace3D *p_space); + + void set_mesh(RID p_mesh); + + void update_rendering_server(RenderingServerHandler *p_rendering_server_handler); + + Vector3 get_vertex_position(int p_index) const; + void set_vertex_position(int p_index, const Vector3 &p_position); + + void pin_vertex(int p_index); + void unpin_vertex(int p_index); + void unpin_all_vertices(); + bool is_vertex_pinned(int p_index) const; + + uint32_t get_node_count() const; + real_t get_node_inv_mass(uint32_t p_node_index) const; + Vector3 get_node_position(uint32_t p_node_index) const; + Vector3 get_node_velocity(uint32_t p_node_index) const; + Vector3 get_node_biased_velocity(uint32_t p_node_index) const; + void apply_node_impulse(uint32_t p_node_index, const Vector3 &p_impulse); + void apply_node_bias_impulse(uint32_t p_node_index, const Vector3 &p_impulse); + + uint32_t get_face_count() const; + void get_face_points(uint32_t p_face_index, Vector3 &r_point_1, Vector3 &r_point_2, Vector3 &r_point_3) const; + Vector3 get_face_normal(uint32_t p_face_index) const; + + void set_iteration_count(int p_val); + _FORCE_INLINE_ real_t get_iteration_count() const { return iteration_count; } + + void set_total_mass(real_t p_val); + _FORCE_INLINE_ real_t get_total_mass() const { return total_mass; } + _FORCE_INLINE_ real_t get_total_inv_mass() const { return inv_total_mass; } + + void set_collision_margin(real_t p_val); + _FORCE_INLINE_ real_t get_collision_margin() const { return collision_margin; } + + void set_linear_stiffness(real_t p_val); + _FORCE_INLINE_ real_t get_linear_stiffness() const { return linear_stiffness; } + + void set_pressure_coefficient(real_t p_val); + _FORCE_INLINE_ real_t get_pressure_coefficient() const { return pressure_coefficient; } + + void set_damping_coefficient(real_t p_val); + _FORCE_INLINE_ real_t get_damping_coefficient() const { return damping_coefficient; } + + void set_drag_coefficient(real_t p_val); + _FORCE_INLINE_ real_t get_drag_coefficient() const { return drag_coefficient; } + + void predict_motion(real_t p_delta); + void solve_constraints(real_t p_delta); + + _FORCE_INLINE_ uint32_t get_node_index(void *p_node) const { return ((Node *)p_node)->index; } + _FORCE_INLINE_ uint32_t get_face_index(void *p_face) const { return ((Face *)p_face)->index; } + + // Return true to stop the query. + // p_index is the node index for AABB query, face index for Ray query. + typedef bool (*QueryResultCallback)(uint32_t p_index, void *p_userdata); + + void query_aabb(const AABB &p_aabb, QueryResultCallback p_result_callback, void *p_userdata); + void query_ray(const Vector3 &p_from, const Vector3 &p_to, QueryResultCallback p_result_callback, void *p_userdata); + +protected: + virtual void _shapes_changed(); + +private: + void update_normals_and_centroids(); + void update_bounds(); + void update_constants(); + void update_area(); + void reset_link_rest_lengths(); + void update_link_constants(); + + void apply_nodes_transform(const Transform3D &p_transform); + + void add_velocity(const Vector3 &p_velocity); + + void apply_forces(bool p_has_wind_forces); + + bool create_from_trimesh(const Vector &p_indices, const Vector &p_vertices); + void generate_bending_constraints(int p_distance); + void reoptimize_link_order(); + void append_link(uint32_t p_node1, uint32_t p_node2); + void append_face(uint32_t p_node1, uint32_t p_node2, uint32_t p_node3); + + void solve_links(real_t kst, real_t ti); + + void initialize_face_tree(); + void update_face_tree(real_t p_delta); + + void initialize_shape(bool p_force_move = true); + void deinitialize_shape(); + + void destroy(); +}; + +class GodotSoftBodyShape3D : public GodotShape3D { + GodotSoftBody3D *soft_body = nullptr; + +public: + GodotSoftBody3D *get_soft_body() const { return soft_body; } + + virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_SOFT_BODY; } + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { r_min = r_max = 0.0; } + virtual Vector3 get_support(const Vector3 &p_normal) const { return Vector3(); } + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; } + + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; + virtual bool intersect_point(const Vector3 &p_point) const; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const { return Vector3(); } + + virtual void set_data(const Variant &p_data) {} + virtual Variant get_data() const { return Variant(); } + + void update_bounds(); + + GodotSoftBodyShape3D(GodotSoftBody3D *p_soft_body); + ~GodotSoftBodyShape3D() {} +}; + +#endif // GODOT_SOFT_BODY_3D_H diff --git a/servers/physics_3d/godot_space_3d.cpp b/servers/physics_3d/godot_space_3d.cpp new file mode 100644 index 0000000000..750bf3a16d --- /dev/null +++ b/servers/physics_3d/godot_space_3d.cpp @@ -0,0 +1,1235 @@ +/*************************************************************************/ +/* godot_space_3d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_space_3d.h" + +#include "godot_collision_solver_3d.h" +#include "godot_physics_server_3d.h" + +#include "core/config/project_settings.h" + +#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05 + +_FORCE_INLINE_ static bool _can_collide_with(GodotCollisionObject3D *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { + if (!(p_object->get_collision_layer() & p_collision_mask)) { + return false; + } + + if (p_object->get_type() == GodotCollisionObject3D::TYPE_AREA && !p_collide_with_areas) { + return false; + } + + if (p_object->get_type() == GodotCollisionObject3D::TYPE_BODY && !p_collide_with_bodies) { + return false; + } + + if (p_object->get_type() == GodotCollisionObject3D::TYPE_SOFT_BODY && !p_collide_with_bodies) { + return false; + } + + return true; +} + +int GodotPhysicsDirectSpaceState3D::intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { + ERR_FAIL_COND_V(space->locked, false); + int amount = space->broadphase->cull_point(p_point, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + int cc = 0; + + //Transform3D ai = p_xform.affine_inverse(); + + for (int i = 0; i < amount; i++) { + if (cc >= p_result_max) { + break; + } + + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + continue; + } + + //area can't be picked by ray (default) + + if (p_exclude.has(space->intersection_query_results[i]->get_self())) { + continue; + } + + const GodotCollisionObject3D *col_obj = space->intersection_query_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; + + Transform3D inv_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + inv_xform.affine_invert(); + + if (!col_obj->get_shape(shape_idx)->intersect_point(inv_xform.xform(p_point))) { + continue; + } + + r_results[cc].collider_id = col_obj->get_instance_id(); + if (r_results[cc].collider_id.is_valid()) { + r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id); + } else { + r_results[cc].collider = nullptr; + } + r_results[cc].rid = col_obj->get_self(); + r_results[cc].shape = shape_idx; + + cc++; + } + + return cc; +} + +bool GodotPhysicsDirectSpaceState3D::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_ray) { + ERR_FAIL_COND_V(space->locked, false); + + Vector3 begin, end; + Vector3 normal; + begin = p_from; + end = p_to; + normal = (end - begin).normalized(); + + int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + //todo, create another array that references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision + + bool collided = false; + Vector3 res_point, res_normal; + int res_shape; + const GodotCollisionObject3D *res_obj; + real_t min_d = 1e10; + + for (int i = 0; i < amount; i++) { + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + continue; + } + + if (p_pick_ray && !(space->intersection_query_results[i]->is_ray_pickable())) { + continue; + } + + if (p_exclude.has(space->intersection_query_results[i]->get_self())) { + continue; + } + + const GodotCollisionObject3D *col_obj = space->intersection_query_results[i]; + + int shape_idx = space->intersection_query_subindex_results[i]; + Transform3D inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform(); + + Vector3 local_from = inv_xform.xform(begin); + Vector3 local_to = inv_xform.xform(end); + + const GodotShape3D *shape = col_obj->get_shape(shape_idx); + + Vector3 shape_point, shape_normal; + + if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) { + Transform3D xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + shape_point = xform.xform(shape_point); + + real_t ld = normal.dot(shape_point); + + if (ld < min_d) { + min_d = ld; + res_point = shape_point; + res_normal = inv_xform.basis.xform_inv(shape_normal).normalized(); + res_shape = shape_idx; + res_obj = col_obj; + collided = true; + } + } + } + + if (!collided) { + return false; + } + + r_result.collider_id = res_obj->get_instance_id(); + if (r_result.collider_id.is_valid()) { + r_result.collider = ObjectDB::get_instance(r_result.collider_id); + } else { + r_result.collider = nullptr; + } + r_result.normal = res_normal; + r_result.position = res_point; + r_result.rid = res_obj->get_self(); + r_result.shape = res_shape; + + return true; +} + +int GodotPhysicsDirectSpaceState3D::intersect_shape(const RID &p_shape, const Transform3D &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { + if (p_result_max <= 0) { + return 0; + } + + GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_shape); + ERR_FAIL_COND_V(!shape, 0); + + AABB aabb = p_xform.xform(shape->get_aabb()); + + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + int cc = 0; + + //Transform3D ai = p_xform.affine_inverse(); + + for (int i = 0; i < amount; i++) { + if (cc >= p_result_max) { + break; + } + + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + continue; + } + + //area can't be picked by ray (default) + + if (p_exclude.has(space->intersection_query_results[i]->get_self())) { + continue; + } + + const GodotCollisionObject3D *col_obj = space->intersection_query_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; + + if (!GodotCollisionSolver3D::solve_static(shape, p_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), nullptr, nullptr, nullptr, p_margin, 0)) { + continue; + } + + if (r_results) { + r_results[cc].collider_id = col_obj->get_instance_id(); + if (r_results[cc].collider_id.is_valid()) { + r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id); + } else { + r_results[cc].collider = nullptr; + } + r_results[cc].rid = col_obj->get_self(); + r_results[cc].shape = shape_idx; + } + + cc++; + } + + return cc; +} + +bool GodotPhysicsDirectSpaceState3D::cast_motion(const RID &p_shape, const Transform3D &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, ShapeRestInfo *r_info) { + GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_shape); + ERR_FAIL_COND_V(!shape, false); + + AABB aabb = p_xform.xform(shape->get_aabb()); + aabb = aabb.merge(AABB(aabb.position + p_motion, aabb.size)); //motion + aabb = aabb.grow(p_margin); + + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + real_t best_safe = 1; + real_t best_unsafe = 1; + + Transform3D xform_inv = p_xform.affine_inverse(); + GodotMotionShape3D mshape; + mshape.shape = shape; + mshape.motion = xform_inv.basis.xform(p_motion); + + bool best_first = true; + + Vector3 motion_normal = p_motion.normalized(); + + Vector3 closest_A, closest_B; + + for (int i = 0; i < amount; i++) { + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + continue; + } + + if (p_exclude.has(space->intersection_query_results[i]->get_self())) { + continue; //ignore excluded + } + + const GodotCollisionObject3D *col_obj = space->intersection_query_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; + + Vector3 point_A, point_B; + Vector3 sep_axis = motion_normal; + + Transform3D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + //test initial overlap, does it collide if going all the way? + if (GodotCollisionSolver3D::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { + continue; + } + + //test initial overlap, ignore objects it's inside of. + sep_axis = motion_normal; + + if (!GodotCollisionSolver3D::solve_distance(shape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { + continue; + } + + //just do kinematic solving + real_t low = 0.0; + real_t hi = 1.0; + real_t fraction_coeff = 0.5; + for (int j = 0; j < 8; j++) { //steps should be customizable.. + real_t fraction = low + (hi - low) * fraction_coeff; + + mshape.motion = xform_inv.basis.xform(p_motion * fraction); + + Vector3 lA, lB; + Vector3 sep = motion_normal; //important optimization for this to work fast enough + bool collided = !GodotCollisionSolver3D::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, aabb, &sep); + + if (collided) { + hi = fraction; + if ((j == 0) || (low > 0.0)) { // Did it not collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When colliding again, converge faster towards low fraction + // for more accurate results with long motions that collide near the start. + fraction_coeff = 0.25; + } + } else { + point_A = lA; + point_B = lB; + low = fraction; + if ((j == 0) || (hi < 1.0)) { // Did it collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When not colliding again, converge faster towards high fraction + // for more accurate results with long motions that collide near the end. + fraction_coeff = 0.75; + } + } + } + + if (low < best_safe) { + best_first = true; //force reset + best_safe = low; + best_unsafe = hi; + } + + if (r_info && (best_first || (point_A.distance_squared_to(point_B) < closest_A.distance_squared_to(closest_B) && low <= best_safe))) { + closest_A = point_A; + closest_B = point_B; + r_info->collider_id = col_obj->get_instance_id(); + r_info->rid = col_obj->get_self(); + r_info->shape = shape_idx; + r_info->point = closest_B; + r_info->normal = (closest_A - closest_B).normalized(); + best_first = false; + if (col_obj->get_type() == GodotCollisionObject3D::TYPE_BODY) { + const GodotBody3D *body = static_cast(col_obj); + Vector3 rel_vec = closest_B - (body->get_transform().origin + body->get_center_of_mass()); + r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec); + } + } + } + + p_closest_safe = best_safe; + p_closest_unsafe = best_unsafe; + + return true; +} + +bool GodotPhysicsDirectSpaceState3D::collide_shape(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { + if (p_result_max <= 0) { + return false; + } + + GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_shape); + ERR_FAIL_COND_V(!shape, 0); + + AABB aabb = p_shape_xform.xform(shape->get_aabb()); + aabb = aabb.grow(p_margin); + + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + bool collided = false; + r_result_count = 0; + + GodotPhysicsServer3D::CollCbkData cbk; + cbk.max = p_result_max; + cbk.amount = 0; + cbk.ptr = r_results; + GodotCollisionSolver3D::CallbackResult cbkres = GodotPhysicsServer3D::_shape_col_cbk; + + GodotPhysicsServer3D::CollCbkData *cbkptr = &cbk; + + for (int i = 0; i < amount; i++) { + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + continue; + } + + const GodotCollisionObject3D *col_obj = space->intersection_query_results[i]; + + if (p_exclude.has(col_obj->get_self())) { + continue; + } + + int shape_idx = space->intersection_query_subindex_results[i]; + + if (GodotCollisionSolver3D::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_margin)) { + collided = true; + } + } + + r_result_count = cbk.amount; + + return collided; +} + +struct _RestResultData { + const GodotCollisionObject3D *object = nullptr; + int local_shape = 0; + int shape = 0; + Vector3 contact; + Vector3 normal; + real_t len = 0.0; +}; + +struct _RestCallbackData { + const GodotCollisionObject3D *object = nullptr; + int local_shape = 0; + int shape = 0; + + real_t min_allowed_depth = 0.0; + + _RestResultData best_result; + + int max_results = 0; + int result_count = 0; + _RestResultData *other_results = nullptr; +}; + +static void _rest_cbk_result(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { + _RestCallbackData *rd = (_RestCallbackData *)p_userdata; + + Vector3 contact_rel = p_point_B - p_point_A; + real_t len = contact_rel.length(); + if (len < rd->min_allowed_depth) { + return; + } + + bool is_best_result = (len > rd->best_result.len); + + if (rd->other_results && rd->result_count > 0) { + // Consider as new result by default. + int prev_result_count = rd->result_count++; + + int result_index = 0; + real_t tested_len = is_best_result ? rd->best_result.len : len; + for (; result_index < prev_result_count - 1; ++result_index) { + if (tested_len > rd->other_results[result_index].len) { + // Re-using a previous result. + rd->result_count--; + break; + } + } + + if (result_index < rd->max_results - 1) { + _RestResultData &result = rd->other_results[result_index]; + + if (is_best_result) { + // Keep the previous best result as separate result. + result = rd->best_result; + } else { + // Keep this result as separate result. + result.len = len; + result.contact = p_point_B; + result.normal = contact_rel / len; + result.object = rd->object; + result.shape = rd->shape; + result.local_shape = rd->local_shape; + } + } else { + // Discarding this result. + rd->result_count--; + } + } else if (is_best_result) { + rd->result_count = 1; + } + + if (!is_best_result) { + return; + } + + rd->best_result.len = len; + rd->best_result.contact = p_point_B; + rd->best_result.normal = contact_rel / len; + rd->best_result.object = rd->object; + rd->best_result.shape = rd->shape; + rd->best_result.local_shape = rd->local_shape; +} + +bool GodotPhysicsDirectSpaceState3D::rest_info(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { + GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_shape); + ERR_FAIL_COND_V(!shape, 0); + + real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; + + AABB aabb = p_shape_xform.xform(shape->get_aabb()); + aabb = aabb.grow(p_margin); + + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + _RestCallbackData rcd; + rcd.min_allowed_depth = min_contact_depth; + + for (int i = 0; i < amount; i++) { + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + continue; + } + + const GodotCollisionObject3D *col_obj = space->intersection_query_results[i]; + + if (p_exclude.has(col_obj->get_self())) { + continue; + } + + int shape_idx = space->intersection_query_subindex_results[i]; + + rcd.object = col_obj; + rcd.shape = shape_idx; + bool sc = GodotCollisionSolver3D::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin); + if (!sc) { + continue; + } + } + + if (rcd.best_result.len == 0 || !rcd.best_result.object) { + return false; + } + + r_info->collider_id = rcd.best_result.object->get_instance_id(); + r_info->shape = rcd.best_result.shape; + r_info->normal = rcd.best_result.normal; + r_info->point = rcd.best_result.contact; + r_info->rid = rcd.best_result.object->get_self(); + if (rcd.best_result.object->get_type() == GodotCollisionObject3D::TYPE_BODY) { + const GodotBody3D *body = static_cast(rcd.best_result.object); + Vector3 rel_vec = rcd.best_result.contact - (body->get_transform().origin + body->get_center_of_mass()); + r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec); + + } else { + r_info->linear_velocity = Vector3(); + } + + return true; +} + +Vector3 GodotPhysicsDirectSpaceState3D::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const { + GodotCollisionObject3D *obj = GodotPhysicsServer3D::godot_singleton->area_owner.get_or_null(p_object); + if (!obj) { + obj = GodotPhysicsServer3D::godot_singleton->body_owner.get_or_null(p_object); + } + ERR_FAIL_COND_V(!obj, Vector3()); + + ERR_FAIL_COND_V(obj->get_space() != space, Vector3()); + + real_t min_distance = 1e20; + Vector3 min_point; + + bool shapes_found = false; + + for (int i = 0; i < obj->get_shape_count(); i++) { + if (obj->is_shape_disabled(i)) { + continue; + } + + Transform3D shape_xform = obj->get_transform() * obj->get_shape_transform(i); + GodotShape3D *shape = obj->get_shape(i); + + Vector3 point = shape->get_closest_point_to(shape_xform.affine_inverse().xform(p_point)); + point = shape_xform.xform(point); + + real_t dist = point.distance_to(p_point); + if (dist < min_distance) { + min_distance = dist; + min_point = point; + } + shapes_found = true; + } + + if (!shapes_found) { + return obj->get_transform().origin; //no shapes found, use distance to origin. + } else { + return min_point; + } +} + +GodotPhysicsDirectSpaceState3D::GodotPhysicsDirectSpaceState3D() { + space = nullptr; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////// + +int GodotSpace3D::_cull_aabb_for_body(GodotBody3D *p_body, const AABB &p_aabb) { + int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results); + + for (int i = 0; i < amount; i++) { + bool keep = true; + + if (intersection_query_results[i] == p_body) { + keep = false; + } else if (intersection_query_results[i]->get_type() == GodotCollisionObject3D::TYPE_AREA) { + keep = false; + } else if (intersection_query_results[i]->get_type() == GodotCollisionObject3D::TYPE_SOFT_BODY) { + keep = false; + } else if (!p_body->collides_with(static_cast(intersection_query_results[i]))) { + keep = false; + } else if (static_cast(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) { + keep = false; + } + + if (!keep) { + if (i < amount - 1) { + SWAP(intersection_query_results[i], intersection_query_results[amount - 1]); + SWAP(intersection_query_subindex_results[i], intersection_query_subindex_results[amount - 1]); + } + + amount--; + i--; + } + } + + return amount; +} + +bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult *r_result) { + //give me back regular physics engine logic + //this is madness + //and most people using this function will think + //what it does is simpler than using physics + //this took about a week to get right.. + //but is it right? who knows at this point.. + + ERR_FAIL_INDEX_V(p_parameters.max_collisions, PhysicsServer3D::MotionResult::MAX_COLLISIONS, false); + + if (r_result) { + *r_result = PhysicsServer3D::MotionResult(); + } + + AABB body_aabb; + bool shapes_found = false; + + for (int i = 0; i < p_body->get_shape_count(); i++) { + if (p_body->is_shape_disabled(i)) { + continue; + } + + if (!shapes_found) { + body_aabb = p_body->get_shape_aabb(i); + shapes_found = true; + } else { + body_aabb = body_aabb.merge(p_body->get_shape_aabb(i)); + } + } + + if (!shapes_found) { + if (r_result) { + r_result->travel = p_parameters.motion; + } + + return false; + } + + // Undo the currently transform the physics server is aware of and apply the provided one + body_aabb = p_parameters.from.xform(p_body->get_inv_transform().xform(body_aabb)); + body_aabb = body_aabb.grow(p_parameters.margin); + + real_t min_contact_depth = p_parameters.margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; + + real_t motion_length = p_parameters.motion.length(); + Vector3 motion_normal = p_parameters.motion / motion_length; + + Transform3D body_transform = p_parameters.from; + + bool recovered = false; + + { + //STEP 1, FREE BODY IF STUCK + + const int max_results = 32; + int recover_attempts = 4; + Vector3 sr[max_results * 2]; + + do { + GodotPhysicsServer3D::CollCbkData cbk; + cbk.max = max_results; + cbk.amount = 0; + cbk.ptr = sr; + + GodotPhysicsServer3D::CollCbkData *cbkptr = &cbk; + GodotCollisionSolver3D::CallbackResult cbkres = GodotPhysicsServer3D::_shape_col_cbk; + + bool collided = false; + + int amount = _cull_aabb_for_body(p_body, body_aabb); + + for (int j = 0; j < p_body->get_shape_count(); j++) { + if (p_body->is_shape_disabled(j)) { + continue; + } + + Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j); + GodotShape3D *body_shape = p_body->get_shape(j); + + for (int i = 0; i < amount; i++) { + const GodotCollisionObject3D *col_obj = intersection_query_results[i]; + if (p_parameters.exclude_bodies.has(col_obj->get_self())) { + continue; + } + if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { + continue; + } + + int shape_idx = intersection_query_subindex_results[i]; + + if (GodotCollisionSolver3D::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_parameters.margin)) { + collided = cbk.amount > 0; + } + } + } + + if (!collided) { + break; + } + + recovered = true; + + Vector3 recover_motion; + for (int i = 0; i < cbk.amount; i++) { + Vector3 a = sr[i * 2 + 0]; + Vector3 b = sr[i * 2 + 1]; + + // Compute plane on b towards a. + Vector3 n = (a - b).normalized(); + real_t d = n.dot(b); + + // Compute depth on recovered motion. + real_t depth = n.dot(a + recover_motion) - d; + if (depth > min_contact_depth + CMP_EPSILON) { + // Only recover if there is penetration. + recover_motion -= n * (depth - min_contact_depth) * 0.4; + } + } + + if (recover_motion == Vector3()) { + collided = false; + break; + } + + body_transform.origin += recover_motion; + body_aabb.position += recover_motion; + + recover_attempts--; + + } while (recover_attempts); + } + + real_t safe = 1.0; + real_t unsafe = 1.0; + int best_shape = -1; + + { + // STEP 2 ATTEMPT MOTION + + AABB motion_aabb = body_aabb; + motion_aabb.position += p_parameters.motion; + motion_aabb = motion_aabb.merge(body_aabb); + + int amount = _cull_aabb_for_body(p_body, motion_aabb); + + for (int j = 0; j < p_body->get_shape_count(); j++) { + if (p_body->is_shape_disabled(j)) { + continue; + } + + GodotShape3D *body_shape = p_body->get_shape(j); + + // Colliding separation rays allows to properly snap to the ground, + // otherwise it's not needed in regular motion. + if (!p_parameters.collide_separation_ray && (body_shape->get_type() == PhysicsServer3D::SHAPE_SEPARATION_RAY)) { + // When slide on slope is on, separation ray shape acts like a regular shape. + if (!static_cast(body_shape)->get_slide_on_slope()) { + continue; + } + } + + Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j); + + Transform3D body_shape_xform_inv = body_shape_xform.affine_inverse(); + GodotMotionShape3D mshape; + mshape.shape = body_shape; + mshape.motion = body_shape_xform_inv.basis.xform(p_parameters.motion); + + bool stuck = false; + + real_t best_safe = 1; + real_t best_unsafe = 1; + + for (int i = 0; i < amount; i++) { + const GodotCollisionObject3D *col_obj = intersection_query_results[i]; + if (p_parameters.exclude_bodies.has(col_obj->get_self())) { + continue; + } + if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { + continue; + } + + int shape_idx = intersection_query_subindex_results[i]; + + //test initial overlap, does it collide if going all the way? + Vector3 point_A, point_B; + Vector3 sep_axis = motion_normal; + + Transform3D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + //test initial overlap, does it collide if going all the way? + if (GodotCollisionSolver3D::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { + continue; + } + sep_axis = motion_normal; + + if (!GodotCollisionSolver3D::solve_distance(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { + stuck = true; + break; + } + + //just do kinematic solving + real_t low = 0.0; + real_t hi = 1.0; + real_t fraction_coeff = 0.5; + for (int k = 0; k < 8; k++) { //steps should be customizable.. + real_t fraction = low + (hi - low) * fraction_coeff; + + mshape.motion = body_shape_xform_inv.basis.xform(p_parameters.motion * fraction); + + Vector3 lA, lB; + Vector3 sep = motion_normal; //important optimization for this to work fast enough + bool collided = !GodotCollisionSolver3D::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, motion_aabb, &sep); + + if (collided) { + hi = fraction; + if ((k == 0) || (low > 0.0)) { // Did it not collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When colliding again, converge faster towards low fraction + // for more accurate results with long motions that collide near the start. + fraction_coeff = 0.25; + } + } else { + point_A = lA; + point_B = lB; + low = fraction; + if ((k == 0) || (hi < 1.0)) { // Did it collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When not colliding again, converge faster towards high fraction + // for more accurate results with long motions that collide near the end. + fraction_coeff = 0.75; + } + } + } + + if (low < best_safe) { + best_safe = low; + best_unsafe = hi; + } + } + + if (stuck) { + safe = 0; + unsafe = 0; + best_shape = j; //sadly it's the best + break; + } + if (best_safe == 1.0) { + continue; + } + if (best_safe < safe) { + safe = best_safe; + unsafe = best_unsafe; + best_shape = j; + } + } + } + + bool collided = false; + if (recovered || (safe < 1)) { + if (safe >= 1) { + best_shape = -1; //no best shape with cast, reset to -1 + } + + //it collided, let's get the rest info in unsafe advance + Transform3D ugt = body_transform; + ugt.origin += p_parameters.motion * unsafe; + + _RestResultData results[PhysicsServer3D::MotionResult::MAX_COLLISIONS]; + + _RestCallbackData rcd; + if (p_parameters.max_collisions > 1) { + rcd.max_results = p_parameters.max_collisions; + rcd.other_results = results; + } + + // Allowed depth can't be lower than motion length, in order to handle contacts at low speed. + rcd.min_allowed_depth = MIN(motion_length, min_contact_depth); + + int from_shape = best_shape != -1 ? best_shape : 0; + int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count(); + + for (int j = from_shape; j < to_shape; j++) { + if (p_body->is_shape_disabled(j)) { + continue; + } + + Transform3D body_shape_xform = ugt * p_body->get_shape_transform(j); + GodotShape3D *body_shape = p_body->get_shape(j); + + body_aabb.position += p_parameters.motion * unsafe; + + int amount = _cull_aabb_for_body(p_body, body_aabb); + + for (int i = 0; i < amount; i++) { + const GodotCollisionObject3D *col_obj = intersection_query_results[i]; + if (p_parameters.exclude_bodies.has(col_obj->get_self())) { + continue; + } + if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { + continue; + } + + int shape_idx = intersection_query_subindex_results[i]; + + rcd.object = col_obj; + rcd.shape = shape_idx; + bool sc = GodotCollisionSolver3D::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_parameters.margin); + if (!sc) { + continue; + } + } + } + + if (rcd.result_count > 0) { + if (r_result) { + for (int collision_index = 0; collision_index < rcd.result_count; ++collision_index) { + const _RestResultData &result = (collision_index > 0) ? rcd.other_results[collision_index - 1] : rcd.best_result; + + PhysicsServer3D::MotionCollision &collision = r_result->collisions[collision_index]; + + collision.collider = result.object->get_self(); + collision.collider_id = result.object->get_instance_id(); + collision.collider_shape = result.shape; + collision.local_shape = result.local_shape; + collision.normal = result.normal; + collision.position = result.contact; + collision.depth = result.len; + + const GodotBody3D *body = static_cast(result.object); + + Vector3 rel_vec = result.contact - (body->get_transform().origin + body->get_center_of_mass()); + collision.collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec); + } + + r_result->travel = safe * p_parameters.motion; + r_result->remainder = p_parameters.motion - safe * p_parameters.motion; + r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin()); + + r_result->collision_safe_fraction = safe; + r_result->collision_unsafe_fraction = unsafe; + + r_result->collision_count = rcd.result_count; + } + + collided = true; + } + } + + if (!collided && r_result) { + r_result->travel = p_parameters.motion; + r_result->remainder = Vector3(); + r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin()); + + r_result->collision_safe_fraction = 1.0; + r_result->collision_unsafe_fraction = 1.0; + } + + return collided; +} + +void *GodotSpace3D::_broadphase_pair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_self) { + if (!A->interacts_with(B)) { + return nullptr; + } + + GodotCollisionObject3D::Type type_A = A->get_type(); + GodotCollisionObject3D::Type type_B = B->get_type(); + if (type_A > type_B) { + SWAP(A, B); + SWAP(p_subindex_A, p_subindex_B); + SWAP(type_A, type_B); + } + + GodotSpace3D *self = (GodotSpace3D *)p_self; + + self->collision_pairs++; + + if (type_A == GodotCollisionObject3D::TYPE_AREA) { + GodotArea3D *area = static_cast(A); + if (type_B == GodotCollisionObject3D::TYPE_AREA) { + GodotArea3D *area_b = static_cast(B); + GodotArea2Pair3D *area2_pair = memnew(GodotArea2Pair3D(area_b, p_subindex_B, area, p_subindex_A)); + return area2_pair; + } else if (type_B == GodotCollisionObject3D::TYPE_SOFT_BODY) { + GodotSoftBody3D *softbody = static_cast(B); + GodotAreaSoftBodyPair3D *soft_area_pair = memnew(GodotAreaSoftBodyPair3D(softbody, p_subindex_B, area, p_subindex_A)); + return soft_area_pair; + } else { + GodotBody3D *body = static_cast(B); + GodotAreaPair3D *area_pair = memnew(GodotAreaPair3D(body, p_subindex_B, area, p_subindex_A)); + return area_pair; + } + } else if (type_A == GodotCollisionObject3D::TYPE_BODY) { + if (type_B == GodotCollisionObject3D::TYPE_SOFT_BODY) { + GodotBodySoftBodyPair3D *soft_pair = memnew(GodotBodySoftBodyPair3D((GodotBody3D *)A, p_subindex_A, (GodotSoftBody3D *)B)); + return soft_pair; + } else { + GodotBodyPair3D *b = memnew(GodotBodyPair3D((GodotBody3D *)A, p_subindex_A, (GodotBody3D *)B, p_subindex_B)); + return b; + } + } else { + // Soft Body/Soft Body, not supported. + } + + return nullptr; +} + +void GodotSpace3D::_broadphase_unpair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_data, void *p_self) { + if (!p_data) { + return; + } + + GodotSpace3D *self = (GodotSpace3D *)p_self; + self->collision_pairs--; + GodotConstraint3D *c = (GodotConstraint3D *)p_data; + memdelete(c); +} + +const SelfList::List &GodotSpace3D::get_active_body_list() const { + return active_list; +} + +void GodotSpace3D::body_add_to_active_list(SelfList *p_body) { + active_list.add(p_body); +} + +void GodotSpace3D::body_remove_from_active_list(SelfList *p_body) { + active_list.remove(p_body); +} + +void GodotSpace3D::body_add_to_mass_properties_update_list(SelfList *p_body) { + mass_properties_update_list.add(p_body); +} + +void GodotSpace3D::body_remove_from_mass_properties_update_list(SelfList *p_body) { + mass_properties_update_list.remove(p_body); +} + +GodotBroadPhase3D *GodotSpace3D::get_broadphase() { + return broadphase; +} + +void GodotSpace3D::add_object(GodotCollisionObject3D *p_object) { + ERR_FAIL_COND(objects.has(p_object)); + objects.insert(p_object); +} + +void GodotSpace3D::remove_object(GodotCollisionObject3D *p_object) { + ERR_FAIL_COND(!objects.has(p_object)); + objects.erase(p_object); +} + +const Set &GodotSpace3D::get_objects() const { + return objects; +} + +void GodotSpace3D::body_add_to_state_query_list(SelfList *p_body) { + state_query_list.add(p_body); +} + +void GodotSpace3D::body_remove_from_state_query_list(SelfList *p_body) { + state_query_list.remove(p_body); +} + +void GodotSpace3D::area_add_to_monitor_query_list(SelfList *p_area) { + monitor_query_list.add(p_area); +} + +void GodotSpace3D::area_remove_from_monitor_query_list(SelfList *p_area) { + monitor_query_list.remove(p_area); +} + +void GodotSpace3D::area_add_to_moved_list(SelfList *p_area) { + area_moved_list.add(p_area); +} + +void GodotSpace3D::area_remove_from_moved_list(SelfList *p_area) { + area_moved_list.remove(p_area); +} + +const SelfList::List &GodotSpace3D::get_moved_area_list() const { + return area_moved_list; +} + +const SelfList::List &GodotSpace3D::get_active_soft_body_list() const { + return active_soft_body_list; +} + +void GodotSpace3D::soft_body_add_to_active_list(SelfList *p_soft_body) { + active_soft_body_list.add(p_soft_body); +} + +void GodotSpace3D::soft_body_remove_from_active_list(SelfList *p_soft_body) { + active_soft_body_list.remove(p_soft_body); +} + +void GodotSpace3D::call_queries() { + while (state_query_list.first()) { + GodotBody3D *b = state_query_list.first()->self(); + state_query_list.remove(state_query_list.first()); + b->call_queries(); + } + + while (monitor_query_list.first()) { + GodotArea3D *a = monitor_query_list.first()->self(); + monitor_query_list.remove(monitor_query_list.first()); + a->call_queries(); + } +} + +void GodotSpace3D::setup() { + contact_debug_count = 0; + while (mass_properties_update_list.first()) { + mass_properties_update_list.first()->self()->update_mass_properties(); + mass_properties_update_list.remove(mass_properties_update_list.first()); + } +} + +void GodotSpace3D::update() { + broadphase->update(); +} + +void GodotSpace3D::set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_value) { + switch (p_param) { + case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: + contact_recycle_radius = p_value; + break; + case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION: + contact_max_separation = p_value; + break; + case PhysicsServer3D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: + contact_max_allowed_penetration = p_value; + break; + case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: + body_linear_velocity_sleep_threshold = p_value; + break; + case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: + body_angular_velocity_sleep_threshold = p_value; + break; + case PhysicsServer3D::SPACE_PARAM_BODY_TIME_TO_SLEEP: + body_time_to_sleep = p_value; + break; + case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: + body_angular_velocity_damp_ratio = p_value; + break; + case PhysicsServer3D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: + constraint_bias = p_value; + break; + } +} + +real_t GodotSpace3D::get_param(PhysicsServer3D::SpaceParameter p_param) const { + switch (p_param) { + case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: + return contact_recycle_radius; + case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION: + return contact_max_separation; + case PhysicsServer3D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: + return contact_max_allowed_penetration; + case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: + return body_linear_velocity_sleep_threshold; + case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: + return body_angular_velocity_sleep_threshold; + case PhysicsServer3D::SPACE_PARAM_BODY_TIME_TO_SLEEP: + return body_time_to_sleep; + case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: + return body_angular_velocity_damp_ratio; + case PhysicsServer3D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: + return constraint_bias; + } + return 0; +} + +void GodotSpace3D::lock() { + locked = true; +} + +void GodotSpace3D::unlock() { + locked = false; +} + +bool GodotSpace3D::is_locked() const { + return locked; +} + +GodotPhysicsDirectSpaceState3D *GodotSpace3D::get_direct_state() { + return direct_access; +} + +GodotSpace3D::GodotSpace3D() { + body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_linear", 0.1); + body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_angular", Math::deg2rad(8.0)); + body_time_to_sleep = GLOBAL_DEF("physics/3d/time_before_sleep", 0.5); + ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/time_before_sleep", PropertyInfo(Variant::FLOAT, "physics/3d/time_before_sleep", PROPERTY_HINT_RANGE, "0,5,0.01,or_greater")); + body_angular_velocity_damp_ratio = 10; + + broadphase = GodotBroadPhase3D::create_func(); + broadphase->set_pair_callback(_broadphase_pair, this); + broadphase->set_unpair_callback(_broadphase_unpair, this); + + direct_access = memnew(GodotPhysicsDirectSpaceState3D); + direct_access->space = this; +} + +GodotSpace3D::~GodotSpace3D() { + memdelete(broadphase); + memdelete(direct_access); +} diff --git a/servers/physics_3d/godot_space_3d.h b/servers/physics_3d/godot_space_3d.h new file mode 100644 index 0000000000..3b36dd346c --- /dev/null +++ b/servers/physics_3d/godot_space_3d.h @@ -0,0 +1,217 @@ +/*************************************************************************/ +/* godot_space_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_SPACE_3D_H +#define GODOT_SPACE_3D_H + +#include "godot_area_3d.h" +#include "godot_area_pair_3d.h" +#include "godot_body_3d.h" +#include "godot_body_pair_3d.h" +#include "godot_broad_phase_3d.h" +#include "godot_collision_object_3d.h" +#include "godot_soft_body_3d.h" + +#include "core/config/project_settings.h" +#include "core/templates/hash_map.h" +#include "core/typedefs.h" + +class GodotPhysicsDirectSpaceState3D : public PhysicsDirectSpaceState3D { + GDCLASS(GodotPhysicsDirectSpaceState3D, PhysicsDirectSpaceState3D); + +public: + GodotSpace3D *space; + + virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_ray = false) override; + virtual int intersect_shape(const RID &p_shape, const Transform3D &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual bool cast_motion(const RID &p_shape, const Transform3D &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, ShapeRestInfo *r_info = nullptr) override; + virtual bool collide_shape(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual bool rest_info(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const override; + + GodotPhysicsDirectSpaceState3D(); +}; + +class GodotSpace3D { +public: + enum ElapsedTime { + ELAPSED_TIME_INTEGRATE_FORCES, + ELAPSED_TIME_GENERATE_ISLANDS, + ELAPSED_TIME_SETUP_CONSTRAINTS, + ELAPSED_TIME_SOLVE_CONSTRAINTS, + ELAPSED_TIME_INTEGRATE_VELOCITIES, + ELAPSED_TIME_MAX + + }; + +private: + uint64_t elapsed_time[ELAPSED_TIME_MAX] = {}; + + GodotPhysicsDirectSpaceState3D *direct_access; + RID self; + + GodotBroadPhase3D *broadphase; + SelfList::List active_list; + SelfList::List mass_properties_update_list; + SelfList::List state_query_list; + SelfList::List monitor_query_list; + SelfList::List area_moved_list; + SelfList::List active_soft_body_list; + + static void *_broadphase_pair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_self); + static void _broadphase_unpair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_data, void *p_self); + + Set objects; + + GodotArea3D *area = nullptr; + + real_t contact_recycle_radius = 0.01; + real_t contact_max_separation = 0.05; + real_t contact_max_allowed_penetration = 0.01; + real_t constraint_bias = 0.01; + + enum { + INTERSECTION_QUERY_MAX = 2048 + }; + + GodotCollisionObject3D *intersection_query_results[INTERSECTION_QUERY_MAX]; + int intersection_query_subindex_results[INTERSECTION_QUERY_MAX]; + + real_t body_linear_velocity_sleep_threshold; + real_t body_angular_velocity_sleep_threshold; + real_t body_time_to_sleep; + real_t body_angular_velocity_damp_ratio; + + bool locked = false; + + real_t last_step = 0.001; + + int island_count = 0; + int active_objects = 0; + int collision_pairs = 0; + + RID static_global_body; + + Vector contact_debug; + int contact_debug_count = 0; + + friend class GodotPhysicsDirectSpaceState3D; + + int _cull_aabb_for_body(GodotBody3D *p_body, const AABB &p_aabb); + +public: + _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + void set_default_area(GodotArea3D *p_area) { area = p_area; } + GodotArea3D *get_default_area() const { return area; } + + const SelfList::List &get_active_body_list() const; + void body_add_to_active_list(SelfList *p_body); + void body_remove_from_active_list(SelfList *p_body); + void body_add_to_mass_properties_update_list(SelfList *p_body); + void body_remove_from_mass_properties_update_list(SelfList *p_body); + + void body_add_to_state_query_list(SelfList *p_body); + void body_remove_from_state_query_list(SelfList *p_body); + + void area_add_to_monitor_query_list(SelfList *p_area); + void area_remove_from_monitor_query_list(SelfList *p_area); + void area_add_to_moved_list(SelfList *p_area); + void area_remove_from_moved_list(SelfList *p_area); + const SelfList::List &get_moved_area_list() const; + + const SelfList::List &get_active_soft_body_list() const; + void soft_body_add_to_active_list(SelfList *p_soft_body); + void soft_body_remove_from_active_list(SelfList *p_soft_body); + + GodotBroadPhase3D *get_broadphase(); + + void add_object(GodotCollisionObject3D *p_object); + void remove_object(GodotCollisionObject3D *p_object); + const Set &get_objects() const; + + _FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; } + _FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; } + _FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; } + _FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; } + _FORCE_INLINE_ real_t get_body_linear_velocity_sleep_threshold() const { return body_linear_velocity_sleep_threshold; } + _FORCE_INLINE_ real_t get_body_angular_velocity_sleep_threshold() const { return body_angular_velocity_sleep_threshold; } + _FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; } + _FORCE_INLINE_ real_t get_body_angular_velocity_damp_ratio() const { return body_angular_velocity_damp_ratio; } + + void update(); + void setup(); + void call_queries(); + + bool is_locked() const; + void lock(); + void unlock(); + + real_t get_last_step() const { return last_step; } + void set_last_step(real_t p_step) { last_step = p_step; } + + void set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_value); + real_t get_param(PhysicsServer3D::SpaceParameter p_param) const; + + void set_island_count(int p_island_count) { island_count = p_island_count; } + int get_island_count() const { return island_count; } + + void set_active_objects(int p_active_objects) { active_objects = p_active_objects; } + int get_active_objects() const { return active_objects; } + + int get_collision_pairs() const { return collision_pairs; } + + GodotPhysicsDirectSpaceState3D *get_direct_state(); + + void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); } + _FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.is_empty(); } + _FORCE_INLINE_ void add_debug_contact(const Vector3 &p_contact) { + if (contact_debug_count < contact_debug.size()) { + contact_debug.write[contact_debug_count++] = p_contact; + } + } + _FORCE_INLINE_ Vector get_debug_contacts() { return contact_debug; } + _FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; } + + void set_static_global_body(RID p_body) { static_global_body = p_body; } + RID get_static_global_body() { return static_global_body; } + + void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; } + uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; } + + bool test_body_motion(GodotBody3D *p_body, const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult *r_result); + + GodotSpace3D(); + ~GodotSpace3D(); +}; + +#endif // GODOT_SPACE_3D_H diff --git a/servers/physics_3d/godot_step_3d.cpp b/servers/physics_3d/godot_step_3d.cpp new file mode 100644 index 0000000000..a8654c617b --- /dev/null +++ b/servers/physics_3d/godot_step_3d.cpp @@ -0,0 +1,420 @@ +/*************************************************************************/ +/* godot_step_3d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "godot_step_3d.h" + +#include "godot_joint_3d.h" + +#include "core/os/os.h" + +#define BODY_ISLAND_COUNT_RESERVE 128 +#define BODY_ISLAND_SIZE_RESERVE 512 +#define ISLAND_COUNT_RESERVE 128 +#define ISLAND_SIZE_RESERVE 512 +#define CONSTRAINT_COUNT_RESERVE 1024 + +void GodotStep3D::_populate_island(GodotBody3D *p_body, LocalVector &p_body_island, LocalVector &p_constraint_island) { + p_body->set_island_step(_step); + + if (p_body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) { + // Only dynamic bodies are tested for activation. + p_body_island.push_back(p_body); + } + + for (const KeyValue &E : p_body->get_constraint_map()) { + GodotConstraint3D *constraint = (GodotConstraint3D *)E.key; + if (constraint->get_island_step() == _step) { + continue; // Already processed. + } + constraint->set_island_step(_step); + p_constraint_island.push_back(constraint); + + all_constraints.push_back(constraint); + + // Find connected rigid bodies. + for (int i = 0; i < constraint->get_body_count(); i++) { + if (i == E.value) { + continue; + } + GodotBody3D *other_body = constraint->get_body_ptr()[i]; + if (other_body->get_island_step() == _step) { + continue; // Already processed. + } + if (other_body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) { + continue; // Static bodies don't connect islands. + } + _populate_island(other_body, p_body_island, p_constraint_island); + } + + // Find connected soft bodies. + for (int i = 0; i < constraint->get_soft_body_count(); i++) { + GodotSoftBody3D *soft_body = constraint->get_soft_body_ptr(i); + if (soft_body->get_island_step() == _step) { + continue; // Already processed. + } + _populate_island_soft_body(soft_body, p_body_island, p_constraint_island); + } + } +} + +void GodotStep3D::_populate_island_soft_body(GodotSoftBody3D *p_soft_body, LocalVector &p_body_island, LocalVector &p_constraint_island) { + p_soft_body->set_island_step(_step); + + for (Set::Element *E = p_soft_body->get_constraints().front(); E; E = E->next()) { + GodotConstraint3D *constraint = (GodotConstraint3D *)E->get(); + if (constraint->get_island_step() == _step) { + continue; // Already processed. + } + constraint->set_island_step(_step); + p_constraint_island.push_back(constraint); + + all_constraints.push_back(constraint); + + // Find connected rigid bodies. + for (int i = 0; i < constraint->get_body_count(); i++) { + GodotBody3D *body = constraint->get_body_ptr()[i]; + if (body->get_island_step() == _step) { + continue; // Already processed. + } + if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) { + continue; // Static bodies don't connect islands. + } + _populate_island(body, p_body_island, p_constraint_island); + } + } +} + +void GodotStep3D::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) { + GodotConstraint3D *constraint = all_constraints[p_constraint_index]; + constraint->setup(delta); +} + +void GodotStep3D::_pre_solve_island(LocalVector &p_constraint_island) const { + uint32_t constraint_count = p_constraint_island.size(); + uint32_t valid_constraint_count = 0; + for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { + GodotConstraint3D *constraint = p_constraint_island[constraint_index]; + if (p_constraint_island[constraint_index]->pre_solve(delta)) { + // Keep this constraint for solving. + p_constraint_island[valid_constraint_count++] = constraint; + } + } + p_constraint_island.resize(valid_constraint_count); +} + +void GodotStep3D::_solve_island(uint32_t p_island_index, void *p_userdata) { + LocalVector &constraint_island = constraint_islands[p_island_index]; + + int current_priority = 1; + + uint32_t constraint_count = constraint_island.size(); + while (constraint_count > 0) { + for (int i = 0; i < iterations; i++) { + // Go through all iterations. + for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { + constraint_island[constraint_index]->solve(delta); + } + } + + // Check priority to keep only higher priority constraints. + uint32_t priority_constraint_count = 0; + ++current_priority; + for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { + GodotConstraint3D *constraint = constraint_island[constraint_index]; + if (constraint->get_priority() >= current_priority) { + // Keep this constraint for the next iteration. + constraint_island[priority_constraint_count++] = constraint; + } + } + constraint_count = priority_constraint_count; + } +} + +void GodotStep3D::_check_suspend(const LocalVector &p_body_island) const { + bool can_sleep = true; + + uint32_t body_count = p_body_island.size(); + for (uint32_t body_index = 0; body_index < body_count; ++body_index) { + GodotBody3D *body = p_body_island[body_index]; + + if (!body->sleep_test(delta)) { + can_sleep = false; + } + } + + // Put all to sleep or wake up everyone. + for (uint32_t body_index = 0; body_index < body_count; ++body_index) { + GodotBody3D *body = p_body_island[body_index]; + + bool active = body->is_active(); + + if (active == can_sleep) { + body->set_active(!can_sleep); + } + } +} + +void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta, int p_iterations) { + p_space->lock(); // can't access space during this + + p_space->setup(); //update inertias, etc + + p_space->set_last_step(p_delta); + + iterations = p_iterations; + delta = p_delta; + + const SelfList::List *body_list = &p_space->get_active_body_list(); + + const SelfList::List *soft_body_list = &p_space->get_active_soft_body_list(); + + /* INTEGRATE FORCES */ + + uint64_t profile_begtime = OS::get_singleton()->get_ticks_usec(); + uint64_t profile_endtime = 0; + + int active_count = 0; + + const SelfList *b = body_list->first(); + while (b) { + b->self()->integrate_forces(p_delta); + b = b->next(); + active_count++; + } + + /* UPDATE SOFT BODY MOTION */ + + const SelfList *sb = soft_body_list->first(); + while (sb) { + sb->self()->predict_motion(p_delta); + sb = sb->next(); + active_count++; + } + + p_space->set_active_objects(active_count); + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* GENERATE CONSTRAINT ISLANDS FOR MOVING AREAS */ + + uint32_t island_count = 0; + + const SelfList::List &aml = p_space->get_moved_area_list(); + + while (aml.first()) { + for (const Set::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { + GodotConstraint3D *constraint = E->get(); + if (constraint->get_island_step() == _step) { + continue; + } + constraint->set_island_step(_step); + + // Each constraint can be on a separate island for areas as there's no solving phase. + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + + all_constraints.push_back(constraint); + constraint_island.push_back(constraint); + } + p_space->area_remove_from_moved_list((SelfList *)aml.first()); //faster to remove here + } + + /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */ + + b = body_list->first(); + + uint32_t body_island_count = 0; + + while (b) { + GodotBody3D *body = b->self(); + + if (body->get_island_step() != _step) { + ++body_island_count; + if (body_islands.size() < body_island_count) { + body_islands.resize(body_island_count); + } + LocalVector &body_island = body_islands[body_island_count - 1]; + body_island.clear(); + body_island.reserve(BODY_ISLAND_SIZE_RESERVE); + + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + constraint_island.reserve(ISLAND_SIZE_RESERVE); + + _populate_island(body, body_island, constraint_island); + + if (body_island.is_empty()) { + --body_island_count; + } + + if (constraint_island.is_empty()) { + --island_count; + } + } + b = b->next(); + } + + /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE SOFT BODIES */ + + sb = soft_body_list->first(); + while (sb) { + GodotSoftBody3D *soft_body = sb->self(); + + if (soft_body->get_island_step() != _step) { + ++body_island_count; + if (body_islands.size() < body_island_count) { + body_islands.resize(body_island_count); + } + LocalVector &body_island = body_islands[body_island_count - 1]; + body_island.clear(); + body_island.reserve(BODY_ISLAND_SIZE_RESERVE); + + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + constraint_island.reserve(ISLAND_SIZE_RESERVE); + + _populate_island_soft_body(soft_body, body_island, constraint_island); + + if (body_island.is_empty()) { + --body_island_count; + } + + if (constraint_island.is_empty()) { + --island_count; + } + } + sb = sb->next(); + } + + p_space->set_island_count((int)island_count); + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* SETUP CONSTRAINTS / PROCESS COLLISIONS */ + + uint32_t total_contraint_count = all_constraints.size(); + work_pool.do_work(total_contraint_count, this, &GodotStep3D::_setup_contraint, nullptr); + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* PRE-SOLVE CONSTRAINT ISLANDS */ + + // Warning: This doesn't run on threads, because it involves thread-unsafe processing. + for (uint32_t island_index = 0; island_index < island_count; ++island_index) { + _pre_solve_island(constraint_islands[island_index]); + } + + /* SOLVE CONSTRAINT ISLANDS */ + + // Warning: _solve_island modifies the constraint islands for optimization purpose, + // their content is not reliable after these calls and shouldn't be used anymore. + if (island_count > 1) { + work_pool.do_work(island_count, this, &GodotStep3D::_solve_island, nullptr); + } else if (island_count > 0) { + _solve_island(0); + } + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* INTEGRATE VELOCITIES */ + + b = body_list->first(); + while (b) { + const SelfList *n = b->next(); + b->self()->integrate_velocities(p_delta); + b = n; + } + + /* SLEEP / WAKE UP ISLANDS */ + + for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) { + _check_suspend(body_islands[island_index]); + } + + /* UPDATE SOFT BODY CONSTRAINTS */ + + sb = soft_body_list->first(); + while (sb) { + sb->self()->solve_constraints(p_delta); + sb = sb->next(); + } + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + all_constraints.clear(); + + p_space->update(); + p_space->unlock(); + _step++; +} + +GodotStep3D::GodotStep3D() { + body_islands.reserve(BODY_ISLAND_COUNT_RESERVE); + constraint_islands.reserve(ISLAND_COUNT_RESERVE); + all_constraints.reserve(CONSTRAINT_COUNT_RESERVE); + + work_pool.init(); +} + +GodotStep3D::~GodotStep3D() { + work_pool.finish(); +} diff --git a/servers/physics_3d/godot_step_3d.h b/servers/physics_3d/godot_step_3d.h new file mode 100644 index 0000000000..23ede4feff --- /dev/null +++ b/servers/physics_3d/godot_step_3d.h @@ -0,0 +1,64 @@ +/*************************************************************************/ +/* godot_step_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GODOT_STEP_3D_H +#define GODOT_STEP_3D_H + +#include "godot_space_3d.h" + +#include "core/templates/local_vector.h" +#include "core/templates/thread_work_pool.h" + +class GodotStep3D { + uint64_t _step = 1; + + int iterations = 0; + real_t delta = 0.0; + + ThreadWorkPool work_pool; + + LocalVector> body_islands; + LocalVector> constraint_islands; + LocalVector all_constraints; + + void _populate_island(GodotBody3D *p_body, LocalVector &p_body_island, LocalVector &p_constraint_island); + void _populate_island_soft_body(GodotSoftBody3D *p_soft_body, LocalVector &p_body_island, LocalVector &p_constraint_island); + void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr); + void _pre_solve_island(LocalVector &p_constraint_island) const; + void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr); + void _check_suspend(const LocalVector &p_body_island) const; + +public: + void step(GodotSpace3D *p_space, real_t p_delta, int p_iterations); + GodotStep3D(); + ~GodotStep3D(); +}; + +#endif // GODOT_STEP_3D_H diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp deleted file mode 100644 index bb9cc1bf67..0000000000 --- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp +++ /dev/null @@ -1,360 +0,0 @@ -/*************************************************************************/ -/* cone_twist_joint_3d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -/* -Adapted to Godot from the Bullet library. -*/ - -/* -Bullet Continuous Collision Detection and Physics Library -ConeTwistJointSW is Copyright (c) 2007 Starbreeze Studios - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. - -Written by: Marcus Hennix -*/ - -#include "cone_twist_joint_3d_sw.h" - -static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) { - if (Math::abs(n.z) > Math_SQRT12) { - // choose p in y-z plane - real_t a = n[1] * n[1] + n[2] * n[2]; - real_t k = 1.0 / Math::sqrt(a); - p = Vector3(0, -n[2] * k, n[1] * k); - // set q = n x p - q = Vector3(a * k, -n[0] * p[2], n[0] * p[1]); - } else { - // choose p in x-y plane - real_t a = n.x * n.x + n.y * n.y; - real_t k = 1.0 / Math::sqrt(a); - p = Vector3(-n.y * k, n.x * k, 0); - // set q = n x p - q = Vector3(-n.z * p.y, n.z * p.x, a * k); - } -} - -static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { - real_t coeff_1 = Math_PI / 4.0f; - real_t coeff_2 = 3.0f * coeff_1; - real_t abs_y = Math::abs(y); - real_t angle; - if (x >= 0.0f) { - real_t r = (x - abs_y) / (x + abs_y); - angle = coeff_1 - coeff_1 * r; - } else { - real_t r = (x + abs_y) / (abs_y - x); - angle = coeff_2 - coeff_1 * r; - } - return (y < 0.0f) ? -angle : angle; -} - -ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame) : - Joint3DSW(_arr, 2) { - A = rbA; - B = rbB; - - m_rbAFrame = rbAFrame; - m_rbBFrame = rbBFrame; - - A->add_constraint(this, 0); - B->add_constraint(this, 1); -} - -bool ConeTwistJoint3DSW::setup(real_t p_timestep) { - dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); - dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); - - if (!dynamic_A && !dynamic_B) { - return false; - } - - m_appliedImpulse = real_t(0.); - - //set bias, sign, clear accumulator - m_swingCorrection = real_t(0.); - m_twistLimitSign = real_t(0.); - m_solveTwistLimit = false; - m_solveSwingLimit = false; - m_accTwistLimitImpulse = real_t(0.); - m_accSwingLimitImpulse = real_t(0.); - - if (!m_angularOnly) { - Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); - Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); - Vector3 relPos = pivotBInW - pivotAInW; - - Vector3 normal[3]; - if (Math::is_zero_approx(relPos.length_squared())) { - normal[0] = Vector3(real_t(1.0), 0, 0); - } else { - normal[0] = relPos.normalized(); - } - - plane_space(normal[0], normal[1], normal[2]); - - for (int i = 0; i < 3; i++) { - memnew_placement(&m_jac[i], JacobianEntry3DSW( - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - pivotAInW - A->get_transform().origin - A->get_center_of_mass(), - pivotBInW - B->get_transform().origin - B->get_center_of_mass(), - normal[i], - A->get_inv_inertia(), - A->get_inv_mass(), - B->get_inv_inertia(), - B->get_inv_mass())); - } - } - - Vector3 b1Axis1, b1Axis2, b1Axis3; - Vector3 b2Axis1, b2Axis2; - - b1Axis1 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(0)); - b2Axis1 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(0)); - - real_t swing1 = real_t(0.), swing2 = real_t(0.); - - real_t swx = real_t(0.), swy = real_t(0.); - real_t thresh = real_t(10.); - real_t fact; - - // Get Frame into world space - if (m_swingSpan1 >= real_t(0.05f)) { - b1Axis2 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(1)); - //swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) ); - swx = b2Axis1.dot(b1Axis1); - swy = b2Axis1.dot(b1Axis2); - swing1 = atan2fast(swy, swx); - fact = (swy * swy + swx * swx) * thresh * thresh; - fact = fact / (fact + real_t(1.0)); - swing1 *= fact; - } - - if (m_swingSpan2 >= real_t(0.05f)) { - b1Axis3 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(2)); - //swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) ); - swx = b2Axis1.dot(b1Axis1); - swy = b2Axis1.dot(b1Axis3); - swing2 = atan2fast(swy, swx); - fact = (swy * swy + swx * swx) * thresh * thresh; - fact = fact / (fact + real_t(1.0)); - swing2 *= fact; - } - - real_t RMaxAngle1Sq = 1.0f / (m_swingSpan1 * m_swingSpan1); - real_t RMaxAngle2Sq = 1.0f / (m_swingSpan2 * m_swingSpan2); - real_t EllipseAngle = Math::abs(swing1 * swing1) * RMaxAngle1Sq + Math::abs(swing2 * swing2) * RMaxAngle2Sq; - - if (EllipseAngle > 1.0f) { - m_swingCorrection = EllipseAngle - 1.0f; - m_solveSwingLimit = true; - - // Calculate necessary axis & factors - m_swingAxis = b2Axis1.cross(b1Axis2 * b2Axis1.dot(b1Axis2) + b1Axis3 * b2Axis1.dot(b1Axis3)); - m_swingAxis.normalize(); - - real_t swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; - m_swingAxis *= swingAxisSign; - - m_kSwing = real_t(1.) / (A->compute_angular_impulse_denominator(m_swingAxis) + - B->compute_angular_impulse_denominator(m_swingAxis)); - } - - // Twist limits - if (m_twistSpan >= real_t(0.)) { - Vector3 b2Axis22 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(1)); - Quaternion rotationArc = Quaternion(b2Axis1, b1Axis1); - Vector3 TwistRef = rotationArc.xform(b2Axis22); - real_t twist = atan2fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2)); - - real_t lockedFreeFactor = (m_twistSpan > real_t(0.05f)) ? m_limitSoftness : real_t(0.); - if (twist <= -m_twistSpan * lockedFreeFactor) { - m_twistCorrection = -(twist + m_twistSpan); - m_solveTwistLimit = true; - - m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; - m_twistAxis.normalize(); - m_twistAxis *= -1.0f; - - m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + - B->compute_angular_impulse_denominator(m_twistAxis)); - - } else if (twist > m_twistSpan * lockedFreeFactor) { - m_twistCorrection = (twist - m_twistSpan); - m_solveTwistLimit = true; - - m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; - m_twistAxis.normalize(); - - m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + - B->compute_angular_impulse_denominator(m_twistAxis)); - } - } - - return true; -} - -void ConeTwistJoint3DSW::solve(real_t p_timestep) { - Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); - Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); - - real_t tau = real_t(0.3); - - //linear part - if (!m_angularOnly) { - Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; - Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; - - Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1); - Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2); - Vector3 vel = vel1 - vel2; - - for (int i = 0; i < 3; i++) { - const Vector3 &normal = m_jac[i].m_linearJointAxis; - real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal(); - - real_t rel_vel; - rel_vel = normal.dot(vel); - //positional error (zeroth order error) - real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - real_t impulse = depth * tau / p_timestep * jacDiagABInv - rel_vel * jacDiagABInv; - m_appliedImpulse += impulse; - Vector3 impulse_vector = normal * impulse; - if (dynamic_A) { - A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); - } - if (dynamic_B) { - B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); - } - } - } - - { - ///solve angular part - const Vector3 &angVelA = A->get_angular_velocity(); - const Vector3 &angVelB = B->get_angular_velocity(); - - // solve swing limit - if (m_solveSwingLimit) { - real_t amplitude = ((angVelB - angVelA).dot(m_swingAxis) * m_relaxationFactor * m_relaxationFactor + m_swingCorrection * (real_t(1.) / p_timestep) * m_biasFactor); - real_t impulseMag = amplitude * m_kSwing; - - // Clamp the accumulated impulse - real_t temp = m_accSwingLimitImpulse; - m_accSwingLimitImpulse = MAX(m_accSwingLimitImpulse + impulseMag, real_t(0.0)); - impulseMag = m_accSwingLimitImpulse - temp; - - Vector3 impulse = m_swingAxis * impulseMag; - - if (dynamic_A) { - A->apply_torque_impulse(impulse); - } - if (dynamic_B) { - B->apply_torque_impulse(-impulse); - } - } - - // solve twist limit - if (m_solveTwistLimit) { - real_t amplitude = ((angVelB - angVelA).dot(m_twistAxis) * m_relaxationFactor * m_relaxationFactor + m_twistCorrection * (real_t(1.) / p_timestep) * m_biasFactor); - real_t impulseMag = amplitude * m_kTwist; - - // Clamp the accumulated impulse - real_t temp = m_accTwistLimitImpulse; - m_accTwistLimitImpulse = MAX(m_accTwistLimitImpulse + impulseMag, real_t(0.0)); - impulseMag = m_accTwistLimitImpulse - temp; - - Vector3 impulse = m_twistAxis * impulseMag; - - if (dynamic_A) { - A->apply_torque_impulse(impulse); - } - if (dynamic_B) { - B->apply_torque_impulse(-impulse); - } - } - } -} - -void ConeTwistJoint3DSW::set_param(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value) { - switch (p_param) { - case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: { - m_swingSpan1 = p_value; - m_swingSpan2 = p_value; - } break; - case PhysicsServer3D::CONE_TWIST_JOINT_TWIST_SPAN: { - m_twistSpan = p_value; - } break; - case PhysicsServer3D::CONE_TWIST_JOINT_BIAS: { - m_biasFactor = p_value; - } break; - case PhysicsServer3D::CONE_TWIST_JOINT_SOFTNESS: { - m_limitSoftness = p_value; - } break; - case PhysicsServer3D::CONE_TWIST_JOINT_RELAXATION: { - m_relaxationFactor = p_value; - } break; - case PhysicsServer3D::CONE_TWIST_MAX: - break; // Can't happen, but silences warning - } -} - -real_t ConeTwistJoint3DSW::get_param(PhysicsServer3D::ConeTwistJointParam p_param) const { - switch (p_param) { - case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: { - return m_swingSpan1; - } break; - case PhysicsServer3D::CONE_TWIST_JOINT_TWIST_SPAN: { - return m_twistSpan; - } break; - case PhysicsServer3D::CONE_TWIST_JOINT_BIAS: { - return m_biasFactor; - } break; - case PhysicsServer3D::CONE_TWIST_JOINT_SOFTNESS: { - return m_limitSoftness; - } break; - case PhysicsServer3D::CONE_TWIST_JOINT_RELAXATION: { - return m_relaxationFactor; - } break; - case PhysicsServer3D::CONE_TWIST_MAX: - break; // Can't happen, but silences warning - } - - return 0; -} diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h b/servers/physics_3d/joints/cone_twist_joint_3d_sw.h deleted file mode 100644 index bf7e593820..0000000000 --- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h +++ /dev/null @@ -1,142 +0,0 @@ -/*************************************************************************/ -/* cone_twist_joint_3d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -/* -Adapted to Godot from the Bullet library. -*/ - -/* -Bullet Continuous Collision Detection and Physics Library -ConeTwistJointSW is Copyright (c) 2007 Starbreeze Studios - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. - -Written by: Marcus Hennix -*/ - -#ifndef CONE_TWIST_JOINT_SW_H -#define CONE_TWIST_JOINT_SW_H - -#include "servers/physics_3d/joints/jacobian_entry_3d_sw.h" -#include "servers/physics_3d/joints_3d_sw.h" - -///ConeTwistJointSW can be used to simulate ragdoll joints (upper arm, leg etc) -class ConeTwistJoint3DSW : public Joint3DSW { -#ifdef IN_PARALLELL_SOLVER -public: -#endif - - union { - struct { - Body3DSW *A; - Body3DSW *B; - }; - - Body3DSW *_arr[2] = { nullptr, nullptr }; - }; - - JacobianEntry3DSW m_jac[3] = {}; //3 orthogonal linear constraints - - real_t m_appliedImpulse = 0.0; - Transform3D m_rbAFrame; - Transform3D m_rbBFrame; - - real_t m_limitSoftness = 0.0; - real_t m_biasFactor = 0.3; - real_t m_relaxationFactor = 1.0; - - real_t m_swingSpan1 = Math_TAU / 8.0; - real_t m_swingSpan2 = 0.0; - real_t m_twistSpan = 0.0; - - Vector3 m_swingAxis; - Vector3 m_twistAxis; - - real_t m_kSwing = 0.0; - real_t m_kTwist = 0.0; - - real_t m_twistLimitSign = 0.0; - real_t m_swingCorrection = 0.0; - real_t m_twistCorrection = 0.0; - - real_t m_accSwingLimitImpulse = 0.0; - real_t m_accTwistLimitImpulse = 0.0; - - bool m_angularOnly = false; - bool m_solveTwistLimit = false; - bool m_solveSwingLimit = false; - -public: - virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; } - - virtual bool setup(real_t p_step) override; - virtual void solve(real_t p_step) override; - - ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame); - - void setAngularOnly(bool angularOnly) { - m_angularOnly = angularOnly; - } - - void setLimit(real_t _swingSpan1, real_t _swingSpan2, real_t _twistSpan, real_t _softness = 0.8f, real_t _biasFactor = 0.3f, real_t _relaxationFactor = 1.0f) { - m_swingSpan1 = _swingSpan1; - m_swingSpan2 = _swingSpan2; - m_twistSpan = _twistSpan; - - m_limitSoftness = _softness; - m_biasFactor = _biasFactor; - m_relaxationFactor = _relaxationFactor; - } - - inline int getSolveTwistLimit() { - return m_solveTwistLimit; - } - - inline int getSolveSwingLimit() { - return m_solveTwistLimit; - } - - inline real_t getTwistLimitSign() { - return m_twistLimitSign; - } - - void set_param(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value); - real_t get_param(PhysicsServer3D::ConeTwistJointParam p_param) const; -}; - -#endif // CONE_TWIST_JOINT_SW_H diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp deleted file mode 100644 index 56aba24b42..0000000000 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp +++ /dev/null @@ -1,671 +0,0 @@ -/*************************************************************************/ -/* generic_6dof_joint_3d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -/* -Adapted to Godot from the Bullet library. -*/ - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -2007-09-09 -Generic6DOFJointSW Refactored by Francisco Le?n -email: projectileman@yahoo.com -http://gimpact.sf.net -*/ - -#include "generic_6dof_joint_3d_sw.h" - -#define GENERIC_D6_DISABLE_WARMSTARTING 1 - -//////////////////////////// G6DOFRotationalLimitMotorSW //////////////////////////////////// - -int G6DOFRotationalLimitMotor3DSW::testLimitValue(real_t test_value) { - if (m_loLimit > m_hiLimit) { - m_currentLimit = 0; //Free from violation - return 0; - } - - if (test_value < m_loLimit) { - m_currentLimit = 1; //low limit violation - m_currentLimitError = test_value - m_loLimit; - return 1; - } else if (test_value > m_hiLimit) { - m_currentLimit = 2; //High limit violation - m_currentLimitError = test_value - m_hiLimit; - return 2; - }; - - m_currentLimit = 0; //Free from violation - return 0; -} - -real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits( - real_t timeStep, Vector3 &axis, real_t jacDiagABInv, - Body3DSW *body0, Body3DSW *body1, bool p_body0_dynamic, bool p_body1_dynamic) { - if (!needApplyTorques()) { - return 0.0f; - } - - real_t target_velocity = m_targetVelocity; - real_t maxMotorForce = m_maxMotorForce; - - //current error correction - if (m_currentLimit != 0) { - target_velocity = -m_ERP * m_currentLimitError / (timeStep); - maxMotorForce = m_maxLimitForce; - } - - maxMotorForce *= timeStep; - - // current velocity difference - Vector3 vel_diff = body0->get_angular_velocity(); - if (body1) { - vel_diff -= body1->get_angular_velocity(); - } - - real_t rel_vel = axis.dot(vel_diff); - - // correction velocity - real_t motor_relvel = m_limitSoftness * (target_velocity - m_damping * rel_vel); - - if (Math::is_zero_approx(motor_relvel)) { - return 0.0f; //no need for applying force - } - - // correction impulse - real_t unclippedMotorImpulse = (1 + m_bounce) * motor_relvel * jacDiagABInv; - - // clip correction impulse - real_t clippedMotorImpulse; - - ///@todo: should clip against accumulated impulse - if (unclippedMotorImpulse > 0.0f) { - clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse; - } else { - clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse; - } - - // sort with accumulated impulses - real_t lo = real_t(-1e30); - real_t hi = real_t(1e30); - - real_t oldaccumImpulse = m_accumulatedImpulse; - real_t sum = oldaccumImpulse + clippedMotorImpulse; - m_accumulatedImpulse = sum > hi ? real_t(0.) : (sum < lo ? real_t(0.) : sum); - - clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; - - Vector3 motorImp = clippedMotorImpulse * axis; - - if (p_body0_dynamic) { - body0->apply_torque_impulse(motorImp); - } - if (body1 && p_body1_dynamic) { - body1->apply_torque_impulse(-motorImp); - } - - return clippedMotorImpulse; -} - -//////////////////////////// End G6DOFRotationalLimitMotorSW //////////////////////////////////// - -//////////////////////////// G6DOFTranslationalLimitMotorSW //////////////////////////////////// -real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis( - real_t timeStep, - real_t jacDiagABInv, - Body3DSW *body1, const Vector3 &pointInA, - Body3DSW *body2, const Vector3 &pointInB, - bool p_body1_dynamic, bool p_body2_dynamic, - int limit_index, - const Vector3 &axis_normal_on_a, - const Vector3 &anchorPos) { - ///find relative velocity - // Vector3 rel_pos1 = pointInA - body1->get_transform().origin; - // Vector3 rel_pos2 = pointInB - body2->get_transform().origin; - Vector3 rel_pos1 = anchorPos - body1->get_transform().origin; - Vector3 rel_pos2 = anchorPos - body2->get_transform().origin; - - Vector3 vel1 = body1->get_velocity_in_local_point(rel_pos1); - Vector3 vel2 = body2->get_velocity_in_local_point(rel_pos2); - Vector3 vel = vel1 - vel2; - - real_t rel_vel = axis_normal_on_a.dot(vel); - - /// apply displacement correction - - //positional error (zeroth order error) - real_t depth = -(pointInA - pointInB).dot(axis_normal_on_a); - real_t lo = real_t(-1e30); - real_t hi = real_t(1e30); - - real_t minLimit = m_lowerLimit[limit_index]; - real_t maxLimit = m_upperLimit[limit_index]; - - //handle the limits - if (minLimit < maxLimit) { - { - if (depth > maxLimit) { - depth -= maxLimit; - lo = real_t(0.); - - } else { - if (depth < minLimit) { - depth -= minLimit; - hi = real_t(0.); - } else { - return 0.0f; - } - } - } - } - - real_t normalImpulse = m_limitSoftness[limit_index] * (m_restitution[limit_index] * depth / timeStep - m_damping[limit_index] * rel_vel) * jacDiagABInv; - - real_t oldNormalImpulse = m_accumulatedImpulse[limit_index]; - real_t sum = oldNormalImpulse + normalImpulse; - m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : (sum < lo ? real_t(0.) : sum); - normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; - - Vector3 impulse_vector = axis_normal_on_a * normalImpulse; - if (p_body1_dynamic) { - body1->apply_impulse(impulse_vector, rel_pos1); - } - if (p_body2_dynamic) { - body2->apply_impulse(-impulse_vector, rel_pos2); - } - return normalImpulse; -} - -//////////////////////////// G6DOFTranslationalLimitMotorSW //////////////////////////////////// - -Generic6DOFJoint3DSW::Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA) : - Joint3DSW(_arr, 2), - m_frameInA(frameInA), - m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameA) { - A = rbA; - B = rbB; - A->add_constraint(this, 0); - B->add_constraint(this, 1); -} - -void Generic6DOFJoint3DSW::calculateAngleInfo() { - Basis relative_frame = m_calculatedTransformB.basis.inverse() * m_calculatedTransformA.basis; - - m_calculatedAxisAngleDiff = relative_frame.get_euler_xyz(); - - // in euler angle mode we do not actually constrain the angular velocity - // along the axes axis[0] and axis[2] (although we do use axis[1]) : - // - // to get constrain w2-w1 along ...not - // ------ --------------------- ------ - // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] - // d(angle[1])/dt = 0 ax[1] - // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] - // - // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. - // to prove the result for angle[0], write the expression for angle[0] from - // GetInfo1 then take the derivative. to prove this for angle[2] it is - // easier to take the euler rate expression for d(angle[2])/dt with respect - // to the components of w and set that to 0. - - Vector3 axis0 = m_calculatedTransformB.basis.get_axis(0); - Vector3 axis2 = m_calculatedTransformA.basis.get_axis(2); - - m_calculatedAxis[1] = axis2.cross(axis0); - m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); - m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); - - /* - if(m_debugDrawer) - { - char buff[300]; - sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ", - m_calculatedAxisAngleDiff[0], - m_calculatedAxisAngleDiff[1], - m_calculatedAxisAngleDiff[2]); - m_debugDrawer->reportErrorWarning(buff); - } - */ -} - -void Generic6DOFJoint3DSW::calculateTransforms() { - m_calculatedTransformA = A->get_transform() * m_frameInA; - m_calculatedTransformB = B->get_transform() * m_frameInB; - - calculateAngleInfo(); -} - -void Generic6DOFJoint3DSW::buildLinearJacobian( - JacobianEntry3DSW &jacLinear, const Vector3 &normalWorld, - const Vector3 &pivotAInW, const Vector3 &pivotBInW) { - memnew_placement(&jacLinear, JacobianEntry3DSW( - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - pivotAInW - A->get_transform().origin - A->get_center_of_mass(), - pivotBInW - B->get_transform().origin - B->get_center_of_mass(), - normalWorld, - A->get_inv_inertia(), - A->get_inv_mass(), - B->get_inv_inertia(), - B->get_inv_mass())); -} - -void Generic6DOFJoint3DSW::buildAngularJacobian( - JacobianEntry3DSW &jacAngular, const Vector3 &jointAxisW) { - memnew_placement(&jacAngular, JacobianEntry3DSW(jointAxisW, - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia())); -} - -bool Generic6DOFJoint3DSW::testAngularLimitMotor(int axis_index) { - real_t angle = m_calculatedAxisAngleDiff[axis_index]; - - //test limits - m_angularLimits[axis_index].testLimitValue(angle); - return m_angularLimits[axis_index].needApplyTorques(); -} - -bool Generic6DOFJoint3DSW::setup(real_t p_timestep) { - dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); - dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); - - if (!dynamic_A && !dynamic_B) { - return false; - } - - // Clear accumulated impulses for the next simulation step - m_linearLimits.m_accumulatedImpulse = Vector3(real_t(0.), real_t(0.), real_t(0.)); - int i; - for (i = 0; i < 3; i++) { - m_angularLimits[i].m_accumulatedImpulse = real_t(0.); - } - //calculates transform - calculateTransforms(); - - // const Vector3& pivotAInW = m_calculatedTransformA.origin; - // const Vector3& pivotBInW = m_calculatedTransformB.origin; - calcAnchorPos(); - Vector3 pivotAInW = m_AnchorPos; - Vector3 pivotBInW = m_AnchorPos; - - // not used here - // Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; - // Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; - - Vector3 normalWorld; - //linear part - for (i = 0; i < 3; i++) { - if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) { - if (m_useLinearReferenceFrameA) { - normalWorld = m_calculatedTransformA.basis.get_axis(i); - } else { - normalWorld = m_calculatedTransformB.basis.get_axis(i); - } - - buildLinearJacobian( - m_jacLinear[i], normalWorld, - pivotAInW, pivotBInW); - } - } - - // angular part - for (i = 0; i < 3; i++) { - //calculates error angle - if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i)) { - normalWorld = this->getAxis(i); - // Create angular atom - buildAngularJacobian(m_jacAng[i], normalWorld); - } - } - - return true; -} - -void Generic6DOFJoint3DSW::solve(real_t p_timestep) { - m_timeStep = p_timestep; - - //calculateTransforms(); - - int i; - - // linear - - Vector3 pointInA = m_calculatedTransformA.origin; - Vector3 pointInB = m_calculatedTransformB.origin; - - real_t jacDiagABInv; - Vector3 linear_axis; - for (i = 0; i < 3; i++) { - if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) { - jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal(); - - if (m_useLinearReferenceFrameA) { - linear_axis = m_calculatedTransformA.basis.get_axis(i); - } else { - linear_axis = m_calculatedTransformB.basis.get_axis(i); - } - - m_linearLimits.solveLinearAxis( - m_timeStep, - jacDiagABInv, - A, pointInA, - B, pointInB, - dynamic_A, dynamic_B, - i, linear_axis, m_AnchorPos); - } - } - - // angular - Vector3 angular_axis; - real_t angularJacDiagABInv; - for (i = 0; i < 3; i++) { - if (m_angularLimits[i].m_enableLimit && m_angularLimits[i].needApplyTorques()) { - // get axis - angular_axis = getAxis(i); - - angularJacDiagABInv = real_t(1.) / m_jacAng[i].getDiagonal(); - - m_angularLimits[i].solveAngularLimits(m_timeStep, angular_axis, angularJacDiagABInv, A, B, dynamic_A, dynamic_B); - } - } -} - -void Generic6DOFJoint3DSW::updateRHS(real_t timeStep) { - (void)timeStep; -} - -Vector3 Generic6DOFJoint3DSW::getAxis(int axis_index) const { - return m_calculatedAxis[axis_index]; -} - -real_t Generic6DOFJoint3DSW::getAngle(int axis_index) const { - return m_calculatedAxisAngleDiff[axis_index]; -} - -void Generic6DOFJoint3DSW::calcAnchorPos() { - real_t imA = A->get_inv_mass(); - real_t imB = B->get_inv_mass(); - real_t weight; - if (imB == real_t(0.0)) { - weight = real_t(1.0); - } else { - weight = imA / (imA + imB); - } - const Vector3 &pA = m_calculatedTransformA.origin; - const Vector3 &pB = m_calculatedTransformB.origin; - m_AnchorPos = pA * weight + pB * (real_t(1.0) - weight); -} // Generic6DOFJointSW::calcAnchorPos() - -void Generic6DOFJoint3DSW::set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value) { - ERR_FAIL_INDEX(p_axis, 3); - switch (p_param) { - case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { - m_linearLimits.m_lowerLimit[p_axis] = p_value; - } break; - case PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT: { - m_linearLimits.m_upperLimit[p_axis] = p_value; - - } break; - case PhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: { - m_linearLimits.m_limitSoftness[p_axis] = p_value; - - } break; - case PhysicsServer3D::G6DOF_JOINT_LINEAR_RESTITUTION: { - m_linearLimits.m_restitution[p_axis] = p_value; - - } break; - case PhysicsServer3D::G6DOF_JOINT_LINEAR_DAMPING: { - m_linearLimits.m_damping[p_axis] = p_value; - - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: { - m_angularLimits[p_axis].m_loLimit = p_value; - - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: { - m_angularLimits[p_axis].m_hiLimit = p_value; - - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: { - m_angularLimits[p_axis].m_limitSoftness = p_value; - - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_DAMPING: { - m_angularLimits[p_axis].m_damping = p_value; - - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_RESTITUTION: { - m_angularLimits[p_axis].m_bounce = p_value; - - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: { - m_angularLimits[p_axis].m_maxLimitForce = p_value; - - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_ERP: { - m_angularLimits[p_axis].m_ERP = p_value; - - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: { - m_angularLimits[p_axis].m_targetVelocity = p_value; - - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: { - m_angularLimits[p_axis].m_maxLimitForce = p_value; - - } break; - case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: { - // Not implemented in GodotPhysics3D backend - } break; - case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: { - // Not implemented in GodotPhysics3D backend - } break; - case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: { - // Not implemented in GodotPhysics3D backend - } break; - case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_DAMPING: { - // Not implemented in GodotPhysics3D backend - } break; - case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: { - // Not implemented in GodotPhysics3D backend - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: { - // Not implemented in GodotPhysics3D backend - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: { - // Not implemented in GodotPhysics3D backend - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: { - // Not implemented in GodotPhysics3D backend - } break; - case PhysicsServer3D::G6DOF_JOINT_MAX: - break; // Can't happen, but silences warning - } -} - -real_t Generic6DOFJoint3DSW::get_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const { - ERR_FAIL_INDEX_V(p_axis, 3, 0); - switch (p_param) { - case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { - return m_linearLimits.m_lowerLimit[p_axis]; - } break; - case PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT: { - return m_linearLimits.m_upperLimit[p_axis]; - - } break; - case PhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: { - return m_linearLimits.m_limitSoftness[p_axis]; - - } break; - case PhysicsServer3D::G6DOF_JOINT_LINEAR_RESTITUTION: { - return m_linearLimits.m_restitution[p_axis]; - - } break; - case PhysicsServer3D::G6DOF_JOINT_LINEAR_DAMPING: { - return m_linearLimits.m_damping[p_axis]; - - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: { - return m_angularLimits[p_axis].m_loLimit; - - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: { - return m_angularLimits[p_axis].m_hiLimit; - - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: { - return m_angularLimits[p_axis].m_limitSoftness; - - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_DAMPING: { - return m_angularLimits[p_axis].m_damping; - - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_RESTITUTION: { - return m_angularLimits[p_axis].m_bounce; - - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: { - return m_angularLimits[p_axis].m_maxLimitForce; - - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_ERP: { - return m_angularLimits[p_axis].m_ERP; - - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: { - return m_angularLimits[p_axis].m_targetVelocity; - - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: { - return m_angularLimits[p_axis].m_maxMotorForce; - - } break; - case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: { - // Not implemented in GodotPhysics3D backend - } break; - case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: { - // Not implemented in GodotPhysics3D backend - } break; - case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: { - // Not implemented in GodotPhysics3D backend - } break; - case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_DAMPING: { - // Not implemented in GodotPhysics3D backend - } break; - case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: { - // Not implemented in GodotPhysics3D backend - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: { - // Not implemented in GodotPhysics3D backend - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: { - // Not implemented in GodotPhysics3D backend - } break; - case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: { - // Not implemented in GodotPhysics3D backend - } break; - case PhysicsServer3D::G6DOF_JOINT_MAX: - break; // Can't happen, but silences warning - } - return 0; -} - -void Generic6DOFJoint3DSW::set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_value) { - ERR_FAIL_INDEX(p_axis, 3); - - switch (p_flag) { - case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: { - m_linearLimits.enable_limit[p_axis] = p_value; - } break; - case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: { - m_angularLimits[p_axis].m_enableLimit = p_value; - } break; - case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR: { - m_angularLimits[p_axis].m_enableMotor = p_value; - } break; - case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: { - // Not implemented in GodotPhysics3D backend - } break; - case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: { - // Not implemented in GodotPhysics3D backend - } break; - case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: { - // Not implemented in GodotPhysics3D backend - } break; - case PhysicsServer3D::G6DOF_JOINT_FLAG_MAX: - break; // Can't happen, but silences warning - } -} - -bool Generic6DOFJoint3DSW::get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const { - ERR_FAIL_INDEX_V(p_axis, 3, 0); - switch (p_flag) { - case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: { - return m_linearLimits.enable_limit[p_axis]; - } break; - case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: { - return m_angularLimits[p_axis].m_enableLimit; - } break; - case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR: { - return m_angularLimits[p_axis].m_enableMotor; - } break; - case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: { - // Not implemented in GodotPhysics3D backend - } break; - case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: { - // Not implemented in GodotPhysics3D backend - } break; - case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: { - // Not implemented in GodotPhysics3D backend - } break; - case PhysicsServer3D::G6DOF_JOINT_FLAG_MAX: - break; // Can't happen, but silences warning - } - - return false; -} diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h deleted file mode 100644 index 6492e40393..0000000000 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h +++ /dev/null @@ -1,347 +0,0 @@ -/*************************************************************************/ -/* generic_6dof_joint_3d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -/* -Adapted to Godot from the Bullet library. -*/ - -#ifndef GENERIC_6DOF_JOINT_SW_H -#define GENERIC_6DOF_JOINT_SW_H - -#include "servers/physics_3d/joints/jacobian_entry_3d_sw.h" -#include "servers/physics_3d/joints_3d_sw.h" - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -2007-09-09 -Generic6DOFJointSW Refactored by Francisco Le?n -email: projectileman@yahoo.com -http://gimpact.sf.net -*/ - -//! Rotation Limit structure for generic joints -class G6DOFRotationalLimitMotor3DSW { -public: - //! limit_parameters - //!@{ - real_t m_loLimit = -1e30; //!< joint limit - real_t m_hiLimit = 1e30; //!< joint limit - real_t m_targetVelocity = 0.0; //!< target motor velocity - real_t m_maxMotorForce = 0.1; //!< max force on motor - real_t m_maxLimitForce = 300.0; //!< max force on limit - real_t m_damping = 1.0; //!< Damping. - real_t m_limitSoftness = 0.5; //! Relaxation factor - real_t m_ERP = 0.5; //!< Error tolerance factor when joint is at limit - real_t m_bounce = 0.0; //!< restitution factor - bool m_enableMotor = false; - bool m_enableLimit = false; - - //!@} - - //! temp_variables - //!@{ - real_t m_currentLimitError = 0.0; //!< How much is violated this limit - int m_currentLimit = 0; //!< 0=free, 1=at lo limit, 2=at hi limit - real_t m_accumulatedImpulse = 0.0; - //!@} - - G6DOFRotationalLimitMotor3DSW() {} - - //! Is limited - bool isLimited() { - return (m_loLimit < m_hiLimit); - } - - //! Need apply correction - bool needApplyTorques() { - return (m_enableMotor || m_currentLimit != 0); - } - - //! calculates error - /*! - calculates m_currentLimit and m_currentLimitError. - */ - int testLimitValue(real_t test_value); - - //! apply the correction impulses for two bodies - real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, Body3DSW *body0, Body3DSW *body1, bool p_body0_dynamic, bool p_body1_dynamic); -}; - -class G6DOFTranslationalLimitMotor3DSW { -public: - Vector3 m_lowerLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint lower limits - Vector3 m_upperLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint upper limits - Vector3 m_accumulatedImpulse = Vector3(0.0, 0.0, 0.0); - //! Linear_Limit_parameters - //!@{ - Vector3 m_limitSoftness = Vector3(0.7, 0.7, 0.7); //!< Softness for linear limit - Vector3 m_damping = Vector3(1.0, 1.0, 1.0); //!< Damping for linear limit - Vector3 m_restitution = Vector3(0.5, 0.5, 0.5); //! Bounce parameter for linear limit - //!@} - bool enable_limit[3] = { true, true, true }; - - //! Test limit - /*! - - free means upper < lower, - - locked means upper == lower - - limited means upper > lower - - limitIndex: first 3 are linear, next 3 are angular - */ - inline bool isLimited(int limitIndex) { - return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); - } - - real_t solveLinearAxis( - real_t timeStep, - real_t jacDiagABInv, - Body3DSW *body1, const Vector3 &pointInA, - Body3DSW *body2, const Vector3 &pointInB, - bool p_body1_dynamic, bool p_body2_dynamic, - int limit_index, - const Vector3 &axis_normal_on_a, - const Vector3 &anchorPos); -}; - -class Generic6DOFJoint3DSW : public Joint3DSW { -protected: - union { - struct { - Body3DSW *A; - Body3DSW *B; - }; - - Body3DSW *_arr[2] = { nullptr, nullptr }; - }; - - //! relative_frames - //!@{ - Transform3D m_frameInA; //!< the constraint space w.r.t body A - Transform3D m_frameInB; //!< the constraint space w.r.t body B - //!@} - - //! Jacobians - //!@{ - JacobianEntry3DSW m_jacLinear[3]; //!< 3 orthogonal linear constraints - JacobianEntry3DSW m_jacAng[3]; //!< 3 orthogonal angular constraints - //!@} - - //! Linear_Limit_parameters - //!@{ - G6DOFTranslationalLimitMotor3DSW m_linearLimits; - //!@} - - //! hinge_parameters - //!@{ - G6DOFRotationalLimitMotor3DSW m_angularLimits[3]; - //!@} - -protected: - //! temporal variables - //!@{ - real_t m_timeStep = 0.0; - Transform3D m_calculatedTransformA; - Transform3D m_calculatedTransformB; - Vector3 m_calculatedAxisAngleDiff; - Vector3 m_calculatedAxis[3]; - - Vector3 m_AnchorPos; // point between pivots of bodies A and B to solve linear axes - - bool m_useLinearReferenceFrameA = false; - - //!@} - - Generic6DOFJoint3DSW(Generic6DOFJoint3DSW const &) = delete; - void operator=(Generic6DOFJoint3DSW const &) = delete; - - void buildLinearJacobian( - JacobianEntry3DSW &jacLinear, const Vector3 &normalWorld, - const Vector3 &pivotAInW, const Vector3 &pivotBInW); - - void buildAngularJacobian(JacobianEntry3DSW &jacAngular, const Vector3 &jointAxisW); - - //! calcs the euler angles between the two bodies. - void calculateAngleInfo(); - -public: - Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA); - - virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_6DOF; } - - virtual bool setup(real_t p_step) override; - virtual void solve(real_t p_step) override; - - //! Calcs global transform of the offsets - /*! - Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies. - \sa Generic6DOFJointSW.getCalculatedTransformA , Generic6DOFJointSW.getCalculatedTransformB, Generic6DOFJointSW.calculateAngleInfo - */ - void calculateTransforms(); - - //! Gets the global transform of the offset for body A - /*! - \sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo. - */ - const Transform3D &getCalculatedTransformA() const { - return m_calculatedTransformA; - } - - //! Gets the global transform of the offset for body B - /*! - \sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo. - */ - const Transform3D &getCalculatedTransformB() const { - return m_calculatedTransformB; - } - - const Transform3D &getFrameOffsetA() const { - return m_frameInA; - } - - const Transform3D &getFrameOffsetB() const { - return m_frameInB; - } - - Transform3D &getFrameOffsetA() { - return m_frameInA; - } - - Transform3D &getFrameOffsetB() { - return m_frameInB; - } - - //! performs Jacobian calculation, and also calculates angle differences and axis - - void updateRHS(real_t timeStep); - - //! Get the rotation axis in global coordinates - /*! - \pre Generic6DOFJointSW.buildJacobian must be called previously. - */ - Vector3 getAxis(int axis_index) const; - - //! Get the relative Euler angle - /*! - \pre Generic6DOFJointSW.buildJacobian must be called previously. - */ - real_t getAngle(int axis_index) const; - - //! Test angular limit. - /*! - Calculates angular correction and returns true if limit needs to be corrected. - \pre Generic6DOFJointSW.buildJacobian must be called previously. - */ - bool testAngularLimitMotor(int axis_index); - - void setLinearLowerLimit(const Vector3 &linearLower) { - m_linearLimits.m_lowerLimit = linearLower; - } - - void setLinearUpperLimit(const Vector3 &linearUpper) { - m_linearLimits.m_upperLimit = linearUpper; - } - - void setAngularLowerLimit(const Vector3 &angularLower) { - m_angularLimits[0].m_loLimit = angularLower.x; - m_angularLimits[1].m_loLimit = angularLower.y; - m_angularLimits[2].m_loLimit = angularLower.z; - } - - void setAngularUpperLimit(const Vector3 &angularUpper) { - m_angularLimits[0].m_hiLimit = angularUpper.x; - m_angularLimits[1].m_hiLimit = angularUpper.y; - m_angularLimits[2].m_hiLimit = angularUpper.z; - } - - //! Retrieves the angular limit information. - G6DOFRotationalLimitMotor3DSW *getRotationalLimitMotor(int index) { - return &m_angularLimits[index]; - } - - //! Retrieves the limit information. - G6DOFTranslationalLimitMotor3DSW *getTranslationalLimitMotor() { - return &m_linearLimits; - } - - //first 3 are linear, next 3 are angular - void setLimit(int axis, real_t lo, real_t hi) { - if (axis < 3) { - m_linearLimits.m_lowerLimit[axis] = lo; - m_linearLimits.m_upperLimit[axis] = hi; - } else { - m_angularLimits[axis - 3].m_loLimit = lo; - m_angularLimits[axis - 3].m_hiLimit = hi; - } - } - - //! Test limit - /*! - - free means upper < lower, - - locked means upper == lower - - limited means upper > lower - - limitIndex: first 3 are linear, next 3 are angular - */ - bool isLimited(int limitIndex) { - if (limitIndex < 3) { - return m_linearLimits.isLimited(limitIndex); - } - return m_angularLimits[limitIndex - 3].isLimited(); - } - - const Body3DSW *getRigidBodyA() const { - return A; - } - const Body3DSW *getRigidBodyB() const { - return B; - } - - virtual void calcAnchorPos(); // overridable - - void set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value); - real_t get_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const; - - void set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_value); - bool get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const; -}; - -#endif // GENERIC_6DOF_JOINT_SW_H diff --git a/servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp b/servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp new file mode 100644 index 0000000000..31a87fc595 --- /dev/null +++ b/servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp @@ -0,0 +1,360 @@ +/*************************************************************************/ +/* godot_cone_twist_joint_3d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +/* +Adapted to Godot from the Bullet library. +*/ + +/* +Bullet Continuous Collision Detection and Physics Library +ConeTwistJointSW is Copyright (c) 2007 Starbreeze Studios + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. + +Written by: Marcus Hennix +*/ + +#include "godot_cone_twist_joint_3d.h" + +static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) { + if (Math::abs(n.z) > Math_SQRT12) { + // choose p in y-z plane + real_t a = n[1] * n[1] + n[2] * n[2]; + real_t k = 1.0 / Math::sqrt(a); + p = Vector3(0, -n[2] * k, n[1] * k); + // set q = n x p + q = Vector3(a * k, -n[0] * p[2], n[0] * p[1]); + } else { + // choose p in x-y plane + real_t a = n.x * n.x + n.y * n.y; + real_t k = 1.0 / Math::sqrt(a); + p = Vector3(-n.y * k, n.x * k, 0); + // set q = n x p + q = Vector3(-n.z * p.y, n.z * p.x, a * k); + } +} + +static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { + real_t coeff_1 = Math_PI / 4.0f; + real_t coeff_2 = 3.0f * coeff_1; + real_t abs_y = Math::abs(y); + real_t angle; + if (x >= 0.0f) { + real_t r = (x - abs_y) / (x + abs_y); + angle = coeff_1 - coeff_1 * r; + } else { + real_t r = (x + abs_y) / (abs_y - x); + angle = coeff_2 - coeff_1 * r; + } + return (y < 0.0f) ? -angle : angle; +} + +GodotConeTwistJoint3D::GodotConeTwistJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame) : + GodotJoint3D(_arr, 2) { + A = rbA; + B = rbB; + + m_rbAFrame = rbAFrame; + m_rbBFrame = rbBFrame; + + A->add_constraint(this, 0); + B->add_constraint(this, 1); +} + +bool GodotConeTwistJoint3D::setup(real_t p_timestep) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + + m_appliedImpulse = real_t(0.); + + //set bias, sign, clear accumulator + m_swingCorrection = real_t(0.); + m_twistLimitSign = real_t(0.); + m_solveTwistLimit = false; + m_solveSwingLimit = false; + m_accTwistLimitImpulse = real_t(0.); + m_accSwingLimitImpulse = real_t(0.); + + if (!m_angularOnly) { + Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); + Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); + Vector3 relPos = pivotBInW - pivotAInW; + + Vector3 normal[3]; + if (Math::is_zero_approx(relPos.length_squared())) { + normal[0] = Vector3(real_t(1.0), 0, 0); + } else { + normal[0] = relPos.normalized(); + } + + plane_space(normal[0], normal[1], normal[2]); + + for (int i = 0; i < 3; i++) { + memnew_placement(&m_jac[i], GodotJacobianEntry3D( + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + pivotAInW - A->get_transform().origin - A->get_center_of_mass(), + pivotBInW - B->get_transform().origin - B->get_center_of_mass(), + normal[i], + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass())); + } + } + + Vector3 b1Axis1, b1Axis2, b1Axis3; + Vector3 b2Axis1, b2Axis2; + + b1Axis1 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(0)); + b2Axis1 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(0)); + + real_t swing1 = real_t(0.), swing2 = real_t(0.); + + real_t swx = real_t(0.), swy = real_t(0.); + real_t thresh = real_t(10.); + real_t fact; + + // Get Frame into world space + if (m_swingSpan1 >= real_t(0.05f)) { + b1Axis2 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(1)); + //swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) ); + swx = b2Axis1.dot(b1Axis1); + swy = b2Axis1.dot(b1Axis2); + swing1 = atan2fast(swy, swx); + fact = (swy * swy + swx * swx) * thresh * thresh; + fact = fact / (fact + real_t(1.0)); + swing1 *= fact; + } + + if (m_swingSpan2 >= real_t(0.05f)) { + b1Axis3 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(2)); + //swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) ); + swx = b2Axis1.dot(b1Axis1); + swy = b2Axis1.dot(b1Axis3); + swing2 = atan2fast(swy, swx); + fact = (swy * swy + swx * swx) * thresh * thresh; + fact = fact / (fact + real_t(1.0)); + swing2 *= fact; + } + + real_t RMaxAngle1Sq = 1.0f / (m_swingSpan1 * m_swingSpan1); + real_t RMaxAngle2Sq = 1.0f / (m_swingSpan2 * m_swingSpan2); + real_t EllipseAngle = Math::abs(swing1 * swing1) * RMaxAngle1Sq + Math::abs(swing2 * swing2) * RMaxAngle2Sq; + + if (EllipseAngle > 1.0f) { + m_swingCorrection = EllipseAngle - 1.0f; + m_solveSwingLimit = true; + + // Calculate necessary axis & factors + m_swingAxis = b2Axis1.cross(b1Axis2 * b2Axis1.dot(b1Axis2) + b1Axis3 * b2Axis1.dot(b1Axis3)); + m_swingAxis.normalize(); + + real_t swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; + m_swingAxis *= swingAxisSign; + + m_kSwing = real_t(1.) / (A->compute_angular_impulse_denominator(m_swingAxis) + + B->compute_angular_impulse_denominator(m_swingAxis)); + } + + // Twist limits + if (m_twistSpan >= real_t(0.)) { + Vector3 b2Axis22 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(1)); + Quaternion rotationArc = Quaternion(b2Axis1, b1Axis1); + Vector3 TwistRef = rotationArc.xform(b2Axis22); + real_t twist = atan2fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2)); + + real_t lockedFreeFactor = (m_twistSpan > real_t(0.05f)) ? m_limitSoftness : real_t(0.); + if (twist <= -m_twistSpan * lockedFreeFactor) { + m_twistCorrection = -(twist + m_twistSpan); + m_solveTwistLimit = true; + + m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; + m_twistAxis.normalize(); + m_twistAxis *= -1.0f; + + m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + + B->compute_angular_impulse_denominator(m_twistAxis)); + + } else if (twist > m_twistSpan * lockedFreeFactor) { + m_twistCorrection = (twist - m_twistSpan); + m_solveTwistLimit = true; + + m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; + m_twistAxis.normalize(); + + m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + + B->compute_angular_impulse_denominator(m_twistAxis)); + } + } + + return true; +} + +void GodotConeTwistJoint3D::solve(real_t p_timestep) { + Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); + Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); + + real_t tau = real_t(0.3); + + //linear part + if (!m_angularOnly) { + Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; + Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; + + Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1); + Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2); + Vector3 vel = vel1 - vel2; + + for (int i = 0; i < 3; i++) { + const Vector3 &normal = m_jac[i].m_linearJointAxis; + real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal(); + + real_t rel_vel; + rel_vel = normal.dot(vel); + //positional error (zeroth order error) + real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + real_t impulse = depth * tau / p_timestep * jacDiagABInv - rel_vel * jacDiagABInv; + m_appliedImpulse += impulse; + Vector3 impulse_vector = normal * impulse; + if (dynamic_A) { + A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + } + } + } + + { + ///solve angular part + const Vector3 &angVelA = A->get_angular_velocity(); + const Vector3 &angVelB = B->get_angular_velocity(); + + // solve swing limit + if (m_solveSwingLimit) { + real_t amplitude = ((angVelB - angVelA).dot(m_swingAxis) * m_relaxationFactor * m_relaxationFactor + m_swingCorrection * (real_t(1.) / p_timestep) * m_biasFactor); + real_t impulseMag = amplitude * m_kSwing; + + // Clamp the accumulated impulse + real_t temp = m_accSwingLimitImpulse; + m_accSwingLimitImpulse = MAX(m_accSwingLimitImpulse + impulseMag, real_t(0.0)); + impulseMag = m_accSwingLimitImpulse - temp; + + Vector3 impulse = m_swingAxis * impulseMag; + + if (dynamic_A) { + A->apply_torque_impulse(impulse); + } + if (dynamic_B) { + B->apply_torque_impulse(-impulse); + } + } + + // solve twist limit + if (m_solveTwistLimit) { + real_t amplitude = ((angVelB - angVelA).dot(m_twistAxis) * m_relaxationFactor * m_relaxationFactor + m_twistCorrection * (real_t(1.) / p_timestep) * m_biasFactor); + real_t impulseMag = amplitude * m_kTwist; + + // Clamp the accumulated impulse + real_t temp = m_accTwistLimitImpulse; + m_accTwistLimitImpulse = MAX(m_accTwistLimitImpulse + impulseMag, real_t(0.0)); + impulseMag = m_accTwistLimitImpulse - temp; + + Vector3 impulse = m_twistAxis * impulseMag; + + if (dynamic_A) { + A->apply_torque_impulse(impulse); + } + if (dynamic_B) { + B->apply_torque_impulse(-impulse); + } + } + } +} + +void GodotConeTwistJoint3D::set_param(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value) { + switch (p_param) { + case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: { + m_swingSpan1 = p_value; + m_swingSpan2 = p_value; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_TWIST_SPAN: { + m_twistSpan = p_value; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_BIAS: { + m_biasFactor = p_value; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_SOFTNESS: { + m_limitSoftness = p_value; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_RELAXATION: { + m_relaxationFactor = p_value; + } break; + case PhysicsServer3D::CONE_TWIST_MAX: + break; // Can't happen, but silences warning + } +} + +real_t GodotConeTwistJoint3D::get_param(PhysicsServer3D::ConeTwistJointParam p_param) const { + switch (p_param) { + case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: { + return m_swingSpan1; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_TWIST_SPAN: { + return m_twistSpan; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_BIAS: { + return m_biasFactor; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_SOFTNESS: { + return m_limitSoftness; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_RELAXATION: { + return m_relaxationFactor; + } break; + case PhysicsServer3D::CONE_TWIST_MAX: + break; // Can't happen, but silences warning + } + + return 0; +} diff --git a/servers/physics_3d/joints/godot_cone_twist_joint_3d.h b/servers/physics_3d/joints/godot_cone_twist_joint_3d.h new file mode 100644 index 0000000000..999d0f0692 --- /dev/null +++ b/servers/physics_3d/joints/godot_cone_twist_joint_3d.h @@ -0,0 +1,142 @@ +/*************************************************************************/ +/* godot_cone_twist_joint_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +/* +Adapted to Godot from the Bullet library. +*/ + +/* +Bullet Continuous Collision Detection and Physics Library +GodotConeTwistJoint3D is Copyright (c) 2007 Starbreeze Studios + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. + +Written by: Marcus Hennix +*/ + +#ifndef GODOT_CONE_TWIST_JOINT_3D_H +#define GODOT_CONE_TWIST_JOINT_3D_H + +#include "servers/physics_3d/godot_joint_3d.h" +#include "servers/physics_3d/joints/godot_jacobian_entry_3d.h" + +// GodotConeTwistJoint3D can be used to simulate ragdoll joints (upper arm, leg etc). +class GodotConeTwistJoint3D : public GodotJoint3D { +#ifdef IN_PARALLELL_SOLVER +public: +#endif + + union { + struct { + GodotBody3D *A; + GodotBody3D *B; + }; + + GodotBody3D *_arr[2] = { nullptr, nullptr }; + }; + + GodotJacobianEntry3D m_jac[3] = {}; //3 orthogonal linear constraints + + real_t m_appliedImpulse = 0.0; + Transform3D m_rbAFrame; + Transform3D m_rbBFrame; + + real_t m_limitSoftness = 0.0; + real_t m_biasFactor = 0.3; + real_t m_relaxationFactor = 1.0; + + real_t m_swingSpan1 = Math_TAU / 8.0; + real_t m_swingSpan2 = 0.0; + real_t m_twistSpan = 0.0; + + Vector3 m_swingAxis; + Vector3 m_twistAxis; + + real_t m_kSwing = 0.0; + real_t m_kTwist = 0.0; + + real_t m_twistLimitSign = 0.0; + real_t m_swingCorrection = 0.0; + real_t m_twistCorrection = 0.0; + + real_t m_accSwingLimitImpulse = 0.0; + real_t m_accTwistLimitImpulse = 0.0; + + bool m_angularOnly = false; + bool m_solveTwistLimit = false; + bool m_solveSwingLimit = false; + +public: + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; } + + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; + + GodotConeTwistJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame); + + void setAngularOnly(bool angularOnly) { + m_angularOnly = angularOnly; + } + + void setLimit(real_t _swingSpan1, real_t _swingSpan2, real_t _twistSpan, real_t _softness = 0.8f, real_t _biasFactor = 0.3f, real_t _relaxationFactor = 1.0f) { + m_swingSpan1 = _swingSpan1; + m_swingSpan2 = _swingSpan2; + m_twistSpan = _twistSpan; + + m_limitSoftness = _softness; + m_biasFactor = _biasFactor; + m_relaxationFactor = _relaxationFactor; + } + + inline int getSolveTwistLimit() { + return m_solveTwistLimit; + } + + inline int getSolveSwingLimit() { + return m_solveTwistLimit; + } + + inline real_t getTwistLimitSign() { + return m_twistLimitSign; + } + + void set_param(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value); + real_t get_param(PhysicsServer3D::ConeTwistJointParam p_param) const; +}; + +#endif // GODOT_CONE_TWIST_JOINT_3D_H diff --git a/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp new file mode 100644 index 0000000000..d7e0537439 --- /dev/null +++ b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp @@ -0,0 +1,670 @@ +/*************************************************************************/ +/* godot_generic_6dof_joint_3d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +/* +Adapted to Godot from the Bullet library. +*/ + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +2007-09-09 +GodotGeneric6DOFJoint3D Refactored by Francisco Le?n +email: projectileman@yahoo.com +http://gimpact.sf.net +*/ + +#include "godot_generic_6dof_joint_3d.h" + +#define GENERIC_D6_DISABLE_WARMSTARTING 1 + +//////////////////////////// GodotG6DOFRotationalLimitMotor3D //////////////////////////////////// + +int GodotG6DOFRotationalLimitMotor3D::testLimitValue(real_t test_value) { + if (m_loLimit > m_hiLimit) { + m_currentLimit = 0; //Free from violation + return 0; + } + + if (test_value < m_loLimit) { + m_currentLimit = 1; //low limit violation + m_currentLimitError = test_value - m_loLimit; + return 1; + } else if (test_value > m_hiLimit) { + m_currentLimit = 2; //High limit violation + m_currentLimitError = test_value - m_hiLimit; + return 2; + }; + + m_currentLimit = 0; //Free from violation + return 0; +} + +real_t GodotG6DOFRotationalLimitMotor3D::solveAngularLimits( + real_t timeStep, Vector3 &axis, real_t jacDiagABInv, + GodotBody3D *body0, GodotBody3D *body1, bool p_body0_dynamic, bool p_body1_dynamic) { + if (!needApplyTorques()) { + return 0.0f; + } + + real_t target_velocity = m_targetVelocity; + real_t maxMotorForce = m_maxMotorForce; + + //current error correction + if (m_currentLimit != 0) { + target_velocity = -m_ERP * m_currentLimitError / (timeStep); + maxMotorForce = m_maxLimitForce; + } + + maxMotorForce *= timeStep; + + // current velocity difference + Vector3 vel_diff = body0->get_angular_velocity(); + if (body1) { + vel_diff -= body1->get_angular_velocity(); + } + + real_t rel_vel = axis.dot(vel_diff); + + // correction velocity + real_t motor_relvel = m_limitSoftness * (target_velocity - m_damping * rel_vel); + + if (Math::is_zero_approx(motor_relvel)) { + return 0.0f; //no need for applying force + } + + // correction impulse + real_t unclippedMotorImpulse = (1 + m_bounce) * motor_relvel * jacDiagABInv; + + // clip correction impulse + real_t clippedMotorImpulse; + + ///@todo: should clip against accumulated impulse + if (unclippedMotorImpulse > 0.0f) { + clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse; + } else { + clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse; + } + + // sort with accumulated impulses + real_t lo = real_t(-1e30); + real_t hi = real_t(1e30); + + real_t oldaccumImpulse = m_accumulatedImpulse; + real_t sum = oldaccumImpulse + clippedMotorImpulse; + m_accumulatedImpulse = sum > hi ? real_t(0.) : (sum < lo ? real_t(0.) : sum); + + clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; + + Vector3 motorImp = clippedMotorImpulse * axis; + + if (p_body0_dynamic) { + body0->apply_torque_impulse(motorImp); + } + if (body1 && p_body1_dynamic) { + body1->apply_torque_impulse(-motorImp); + } + + return clippedMotorImpulse; +} + +//////////////////////////// GodotG6DOFTranslationalLimitMotor3D //////////////////////////////////// + +real_t GodotG6DOFTranslationalLimitMotor3D::solveLinearAxis( + real_t timeStep, + real_t jacDiagABInv, + GodotBody3D *body1, const Vector3 &pointInA, + GodotBody3D *body2, const Vector3 &pointInB, + bool p_body1_dynamic, bool p_body2_dynamic, + int limit_index, + const Vector3 &axis_normal_on_a, + const Vector3 &anchorPos) { + ///find relative velocity + // Vector3 rel_pos1 = pointInA - body1->get_transform().origin; + // Vector3 rel_pos2 = pointInB - body2->get_transform().origin; + Vector3 rel_pos1 = anchorPos - body1->get_transform().origin; + Vector3 rel_pos2 = anchorPos - body2->get_transform().origin; + + Vector3 vel1 = body1->get_velocity_in_local_point(rel_pos1); + Vector3 vel2 = body2->get_velocity_in_local_point(rel_pos2); + Vector3 vel = vel1 - vel2; + + real_t rel_vel = axis_normal_on_a.dot(vel); + + /// apply displacement correction + + //positional error (zeroth order error) + real_t depth = -(pointInA - pointInB).dot(axis_normal_on_a); + real_t lo = real_t(-1e30); + real_t hi = real_t(1e30); + + real_t minLimit = m_lowerLimit[limit_index]; + real_t maxLimit = m_upperLimit[limit_index]; + + //handle the limits + if (minLimit < maxLimit) { + { + if (depth > maxLimit) { + depth -= maxLimit; + lo = real_t(0.); + + } else { + if (depth < minLimit) { + depth -= minLimit; + hi = real_t(0.); + } else { + return 0.0f; + } + } + } + } + + real_t normalImpulse = m_limitSoftness[limit_index] * (m_restitution[limit_index] * depth / timeStep - m_damping[limit_index] * rel_vel) * jacDiagABInv; + + real_t oldNormalImpulse = m_accumulatedImpulse[limit_index]; + real_t sum = oldNormalImpulse + normalImpulse; + m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : (sum < lo ? real_t(0.) : sum); + normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; + + Vector3 impulse_vector = axis_normal_on_a * normalImpulse; + if (p_body1_dynamic) { + body1->apply_impulse(impulse_vector, rel_pos1); + } + if (p_body2_dynamic) { + body2->apply_impulse(-impulse_vector, rel_pos2); + } + return normalImpulse; +} + +//////////////////////////// GodotGeneric6DOFJoint3D //////////////////////////////////// + +GodotGeneric6DOFJoint3D::GodotGeneric6DOFJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA) : + GodotJoint3D(_arr, 2), + m_frameInA(frameInA), + m_frameInB(frameInB), + m_useLinearReferenceFrameA(useLinearReferenceFrameA) { + A = rbA; + B = rbB; + A->add_constraint(this, 0); + B->add_constraint(this, 1); +} + +void GodotGeneric6DOFJoint3D::calculateAngleInfo() { + Basis relative_frame = m_calculatedTransformB.basis.inverse() * m_calculatedTransformA.basis; + + m_calculatedAxisAngleDiff = relative_frame.get_euler_xyz(); + + // in euler angle mode we do not actually constrain the angular velocity + // along the axes axis[0] and axis[2] (although we do use axis[1]) : + // + // to get constrain w2-w1 along ...not + // ------ --------------------- ------ + // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] + // d(angle[1])/dt = 0 ax[1] + // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] + // + // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. + // to prove the result for angle[0], write the expression for angle[0] from + // GetInfo1 then take the derivative. to prove this for angle[2] it is + // easier to take the euler rate expression for d(angle[2])/dt with respect + // to the components of w and set that to 0. + + Vector3 axis0 = m_calculatedTransformB.basis.get_axis(0); + Vector3 axis2 = m_calculatedTransformA.basis.get_axis(2); + + m_calculatedAxis[1] = axis2.cross(axis0); + m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); + m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); + + /* + if(m_debugDrawer) + { + char buff[300]; + sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ", + m_calculatedAxisAngleDiff[0], + m_calculatedAxisAngleDiff[1], + m_calculatedAxisAngleDiff[2]); + m_debugDrawer->reportErrorWarning(buff); + } + */ +} + +void GodotGeneric6DOFJoint3D::calculateTransforms() { + m_calculatedTransformA = A->get_transform() * m_frameInA; + m_calculatedTransformB = B->get_transform() * m_frameInB; + + calculateAngleInfo(); +} + +void GodotGeneric6DOFJoint3D::buildLinearJacobian( + GodotJacobianEntry3D &jacLinear, const Vector3 &normalWorld, + const Vector3 &pivotAInW, const Vector3 &pivotBInW) { + memnew_placement(&jacLinear, GodotJacobianEntry3D( + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + pivotAInW - A->get_transform().origin - A->get_center_of_mass(), + pivotBInW - B->get_transform().origin - B->get_center_of_mass(), + normalWorld, + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass())); +} + +void GodotGeneric6DOFJoint3D::buildAngularJacobian( + GodotJacobianEntry3D &jacAngular, const Vector3 &jointAxisW) { + memnew_placement(&jacAngular, GodotJacobianEntry3D(jointAxisW, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); +} + +bool GodotGeneric6DOFJoint3D::testAngularLimitMotor(int axis_index) { + real_t angle = m_calculatedAxisAngleDiff[axis_index]; + + //test limits + m_angularLimits[axis_index].testLimitValue(angle); + return m_angularLimits[axis_index].needApplyTorques(); +} + +bool GodotGeneric6DOFJoint3D::setup(real_t p_timestep) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + + // Clear accumulated impulses for the next simulation step + m_linearLimits.m_accumulatedImpulse = Vector3(real_t(0.), real_t(0.), real_t(0.)); + int i; + for (i = 0; i < 3; i++) { + m_angularLimits[i].m_accumulatedImpulse = real_t(0.); + } + //calculates transform + calculateTransforms(); + + // const Vector3& pivotAInW = m_calculatedTransformA.origin; + // const Vector3& pivotBInW = m_calculatedTransformB.origin; + calcAnchorPos(); + Vector3 pivotAInW = m_AnchorPos; + Vector3 pivotBInW = m_AnchorPos; + + // not used here + // Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; + // Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; + + Vector3 normalWorld; + //linear part + for (i = 0; i < 3; i++) { + if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) { + if (m_useLinearReferenceFrameA) { + normalWorld = m_calculatedTransformA.basis.get_axis(i); + } else { + normalWorld = m_calculatedTransformB.basis.get_axis(i); + } + + buildLinearJacobian( + m_jacLinear[i], normalWorld, + pivotAInW, pivotBInW); + } + } + + // angular part + for (i = 0; i < 3; i++) { + //calculates error angle + if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i)) { + normalWorld = this->getAxis(i); + // Create angular atom + buildAngularJacobian(m_jacAng[i], normalWorld); + } + } + + return true; +} + +void GodotGeneric6DOFJoint3D::solve(real_t p_timestep) { + m_timeStep = p_timestep; + + //calculateTransforms(); + + int i; + + // linear + + Vector3 pointInA = m_calculatedTransformA.origin; + Vector3 pointInB = m_calculatedTransformB.origin; + + real_t jacDiagABInv; + Vector3 linear_axis; + for (i = 0; i < 3; i++) { + if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) { + jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal(); + + if (m_useLinearReferenceFrameA) { + linear_axis = m_calculatedTransformA.basis.get_axis(i); + } else { + linear_axis = m_calculatedTransformB.basis.get_axis(i); + } + + m_linearLimits.solveLinearAxis( + m_timeStep, + jacDiagABInv, + A, pointInA, + B, pointInB, + dynamic_A, dynamic_B, + i, linear_axis, m_AnchorPos); + } + } + + // angular + Vector3 angular_axis; + real_t angularJacDiagABInv; + for (i = 0; i < 3; i++) { + if (m_angularLimits[i].m_enableLimit && m_angularLimits[i].needApplyTorques()) { + // get axis + angular_axis = getAxis(i); + + angularJacDiagABInv = real_t(1.) / m_jacAng[i].getDiagonal(); + + m_angularLimits[i].solveAngularLimits(m_timeStep, angular_axis, angularJacDiagABInv, A, B, dynamic_A, dynamic_B); + } + } +} + +void GodotGeneric6DOFJoint3D::updateRHS(real_t timeStep) { + (void)timeStep; +} + +Vector3 GodotGeneric6DOFJoint3D::getAxis(int axis_index) const { + return m_calculatedAxis[axis_index]; +} + +real_t GodotGeneric6DOFJoint3D::getAngle(int axis_index) const { + return m_calculatedAxisAngleDiff[axis_index]; +} + +void GodotGeneric6DOFJoint3D::calcAnchorPos() { + real_t imA = A->get_inv_mass(); + real_t imB = B->get_inv_mass(); + real_t weight; + if (imB == real_t(0.0)) { + weight = real_t(1.0); + } else { + weight = imA / (imA + imB); + } + const Vector3 &pA = m_calculatedTransformA.origin; + const Vector3 &pB = m_calculatedTransformB.origin; + m_AnchorPos = pA * weight + pB * (real_t(1.0) - weight); +} + +void GodotGeneric6DOFJoint3D::set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value) { + ERR_FAIL_INDEX(p_axis, 3); + switch (p_param) { + case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { + m_linearLimits.m_lowerLimit[p_axis] = p_value; + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT: { + m_linearLimits.m_upperLimit[p_axis] = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: { + m_linearLimits.m_limitSoftness[p_axis] = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_RESTITUTION: { + m_linearLimits.m_restitution[p_axis] = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_DAMPING: { + m_linearLimits.m_damping[p_axis] = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: { + m_angularLimits[p_axis].m_loLimit = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: { + m_angularLimits[p_axis].m_hiLimit = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: { + m_angularLimits[p_axis].m_limitSoftness = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_DAMPING: { + m_angularLimits[p_axis].m_damping = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_RESTITUTION: { + m_angularLimits[p_axis].m_bounce = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: { + m_angularLimits[p_axis].m_maxLimitForce = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_ERP: { + m_angularLimits[p_axis].m_ERP = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: { + m_angularLimits[p_axis].m_targetVelocity = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: { + m_angularLimits[p_axis].m_maxLimitForce = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_DAMPING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_MAX: + break; // Can't happen, but silences warning + } +} + +real_t GodotGeneric6DOFJoint3D::get_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const { + ERR_FAIL_INDEX_V(p_axis, 3, 0); + switch (p_param) { + case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { + return m_linearLimits.m_lowerLimit[p_axis]; + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT: { + return m_linearLimits.m_upperLimit[p_axis]; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: { + return m_linearLimits.m_limitSoftness[p_axis]; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_RESTITUTION: { + return m_linearLimits.m_restitution[p_axis]; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_DAMPING: { + return m_linearLimits.m_damping[p_axis]; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: { + return m_angularLimits[p_axis].m_loLimit; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: { + return m_angularLimits[p_axis].m_hiLimit; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: { + return m_angularLimits[p_axis].m_limitSoftness; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_DAMPING: { + return m_angularLimits[p_axis].m_damping; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_RESTITUTION: { + return m_angularLimits[p_axis].m_bounce; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: { + return m_angularLimits[p_axis].m_maxLimitForce; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_ERP: { + return m_angularLimits[p_axis].m_ERP; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: { + return m_angularLimits[p_axis].m_targetVelocity; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: { + return m_angularLimits[p_axis].m_maxMotorForce; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_DAMPING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_MAX: + break; // Can't happen, but silences warning + } + return 0; +} + +void GodotGeneric6DOFJoint3D::set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_value) { + ERR_FAIL_INDEX(p_axis, 3); + + switch (p_flag) { + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: { + m_linearLimits.enable_limit[p_axis] = p_value; + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: { + m_angularLimits[p_axis].m_enableLimit = p_value; + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR: { + m_angularLimits[p_axis].m_enableMotor = p_value; + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_MAX: + break; // Can't happen, but silences warning + } +} + +bool GodotGeneric6DOFJoint3D::get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const { + ERR_FAIL_INDEX_V(p_axis, 3, 0); + switch (p_flag) { + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: { + return m_linearLimits.enable_limit[p_axis]; + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: { + return m_angularLimits[p_axis].m_enableLimit; + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR: { + return m_angularLimits[p_axis].m_enableMotor; + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_MAX: + break; // Can't happen, but silences warning + } + + return false; +} diff --git a/servers/physics_3d/joints/godot_generic_6dof_joint_3d.h b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.h new file mode 100644 index 0000000000..729b3fa1f9 --- /dev/null +++ b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.h @@ -0,0 +1,322 @@ +/*************************************************************************/ +/* godot_generic_6dof_joint_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +/* +Adapted to Godot from the Bullet library. +*/ + +#ifndef GODOT_GENERIC_6DOF_JOINT_3D_H +#define GODOT_GENERIC_6DOF_JOINT_3D_H + +#include "servers/physics_3d/godot_joint_3d.h" +#include "servers/physics_3d/joints/godot_jacobian_entry_3d.h" + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +2007-09-09 +GodotGeneric6DOFJoint3D Refactored by Francisco Le?n +email: projectileman@yahoo.com +http://gimpact.sf.net +*/ + +//! Rotation Limit structure for generic joints +class GodotG6DOFRotationalLimitMotor3D { +public: + //! limit_parameters + //!@{ + real_t m_loLimit = -1e30; //!< joint limit + real_t m_hiLimit = 1e30; //!< joint limit + real_t m_targetVelocity = 0.0; //!< target motor velocity + real_t m_maxMotorForce = 0.1; //!< max force on motor + real_t m_maxLimitForce = 300.0; //!< max force on limit + real_t m_damping = 1.0; //!< Damping. + real_t m_limitSoftness = 0.5; //! Relaxation factor + real_t m_ERP = 0.5; //!< Error tolerance factor when joint is at limit + real_t m_bounce = 0.0; //!< restitution factor + bool m_enableMotor = false; + bool m_enableLimit = false; + + //!@} + + //! temp_variables + //!@{ + real_t m_currentLimitError = 0.0; //!< How much is violated this limit + int m_currentLimit = 0; //!< 0=free, 1=at lo limit, 2=at hi limit + real_t m_accumulatedImpulse = 0.0; + //!@} + + GodotG6DOFRotationalLimitMotor3D() {} + + bool isLimited() { + return (m_loLimit < m_hiLimit); + } + + // Need apply correction. + bool needApplyTorques() { + return (m_enableMotor || m_currentLimit != 0); + } + + // Calculates m_currentLimit and m_currentLimitError. + int testLimitValue(real_t test_value); + + // Apply the correction impulses for two bodies. + real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, GodotBody3D *body0, GodotBody3D *body1, bool p_body0_dynamic, bool p_body1_dynamic); +}; + +class GodotG6DOFTranslationalLimitMotor3D { +public: + Vector3 m_lowerLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint lower limits + Vector3 m_upperLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint upper limits + Vector3 m_accumulatedImpulse = Vector3(0.0, 0.0, 0.0); + //! Linear_Limit_parameters + //!@{ + Vector3 m_limitSoftness = Vector3(0.7, 0.7, 0.7); //!< Softness for linear limit + Vector3 m_damping = Vector3(1.0, 1.0, 1.0); //!< Damping for linear limit + Vector3 m_restitution = Vector3(0.5, 0.5, 0.5); //! Bounce parameter for linear limit + //!@} + bool enable_limit[3] = { true, true, true }; + + //! Test limit + /*! + - free means upper < lower, + - locked means upper == lower + - limited means upper > lower + - limitIndex: first 3 are linear, next 3 are angular + */ + inline bool isLimited(int limitIndex) { + return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); + } + + real_t solveLinearAxis( + real_t timeStep, + real_t jacDiagABInv, + GodotBody3D *body1, const Vector3 &pointInA, + GodotBody3D *body2, const Vector3 &pointInB, + bool p_body1_dynamic, bool p_body2_dynamic, + int limit_index, + const Vector3 &axis_normal_on_a, + const Vector3 &anchorPos); +}; + +class GodotGeneric6DOFJoint3D : public GodotJoint3D { +protected: + union { + struct { + GodotBody3D *A; + GodotBody3D *B; + }; + + GodotBody3D *_arr[2] = { nullptr, nullptr }; + }; + + //! relative_frames + //!@{ + Transform3D m_frameInA; //!< the constraint space w.r.t body A + Transform3D m_frameInB; //!< the constraint space w.r.t body B + //!@} + + //! Jacobians + //!@{ + GodotJacobianEntry3D m_jacLinear[3]; //!< 3 orthogonal linear constraints + GodotJacobianEntry3D m_jacAng[3]; //!< 3 orthogonal angular constraints + //!@} + + //! Linear_Limit_parameters + //!@{ + GodotG6DOFTranslationalLimitMotor3D m_linearLimits; + //!@} + + //! hinge_parameters + //!@{ + GodotG6DOFRotationalLimitMotor3D m_angularLimits[3]; + //!@} + +protected: + //! temporal variables + //!@{ + real_t m_timeStep = 0.0; + Transform3D m_calculatedTransformA; + Transform3D m_calculatedTransformB; + Vector3 m_calculatedAxisAngleDiff; + Vector3 m_calculatedAxis[3]; + + Vector3 m_AnchorPos; // point between pivots of bodies A and B to solve linear axes + + bool m_useLinearReferenceFrameA = false; + + //!@} + + GodotGeneric6DOFJoint3D(GodotGeneric6DOFJoint3D const &) = delete; + void operator=(GodotGeneric6DOFJoint3D const &) = delete; + + void buildLinearJacobian( + GodotJacobianEntry3D &jacLinear, const Vector3 &normalWorld, + const Vector3 &pivotAInW, const Vector3 &pivotBInW); + + void buildAngularJacobian(GodotJacobianEntry3D &jacAngular, const Vector3 &jointAxisW); + + //! calcs the euler angles between the two bodies. + void calculateAngleInfo(); + +public: + GodotGeneric6DOFJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA); + + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_6DOF; } + + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; + + // Calcs the global transform for the joint offset for body A an B, and also calcs the angle differences between the bodies. + void calculateTransforms(); + + // Gets the global transform of the offset for body A. + const Transform3D &getCalculatedTransformA() const { + return m_calculatedTransformA; + } + + // Gets the global transform of the offset for body B. + const Transform3D &getCalculatedTransformB() const { + return m_calculatedTransformB; + } + + const Transform3D &getFrameOffsetA() const { + return m_frameInA; + } + + const Transform3D &getFrameOffsetB() const { + return m_frameInB; + } + + Transform3D &getFrameOffsetA() { + return m_frameInA; + } + + Transform3D &getFrameOffsetB() { + return m_frameInB; + } + + // Performs Jacobian calculation, and also calculates angle differences and axis. + void updateRHS(real_t timeStep); + + // Get the rotation axis in global coordinates. + Vector3 getAxis(int axis_index) const; + + // Get the relative Euler angle. + real_t getAngle(int axis_index) const; + + // Calculates angular correction and returns true if limit needs to be corrected. + bool testAngularLimitMotor(int axis_index); + + void setLinearLowerLimit(const Vector3 &linearLower) { + m_linearLimits.m_lowerLimit = linearLower; + } + + void setLinearUpperLimit(const Vector3 &linearUpper) { + m_linearLimits.m_upperLimit = linearUpper; + } + + void setAngularLowerLimit(const Vector3 &angularLower) { + m_angularLimits[0].m_loLimit = angularLower.x; + m_angularLimits[1].m_loLimit = angularLower.y; + m_angularLimits[2].m_loLimit = angularLower.z; + } + + void setAngularUpperLimit(const Vector3 &angularUpper) { + m_angularLimits[0].m_hiLimit = angularUpper.x; + m_angularLimits[1].m_hiLimit = angularUpper.y; + m_angularLimits[2].m_hiLimit = angularUpper.z; + } + + // Retrieves the angular limit information. + GodotG6DOFRotationalLimitMotor3D *getRotationalLimitMotor(int index) { + return &m_angularLimits[index]; + } + + // Retrieves the limit information. + GodotG6DOFTranslationalLimitMotor3D *getTranslationalLimitMotor() { + return &m_linearLimits; + } + + // First 3 are linear, next 3 are angular. + void setLimit(int axis, real_t lo, real_t hi) { + if (axis < 3) { + m_linearLimits.m_lowerLimit[axis] = lo; + m_linearLimits.m_upperLimit[axis] = hi; + } else { + m_angularLimits[axis - 3].m_loLimit = lo; + m_angularLimits[axis - 3].m_hiLimit = hi; + } + } + + //! Test limit + /*! + - free means upper < lower, + - locked means upper == lower + - limited means upper > lower + - limitIndex: first 3 are linear, next 3 are angular + */ + bool isLimited(int limitIndex) { + if (limitIndex < 3) { + return m_linearLimits.isLimited(limitIndex); + } + return m_angularLimits[limitIndex - 3].isLimited(); + } + + const GodotBody3D *getRigidBodyA() const { + return A; + } + const GodotBody3D *getRigidBodyB() const { + return B; + } + + virtual void calcAnchorPos(); // overridable + + void set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value); + real_t get_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const; + + void set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_value); + bool get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const; +}; + +#endif // GODOT_GENERIC_6DOF_JOINT_3D_H diff --git a/servers/physics_3d/joints/godot_hinge_joint_3d.cpp b/servers/physics_3d/joints/godot_hinge_joint_3d.cpp new file mode 100644 index 0000000000..7b7ca1b3ac --- /dev/null +++ b/servers/physics_3d/joints/godot_hinge_joint_3d.cpp @@ -0,0 +1,468 @@ +/*************************************************************************/ +/* godot_hinge_joint_3d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +/* +Adapted to Godot from the Bullet library. +*/ + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "godot_hinge_joint_3d.h" + +static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) { + if (Math::abs(n.z) > Math_SQRT12) { + // choose p in y-z plane + real_t a = n[1] * n[1] + n[2] * n[2]; + real_t k = 1.0 / Math::sqrt(a); + p = Vector3(0, -n[2] * k, n[1] * k); + // set q = n x p + q = Vector3(a * k, -n[0] * p[2], n[0] * p[1]); + } else { + // choose p in x-y plane + real_t a = n.x * n.x + n.y * n.y; + real_t k = 1.0 / Math::sqrt(a); + p = Vector3(-n.y * k, n.x * k, 0); + // set q = n x p + q = Vector3(-n.z * p.y, n.z * p.x, a * k); + } +} + +GodotHingeJoint3D::GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameA, const Transform3D &frameB) : + GodotJoint3D(_arr, 2) { + A = rbA; + B = rbB; + + m_rbAFrame = frameA; + m_rbBFrame = frameB; + // flip axis + m_rbBFrame.basis[0][2] *= real_t(-1.); + m_rbBFrame.basis[1][2] *= real_t(-1.); + m_rbBFrame.basis[2][2] *= real_t(-1.); + + A->add_constraint(this, 0); + B->add_constraint(this, 1); +} + +GodotHingeJoint3D::GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, + const Vector3 &axisInA, const Vector3 &axisInB) : + GodotJoint3D(_arr, 2) { + A = rbA; + B = rbB; + + m_rbAFrame.origin = pivotInA; + + // since no frame is given, assume this to be zero angle and just pick rb transform axis + Vector3 rbAxisA1 = rbA->get_transform().basis.get_axis(0); + + Vector3 rbAxisA2; + real_t projection = axisInA.dot(rbAxisA1); + if (projection >= 1.0f - CMP_EPSILON) { + rbAxisA1 = -rbA->get_transform().basis.get_axis(2); + rbAxisA2 = rbA->get_transform().basis.get_axis(1); + } else if (projection <= -1.0f + CMP_EPSILON) { + rbAxisA1 = rbA->get_transform().basis.get_axis(2); + rbAxisA2 = rbA->get_transform().basis.get_axis(1); + } else { + rbAxisA2 = axisInA.cross(rbAxisA1); + rbAxisA1 = rbAxisA2.cross(axisInA); + } + + m_rbAFrame.basis = Basis(rbAxisA1.x, rbAxisA2.x, axisInA.x, + rbAxisA1.y, rbAxisA2.y, axisInA.y, + rbAxisA1.z, rbAxisA2.z, axisInA.z); + + Quaternion rotationArc = Quaternion(axisInA, axisInB); + Vector3 rbAxisB1 = rotationArc.xform(rbAxisA1); + Vector3 rbAxisB2 = axisInB.cross(rbAxisB1); + + m_rbBFrame.origin = pivotInB; + m_rbBFrame.basis = Basis(rbAxisB1.x, rbAxisB2.x, -axisInB.x, + rbAxisB1.y, rbAxisB2.y, -axisInB.y, + rbAxisB1.z, rbAxisB2.z, -axisInB.z); + + A->add_constraint(this, 0); + B->add_constraint(this, 1); +} + +bool GodotHingeJoint3D::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + + m_appliedImpulse = real_t(0.); + + if (!m_angularOnly) { + Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); + Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); + Vector3 relPos = pivotBInW - pivotAInW; + + Vector3 normal[3]; + if (Math::is_zero_approx(relPos.length_squared())) { + normal[0] = Vector3(real_t(1.0), 0, 0); + } else { + normal[0] = relPos.normalized(); + } + + plane_space(normal[0], normal[1], normal[2]); + + for (int i = 0; i < 3; i++) { + memnew_placement(&m_jac[i], GodotJacobianEntry3D( + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + pivotAInW - A->get_transform().origin - A->get_center_of_mass(), + pivotBInW - B->get_transform().origin - B->get_center_of_mass(), + normal[i], + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass())); + } + } + + //calculate two perpendicular jointAxis, orthogonal to hingeAxis + //these two jointAxis require equal angular velocities for both bodies + + //this is unused for now, it's a todo + Vector3 jointAxis0local; + Vector3 jointAxis1local; + + plane_space(m_rbAFrame.basis.get_axis(2), jointAxis0local, jointAxis1local); + + Vector3 jointAxis0 = A->get_transform().basis.xform(jointAxis0local); + Vector3 jointAxis1 = A->get_transform().basis.xform(jointAxis1local); + Vector3 hingeAxisWorld = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); + + memnew_placement(&m_jacAng[0], GodotJacobianEntry3D(jointAxis0, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); + + memnew_placement(&m_jacAng[1], GodotJacobianEntry3D(jointAxis1, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); + + memnew_placement(&m_jacAng[2], GodotJacobianEntry3D(hingeAxisWorld, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); + + // Compute limit information + real_t hingeAngle = get_hinge_angle(); + + //set bias, sign, clear accumulator + m_correction = real_t(0.); + m_limitSign = real_t(0.); + m_solveLimit = false; + m_accLimitImpulse = real_t(0.); + + //if (m_lowerLimit < m_upperLimit) + if (m_useLimit && m_lowerLimit <= m_upperLimit) { + //if (hingeAngle <= m_lowerLimit*m_limitSoftness) + if (hingeAngle <= m_lowerLimit) { + m_correction = (m_lowerLimit - hingeAngle); + m_limitSign = 1.0f; + m_solveLimit = true; + } + //else if (hingeAngle >= m_upperLimit*m_limitSoftness) + else if (hingeAngle >= m_upperLimit) { + m_correction = m_upperLimit - hingeAngle; + m_limitSign = -1.0f; + m_solveLimit = true; + } + } + + //Compute K = J*W*J' for hinge axis + Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); + m_kHinge = 1.0f / (A->compute_angular_impulse_denominator(axisA) + + B->compute_angular_impulse_denominator(axisA)); + + return true; +} + +void GodotHingeJoint3D::solve(real_t p_step) { + Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); + Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); + + //real_t tau = real_t(0.3); + + //linear part + if (!m_angularOnly) { + Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; + Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; + + Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1); + Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2); + Vector3 vel = vel1 - vel2; + + for (int i = 0; i < 3; i++) { + const Vector3 &normal = m_jac[i].m_linearJointAxis; + real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal(); + + real_t rel_vel; + rel_vel = normal.dot(vel); + //positional error (zeroth order error) + real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + real_t impulse = depth * tau / p_step * jacDiagABInv - rel_vel * jacDiagABInv; + m_appliedImpulse += impulse; + Vector3 impulse_vector = normal * impulse; + if (dynamic_A) { + A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + } + } + } + + { + ///solve angular part + + // get axes in world space + Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); + Vector3 axisB = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(2)); + + const Vector3 &angVelA = A->get_angular_velocity(); + const Vector3 &angVelB = B->get_angular_velocity(); + + Vector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); + Vector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); + + Vector3 angAorthog = angVelA - angVelAroundHingeAxisA; + Vector3 angBorthog = angVelB - angVelAroundHingeAxisB; + Vector3 velrelOrthog = angAorthog - angBorthog; + { + //solve orthogonal angular velocity correction + real_t relaxation = real_t(1.); + real_t len = velrelOrthog.length(); + if (len > real_t(0.00001)) { + Vector3 normal = velrelOrthog.normalized(); + real_t denom = A->compute_angular_impulse_denominator(normal) + + B->compute_angular_impulse_denominator(normal); + // scale for mass and relaxation + velrelOrthog *= (real_t(1.) / denom) * m_relaxationFactor; + } + + //solve angular positional correction + Vector3 angularError = -axisA.cross(axisB) * (real_t(1.) / p_step); + real_t len2 = angularError.length(); + if (len2 > real_t(0.00001)) { + Vector3 normal2 = angularError.normalized(); + real_t denom2 = A->compute_angular_impulse_denominator(normal2) + + B->compute_angular_impulse_denominator(normal2); + angularError *= (real_t(1.) / denom2) * relaxation; + } + + if (dynamic_A) { + A->apply_torque_impulse(-velrelOrthog + angularError); + } + if (dynamic_B) { + B->apply_torque_impulse(velrelOrthog - angularError); + } + + // solve limit + if (m_solveLimit) { + real_t amplitude = ((angVelB - angVelA).dot(axisA) * m_relaxationFactor + m_correction * (real_t(1.) / p_step) * m_biasFactor) * m_limitSign; + + real_t impulseMag = amplitude * m_kHinge; + + // Clamp the accumulated impulse + real_t temp = m_accLimitImpulse; + m_accLimitImpulse = MAX(m_accLimitImpulse + impulseMag, real_t(0)); + impulseMag = m_accLimitImpulse - temp; + + Vector3 impulse = axisA * impulseMag * m_limitSign; + if (dynamic_A) { + A->apply_torque_impulse(impulse); + } + if (dynamic_B) { + B->apply_torque_impulse(-impulse); + } + } + } + + //apply motor + if (m_enableAngularMotor) { + //todo: add limits too + Vector3 angularLimit(0, 0, 0); + + Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; + real_t projRelVel = velrel.dot(axisA); + + real_t desiredMotorVel = m_motorTargetVelocity; + real_t motor_relvel = desiredMotorVel - projRelVel; + + real_t unclippedMotorImpulse = m_kHinge * motor_relvel; + //todo: should clip against accumulated impulse + real_t clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; + clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; + Vector3 motorImp = clippedMotorImpulse * axisA; + + if (dynamic_A) { + A->apply_torque_impulse(motorImp + angularLimit); + } + if (dynamic_B) { + B->apply_torque_impulse(-motorImp - angularLimit); + } + } + } +} + +/* +void HingeJointSW::updateRHS(real_t timeStep) +{ + (void)timeStep; +} + +*/ + +static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { + real_t coeff_1 = Math_PI / 4.0f; + real_t coeff_2 = 3.0f * coeff_1; + real_t abs_y = Math::abs(y); + real_t angle; + if (x >= 0.0f) { + real_t r = (x - abs_y) / (x + abs_y); + angle = coeff_1 - coeff_1 * r; + } else { + real_t r = (x + abs_y) / (abs_y - x); + angle = coeff_2 - coeff_1 * r; + } + return (y < 0.0f) ? -angle : angle; +} + +real_t GodotHingeJoint3D::get_hinge_angle() { + const Vector3 refAxis0 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(0)); + const Vector3 refAxis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(1)); + const Vector3 swingAxis = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(1)); + + return atan2fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); +} + +void GodotHingeJoint3D::set_param(PhysicsServer3D::HingeJointParam p_param, real_t p_value) { + switch (p_param) { + case PhysicsServer3D::HINGE_JOINT_BIAS: + tau = p_value; + break; + case PhysicsServer3D::HINGE_JOINT_LIMIT_UPPER: + m_upperLimit = p_value; + break; + case PhysicsServer3D::HINGE_JOINT_LIMIT_LOWER: + m_lowerLimit = p_value; + break; + case PhysicsServer3D::HINGE_JOINT_LIMIT_BIAS: + m_biasFactor = p_value; + break; + case PhysicsServer3D::HINGE_JOINT_LIMIT_SOFTNESS: + m_limitSoftness = p_value; + break; + case PhysicsServer3D::HINGE_JOINT_LIMIT_RELAXATION: + m_relaxationFactor = p_value; + break; + case PhysicsServer3D::HINGE_JOINT_MOTOR_TARGET_VELOCITY: + m_motorTargetVelocity = p_value; + break; + case PhysicsServer3D::HINGE_JOINT_MOTOR_MAX_IMPULSE: + m_maxMotorImpulse = p_value; + break; + case PhysicsServer3D::HINGE_JOINT_MAX: + break; // Can't happen, but silences warning + } +} + +real_t GodotHingeJoint3D::get_param(PhysicsServer3D::HingeJointParam p_param) const { + switch (p_param) { + case PhysicsServer3D::HINGE_JOINT_BIAS: + return tau; + case PhysicsServer3D::HINGE_JOINT_LIMIT_UPPER: + return m_upperLimit; + case PhysicsServer3D::HINGE_JOINT_LIMIT_LOWER: + return m_lowerLimit; + case PhysicsServer3D::HINGE_JOINT_LIMIT_BIAS: + return m_biasFactor; + case PhysicsServer3D::HINGE_JOINT_LIMIT_SOFTNESS: + return m_limitSoftness; + case PhysicsServer3D::HINGE_JOINT_LIMIT_RELAXATION: + return m_relaxationFactor; + case PhysicsServer3D::HINGE_JOINT_MOTOR_TARGET_VELOCITY: + return m_motorTargetVelocity; + case PhysicsServer3D::HINGE_JOINT_MOTOR_MAX_IMPULSE: + return m_maxMotorImpulse; + case PhysicsServer3D::HINGE_JOINT_MAX: + break; // Can't happen, but silences warning + } + + return 0; +} + +void GodotHingeJoint3D::set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value) { + switch (p_flag) { + case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT: + m_useLimit = p_value; + break; + case PhysicsServer3D::HINGE_JOINT_FLAG_ENABLE_MOTOR: + m_enableAngularMotor = p_value; + break; + case PhysicsServer3D::HINGE_JOINT_FLAG_MAX: + break; // Can't happen, but silences warning + } +} + +bool GodotHingeJoint3D::get_flag(PhysicsServer3D::HingeJointFlag p_flag) const { + switch (p_flag) { + case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT: + return m_useLimit; + case PhysicsServer3D::HINGE_JOINT_FLAG_ENABLE_MOTOR: + return m_enableAngularMotor; + case PhysicsServer3D::HINGE_JOINT_FLAG_MAX: + break; // Can't happen, but silences warning + } + + return false; +} diff --git a/servers/physics_3d/joints/godot_hinge_joint_3d.h b/servers/physics_3d/joints/godot_hinge_joint_3d.h new file mode 100644 index 0000000000..ff1fbe0f25 --- /dev/null +++ b/servers/physics_3d/joints/godot_hinge_joint_3d.h @@ -0,0 +1,116 @@ +/*************************************************************************/ +/* godot_hinge_joint_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +/* +Adapted to Godot from the Bullet library. +*/ + +#ifndef GODOT_HINGE_JOINT_3D_H +#define GODOT_HINGE_JOINT_3D_H + +#include "servers/physics_3d/godot_joint_3d.h" +#include "servers/physics_3d/joints/godot_jacobian_entry_3d.h" + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +class GodotHingeJoint3D : public GodotJoint3D { + union { + struct { + GodotBody3D *A; + GodotBody3D *B; + }; + + GodotBody3D *_arr[2] = {}; + }; + + GodotJacobianEntry3D m_jac[3]; //3 orthogonal linear constraints + GodotJacobianEntry3D m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor + + Transform3D m_rbAFrame; // constraint axii. Assumes z is hinge axis. + Transform3D m_rbBFrame; + + real_t m_motorTargetVelocity = 0.0; + real_t m_maxMotorImpulse = 0.0; + + real_t m_limitSoftness = 0.9; + real_t m_biasFactor = 0.3; + real_t m_relaxationFactor = 1.0; + + real_t m_lowerLimit = Math_PI; + real_t m_upperLimit = -Math_PI; + + real_t m_kHinge = 0.0; + + real_t m_limitSign = 0.0; + real_t m_correction = 0.0; + + real_t m_accLimitImpulse = 0.0; + + real_t tau = 0.3; + + bool m_useLimit = false; + bool m_angularOnly = false; + bool m_enableAngularMotor = false; + bool m_solveLimit = false; + + real_t m_appliedImpulse = 0.0; + +public: + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_HINGE; } + + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; + + real_t get_hinge_angle(); + + void set_param(PhysicsServer3D::HingeJointParam p_param, real_t p_value); + real_t get_param(PhysicsServer3D::HingeJointParam p_param) const; + + void set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value); + bool get_flag(PhysicsServer3D::HingeJointFlag p_flag) const; + + GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameA, const Transform3D &frameB); + GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB); +}; + +#endif // GODOT_HINGE_JOINT_3D_H diff --git a/servers/physics_3d/joints/godot_jacobian_entry_3d.h b/servers/physics_3d/joints/godot_jacobian_entry_3d.h new file mode 100644 index 0000000000..90a77a9b61 --- /dev/null +++ b/servers/physics_3d/joints/godot_jacobian_entry_3d.h @@ -0,0 +1,169 @@ +/*************************************************************************/ +/* godot_jacobian_entry_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +/* +Adapted to Godot from the Bullet library. +*/ + +#ifndef GODOT_JACOBIAN_ENTRY_3D_H +#define GODOT_JACOBIAN_ENTRY_3D_H + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "core/math/transform_3d.h" + +class GodotJacobianEntry3D { +public: + GodotJacobianEntry3D() {} + //constraint between two different rigidbodies + GodotJacobianEntry3D( + const Basis &world2A, + const Basis &world2B, + const Vector3 &rel_pos1, const Vector3 &rel_pos2, + const Vector3 &jointAxis, + const Vector3 &inertiaInvA, + const real_t massInvA, + const Vector3 &inertiaInvB, + const real_t massInvB) : + m_linearJointAxis(jointAxis) { + m_aJ = world2A.xform(rel_pos1.cross(m_linearJointAxis)); + m_bJ = world2B.xform(rel_pos2.cross(-m_linearJointAxis)); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); + + ERR_FAIL_COND(m_Adiag <= real_t(0.0)); + } + + //angular constraint between two different rigidbodies + GodotJacobianEntry3D(const Vector3 &jointAxis, + const Basis &world2A, + const Basis &world2B, + const Vector3 &inertiaInvA, + const Vector3 &inertiaInvB) : + m_linearJointAxis(Vector3(real_t(0.), real_t(0.), real_t(0.))) { + m_aJ = world2A.xform(jointAxis); + m_bJ = world2B.xform(-jointAxis); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); + + ERR_FAIL_COND(m_Adiag <= real_t(0.0)); + } + + //angular constraint between two different rigidbodies + GodotJacobianEntry3D(const Vector3 &axisInA, + const Vector3 &axisInB, + const Vector3 &inertiaInvA, + const Vector3 &inertiaInvB) : + m_linearJointAxis(Vector3(real_t(0.), real_t(0.), real_t(0.))), + m_aJ(axisInA), + m_bJ(-axisInB) { + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); + + ERR_FAIL_COND(m_Adiag <= real_t(0.0)); + } + + //constraint on one rigidbody + GodotJacobianEntry3D( + const Basis &world2A, + const Vector3 &rel_pos1, const Vector3 &rel_pos2, + const Vector3 &jointAxis, + const Vector3 &inertiaInvA, + const real_t massInvA) : + m_linearJointAxis(jointAxis) { + m_aJ = world2A.xform(rel_pos1.cross(jointAxis)); + m_bJ = world2A.xform(rel_pos2.cross(-jointAxis)); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = Vector3(real_t(0.), real_t(0.), real_t(0.)); + m_Adiag = massInvA + m_0MinvJt.dot(m_aJ); + + ERR_FAIL_COND(m_Adiag <= real_t(0.0)); + } + + real_t getDiagonal() const { return m_Adiag; } + + // for two constraints on the same rigidbody (for example vehicle friction) + real_t getNonDiagonal(const GodotJacobianEntry3D &jacB, const real_t massInvA) const { + const GodotJacobianEntry3D &jacA = *this; + real_t lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis); + real_t ang = jacA.m_0MinvJt.dot(jacB.m_aJ); + return lin + ang; + } + + // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) + real_t getNonDiagonal(const GodotJacobianEntry3D &jacB, const real_t massInvA, const real_t massInvB) const { + const GodotJacobianEntry3D &jacA = *this; + Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis; + Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ; + Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ; + Vector3 lin0 = massInvA * lin; + Vector3 lin1 = massInvB * lin; + Vector3 sum = ang0 + ang1 + lin0 + lin1; + return sum[0] + sum[1] + sum[2]; + } + + real_t getRelativeVelocity(const Vector3 &linvelA, const Vector3 &angvelA, const Vector3 &linvelB, const Vector3 &angvelB) { + Vector3 linrel = linvelA - linvelB; + Vector3 angvela = angvelA * m_aJ; + Vector3 angvelb = angvelB * m_bJ; + linrel *= m_linearJointAxis; + angvela += angvelb; + angvela += linrel; + real_t rel_vel2 = angvela[0] + angvela[1] + angvela[2]; + return rel_vel2 + CMP_EPSILON; + } + //private: + + Vector3 m_linearJointAxis; + Vector3 m_aJ; + Vector3 m_bJ; + Vector3 m_0MinvJt; + Vector3 m_1MinvJt; + //Optimization: can be stored in the w/last component of one of the vectors + real_t m_Adiag = 1.0; +}; + +#endif // GODOT_JACOBIAN_ENTRY_3D_H diff --git a/servers/physics_3d/joints/godot_pin_joint_3d.cpp b/servers/physics_3d/joints/godot_pin_joint_3d.cpp new file mode 100644 index 0000000000..10d52ad5e9 --- /dev/null +++ b/servers/physics_3d/joints/godot_pin_joint_3d.cpp @@ -0,0 +1,179 @@ +/*************************************************************************/ +/* godot_pin_joint_3d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +/* +Adapted to Godot from the Bullet library. +*/ + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "godot_pin_joint_3d.h" + +bool GodotPinJoint3D::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + + m_appliedImpulse = real_t(0.); + + Vector3 normal(0, 0, 0); + + for (int i = 0; i < 3; i++) { + normal[i] = 1; + memnew_placement(&m_jac[i], GodotJacobianEntry3D( + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_transform().xform(m_pivotInA) - A->get_transform().origin - A->get_center_of_mass(), + B->get_transform().xform(m_pivotInB) - B->get_transform().origin - B->get_center_of_mass(), + normal, + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass())); + normal[i] = 0; + } + + return true; +} + +void GodotPinJoint3D::solve(real_t p_step) { + Vector3 pivotAInW = A->get_transform().xform(m_pivotInA); + Vector3 pivotBInW = B->get_transform().xform(m_pivotInB); + + Vector3 normal(0, 0, 0); + + //Vector3 angvelA = A->get_transform().origin.getBasis().transpose() * A->getAngularVelocity(); + //Vector3 angvelB = B->get_transform().origin.getBasis().transpose() * B->getAngularVelocity(); + + for (int i = 0; i < 3; i++) { + normal[i] = 1; + real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal(); + + Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; + Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; + //this jacobian entry could be re-used for all iterations + + Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1); + Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2); + Vector3 vel = vel1 - vel2; + + real_t rel_vel; + rel_vel = normal.dot(vel); + + /* + //velocity error (first order error) + real_t rel_vel = m_jac[i].getRelativeVelocity(A->getLinearVelocity(),angvelA, + B->getLinearVelocity(),angvelB); + */ + + //positional error (zeroth order error) + real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + + real_t impulse = depth * m_tau / p_step * jacDiagABInv - m_damping * rel_vel * jacDiagABInv; + + real_t impulseClamp = m_impulseClamp; + if (impulseClamp > 0) { + if (impulse < -impulseClamp) { + impulse = -impulseClamp; + } + if (impulse > impulseClamp) { + impulse = impulseClamp; + } + } + + m_appliedImpulse += impulse; + Vector3 impulse_vector = normal * impulse; + if (dynamic_A) { + A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + } + + normal[i] = 0; + } +} + +void GodotPinJoint3D::set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value) { + switch (p_param) { + case PhysicsServer3D::PIN_JOINT_BIAS: + m_tau = p_value; + break; + case PhysicsServer3D::PIN_JOINT_DAMPING: + m_damping = p_value; + break; + case PhysicsServer3D::PIN_JOINT_IMPULSE_CLAMP: + m_impulseClamp = p_value; + break; + } +} + +real_t GodotPinJoint3D::get_param(PhysicsServer3D::PinJointParam p_param) const { + switch (p_param) { + case PhysicsServer3D::PIN_JOINT_BIAS: + return m_tau; + case PhysicsServer3D::PIN_JOINT_DAMPING: + return m_damping; + case PhysicsServer3D::PIN_JOINT_IMPULSE_CLAMP: + return m_impulseClamp; + } + + return 0; +} + +GodotPinJoint3D::GodotPinJoint3D(GodotBody3D *p_body_a, const Vector3 &p_pos_a, GodotBody3D *p_body_b, const Vector3 &p_pos_b) : + GodotJoint3D(_arr, 2) { + A = p_body_a; + B = p_body_b; + m_pivotInA = p_pos_a; + m_pivotInB = p_pos_b; + + A->add_constraint(this, 0); + B->add_constraint(this, 1); +} + +GodotPinJoint3D::~GodotPinJoint3D() { +} diff --git a/servers/physics_3d/joints/godot_pin_joint_3d.h b/servers/physics_3d/joints/godot_pin_joint_3d.h new file mode 100644 index 0000000000..17e2e6e973 --- /dev/null +++ b/servers/physics_3d/joints/godot_pin_joint_3d.h @@ -0,0 +1,95 @@ +/*************************************************************************/ +/* godot_pin_joint_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +/* +Adapted to Godot from the Bullet library. +*/ + +#ifndef GODOT_PIN_JOINT_3D_H +#define GODOT_PIN_JOINT_3D_H + +#include "servers/physics_3d/godot_joint_3d.h" +#include "servers/physics_3d/joints/godot_jacobian_entry_3d.h" + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +class GodotPinJoint3D : public GodotJoint3D { + union { + struct { + GodotBody3D *A; + GodotBody3D *B; + }; + + GodotBody3D *_arr[2] = {}; + }; + + real_t m_tau = 0.3; //bias + real_t m_damping = 1.0; + real_t m_impulseClamp = 0.0; + real_t m_appliedImpulse = 0.0; + + GodotJacobianEntry3D m_jac[3] = {}; //3 orthogonal linear constraints + + Vector3 m_pivotInA; + Vector3 m_pivotInB; + +public: + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_PIN; } + + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; + + void set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value); + real_t get_param(PhysicsServer3D::PinJointParam p_param) const; + + void set_pos_a(const Vector3 &p_pos) { m_pivotInA = p_pos; } + void set_pos_b(const Vector3 &p_pos) { m_pivotInB = p_pos; } + + Vector3 get_position_a() { return m_pivotInA; } + Vector3 get_position_b() { return m_pivotInB; } + + GodotPinJoint3D(GodotBody3D *p_body_a, const Vector3 &p_pos_a, GodotBody3D *p_body_b, const Vector3 &p_pos_b); + ~GodotPinJoint3D(); +}; + +#endif // GODOT_PIN_JOINT_3D_H diff --git a/servers/physics_3d/joints/godot_slider_joint_3d.cpp b/servers/physics_3d/joints/godot_slider_joint_3d.cpp new file mode 100644 index 0000000000..3be111ac92 --- /dev/null +++ b/servers/physics_3d/joints/godot_slider_joint_3d.cpp @@ -0,0 +1,493 @@ +/*************************************************************************/ +/* godot_slider_joint_3d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +/* +Adapted to Godot from the Bullet library. +*/ + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +Added by Roman Ponomarev (rponom@gmail.com) +April 04, 2008 + +*/ + +#include "godot_slider_joint_3d.h" + +//----------------------------------------------------------------------------- + +static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { + real_t coeff_1 = Math_PI / 4.0f; + real_t coeff_2 = 3.0f * coeff_1; + real_t abs_y = Math::abs(y); + real_t angle; + if (x >= 0.0f) { + real_t r = (x - abs_y) / (x + abs_y); + angle = coeff_1 - coeff_1 * r; + } else { + real_t r = (x + abs_y) / (abs_y - x); + angle = coeff_2 - coeff_1 * r; + } + return (y < 0.0f) ? -angle : angle; +} + +//----------------------------------------------------------------------------- + +//----------------------------------------------------------------------------- + +GodotSliderJoint3D::GodotSliderJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB) : + GodotJoint3D(_arr, 2), + m_frameInA(frameInA), + m_frameInB(frameInB) { + A = rbA; + B = rbB; + + A->add_constraint(this, 0); + B->add_constraint(this, 1); +} + +//----------------------------------------------------------------------------- + +bool GodotSliderJoint3D::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + + //calculate transforms + m_calculatedTransformA = A->get_transform() * m_frameInA; + m_calculatedTransformB = B->get_transform() * m_frameInB; + m_realPivotAInW = m_calculatedTransformA.origin; + m_realPivotBInW = m_calculatedTransformB.origin; + m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X + m_delta = m_realPivotBInW - m_realPivotAInW; + m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; + m_relPosA = m_projPivotInW - A->get_transform().origin; + m_relPosB = m_realPivotBInW - B->get_transform().origin; + Vector3 normalWorld; + int i; + //linear part + for (i = 0; i < 3; i++) { + normalWorld = m_calculatedTransformA.basis.get_axis(i); + memnew_placement(&m_jacLin[i], GodotJacobianEntry3D( + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + m_relPosA - A->get_center_of_mass(), + m_relPosB - B->get_center_of_mass(), + normalWorld, + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass())); + m_jacLinDiagABInv[i] = real_t(1.) / m_jacLin[i].getDiagonal(); + m_depth[i] = m_delta.dot(normalWorld); + } + testLinLimits(); + // angular part + for (i = 0; i < 3; i++) { + normalWorld = m_calculatedTransformA.basis.get_axis(i); + memnew_placement(&m_jacAng[i], GodotJacobianEntry3D( + normalWorld, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); + } + testAngLimits(); + Vector3 axisA = m_calculatedTransformA.basis.get_axis(0); + m_kAngle = real_t(1.0) / (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA)); + // clear accumulator for motors + m_accumulatedLinMotorImpulse = real_t(0.0); + m_accumulatedAngMotorImpulse = real_t(0.0); + + return true; +} + +//----------------------------------------------------------------------------- + +void GodotSliderJoint3D::solve(real_t p_step) { + int i; + // linear + Vector3 velA = A->get_velocity_in_local_point(m_relPosA); + Vector3 velB = B->get_velocity_in_local_point(m_relPosB); + Vector3 vel = velA - velB; + for (i = 0; i < 3; i++) { + const Vector3 &normal = m_jacLin[i].m_linearJointAxis; + real_t rel_vel = normal.dot(vel); + // calculate positional error + real_t depth = m_depth[i]; + // get parameters + real_t softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin); + real_t restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin); + real_t damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin); + // Calculate and apply impulse. + real_t normalImpulse = softness * (restitution * depth / p_step - damping * rel_vel) * m_jacLinDiagABInv[i]; + Vector3 impulse_vector = normal * normalImpulse; + if (dynamic_A) { + A->apply_impulse(impulse_vector, m_relPosA); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, m_relPosB); + } + if (m_poweredLinMotor && (!i)) { // apply linear motor + if (m_accumulatedLinMotorImpulse < m_maxLinMotorForce) { + real_t desiredMotorVel = m_targetLinMotorVelocity; + real_t motor_relvel = desiredMotorVel + rel_vel; + normalImpulse = -motor_relvel * m_jacLinDiagABInv[i]; + // clamp accumulated impulse + real_t new_acc = m_accumulatedLinMotorImpulse + Math::abs(normalImpulse); + if (new_acc > m_maxLinMotorForce) { + new_acc = m_maxLinMotorForce; + } + real_t del = new_acc - m_accumulatedLinMotorImpulse; + if (normalImpulse < real_t(0.0)) { + normalImpulse = -del; + } else { + normalImpulse = del; + } + m_accumulatedLinMotorImpulse = new_acc; + // apply clamped impulse + impulse_vector = normal * normalImpulse; + if (dynamic_A) { + A->apply_impulse(impulse_vector, m_relPosA); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, m_relPosB); + } + } + } + } + // angular + // get axes in world space + Vector3 axisA = m_calculatedTransformA.basis.get_axis(0); + Vector3 axisB = m_calculatedTransformB.basis.get_axis(0); + + const Vector3 &angVelA = A->get_angular_velocity(); + const Vector3 &angVelB = B->get_angular_velocity(); + + Vector3 angVelAroundAxisA = axisA * axisA.dot(angVelA); + Vector3 angVelAroundAxisB = axisB * axisB.dot(angVelB); + + Vector3 angAorthog = angVelA - angVelAroundAxisA; + Vector3 angBorthog = angVelB - angVelAroundAxisB; + Vector3 velrelOrthog = angAorthog - angBorthog; + //solve orthogonal angular velocity correction + real_t len = velrelOrthog.length(); + if (len > real_t(0.00001)) { + Vector3 normal = velrelOrthog.normalized(); + real_t denom = A->compute_angular_impulse_denominator(normal) + B->compute_angular_impulse_denominator(normal); + velrelOrthog *= (real_t(1.) / denom) * m_dampingOrthoAng * m_softnessOrthoAng; + } + //solve angular positional correction + Vector3 angularError = axisA.cross(axisB) * (real_t(1.) / p_step); + real_t len2 = angularError.length(); + if (len2 > real_t(0.00001)) { + Vector3 normal2 = angularError.normalized(); + real_t denom2 = A->compute_angular_impulse_denominator(normal2) + B->compute_angular_impulse_denominator(normal2); + angularError *= (real_t(1.) / denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; + } + // apply impulse + if (dynamic_A) { + A->apply_torque_impulse(-velrelOrthog + angularError); + } + if (dynamic_B) { + B->apply_torque_impulse(velrelOrthog - angularError); + } + real_t impulseMag; + //solve angular limits + if (m_solveAngLim) { + impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / p_step; + impulseMag *= m_kAngle * m_softnessLimAng; + } else { + impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / p_step; + impulseMag *= m_kAngle * m_softnessDirAng; + } + Vector3 impulse = axisA * impulseMag; + if (dynamic_A) { + A->apply_torque_impulse(impulse); + } + if (dynamic_B) { + B->apply_torque_impulse(-impulse); + } + //apply angular motor + if (m_poweredAngMotor) { + if (m_accumulatedAngMotorImpulse < m_maxAngMotorForce) { + Vector3 velrel = angVelAroundAxisA - angVelAroundAxisB; + real_t projRelVel = velrel.dot(axisA); + + real_t desiredMotorVel = m_targetAngMotorVelocity; + real_t motor_relvel = desiredMotorVel - projRelVel; + + real_t angImpulse = m_kAngle * motor_relvel; + // clamp accumulated impulse + real_t new_acc = m_accumulatedAngMotorImpulse + Math::abs(angImpulse); + if (new_acc > m_maxAngMotorForce) { + new_acc = m_maxAngMotorForce; + } + real_t del = new_acc - m_accumulatedAngMotorImpulse; + if (angImpulse < real_t(0.0)) { + angImpulse = -del; + } else { + angImpulse = del; + } + m_accumulatedAngMotorImpulse = new_acc; + // apply clamped impulse + Vector3 motorImp = angImpulse * axisA; + if (dynamic_A) { + A->apply_torque_impulse(motorImp); + } + if (dynamic_B) { + B->apply_torque_impulse(-motorImp); + } + } + } +} + +//----------------------------------------------------------------------------- + +void GodotSliderJoint3D::calculateTransforms() { + m_calculatedTransformA = A->get_transform() * m_frameInA; + m_calculatedTransformB = B->get_transform() * m_frameInB; + m_realPivotAInW = m_calculatedTransformA.origin; + m_realPivotBInW = m_calculatedTransformB.origin; + m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X + m_delta = m_realPivotBInW - m_realPivotAInW; + m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; + Vector3 normalWorld; + int i; + //linear part + for (i = 0; i < 3; i++) { + normalWorld = m_calculatedTransformA.basis.get_axis(i); + m_depth[i] = m_delta.dot(normalWorld); + } +} + +//----------------------------------------------------------------------------- + +void GodotSliderJoint3D::testLinLimits() { + m_solveLinLim = false; + m_linPos = m_depth[0]; + if (m_lowerLinLimit <= m_upperLinLimit) { + if (m_depth[0] > m_upperLinLimit) { + m_depth[0] -= m_upperLinLimit; + m_solveLinLim = true; + } else if (m_depth[0] < m_lowerLinLimit) { + m_depth[0] -= m_lowerLinLimit; + m_solveLinLim = true; + } else { + m_depth[0] = real_t(0.); + } + } else { + m_depth[0] = real_t(0.); + } +} + +//----------------------------------------------------------------------------- + +void GodotSliderJoint3D::testAngLimits() { + m_angDepth = real_t(0.); + m_solveAngLim = false; + if (m_lowerAngLimit <= m_upperAngLimit) { + const Vector3 axisA0 = m_calculatedTransformA.basis.get_axis(1); + const Vector3 axisA1 = m_calculatedTransformA.basis.get_axis(2); + const Vector3 axisB0 = m_calculatedTransformB.basis.get_axis(1); + real_t rot = atan2fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); + if (rot < m_lowerAngLimit) { + m_angDepth = rot - m_lowerAngLimit; + m_solveAngLim = true; + } else if (rot > m_upperAngLimit) { + m_angDepth = rot - m_upperAngLimit; + m_solveAngLim = true; + } + } +} + +//----------------------------------------------------------------------------- + +Vector3 GodotSliderJoint3D::getAncorInA() { + Vector3 ancorInA; + ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * real_t(0.5) * m_sliderAxis; + ancorInA = A->get_transform().inverse().xform(ancorInA); + return ancorInA; +} + +//----------------------------------------------------------------------------- + +Vector3 GodotSliderJoint3D::getAncorInB() { + Vector3 ancorInB; + ancorInB = m_frameInB.origin; + return ancorInB; +} + +void GodotSliderJoint3D::set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value) { + switch (p_param) { + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER: + m_upperLinLimit = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_LOWER: + m_lowerLinLimit = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: + m_softnessLimLin = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: + m_restitutionLimLin = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: + m_dampingLimLin = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: + m_softnessDirLin = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: + m_restitutionDirLin = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_DAMPING: + m_dampingDirLin = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: + m_softnessOrthoLin = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: + m_restitutionOrthoLin = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: + m_dampingOrthoLin = p_value; + break; + + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: + m_upperAngLimit = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: + m_lowerAngLimit = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: + m_softnessLimAng = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: + m_restitutionLimAng = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: + m_dampingLimAng = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: + m_softnessDirAng = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: + m_restitutionDirAng = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: + m_dampingDirAng = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: + m_softnessOrthoAng = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: + m_restitutionOrthoAng = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: + m_dampingOrthoAng = p_value; + break; + + case PhysicsServer3D::SLIDER_JOINT_MAX: + break; // Can't happen, but silences warning + } +} + +real_t GodotSliderJoint3D::get_param(PhysicsServer3D::SliderJointParam p_param) const { + switch (p_param) { + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER: + return m_upperLinLimit; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_LOWER: + return m_lowerLinLimit; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: + return m_softnessLimLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: + return m_restitutionLimLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: + return m_dampingLimLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: + return m_softnessDirLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: + return m_restitutionDirLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_DAMPING: + return m_dampingDirLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: + return m_softnessOrthoLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: + return m_restitutionOrthoLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: + return m_dampingOrthoLin; + + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: + return m_upperAngLimit; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: + return m_lowerAngLimit; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: + return m_softnessLimAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: + return m_restitutionLimAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: + return m_dampingLimAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: + return m_softnessDirAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: + return m_restitutionDirAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: + return m_dampingDirAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: + return m_softnessOrthoAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: + return m_restitutionOrthoAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: + return m_dampingOrthoAng; + + case PhysicsServer3D::SLIDER_JOINT_MAX: + break; // Can't happen, but silences warning + } + + return 0; +} diff --git a/servers/physics_3d/joints/godot_slider_joint_3d.h b/servers/physics_3d/joints/godot_slider_joint_3d.h new file mode 100644 index 0000000000..9baaf1fa40 --- /dev/null +++ b/servers/physics_3d/joints/godot_slider_joint_3d.h @@ -0,0 +1,246 @@ +/*************************************************************************/ +/* godot_slider_joint_3d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +/* +Adapted to Godot from the Bullet library. +*/ + +#ifndef GODOT_SLIDER_JOINT_3D_H +#define GODOT_SLIDER_JOINT_3D_H + +#include "servers/physics_3d/godot_joint_3d.h" +#include "servers/physics_3d/joints/godot_jacobian_entry_3d.h" + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +Added by Roman Ponomarev (rponom@gmail.com) +April 04, 2008 + +*/ + +#define SLIDER_CONSTRAINT_DEF_SOFTNESS (real_t(1.0)) +#define SLIDER_CONSTRAINT_DEF_DAMPING (real_t(1.0)) +#define SLIDER_CONSTRAINT_DEF_RESTITUTION (real_t(0.7)) + +//----------------------------------------------------------------------------- + +class GodotSliderJoint3D : public GodotJoint3D { +protected: + union { + struct { + GodotBody3D *A; + GodotBody3D *B; + }; + + GodotBody3D *_arr[2] = { nullptr, nullptr }; + }; + + Transform3D m_frameInA; + Transform3D m_frameInB; + + // linear limits + real_t m_lowerLinLimit = 1.0; + real_t m_upperLinLimit = -1.0; + // angular limits + real_t m_lowerAngLimit = 0.0; + real_t m_upperAngLimit = 0.0; + // softness, restitution and damping for different cases + // DirLin - moving inside linear limits + // LimLin - hitting linear limit + // DirAng - moving inside angular limits + // LimAng - hitting angular limit + // OrthoLin, OrthoAng - against constraint axis + real_t m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingDirLin = 0.0; + real_t m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingDirAng = 0.0; + real_t m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING; + real_t m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING; + real_t m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING; + real_t m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING; + + // for interlal use + bool m_solveLinLim = false; + bool m_solveAngLim = false; + + GodotJacobianEntry3D m_jacLin[3] = {}; + real_t m_jacLinDiagABInv[3] = {}; + + GodotJacobianEntry3D m_jacAng[3] = {}; + + real_t m_timeStep = 0.0; + Transform3D m_calculatedTransformA; + Transform3D m_calculatedTransformB; + + Vector3 m_sliderAxis; + Vector3 m_realPivotAInW; + Vector3 m_realPivotBInW; + Vector3 m_projPivotInW; + Vector3 m_delta; + Vector3 m_depth; + Vector3 m_relPosA; + Vector3 m_relPosB; + + real_t m_linPos = 0.0; + + real_t m_angDepth = 0.0; + real_t m_kAngle = 0.0; + + bool m_poweredLinMotor = false; + real_t m_targetLinMotorVelocity = 0.0; + real_t m_maxLinMotorForce = 0.0; + real_t m_accumulatedLinMotorImpulse = 0.0; + + bool m_poweredAngMotor = false; + real_t m_targetAngMotorVelocity = 0.0; + real_t m_maxAngMotorForce = 0.0; + real_t m_accumulatedAngMotorImpulse = 0.0; + +public: + // constructors + GodotSliderJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB); + //SliderJointSW(); + // overrides + + // access + const GodotBody3D *getRigidBodyA() const { return A; } + const GodotBody3D *getRigidBodyB() const { return B; } + const Transform3D &getCalculatedTransformA() const { return m_calculatedTransformA; } + const Transform3D &getCalculatedTransformB() const { return m_calculatedTransformB; } + const Transform3D &getFrameOffsetA() const { return m_frameInA; } + const Transform3D &getFrameOffsetB() const { return m_frameInB; } + Transform3D &getFrameOffsetA() { return m_frameInA; } + Transform3D &getFrameOffsetB() { return m_frameInB; } + real_t getLowerLinLimit() { return m_lowerLinLimit; } + void setLowerLinLimit(real_t lowerLimit) { m_lowerLinLimit = lowerLimit; } + real_t getUpperLinLimit() { return m_upperLinLimit; } + void setUpperLinLimit(real_t upperLimit) { m_upperLinLimit = upperLimit; } + real_t getLowerAngLimit() { return m_lowerAngLimit; } + void setLowerAngLimit(real_t lowerLimit) { m_lowerAngLimit = lowerLimit; } + real_t getUpperAngLimit() { return m_upperAngLimit; } + void setUpperAngLimit(real_t upperLimit) { m_upperAngLimit = upperLimit; } + + real_t getSoftnessDirLin() { return m_softnessDirLin; } + real_t getRestitutionDirLin() { return m_restitutionDirLin; } + real_t getDampingDirLin() { return m_dampingDirLin; } + real_t getSoftnessDirAng() { return m_softnessDirAng; } + real_t getRestitutionDirAng() { return m_restitutionDirAng; } + real_t getDampingDirAng() { return m_dampingDirAng; } + real_t getSoftnessLimLin() { return m_softnessLimLin; } + real_t getRestitutionLimLin() { return m_restitutionLimLin; } + real_t getDampingLimLin() { return m_dampingLimLin; } + real_t getSoftnessLimAng() { return m_softnessLimAng; } + real_t getRestitutionLimAng() { return m_restitutionLimAng; } + real_t getDampingLimAng() { return m_dampingLimAng; } + real_t getSoftnessOrthoLin() { return m_softnessOrthoLin; } + real_t getRestitutionOrthoLin() { return m_restitutionOrthoLin; } + real_t getDampingOrthoLin() { return m_dampingOrthoLin; } + real_t getSoftnessOrthoAng() { return m_softnessOrthoAng; } + real_t getRestitutionOrthoAng() { return m_restitutionOrthoAng; } + real_t getDampingOrthoAng() { return m_dampingOrthoAng; } + void setSoftnessDirLin(real_t softnessDirLin) { m_softnessDirLin = softnessDirLin; } + void setRestitutionDirLin(real_t restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; } + void setDampingDirLin(real_t dampingDirLin) { m_dampingDirLin = dampingDirLin; } + void setSoftnessDirAng(real_t softnessDirAng) { m_softnessDirAng = softnessDirAng; } + void setRestitutionDirAng(real_t restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; } + void setDampingDirAng(real_t dampingDirAng) { m_dampingDirAng = dampingDirAng; } + void setSoftnessLimLin(real_t softnessLimLin) { m_softnessLimLin = softnessLimLin; } + void setRestitutionLimLin(real_t restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; } + void setDampingLimLin(real_t dampingLimLin) { m_dampingLimLin = dampingLimLin; } + void setSoftnessLimAng(real_t softnessLimAng) { m_softnessLimAng = softnessLimAng; } + void setRestitutionLimAng(real_t restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; } + void setDampingLimAng(real_t dampingLimAng) { m_dampingLimAng = dampingLimAng; } + void setSoftnessOrthoLin(real_t softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; } + void setRestitutionOrthoLin(real_t restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; } + void setDampingOrthoLin(real_t dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; } + void setSoftnessOrthoAng(real_t softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; } + void setRestitutionOrthoAng(real_t restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; } + void setDampingOrthoAng(real_t dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; } + void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; } + bool getPoweredLinMotor() { return m_poweredLinMotor; } + void setTargetLinMotorVelocity(real_t targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; } + real_t getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; } + void setMaxLinMotorForce(real_t maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; } + real_t getMaxLinMotorForce() { return m_maxLinMotorForce; } + void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; } + bool getPoweredAngMotor() { return m_poweredAngMotor; } + void setTargetAngMotorVelocity(real_t targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; } + real_t getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; } + void setMaxAngMotorForce(real_t maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; } + real_t getMaxAngMotorForce() { return m_maxAngMotorForce; } + real_t getLinearPos() { return m_linPos; } + + // access for ODE solver + bool getSolveLinLimit() { return m_solveLinLim; } + real_t getLinDepth() { return m_depth[0]; } + bool getSolveAngLimit() { return m_solveAngLim; } + real_t getAngDepth() { return m_angDepth; } + // shared code used by ODE solver + void calculateTransforms(); + void testLinLimits(); + void testAngLimits(); + // access for PE Solver + Vector3 getAncorInA(); + Vector3 getAncorInB(); + + void set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value); + real_t get_param(PhysicsServer3D::SliderJointParam p_param) const; + + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; + + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_SLIDER; } +}; + +#endif // GODOT_SLIDER_JOINT_3D_H diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp deleted file mode 100644 index a45fcf7eb5..0000000000 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp +++ /dev/null @@ -1,468 +0,0 @@ -/*************************************************************************/ -/* hinge_joint_3d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -/* -Adapted to Godot from the Bullet library. -*/ - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "hinge_joint_3d_sw.h" - -static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) { - if (Math::abs(n.z) > Math_SQRT12) { - // choose p in y-z plane - real_t a = n[1] * n[1] + n[2] * n[2]; - real_t k = 1.0 / Math::sqrt(a); - p = Vector3(0, -n[2] * k, n[1] * k); - // set q = n x p - q = Vector3(a * k, -n[0] * p[2], n[0] * p[1]); - } else { - // choose p in x-y plane - real_t a = n.x * n.x + n.y * n.y; - real_t k = 1.0 / Math::sqrt(a); - p = Vector3(-n.y * k, n.x * k, 0); - // set q = n x p - q = Vector3(-n.z * p.y, n.z * p.x, a * k); - } -} - -HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameA, const Transform3D &frameB) : - Joint3DSW(_arr, 2) { - A = rbA; - B = rbB; - - m_rbAFrame = frameA; - m_rbBFrame = frameB; - // flip axis - m_rbBFrame.basis[0][2] *= real_t(-1.); - m_rbBFrame.basis[1][2] *= real_t(-1.); - m_rbBFrame.basis[2][2] *= real_t(-1.); - - A->add_constraint(this, 0); - B->add_constraint(this, 1); -} - -HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, - const Vector3 &axisInA, const Vector3 &axisInB) : - Joint3DSW(_arr, 2) { - A = rbA; - B = rbB; - - m_rbAFrame.origin = pivotInA; - - // since no frame is given, assume this to be zero angle and just pick rb transform axis - Vector3 rbAxisA1 = rbA->get_transform().basis.get_axis(0); - - Vector3 rbAxisA2; - real_t projection = axisInA.dot(rbAxisA1); - if (projection >= 1.0f - CMP_EPSILON) { - rbAxisA1 = -rbA->get_transform().basis.get_axis(2); - rbAxisA2 = rbA->get_transform().basis.get_axis(1); - } else if (projection <= -1.0f + CMP_EPSILON) { - rbAxisA1 = rbA->get_transform().basis.get_axis(2); - rbAxisA2 = rbA->get_transform().basis.get_axis(1); - } else { - rbAxisA2 = axisInA.cross(rbAxisA1); - rbAxisA1 = rbAxisA2.cross(axisInA); - } - - m_rbAFrame.basis = Basis(rbAxisA1.x, rbAxisA2.x, axisInA.x, - rbAxisA1.y, rbAxisA2.y, axisInA.y, - rbAxisA1.z, rbAxisA2.z, axisInA.z); - - Quaternion rotationArc = Quaternion(axisInA, axisInB); - Vector3 rbAxisB1 = rotationArc.xform(rbAxisA1); - Vector3 rbAxisB2 = axisInB.cross(rbAxisB1); - - m_rbBFrame.origin = pivotInB; - m_rbBFrame.basis = Basis(rbAxisB1.x, rbAxisB2.x, -axisInB.x, - rbAxisB1.y, rbAxisB2.y, -axisInB.y, - rbAxisB1.z, rbAxisB2.z, -axisInB.z); - - A->add_constraint(this, 0); - B->add_constraint(this, 1); -} - -bool HingeJoint3DSW::setup(real_t p_step) { - dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); - dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); - - if (!dynamic_A && !dynamic_B) { - return false; - } - - m_appliedImpulse = real_t(0.); - - if (!m_angularOnly) { - Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); - Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); - Vector3 relPos = pivotBInW - pivotAInW; - - Vector3 normal[3]; - if (Math::is_zero_approx(relPos.length_squared())) { - normal[0] = Vector3(real_t(1.0), 0, 0); - } else { - normal[0] = relPos.normalized(); - } - - plane_space(normal[0], normal[1], normal[2]); - - for (int i = 0; i < 3; i++) { - memnew_placement(&m_jac[i], JacobianEntry3DSW( - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - pivotAInW - A->get_transform().origin - A->get_center_of_mass(), - pivotBInW - B->get_transform().origin - B->get_center_of_mass(), - normal[i], - A->get_inv_inertia(), - A->get_inv_mass(), - B->get_inv_inertia(), - B->get_inv_mass())); - } - } - - //calculate two perpendicular jointAxis, orthogonal to hingeAxis - //these two jointAxis require equal angular velocities for both bodies - - //this is unused for now, it's a todo - Vector3 jointAxis0local; - Vector3 jointAxis1local; - - plane_space(m_rbAFrame.basis.get_axis(2), jointAxis0local, jointAxis1local); - - Vector3 jointAxis0 = A->get_transform().basis.xform(jointAxis0local); - Vector3 jointAxis1 = A->get_transform().basis.xform(jointAxis1local); - Vector3 hingeAxisWorld = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); - - memnew_placement(&m_jacAng[0], JacobianEntry3DSW(jointAxis0, - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia())); - - memnew_placement(&m_jacAng[1], JacobianEntry3DSW(jointAxis1, - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia())); - - memnew_placement(&m_jacAng[2], JacobianEntry3DSW(hingeAxisWorld, - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia())); - - // Compute limit information - real_t hingeAngle = get_hinge_angle(); - - //set bias, sign, clear accumulator - m_correction = real_t(0.); - m_limitSign = real_t(0.); - m_solveLimit = false; - m_accLimitImpulse = real_t(0.); - - //if (m_lowerLimit < m_upperLimit) - if (m_useLimit && m_lowerLimit <= m_upperLimit) { - //if (hingeAngle <= m_lowerLimit*m_limitSoftness) - if (hingeAngle <= m_lowerLimit) { - m_correction = (m_lowerLimit - hingeAngle); - m_limitSign = 1.0f; - m_solveLimit = true; - } - //else if (hingeAngle >= m_upperLimit*m_limitSoftness) - else if (hingeAngle >= m_upperLimit) { - m_correction = m_upperLimit - hingeAngle; - m_limitSign = -1.0f; - m_solveLimit = true; - } - } - - //Compute K = J*W*J' for hinge axis - Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); - m_kHinge = 1.0f / (A->compute_angular_impulse_denominator(axisA) + - B->compute_angular_impulse_denominator(axisA)); - - return true; -} - -void HingeJoint3DSW::solve(real_t p_step) { - Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); - Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); - - //real_t tau = real_t(0.3); - - //linear part - if (!m_angularOnly) { - Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; - Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; - - Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1); - Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2); - Vector3 vel = vel1 - vel2; - - for (int i = 0; i < 3; i++) { - const Vector3 &normal = m_jac[i].m_linearJointAxis; - real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal(); - - real_t rel_vel; - rel_vel = normal.dot(vel); - //positional error (zeroth order error) - real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - real_t impulse = depth * tau / p_step * jacDiagABInv - rel_vel * jacDiagABInv; - m_appliedImpulse += impulse; - Vector3 impulse_vector = normal * impulse; - if (dynamic_A) { - A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); - } - if (dynamic_B) { - B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); - } - } - } - - { - ///solve angular part - - // get axes in world space - Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); - Vector3 axisB = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(2)); - - const Vector3 &angVelA = A->get_angular_velocity(); - const Vector3 &angVelB = B->get_angular_velocity(); - - Vector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); - Vector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); - - Vector3 angAorthog = angVelA - angVelAroundHingeAxisA; - Vector3 angBorthog = angVelB - angVelAroundHingeAxisB; - Vector3 velrelOrthog = angAorthog - angBorthog; - { - //solve orthogonal angular velocity correction - real_t relaxation = real_t(1.); - real_t len = velrelOrthog.length(); - if (len > real_t(0.00001)) { - Vector3 normal = velrelOrthog.normalized(); - real_t denom = A->compute_angular_impulse_denominator(normal) + - B->compute_angular_impulse_denominator(normal); - // scale for mass and relaxation - velrelOrthog *= (real_t(1.) / denom) * m_relaxationFactor; - } - - //solve angular positional correction - Vector3 angularError = -axisA.cross(axisB) * (real_t(1.) / p_step); - real_t len2 = angularError.length(); - if (len2 > real_t(0.00001)) { - Vector3 normal2 = angularError.normalized(); - real_t denom2 = A->compute_angular_impulse_denominator(normal2) + - B->compute_angular_impulse_denominator(normal2); - angularError *= (real_t(1.) / denom2) * relaxation; - } - - if (dynamic_A) { - A->apply_torque_impulse(-velrelOrthog + angularError); - } - if (dynamic_B) { - B->apply_torque_impulse(velrelOrthog - angularError); - } - - // solve limit - if (m_solveLimit) { - real_t amplitude = ((angVelB - angVelA).dot(axisA) * m_relaxationFactor + m_correction * (real_t(1.) / p_step) * m_biasFactor) * m_limitSign; - - real_t impulseMag = amplitude * m_kHinge; - - // Clamp the accumulated impulse - real_t temp = m_accLimitImpulse; - m_accLimitImpulse = MAX(m_accLimitImpulse + impulseMag, real_t(0)); - impulseMag = m_accLimitImpulse - temp; - - Vector3 impulse = axisA * impulseMag * m_limitSign; - if (dynamic_A) { - A->apply_torque_impulse(impulse); - } - if (dynamic_B) { - B->apply_torque_impulse(-impulse); - } - } - } - - //apply motor - if (m_enableAngularMotor) { - //todo: add limits too - Vector3 angularLimit(0, 0, 0); - - Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; - real_t projRelVel = velrel.dot(axisA); - - real_t desiredMotorVel = m_motorTargetVelocity; - real_t motor_relvel = desiredMotorVel - projRelVel; - - real_t unclippedMotorImpulse = m_kHinge * motor_relvel; - //todo: should clip against accumulated impulse - real_t clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; - clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; - Vector3 motorImp = clippedMotorImpulse * axisA; - - if (dynamic_A) { - A->apply_torque_impulse(motorImp + angularLimit); - } - if (dynamic_B) { - B->apply_torque_impulse(-motorImp - angularLimit); - } - } - } -} - -/* -void HingeJointSW::updateRHS(real_t timeStep) -{ - (void)timeStep; -} - -*/ - -static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { - real_t coeff_1 = Math_PI / 4.0f; - real_t coeff_2 = 3.0f * coeff_1; - real_t abs_y = Math::abs(y); - real_t angle; - if (x >= 0.0f) { - real_t r = (x - abs_y) / (x + abs_y); - angle = coeff_1 - coeff_1 * r; - } else { - real_t r = (x + abs_y) / (abs_y - x); - angle = coeff_2 - coeff_1 * r; - } - return (y < 0.0f) ? -angle : angle; -} - -real_t HingeJoint3DSW::get_hinge_angle() { - const Vector3 refAxis0 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(0)); - const Vector3 refAxis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(1)); - const Vector3 swingAxis = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(1)); - - return atan2fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); -} - -void HingeJoint3DSW::set_param(PhysicsServer3D::HingeJointParam p_param, real_t p_value) { - switch (p_param) { - case PhysicsServer3D::HINGE_JOINT_BIAS: - tau = p_value; - break; - case PhysicsServer3D::HINGE_JOINT_LIMIT_UPPER: - m_upperLimit = p_value; - break; - case PhysicsServer3D::HINGE_JOINT_LIMIT_LOWER: - m_lowerLimit = p_value; - break; - case PhysicsServer3D::HINGE_JOINT_LIMIT_BIAS: - m_biasFactor = p_value; - break; - case PhysicsServer3D::HINGE_JOINT_LIMIT_SOFTNESS: - m_limitSoftness = p_value; - break; - case PhysicsServer3D::HINGE_JOINT_LIMIT_RELAXATION: - m_relaxationFactor = p_value; - break; - case PhysicsServer3D::HINGE_JOINT_MOTOR_TARGET_VELOCITY: - m_motorTargetVelocity = p_value; - break; - case PhysicsServer3D::HINGE_JOINT_MOTOR_MAX_IMPULSE: - m_maxMotorImpulse = p_value; - break; - case PhysicsServer3D::HINGE_JOINT_MAX: - break; // Can't happen, but silences warning - } -} - -real_t HingeJoint3DSW::get_param(PhysicsServer3D::HingeJointParam p_param) const { - switch (p_param) { - case PhysicsServer3D::HINGE_JOINT_BIAS: - return tau; - case PhysicsServer3D::HINGE_JOINT_LIMIT_UPPER: - return m_upperLimit; - case PhysicsServer3D::HINGE_JOINT_LIMIT_LOWER: - return m_lowerLimit; - case PhysicsServer3D::HINGE_JOINT_LIMIT_BIAS: - return m_biasFactor; - case PhysicsServer3D::HINGE_JOINT_LIMIT_SOFTNESS: - return m_limitSoftness; - case PhysicsServer3D::HINGE_JOINT_LIMIT_RELAXATION: - return m_relaxationFactor; - case PhysicsServer3D::HINGE_JOINT_MOTOR_TARGET_VELOCITY: - return m_motorTargetVelocity; - case PhysicsServer3D::HINGE_JOINT_MOTOR_MAX_IMPULSE: - return m_maxMotorImpulse; - case PhysicsServer3D::HINGE_JOINT_MAX: - break; // Can't happen, but silences warning - } - - return 0; -} - -void HingeJoint3DSW::set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value) { - switch (p_flag) { - case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT: - m_useLimit = p_value; - break; - case PhysicsServer3D::HINGE_JOINT_FLAG_ENABLE_MOTOR: - m_enableAngularMotor = p_value; - break; - case PhysicsServer3D::HINGE_JOINT_FLAG_MAX: - break; // Can't happen, but silences warning - } -} - -bool HingeJoint3DSW::get_flag(PhysicsServer3D::HingeJointFlag p_flag) const { - switch (p_flag) { - case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT: - return m_useLimit; - case PhysicsServer3D::HINGE_JOINT_FLAG_ENABLE_MOTOR: - return m_enableAngularMotor; - case PhysicsServer3D::HINGE_JOINT_FLAG_MAX: - break; // Can't happen, but silences warning - } - - return false; -} diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.h b/servers/physics_3d/joints/hinge_joint_3d_sw.h deleted file mode 100644 index a4ceff9ffe..0000000000 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.h +++ /dev/null @@ -1,116 +0,0 @@ -/*************************************************************************/ -/* hinge_joint_3d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -/* -Adapted to Godot from the Bullet library. -*/ - -#ifndef HINGE_JOINT_SW_H -#define HINGE_JOINT_SW_H - -#include "servers/physics_3d/joints/jacobian_entry_3d_sw.h" -#include "servers/physics_3d/joints_3d_sw.h" - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -class HingeJoint3DSW : public Joint3DSW { - union { - struct { - Body3DSW *A; - Body3DSW *B; - }; - - Body3DSW *_arr[2] = {}; - }; - - JacobianEntry3DSW m_jac[3]; //3 orthogonal linear constraints - JacobianEntry3DSW m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor - - Transform3D m_rbAFrame; // constraint axii. Assumes z is hinge axis. - Transform3D m_rbBFrame; - - real_t m_motorTargetVelocity = 0.0; - real_t m_maxMotorImpulse = 0.0; - - real_t m_limitSoftness = 0.9; - real_t m_biasFactor = 0.3; - real_t m_relaxationFactor = 1.0; - - real_t m_lowerLimit = Math_PI; - real_t m_upperLimit = -Math_PI; - - real_t m_kHinge = 0.0; - - real_t m_limitSign = 0.0; - real_t m_correction = 0.0; - - real_t m_accLimitImpulse = 0.0; - - real_t tau = 0.3; - - bool m_useLimit = false; - bool m_angularOnly = false; - bool m_enableAngularMotor = false; - bool m_solveLimit = false; - - real_t m_appliedImpulse = 0.0; - -public: - virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_HINGE; } - - virtual bool setup(real_t p_step) override; - virtual void solve(real_t p_step) override; - - real_t get_hinge_angle(); - - void set_param(PhysicsServer3D::HingeJointParam p_param, real_t p_value); - real_t get_param(PhysicsServer3D::HingeJointParam p_param) const; - - void set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value); - bool get_flag(PhysicsServer3D::HingeJointFlag p_flag) const; - - HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameA, const Transform3D &frameB); - HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB); -}; - -#endif // HINGE_JOINT_SW_H diff --git a/servers/physics_3d/joints/jacobian_entry_3d_sw.h b/servers/physics_3d/joints/jacobian_entry_3d_sw.h deleted file mode 100644 index 7294ff78e3..0000000000 --- a/servers/physics_3d/joints/jacobian_entry_3d_sw.h +++ /dev/null @@ -1,169 +0,0 @@ -/*************************************************************************/ -/* jacobian_entry_3d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -/* -Adapted to Godot from the Bullet library. -*/ - -#ifndef JACOBIAN_ENTRY_SW_H -#define JACOBIAN_ENTRY_SW_H - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "core/math/transform_3d.h" - -class JacobianEntry3DSW { -public: - JacobianEntry3DSW() {} - //constraint between two different rigidbodies - JacobianEntry3DSW( - const Basis &world2A, - const Basis &world2B, - const Vector3 &rel_pos1, const Vector3 &rel_pos2, - const Vector3 &jointAxis, - const Vector3 &inertiaInvA, - const real_t massInvA, - const Vector3 &inertiaInvB, - const real_t massInvB) : - m_linearJointAxis(jointAxis) { - m_aJ = world2A.xform(rel_pos1.cross(m_linearJointAxis)); - m_bJ = world2B.xform(rel_pos2.cross(-m_linearJointAxis)); - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = inertiaInvB * m_bJ; - m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); - - ERR_FAIL_COND(m_Adiag <= real_t(0.0)); - } - - //angular constraint between two different rigidbodies - JacobianEntry3DSW(const Vector3 &jointAxis, - const Basis &world2A, - const Basis &world2B, - const Vector3 &inertiaInvA, - const Vector3 &inertiaInvB) : - m_linearJointAxis(Vector3(real_t(0.), real_t(0.), real_t(0.))) { - m_aJ = world2A.xform(jointAxis); - m_bJ = world2B.xform(-jointAxis); - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = inertiaInvB * m_bJ; - m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); - - ERR_FAIL_COND(m_Adiag <= real_t(0.0)); - } - - //angular constraint between two different rigidbodies - JacobianEntry3DSW(const Vector3 &axisInA, - const Vector3 &axisInB, - const Vector3 &inertiaInvA, - const Vector3 &inertiaInvB) : - m_linearJointAxis(Vector3(real_t(0.), real_t(0.), real_t(0.))), - m_aJ(axisInA), - m_bJ(-axisInB) { - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = inertiaInvB * m_bJ; - m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); - - ERR_FAIL_COND(m_Adiag <= real_t(0.0)); - } - - //constraint on one rigidbody - JacobianEntry3DSW( - const Basis &world2A, - const Vector3 &rel_pos1, const Vector3 &rel_pos2, - const Vector3 &jointAxis, - const Vector3 &inertiaInvA, - const real_t massInvA) : - m_linearJointAxis(jointAxis) { - m_aJ = world2A.xform(rel_pos1.cross(jointAxis)); - m_bJ = world2A.xform(rel_pos2.cross(-jointAxis)); - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = Vector3(real_t(0.), real_t(0.), real_t(0.)); - m_Adiag = massInvA + m_0MinvJt.dot(m_aJ); - - ERR_FAIL_COND(m_Adiag <= real_t(0.0)); - } - - real_t getDiagonal() const { return m_Adiag; } - - // for two constraints on the same rigidbody (for example vehicle friction) - real_t getNonDiagonal(const JacobianEntry3DSW &jacB, const real_t massInvA) const { - const JacobianEntry3DSW &jacA = *this; - real_t lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis); - real_t ang = jacA.m_0MinvJt.dot(jacB.m_aJ); - return lin + ang; - } - - // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) - real_t getNonDiagonal(const JacobianEntry3DSW &jacB, const real_t massInvA, const real_t massInvB) const { - const JacobianEntry3DSW &jacA = *this; - Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis; - Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ; - Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ; - Vector3 lin0 = massInvA * lin; - Vector3 lin1 = massInvB * lin; - Vector3 sum = ang0 + ang1 + lin0 + lin1; - return sum[0] + sum[1] + sum[2]; - } - - real_t getRelativeVelocity(const Vector3 &linvelA, const Vector3 &angvelA, const Vector3 &linvelB, const Vector3 &angvelB) { - Vector3 linrel = linvelA - linvelB; - Vector3 angvela = angvelA * m_aJ; - Vector3 angvelb = angvelB * m_bJ; - linrel *= m_linearJointAxis; - angvela += angvelb; - angvela += linrel; - real_t rel_vel2 = angvela[0] + angvela[1] + angvela[2]; - return rel_vel2 + CMP_EPSILON; - } - //private: - - Vector3 m_linearJointAxis; - Vector3 m_aJ; - Vector3 m_bJ; - Vector3 m_0MinvJt; - Vector3 m_1MinvJt; - //Optimization: can be stored in the w/last component of one of the vectors - real_t m_Adiag = 1.0; -}; - -#endif // JACOBIAN_ENTRY_SW_H diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.cpp b/servers/physics_3d/joints/pin_joint_3d_sw.cpp deleted file mode 100644 index f41151ec0e..0000000000 --- a/servers/physics_3d/joints/pin_joint_3d_sw.cpp +++ /dev/null @@ -1,179 +0,0 @@ -/*************************************************************************/ -/* pin_joint_3d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -/* -Adapted to Godot from the Bullet library. -*/ - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "pin_joint_3d_sw.h" - -bool PinJoint3DSW::setup(real_t p_step) { - dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); - dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); - - if (!dynamic_A && !dynamic_B) { - return false; - } - - m_appliedImpulse = real_t(0.); - - Vector3 normal(0, 0, 0); - - for (int i = 0; i < 3; i++) { - normal[i] = 1; - memnew_placement(&m_jac[i], JacobianEntry3DSW( - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_transform().xform(m_pivotInA) - A->get_transform().origin - A->get_center_of_mass(), - B->get_transform().xform(m_pivotInB) - B->get_transform().origin - B->get_center_of_mass(), - normal, - A->get_inv_inertia(), - A->get_inv_mass(), - B->get_inv_inertia(), - B->get_inv_mass())); - normal[i] = 0; - } - - return true; -} - -void PinJoint3DSW::solve(real_t p_step) { - Vector3 pivotAInW = A->get_transform().xform(m_pivotInA); - Vector3 pivotBInW = B->get_transform().xform(m_pivotInB); - - Vector3 normal(0, 0, 0); - - //Vector3 angvelA = A->get_transform().origin.getBasis().transpose() * A->getAngularVelocity(); - //Vector3 angvelB = B->get_transform().origin.getBasis().transpose() * B->getAngularVelocity(); - - for (int i = 0; i < 3; i++) { - normal[i] = 1; - real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal(); - - Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; - Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; - //this jacobian entry could be re-used for all iterations - - Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1); - Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2); - Vector3 vel = vel1 - vel2; - - real_t rel_vel; - rel_vel = normal.dot(vel); - - /* - //velocity error (first order error) - real_t rel_vel = m_jac[i].getRelativeVelocity(A->getLinearVelocity(),angvelA, - B->getLinearVelocity(),angvelB); - */ - - //positional error (zeroth order error) - real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - - real_t impulse = depth * m_tau / p_step * jacDiagABInv - m_damping * rel_vel * jacDiagABInv; - - real_t impulseClamp = m_impulseClamp; - if (impulseClamp > 0) { - if (impulse < -impulseClamp) { - impulse = -impulseClamp; - } - if (impulse > impulseClamp) { - impulse = impulseClamp; - } - } - - m_appliedImpulse += impulse; - Vector3 impulse_vector = normal * impulse; - if (dynamic_A) { - A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); - } - if (dynamic_B) { - B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); - } - - normal[i] = 0; - } -} - -void PinJoint3DSW::set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value) { - switch (p_param) { - case PhysicsServer3D::PIN_JOINT_BIAS: - m_tau = p_value; - break; - case PhysicsServer3D::PIN_JOINT_DAMPING: - m_damping = p_value; - break; - case PhysicsServer3D::PIN_JOINT_IMPULSE_CLAMP: - m_impulseClamp = p_value; - break; - } -} - -real_t PinJoint3DSW::get_param(PhysicsServer3D::PinJointParam p_param) const { - switch (p_param) { - case PhysicsServer3D::PIN_JOINT_BIAS: - return m_tau; - case PhysicsServer3D::PIN_JOINT_DAMPING: - return m_damping; - case PhysicsServer3D::PIN_JOINT_IMPULSE_CLAMP: - return m_impulseClamp; - } - - return 0; -} - -PinJoint3DSW::PinJoint3DSW(Body3DSW *p_body_a, const Vector3 &p_pos_a, Body3DSW *p_body_b, const Vector3 &p_pos_b) : - Joint3DSW(_arr, 2) { - A = p_body_a; - B = p_body_b; - m_pivotInA = p_pos_a; - m_pivotInB = p_pos_b; - - A->add_constraint(this, 0); - B->add_constraint(this, 1); -} - -PinJoint3DSW::~PinJoint3DSW() { -} diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.h b/servers/physics_3d/joints/pin_joint_3d_sw.h deleted file mode 100644 index 79af48f2a5..0000000000 --- a/servers/physics_3d/joints/pin_joint_3d_sw.h +++ /dev/null @@ -1,95 +0,0 @@ -/*************************************************************************/ -/* pin_joint_3d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -/* -Adapted to Godot from the Bullet library. -*/ - -#ifndef PIN_JOINT_SW_H -#define PIN_JOINT_SW_H - -#include "servers/physics_3d/joints/jacobian_entry_3d_sw.h" -#include "servers/physics_3d/joints_3d_sw.h" - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -class PinJoint3DSW : public Joint3DSW { - union { - struct { - Body3DSW *A; - Body3DSW *B; - }; - - Body3DSW *_arr[2] = {}; - }; - - real_t m_tau = 0.3; //bias - real_t m_damping = 1.0; - real_t m_impulseClamp = 0.0; - real_t m_appliedImpulse = 0.0; - - JacobianEntry3DSW m_jac[3] = {}; //3 orthogonal linear constraints - - Vector3 m_pivotInA; - Vector3 m_pivotInB; - -public: - virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_PIN; } - - virtual bool setup(real_t p_step) override; - virtual void solve(real_t p_step) override; - - void set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value); - real_t get_param(PhysicsServer3D::PinJointParam p_param) const; - - void set_pos_a(const Vector3 &p_pos) { m_pivotInA = p_pos; } - void set_pos_b(const Vector3 &p_pos) { m_pivotInB = p_pos; } - - Vector3 get_position_a() { return m_pivotInA; } - Vector3 get_position_b() { return m_pivotInB; } - - PinJoint3DSW(Body3DSW *p_body_a, const Vector3 &p_pos_a, Body3DSW *p_body_b, const Vector3 &p_pos_b); - ~PinJoint3DSW(); -}; - -#endif // PIN_JOINT_SW_H diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.cpp b/servers/physics_3d/joints/slider_joint_3d_sw.cpp deleted file mode 100644 index e10ed436d5..0000000000 --- a/servers/physics_3d/joints/slider_joint_3d_sw.cpp +++ /dev/null @@ -1,495 +0,0 @@ -/*************************************************************************/ -/* slider_joint_3d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -/* -Adapted to Godot from the Bullet library. -*/ - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -Added by Roman Ponomarev (rponom@gmail.com) -April 04, 2008 - -*/ - -#include "slider_joint_3d_sw.h" - -//----------------------------------------------------------------------------- - -static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { - real_t coeff_1 = Math_PI / 4.0f; - real_t coeff_2 = 3.0f * coeff_1; - real_t abs_y = Math::abs(y); - real_t angle; - if (x >= 0.0f) { - real_t r = (x - abs_y) / (x + abs_y); - angle = coeff_1 - coeff_1 * r; - } else { - real_t r = (x + abs_y) / (abs_y - x); - angle = coeff_2 - coeff_1 * r; - } - return (y < 0.0f) ? -angle : angle; -} - -//----------------------------------------------------------------------------- - -//----------------------------------------------------------------------------- - -SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB) : - Joint3DSW(_arr, 2), - m_frameInA(frameInA), - m_frameInB(frameInB) { - A = rbA; - B = rbB; - - A->add_constraint(this, 0); - B->add_constraint(this, 1); -} // SliderJointSW::SliderJointSW() - -//----------------------------------------------------------------------------- - -bool SliderJoint3DSW::setup(real_t p_step) { - dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); - dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); - - if (!dynamic_A && !dynamic_B) { - return false; - } - - //calculate transforms - m_calculatedTransformA = A->get_transform() * m_frameInA; - m_calculatedTransformB = B->get_transform() * m_frameInB; - m_realPivotAInW = m_calculatedTransformA.origin; - m_realPivotBInW = m_calculatedTransformB.origin; - m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X - m_delta = m_realPivotBInW - m_realPivotAInW; - m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; - m_relPosA = m_projPivotInW - A->get_transform().origin; - m_relPosB = m_realPivotBInW - B->get_transform().origin; - Vector3 normalWorld; - int i; - //linear part - for (i = 0; i < 3; i++) { - normalWorld = m_calculatedTransformA.basis.get_axis(i); - memnew_placement(&m_jacLin[i], JacobianEntry3DSW( - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - m_relPosA - A->get_center_of_mass(), - m_relPosB - B->get_center_of_mass(), - normalWorld, - A->get_inv_inertia(), - A->get_inv_mass(), - B->get_inv_inertia(), - B->get_inv_mass())); - m_jacLinDiagABInv[i] = real_t(1.) / m_jacLin[i].getDiagonal(); - m_depth[i] = m_delta.dot(normalWorld); - } - testLinLimits(); - // angular part - for (i = 0; i < 3; i++) { - normalWorld = m_calculatedTransformA.basis.get_axis(i); - memnew_placement(&m_jacAng[i], JacobianEntry3DSW( - normalWorld, - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia())); - } - testAngLimits(); - Vector3 axisA = m_calculatedTransformA.basis.get_axis(0); - m_kAngle = real_t(1.0) / (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA)); - // clear accumulator for motors - m_accumulatedLinMotorImpulse = real_t(0.0); - m_accumulatedAngMotorImpulse = real_t(0.0); - - return true; -} // SliderJointSW::buildJacobianInt() - -//----------------------------------------------------------------------------- - -void SliderJoint3DSW::solve(real_t p_step) { - int i; - // linear - Vector3 velA = A->get_velocity_in_local_point(m_relPosA); - Vector3 velB = B->get_velocity_in_local_point(m_relPosB); - Vector3 vel = velA - velB; - for (i = 0; i < 3; i++) { - const Vector3 &normal = m_jacLin[i].m_linearJointAxis; - real_t rel_vel = normal.dot(vel); - // calculate positional error - real_t depth = m_depth[i]; - // get parameters - real_t softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin); - real_t restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin); - real_t damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin); - // Calculate and apply impulse. - real_t normalImpulse = softness * (restitution * depth / p_step - damping * rel_vel) * m_jacLinDiagABInv[i]; - Vector3 impulse_vector = normal * normalImpulse; - if (dynamic_A) { - A->apply_impulse(impulse_vector, m_relPosA); - } - if (dynamic_B) { - B->apply_impulse(-impulse_vector, m_relPosB); - } - if (m_poweredLinMotor && (!i)) { // apply linear motor - if (m_accumulatedLinMotorImpulse < m_maxLinMotorForce) { - real_t desiredMotorVel = m_targetLinMotorVelocity; - real_t motor_relvel = desiredMotorVel + rel_vel; - normalImpulse = -motor_relvel * m_jacLinDiagABInv[i]; - // clamp accumulated impulse - real_t new_acc = m_accumulatedLinMotorImpulse + Math::abs(normalImpulse); - if (new_acc > m_maxLinMotorForce) { - new_acc = m_maxLinMotorForce; - } - real_t del = new_acc - m_accumulatedLinMotorImpulse; - if (normalImpulse < real_t(0.0)) { - normalImpulse = -del; - } else { - normalImpulse = del; - } - m_accumulatedLinMotorImpulse = new_acc; - // apply clamped impulse - impulse_vector = normal * normalImpulse; - if (dynamic_A) { - A->apply_impulse(impulse_vector, m_relPosA); - } - if (dynamic_B) { - B->apply_impulse(-impulse_vector, m_relPosB); - } - } - } - } - // angular - // get axes in world space - Vector3 axisA = m_calculatedTransformA.basis.get_axis(0); - Vector3 axisB = m_calculatedTransformB.basis.get_axis(0); - - const Vector3 &angVelA = A->get_angular_velocity(); - const Vector3 &angVelB = B->get_angular_velocity(); - - Vector3 angVelAroundAxisA = axisA * axisA.dot(angVelA); - Vector3 angVelAroundAxisB = axisB * axisB.dot(angVelB); - - Vector3 angAorthog = angVelA - angVelAroundAxisA; - Vector3 angBorthog = angVelB - angVelAroundAxisB; - Vector3 velrelOrthog = angAorthog - angBorthog; - //solve orthogonal angular velocity correction - real_t len = velrelOrthog.length(); - if (len > real_t(0.00001)) { - Vector3 normal = velrelOrthog.normalized(); - real_t denom = A->compute_angular_impulse_denominator(normal) + B->compute_angular_impulse_denominator(normal); - velrelOrthog *= (real_t(1.) / denom) * m_dampingOrthoAng * m_softnessOrthoAng; - } - //solve angular positional correction - Vector3 angularError = axisA.cross(axisB) * (real_t(1.) / p_step); - real_t len2 = angularError.length(); - if (len2 > real_t(0.00001)) { - Vector3 normal2 = angularError.normalized(); - real_t denom2 = A->compute_angular_impulse_denominator(normal2) + B->compute_angular_impulse_denominator(normal2); - angularError *= (real_t(1.) / denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; - } - // apply impulse - if (dynamic_A) { - A->apply_torque_impulse(-velrelOrthog + angularError); - } - if (dynamic_B) { - B->apply_torque_impulse(velrelOrthog - angularError); - } - real_t impulseMag; - //solve angular limits - if (m_solveAngLim) { - impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / p_step; - impulseMag *= m_kAngle * m_softnessLimAng; - } else { - impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / p_step; - impulseMag *= m_kAngle * m_softnessDirAng; - } - Vector3 impulse = axisA * impulseMag; - if (dynamic_A) { - A->apply_torque_impulse(impulse); - } - if (dynamic_B) { - B->apply_torque_impulse(-impulse); - } - //apply angular motor - if (m_poweredAngMotor) { - if (m_accumulatedAngMotorImpulse < m_maxAngMotorForce) { - Vector3 velrel = angVelAroundAxisA - angVelAroundAxisB; - real_t projRelVel = velrel.dot(axisA); - - real_t desiredMotorVel = m_targetAngMotorVelocity; - real_t motor_relvel = desiredMotorVel - projRelVel; - - real_t angImpulse = m_kAngle * motor_relvel; - // clamp accumulated impulse - real_t new_acc = m_accumulatedAngMotorImpulse + Math::abs(angImpulse); - if (new_acc > m_maxAngMotorForce) { - new_acc = m_maxAngMotorForce; - } - real_t del = new_acc - m_accumulatedAngMotorImpulse; - if (angImpulse < real_t(0.0)) { - angImpulse = -del; - } else { - angImpulse = del; - } - m_accumulatedAngMotorImpulse = new_acc; - // apply clamped impulse - Vector3 motorImp = angImpulse * axisA; - if (dynamic_A) { - A->apply_torque_impulse(motorImp); - } - if (dynamic_B) { - B->apply_torque_impulse(-motorImp); - } - } - } -} // SliderJointSW::solveConstraint() - -//----------------------------------------------------------------------------- - -//----------------------------------------------------------------------------- - -void SliderJoint3DSW::calculateTransforms() { - m_calculatedTransformA = A->get_transform() * m_frameInA; - m_calculatedTransformB = B->get_transform() * m_frameInB; - m_realPivotAInW = m_calculatedTransformA.origin; - m_realPivotBInW = m_calculatedTransformB.origin; - m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X - m_delta = m_realPivotBInW - m_realPivotAInW; - m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; - Vector3 normalWorld; - int i; - //linear part - for (i = 0; i < 3; i++) { - normalWorld = m_calculatedTransformA.basis.get_axis(i); - m_depth[i] = m_delta.dot(normalWorld); - } -} // SliderJointSW::calculateTransforms() - -//----------------------------------------------------------------------------- - -void SliderJoint3DSW::testLinLimits() { - m_solveLinLim = false; - m_linPos = m_depth[0]; - if (m_lowerLinLimit <= m_upperLinLimit) { - if (m_depth[0] > m_upperLinLimit) { - m_depth[0] -= m_upperLinLimit; - m_solveLinLim = true; - } else if (m_depth[0] < m_lowerLinLimit) { - m_depth[0] -= m_lowerLinLimit; - m_solveLinLim = true; - } else { - m_depth[0] = real_t(0.); - } - } else { - m_depth[0] = real_t(0.); - } -} // SliderJointSW::testLinLimits() - -//----------------------------------------------------------------------------- - -void SliderJoint3DSW::testAngLimits() { - m_angDepth = real_t(0.); - m_solveAngLim = false; - if (m_lowerAngLimit <= m_upperAngLimit) { - const Vector3 axisA0 = m_calculatedTransformA.basis.get_axis(1); - const Vector3 axisA1 = m_calculatedTransformA.basis.get_axis(2); - const Vector3 axisB0 = m_calculatedTransformB.basis.get_axis(1); - real_t rot = atan2fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); - if (rot < m_lowerAngLimit) { - m_angDepth = rot - m_lowerAngLimit; - m_solveAngLim = true; - } else if (rot > m_upperAngLimit) { - m_angDepth = rot - m_upperAngLimit; - m_solveAngLim = true; - } - } -} // SliderJointSW::testAngLimits() - -//----------------------------------------------------------------------------- - -Vector3 SliderJoint3DSW::getAncorInA() { - Vector3 ancorInA; - ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * real_t(0.5) * m_sliderAxis; - ancorInA = A->get_transform().inverse().xform(ancorInA); - return ancorInA; -} // SliderJointSW::getAncorInA() - -//----------------------------------------------------------------------------- - -Vector3 SliderJoint3DSW::getAncorInB() { - Vector3 ancorInB; - ancorInB = m_frameInB.origin; - return ancorInB; -} // SliderJointSW::getAncorInB(); - -void SliderJoint3DSW::set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value) { - switch (p_param) { - case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER: - m_upperLinLimit = p_value; - break; - case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_LOWER: - m_lowerLinLimit = p_value; - break; - case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: - m_softnessLimLin = p_value; - break; - case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: - m_restitutionLimLin = p_value; - break; - case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: - m_dampingLimLin = p_value; - break; - case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: - m_softnessDirLin = p_value; - break; - case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: - m_restitutionDirLin = p_value; - break; - case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_DAMPING: - m_dampingDirLin = p_value; - break; - case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: - m_softnessOrthoLin = p_value; - break; - case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: - m_restitutionOrthoLin = p_value; - break; - case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: - m_dampingOrthoLin = p_value; - break; - - case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: - m_upperAngLimit = p_value; - break; - case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: - m_lowerAngLimit = p_value; - break; - case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: - m_softnessLimAng = p_value; - break; - case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: - m_restitutionLimAng = p_value; - break; - case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: - m_dampingLimAng = p_value; - break; - case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: - m_softnessDirAng = p_value; - break; - case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: - m_restitutionDirAng = p_value; - break; - case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: - m_dampingDirAng = p_value; - break; - case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: - m_softnessOrthoAng = p_value; - break; - case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: - m_restitutionOrthoAng = p_value; - break; - case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: - m_dampingOrthoAng = p_value; - break; - - case PhysicsServer3D::SLIDER_JOINT_MAX: - break; // Can't happen, but silences warning - } -} - -real_t SliderJoint3DSW::get_param(PhysicsServer3D::SliderJointParam p_param) const { - switch (p_param) { - case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER: - return m_upperLinLimit; - case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_LOWER: - return m_lowerLinLimit; - case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: - return m_softnessLimLin; - case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: - return m_restitutionLimLin; - case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: - return m_dampingLimLin; - case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: - return m_softnessDirLin; - case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: - return m_restitutionDirLin; - case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_DAMPING: - return m_dampingDirLin; - case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: - return m_softnessOrthoLin; - case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: - return m_restitutionOrthoLin; - case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: - return m_dampingOrthoLin; - - case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: - return m_upperAngLimit; - case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: - return m_lowerAngLimit; - case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: - return m_softnessLimAng; - case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: - return m_restitutionLimAng; - case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: - return m_dampingLimAng; - case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: - return m_softnessDirAng; - case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: - return m_restitutionDirAng; - case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: - return m_dampingDirAng; - case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: - return m_softnessOrthoAng; - case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: - return m_restitutionOrthoAng; - case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: - return m_dampingOrthoAng; - - case PhysicsServer3D::SLIDER_JOINT_MAX: - break; // Can't happen, but silences warning - } - - return 0; -} diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.h b/servers/physics_3d/joints/slider_joint_3d_sw.h deleted file mode 100644 index d32ad9469e..0000000000 --- a/servers/physics_3d/joints/slider_joint_3d_sw.h +++ /dev/null @@ -1,246 +0,0 @@ -/*************************************************************************/ -/* slider_joint_3d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -/* -Adapted to Godot from the Bullet library. -*/ - -#ifndef SLIDER_JOINT_SW_H -#define SLIDER_JOINT_SW_H - -#include "servers/physics_3d/joints/jacobian_entry_3d_sw.h" -#include "servers/physics_3d/joints_3d_sw.h" - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -Added by Roman Ponomarev (rponom@gmail.com) -April 04, 2008 - -*/ - -#define SLIDER_CONSTRAINT_DEF_SOFTNESS (real_t(1.0)) -#define SLIDER_CONSTRAINT_DEF_DAMPING (real_t(1.0)) -#define SLIDER_CONSTRAINT_DEF_RESTITUTION (real_t(0.7)) - -//----------------------------------------------------------------------------- - -class SliderJoint3DSW : public Joint3DSW { -protected: - union { - struct { - Body3DSW *A; - Body3DSW *B; - }; - - Body3DSW *_arr[2] = { nullptr, nullptr }; - }; - - Transform3D m_frameInA; - Transform3D m_frameInB; - - // linear limits - real_t m_lowerLinLimit = 1.0; - real_t m_upperLinLimit = -1.0; - // angular limits - real_t m_lowerAngLimit = 0.0; - real_t m_upperAngLimit = 0.0; - // softness, restitution and damping for different cases - // DirLin - moving inside linear limits - // LimLin - hitting linear limit - // DirAng - moving inside angular limits - // LimAng - hitting angular limit - // OrthoLin, OrthoAng - against constraint axis - real_t m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - real_t m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - real_t m_dampingDirLin = 0.0; - real_t m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - real_t m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - real_t m_dampingDirAng = 0.0; - real_t m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - real_t m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - real_t m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING; - real_t m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - real_t m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - real_t m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING; - real_t m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - real_t m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - real_t m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING; - real_t m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - real_t m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - real_t m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING; - - // for interlal use - bool m_solveLinLim = false; - bool m_solveAngLim = false; - - JacobianEntry3DSW m_jacLin[3] = {}; - real_t m_jacLinDiagABInv[3] = {}; - - JacobianEntry3DSW m_jacAng[3] = {}; - - real_t m_timeStep = 0.0; - Transform3D m_calculatedTransformA; - Transform3D m_calculatedTransformB; - - Vector3 m_sliderAxis; - Vector3 m_realPivotAInW; - Vector3 m_realPivotBInW; - Vector3 m_projPivotInW; - Vector3 m_delta; - Vector3 m_depth; - Vector3 m_relPosA; - Vector3 m_relPosB; - - real_t m_linPos = 0.0; - - real_t m_angDepth = 0.0; - real_t m_kAngle = 0.0; - - bool m_poweredLinMotor = false; - real_t m_targetLinMotorVelocity = 0.0; - real_t m_maxLinMotorForce = 0.0; - real_t m_accumulatedLinMotorImpulse = 0.0; - - bool m_poweredAngMotor = false; - real_t m_targetAngMotorVelocity = 0.0; - real_t m_maxAngMotorForce = 0.0; - real_t m_accumulatedAngMotorImpulse = 0.0; - -public: - // constructors - SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB); - //SliderJointSW(); - // overrides - - // access - const Body3DSW *getRigidBodyA() const { return A; } - const Body3DSW *getRigidBodyB() const { return B; } - const Transform3D &getCalculatedTransformA() const { return m_calculatedTransformA; } - const Transform3D &getCalculatedTransformB() const { return m_calculatedTransformB; } - const Transform3D &getFrameOffsetA() const { return m_frameInA; } - const Transform3D &getFrameOffsetB() const { return m_frameInB; } - Transform3D &getFrameOffsetA() { return m_frameInA; } - Transform3D &getFrameOffsetB() { return m_frameInB; } - real_t getLowerLinLimit() { return m_lowerLinLimit; } - void setLowerLinLimit(real_t lowerLimit) { m_lowerLinLimit = lowerLimit; } - real_t getUpperLinLimit() { return m_upperLinLimit; } - void setUpperLinLimit(real_t upperLimit) { m_upperLinLimit = upperLimit; } - real_t getLowerAngLimit() { return m_lowerAngLimit; } - void setLowerAngLimit(real_t lowerLimit) { m_lowerAngLimit = lowerLimit; } - real_t getUpperAngLimit() { return m_upperAngLimit; } - void setUpperAngLimit(real_t upperLimit) { m_upperAngLimit = upperLimit; } - - real_t getSoftnessDirLin() { return m_softnessDirLin; } - real_t getRestitutionDirLin() { return m_restitutionDirLin; } - real_t getDampingDirLin() { return m_dampingDirLin; } - real_t getSoftnessDirAng() { return m_softnessDirAng; } - real_t getRestitutionDirAng() { return m_restitutionDirAng; } - real_t getDampingDirAng() { return m_dampingDirAng; } - real_t getSoftnessLimLin() { return m_softnessLimLin; } - real_t getRestitutionLimLin() { return m_restitutionLimLin; } - real_t getDampingLimLin() { return m_dampingLimLin; } - real_t getSoftnessLimAng() { return m_softnessLimAng; } - real_t getRestitutionLimAng() { return m_restitutionLimAng; } - real_t getDampingLimAng() { return m_dampingLimAng; } - real_t getSoftnessOrthoLin() { return m_softnessOrthoLin; } - real_t getRestitutionOrthoLin() { return m_restitutionOrthoLin; } - real_t getDampingOrthoLin() { return m_dampingOrthoLin; } - real_t getSoftnessOrthoAng() { return m_softnessOrthoAng; } - real_t getRestitutionOrthoAng() { return m_restitutionOrthoAng; } - real_t getDampingOrthoAng() { return m_dampingOrthoAng; } - void setSoftnessDirLin(real_t softnessDirLin) { m_softnessDirLin = softnessDirLin; } - void setRestitutionDirLin(real_t restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; } - void setDampingDirLin(real_t dampingDirLin) { m_dampingDirLin = dampingDirLin; } - void setSoftnessDirAng(real_t softnessDirAng) { m_softnessDirAng = softnessDirAng; } - void setRestitutionDirAng(real_t restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; } - void setDampingDirAng(real_t dampingDirAng) { m_dampingDirAng = dampingDirAng; } - void setSoftnessLimLin(real_t softnessLimLin) { m_softnessLimLin = softnessLimLin; } - void setRestitutionLimLin(real_t restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; } - void setDampingLimLin(real_t dampingLimLin) { m_dampingLimLin = dampingLimLin; } - void setSoftnessLimAng(real_t softnessLimAng) { m_softnessLimAng = softnessLimAng; } - void setRestitutionLimAng(real_t restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; } - void setDampingLimAng(real_t dampingLimAng) { m_dampingLimAng = dampingLimAng; } - void setSoftnessOrthoLin(real_t softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; } - void setRestitutionOrthoLin(real_t restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; } - void setDampingOrthoLin(real_t dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; } - void setSoftnessOrthoAng(real_t softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; } - void setRestitutionOrthoAng(real_t restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; } - void setDampingOrthoAng(real_t dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; } - void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; } - bool getPoweredLinMotor() { return m_poweredLinMotor; } - void setTargetLinMotorVelocity(real_t targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; } - real_t getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; } - void setMaxLinMotorForce(real_t maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; } - real_t getMaxLinMotorForce() { return m_maxLinMotorForce; } - void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; } - bool getPoweredAngMotor() { return m_poweredAngMotor; } - void setTargetAngMotorVelocity(real_t targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; } - real_t getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; } - void setMaxAngMotorForce(real_t maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; } - real_t getMaxAngMotorForce() { return m_maxAngMotorForce; } - real_t getLinearPos() { return m_linPos; } - - // access for ODE solver - bool getSolveLinLimit() { return m_solveLinLim; } - real_t getLinDepth() { return m_depth[0]; } - bool getSolveAngLimit() { return m_solveAngLim; } - real_t getAngDepth() { return m_angDepth; } - // shared code used by ODE solver - void calculateTransforms(); - void testLinLimits(); - void testAngLimits(); - // access for PE Solver - Vector3 getAncorInA(); - Vector3 getAncorInB(); - - void set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value); - real_t get_param(PhysicsServer3D::SliderJointParam p_param) const; - - virtual bool setup(real_t p_step) override; - virtual void solve(real_t p_step) override; - - virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_SLIDER; } -}; - -#endif // SLIDER_JOINT_SW_H diff --git a/servers/physics_3d/joints_3d_sw.h b/servers/physics_3d/joints_3d_sw.h deleted file mode 100644 index e2514674ea..0000000000 --- a/servers/physics_3d/joints_3d_sw.h +++ /dev/null @@ -1,68 +0,0 @@ -/*************************************************************************/ -/* joints_3d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 JOINTS_SW_H -#define JOINTS_SW_H - -#include "body_3d_sw.h" -#include "constraint_3d_sw.h" - -class Joint3DSW : public Constraint3DSW { -protected: - bool dynamic_A = false; - bool dynamic_B = false; - -public: - virtual bool setup(real_t p_step) override { return false; } - virtual bool pre_solve(real_t p_step) override { return true; } - virtual void solve(real_t p_step) override {} - - void copy_settings_from(Joint3DSW *p_joint) { - set_self(p_joint->get_self()); - set_priority(p_joint->get_priority()); - disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies()); - } - - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_MAX; } - _FORCE_INLINE_ Joint3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) : - Constraint3DSW(p_body_ptr, p_body_count) { - } - - virtual ~Joint3DSW() { - for (int i = 0; i < get_body_count(); i++) { - Body3DSW *body = get_body_ptr()[i]; - if (body) { - body->remove_constraint(this); - } - } - } -}; - -#endif // JOINTS_SW_H diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp deleted file mode 100644 index b16e199a03..0000000000 --- a/servers/physics_3d/physics_server_3d_sw.cpp +++ /dev/null @@ -1,1748 +0,0 @@ -/*************************************************************************/ -/* physics_server_3d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "physics_server_3d_sw.h" - -#include "body_direct_state_3d_sw.h" -#include "broad_phase_3d_bvh.h" -#include "core/debugger/engine_debugger.h" -#include "core/os/os.h" -#include "joints/cone_twist_joint_3d_sw.h" -#include "joints/generic_6dof_joint_3d_sw.h" -#include "joints/hinge_joint_3d_sw.h" -#include "joints/pin_joint_3d_sw.h" -#include "joints/slider_joint_3d_sw.h" - -#define FLUSH_QUERY_CHECK(m_object) \ - ERR_FAIL_COND_MSG(m_object->get_space() && flushing_queries, "Can't change this state while flushing queries. Use call_deferred() or set_deferred() to change monitoring state instead."); - -RID PhysicsServer3DSW::world_boundary_shape_create() { - Shape3DSW *shape = memnew(WorldBoundaryShape3DSW); - RID rid = shape_owner.make_rid(shape); - shape->set_self(rid); - return rid; -} -RID PhysicsServer3DSW::separation_ray_shape_create() { - Shape3DSW *shape = memnew(SeparationRayShape3DSW); - RID rid = shape_owner.make_rid(shape); - shape->set_self(rid); - return rid; -} -RID PhysicsServer3DSW::sphere_shape_create() { - Shape3DSW *shape = memnew(SphereShape3DSW); - RID rid = shape_owner.make_rid(shape); - shape->set_self(rid); - return rid; -} -RID PhysicsServer3DSW::box_shape_create() { - Shape3DSW *shape = memnew(BoxShape3DSW); - RID rid = shape_owner.make_rid(shape); - shape->set_self(rid); - return rid; -} -RID PhysicsServer3DSW::capsule_shape_create() { - Shape3DSW *shape = memnew(CapsuleShape3DSW); - RID rid = shape_owner.make_rid(shape); - shape->set_self(rid); - return rid; -} -RID PhysicsServer3DSW::cylinder_shape_create() { - Shape3DSW *shape = memnew(CylinderShape3DSW); - RID rid = shape_owner.make_rid(shape); - shape->set_self(rid); - return rid; -} -RID PhysicsServer3DSW::convex_polygon_shape_create() { - Shape3DSW *shape = memnew(ConvexPolygonShape3DSW); - RID rid = shape_owner.make_rid(shape); - shape->set_self(rid); - return rid; -} -RID PhysicsServer3DSW::concave_polygon_shape_create() { - Shape3DSW *shape = memnew(ConcavePolygonShape3DSW); - RID rid = shape_owner.make_rid(shape); - shape->set_self(rid); - return rid; -} -RID PhysicsServer3DSW::heightmap_shape_create() { - Shape3DSW *shape = memnew(HeightMapShape3DSW); - RID rid = shape_owner.make_rid(shape); - shape->set_self(rid); - return rid; -} -RID PhysicsServer3DSW::custom_shape_create() { - ERR_FAIL_V(RID()); -} - -void PhysicsServer3DSW::shape_set_data(RID p_shape, const Variant &p_data) { - Shape3DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND(!shape); - shape->set_data(p_data); -}; - -void PhysicsServer3DSW::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) { - Shape3DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND(!shape); - shape->set_custom_bias(p_bias); -} - -PhysicsServer3D::ShapeType PhysicsServer3DSW::shape_get_type(RID p_shape) const { - const Shape3DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND_V(!shape, SHAPE_CUSTOM); - return shape->get_type(); -}; - -Variant PhysicsServer3DSW::shape_get_data(RID p_shape) const { - const Shape3DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND_V(!shape, Variant()); - ERR_FAIL_COND_V(!shape->is_configured(), Variant()); - return shape->get_data(); -}; - -void PhysicsServer3DSW::shape_set_margin(RID p_shape, real_t p_margin) { -} - -real_t PhysicsServer3DSW::shape_get_margin(RID p_shape) const { - return 0.0; -} - -real_t PhysicsServer3DSW::shape_get_custom_solver_bias(RID p_shape) const { - const Shape3DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND_V(!shape, 0); - return shape->get_custom_bias(); -} - -RID PhysicsServer3DSW::space_create() { - Space3DSW *space = memnew(Space3DSW); - RID id = space_owner.make_rid(space); - space->set_self(id); - RID area_id = area_create(); - Area3DSW *area = area_owner.get_or_null(area_id); - ERR_FAIL_COND_V(!area, RID()); - space->set_default_area(area); - area->set_space(space); - area->set_priority(-1); - RID sgb = body_create(); - body_set_space(sgb, id); - body_set_mode(sgb, BODY_MODE_STATIC); - space->set_static_global_body(sgb); - - return id; -}; - -void PhysicsServer3DSW::space_set_active(RID p_space, bool p_active) { - Space3DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND(!space); - if (p_active) { - active_spaces.insert(space); - } else { - active_spaces.erase(space); - } -} - -bool PhysicsServer3DSW::space_is_active(RID p_space) const { - const Space3DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND_V(!space, false); - - return active_spaces.has(space); -} - -void PhysicsServer3DSW::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) { - Space3DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND(!space); - - space->set_param(p_param, p_value); -} - -real_t PhysicsServer3DSW::space_get_param(RID p_space, SpaceParameter p_param) const { - const Space3DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND_V(!space, 0); - return space->get_param(p_param); -} - -PhysicsDirectSpaceState3D *PhysicsServer3DSW::space_get_direct_state(RID p_space) { - Space3DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND_V(!space, nullptr); - ERR_FAIL_COND_V_MSG((using_threads && !doing_sync) || space->is_locked(), nullptr, "Space state is inaccessible right now, wait for iteration or physics process notification."); - - return space->get_direct_state(); -} - -void PhysicsServer3DSW::space_set_debug_contacts(RID p_space, int p_max_contacts) { - Space3DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND(!space); - space->set_debug_contacts(p_max_contacts); -} - -Vector PhysicsServer3DSW::space_get_contacts(RID p_space) const { - Space3DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND_V(!space, Vector()); - return space->get_debug_contacts(); -} - -int PhysicsServer3DSW::space_get_contact_count(RID p_space) const { - Space3DSW *space = space_owner.get_or_null(p_space); - ERR_FAIL_COND_V(!space, 0); - return space->get_debug_contact_count(); -} - -RID PhysicsServer3DSW::area_create() { - Area3DSW *area = memnew(Area3DSW); - RID rid = area_owner.make_rid(area); - area->set_self(rid); - return rid; -}; - -void PhysicsServer3DSW::area_set_space(RID p_area, RID p_space) { - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - Space3DSW *space = nullptr; - if (p_space.is_valid()) { - space = space_owner.get_or_null(p_space); - ERR_FAIL_COND(!space); - } - - if (area->get_space() == space) { - return; //pointless - } - - area->clear_constraints(); - area->set_space(space); -}; - -RID PhysicsServer3DSW::area_get_space(RID p_area) const { - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, RID()); - - Space3DSW *space = area->get_space(); - if (!space) { - return RID(); - } - return space->get_self(); -}; - -void PhysicsServer3DSW::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) { - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->set_space_override_mode(p_mode); -} - -PhysicsServer3D::AreaSpaceOverrideMode PhysicsServer3DSW::area_get_space_override_mode(RID p_area) const { - const Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, AREA_SPACE_OVERRIDE_DISABLED); - - return area->get_space_override_mode(); -} - -void PhysicsServer3DSW::area_add_shape(RID p_area, RID p_shape, const Transform3D &p_transform, bool p_disabled) { - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - Shape3DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND(!shape); - - area->add_shape(shape, p_transform, p_disabled); -} - -void PhysicsServer3DSW::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) { - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - Shape3DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND(!shape); - ERR_FAIL_COND(!shape->is_configured()); - - area->set_shape(p_shape_idx, shape); -} - -void PhysicsServer3DSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform3D &p_transform) { - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->set_shape_transform(p_shape_idx, p_transform); -} - -int PhysicsServer3DSW::area_get_shape_count(RID p_area) const { - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, -1); - - return area->get_shape_count(); -} - -RID PhysicsServer3DSW::area_get_shape(RID p_area, int p_shape_idx) const { - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, RID()); - - Shape3DSW *shape = area->get_shape(p_shape_idx); - ERR_FAIL_COND_V(!shape, RID()); - - return shape->get_self(); -} - -Transform3D PhysicsServer3DSW::area_get_shape_transform(RID p_area, int p_shape_idx) const { - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, Transform3D()); - - return area->get_shape_transform(p_shape_idx); -} - -void PhysicsServer3DSW::area_remove_shape(RID p_area, int p_shape_idx) { - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->remove_shape(p_shape_idx); -} - -void PhysicsServer3DSW::area_clear_shapes(RID p_area) { - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - while (area->get_shape_count()) { - area->remove_shape(0); - } -} - -void PhysicsServer3DSW::area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) { - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - ERR_FAIL_INDEX(p_shape_idx, area->get_shape_count()); - FLUSH_QUERY_CHECK(area); - area->set_shape_disabled(p_shape_idx, p_disabled); -} - -void PhysicsServer3DSW::area_attach_object_instance_id(RID p_area, ObjectID p_id) { - if (space_owner.owns(p_area)) { - Space3DSW *space = space_owner.get_or_null(p_area); - p_area = space->get_default_area()->get_self(); - } - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - area->set_instance_id(p_id); -} - -ObjectID PhysicsServer3DSW::area_get_object_instance_id(RID p_area) const { - if (space_owner.owns(p_area)) { - Space3DSW *space = space_owner.get_or_null(p_area); - p_area = space->get_default_area()->get_self(); - } - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, ObjectID()); - return area->get_instance_id(); -} - -void PhysicsServer3DSW::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) { - if (space_owner.owns(p_area)) { - Space3DSW *space = space_owner.get_or_null(p_area); - p_area = space->get_default_area()->get_self(); - } - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - area->set_param(p_param, p_value); -}; - -void PhysicsServer3DSW::area_set_transform(RID p_area, const Transform3D &p_transform) { - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - area->set_transform(p_transform); -}; - -Variant PhysicsServer3DSW::area_get_param(RID p_area, AreaParameter p_param) const { - if (space_owner.owns(p_area)) { - Space3DSW *space = space_owner.get_or_null(p_area); - p_area = space->get_default_area()->get_self(); - } - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, Variant()); - - return area->get_param(p_param); -}; - -Transform3D PhysicsServer3DSW::area_get_transform(RID p_area) const { - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, Transform3D()); - - return area->get_transform(); -}; - -void PhysicsServer3DSW::area_set_collision_layer(RID p_area, uint32_t p_layer) { - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->set_collision_layer(p_layer); -} - -void PhysicsServer3DSW::area_set_collision_mask(RID p_area, uint32_t p_mask) { - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->set_collision_mask(p_mask); -} - -void PhysicsServer3DSW::area_set_monitorable(RID p_area, bool p_monitorable) { - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - FLUSH_QUERY_CHECK(area); - - area->set_monitorable(p_monitorable); -} - -void PhysicsServer3DSW::area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->set_monitor_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method); -} - -void PhysicsServer3DSW::area_set_ray_pickable(RID p_area, bool p_enable) { - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->set_ray_pickable(p_enable); -} - -void PhysicsServer3DSW::area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { - Area3DSW *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->set_area_monitor_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method); -} - -/* BODY API */ - -RID PhysicsServer3DSW::body_create() { - Body3DSW *body = memnew(Body3DSW); - RID rid = body_owner.make_rid(body); - body->set_self(rid); - return rid; -}; - -void PhysicsServer3DSW::body_set_space(RID p_body, RID p_space) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - Space3DSW *space = nullptr; - if (p_space.is_valid()) { - space = space_owner.get_or_null(p_space); - ERR_FAIL_COND(!space); - } - - if (body->get_space() == space) { - return; //pointless - } - - body->clear_constraint_map(); - body->set_space(space); -}; - -RID PhysicsServer3DSW::body_get_space(RID p_body) const { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, RID()); - - Space3DSW *space = body->get_space(); - if (!space) { - return RID(); - } - return space->get_self(); -}; - -void PhysicsServer3DSW::body_set_mode(RID p_body, BodyMode p_mode) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_mode(p_mode); -}; - -PhysicsServer3D::BodyMode PhysicsServer3DSW::body_get_mode(RID p_body) const { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, BODY_MODE_STATIC); - - return body->get_mode(); -}; - -void PhysicsServer3DSW::body_add_shape(RID p_body, RID p_shape, const Transform3D &p_transform, bool p_disabled) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - Shape3DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND(!shape); - - body->add_shape(shape, p_transform, p_disabled); -} - -void PhysicsServer3DSW::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - Shape3DSW *shape = shape_owner.get_or_null(p_shape); - ERR_FAIL_COND(!shape); - ERR_FAIL_COND(!shape->is_configured()); - - body->set_shape(p_shape_idx, shape); -} -void PhysicsServer3DSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform3D &p_transform) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_shape_transform(p_shape_idx, p_transform); -} - -int PhysicsServer3DSW::body_get_shape_count(RID p_body) const { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, -1); - - return body->get_shape_count(); -} - -RID PhysicsServer3DSW::body_get_shape(RID p_body, int p_shape_idx) const { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, RID()); - - Shape3DSW *shape = body->get_shape(p_shape_idx); - ERR_FAIL_COND_V(!shape, RID()); - - return shape->get_self(); -} - -void PhysicsServer3DSW::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count()); - FLUSH_QUERY_CHECK(body); - - body->set_shape_disabled(p_shape_idx, p_disabled); -} - -Transform3D PhysicsServer3DSW::body_get_shape_transform(RID p_body, int p_shape_idx) const { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, Transform3D()); - - return body->get_shape_transform(p_shape_idx); -} - -void PhysicsServer3DSW::body_remove_shape(RID p_body, int p_shape_idx) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->remove_shape(p_shape_idx); -} - -void PhysicsServer3DSW::body_clear_shapes(RID p_body) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - while (body->get_shape_count()) { - body->remove_shape(0); - } -} - -void PhysicsServer3DSW::body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_continuous_collision_detection(p_enable); -} - -bool PhysicsServer3DSW::body_is_continuous_collision_detection_enabled(RID p_body) const { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, false); - - return body->is_continuous_collision_detection_enabled(); -} - -void PhysicsServer3DSW::body_set_collision_layer(RID p_body, uint32_t p_layer) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_collision_layer(p_layer); - body->wakeup(); -} - -uint32_t PhysicsServer3DSW::body_get_collision_layer(RID p_body) const { - const Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, 0); - - return body->get_collision_layer(); -} - -void PhysicsServer3DSW::body_set_collision_mask(RID p_body, uint32_t p_mask) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_collision_mask(p_mask); - body->wakeup(); -} - -uint32_t PhysicsServer3DSW::body_get_collision_mask(RID p_body) const { - const Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, 0); - - return body->get_collision_mask(); -} - -void PhysicsServer3DSW::body_attach_object_instance_id(RID p_body, ObjectID p_id) { - Body3DSW *body = body_owner.get_or_null(p_body); - if (body) { - body->set_instance_id(p_id); - return; - } - - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - if (soft_body) { - soft_body->set_instance_id(p_id); - return; - } - - ERR_FAIL_MSG("Invalid ID."); -}; - -ObjectID PhysicsServer3DSW::body_get_object_instance_id(RID p_body) const { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, ObjectID()); - - return body->get_instance_id(); -}; - -void PhysicsServer3DSW::body_set_user_flags(RID p_body, uint32_t p_flags) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); -}; - -uint32_t PhysicsServer3DSW::body_get_user_flags(RID p_body) const { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, 0); - - return 0; -}; - -void PhysicsServer3DSW::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_param(p_param, p_value); -}; - -Variant PhysicsServer3DSW::body_get_param(RID p_body, BodyParameter p_param) const { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, 0); - - return body->get_param(p_param); -}; - -void PhysicsServer3DSW::body_reset_mass_properties(RID p_body) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - return body->reset_mass_properties(); -} - -void PhysicsServer3DSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_state(p_state, p_variant); -}; - -Variant PhysicsServer3DSW::body_get_state(RID p_body, BodyState p_state) const { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, Variant()); - - return body->get_state(p_state); -}; - -void PhysicsServer3DSW::body_set_applied_force(RID p_body, const Vector3 &p_force) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_applied_force(p_force); - body->wakeup(); -}; - -Vector3 PhysicsServer3DSW::body_get_applied_force(RID p_body) const { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, Vector3()); - return body->get_applied_force(); -}; - -void PhysicsServer3DSW::body_set_applied_torque(RID p_body, const Vector3 &p_torque) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_applied_torque(p_torque); - body->wakeup(); -}; - -Vector3 PhysicsServer3DSW::body_get_applied_torque(RID p_body) const { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, Vector3()); - - return body->get_applied_torque(); -}; - -void PhysicsServer3DSW::body_add_central_force(RID p_body, const Vector3 &p_force) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->add_central_force(p_force); - body->wakeup(); -} - -void PhysicsServer3DSW::body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->add_force(p_force, p_position); - body->wakeup(); -}; - -void PhysicsServer3DSW::body_add_torque(RID p_body, const Vector3 &p_torque) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->add_torque(p_torque); - body->wakeup(); -}; - -void PhysicsServer3DSW::body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - _update_shapes(); - - body->apply_central_impulse(p_impulse); - body->wakeup(); -} - -void PhysicsServer3DSW::body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - _update_shapes(); - - body->apply_impulse(p_impulse, p_position); - body->wakeup(); -}; - -void PhysicsServer3DSW::body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - _update_shapes(); - - body->apply_torque_impulse(p_impulse); - body->wakeup(); -}; - -void PhysicsServer3DSW::body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - _update_shapes(); - - Vector3 v = body->get_linear_velocity(); - Vector3 axis = p_axis_velocity.normalized(); - v -= axis * axis.dot(v); - v += p_axis_velocity; - body->set_linear_velocity(v); - body->wakeup(); -}; - -void PhysicsServer3DSW::body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_axis_lock(p_axis, p_lock); - body->wakeup(); -} - -bool PhysicsServer3DSW::body_is_axis_locked(RID p_body, BodyAxis p_axis) const { - const Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, 0); - return body->is_axis_locked(p_axis); -} - -void PhysicsServer3DSW::body_add_collision_exception(RID p_body, RID p_body_b) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->add_exception(p_body_b); - body->wakeup(); -}; - -void PhysicsServer3DSW::body_remove_collision_exception(RID p_body, RID p_body_b) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->remove_exception(p_body_b); - body->wakeup(); -}; - -void PhysicsServer3DSW::body_get_collision_exceptions(RID p_body, List *p_exceptions) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - for (int i = 0; i < body->get_exceptions().size(); i++) { - p_exceptions->push_back(body->get_exceptions()[i]); - } -}; - -void PhysicsServer3DSW::body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); -}; - -real_t PhysicsServer3DSW::body_get_contacts_reported_depth_threshold(RID p_body) const { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, 0); - return 0; -}; - -void PhysicsServer3DSW::body_set_omit_force_integration(RID p_body, bool p_omit) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - - body->set_omit_force_integration(p_omit); -}; - -bool PhysicsServer3DSW::body_is_omitting_force_integration(RID p_body) const { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, false); - return body->get_omit_force_integration(); -}; - -void PhysicsServer3DSW::body_set_max_contacts_reported(RID p_body, int p_contacts) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - body->set_max_contacts_reported(p_contacts); -} - -int PhysicsServer3DSW::body_get_max_contacts_reported(RID p_body) const { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, -1); - return body->get_max_contacts_reported(); -} - -void PhysicsServer3DSW::body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - body->set_state_sync_callback(p_instance, p_callback); -} - -void PhysicsServer3DSW::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - body->set_force_integration_callback(p_callable, p_udata); -} - -void PhysicsServer3DSW::body_set_ray_pickable(RID p_body, bool p_enable) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND(!body); - body->set_ray_pickable(p_enable); -} - -bool PhysicsServer3DSW::body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result) { - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, false); - ERR_FAIL_COND_V(!body->get_space(), false); - ERR_FAIL_COND_V(body->get_space()->is_locked(), false); - - _update_shapes(); - - return body->get_space()->test_body_motion(body, p_parameters, r_result); -} - -PhysicsDirectBodyState3D *PhysicsServer3DSW::body_get_direct_state(RID p_body) { - ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); - - Body3DSW *body = body_owner.get_or_null(p_body); - ERR_FAIL_NULL_V(body, nullptr); - - ERR_FAIL_NULL_V(body->get_space(), nullptr); - ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); - - return body->get_direct_state(); -} - -/* SOFT BODY */ - -RID PhysicsServer3DSW::soft_body_create() { - SoftBody3DSW *soft_body = memnew(SoftBody3DSW); - RID rid = soft_body_owner.make_rid(soft_body); - soft_body->set_self(rid); - return rid; -} - -void PhysicsServer3DSW::soft_body_update_rendering_server(RID p_body, RenderingServerHandler *p_rendering_server_handler) { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND(!soft_body); - - soft_body->update_rendering_server(p_rendering_server_handler); -} - -void PhysicsServer3DSW::soft_body_set_space(RID p_body, RID p_space) { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND(!soft_body); - - Space3DSW *space = nullptr; - if (p_space.is_valid()) { - space = space_owner.get_or_null(p_space); - ERR_FAIL_COND(!space); - } - - if (soft_body->get_space() == space) { - return; - } - - soft_body->set_space(space); -} - -RID PhysicsServer3DSW::soft_body_get_space(RID p_body) const { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!soft_body, RID()); - - Space3DSW *space = soft_body->get_space(); - if (!space) { - return RID(); - } - return space->get_self(); -} - -void PhysicsServer3DSW::soft_body_set_collision_layer(RID p_body, uint32_t p_layer) { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND(!soft_body); - - soft_body->set_collision_layer(p_layer); -} - -uint32_t PhysicsServer3DSW::soft_body_get_collision_layer(RID p_body) const { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!soft_body, 0); - - return soft_body->get_collision_layer(); -} - -void PhysicsServer3DSW::soft_body_set_collision_mask(RID p_body, uint32_t p_mask) { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND(!soft_body); - - soft_body->set_collision_mask(p_mask); -} - -uint32_t PhysicsServer3DSW::soft_body_get_collision_mask(RID p_body) const { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!soft_body, 0); - - return soft_body->get_collision_mask(); -} - -void PhysicsServer3DSW::soft_body_add_collision_exception(RID p_body, RID p_body_b) { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND(!soft_body); - - soft_body->add_exception(p_body_b); -} - -void PhysicsServer3DSW::soft_body_remove_collision_exception(RID p_body, RID p_body_b) { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND(!soft_body); - - soft_body->remove_exception(p_body_b); -} - -void PhysicsServer3DSW::soft_body_get_collision_exceptions(RID p_body, List *p_exceptions) { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND(!soft_body); - - for (int i = 0; i < soft_body->get_exceptions().size(); i++) { - p_exceptions->push_back(soft_body->get_exceptions()[i]); - } -} - -void PhysicsServer3DSW::soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND(!soft_body); - - soft_body->set_state(p_state, p_variant); -} - -Variant PhysicsServer3DSW::soft_body_get_state(RID p_body, BodyState p_state) const { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!soft_body, Variant()); - - return soft_body->get_state(p_state); -} - -void PhysicsServer3DSW::soft_body_set_transform(RID p_body, const Transform3D &p_transform) { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND(!soft_body); - - soft_body->set_state(BODY_STATE_TRANSFORM, p_transform); -} - -void PhysicsServer3DSW::soft_body_set_ray_pickable(RID p_body, bool p_enable) { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND(!soft_body); - - soft_body->set_ray_pickable(p_enable); -} - -void PhysicsServer3DSW::soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND(!soft_body); - - soft_body->set_iteration_count(p_simulation_precision); -} - -int PhysicsServer3DSW::soft_body_get_simulation_precision(RID p_body) const { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!soft_body, 0.f); - - return soft_body->get_iteration_count(); -} - -void PhysicsServer3DSW::soft_body_set_total_mass(RID p_body, real_t p_total_mass) { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND(!soft_body); - - soft_body->set_total_mass(p_total_mass); -} - -real_t PhysicsServer3DSW::soft_body_get_total_mass(RID p_body) const { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!soft_body, 0.f); - - return soft_body->get_total_mass(); -} - -void PhysicsServer3DSW::soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND(!soft_body); - - soft_body->set_linear_stiffness(p_stiffness); -} - -real_t PhysicsServer3DSW::soft_body_get_linear_stiffness(RID p_body) const { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!soft_body, 0.f); - - return soft_body->get_linear_stiffness(); -} - -void PhysicsServer3DSW::soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND(!soft_body); - - soft_body->set_pressure_coefficient(p_pressure_coefficient); -} - -real_t PhysicsServer3DSW::soft_body_get_pressure_coefficient(RID p_body) const { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!soft_body, 0.f); - - return soft_body->get_pressure_coefficient(); -} - -void PhysicsServer3DSW::soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND(!soft_body); - - soft_body->set_damping_coefficient(p_damping_coefficient); -} - -real_t PhysicsServer3DSW::soft_body_get_damping_coefficient(RID p_body) const { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!soft_body, 0.f); - - return soft_body->get_damping_coefficient(); -} - -void PhysicsServer3DSW::soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND(!soft_body); - - soft_body->set_drag_coefficient(p_drag_coefficient); -} - -real_t PhysicsServer3DSW::soft_body_get_drag_coefficient(RID p_body) const { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!soft_body, 0.f); - - return soft_body->get_drag_coefficient(); -} - -void PhysicsServer3DSW::soft_body_set_mesh(RID p_body, RID p_mesh) { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND(!soft_body); - - soft_body->set_mesh(p_mesh); -} - -AABB PhysicsServer3DSW::soft_body_get_bounds(RID p_body) const { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!soft_body, AABB()); - - return soft_body->get_bounds(); -} - -void PhysicsServer3DSW::soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND(!soft_body); - - soft_body->set_vertex_position(p_point_index, p_global_position); -} - -Vector3 PhysicsServer3DSW::soft_body_get_point_global_position(RID p_body, int p_point_index) const { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!soft_body, Vector3()); - - return soft_body->get_vertex_position(p_point_index); -} - -void PhysicsServer3DSW::soft_body_remove_all_pinned_points(RID p_body) { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND(!soft_body); - - soft_body->unpin_all_vertices(); -} - -void PhysicsServer3DSW::soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND(!soft_body); - - if (p_pin) { - soft_body->pin_vertex(p_point_index); - } else { - soft_body->unpin_vertex(p_point_index); - } -} - -bool PhysicsServer3DSW::soft_body_is_point_pinned(RID p_body, int p_point_index) const { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!soft_body, false); - - return soft_body->is_vertex_pinned(p_point_index); -} - -/* JOINT API */ - -RID PhysicsServer3DSW::joint_create() { - Joint3DSW *joint = memnew(Joint3DSW); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; -} - -void PhysicsServer3DSW::joint_clear(RID p_joint) { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - if (joint->get_type() != JOINT_TYPE_MAX) { - Joint3DSW *empty_joint = memnew(Joint3DSW); - empty_joint->copy_settings_from(joint); - - joint_owner.replace(p_joint, empty_joint); - memdelete(joint); - } -} - -void PhysicsServer3DSW::joint_make_pin(RID p_joint, RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) { - Body3DSW *body_A = body_owner.get_or_null(p_body_A); - ERR_FAIL_COND(!body_A); - - if (!p_body_B.is_valid()) { - ERR_FAIL_COND(!body_A->get_space()); - p_body_B = body_A->get_space()->get_static_global_body(); - } - - Body3DSW *body_B = body_owner.get_or_null(p_body_B); - ERR_FAIL_COND(!body_B); - - ERR_FAIL_COND(body_A == body_B); - - Joint3DSW *prev_joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(prev_joint == nullptr); - - Joint3DSW *joint = memnew(PinJoint3DSW(body_A, p_local_A, body_B, p_local_B)); - - joint->copy_settings_from(prev_joint); - joint_owner.replace(p_joint, joint); - memdelete(prev_joint); -} - -void PhysicsServer3DSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); - PinJoint3DSW *pin_joint = static_cast(joint); - pin_joint->set_param(p_param, p_value); -} - -real_t PhysicsServer3DSW::pin_joint_get_param(RID p_joint, PinJointParam p_param) const { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, 0); - PinJoint3DSW *pin_joint = static_cast(joint); - return pin_joint->get_param(p_param); -} - -void PhysicsServer3DSW::pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); - PinJoint3DSW *pin_joint = static_cast(joint); - pin_joint->set_pos_a(p_A); -} - -Vector3 PhysicsServer3DSW::pin_joint_get_local_a(RID p_joint) const { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!joint, Vector3()); - ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, Vector3()); - PinJoint3DSW *pin_joint = static_cast(joint); - return pin_joint->get_position_a(); -} - -void PhysicsServer3DSW::pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); - PinJoint3DSW *pin_joint = static_cast(joint); - pin_joint->set_pos_b(p_B); -} - -Vector3 PhysicsServer3DSW::pin_joint_get_local_b(RID p_joint) const { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!joint, Vector3()); - ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, Vector3()); - PinJoint3DSW *pin_joint = static_cast(joint); - return pin_joint->get_position_b(); -} - -void PhysicsServer3DSW::joint_make_hinge(RID p_joint, RID p_body_A, const Transform3D &p_frame_A, RID p_body_B, const Transform3D &p_frame_B) { - Body3DSW *body_A = body_owner.get_or_null(p_body_A); - ERR_FAIL_COND(!body_A); - - if (!p_body_B.is_valid()) { - ERR_FAIL_COND(!body_A->get_space()); - p_body_B = body_A->get_space()->get_static_global_body(); - } - - Body3DSW *body_B = body_owner.get_or_null(p_body_B); - ERR_FAIL_COND(!body_B); - - ERR_FAIL_COND(body_A == body_B); - - Joint3DSW *prev_joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(prev_joint == nullptr); - - Joint3DSW *joint = memnew(HingeJoint3DSW(body_A, body_B, p_frame_A, p_frame_B)); - - joint->copy_settings_from(prev_joint); - joint_owner.replace(p_joint, joint); - memdelete(prev_joint); -} - -void PhysicsServer3DSW::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) { - Body3DSW *body_A = body_owner.get_or_null(p_body_A); - ERR_FAIL_COND(!body_A); - - if (!p_body_B.is_valid()) { - ERR_FAIL_COND(!body_A->get_space()); - p_body_B = body_A->get_space()->get_static_global_body(); - } - - Body3DSW *body_B = body_owner.get_or_null(p_body_B); - ERR_FAIL_COND(!body_B); - - ERR_FAIL_COND(body_A == body_B); - - Joint3DSW *prev_joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(prev_joint == nullptr); - - Joint3DSW *joint = memnew(HingeJoint3DSW(body_A, body_B, p_pivot_A, p_pivot_B, p_axis_A, p_axis_B)); - - joint->copy_settings_from(prev_joint); - joint_owner.replace(p_joint, joint); - memdelete(prev_joint); -} - -void PhysicsServer3DSW::hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE); - HingeJoint3DSW *hinge_joint = static_cast(joint); - hinge_joint->set_param(p_param, p_value); -} - -real_t PhysicsServer3DSW::hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, 0); - HingeJoint3DSW *hinge_joint = static_cast(joint); - return hinge_joint->get_param(p_param); -} - -void PhysicsServer3DSW::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE); - HingeJoint3DSW *hinge_joint = static_cast(joint); - hinge_joint->set_flag(p_flag, p_value); -} - -bool PhysicsServer3DSW::hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!joint, false); - ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, false); - HingeJoint3DSW *hinge_joint = static_cast(joint); - return hinge_joint->get_flag(p_flag); -} - -void PhysicsServer3DSW::joint_set_solver_priority(RID p_joint, int p_priority) { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(!joint); - joint->set_priority(p_priority); -} - -int PhysicsServer3DSW::joint_get_solver_priority(RID p_joint) const { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!joint, 0); - return joint->get_priority(); -} - -void PhysicsServer3DSW::joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(!joint); - - joint->disable_collisions_between_bodies(p_disable); - - if (2 == joint->get_body_count()) { - Body3DSW *body_a = *joint->get_body_ptr(); - Body3DSW *body_b = *(joint->get_body_ptr() + 1); - - if (p_disable) { - body_add_collision_exception(body_a->get_self(), body_b->get_self()); - body_add_collision_exception(body_b->get_self(), body_a->get_self()); - } else { - body_remove_collision_exception(body_a->get_self(), body_b->get_self()); - body_remove_collision_exception(body_b->get_self(), body_a->get_self()); - } - } -} - -bool PhysicsServer3DSW::joint_is_disabled_collisions_between_bodies(RID p_joint) const { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!joint, true); - - return joint->is_disabled_collisions_between_bodies(); -} - -PhysicsServer3DSW::JointType PhysicsServer3DSW::joint_get_type(RID p_joint) const { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!joint, JOINT_TYPE_PIN); - return joint->get_type(); -} - -void PhysicsServer3DSW::joint_make_slider(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) { - Body3DSW *body_A = body_owner.get_or_null(p_body_A); - ERR_FAIL_COND(!body_A); - - if (!p_body_B.is_valid()) { - ERR_FAIL_COND(!body_A->get_space()); - p_body_B = body_A->get_space()->get_static_global_body(); - } - - Body3DSW *body_B = body_owner.get_or_null(p_body_B); - ERR_FAIL_COND(!body_B); - - ERR_FAIL_COND(body_A == body_B); - - Joint3DSW *prev_joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(prev_joint == nullptr); - - Joint3DSW *joint = memnew(SliderJoint3DSW(body_A, body_B, p_local_frame_A, p_local_frame_B)); - - joint->copy_settings_from(prev_joint); - joint_owner.replace(p_joint, joint); - memdelete(prev_joint); -} - -void PhysicsServer3DSW::slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_SLIDER); - SliderJoint3DSW *slider_joint = static_cast(joint); - slider_joint->set_param(p_param, p_value); -} - -real_t PhysicsServer3DSW::slider_joint_get_param(RID p_joint, SliderJointParam p_param) const { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0); - SliderJoint3DSW *slider_joint = static_cast(joint); - return slider_joint->get_param(p_param); -} - -void PhysicsServer3DSW::joint_make_cone_twist(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) { - Body3DSW *body_A = body_owner.get_or_null(p_body_A); - ERR_FAIL_COND(!body_A); - - if (!p_body_B.is_valid()) { - ERR_FAIL_COND(!body_A->get_space()); - p_body_B = body_A->get_space()->get_static_global_body(); - } - - Body3DSW *body_B = body_owner.get_or_null(p_body_B); - ERR_FAIL_COND(!body_B); - - ERR_FAIL_COND(body_A == body_B); - - Joint3DSW *prev_joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(prev_joint == nullptr); - - Joint3DSW *joint = memnew(ConeTwistJoint3DSW(body_A, body_B, p_local_frame_A, p_local_frame_B)); - - joint->copy_settings_from(prev_joint); - joint_owner.replace(p_joint, joint); - memdelete(prev_joint); -} - -void PhysicsServer3DSW::cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_CONE_TWIST); - ConeTwistJoint3DSW *cone_twist_joint = static_cast(joint); - cone_twist_joint->set_param(p_param, p_value); -} - -real_t PhysicsServer3DSW::cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0); - ConeTwistJoint3DSW *cone_twist_joint = static_cast(joint); - return cone_twist_joint->get_param(p_param); -} - -void PhysicsServer3DSW::joint_make_generic_6dof(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) { - Body3DSW *body_A = body_owner.get_or_null(p_body_A); - ERR_FAIL_COND(!body_A); - - if (!p_body_B.is_valid()) { - ERR_FAIL_COND(!body_A->get_space()); - p_body_B = body_A->get_space()->get_static_global_body(); - } - - Body3DSW *body_B = body_owner.get_or_null(p_body_B); - ERR_FAIL_COND(!body_B); - - ERR_FAIL_COND(body_A == body_B); - - Joint3DSW *prev_joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(prev_joint == nullptr); - - Joint3DSW *joint = memnew(Generic6DOFJoint3DSW(body_A, body_B, p_local_frame_A, p_local_frame_B, true)); - - joint->copy_settings_from(prev_joint); - joint_owner.replace(p_joint, joint); - memdelete(prev_joint); -} - -void PhysicsServer3DSW::generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param, real_t p_value) { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF); - Generic6DOFJoint3DSW *generic_6dof_joint = static_cast(joint); - generic_6dof_joint->set_param(p_axis, p_param, p_value); -} - -real_t PhysicsServer3DSW::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param) const { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, 0); - Generic6DOFJoint3DSW *generic_6dof_joint = static_cast(joint); - return generic_6dof_joint->get_param(p_axis, p_param); -} - -void PhysicsServer3DSW::generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag, bool p_enable) { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF); - Generic6DOFJoint3DSW *generic_6dof_joint = static_cast(joint); - generic_6dof_joint->set_flag(p_axis, p_flag, p_enable); -} - -bool PhysicsServer3DSW::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag) const { - Joint3DSW *joint = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!joint, false); - ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, false); - Generic6DOFJoint3DSW *generic_6dof_joint = static_cast(joint); - return generic_6dof_joint->get_flag(p_axis, p_flag); -} - -void PhysicsServer3DSW::free(RID p_rid) { - _update_shapes(); //just in case - - if (shape_owner.owns(p_rid)) { - Shape3DSW *shape = shape_owner.get_or_null(p_rid); - - while (shape->get_owners().size()) { - ShapeOwner3DSW *so = shape->get_owners().front()->key(); - so->remove_shape(shape); - } - - shape_owner.free(p_rid); - memdelete(shape); - } else if (body_owner.owns(p_rid)) { - Body3DSW *body = body_owner.get_or_null(p_rid); - - /* - if (body->get_state_query()) - _clear_query(body->get_state_query()); - - if (body->get_direct_state_query()) - _clear_query(body->get_direct_state_query()); - */ - - body->set_space(nullptr); - - while (body->get_shape_count()) { - body->remove_shape(0); - } - - body_owner.free(p_rid); - memdelete(body); - } else if (soft_body_owner.owns(p_rid)) { - SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_rid); - - soft_body->set_space(nullptr); - - soft_body_owner.free(p_rid); - memdelete(soft_body); - } else if (area_owner.owns(p_rid)) { - Area3DSW *area = area_owner.get_or_null(p_rid); - - /* - if (area->get_monitor_query()) - _clear_query(area->get_monitor_query()); - */ - - area->set_space(nullptr); - - while (area->get_shape_count()) { - area->remove_shape(0); - } - - area_owner.free(p_rid); - memdelete(area); - } else if (space_owner.owns(p_rid)) { - Space3DSW *space = space_owner.get_or_null(p_rid); - - while (space->get_objects().size()) { - CollisionObject3DSW *co = (CollisionObject3DSW *)space->get_objects().front()->get(); - co->set_space(nullptr); - } - - active_spaces.erase(space); - free(space->get_default_area()->get_self()); - free(space->get_static_global_body()); - - space_owner.free(p_rid); - memdelete(space); - } else if (joint_owner.owns(p_rid)) { - Joint3DSW *joint = joint_owner.get_or_null(p_rid); - - joint_owner.free(p_rid); - memdelete(joint); - - } else { - ERR_FAIL_MSG("Invalid ID."); - } -}; - -void PhysicsServer3DSW::set_active(bool p_active) { - active = p_active; -}; - -void PhysicsServer3DSW::set_collision_iterations(int p_iterations) { - iterations = p_iterations; -}; - -void PhysicsServer3DSW::init() { - iterations = 8; // 8? - stepper = memnew(Step3DSW); -}; - -void PhysicsServer3DSW::step(real_t p_step) { -#ifndef _3D_DISABLED - - if (!active) { - return; - } - - _update_shapes(); - - island_count = 0; - active_objects = 0; - collision_pairs = 0; - for (Set::Element *E = active_spaces.front(); E; E = E->next()) { - stepper->step((Space3DSW *)E->get(), p_step, iterations); - island_count += E->get()->get_island_count(); - active_objects += E->get()->get_active_objects(); - collision_pairs += E->get()->get_collision_pairs(); - } -#endif -} - -void PhysicsServer3DSW::sync() { - doing_sync = true; -}; - -void PhysicsServer3DSW::flush_queries() { -#ifndef _3D_DISABLED - - if (!active) { - return; - } - - flushing_queries = true; - - uint64_t time_beg = OS::get_singleton()->get_ticks_usec(); - - for (Set::Element *E = active_spaces.front(); E; E = E->next()) { - Space3DSW *space = (Space3DSW *)E->get(); - space->call_queries(); - } - - flushing_queries = false; - - if (EngineDebugger::is_profiling("servers")) { - uint64_t total_time[Space3DSW::ELAPSED_TIME_MAX]; - static const char *time_name[Space3DSW::ELAPSED_TIME_MAX] = { - "integrate_forces", - "generate_islands", - "setup_constraints", - "solve_constraints", - "integrate_velocities" - }; - - for (int i = 0; i < Space3DSW::ELAPSED_TIME_MAX; i++) { - total_time[i] = 0; - } - - for (Set::Element *E = active_spaces.front(); E; E = E->next()) { - for (int i = 0; i < Space3DSW::ELAPSED_TIME_MAX; i++) { - total_time[i] += E->get()->get_elapsed_time(Space3DSW::ElapsedTime(i)); - } - } - - Array values; - values.resize(Space3DSW::ELAPSED_TIME_MAX * 2); - for (int i = 0; i < Space3DSW::ELAPSED_TIME_MAX; i++) { - values[i * 2 + 0] = time_name[i]; - values[i * 2 + 1] = USEC_TO_SEC(total_time[i]); - } - values.push_back("flush_queries"); - values.push_back(USEC_TO_SEC(OS::get_singleton()->get_ticks_usec() - time_beg)); - - values.push_front("physics"); - EngineDebugger::profiler_add_frame_data("servers", values); - } -#endif -}; - -void PhysicsServer3DSW::end_sync() { - doing_sync = false; -}; - -void PhysicsServer3DSW::finish() { - memdelete(stepper); -}; - -int PhysicsServer3DSW::get_process_info(ProcessInfo p_info) { - switch (p_info) { - case INFO_ACTIVE_OBJECTS: { - return active_objects; - } break; - case INFO_COLLISION_PAIRS: { - return collision_pairs; - } break; - case INFO_ISLAND_COUNT: { - return island_count; - } break; - } - - return 0; -} - -void PhysicsServer3DSW::_update_shapes() { - while (pending_shape_update_list.first()) { - pending_shape_update_list.first()->self()->_shape_changed(); - pending_shape_update_list.remove(pending_shape_update_list.first()); - } -} - -void PhysicsServer3DSW::_shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { - CollCbkData *cbk = (CollCbkData *)p_userdata; - - if (cbk->max == 0) { - return; - } - - if (cbk->amount == cbk->max) { - //find least deep - real_t min_depth = 1e20; - int min_depth_idx = 0; - for (int i = 0; i < cbk->amount; i++) { - real_t d = cbk->ptr[i * 2 + 0].distance_squared_to(cbk->ptr[i * 2 + 1]); - if (d < min_depth) { - min_depth = d; - min_depth_idx = i; - } - } - - real_t d = p_point_A.distance_squared_to(p_point_B); - if (d < min_depth) { - return; - } - cbk->ptr[min_depth_idx * 2 + 0] = p_point_A; - cbk->ptr[min_depth_idx * 2 + 1] = p_point_B; - - } else { - cbk->ptr[cbk->amount * 2 + 0] = p_point_A; - cbk->ptr[cbk->amount * 2 + 1] = p_point_B; - cbk->amount++; - } -} - -PhysicsServer3DSW *PhysicsServer3DSW::singletonsw = nullptr; -PhysicsServer3DSW::PhysicsServer3DSW(bool p_using_threads) { - singletonsw = this; - BroadPhase3DSW::create_func = BroadPhase3DBVH::_create; - - using_threads = p_using_threads; -}; diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/physics_server_3d_sw.h deleted file mode 100644 index 54a787198d..0000000000 --- a/servers/physics_3d/physics_server_3d_sw.h +++ /dev/null @@ -1,379 +0,0 @@ -/*************************************************************************/ -/* physics_server_3d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 PHYSICS_SERVER_SW -#define PHYSICS_SERVER_SW - -#include "core/templates/rid_owner.h" -#include "joints_3d_sw.h" -#include "servers/physics_server_3d.h" -#include "shape_3d_sw.h" -#include "space_3d_sw.h" -#include "step_3d_sw.h" - -class PhysicsServer3DSW : public PhysicsServer3D { - GDCLASS(PhysicsServer3DSW, PhysicsServer3D); - - friend class PhysicsDirectSpaceState3DSW; - bool active = true; - int iterations = 0; - - int island_count = 0; - int active_objects = 0; - int collision_pairs = 0; - - bool using_threads = false; - bool doing_sync = false; - bool flushing_queries = false; - - Step3DSW *stepper = nullptr; - Set active_spaces; - - mutable RID_PtrOwner shape_owner; - mutable RID_PtrOwner space_owner; - mutable RID_PtrOwner area_owner; - mutable RID_PtrOwner body_owner; - mutable RID_PtrOwner soft_body_owner; - mutable RID_PtrOwner joint_owner; - - //void _clear_query(QuerySW *p_query); - friend class CollisionObject3DSW; - SelfList::List pending_shape_update_list; - void _update_shapes(); - - static PhysicsServer3DSW *singletonsw; - -public: - struct CollCbkData { - int max; - int amount; - Vector3 *ptr; - }; - - static void _shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); - - virtual RID world_boundary_shape_create() override; - virtual RID separation_ray_shape_create() override; - virtual RID sphere_shape_create() override; - virtual RID box_shape_create() override; - virtual RID capsule_shape_create() override; - virtual RID cylinder_shape_create() override; - virtual RID convex_polygon_shape_create() override; - virtual RID concave_polygon_shape_create() override; - virtual RID heightmap_shape_create() override; - virtual RID custom_shape_create() override; - - virtual void shape_set_data(RID p_shape, const Variant &p_data) override; - virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias) override; - - virtual ShapeType shape_get_type(RID p_shape) const override; - virtual Variant shape_get_data(RID p_shape) const override; - - virtual void shape_set_margin(RID p_shape, real_t p_margin) override; - virtual real_t shape_get_margin(RID p_shape) const override; - - virtual real_t shape_get_custom_solver_bias(RID p_shape) const override; - - /* SPACE API */ - - virtual RID space_create() override; - virtual void space_set_active(RID p_space, bool p_active) override; - virtual bool space_is_active(RID p_space) const override; - - virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) override; - virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const override; - - // this function only works on physics process, errors and returns null otherwise - virtual PhysicsDirectSpaceState3D *space_get_direct_state(RID p_space) override; - - virtual void space_set_debug_contacts(RID p_space, int p_max_contacts) override; - virtual Vector space_get_contacts(RID p_space) const override; - virtual int space_get_contact_count(RID p_space) const override; - - /* AREA API */ - - virtual RID area_create() override; - - virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) override; - virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const override; - - virtual void area_set_space(RID p_area, RID p_space) override; - virtual RID area_get_space(RID p_area) const override; - - virtual void area_add_shape(RID p_area, RID p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false) override; - virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape) override; - virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform3D &p_transform) override; - - virtual int area_get_shape_count(RID p_area) const override; - virtual RID area_get_shape(RID p_area, int p_shape_idx) const override; - virtual Transform3D area_get_shape_transform(RID p_area, int p_shape_idx) const override; - - virtual void area_remove_shape(RID p_area, int p_shape_idx) override; - virtual void area_clear_shapes(RID p_area) override; - - virtual void area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) override; - - virtual void area_attach_object_instance_id(RID p_area, ObjectID p_id) override; - virtual ObjectID area_get_object_instance_id(RID p_area) const override; - - virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) override; - virtual void area_set_transform(RID p_area, const Transform3D &p_transform) override; - - virtual Variant area_get_param(RID p_area, AreaParameter p_param) const override; - virtual Transform3D area_get_transform(RID p_area) const override; - - virtual void area_set_ray_pickable(RID p_area, bool p_enable) override; - - virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) override; - virtual void area_set_collision_layer(RID p_area, uint32_t p_layer) override; - - virtual void area_set_monitorable(RID p_area, bool p_monitorable) override; - - virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) override; - virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) override; - - /* BODY API */ - - // create a body of a given type - virtual RID body_create() override; - - virtual void body_set_space(RID p_body, RID p_space) override; - virtual RID body_get_space(RID p_body) const override; - - virtual void body_set_mode(RID p_body, BodyMode p_mode) override; - virtual BodyMode body_get_mode(RID p_body) const override; - - virtual void body_add_shape(RID p_body, RID p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false) override; - virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape) override; - virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform3D &p_transform) override; - - virtual int body_get_shape_count(RID p_body) const override; - virtual RID body_get_shape(RID p_body, int p_shape_idx) const override; - virtual Transform3D body_get_shape_transform(RID p_body, int p_shape_idx) const override; - - virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) override; - - virtual void body_remove_shape(RID p_body, int p_shape_idx) override; - virtual void body_clear_shapes(RID p_body) override; - - virtual void body_attach_object_instance_id(RID p_body, ObjectID p_id) override; - virtual ObjectID body_get_object_instance_id(RID p_body) const override; - - virtual void body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) override; - virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const override; - - virtual void body_set_collision_layer(RID p_body, uint32_t p_layer) override; - virtual uint32_t body_get_collision_layer(RID p_body) const override; - - virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) override; - virtual uint32_t body_get_collision_mask(RID p_body) const override; - - virtual void body_set_user_flags(RID p_body, uint32_t p_flags) override; - virtual uint32_t body_get_user_flags(RID p_body) const override; - - virtual void body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) override; - virtual Variant body_get_param(RID p_body, BodyParameter p_param) const override; - - virtual void body_reset_mass_properties(RID p_body) override; - - virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override; - virtual Variant body_get_state(RID p_body, BodyState p_state) const override; - - virtual void body_set_applied_force(RID p_body, const Vector3 &p_force) override; - virtual Vector3 body_get_applied_force(RID p_body) const override; - - virtual void body_set_applied_torque(RID p_body, const Vector3 &p_torque) override; - virtual Vector3 body_get_applied_torque(RID p_body) const override; - - virtual void body_add_central_force(RID p_body, const Vector3 &p_force) override; - virtual void body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position = Vector3()) override; - virtual void body_add_torque(RID p_body, const Vector3 &p_torque) override; - - virtual void body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) override; - virtual void body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) override; - virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) override; - virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) override; - - virtual void body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) override; - virtual bool body_is_axis_locked(RID p_body, BodyAxis p_axis) const override; - - virtual void body_add_collision_exception(RID p_body, RID p_body_b) override; - virtual void body_remove_collision_exception(RID p_body, RID p_body_b) override; - virtual void body_get_collision_exceptions(RID p_body, List *p_exceptions) override; - - virtual void body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) override; - virtual real_t body_get_contacts_reported_depth_threshold(RID p_body) const override; - - virtual void body_set_omit_force_integration(RID p_body, bool p_omit) override; - virtual bool body_is_omitting_force_integration(RID p_body) const override; - - virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) override; - virtual int body_get_max_contacts_reported(RID p_body) const override; - - virtual void body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) override; - virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override; - - virtual void body_set_ray_pickable(RID p_body, bool p_enable) override; - - virtual bool body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result = nullptr) override; - - // this function only works on physics process, errors and returns null otherwise - virtual PhysicsDirectBodyState3D *body_get_direct_state(RID p_body) override; - - /* SOFT BODY */ - - virtual RID soft_body_create() override; - - virtual void soft_body_update_rendering_server(RID p_body, RenderingServerHandler *p_rendering_server_handler) override; - - virtual void soft_body_set_space(RID p_body, RID p_space) override; - virtual RID soft_body_get_space(RID p_body) const override; - - virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer) override; - virtual uint32_t soft_body_get_collision_layer(RID p_body) const override; - - virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask) override; - virtual uint32_t soft_body_get_collision_mask(RID p_body) const override; - - virtual void soft_body_add_collision_exception(RID p_body, RID p_body_b) override; - virtual void soft_body_remove_collision_exception(RID p_body, RID p_body_b) override; - virtual void soft_body_get_collision_exceptions(RID p_body, List *p_exceptions) override; - - virtual void soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override; - virtual Variant soft_body_get_state(RID p_body, BodyState p_state) const override; - - virtual void soft_body_set_transform(RID p_body, const Transform3D &p_transform) override; - - virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) override; - - virtual void soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) override; - virtual int soft_body_get_simulation_precision(RID p_body) const override; - - virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass) override; - virtual real_t soft_body_get_total_mass(RID p_body) const override; - - virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) override; - virtual real_t soft_body_get_linear_stiffness(RID p_body) const override; - - virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) override; - virtual real_t soft_body_get_pressure_coefficient(RID p_body) const override; - - virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) override; - virtual real_t soft_body_get_damping_coefficient(RID p_body) const override; - - virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) override; - virtual real_t soft_body_get_drag_coefficient(RID p_body) const override; - - virtual void soft_body_set_mesh(RID p_body, RID p_mesh) override; - - virtual AABB soft_body_get_bounds(RID p_body) const override; - - virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) override; - virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) const override; - - virtual void soft_body_remove_all_pinned_points(RID p_body) override; - virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) override; - virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) const override; - - /* JOINT API */ - - virtual RID joint_create() override; - - virtual void joint_clear(RID p_joint) override; //resets type - - virtual void joint_make_pin(RID p_joint, RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) override; - - virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) override; - virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const override; - - virtual void pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) override; - virtual Vector3 pin_joint_get_local_a(RID p_joint) const override; - - virtual void pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) override; - virtual Vector3 pin_joint_get_local_b(RID p_joint) const override; - - virtual void joint_make_hinge(RID p_joint, RID p_body_A, const Transform3D &p_frame_A, RID p_body_B, const Transform3D &p_frame_B) override; - virtual void joint_make_hinge_simple(RID p_joint, RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) override; - - virtual void hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) override; - virtual real_t hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const override; - - virtual void hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) override; - virtual bool hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const override; - - virtual void joint_make_slider(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) override; //reference frame is A - - virtual void slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) override; - virtual real_t slider_joint_get_param(RID p_joint, SliderJointParam p_param) const override; - - virtual void joint_make_cone_twist(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) override; //reference frame is A - - virtual void cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) override; - virtual real_t cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const override; - - virtual void joint_make_generic_6dof(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) override; //reference frame is A - - virtual void generic_6dof_joint_set_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param, real_t p_value) override; - virtual real_t generic_6dof_joint_get_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param) const override; - - virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag, bool p_enable) override; - virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag) const override; - - virtual JointType joint_get_type(RID p_joint) const override; - - virtual void joint_set_solver_priority(RID p_joint, int p_priority) override; - virtual int joint_get_solver_priority(RID p_joint) const override; - - virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) override; - virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const override; - - /* MISC */ - - virtual void free(RID p_rid) override; - - virtual void set_active(bool p_active) override; - virtual void init() override; - virtual void step(real_t p_step) override; - virtual void sync() override; - virtual void flush_queries() override; - virtual void end_sync() override; - virtual void finish() override; - - virtual void set_collision_iterations(int p_iterations) override; - - virtual bool is_flushing_queries() const override { return flushing_queries; } - - int get_process_info(ProcessInfo p_info) override; - - PhysicsServer3DSW(bool p_using_threads = false); - ~PhysicsServer3DSW() {} -}; - -#endif diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.cpp b/servers/physics_3d/physics_server_3d_wrap_mt.cpp deleted file mode 100644 index c424100bba..0000000000 --- a/servers/physics_3d/physics_server_3d_wrap_mt.cpp +++ /dev/null @@ -1,137 +0,0 @@ -/*************************************************************************/ -/* physics_server_3d_wrap_mt.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "physics_server_3d_wrap_mt.h" - -#include "core/os/os.h" - -void PhysicsServer3DWrapMT::thread_exit() { - exit = true; -} - -void PhysicsServer3DWrapMT::thread_step(real_t p_delta) { - physics_3d_server->step(p_delta); - step_sem.post(); -} - -void PhysicsServer3DWrapMT::_thread_callback(void *_instance) { - PhysicsServer3DWrapMT *vsmt = reinterpret_cast(_instance); - - vsmt->thread_loop(); -} - -void PhysicsServer3DWrapMT::thread_loop() { - server_thread = Thread::get_caller_id(); - - physics_3d_server->init(); - - exit = false; - step_thread_up = true; - while (!exit) { - // flush commands one by one, until exit is requested - command_queue.wait_and_flush(); - } - - command_queue.flush_all(); // flush all - - physics_3d_server->finish(); -} - -/* EVENT QUEUING */ - -void PhysicsServer3DWrapMT::step(real_t p_step) { - if (create_thread) { - command_queue.push(this, &PhysicsServer3DWrapMT::thread_step, p_step); - } else { - command_queue.flush_all(); //flush all pending from other threads - physics_3d_server->step(p_step); - } -} - -void PhysicsServer3DWrapMT::sync() { - if (create_thread) { - if (first_frame) { - first_frame = false; - } else { - step_sem.wait(); //must not wait if a step was not issued - } - } - physics_3d_server->sync(); -} - -void PhysicsServer3DWrapMT::flush_queries() { - physics_3d_server->flush_queries(); -} - -void PhysicsServer3DWrapMT::end_sync() { - physics_3d_server->end_sync(); -} - -void PhysicsServer3DWrapMT::init() { - if (create_thread) { - //OS::get_singleton()->release_rendering_thread(); - thread.start(_thread_callback, this); - while (!step_thread_up) { - OS::get_singleton()->delay_usec(1000); - } - } else { - physics_3d_server->init(); - } -} - -void PhysicsServer3DWrapMT::finish() { - if (thread.is_started()) { - command_queue.push(this, &PhysicsServer3DWrapMT::thread_exit); - thread.wait_to_finish(); - } else { - physics_3d_server->finish(); - } -} - -PhysicsServer3DWrapMT::PhysicsServer3DWrapMT(PhysicsServer3D *p_contained, bool p_create_thread) : - command_queue(p_create_thread) { - physics_3d_server = p_contained; - create_thread = p_create_thread; - - pool_max_size = GLOBAL_GET("memory/limits/multithreaded_server/rid_pool_prealloc"); - - if (!p_create_thread) { - server_thread = Thread::get_caller_id(); - } else { - server_thread = 0; - } - - main_thread = Thread::get_caller_id(); -} - -PhysicsServer3DWrapMT::~PhysicsServer3DWrapMT() { - memdelete(physics_3d_server); - //finish(); -} diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.h b/servers/physics_3d/physics_server_3d_wrap_mt.h deleted file mode 100644 index 17d02addda..0000000000 --- a/servers/physics_3d/physics_server_3d_wrap_mt.h +++ /dev/null @@ -1,409 +0,0 @@ -/*************************************************************************/ -/* physics_server_3d_wrap_mt.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 PHYSICS3DSERVERWRAPMT_H -#define PHYSICS3DSERVERWRAPMT_H - -#include "core/config/project_settings.h" -#include "core/os/thread.h" -#include "core/templates/command_queue_mt.h" -#include "servers/physics_server_3d.h" - -#ifdef DEBUG_SYNC -#define SYNC_DEBUG print_line("sync on: " + String(__FUNCTION__)); -#else -#define SYNC_DEBUG -#endif - -class PhysicsServer3DWrapMT : public PhysicsServer3D { - mutable PhysicsServer3D *physics_3d_server; - - mutable CommandQueueMT command_queue; - - static void _thread_callback(void *_instance); - void thread_loop(); - - Thread::ID server_thread; - Thread::ID main_thread; - volatile bool exit = false; - Thread thread; - volatile bool step_thread_up = false; - bool create_thread = false; - - Semaphore step_sem; - int step_pending = 0; - void thread_step(real_t p_delta); - void thread_flush(); - - void thread_exit(); - - bool first_frame = true; - - Mutex alloc_mutex; - int pool_max_size = 0; - -public: -#define ServerName PhysicsServer3D -#define ServerNameWrapMT PhysicsServer3DWrapMT -#define server_name physics_3d_server -#define WRITE_ACTION - -#include "servers/server_wrap_mt_common.h" - - //FUNC1RID(shape,ShapeType); todo fix - FUNCRID(world_boundary_shape) - FUNCRID(separation_ray_shape) - FUNCRID(sphere_shape) - FUNCRID(box_shape) - FUNCRID(capsule_shape) - FUNCRID(cylinder_shape) - FUNCRID(convex_polygon_shape) - FUNCRID(concave_polygon_shape) - FUNCRID(heightmap_shape) - FUNCRID(custom_shape) - - FUNC2(shape_set_data, RID, const Variant &); - FUNC2(shape_set_custom_solver_bias, RID, real_t); - - FUNC2(shape_set_margin, RID, real_t) - FUNC1RC(real_t, shape_get_margin, RID) - - FUNC1RC(ShapeType, shape_get_type, RID); - FUNC1RC(Variant, shape_get_data, RID); - FUNC1RC(real_t, shape_get_custom_solver_bias, RID); -#if 0 - //these work well, but should be used from the main thread only - bool shape_collide(RID p_shape_A, const Transform &p_xform_A, const Vector3 &p_motion_A, RID p_shape_B, const Transform &p_xform_B, const Vector3 &p_motion_B, Vector3 *r_results, int p_result_max, int &r_result_count) { - ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); - return physics_3d_server->shape_collide(p_shape_A, p_xform_A, p_motion_A, p_shape_B, p_xform_B, p_motion_B, r_results, p_result_max, r_result_count); - } -#endif - /* SPACE API */ - - FUNCRID(space); - FUNC2(space_set_active, RID, bool); - FUNC1RC(bool, space_is_active, RID); - - FUNC3(space_set_param, RID, SpaceParameter, real_t); - FUNC2RC(real_t, space_get_param, RID, SpaceParameter); - - // this function only works on physics process, errors and returns null otherwise - PhysicsDirectSpaceState3D *space_get_direct_state(RID p_space) override { - ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr); - return physics_3d_server->space_get_direct_state(p_space); - } - - FUNC2(space_set_debug_contacts, RID, int); - virtual Vector space_get_contacts(RID p_space) const override { - ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), Vector()); - return physics_3d_server->space_get_contacts(p_space); - } - - virtual int space_get_contact_count(RID p_space) const override { - ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), 0); - return physics_3d_server->space_get_contact_count(p_space); - } - - /* AREA API */ - - //FUNC0RID(area); - FUNCRID(area); - - FUNC2(area_set_space, RID, RID); - FUNC1RC(RID, area_get_space, RID); - - FUNC2(area_set_space_override_mode, RID, AreaSpaceOverrideMode); - FUNC1RC(AreaSpaceOverrideMode, area_get_space_override_mode, RID); - - FUNC4(area_add_shape, RID, RID, const Transform3D &, bool); - FUNC3(area_set_shape, RID, int, RID); - FUNC3(area_set_shape_transform, RID, int, const Transform3D &); - FUNC3(area_set_shape_disabled, RID, int, bool); - - FUNC1RC(int, area_get_shape_count, RID); - FUNC2RC(RID, area_get_shape, RID, int); - FUNC2RC(Transform3D, area_get_shape_transform, RID, int); - FUNC2(area_remove_shape, RID, int); - FUNC1(area_clear_shapes, RID); - - FUNC2(area_attach_object_instance_id, RID, ObjectID); - FUNC1RC(ObjectID, area_get_object_instance_id, RID); - - FUNC3(area_set_param, RID, AreaParameter, const Variant &); - FUNC2(area_set_transform, RID, const Transform3D &); - - FUNC2RC(Variant, area_get_param, RID, AreaParameter); - FUNC1RC(Transform3D, area_get_transform, RID); - - FUNC2(area_set_collision_mask, RID, uint32_t); - FUNC2(area_set_collision_layer, RID, uint32_t); - - FUNC2(area_set_monitorable, RID, bool); - FUNC2(area_set_ray_pickable, RID, bool); - - FUNC3(area_set_monitor_callback, RID, Object *, const StringName &); - FUNC3(area_set_area_monitor_callback, RID, Object *, const StringName &); - - /* BODY API */ - - //FUNC2RID(body,BodyMode,bool); - FUNCRID(body) - - FUNC2(body_set_space, RID, RID); - FUNC1RC(RID, body_get_space, RID); - - FUNC2(body_set_mode, RID, BodyMode); - FUNC1RC(BodyMode, body_get_mode, RID); - - FUNC4(body_add_shape, RID, RID, const Transform3D &, bool); - FUNC3(body_set_shape, RID, int, RID); - FUNC3(body_set_shape_transform, RID, int, const Transform3D &); - - FUNC1RC(int, body_get_shape_count, RID); - FUNC2RC(Transform3D, body_get_shape_transform, RID, int); - FUNC2RC(RID, body_get_shape, RID, int); - - FUNC3(body_set_shape_disabled, RID, int, bool); - - FUNC2(body_remove_shape, RID, int); - FUNC1(body_clear_shapes, RID); - - FUNC2(body_attach_object_instance_id, RID, ObjectID); - FUNC1RC(ObjectID, body_get_object_instance_id, RID); - - FUNC2(body_set_enable_continuous_collision_detection, RID, bool); - FUNC1RC(bool, body_is_continuous_collision_detection_enabled, RID); - - FUNC2(body_set_collision_layer, RID, uint32_t); - FUNC1RC(uint32_t, body_get_collision_layer, RID); - - FUNC2(body_set_collision_mask, RID, uint32_t); - FUNC1RC(uint32_t, body_get_collision_mask, RID); - - FUNC2(body_set_user_flags, RID, uint32_t); - FUNC1RC(uint32_t, body_get_user_flags, RID); - - FUNC3(body_set_param, RID, BodyParameter, const Variant &); - FUNC2RC(Variant, body_get_param, RID, BodyParameter); - - FUNC1(body_reset_mass_properties, RID); - - FUNC3(body_set_state, RID, BodyState, const Variant &); - FUNC2RC(Variant, body_get_state, RID, BodyState); - - FUNC2(body_set_applied_force, RID, const Vector3 &); - FUNC1RC(Vector3, body_get_applied_force, RID); - - FUNC2(body_set_applied_torque, RID, const Vector3 &); - FUNC1RC(Vector3, body_get_applied_torque, RID); - - FUNC2(body_add_central_force, RID, const Vector3 &); - FUNC3(body_add_force, RID, const Vector3 &, const Vector3 &); - FUNC2(body_add_torque, RID, const Vector3 &); - FUNC2(body_apply_torque_impulse, RID, const Vector3 &); - FUNC2(body_apply_central_impulse, RID, const Vector3 &); - FUNC3(body_apply_impulse, RID, const Vector3 &, const Vector3 &); - FUNC2(body_set_axis_velocity, RID, const Vector3 &); - - FUNC3(body_set_axis_lock, RID, BodyAxis, bool); - FUNC2RC(bool, body_is_axis_locked, RID, BodyAxis); - - FUNC2(body_add_collision_exception, RID, RID); - FUNC2(body_remove_collision_exception, RID, RID); - FUNC2S(body_get_collision_exceptions, RID, List *); - - FUNC2(body_set_max_contacts_reported, RID, int); - FUNC1RC(int, body_get_max_contacts_reported, RID); - - FUNC2(body_set_contacts_reported_depth_threshold, RID, real_t); - FUNC1RC(real_t, body_get_contacts_reported_depth_threshold, RID); - - FUNC2(body_set_omit_force_integration, RID, bool); - FUNC1RC(bool, body_is_omitting_force_integration, RID); - - FUNC3(body_set_state_sync_callback, RID, void *, BodyStateCallback); - FUNC3(body_set_force_integration_callback, RID, const Callable &, const Variant &); - - FUNC2(body_set_ray_pickable, RID, bool); - - bool body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result = nullptr) override { - ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); - return physics_3d_server->body_test_motion(p_body, p_parameters, r_result); - } - - // this function only works on physics process, errors and returns null otherwise - PhysicsDirectBodyState3D *body_get_direct_state(RID p_body) override { - ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr); - return physics_3d_server->body_get_direct_state(p_body); - } - - /* SOFT BODY API */ - - FUNCRID(soft_body) - - FUNC2(soft_body_update_rendering_server, RID, class RenderingServerHandler *) - - FUNC2(soft_body_set_space, RID, RID) - FUNC1RC(RID, soft_body_get_space, RID) - - FUNC2(soft_body_set_ray_pickable, RID, bool); - - FUNC2(soft_body_set_collision_layer, RID, uint32_t) - FUNC1RC(uint32_t, soft_body_get_collision_layer, RID) - - FUNC2(soft_body_set_collision_mask, RID, uint32_t) - FUNC1RC(uint32_t, soft_body_get_collision_mask, RID) - - FUNC2(soft_body_add_collision_exception, RID, RID) - FUNC2(soft_body_remove_collision_exception, RID, RID) - FUNC2S(soft_body_get_collision_exceptions, RID, List *) - - FUNC3(soft_body_set_state, RID, BodyState, const Variant &); - FUNC2RC(Variant, soft_body_get_state, RID, BodyState); - - FUNC2(soft_body_set_transform, RID, const Transform3D &); - - FUNC2(soft_body_set_simulation_precision, RID, int); - FUNC1RC(int, soft_body_get_simulation_precision, RID); - - FUNC2(soft_body_set_total_mass, RID, real_t); - FUNC1RC(real_t, soft_body_get_total_mass, RID); - - FUNC2(soft_body_set_linear_stiffness, RID, real_t); - FUNC1RC(real_t, soft_body_get_linear_stiffness, RID); - - FUNC2(soft_body_set_pressure_coefficient, RID, real_t); - FUNC1RC(real_t, soft_body_get_pressure_coefficient, RID); - - FUNC2(soft_body_set_damping_coefficient, RID, real_t); - FUNC1RC(real_t, soft_body_get_damping_coefficient, RID); - - FUNC2(soft_body_set_drag_coefficient, RID, real_t); - FUNC1RC(real_t, soft_body_get_drag_coefficient, RID); - - FUNC2(soft_body_set_mesh, RID, RID); - - FUNC1RC(AABB, soft_body_get_bounds, RID); - - FUNC3(soft_body_move_point, RID, int, const Vector3 &); - FUNC2RC(Vector3, soft_body_get_point_global_position, RID, int); - - FUNC1(soft_body_remove_all_pinned_points, RID); - FUNC3(soft_body_pin_point, RID, int, bool); - FUNC2RC(bool, soft_body_is_point_pinned, RID, int); - - /* JOINT API */ - - FUNCRID(joint) - - FUNC1(joint_clear, RID) - - FUNC5(joint_make_pin, RID, RID, const Vector3 &, RID, const Vector3 &) - - FUNC3(pin_joint_set_param, RID, PinJointParam, real_t) - FUNC2RC(real_t, pin_joint_get_param, RID, PinJointParam) - - FUNC2(pin_joint_set_local_a, RID, const Vector3 &) - FUNC1RC(Vector3, pin_joint_get_local_a, RID) - - FUNC2(pin_joint_set_local_b, RID, const Vector3 &) - FUNC1RC(Vector3, pin_joint_get_local_b, RID) - - FUNC5(joint_make_hinge, RID, RID, const Transform3D &, RID, const Transform3D &) - FUNC7(joint_make_hinge_simple, RID, RID, const Vector3 &, const Vector3 &, RID, const Vector3 &, const Vector3 &) - - FUNC3(hinge_joint_set_param, RID, HingeJointParam, real_t) - FUNC2RC(real_t, hinge_joint_get_param, RID, HingeJointParam) - - FUNC3(hinge_joint_set_flag, RID, HingeJointFlag, bool) - FUNC2RC(bool, hinge_joint_get_flag, RID, HingeJointFlag) - - FUNC5(joint_make_slider, RID, RID, const Transform3D &, RID, const Transform3D &) - - FUNC3(slider_joint_set_param, RID, SliderJointParam, real_t) - FUNC2RC(real_t, slider_joint_get_param, RID, SliderJointParam) - - FUNC5(joint_make_cone_twist, RID, RID, const Transform3D &, RID, const Transform3D &) - - FUNC3(cone_twist_joint_set_param, RID, ConeTwistJointParam, real_t) - FUNC2RC(real_t, cone_twist_joint_get_param, RID, ConeTwistJointParam) - - FUNC5(joint_make_generic_6dof, RID, RID, const Transform3D &, RID, const Transform3D &) - - FUNC4(generic_6dof_joint_set_param, RID, Vector3::Axis, G6DOFJointAxisParam, real_t) - FUNC3RC(real_t, generic_6dof_joint_get_param, RID, Vector3::Axis, G6DOFJointAxisParam) - - FUNC4(generic_6dof_joint_set_flag, RID, Vector3::Axis, G6DOFJointAxisFlag, bool) - FUNC3RC(bool, generic_6dof_joint_get_flag, RID, Vector3::Axis, G6DOFJointAxisFlag) - - FUNC1RC(JointType, joint_get_type, RID); - - FUNC2(joint_set_solver_priority, RID, int); - FUNC1RC(int, joint_get_solver_priority, RID); - - FUNC2(joint_disable_collisions_between_bodies, RID, const bool); - FUNC1RC(bool, joint_is_disabled_collisions_between_bodies, RID); - - /* MISC */ - - FUNC1(free, RID); - FUNC1(set_active, bool); - FUNC1(set_collision_iterations, int); - - virtual void init() override; - virtual void step(real_t p_step) override; - virtual void sync() override; - virtual void end_sync() override; - virtual void flush_queries() override; - virtual void finish() override; - - virtual bool is_flushing_queries() const override { - return physics_3d_server->is_flushing_queries(); - } - - int get_process_info(ProcessInfo p_info) override { - return physics_3d_server->get_process_info(p_info); - } - - PhysicsServer3DWrapMT(PhysicsServer3D *p_contained, bool p_create_thread); - ~PhysicsServer3DWrapMT(); - -#undef ServerNameWrapMT -#undef ServerName -#undef server_name -#undef WRITE_ACTION -}; - -#ifdef DEBUG_SYNC -#undef DEBUG_SYNC -#endif -#undef SYNC_DEBUG - -#endif // PHYSICS3DSERVERWRAPMT_H diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp deleted file mode 100644 index 789fc6ab33..0000000000 --- a/servers/physics_3d/shape_3d_sw.cpp +++ /dev/null @@ -1,2202 +0,0 @@ -/*************************************************************************/ -/* shape_3d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "shape_3d_sw.h" - -#include "core/io/image.h" -#include "core/math/convex_hull.h" -#include "core/math/geometry_3d.h" -#include "core/templates/sort_array.h" - -// HeightMapShape3DSW is based on Bullet btHeightfieldTerrainShape. - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.0002 -#define _FACE_IS_VALID_SUPPORT_THRESHOLD 0.9998 - -#define _CYLINDER_EDGE_IS_VALID_SUPPORT_THRESHOLD 0.002 -#define _CYLINDER_FACE_IS_VALID_SUPPORT_THRESHOLD 0.999 - -void Shape3DSW::configure(const AABB &p_aabb) { - aabb = p_aabb; - configured = true; - for (const KeyValue &E : owners) { - ShapeOwner3DSW *co = (ShapeOwner3DSW *)E.key; - co->_shape_changed(); - } -} - -Vector3 Shape3DSW::get_support(const Vector3 &p_normal) const { - Vector3 res; - int amnt; - FeatureType type; - get_supports(p_normal, 1, &res, amnt, type); - return res; -} - -void Shape3DSW::add_owner(ShapeOwner3DSW *p_owner) { - Map::Element *E = owners.find(p_owner); - if (E) { - E->get()++; - } else { - owners[p_owner] = 1; - } -} - -void Shape3DSW::remove_owner(ShapeOwner3DSW *p_owner) { - Map::Element *E = owners.find(p_owner); - ERR_FAIL_COND(!E); - E->get()--; - if (E->get() == 0) { - owners.erase(E); - } -} - -bool Shape3DSW::is_owner(ShapeOwner3DSW *p_owner) const { - return owners.has(p_owner); -} - -const Map &Shape3DSW::get_owners() const { - return owners; -} - -Shape3DSW::~Shape3DSW() { - ERR_FAIL_COND(owners.size()); -} - -Plane WorldBoundaryShape3DSW::get_plane() const { - return plane; -} - -void WorldBoundaryShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { - // gibberish, a plane is infinity - r_min = -1e7; - r_max = 1e7; -} - -Vector3 WorldBoundaryShape3DSW::get_support(const Vector3 &p_normal) const { - return p_normal * 1e15; -} - -bool WorldBoundaryShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - bool inters = plane.intersects_segment(p_begin, p_end, &r_result); - if (inters) { - r_normal = plane.normal; - } - return inters; -} - -bool WorldBoundaryShape3DSW::intersect_point(const Vector3 &p_point) const { - return plane.distance_to(p_point) < 0; -} - -Vector3 WorldBoundaryShape3DSW::get_closest_point_to(const Vector3 &p_point) const { - if (plane.is_point_over(p_point)) { - return plane.project(p_point); - } else { - return p_point; - } -} - -Vector3 WorldBoundaryShape3DSW::get_moment_of_inertia(real_t p_mass) const { - return Vector3(); // not applicable. -} - -void WorldBoundaryShape3DSW::_setup(const Plane &p_plane) { - plane = p_plane; - configure(AABB(Vector3(-1e4, -1e4, -1e4), Vector3(1e4 * 2, 1e4 * 2, 1e4 * 2))); -} - -void WorldBoundaryShape3DSW::set_data(const Variant &p_data) { - _setup(p_data); -} - -Variant WorldBoundaryShape3DSW::get_data() const { - return plane; -} - -WorldBoundaryShape3DSW::WorldBoundaryShape3DSW() { -} - -// - -real_t SeparationRayShape3DSW::get_length() const { - return length; -} - -bool SeparationRayShape3DSW::get_slide_on_slope() const { - return slide_on_slope; -} - -void SeparationRayShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { - // don't think this will be even used - r_min = 0; - r_max = 1; -} - -Vector3 SeparationRayShape3DSW::get_support(const Vector3 &p_normal) const { - if (p_normal.z > 0) { - return Vector3(0, 0, length); - } else { - return Vector3(0, 0, 0); - } -} - -void SeparationRayShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { - if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { - r_amount = 2; - r_type = FEATURE_EDGE; - r_supports[0] = Vector3(0, 0, 0); - r_supports[1] = Vector3(0, 0, length); - } else if (p_normal.z > 0) { - r_amount = 1; - r_type = FEATURE_POINT; - *r_supports = Vector3(0, 0, length); - } else { - r_amount = 1; - r_type = FEATURE_POINT; - *r_supports = Vector3(0, 0, 0); - } -} - -bool SeparationRayShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - return false; //simply not possible -} - -bool SeparationRayShape3DSW::intersect_point(const Vector3 &p_point) const { - return false; //simply not possible -} - -Vector3 SeparationRayShape3DSW::get_closest_point_to(const Vector3 &p_point) const { - Vector3 s[2] = { - Vector3(0, 0, 0), - Vector3(0, 0, length) - }; - - return Geometry3D::get_closest_point_to_segment(p_point, s); -} - -Vector3 SeparationRayShape3DSW::get_moment_of_inertia(real_t p_mass) const { - return Vector3(); -} - -void SeparationRayShape3DSW::_setup(real_t p_length, bool p_slide_on_slope) { - length = p_length; - slide_on_slope = p_slide_on_slope; - configure(AABB(Vector3(0, 0, 0), Vector3(0.1, 0.1, length))); -} - -void SeparationRayShape3DSW::set_data(const Variant &p_data) { - Dictionary d = p_data; - _setup(d["length"], d["slide_on_slope"]); -} - -Variant SeparationRayShape3DSW::get_data() const { - Dictionary d; - d["length"] = length; - d["slide_on_slope"] = slide_on_slope; - return d; -} - -SeparationRayShape3DSW::SeparationRayShape3DSW() {} - -/********** SPHERE *************/ - -real_t SphereShape3DSW::get_radius() const { - return radius; -} - -void SphereShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { - real_t d = p_normal.dot(p_transform.origin); - - // figure out scale at point - Vector3 local_normal = p_transform.basis.xform_inv(p_normal); - real_t scale = local_normal.length(); - - r_min = d - (radius)*scale; - r_max = d + (radius)*scale; -} - -Vector3 SphereShape3DSW::get_support(const Vector3 &p_normal) const { - return p_normal * radius; -} - -void SphereShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { - *r_supports = p_normal * radius; - r_amount = 1; - r_type = FEATURE_POINT; -} - -bool SphereShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - return Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal); -} - -bool SphereShape3DSW::intersect_point(const Vector3 &p_point) const { - return p_point.length() < radius; -} - -Vector3 SphereShape3DSW::get_closest_point_to(const Vector3 &p_point) const { - Vector3 p = p_point; - real_t l = p.length(); - if (l < radius) { - return p_point; - } - return (p / l) * radius; -} - -Vector3 SphereShape3DSW::get_moment_of_inertia(real_t p_mass) const { - real_t s = 0.4 * p_mass * radius * radius; - return Vector3(s, s, s); -} - -void SphereShape3DSW::_setup(real_t p_radius) { - radius = p_radius; - configure(AABB(Vector3(-radius, -radius, -radius), Vector3(radius * 2.0, radius * 2.0, radius * 2.0))); -} - -void SphereShape3DSW::set_data(const Variant &p_data) { - _setup(p_data); -} - -Variant SphereShape3DSW::get_data() const { - return radius; -} - -SphereShape3DSW::SphereShape3DSW() {} - -/********** BOX *************/ - -void BoxShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { - // no matter the angle, the box is mirrored anyway - Vector3 local_normal = p_transform.basis.xform_inv(p_normal); - - real_t length = local_normal.abs().dot(half_extents); - real_t distance = p_normal.dot(p_transform.origin); - - r_min = distance - length; - r_max = distance + length; -} - -Vector3 BoxShape3DSW::get_support(const Vector3 &p_normal) const { - Vector3 point( - (p_normal.x < 0) ? -half_extents.x : half_extents.x, - (p_normal.y < 0) ? -half_extents.y : half_extents.y, - (p_normal.z < 0) ? -half_extents.z : half_extents.z); - - return point; -} - -void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { - static const int next[3] = { 1, 2, 0 }; - static const int next2[3] = { 2, 0, 1 }; - - for (int i = 0; i < 3; i++) { - Vector3 axis; - axis[i] = 1.0; - real_t dot = p_normal.dot(axis); - if (Math::abs(dot) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { - //Vector3 axis_b; - - bool neg = dot < 0; - r_amount = 4; - r_type = FEATURE_FACE; - - Vector3 point; - point[i] = half_extents[i]; - - int i_n = next[i]; - int i_n2 = next2[i]; - - static const real_t sign[4][2] = { - { -1.0, 1.0 }, - { 1.0, 1.0 }, - { 1.0, -1.0 }, - { -1.0, -1.0 }, - }; - - for (int j = 0; j < 4; j++) { - point[i_n] = sign[j][0] * half_extents[i_n]; - point[i_n2] = sign[j][1] * half_extents[i_n2]; - r_supports[j] = neg ? -point : point; - } - - if (neg) { - SWAP(r_supports[1], r_supports[2]); - SWAP(r_supports[0], r_supports[3]); - } - - return; - } - - r_amount = 0; - } - - for (int i = 0; i < 3; i++) { - Vector3 axis; - axis[i] = 1.0; - - if (Math::abs(p_normal.dot(axis)) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { - r_amount = 2; - r_type = FEATURE_EDGE; - - int i_n = next[i]; - int i_n2 = next2[i]; - - Vector3 point = half_extents; - - if (p_normal[i_n] < 0) { - point[i_n] = -point[i_n]; - } - if (p_normal[i_n2] < 0) { - point[i_n2] = -point[i_n2]; - } - - r_supports[0] = point; - point[i] = -point[i]; - r_supports[1] = point; - return; - } - } - /* USE POINT */ - - Vector3 point( - (p_normal.x < 0) ? -half_extents.x : half_extents.x, - (p_normal.y < 0) ? -half_extents.y : half_extents.y, - (p_normal.z < 0) ? -half_extents.z : half_extents.z); - - r_amount = 1; - r_type = FEATURE_POINT; - r_supports[0] = point; -} - -bool BoxShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - AABB aabb(-half_extents, half_extents * 2.0); - - return aabb.intersects_segment(p_begin, p_end, &r_result, &r_normal); -} - -bool BoxShape3DSW::intersect_point(const Vector3 &p_point) const { - return (Math::abs(p_point.x) < half_extents.x && Math::abs(p_point.y) < half_extents.y && Math::abs(p_point.z) < half_extents.z); -} - -Vector3 BoxShape3DSW::get_closest_point_to(const Vector3 &p_point) const { - int outside = 0; - Vector3 min_point; - - for (int i = 0; i < 3; i++) { - if (Math::abs(p_point[i]) > half_extents[i]) { - outside++; - if (outside == 1) { - //use plane if only one side matches - Vector3 n; - n[i] = SGN(p_point[i]); - - Plane p(n, half_extents[i]); - min_point = p.project(p_point); - } - } - } - - if (!outside) { - return p_point; //it's inside, don't do anything else - } - - if (outside == 1) { //if only above one plane, this plane clearly wins - return min_point; - } - - //check segments - real_t min_distance = 1e20; - Vector3 closest_vertex = half_extents * p_point.sign(); - Vector3 s[2] = { - closest_vertex, - closest_vertex - }; - - for (int i = 0; i < 3; i++) { - s[1] = closest_vertex; - s[1][i] = -s[1][i]; //edge - - Vector3 closest_edge = Geometry3D::get_closest_point_to_segment(p_point, s); - - real_t d = p_point.distance_to(closest_edge); - if (d < min_distance) { - min_point = closest_edge; - min_distance = d; - } - } - - return min_point; -} - -Vector3 BoxShape3DSW::get_moment_of_inertia(real_t p_mass) const { - real_t lx = half_extents.x; - real_t ly = half_extents.y; - real_t lz = half_extents.z; - - return Vector3((p_mass / 3.0) * (ly * ly + lz * lz), (p_mass / 3.0) * (lx * lx + lz * lz), (p_mass / 3.0) * (lx * lx + ly * ly)); -} - -void BoxShape3DSW::_setup(const Vector3 &p_half_extents) { - half_extents = p_half_extents.abs(); - - configure(AABB(-half_extents, half_extents * 2)); -} - -void BoxShape3DSW::set_data(const Variant &p_data) { - _setup(p_data); -} - -Variant BoxShape3DSW::get_data() const { - return half_extents; -} - -BoxShape3DSW::BoxShape3DSW() {} - -/********** CAPSULE *************/ - -void CapsuleShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { - Vector3 n = p_transform.basis.xform_inv(p_normal).normalized(); - real_t h = height * 0.5 - radius; - - n *= radius; - n.y += (n.y > 0) ? h : -h; - - r_max = p_normal.dot(p_transform.xform(n)); - r_min = p_normal.dot(p_transform.xform(-n)); -} - -Vector3 CapsuleShape3DSW::get_support(const Vector3 &p_normal) const { - Vector3 n = p_normal; - - real_t h = height * 0.5 - radius; - - n *= radius; - n.y += (n.y > 0) ? h : -h; - return n; -} - -void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { - Vector3 n = p_normal; - - real_t d = n.y; - - if (Math::abs(d) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { - // make it flat - n.y = 0.0; - n.normalize(); - n *= radius; - - r_amount = 2; - r_type = FEATURE_EDGE; - r_supports[0] = n; - r_supports[0].y += height * 0.5 - radius; - r_supports[1] = n; - r_supports[1].y -= height * 0.5 - radius; - - } else { - real_t h = height * 0.5 - radius; - - n *= radius; - n.y += (d > 0) ? h : -h; - r_amount = 1; - r_type = FEATURE_POINT; - *r_supports = n; - } -} - -bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - Vector3 norm = (p_end - p_begin).normalized(); - real_t min_d = 1e20; - - Vector3 res, n; - bool collision = false; - - Vector3 auxres, auxn; - bool collided; - - // test against cylinder and spheres :-| - - collided = Geometry3D::segment_intersects_cylinder(p_begin, p_end, height - radius * 2.0, radius, &auxres, &auxn, 1); - - if (collided) { - real_t d = norm.dot(auxres); - if (d < min_d) { - min_d = d; - res = auxres; - n = auxn; - collision = true; - } - } - - collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * 0.5 - radius, 0), radius, &auxres, &auxn); - - if (collided) { - real_t d = norm.dot(auxres); - if (d < min_d) { - min_d = d; - res = auxres; - n = auxn; - collision = true; - } - } - - collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * -0.5 + radius, 0), radius, &auxres, &auxn); - - if (collided) { - real_t d = norm.dot(auxres); - - if (d < min_d) { - min_d = d; - res = auxres; - n = auxn; - collision = true; - } - } - - if (collision) { - r_result = res; - r_normal = n; - } - return collision; -} - -bool CapsuleShape3DSW::intersect_point(const Vector3 &p_point) const { - if (Math::abs(p_point.y) < height * 0.5 - radius) { - return Vector3(p_point.x, 0, p_point.z).length() < radius; - } else { - Vector3 p = p_point; - p.y = Math::abs(p.y) - height * 0.5 + radius; - return p.length() < radius; - } -} - -Vector3 CapsuleShape3DSW::get_closest_point_to(const Vector3 &p_point) const { - Vector3 s[2] = { - Vector3(0, -height * 0.5 + radius, 0), - Vector3(0, height * 0.5 - radius, 0), - }; - - Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, s); - - if (p.distance_to(p_point) < radius) { - return p_point; - } - - return p + (p_point - p).normalized() * radius; -} - -Vector3 CapsuleShape3DSW::get_moment_of_inertia(real_t p_mass) const { - // use bad AABB approximation - Vector3 extents = get_aabb().size * 0.5; - - return Vector3( - (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), - (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), - (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); -} - -void CapsuleShape3DSW::_setup(real_t p_height, real_t p_radius) { - height = p_height; - radius = p_radius; - configure(AABB(Vector3(-radius, -height * 0.5, -radius), Vector3(radius * 2, height, radius * 2))); -} - -void CapsuleShape3DSW::set_data(const Variant &p_data) { - Dictionary d = p_data; - ERR_FAIL_COND(!d.has("radius")); - ERR_FAIL_COND(!d.has("height")); - _setup(d["height"], d["radius"]); -} - -Variant CapsuleShape3DSW::get_data() const { - Dictionary d; - d["radius"] = radius; - d["height"] = height; - return d; -} - -CapsuleShape3DSW::CapsuleShape3DSW() {} - -/********** CYLINDER *************/ - -void CylinderShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { - Vector3 cylinder_axis = p_transform.basis.get_axis(1).normalized(); - real_t axis_dot = cylinder_axis.dot(p_normal); - - Vector3 local_normal = p_transform.basis.xform_inv(p_normal); - real_t scale = local_normal.length(); - real_t scaled_radius = radius * scale; - real_t scaled_height = height * scale; - - real_t length; - if (Math::abs(axis_dot) > 1.0) { - length = scaled_height * 0.5; - } else { - length = Math::abs(axis_dot * scaled_height * 0.5) + scaled_radius * Math::sqrt(1.0 - axis_dot * axis_dot); - } - - real_t distance = p_normal.dot(p_transform.origin); - - r_min = distance - length; - r_max = distance + length; -} - -Vector3 CylinderShape3DSW::get_support(const Vector3 &p_normal) const { - Vector3 n = p_normal; - real_t h = (n.y > 0) ? height : -height; - real_t s = Math::sqrt(n.x * n.x + n.z * n.z); - if (Math::is_zero_approx(s)) { - n.x = radius; - n.y = h * 0.5; - n.z = 0.0; - } else { - real_t d = radius / s; - n.x = n.x * d; - n.y = h * 0.5; - n.z = n.z * d; - } - - return n; -} - -void CylinderShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { - real_t d = p_normal.y; - if (Math::abs(d) > _CYLINDER_FACE_IS_VALID_SUPPORT_THRESHOLD) { - real_t h = (d > 0) ? height : -height; - - Vector3 n = p_normal; - n.x = 0.0; - n.z = 0.0; - n.y = h * 0.5; - - r_amount = 3; - r_type = FEATURE_CIRCLE; - r_supports[0] = n; - r_supports[1] = n; - r_supports[1].x += radius; - r_supports[2] = n; - r_supports[2].z += radius; - } else if (Math::abs(d) < _CYLINDER_EDGE_IS_VALID_SUPPORT_THRESHOLD) { - // make it flat - Vector3 n = p_normal; - n.y = 0.0; - n.normalize(); - n *= radius; - - r_amount = 2; - r_type = FEATURE_EDGE; - r_supports[0] = n; - r_supports[0].y += height * 0.5; - r_supports[1] = n; - r_supports[1].y -= height * 0.5; - } else { - r_amount = 1; - r_type = FEATURE_POINT; - r_supports[0] = get_support(p_normal); - return; - - Vector3 n = p_normal; - real_t h = n.y * Math::sqrt(0.25 * height * height + radius * radius); - if (Math::abs(h) > 1.0) { - // Top or bottom surface. - n.y = (n.y > 0.0) ? height * 0.5 : -height * 0.5; - } else { - // Lateral surface. - n.y = height * 0.5 * h; - } - - real_t s = Math::sqrt(n.x * n.x + n.z * n.z); - if (Math::is_zero_approx(s)) { - n.x = 0.0; - n.z = 0.0; - } else { - real_t scaled_radius = radius / s; - n.x = n.x * scaled_radius; - n.z = n.z * scaled_radius; - } - - r_supports[0] = n; - } -} - -bool CylinderShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - return Geometry3D::segment_intersects_cylinder(p_begin, p_end, height, radius, &r_result, &r_normal, 1); -} - -bool CylinderShape3DSW::intersect_point(const Vector3 &p_point) const { - if (Math::abs(p_point.y) < height * 0.5) { - return Vector3(p_point.x, 0, p_point.z).length() < radius; - } - return false; -} - -Vector3 CylinderShape3DSW::get_closest_point_to(const Vector3 &p_point) const { - if (Math::absf(p_point.y) > height * 0.5) { - // Project point to top disk. - real_t dir = p_point.y > 0.0 ? 1.0 : -1.0; - Vector3 circle_pos(0.0, dir * height * 0.5, 0.0); - Plane circle_plane(Vector3(0.0, dir, 0.0), circle_pos); - Vector3 proj_point = circle_plane.project(p_point); - - // Clip position. - Vector3 delta_point_1 = proj_point - circle_pos; - real_t dist_point_1 = delta_point_1.length_squared(); - if (!Math::is_zero_approx(dist_point_1)) { - dist_point_1 = Math::sqrt(dist_point_1); - proj_point = circle_pos + delta_point_1 * MIN(dist_point_1, radius) / dist_point_1; - } - - return proj_point; - } else { - Vector3 s[2] = { - Vector3(0, -height * 0.5, 0), - Vector3(0, height * 0.5, 0), - }; - - Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, s); - - if (p.distance_to(p_point) < radius) { - return p_point; - } - - return p + (p_point - p).normalized() * radius; - } -} - -Vector3 CylinderShape3DSW::get_moment_of_inertia(real_t p_mass) const { - // use bad AABB approximation - Vector3 extents = get_aabb().size * 0.5; - - return Vector3( - (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), - (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), - (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); -} - -void CylinderShape3DSW::_setup(real_t p_height, real_t p_radius) { - height = p_height; - radius = p_radius; - configure(AABB(Vector3(-radius, -height * 0.5, -radius), Vector3(radius * 2.0, height, radius * 2.0))); -} - -void CylinderShape3DSW::set_data(const Variant &p_data) { - Dictionary d = p_data; - ERR_FAIL_COND(!d.has("radius")); - ERR_FAIL_COND(!d.has("height")); - _setup(d["height"], d["radius"]); -} - -Variant CylinderShape3DSW::get_data() const { - Dictionary d; - d["radius"] = radius; - d["height"] = height; - return d; -} - -CylinderShape3DSW::CylinderShape3DSW() {} - -/********** CONVEX POLYGON *************/ - -void ConvexPolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { - int vertex_count = mesh.vertices.size(); - if (vertex_count == 0) { - return; - } - - const Vector3 *vrts = &mesh.vertices[0]; - - for (int i = 0; i < vertex_count; i++) { - real_t d = p_normal.dot(p_transform.xform(vrts[i])); - - if (i == 0 || d > r_max) { - r_max = d; - } - if (i == 0 || d < r_min) { - r_min = d; - } - } -} - -Vector3 ConvexPolygonShape3DSW::get_support(const Vector3 &p_normal) const { - Vector3 n = p_normal; - - int vert_support_idx = -1; - real_t support_max = 0; - - int vertex_count = mesh.vertices.size(); - if (vertex_count == 0) { - return Vector3(); - } - - const Vector3 *vrts = &mesh.vertices[0]; - - for (int i = 0; i < vertex_count; i++) { - real_t d = n.dot(vrts[i]); - - if (i == 0 || d > support_max) { - support_max = d; - vert_support_idx = i; - } - } - - return vrts[vert_support_idx]; -} - -void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { - const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); - int fc = mesh.faces.size(); - - const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); - int ec = mesh.edges.size(); - - const Vector3 *vertices = mesh.vertices.ptr(); - int vc = mesh.vertices.size(); - - r_amount = 0; - ERR_FAIL_COND_MSG(vc == 0, "Convex polygon shape has no vertices."); - - //find vertex first - real_t max = 0; - int vtx = 0; - - for (int i = 0; i < vc; i++) { - real_t d = p_normal.dot(vertices[i]); - - if (i == 0 || d > max) { - max = d; - vtx = i; - } - } - - for (int i = 0; i < fc; i++) { - if (faces[i].plane.normal.dot(p_normal) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { - int ic = faces[i].indices.size(); - const int *ind = faces[i].indices.ptr(); - - bool valid = false; - for (int j = 0; j < ic; j++) { - if (ind[j] == vtx) { - valid = true; - break; - } - } - - if (!valid) { - continue; - } - - int m = MIN(p_max, ic); - for (int j = 0; j < m; j++) { - r_supports[j] = vertices[ind[j]]; - } - r_amount = m; - r_type = FEATURE_FACE; - return; - } - } - - for (int i = 0; i < ec; i++) { - real_t dot = (vertices[edges[i].a] - vertices[edges[i].b]).normalized().dot(p_normal); - dot = ABS(dot); - if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD && (edges[i].a == vtx || edges[i].b == vtx)) { - r_amount = 2; - r_type = FEATURE_EDGE; - r_supports[0] = vertices[edges[i].a]; - r_supports[1] = vertices[edges[i].b]; - return; - } - } - - r_supports[0] = vertices[vtx]; - r_amount = 1; - r_type = FEATURE_POINT; -} - -bool ConvexPolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); - int fc = mesh.faces.size(); - - const Vector3 *vertices = mesh.vertices.ptr(); - - Vector3 n = p_end - p_begin; - real_t min = 1e20; - bool col = false; - - for (int i = 0; i < fc; i++) { - if (faces[i].plane.normal.dot(n) > 0) { - continue; //opposing face - } - - int ic = faces[i].indices.size(); - const int *ind = faces[i].indices.ptr(); - - for (int j = 1; j < ic - 1; j++) { - Face3 f(vertices[ind[0]], vertices[ind[j]], vertices[ind[j + 1]]); - Vector3 result; - if (f.intersects_segment(p_begin, p_end, &result)) { - real_t d = n.dot(result); - if (d < min) { - min = d; - r_result = result; - r_normal = faces[i].plane.normal; - col = true; - } - - break; - } - } - } - - return col; -} - -bool ConvexPolygonShape3DSW::intersect_point(const Vector3 &p_point) const { - const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); - int fc = mesh.faces.size(); - - for (int i = 0; i < fc; i++) { - if (faces[i].plane.distance_to(p_point) >= 0) { - return false; - } - } - - return true; -} - -Vector3 ConvexPolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) const { - const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); - int fc = mesh.faces.size(); - const Vector3 *vertices = mesh.vertices.ptr(); - - bool all_inside = true; - for (int i = 0; i < fc; i++) { - if (!faces[i].plane.is_point_over(p_point)) { - continue; - } - - all_inside = false; - bool is_inside = true; - int ic = faces[i].indices.size(); - const int *indices = faces[i].indices.ptr(); - - for (int j = 0; j < ic; j++) { - Vector3 a = vertices[indices[j]]; - Vector3 b = vertices[indices[(j + 1) % ic]]; - Vector3 n = (a - b).cross(faces[i].plane.normal).normalized(); - if (Plane(n, a).is_point_over(p_point)) { - is_inside = false; - break; - } - } - - if (is_inside) { - return faces[i].plane.project(p_point); - } - } - - if (all_inside) { - return p_point; - } - - real_t min_distance = 1e20; - Vector3 min_point; - - //check edges - const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); - int ec = mesh.edges.size(); - for (int i = 0; i < ec; i++) { - Vector3 s[2] = { - vertices[edges[i].a], - vertices[edges[i].b] - }; - - Vector3 closest = Geometry3D::get_closest_point_to_segment(p_point, s); - real_t d = closest.distance_to(p_point); - if (d < min_distance) { - min_distance = d; - min_point = closest; - } - } - - return min_point; -} - -Vector3 ConvexPolygonShape3DSW::get_moment_of_inertia(real_t p_mass) const { - // use bad AABB approximation - Vector3 extents = get_aabb().size * 0.5; - - return Vector3( - (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), - (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), - (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); -} - -void ConvexPolygonShape3DSW::_setup(const Vector &p_vertices) { - Error err = ConvexHullComputer::convex_hull(p_vertices, mesh); - if (err != OK) { - ERR_PRINT("Failed to build convex hull"); - } - - AABB _aabb; - - for (int i = 0; i < mesh.vertices.size(); i++) { - if (i == 0) { - _aabb.position = mesh.vertices[i]; - } else { - _aabb.expand_to(mesh.vertices[i]); - } - } - - configure(_aabb); -} - -void ConvexPolygonShape3DSW::set_data(const Variant &p_data) { - _setup(p_data); -} - -Variant ConvexPolygonShape3DSW::get_data() const { - return mesh.vertices; -} - -ConvexPolygonShape3DSW::ConvexPolygonShape3DSW() { -} - -/********** FACE POLYGON *************/ - -void FaceShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { - for (int i = 0; i < 3; i++) { - Vector3 v = p_transform.xform(vertex[i]); - real_t d = p_normal.dot(v); - - if (i == 0 || d > r_max) { - r_max = d; - } - - if (i == 0 || d < r_min) { - r_min = d; - } - } -} - -Vector3 FaceShape3DSW::get_support(const Vector3 &p_normal) const { - int vert_support_idx = -1; - real_t support_max = 0; - - for (int i = 0; i < 3; i++) { - real_t d = p_normal.dot(vertex[i]); - - if (i == 0 || d > support_max) { - support_max = d; - vert_support_idx = i; - } - } - - return vertex[vert_support_idx]; -} - -void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { - Vector3 n = p_normal; - - /** TEST FACE AS SUPPORT **/ - if (Math::abs(normal.dot(n)) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { - r_amount = 3; - r_type = FEATURE_FACE; - for (int i = 0; i < 3; i++) { - r_supports[i] = vertex[i]; - } - return; - } - - /** FIND SUPPORT VERTEX **/ - - int vert_support_idx = -1; - real_t support_max = 0; - - for (int i = 0; i < 3; i++) { - real_t d = n.dot(vertex[i]); - - if (i == 0 || d > support_max) { - support_max = d; - vert_support_idx = i; - } - } - - /** TEST EDGES AS SUPPORT **/ - - for (int i = 0; i < 3; i++) { - int nx = (i + 1) % 3; - if (i != vert_support_idx && nx != vert_support_idx) { - continue; - } - - // check if edge is valid as a support - real_t dot = (vertex[i] - vertex[nx]).normalized().dot(n); - dot = ABS(dot); - if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { - r_amount = 2; - r_type = FEATURE_EDGE; - r_supports[0] = vertex[i]; - r_supports[1] = vertex[nx]; - return; - } - } - - r_amount = 1; - r_type = FEATURE_POINT; - r_supports[0] = vertex[vert_support_idx]; -} - -bool FaceShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - bool c = Geometry3D::segment_intersects_triangle(p_begin, p_end, vertex[0], vertex[1], vertex[2], &r_result); - if (c) { - r_normal = Plane(vertex[0], vertex[1], vertex[2]).normal; - if (r_normal.dot(p_end - p_begin) > 0) { - if (backface_collision) { - r_normal = -r_normal; - } else { - c = false; - } - } - } - - return c; -} - -bool FaceShape3DSW::intersect_point(const Vector3 &p_point) const { - return false; //face is flat -} - -Vector3 FaceShape3DSW::get_closest_point_to(const Vector3 &p_point) const { - return Face3(vertex[0], vertex[1], vertex[2]).get_closest_point_to(p_point); -} - -Vector3 FaceShape3DSW::get_moment_of_inertia(real_t p_mass) const { - return Vector3(); // Sorry, but i don't think anyone cares, FaceShape! -} - -FaceShape3DSW::FaceShape3DSW() { - configure(AABB()); -} - -Vector ConcavePolygonShape3DSW::get_faces() const { - Vector rfaces; - rfaces.resize(faces.size() * 3); - - for (int i = 0; i < faces.size(); i++) { - Face f = faces.get(i); - - for (int j = 0; j < 3; j++) { - rfaces.set(i * 3 + j, vertices.get(f.indices[j])); - } - } - - return rfaces; -} - -void ConcavePolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { - int count = vertices.size(); - if (count == 0) { - r_min = 0; - r_max = 0; - return; - } - const Vector3 *vptr = vertices.ptr(); - - for (int i = 0; i < count; i++) { - real_t d = p_normal.dot(p_transform.xform(vptr[i])); - - if (i == 0 || d > r_max) { - r_max = d; - } - if (i == 0 || d < r_min) { - r_min = d; - } - } -} - -Vector3 ConcavePolygonShape3DSW::get_support(const Vector3 &p_normal) const { - int count = vertices.size(); - if (count == 0) { - return Vector3(); - } - - const Vector3 *vptr = vertices.ptr(); - - Vector3 n = p_normal; - - int vert_support_idx = -1; - real_t support_max = 0; - - for (int i = 0; i < count; i++) { - real_t d = n.dot(vptr[i]); - - if (i == 0 || d > support_max) { - support_max = d; - vert_support_idx = i; - } - } - - return vptr[vert_support_idx]; -} - -void ConcavePolygonShape3DSW::_cull_segment(int p_idx, _SegmentCullParams *p_params) const { - const BVH *bvh = &p_params->bvh[p_idx]; - - /* - if (p_params->dir.dot(bvh->aabb.get_support(-p_params->dir))>p_params->min_d) - return; //test against whole AABB, which isn't very costly - */ - - //printf("addr: %p\n",bvh); - if (!bvh->aabb.intersects_segment(p_params->from, p_params->to)) { - return; - } - - if (bvh->face_index >= 0) { - const Face *f = &p_params->faces[bvh->face_index]; - FaceShape3DSW *face = p_params->face; - face->normal = f->normal; - face->vertex[0] = p_params->vertices[f->indices[0]]; - face->vertex[1] = p_params->vertices[f->indices[1]]; - face->vertex[2] = p_params->vertices[f->indices[2]]; - - Vector3 res; - Vector3 normal; - if (face->intersect_segment(p_params->from, p_params->to, res, normal)) { - real_t d = p_params->dir.dot(res) - p_params->dir.dot(p_params->from); - if ((d > 0) && (d < p_params->min_d)) { - p_params->min_d = d; - p_params->result = res; - p_params->normal = normal; - p_params->collisions++; - } - } - } else { - if (bvh->left >= 0) { - _cull_segment(bvh->left, p_params); - } - if (bvh->right >= 0) { - _cull_segment(bvh->right, p_params); - } - } -} - -bool ConcavePolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - if (faces.size() == 0) { - return false; - } - - // unlock data - const Face *fr = faces.ptr(); - const Vector3 *vr = vertices.ptr(); - const BVH *br = bvh.ptr(); - - FaceShape3DSW face; - face.backface_collision = backface_collision; - - _SegmentCullParams params; - params.from = p_begin; - params.to = p_end; - params.dir = (p_end - p_begin).normalized(); - - params.faces = fr; - params.vertices = vr; - params.bvh = br; - - params.face = &face; - - // cull - _cull_segment(0, ¶ms); - - if (params.collisions > 0) { - r_result = params.result; - r_normal = params.normal; - return true; - } else { - return false; - } -} - -bool ConcavePolygonShape3DSW::intersect_point(const Vector3 &p_point) const { - return false; //face is flat -} - -Vector3 ConcavePolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) const { - return Vector3(); -} - -bool ConcavePolygonShape3DSW::_cull(int p_idx, _CullParams *p_params) const { - const BVH *bvh = &p_params->bvh[p_idx]; - - if (!p_params->aabb.intersects(bvh->aabb)) { - return false; - } - - if (bvh->face_index >= 0) { - const Face *f = &p_params->faces[bvh->face_index]; - FaceShape3DSW *face = p_params->face; - face->normal = f->normal; - face->vertex[0] = p_params->vertices[f->indices[0]]; - face->vertex[1] = p_params->vertices[f->indices[1]]; - face->vertex[2] = p_params->vertices[f->indices[2]]; - if (p_params->callback(p_params->userdata, face)) { - return true; - } - } else { - if (bvh->left >= 0) { - if (_cull(bvh->left, p_params)) { - return true; - } - } - - if (bvh->right >= 0) { - if (_cull(bvh->right, p_params)) { - return true; - } - } - } - - return false; -} - -void ConcavePolygonShape3DSW::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const { - // make matrix local to concave - if (faces.size() == 0) { - return; - } - - AABB local_aabb = p_local_aabb; - - // unlock data - const Face *fr = faces.ptr(); - const Vector3 *vr = vertices.ptr(); - const BVH *br = bvh.ptr(); - - FaceShape3DSW face; // use this to send in the callback - face.backface_collision = backface_collision; - - _CullParams params; - params.aabb = local_aabb; - params.face = &face; - params.faces = fr; - params.vertices = vr; - params.bvh = br; - params.callback = p_callback; - params.userdata = p_userdata; - - // cull - _cull(0, ¶ms); -} - -Vector3 ConcavePolygonShape3DSW::get_moment_of_inertia(real_t p_mass) const { - // use bad AABB approximation - Vector3 extents = get_aabb().size * 0.5; - - return Vector3( - (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), - (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), - (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); -} - -struct _VolumeSW_BVH_Element { - AABB aabb; - Vector3 center; - int face_index; -}; - -struct _VolumeSW_BVH_CompareX { - _FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const { - return a.center.x < b.center.x; - } -}; - -struct _VolumeSW_BVH_CompareY { - _FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const { - return a.center.y < b.center.y; - } -}; - -struct _VolumeSW_BVH_CompareZ { - _FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const { - return a.center.z < b.center.z; - } -}; - -struct _VolumeSW_BVH { - AABB aabb; - _VolumeSW_BVH *left; - _VolumeSW_BVH *right; - - int face_index; -}; - -_VolumeSW_BVH *_volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements, int p_size, int &count) { - _VolumeSW_BVH *bvh = memnew(_VolumeSW_BVH); - - if (p_size == 1) { - //leaf - bvh->aabb = p_elements[0].aabb; - bvh->left = nullptr; - bvh->right = nullptr; - bvh->face_index = p_elements->face_index; - count++; - return bvh; - } else { - bvh->face_index = -1; - } - - AABB aabb; - for (int i = 0; i < p_size; i++) { - if (i == 0) { - aabb = p_elements[i].aabb; - } else { - aabb.merge_with(p_elements[i].aabb); - } - } - bvh->aabb = aabb; - switch (aabb.get_longest_axis_index()) { - case 0: { - SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareX> sort_x; - sort_x.sort(p_elements, p_size); - - } break; - case 1: { - SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareY> sort_y; - sort_y.sort(p_elements, p_size); - } break; - case 2: { - SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareZ> sort_z; - sort_z.sort(p_elements, p_size); - } break; - } - - int split = p_size / 2; - bvh->left = _volume_sw_build_bvh(p_elements, split, count); - bvh->right = _volume_sw_build_bvh(&p_elements[split], p_size - split, count); - - //printf("branch at %p - %i: %i\n",bvh,count,bvh->face_index); - count++; - return bvh; -} - -void ConcavePolygonShape3DSW::_fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx) { - int idx = p_idx; - - p_bvh_array[idx].aabb = p_bvh_tree->aabb; - p_bvh_array[idx].face_index = p_bvh_tree->face_index; - //printf("%p - %i: %i(%p) -- %p:%p\n",%p_bvh_array[idx],p_idx,p_bvh_array[i]->face_index,&p_bvh_tree->face_index,p_bvh_tree->left,p_bvh_tree->right); - - if (p_bvh_tree->left) { - p_bvh_array[idx].left = ++p_idx; - _fill_bvh(p_bvh_tree->left, p_bvh_array, p_idx); - - } else { - p_bvh_array[p_idx].left = -1; - } - - if (p_bvh_tree->right) { - p_bvh_array[idx].right = ++p_idx; - _fill_bvh(p_bvh_tree->right, p_bvh_array, p_idx); - - } else { - p_bvh_array[p_idx].right = -1; - } - - memdelete(p_bvh_tree); -} - -void ConcavePolygonShape3DSW::_setup(const Vector &p_faces, bool p_backface_collision) { - int src_face_count = p_faces.size(); - if (src_face_count == 0) { - configure(AABB()); - return; - } - ERR_FAIL_COND(src_face_count % 3); - src_face_count /= 3; - - const Vector3 *facesr = p_faces.ptr(); - - Vector<_VolumeSW_BVH_Element> bvh_array; - bvh_array.resize(src_face_count); - - _VolumeSW_BVH_Element *bvh_arrayw = bvh_array.ptrw(); - - faces.resize(src_face_count); - Face *facesw = faces.ptrw(); - - vertices.resize(src_face_count * 3); - - Vector3 *verticesw = vertices.ptrw(); - - AABB _aabb; - - for (int i = 0; i < src_face_count; i++) { - Face3 face(facesr[i * 3 + 0], facesr[i * 3 + 1], facesr[i * 3 + 2]); - - bvh_arrayw[i].aabb = face.get_aabb(); - bvh_arrayw[i].center = bvh_arrayw[i].aabb.get_center(); - bvh_arrayw[i].face_index = i; - facesw[i].indices[0] = i * 3 + 0; - facesw[i].indices[1] = i * 3 + 1; - facesw[i].indices[2] = i * 3 + 2; - facesw[i].normal = face.get_plane().normal; - verticesw[i * 3 + 0] = face.vertex[0]; - verticesw[i * 3 + 1] = face.vertex[1]; - verticesw[i * 3 + 2] = face.vertex[2]; - if (i == 0) { - _aabb = bvh_arrayw[i].aabb; - } else { - _aabb.merge_with(bvh_arrayw[i].aabb); - } - } - - int count = 0; - _VolumeSW_BVH *bvh_tree = _volume_sw_build_bvh(bvh_arrayw, src_face_count, count); - - bvh.resize(count + 1); - - BVH *bvh_arrayw2 = bvh.ptrw(); - - int idx = 0; - _fill_bvh(bvh_tree, bvh_arrayw2, idx); - - backface_collision = p_backface_collision; - - configure(_aabb); // this type of shape has no margin -} - -void ConcavePolygonShape3DSW::set_data(const Variant &p_data) { - Dictionary d = p_data; - ERR_FAIL_COND(!d.has("faces")); - - _setup(d["faces"], d["backface_collision"]); -} - -Variant ConcavePolygonShape3DSW::get_data() const { - Dictionary d; - d["faces"] = get_faces(); - d["backface_collision"] = backface_collision; - - return d; -} - -ConcavePolygonShape3DSW::ConcavePolygonShape3DSW() { -} - -/* HEIGHT MAP SHAPE */ - -Vector HeightMapShape3DSW::get_heights() const { - return heights; -} - -int HeightMapShape3DSW::get_width() const { - return width; -} - -int HeightMapShape3DSW::get_depth() const { - return depth; -} - -void HeightMapShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { - //not very useful, but not very used either - p_transform.xform(get_aabb()).project_range_in_plane(Plane(p_normal), r_min, r_max); -} - -Vector3 HeightMapShape3DSW::get_support(const Vector3 &p_normal) const { - //not very useful, but not very used either - return get_aabb().get_support(p_normal); -} - -struct _HeightmapSegmentCullParams { - Vector3 from; - Vector3 to; - Vector3 dir; - - Vector3 result; - Vector3 normal; - - const HeightMapShape3DSW *heightmap = nullptr; - FaceShape3DSW *face = nullptr; -}; - -struct _HeightmapGridCullState { - real_t length = 0.0; - real_t length_flat = 0.0; - - real_t dist = 0.0; - real_t prev_dist = 0.0; - - int x = 0; - int z = 0; -}; - -_FORCE_INLINE_ bool _heightmap_face_cull_segment(_HeightmapSegmentCullParams &p_params) { - Vector3 res; - Vector3 normal; - if (p_params.face->intersect_segment(p_params.from, p_params.to, res, normal)) { - p_params.result = res; - p_params.normal = normal; - return true; - } - - return false; -} - -_FORCE_INLINE_ bool _heightmap_cell_cull_segment(_HeightmapSegmentCullParams &p_params, const _HeightmapGridCullState &p_state) { - // First triangle. - p_params.heightmap->_get_point(p_state.x, p_state.z, p_params.face->vertex[0]); - p_params.heightmap->_get_point(p_state.x + 1, p_state.z, p_params.face->vertex[1]); - p_params.heightmap->_get_point(p_state.x, p_state.z + 1, p_params.face->vertex[2]); - p_params.face->normal = Plane(p_params.face->vertex[0], p_params.face->vertex[1], p_params.face->vertex[2]).normal; - if (_heightmap_face_cull_segment(p_params)) { - return true; - } - - // Second triangle. - p_params.face->vertex[0] = p_params.face->vertex[1]; - p_params.heightmap->_get_point(p_state.x + 1, p_state.z + 1, p_params.face->vertex[1]); - p_params.face->normal = Plane(p_params.face->vertex[0], p_params.face->vertex[1], p_params.face->vertex[2]).normal; - if (_heightmap_face_cull_segment(p_params)) { - return true; - } - - return false; -} - -_FORCE_INLINE_ bool _heightmap_chunk_cull_segment(_HeightmapSegmentCullParams &p_params, const _HeightmapGridCullState &p_state) { - const HeightMapShape3DSW::Range &chunk = p_params.heightmap->_get_bounds_chunk(p_state.x, p_state.z); - - Vector3 enter_pos; - Vector3 exit_pos; - - if (p_state.length_flat > CMP_EPSILON) { - real_t flat_to_3d = p_state.length / p_state.length_flat; - real_t enter_param = p_state.prev_dist * flat_to_3d; - real_t exit_param = p_state.dist * flat_to_3d; - enter_pos = p_params.from + p_params.dir * enter_param; - exit_pos = p_params.from + p_params.dir * exit_param; - } else { - // Consider the ray vertical. - // (though we shouldn't reach this often because there is an early check up-front) - enter_pos = p_params.from; - exit_pos = p_params.to; - } - - // Transform positions to heightmap space. - enter_pos *= HeightMapShape3DSW::BOUNDS_CHUNK_SIZE; - exit_pos *= HeightMapShape3DSW::BOUNDS_CHUNK_SIZE; - - // We did enter the flat projection of the AABB, - // but we have to check if we intersect it on the vertical axis. - if ((enter_pos.y > chunk.max) && (exit_pos.y > chunk.max)) { - return false; - } - if ((enter_pos.y < chunk.min) && (exit_pos.y < chunk.min)) { - return false; - } - - return p_params.heightmap->_intersect_grid_segment(_heightmap_cell_cull_segment, enter_pos, exit_pos, p_params.heightmap->width, p_params.heightmap->depth, p_params.heightmap->local_origin, p_params.result, p_params.normal); -} - -template -bool HeightMapShape3DSW::_intersect_grid_segment(ProcessFunction &p_process, const Vector3 &p_begin, const Vector3 &p_end, int p_width, int p_depth, const Vector3 &offset, Vector3 &r_point, Vector3 &r_normal) const { - Vector3 delta = (p_end - p_begin); - real_t length = delta.length(); - - if (length < CMP_EPSILON) { - return false; - } - - Vector3 local_begin = p_begin + offset; - - FaceShape3DSW face; - face.backface_collision = false; - - _HeightmapSegmentCullParams params; - params.from = p_begin; - params.to = p_end; - params.dir = delta / length; - params.heightmap = this; - params.face = &face; - - _HeightmapGridCullState state; - - // Perform grid query from projected ray. - Vector2 ray_dir_flat(delta.x, delta.z); - state.length = length; - state.length_flat = ray_dir_flat.length(); - - if (state.length_flat < CMP_EPSILON) { - ray_dir_flat = Vector2(); - } else { - ray_dir_flat /= state.length_flat; - } - - const int x_step = (ray_dir_flat.x > CMP_EPSILON) ? 1 : ((ray_dir_flat.x < -CMP_EPSILON) ? -1 : 0); - const int z_step = (ray_dir_flat.y > CMP_EPSILON) ? 1 : ((ray_dir_flat.y < -CMP_EPSILON) ? -1 : 0); - - const real_t infinite = 1e20; - const real_t delta_x = (x_step != 0) ? 1.f / Math::abs(ray_dir_flat.x) : infinite; - const real_t delta_z = (z_step != 0) ? 1.f / Math::abs(ray_dir_flat.y) : infinite; - - real_t cross_x; // At which value of `param` we will cross a x-axis lane? - real_t cross_z; // At which value of `param` we will cross a z-axis lane? - - // X initialization. - if (x_step != 0) { - if (x_step == 1) { - cross_x = (Math::ceil(local_begin.x) - local_begin.x) * delta_x; - } else { - cross_x = (local_begin.x - Math::floor(local_begin.x)) * delta_x; - } - } else { - cross_x = infinite; // Will never cross on X. - } - - // Z initialization. - if (z_step != 0) { - if (z_step == 1) { - cross_z = (Math::ceil(local_begin.z) - local_begin.z) * delta_z; - } else { - cross_z = (local_begin.z - Math::floor(local_begin.z)) * delta_z; - } - } else { - cross_z = infinite; // Will never cross on Z. - } - - int x = Math::floor(local_begin.x); - int z = Math::floor(local_begin.z); - - // Workaround cases where the ray starts at an integer position. - if (Math::is_zero_approx(cross_x)) { - cross_x += delta_x; - // If going backwards, we should ignore the position we would get by the above flooring, - // because the ray is not heading in that direction. - if (x_step == -1) { - x -= 1; - } - } - - if (Math::is_zero_approx(cross_z)) { - cross_z += delta_z; - if (z_step == -1) { - z -= 1; - } - } - - // Start inside the grid. - int x_start = MAX(MIN(x, p_width - 2), 0); - int z_start = MAX(MIN(z, p_depth - 2), 0); - - // Adjust initial cross values. - cross_x += delta_x * x_step * (x_start - x); - cross_z += delta_z * z_step * (z_start - z); - - x = x_start; - z = z_start; - - while (true) { - state.prev_dist = state.dist; - state.x = x; - state.z = z; - - if (cross_x < cross_z) { - // X lane. - x += x_step; - // Assign before advancing the param, - // to be in sync with the initialization step. - state.dist = cross_x; - cross_x += delta_x; - } else { - // Z lane. - z += z_step; - state.dist = cross_z; - cross_z += delta_z; - } - - if (state.dist > state.length_flat) { - state.dist = state.length_flat; - if (p_process(params, state)) { - r_point = params.result; - r_normal = params.normal; - return true; - } - break; - } - - if (p_process(params, state)) { - r_point = params.result; - r_normal = params.normal; - return true; - } - - // Stop when outside the grid. - if ((x < 0) || (z < 0) || (x >= p_width - 1) || (z >= p_depth - 1)) { - break; - } - } - - return false; -} - -bool HeightMapShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const { - if (heights.is_empty()) { - return false; - } - - Vector3 local_begin = p_begin + local_origin; - Vector3 local_end = p_end + local_origin; - - // Quantize the ray begin/end. - int begin_x = Math::floor(local_begin.x); - int begin_z = Math::floor(local_begin.z); - int end_x = Math::floor(local_end.x); - int end_z = Math::floor(local_end.z); - - if ((begin_x == end_x) && (begin_z == end_z)) { - // Simple case for rays that don't traverse the grid horizontally. - // Just perform a test on the given cell. - FaceShape3DSW face; - face.backface_collision = false; - - _HeightmapSegmentCullParams params; - params.from = p_begin; - params.to = p_end; - params.dir = (p_end - p_begin).normalized(); - - params.heightmap = this; - params.face = &face; - - _HeightmapGridCullState state; - state.x = MAX(MIN(begin_x, width - 2), 0); - state.z = MAX(MIN(begin_z, depth - 2), 0); - if (_heightmap_cell_cull_segment(params, state)) { - r_point = params.result; - r_normal = params.normal; - return true; - } - } else if (bounds_grid.is_empty()) { - // Process all cells intersecting the flat projection of the ray. - return _intersect_grid_segment(_heightmap_cell_cull_segment, p_begin, p_end, width, depth, local_origin, r_point, r_normal); - } else { - Vector3 ray_diff = (p_end - p_begin); - real_t length_flat_sqr = ray_diff.x * ray_diff.x + ray_diff.z * ray_diff.z; - if (length_flat_sqr < BOUNDS_CHUNK_SIZE * BOUNDS_CHUNK_SIZE) { - // Don't use chunks, the ray is too short in the plane. - return _intersect_grid_segment(_heightmap_cell_cull_segment, p_begin, p_end, width, depth, local_origin, r_point, r_normal); - } else { - // The ray is long, run raycast on a higher-level grid. - Vector3 bounds_from = p_begin / BOUNDS_CHUNK_SIZE; - Vector3 bounds_to = p_end / BOUNDS_CHUNK_SIZE; - Vector3 bounds_offset = local_origin / BOUNDS_CHUNK_SIZE; - return _intersect_grid_segment(_heightmap_chunk_cull_segment, bounds_from, bounds_to, bounds_grid_width, bounds_grid_depth, bounds_offset, r_point, r_normal); - } - } - - return false; -} - -bool HeightMapShape3DSW::intersect_point(const Vector3 &p_point) const { - return false; -} - -Vector3 HeightMapShape3DSW::get_closest_point_to(const Vector3 &p_point) const { - return Vector3(); -} - -void HeightMapShape3DSW::_get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const { - const AABB &aabb = get_aabb(); - - Vector3 pos_local = aabb.position + local_origin; - - Vector3 clamped_point(p_point); - clamped_point.x = CLAMP(p_point.x, pos_local.x, pos_local.x + aabb.size.x); - clamped_point.y = CLAMP(p_point.y, pos_local.y, pos_local.y + aabb.size.y); - clamped_point.z = CLAMP(p_point.z, pos_local.z, pos_local.x + aabb.size.z); - - r_x = (clamped_point.x < 0.0) ? (clamped_point.x - 0.5) : (clamped_point.x + 0.5); - r_y = (clamped_point.y < 0.0) ? (clamped_point.y - 0.5) : (clamped_point.y + 0.5); - r_z = (clamped_point.z < 0.0) ? (clamped_point.z - 0.5) : (clamped_point.z + 0.5); -} - -void HeightMapShape3DSW::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const { - if (heights.is_empty()) { - return; - } - - AABB local_aabb = p_local_aabb; - local_aabb.position += local_origin; - - // Quantize the aabb, and adjust the start/end ranges. - int aabb_min[3]; - int aabb_max[3]; - _get_cell(local_aabb.position, aabb_min[0], aabb_min[1], aabb_min[2]); - _get_cell(local_aabb.position + local_aabb.size, aabb_max[0], aabb_max[1], aabb_max[2]); - - // Expand the min/max quantized values. - // This is to catch the case where the input aabb falls between grid points. - for (int i = 0; i < 3; ++i) { - aabb_min[i]--; - aabb_max[i]++; - } - - int start_x = MAX(0, aabb_min[0]); - int end_x = MIN(width - 1, aabb_max[0]); - int start_z = MAX(0, aabb_min[2]); - int end_z = MIN(depth - 1, aabb_max[2]); - - FaceShape3DSW face; - face.backface_collision = true; - - for (int z = start_z; z < end_z; z++) { - for (int x = start_x; x < end_x; x++) { - // First triangle. - _get_point(x, z, face.vertex[0]); - _get_point(x + 1, z, face.vertex[1]); - _get_point(x, z + 1, face.vertex[2]); - face.normal = Plane(face.vertex[0], face.vertex[1], face.vertex[2]).normal; - if (p_callback(p_userdata, &face)) { - return; - } - - // Second triangle. - face.vertex[0] = face.vertex[1]; - _get_point(x + 1, z + 1, face.vertex[1]); - face.normal = Plane(face.vertex[0], face.vertex[1], face.vertex[2]).normal; - if (p_callback(p_userdata, &face)) { - return; - } - } - } -} - -Vector3 HeightMapShape3DSW::get_moment_of_inertia(real_t p_mass) const { - // use bad AABB approximation - Vector3 extents = get_aabb().size * 0.5; - - return Vector3( - (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), - (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), - (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); -} - -void HeightMapShape3DSW::_build_accelerator() { - bounds_grid.clear(); - - bounds_grid_width = width / BOUNDS_CHUNK_SIZE; - bounds_grid_depth = depth / BOUNDS_CHUNK_SIZE; - - if (width % BOUNDS_CHUNK_SIZE > 0) { - ++bounds_grid_width; // In case terrain size isn't dividable by chunk size. - } - - if (depth % BOUNDS_CHUNK_SIZE > 0) { - ++bounds_grid_depth; - } - - uint32_t bound_grid_size = (uint32_t)(bounds_grid_width * bounds_grid_depth); - - if (bound_grid_size < 2) { - // Grid is empty or just one chunk. - return; - } - - bounds_grid.resize(bound_grid_size); - - // Compute min and max height for all chunks. - for (int cz = 0; cz < bounds_grid_depth; ++cz) { - int z0 = cz * BOUNDS_CHUNK_SIZE; - - for (int cx = 0; cx < bounds_grid_width; ++cx) { - int x0 = cx * BOUNDS_CHUNK_SIZE; - - Range r; - - r.min = _get_height(x0, z0); - r.max = r.min; - - // Compute min and max height for this chunk. - // We have to include one extra cell to account for neighbors. - // Here is why: - // Say we have a flat terrain, and a plateau that fits a chunk perfectly. - // - // Left Right - // 0---0---0---1---1---1 - // | | | | | | - // 0---0---0---1---1---1 - // | | | | | | - // 0---0---0---1---1---1 - // x - // - // If the AABB for the Left chunk did not share vertices with the Right, - // then we would fail collision tests at x due to a gap. - // - int z_max = MIN(z0 + BOUNDS_CHUNK_SIZE + 1, depth); - int x_max = MIN(x0 + BOUNDS_CHUNK_SIZE + 1, width); - for (int z = z0; z < z_max; ++z) { - for (int x = x0; x < x_max; ++x) { - real_t height = _get_height(x, z); - if (height < r.min) { - r.min = height; - } else if (height > r.max) { - r.max = height; - } - } - } - - bounds_grid[cx + cz * bounds_grid_width] = r; - } - } -} - -void HeightMapShape3DSW::_setup(const Vector &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height) { - heights = p_heights; - width = p_width; - depth = p_depth; - - // Initialize aabb. - AABB aabb; - aabb.position = Vector3(0.0, p_min_height, 0.0); - aabb.size = Vector3(p_width - 1, p_max_height - p_min_height, p_depth - 1); - - // Initialize origin as the aabb center. - local_origin = aabb.position + 0.5 * aabb.size; - local_origin.y = 0.0; - - aabb.position -= local_origin; - - _build_accelerator(); - - configure(aabb); -} - -void HeightMapShape3DSW::set_data(const Variant &p_data) { - ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY); - - Dictionary d = p_data; - ERR_FAIL_COND(!d.has("width")); - ERR_FAIL_COND(!d.has("depth")); - ERR_FAIL_COND(!d.has("heights")); - - int width = d["width"]; - int depth = d["depth"]; - - ERR_FAIL_COND(width <= 0.0); - ERR_FAIL_COND(depth <= 0.0); - - Variant heights_variant = d["heights"]; - Vector heights_buffer; -#ifdef REAL_T_IS_DOUBLE - if (heights_variant.get_type() == Variant::PACKED_FLOAT64_ARRAY) { -#else - if (heights_variant.get_type() == Variant::PACKED_FLOAT32_ARRAY) { -#endif - // Ready-to-use heights can be passed. - heights_buffer = heights_variant; - } else if (heights_variant.get_type() == Variant::OBJECT) { - // If an image is passed, we have to convert it. - // This would be expensive to do with a script, so it's nice to have it here. - Ref image = heights_variant; - ERR_FAIL_COND(image.is_null()); - ERR_FAIL_COND(image->get_format() != Image::FORMAT_RF); - - PackedByteArray im_data = image->get_data(); - heights_buffer.resize(image->get_width() * image->get_height()); - - real_t *w = heights_buffer.ptrw(); - real_t *rp = (real_t *)im_data.ptr(); - for (int i = 0; i < heights_buffer.size(); ++i) { - w[i] = rp[i]; - } - } else { -#ifdef REAL_T_IS_DOUBLE - ERR_FAIL_MSG("Expected PackedFloat64Array or float Image."); -#else - ERR_FAIL_MSG("Expected PackedFloat32Array or float Image."); -#endif - } - - // Compute min and max heights or use precomputed values. - real_t min_height = 0.0; - real_t max_height = 0.0; - if (d.has("min_height") && d.has("max_height")) { - min_height = d["min_height"]; - max_height = d["max_height"]; - } else { - int heights_size = heights.size(); - for (int i = 0; i < heights_size; ++i) { - real_t h = heights[i]; - if (h < min_height) { - min_height = h; - } else if (h > max_height) { - max_height = h; - } - } - } - - ERR_FAIL_COND(min_height > max_height); - - ERR_FAIL_COND(heights_buffer.size() != (width * depth)); - - // If specified, min and max height will be used as precomputed values. - _setup(heights_buffer, width, depth, min_height, max_height); -} - -Variant HeightMapShape3DSW::get_data() const { - Dictionary d; - d["width"] = width; - d["depth"] = depth; - - const AABB &aabb = get_aabb(); - d["min_height"] = aabb.position.y; - d["max_height"] = aabb.position.y + aabb.size.y; - - d["heights"] = heights; - - return d; -} - -HeightMapShape3DSW::HeightMapShape3DSW() { -} diff --git a/servers/physics_3d/shape_3d_sw.h b/servers/physics_3d/shape_3d_sw.h deleted file mode 100644 index 061d66a085..0000000000 --- a/servers/physics_3d/shape_3d_sw.h +++ /dev/null @@ -1,510 +0,0 @@ -/*************************************************************************/ -/* shape_3d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 SHAPE_SW_H -#define SHAPE_SW_H - -#include "core/math/geometry_3d.h" -#include "core/templates/local_vector.h" -#include "servers/physics_server_3d.h" - -class Shape3DSW; - -class ShapeOwner3DSW { -public: - virtual void _shape_changed() = 0; - virtual void remove_shape(Shape3DSW *p_shape) = 0; - - virtual ~ShapeOwner3DSW() {} -}; - -class Shape3DSW { - RID self; - AABB aabb; - bool configured = false; - real_t custom_bias = 0.0; - - Map owners; - -protected: - void configure(const AABB &p_aabb); - -public: - enum FeatureType { - FEATURE_POINT, - FEATURE_EDGE, - FEATURE_FACE, - FEATURE_CIRCLE, - }; - - virtual real_t get_area() const { return aabb.get_area(); } - - _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } - _FORCE_INLINE_ RID get_self() const { return self; } - - virtual PhysicsServer3D::ShapeType get_type() const = 0; - - _FORCE_INLINE_ const AABB &get_aabb() const { return aabb; } - _FORCE_INLINE_ bool is_configured() const { return configured; } - - virtual bool is_concave() const { return false; } - - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const = 0; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const = 0; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const = 0; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const = 0; - virtual bool intersect_point(const Vector3 &p_point) const = 0; - virtual Vector3 get_moment_of_inertia(real_t p_mass) const = 0; - - virtual void set_data(const Variant &p_data) = 0; - virtual Variant get_data() const = 0; - - _FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias = p_bias; } - _FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; } - - void add_owner(ShapeOwner3DSW *p_owner); - void remove_owner(ShapeOwner3DSW *p_owner); - bool is_owner(ShapeOwner3DSW *p_owner) const; - const Map &get_owners() const; - - Shape3DSW() {} - virtual ~Shape3DSW(); -}; - -class ConcaveShape3DSW : public Shape3DSW { -public: - virtual bool is_concave() const override { return true; } - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; } - - // Returns true to stop the query. - typedef bool (*QueryCallback)(void *p_userdata, Shape3DSW *p_convex); - - virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0; - - ConcaveShape3DSW() {} -}; - -class WorldBoundaryShape3DSW : public Shape3DSW { - Plane plane; - - void _setup(const Plane &p_plane); - -public: - Plane get_plane() const; - - virtual real_t get_area() const override { return INFINITY; } - virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_WORLD_BOUNDARY; } - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; - virtual Vector3 get_support(const Vector3 &p_normal) const override; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; } - - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; - virtual bool intersect_point(const Vector3 &p_point) const override; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - WorldBoundaryShape3DSW(); -}; - -class SeparationRayShape3DSW : public Shape3DSW { - real_t length = 1.0; - bool slide_on_slope = false; - - void _setup(real_t p_length, bool p_slide_on_slope); - -public: - real_t get_length() const; - bool get_slide_on_slope() const; - - virtual real_t get_area() const override { return 0.0; } - virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_SEPARATION_RAY; } - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; - virtual Vector3 get_support(const Vector3 &p_normal) const override; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; - - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; - virtual bool intersect_point(const Vector3 &p_point) const override; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - - virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - SeparationRayShape3DSW(); -}; - -class SphereShape3DSW : public Shape3DSW { - real_t radius = 0.0; - - void _setup(real_t p_radius); - -public: - real_t get_radius() const; - - virtual real_t get_area() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius; } - - virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_SPHERE; } - - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; - virtual Vector3 get_support(const Vector3 &p_normal) const override; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; - virtual bool intersect_point(const Vector3 &p_point) const override; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - - virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - SphereShape3DSW(); -}; - -class BoxShape3DSW : public Shape3DSW { - Vector3 half_extents; - void _setup(const Vector3 &p_half_extents); - -public: - _FORCE_INLINE_ Vector3 get_half_extents() const { return half_extents; } - virtual real_t get_area() const override { return 8 * half_extents.x * half_extents.y * half_extents.z; } - - virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_BOX; } - - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; - virtual Vector3 get_support(const Vector3 &p_normal) const override; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; - virtual bool intersect_point(const Vector3 &p_point) const override; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - - virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - BoxShape3DSW(); -}; - -class CapsuleShape3DSW : public Shape3DSW { - real_t height = 0.0; - real_t radius = 0.0; - - void _setup(real_t p_height, real_t p_radius); - -public: - _FORCE_INLINE_ real_t get_height() const { return height; } - _FORCE_INLINE_ real_t get_radius() const { return radius; } - - virtual real_t get_area() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius + (height - radius * 2.0) * Math_PI * radius * radius; } - - virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CAPSULE; } - - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; - virtual Vector3 get_support(const Vector3 &p_normal) const override; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; - virtual bool intersect_point(const Vector3 &p_point) const override; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - - virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - CapsuleShape3DSW(); -}; - -class CylinderShape3DSW : public Shape3DSW { - real_t height = 0.0; - real_t radius = 0.0; - - void _setup(real_t p_height, real_t p_radius); - -public: - _FORCE_INLINE_ real_t get_height() const { return height; } - _FORCE_INLINE_ real_t get_radius() const { return radius; } - - virtual real_t get_area() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius + height * Math_PI * radius * radius; } - - virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CYLINDER; } - - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; - virtual Vector3 get_support(const Vector3 &p_normal) const override; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; - virtual bool intersect_point(const Vector3 &p_point) const override; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - - virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - CylinderShape3DSW(); -}; - -struct ConvexPolygonShape3DSW : public Shape3DSW { - Geometry3D::MeshData mesh; - - void _setup(const Vector &p_vertices); - -public: - const Geometry3D::MeshData &get_mesh() const { return mesh; } - - virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; } - - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; - virtual Vector3 get_support(const Vector3 &p_normal) const override; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; - virtual bool intersect_point(const Vector3 &p_point) const override; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - - virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - ConvexPolygonShape3DSW(); -}; - -struct _VolumeSW_BVH; -struct FaceShape3DSW; - -struct ConcavePolygonShape3DSW : public ConcaveShape3DSW { - // always a trimesh - - struct Face { - Vector3 normal; - int indices[3] = {}; - }; - - Vector faces; - Vector vertices; - - struct BVH { - AABB aabb; - int left = 0; - int right = 0; - - int face_index = 0; - }; - - Vector bvh; - - struct _CullParams { - AABB aabb; - QueryCallback callback = nullptr; - void *userdata = nullptr; - const Face *faces = nullptr; - const Vector3 *vertices = nullptr; - const BVH *bvh = nullptr; - FaceShape3DSW *face = nullptr; - }; - - struct _SegmentCullParams { - Vector3 from; - Vector3 to; - Vector3 dir; - const Face *faces = nullptr; - const Vector3 *vertices = nullptr; - const BVH *bvh = nullptr; - FaceShape3DSW *face = nullptr; - - Vector3 result; - Vector3 normal; - real_t min_d = 1e20; - int collisions = 0; - }; - - bool backface_collision = false; - - void _cull_segment(int p_idx, _SegmentCullParams *p_params) const; - bool _cull(int p_idx, _CullParams *p_params) const; - - void _fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx); - - void _setup(const Vector &p_faces, bool p_backface_collision); - -public: - Vector get_faces() const; - - virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; } - - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; - virtual Vector3 get_support(const Vector3 &p_normal) const override; - - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; - virtual bool intersect_point(const Vector3 &p_point) const override; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - - virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override; - - virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - ConcavePolygonShape3DSW(); -}; - -struct HeightMapShape3DSW : public ConcaveShape3DSW { - Vector heights; - int width = 0; - int depth = 0; - Vector3 local_origin; - - // Accelerator. - struct Range { - real_t min = 0.0; - real_t max = 0.0; - }; - LocalVector bounds_grid; - int bounds_grid_width = 0; - int bounds_grid_depth = 0; - - static const int BOUNDS_CHUNK_SIZE = 16; - - _FORCE_INLINE_ const Range &_get_bounds_chunk(int p_x, int p_z) const { - return bounds_grid[(p_z * bounds_grid_width) + p_x]; - } - - _FORCE_INLINE_ real_t _get_height(int p_x, int p_z) const { - return heights[(p_z * width) + p_x]; - } - - _FORCE_INLINE_ void _get_point(int p_x, int p_z, Vector3 &r_point) const { - r_point.x = p_x - 0.5 * (width - 1.0); - r_point.y = _get_height(p_x, p_z); - r_point.z = p_z - 0.5 * (depth - 1.0); - } - - void _get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const; - - void _build_accelerator(); - - template - bool _intersect_grid_segment(ProcessFunction &p_process, const Vector3 &p_begin, const Vector3 &p_end, int p_width, int p_depth, const Vector3 &offset, Vector3 &r_point, Vector3 &r_normal) const; - - void _setup(const Vector &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height); - -public: - Vector get_heights() const; - int get_width() const; - int get_depth() const; - - virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_HEIGHTMAP; } - - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; - virtual Vector3 get_support(const Vector3 &p_normal) const override; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const override; - virtual bool intersect_point(const Vector3 &p_point) const override; - - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override; - - virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - HeightMapShape3DSW(); -}; - -//used internally -struct FaceShape3DSW : public Shape3DSW { - Vector3 normal; //cache - Vector3 vertex[3]; - bool backface_collision = false; - - virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; } - - const Vector3 &get_vertex(int p_idx) const { return vertex[p_idx]; } - - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; - virtual Vector3 get_support(const Vector3 &p_normal) const override; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; - virtual bool intersect_point(const Vector3 &p_point) const override; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - - virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - - virtual void set_data(const Variant &p_data) override {} - virtual Variant get_data() const override { return Variant(); } - - FaceShape3DSW(); -}; - -struct MotionShape3DSW : public Shape3DSW { - Shape3DSW *shape = nullptr; - Vector3 motion; - - virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; } - - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override { - Vector3 cast = p_transform.basis.xform(motion); - real_t mina, maxa; - real_t minb, maxb; - Transform3D ofsb = p_transform; - ofsb.origin += cast; - shape->project_range(p_normal, p_transform, mina, maxa); - shape->project_range(p_normal, ofsb, minb, maxb); - r_min = MIN(mina, minb); - r_max = MAX(maxa, maxb); - } - - virtual Vector3 get_support(const Vector3 &p_normal) const override { - Vector3 support = shape->get_support(p_normal); - if (p_normal.dot(motion) > 0) { - support += motion; - } - return support; - } - - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; } - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override { return false; } - virtual bool intersect_point(const Vector3 &p_point) const override { return false; } - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override { return p_point; } - - virtual Vector3 get_moment_of_inertia(real_t p_mass) const override { return Vector3(); } - - virtual void set_data(const Variant &p_data) override {} - virtual Variant get_data() const override { return Variant(); } - - MotionShape3DSW() { configure(AABB()); } -}; - -#endif // SHAPE_SW_H diff --git a/servers/physics_3d/soft_body_3d_sw.cpp b/servers/physics_3d/soft_body_3d_sw.cpp deleted file mode 100644 index c9166810fe..0000000000 --- a/servers/physics_3d/soft_body_3d_sw.cpp +++ /dev/null @@ -1,1322 +0,0 @@ -/*************************************************************************/ -/* soft_body_3d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "soft_body_3d_sw.h" -#include "space_3d_sw.h" - -#include "core/math/geometry_3d.h" -#include "core/templates/map.h" -#include "servers/rendering_server.h" - -// Based on Bullet soft body. - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///btSoftBody implementation by Nathanael Presson - -SoftBody3DSW::SoftBody3DSW() : - CollisionObject3DSW(TYPE_SOFT_BODY), - active_list(this) { - _set_static(false); -} - -void SoftBody3DSW::_shapes_changed() { -} - -void SoftBody3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) { - switch (p_state) { - case PhysicsServer3D::BODY_STATE_TRANSFORM: { - _set_transform(p_variant); - _set_inv_transform(get_transform().inverse()); - - apply_nodes_transform(get_transform()); - - } break; - case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { - // Not supported. - ERR_FAIL_MSG("Linear velocity is not supported for Soft bodies."); - } break; - case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { - ERR_FAIL_MSG("Angular velocity is not supported for Soft bodies."); - } break; - case PhysicsServer3D::BODY_STATE_SLEEPING: { - ERR_FAIL_MSG("Sleeping state is not supported for Soft bodies."); - } break; - case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { - ERR_FAIL_MSG("Sleeping state is not supported for Soft bodies."); - } break; - } -} - -Variant SoftBody3DSW::get_state(PhysicsServer3D::BodyState p_state) const { - switch (p_state) { - case PhysicsServer3D::BODY_STATE_TRANSFORM: { - return get_transform(); - } break; - case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { - ERR_FAIL_V_MSG(Vector3(), "Linear velocity is not supported for Soft bodies."); - } break; - case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { - ERR_FAIL_V_MSG(Vector3(), "Angular velocity is not supported for Soft bodies."); - return Vector3(); - } break; - case PhysicsServer3D::BODY_STATE_SLEEPING: { - ERR_FAIL_V_MSG(false, "Sleeping state is not supported for Soft bodies."); - } break; - case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { - ERR_FAIL_V_MSG(false, "Sleeping state is not supported for Soft bodies."); - } break; - } - - return Variant(); -} - -void SoftBody3DSW::set_space(Space3DSW *p_space) { - if (get_space()) { - get_space()->soft_body_remove_from_active_list(&active_list); - - deinitialize_shape(); - } - - _set_space(p_space); - - if (get_space()) { - get_space()->soft_body_add_to_active_list(&active_list); - - if (bounds != AABB()) { - initialize_shape(true); - } - } -} - -void SoftBody3DSW::set_mesh(RID p_mesh) { - destroy(); - - soft_mesh = p_mesh; - - if (soft_mesh.is_null()) { - return; - } - - Array arrays = RenderingServer::get_singleton()->mesh_surface_get_arrays(soft_mesh, 0); - - bool success = create_from_trimesh(arrays[RenderingServer::ARRAY_INDEX], arrays[RenderingServer::ARRAY_VERTEX]); - if (!success) { - destroy(); - } -} - -void SoftBody3DSW::update_rendering_server(RenderingServerHandler *p_rendering_server_handler) { - if (soft_mesh.is_null()) { - return; - } - - const uint32_t vertex_count = map_visual_to_physics.size(); - for (uint32_t i = 0; i < vertex_count; ++i) { - const uint32_t node_index = map_visual_to_physics[i]; - const Node &node = nodes[node_index]; - const Vector3 &vertex_position = node.x; - const Vector3 &vertex_normal = node.n; - - p_rendering_server_handler->set_vertex(i, &vertex_position); - p_rendering_server_handler->set_normal(i, &vertex_normal); - } - - p_rendering_server_handler->set_aabb(bounds); -} - -void SoftBody3DSW::update_normals_and_centroids() { - uint32_t i, ni; - - for (i = 0, ni = nodes.size(); i < ni; ++i) { - nodes[i].n = Vector3(); - } - - for (i = 0, ni = faces.size(); i < ni; ++i) { - Face &face = faces[i]; - const Vector3 n = vec3_cross(face.n[0]->x - face.n[2]->x, face.n[0]->x - face.n[1]->x); - face.n[0]->n += n; - face.n[1]->n += n; - face.n[2]->n += n; - face.normal = n; - face.normal.normalize(); - face.centroid = 0.33333333333 * (face.n[0]->x + face.n[1]->x + face.n[2]->x); - } - - for (i = 0, ni = nodes.size(); i < ni; ++i) { - Node &node = nodes[i]; - real_t len = node.n.length(); - if (len > CMP_EPSILON) { - node.n /= len; - } - } -} - -void SoftBody3DSW::update_bounds() { - AABB prev_bounds = bounds; - prev_bounds.grow_by(collision_margin); - - bounds = AABB(); - - const uint32_t nodes_count = nodes.size(); - if (nodes_count == 0) { - deinitialize_shape(); - return; - } - - bool first = true; - bool moved = false; - for (uint32_t node_index = 0; node_index < nodes_count; ++node_index) { - const Node &node = nodes[node_index]; - if (!prev_bounds.has_point(node.x)) { - moved = true; - } - if (first) { - bounds.position = node.x; - first = false; - } else { - bounds.expand_to(node.x); - } - } - - if (get_space()) { - initialize_shape(moved); - } -} - -void SoftBody3DSW::update_constants() { - reset_link_rest_lengths(); - update_link_constants(); - update_area(); -} - -void SoftBody3DSW::update_area() { - int i, ni; - - // Face area. - for (i = 0, ni = faces.size(); i < ni; ++i) { - Face &face = faces[i]; - - const Vector3 &x0 = face.n[0]->x; - const Vector3 &x1 = face.n[1]->x; - const Vector3 &x2 = face.n[2]->x; - - const Vector3 a = x1 - x0; - const Vector3 b = x2 - x0; - const Vector3 cr = vec3_cross(a, b); - face.ra = cr.length(); - } - - // Node area. - LocalVector counts; - if (nodes.size() > 0) { - counts.resize(nodes.size()); - memset(counts.ptr(), 0, counts.size() * sizeof(int)); - } - - for (i = 0, ni = nodes.size(); i < ni; ++i) { - nodes[i].area = 0.0; - } - - for (i = 0, ni = faces.size(); i < ni; ++i) { - const Face &face = faces[i]; - for (int j = 0; j < 3; ++j) { - const int index = (int)(face.n[j] - &nodes[0]); - counts[index]++; - face.n[j]->area += Math::abs(face.ra); - } - } - - for (i = 0, ni = nodes.size(); i < ni; ++i) { - if (counts[i] > 0) { - nodes[i].area /= (real_t)counts[i]; - } else { - nodes[i].area = 0.0; - } - } -} - -void SoftBody3DSW::reset_link_rest_lengths() { - for (uint32_t i = 0, ni = links.size(); i < ni; ++i) { - Link &link = links[i]; - link.rl = (link.n[0]->x - link.n[1]->x).length(); - link.c1 = link.rl * link.rl; - } -} - -void SoftBody3DSW::update_link_constants() { - real_t inv_linear_stiffness = 1.0 / linear_stiffness; - for (uint32_t i = 0, ni = links.size(); i < ni; ++i) { - Link &link = links[i]; - link.c0 = (link.n[0]->im + link.n[1]->im) * inv_linear_stiffness; - } -} - -void SoftBody3DSW::apply_nodes_transform(const Transform3D &p_transform) { - if (soft_mesh.is_null()) { - return; - } - - uint32_t node_count = nodes.size(); - Vector3 leaf_size = Vector3(collision_margin, collision_margin, collision_margin) * 2.0; - for (uint32_t node_index = 0; node_index < node_count; ++node_index) { - Node &node = nodes[node_index]; - - node.x = p_transform.xform(node.x); - node.q = node.x; - node.v = Vector3(); - node.bv = Vector3(); - - AABB node_aabb(node.x, leaf_size); - node_tree.update(node.leaf, node_aabb); - } - - face_tree.clear(); - - update_normals_and_centroids(); - update_bounds(); - update_constants(); -} - -Vector3 SoftBody3DSW::get_vertex_position(int p_index) const { - ERR_FAIL_COND_V(p_index < 0, Vector3()); - - if (soft_mesh.is_null()) { - return Vector3(); - } - - ERR_FAIL_COND_V(p_index >= (int)map_visual_to_physics.size(), Vector3()); - uint32_t node_index = map_visual_to_physics[p_index]; - - ERR_FAIL_COND_V(node_index >= nodes.size(), Vector3()); - return nodes[node_index].x; -} - -void SoftBody3DSW::set_vertex_position(int p_index, const Vector3 &p_position) { - ERR_FAIL_COND(p_index < 0); - - if (soft_mesh.is_null()) { - return; - } - - ERR_FAIL_COND(p_index >= (int)map_visual_to_physics.size()); - uint32_t node_index = map_visual_to_physics[p_index]; - - ERR_FAIL_COND(node_index >= nodes.size()); - Node &node = nodes[node_index]; - node.q = node.x; - node.x = p_position; -} - -void SoftBody3DSW::pin_vertex(int p_index) { - ERR_FAIL_COND(p_index < 0); - - if (is_vertex_pinned(p_index)) { - return; - } - - pinned_vertices.push_back(p_index); - - if (!soft_mesh.is_null()) { - ERR_FAIL_COND(p_index >= (int)map_visual_to_physics.size()); - uint32_t node_index = map_visual_to_physics[p_index]; - - ERR_FAIL_COND(node_index >= nodes.size()); - Node &node = nodes[node_index]; - node.im = 0.0; - } -} - -void SoftBody3DSW::unpin_vertex(int p_index) { - ERR_FAIL_COND(p_index < 0); - - uint32_t pinned_count = pinned_vertices.size(); - for (uint32_t i = 0; i < pinned_count; ++i) { - if (p_index == pinned_vertices[i]) { - pinned_vertices.remove(i); - - if (!soft_mesh.is_null()) { - ERR_FAIL_COND(p_index >= (int)map_visual_to_physics.size()); - uint32_t node_index = map_visual_to_physics[p_index]; - - ERR_FAIL_COND(node_index >= nodes.size()); - real_t inv_node_mass = nodes.size() * inv_total_mass; - - Node &node = nodes[node_index]; - node.im = inv_node_mass; - } - - return; - } - } -} - -void SoftBody3DSW::unpin_all_vertices() { - if (!soft_mesh.is_null()) { - real_t inv_node_mass = nodes.size() * inv_total_mass; - uint32_t pinned_count = pinned_vertices.size(); - for (uint32_t i = 0; i < pinned_count; ++i) { - int pinned_vertex = pinned_vertices[i]; - - ERR_CONTINUE(pinned_vertex >= (int)map_visual_to_physics.size()); - uint32_t node_index = map_visual_to_physics[pinned_vertex]; - - ERR_CONTINUE(node_index >= nodes.size()); - Node &node = nodes[node_index]; - node.im = inv_node_mass; - } - } - - pinned_vertices.clear(); -} - -bool SoftBody3DSW::is_vertex_pinned(int p_index) const { - ERR_FAIL_COND_V(p_index < 0, false); - - uint32_t pinned_count = pinned_vertices.size(); - for (uint32_t i = 0; i < pinned_count; ++i) { - if (p_index == pinned_vertices[i]) { - return true; - } - } - - return false; -} - -uint32_t SoftBody3DSW::get_node_count() const { - return nodes.size(); -} - -real_t SoftBody3DSW::get_node_inv_mass(uint32_t p_node_index) const { - ERR_FAIL_COND_V(p_node_index >= nodes.size(), 0.0); - return nodes[p_node_index].im; -} - -Vector3 SoftBody3DSW::get_node_position(uint32_t p_node_index) const { - ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); - return nodes[p_node_index].x; -} - -Vector3 SoftBody3DSW::get_node_velocity(uint32_t p_node_index) const { - ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); - return nodes[p_node_index].v; -} - -Vector3 SoftBody3DSW::get_node_biased_velocity(uint32_t p_node_index) const { - ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); - return nodes[p_node_index].bv; -} - -void SoftBody3DSW::apply_node_impulse(uint32_t p_node_index, const Vector3 &p_impulse) { - ERR_FAIL_COND(p_node_index >= nodes.size()); - Node &node = nodes[p_node_index]; - node.v += p_impulse * node.im; -} - -void SoftBody3DSW::apply_node_bias_impulse(uint32_t p_node_index, const Vector3 &p_impulse) { - ERR_FAIL_COND(p_node_index >= nodes.size()); - Node &node = nodes[p_node_index]; - node.bv += p_impulse * node.im; -} - -uint32_t SoftBody3DSW::get_face_count() const { - return faces.size(); -} - -void SoftBody3DSW::get_face_points(uint32_t p_face_index, Vector3 &r_point_1, Vector3 &r_point_2, Vector3 &r_point_3) const { - ERR_FAIL_COND(p_face_index >= faces.size()); - const Face &face = faces[p_face_index]; - r_point_1 = face.n[0]->x; - r_point_2 = face.n[1]->x; - r_point_3 = face.n[2]->x; -} - -Vector3 SoftBody3DSW::get_face_normal(uint32_t p_face_index) const { - ERR_FAIL_COND_V(p_face_index >= faces.size(), Vector3()); - return faces[p_face_index].normal; -} - -bool SoftBody3DSW::create_from_trimesh(const Vector &p_indices, const Vector &p_vertices) { - ERR_FAIL_COND_V(p_indices.is_empty(), false); - ERR_FAIL_COND_V(p_vertices.is_empty(), false); - - uint32_t node_count = 0; - LocalVector vertices; - const int visual_vertex_count(p_vertices.size()); - - LocalVector triangles; - const uint32_t triangle_count(p_indices.size() / 3); - triangles.resize(triangle_count * 3); - - // Merge all overlapping vertices and create a map of physical vertices to visual vertices. - { - // Process vertices. - { - uint32_t vertex_count = 0; - Map unique_vertices; - - vertices.resize(visual_vertex_count); - map_visual_to_physics.resize(visual_vertex_count); - - for (int visual_vertex_index = 0; visual_vertex_index < visual_vertex_count; ++visual_vertex_index) { - const Vector3 &vertex = p_vertices[visual_vertex_index]; - - Map::Element *e = unique_vertices.find(vertex); - uint32_t vertex_id; - if (e) { - // Already existing. - vertex_id = e->value(); - } else { - // Create new one. - vertex_id = vertex_count++; - unique_vertices[vertex] = vertex_id; - vertices[vertex_id] = vertex; - } - - map_visual_to_physics[visual_vertex_index] = vertex_id; - } - - vertices.resize(vertex_count); - } - - // Process triangles. - { - for (uint32_t triangle_index = 0; triangle_index < triangle_count; ++triangle_index) { - for (int i = 0; i < 3; ++i) { - int visual_index = 3 * triangle_index + i; - int physics_index = map_visual_to_physics[p_indices[visual_index]]; - triangles[visual_index] = physics_index; - node_count = MAX((int)node_count, physics_index); - } - } - } - } - - ++node_count; - - // Create nodes from vertices. - nodes.resize(node_count); - real_t inv_node_mass = node_count * inv_total_mass; - Vector3 leaf_size = Vector3(collision_margin, collision_margin, collision_margin) * 2.0; - for (uint32_t i = 0; i < node_count; ++i) { - Node &node = nodes[i]; - node.s = vertices[i]; - node.x = node.s; - node.q = node.s; - node.im = inv_node_mass; - - AABB node_aabb(node.x, leaf_size); - node.leaf = node_tree.insert(node_aabb, &node); - - node.index = i; - } - - // Create links and faces from triangles. - LocalVector chks; - chks.resize(node_count * node_count); - memset(chks.ptr(), 0, chks.size() * sizeof(bool)); - - for (uint32_t i = 0; i < triangle_count * 3; i += 3) { - const int idx[] = { triangles[i], triangles[i + 1], triangles[i + 2] }; - - for (int j = 2, k = 0; k < 3; j = k++) { - int chk = idx[k] * node_count + idx[j]; - if (!chks[chk]) { - chks[chk] = true; - int inv_chk = idx[j] * node_count + idx[k]; - chks[inv_chk] = true; - - append_link(idx[j], idx[k]); - } - } - - append_face(idx[0], idx[1], idx[2]); - } - - // Set pinned nodes. - uint32_t pinned_count = pinned_vertices.size(); - for (uint32_t i = 0; i < pinned_count; ++i) { - int pinned_vertex = pinned_vertices[i]; - - ERR_CONTINUE(pinned_vertex >= visual_vertex_count); - uint32_t node_index = map_visual_to_physics[pinned_vertex]; - - ERR_CONTINUE(node_index >= node_count); - Node &node = nodes[node_index]; - node.im = 0.0; - } - - generate_bending_constraints(2); - reoptimize_link_order(); - - update_constants(); - update_normals_and_centroids(); - update_bounds(); - - return true; -} - -void SoftBody3DSW::generate_bending_constraints(int p_distance) { - uint32_t i, j; - - if (p_distance > 1) { - // Build graph. - const uint32_t n = nodes.size(); - const unsigned inf = (~(unsigned)0) >> 1; - const uint32_t adj_size = n * n; - unsigned *adj = memnew_arr(unsigned, adj_size); - -#define IDX(_x_, _y_) ((_y_)*n + (_x_)) - for (j = 0; j < n; ++j) { - for (i = 0; i < n; ++i) { - int idx_ij = j * n + i; - int idx_ji = i * n + j; - if (i != j) { - adj[idx_ij] = adj[idx_ji] = inf; - } else { - adj[idx_ij] = adj[idx_ji] = 0; - } - } - } - for (i = 0; i < links.size(); ++i) { - const int ia = (int)(links[i].n[0] - &nodes[0]); - const int ib = (int)(links[i].n[1] - &nodes[0]); - int idx = ib * n + ia; - int idx_inv = ia * n + ib; - adj[idx] = 1; - adj[idx_inv] = 1; - } - - // Special optimized case for distance == 2. - if (p_distance == 2) { - LocalVector> node_links; - - // Build node links. - node_links.resize(nodes.size()); - - for (i = 0; i < links.size(); ++i) { - const int ia = (int)(links[i].n[0] - &nodes[0]); - const int ib = (int)(links[i].n[1] - &nodes[0]); - if (node_links[ia].find(ib) == -1) { - node_links[ia].push_back(ib); - } - - if (node_links[ib].find(ia) == -1) { - node_links[ib].push_back(ia); - } - } - for (uint32_t ii = 0; ii < node_links.size(); ii++) { - for (uint32_t jj = 0; jj < node_links[ii].size(); jj++) { - int k = node_links[ii][jj]; - for (uint32_t kk = 0; kk < node_links[k].size(); kk++) { - int l = node_links[k][kk]; - if ((int)ii != l) { - int idx_ik = k * n + ii; - int idx_kj = l * n + k; - const unsigned sum = adj[idx_ik] + adj[idx_kj]; - ERR_FAIL_COND(sum != 2); - int idx_ij = l * n + ii; - if (adj[idx_ij] > sum) { - int idx_ji = l * n + ii; - adj[idx_ij] = adj[idx_ji] = sum; - } - } - } - } - } - } else { - // Generic Floyd's algorithm. - for (uint32_t k = 0; k < n; ++k) { - for (j = 0; j < n; ++j) { - for (i = j + 1; i < n; ++i) { - int idx_ik = k * n + i; - int idx_kj = j * n + k; - const unsigned sum = adj[idx_ik] + adj[idx_kj]; - int idx_ij = j * n + i; - if (adj[idx_ij] > sum) { - int idx_ji = j * n + i; - adj[idx_ij] = adj[idx_ji] = sum; - } - } - } - } - } - - // Build links. - for (j = 0; j < n; ++j) { - for (i = j + 1; i < n; ++i) { - int idx_ij = j * n + i; - if (adj[idx_ij] == (unsigned)p_distance) { - append_link(i, j); - } - } - } - memdelete_arr(adj); - } -} - -//=================================================================== -// -// -// This function takes in a list of interdependent Links and tries -// to maximize the distance between calculation -// of dependent links. This increases the amount of parallelism that can -// be exploited by out-of-order instruction processors with large but -// (inevitably) finite instruction windows. -// -//=================================================================== - -// A small structure to track lists of dependent link calculations. -class LinkDeps { -public: - int value; // A link calculation that is dependent on this one - // Positive values = "input A" while negative values = "input B" - LinkDeps *next; // Next dependence in the list -}; -typedef LinkDeps *LinkDepsPtr; - -void SoftBody3DSW::reoptimize_link_order() { - const int reop_not_dependent = -1; - const int reop_node_complete = -2; - - uint32_t i, link_count = links.size(), node_count = nodes.size(); - Link *lr; - int ar, br; - Node *node0 = &(nodes[0]); - Node *node1 = &(nodes[1]); - LinkDepsPtr link_dep; - int ready_list_head, ready_list_tail, link_num, link_dep_frees, dep_link; - - // Allocate temporary buffers. - int *node_written_at = memnew_arr(int, node_count + 1); // What link calculation produced this node's current values? - int *link_dep_A = memnew_arr(int, link_count); // Link calculation input is dependent upon prior calculation #N - int *link_dep_B = memnew_arr(int, link_count); - int *ready_list = memnew_arr(int, link_count); // List of ready-to-process link calculations (# of links, maximum) - LinkDeps *link_dep_free_list = memnew_arr(LinkDeps, 2 * link_count); // Dependent-on-me list elements (2x# of links, maximum) - LinkDepsPtr *link_dep_list_starts = memnew_arr(LinkDepsPtr, link_count); // Start nodes of dependent-on-me lists, one for each link - - // Copy the original, unsorted links to a side buffer. - Link *link_buffer = memnew_arr(Link, link_count); - memcpy(link_buffer, &(links[0]), sizeof(Link) * link_count); - - // Clear out the node setup and ready list. - for (i = 0; i < node_count + 1; i++) { - node_written_at[i] = reop_not_dependent; - } - for (i = 0; i < link_count; i++) { - link_dep_list_starts[i] = nullptr; - } - ready_list_head = ready_list_tail = link_dep_frees = 0; - - // Initial link analysis to set up data structures. - for (i = 0; i < link_count; i++) { - // Note which prior link calculations we are dependent upon & build up dependence lists. - lr = &(links[i]); - ar = (lr->n[0] - node0) / (node1 - node0); - br = (lr->n[1] - node0) / (node1 - node0); - if (node_written_at[ar] > reop_not_dependent) { - link_dep_A[i] = node_written_at[ar]; - link_dep = &link_dep_free_list[link_dep_frees++]; - link_dep->value = i; - link_dep->next = link_dep_list_starts[node_written_at[ar]]; - link_dep_list_starts[node_written_at[ar]] = link_dep; - } else { - link_dep_A[i] = reop_not_dependent; - } - if (node_written_at[br] > reop_not_dependent) { - link_dep_B[i] = node_written_at[br]; - link_dep = &link_dep_free_list[link_dep_frees++]; - link_dep->value = -(int)(i + 1); - link_dep->next = link_dep_list_starts[node_written_at[br]]; - link_dep_list_starts[node_written_at[br]] = link_dep; - } else { - link_dep_B[i] = reop_not_dependent; - } - - // Add this link to the initial ready list, if it is not dependent on any other links. - if ((link_dep_A[i] == reop_not_dependent) && (link_dep_B[i] == reop_not_dependent)) { - ready_list[ready_list_tail++] = i; - link_dep_A[i] = link_dep_B[i] = reop_node_complete; // Probably not needed now. - } - - // Update the nodes to mark which ones are calculated by this link. - node_written_at[ar] = node_written_at[br] = i; - } - - // Process the ready list and create the sorted list of links: - // -- By treating the ready list as a queue, we maximize the distance between any - // inter-dependent node calculations. - // -- All other (non-related) nodes in the ready list will automatically be inserted - // in between each set of inter-dependent link calculations by this loop. - i = 0; - while (ready_list_head != ready_list_tail) { - // Use ready list to select the next link to process. - link_num = ready_list[ready_list_head++]; - // Copy the next-to-calculate link back into the original link array. - links[i++] = link_buffer[link_num]; - - // Free up any link inputs that are dependent on this one. - link_dep = link_dep_list_starts[link_num]; - while (link_dep) { - dep_link = link_dep->value; - if (dep_link >= 0) { - link_dep_A[dep_link] = reop_not_dependent; - } else { - dep_link = -dep_link - 1; - link_dep_B[dep_link] = reop_not_dependent; - } - // Add this dependent link calculation to the ready list if *both* inputs are clear. - if ((link_dep_A[dep_link] == reop_not_dependent) && (link_dep_B[dep_link] == reop_not_dependent)) { - ready_list[ready_list_tail++] = dep_link; - link_dep_A[dep_link] = link_dep_B[dep_link] = reop_node_complete; // Probably not needed now. - } - link_dep = link_dep->next; - } - } - - // Delete the temporary buffers. - memdelete_arr(node_written_at); - memdelete_arr(link_dep_A); - memdelete_arr(link_dep_B); - memdelete_arr(ready_list); - memdelete_arr(link_dep_free_list); - memdelete_arr(link_dep_list_starts); - memdelete_arr(link_buffer); -} - -void SoftBody3DSW::append_link(uint32_t p_node1, uint32_t p_node2) { - if (p_node1 == p_node2) { - return; - } - - Node *node1 = &nodes[p_node1]; - Node *node2 = &nodes[p_node2]; - - Link link; - link.n[0] = node1; - link.n[1] = node2; - link.rl = (node1->x - node2->x).length(); - - links.push_back(link); -} - -void SoftBody3DSW::append_face(uint32_t p_node1, uint32_t p_node2, uint32_t p_node3) { - if (p_node1 == p_node2) { - return; - } - if (p_node1 == p_node3) { - return; - } - if (p_node2 == p_node3) { - return; - } - - Node *node1 = &nodes[p_node1]; - Node *node2 = &nodes[p_node2]; - Node *node3 = &nodes[p_node3]; - - Face face; - face.n[0] = node1; - face.n[1] = node2; - face.n[2] = node3; - - face.index = faces.size(); - - faces.push_back(face); -} - -void SoftBody3DSW::set_iteration_count(int p_val) { - iteration_count = p_val; -} - -void SoftBody3DSW::set_total_mass(real_t p_val) { - ERR_FAIL_COND(p_val < 0.0); - - inv_total_mass = 1.0 / p_val; - real_t mass_factor = total_mass * inv_total_mass; - total_mass = p_val; - - uint32_t node_count = nodes.size(); - for (uint32_t node_index = 0; node_index < node_count; ++node_index) { - Node &node = nodes[node_index]; - node.im *= mass_factor; - } - - update_constants(); -} - -void SoftBody3DSW::set_collision_margin(real_t p_val) { - collision_margin = p_val; -} - -void SoftBody3DSW::set_linear_stiffness(real_t p_val) { - linear_stiffness = p_val; -} - -void SoftBody3DSW::set_pressure_coefficient(real_t p_val) { - pressure_coefficient = p_val; -} - -void SoftBody3DSW::set_damping_coefficient(real_t p_val) { - damping_coefficient = p_val; -} - -void SoftBody3DSW::set_drag_coefficient(real_t p_val) { - drag_coefficient = p_val; -} - -void SoftBody3DSW::add_velocity(const Vector3 &p_velocity) { - for (uint32_t i = 0, ni = nodes.size(); i < ni; ++i) { - Node &node = nodes[i]; - if (node.im > 0) { - node.v += p_velocity; - } - } -} - -void SoftBody3DSW::apply_forces(bool p_has_wind_forces) { - int ac = areas.size(); - - if (nodes.is_empty()) { - return; - } - - uint32_t i, ni; - int32_t j; - - real_t volume = 0.0; - const Vector3 &org = nodes[0].x; - - // Iterate over faces (try not to iterate elsewhere if possible). - for (i = 0, ni = faces.size(); i < ni; ++i) { - bool stopped = false; - const Face &face = faces[i]; - - Vector3 wind_force(0, 0, 0); - - // Compute volume. - volume += vec3_dot(face.n[0]->x - org, vec3_cross(face.n[1]->x - org, face.n[2]->x - org)); - - // Compute nodal forces from area winds. - if (ac && p_has_wind_forces) { - const AreaCMP *aa = &areas[0]; - for (j = ac - 1; j >= 0 && !stopped; j--) { - PhysicsServer3D::AreaSpaceOverrideMode mode = aa[j].area->get_space_override_mode(); - switch (mode) { - case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: - case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { - wind_force += _compute_area_windforce(aa[j].area, &face); - stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; - } break; - case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: - case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { - wind_force = _compute_area_windforce(aa[j].area, &face); - stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE; - } break; - default: { - } - } - } - - for (j = 0; j < 3; j++) { - Node *current_node = face.n[j]; - current_node->f += wind_force; - } - } - } - volume /= 6.0; - - // Apply nodal pressure forces. - if (pressure_coefficient > CMP_EPSILON) { - real_t ivolumetp = 1.0 / Math::abs(volume) * pressure_coefficient; - for (i = 0, ni = nodes.size(); i < ni; ++i) { - Node &node = nodes[i]; - if (node.im > 0) { - node.f += node.n * (node.area * ivolumetp); - } - } - } -} - -void SoftBody3DSW::_compute_area_gravity(const Area3DSW *p_area) { - Vector3 area_gravity; - p_area->compute_gravity(get_transform().get_origin(), area_gravity); - gravity += area_gravity; -} - -Vector3 SoftBody3DSW::_compute_area_windforce(const Area3DSW *p_area, const Face *p_face) { - real_t wfm = p_area->get_wind_force_magnitude(); - real_t waf = p_area->get_wind_attenuation_factor(); - const Vector3 &wd = p_area->get_wind_direction(); - const Vector3 &ws = p_area->get_wind_source(); - real_t projection_on_tri_normal = vec3_dot(p_face->normal, wd); - real_t projection_toward_centroid = vec3_dot(p_face->centroid - ws, wd); - real_t attenuation_over_distance = pow(projection_toward_centroid, -waf); - real_t nodal_force_magnitude = wfm * 0.33333333333 * p_face->ra * projection_on_tri_normal * attenuation_over_distance; - return nodal_force_magnitude * p_face->normal; -} - -void SoftBody3DSW::predict_motion(real_t p_delta) { - const real_t inv_delta = 1.0 / p_delta; - - ERR_FAIL_COND(!get_space()); - - Area3DSW *def_area = get_space()->get_default_area(); - ERR_FAIL_COND(!def_area); - gravity = def_area->get_gravity_vector() * def_area->get_gravity(); - - int ac = areas.size(); - bool stopped = false; - bool has_wind_forces = false; - - if (ac) { - areas.sort(); - const AreaCMP *aa = &areas[0]; - for (int i = ac - 1; i >= 0 && !stopped; i--) { - // Avoids unnecessary loop in apply_forces(). - has_wind_forces = has_wind_forces || aa[i].area->get_wind_force_magnitude() > CMP_EPSILON; - - PhysicsServer3D::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode(); - switch (mode) { - case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: - case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { - _compute_area_gravity(aa[i].area); - stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; - } break; - case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: - case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { - gravity = Vector3(0, 0, 0); - _compute_area_gravity(aa[i].area); - stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE; - } break; - default: { - } - } - } - } - - // Apply forces. - add_velocity(gravity * p_delta); - if (pressure_coefficient > CMP_EPSILON || has_wind_forces) { - apply_forces(has_wind_forces); - } - - // Avoid soft body from 'exploding' so use some upper threshold of maximum motion - // that a node can travel per frame. - const real_t max_displacement = 1000.0; - real_t clamp_delta_v = max_displacement * inv_delta; - - // Integrate. - uint32_t i, ni; - for (i = 0, ni = nodes.size(); i < ni; ++i) { - Node &node = nodes[i]; - node.q = node.x; - Vector3 delta_v = node.f * node.im * p_delta; - for (int c = 0; c < 3; c++) { - delta_v[c] = CLAMP(delta_v[c], -clamp_delta_v, clamp_delta_v); - } - node.v += delta_v; - node.x += node.v * p_delta; - node.f = Vector3(); - } - - // Bounds and tree update. - update_bounds(); - - // Node tree update. - for (i = 0, ni = nodes.size(); i < ni; ++i) { - const Node &node = nodes[i]; - - AABB node_aabb(node.x, Vector3()); - node_aabb.expand_to(node.x + node.v * p_delta); - node_aabb.grow_by(collision_margin); - - node_tree.update(node.leaf, node_aabb); - } - - // Face tree update. - if (!face_tree.is_empty()) { - update_face_tree(p_delta); - } - - // Optimize node tree. - node_tree.optimize_incremental(1); - face_tree.optimize_incremental(1); -} - -void SoftBody3DSW::solve_constraints(real_t p_delta) { - const real_t inv_delta = 1.0 / p_delta; - - uint32_t i, ni; - - for (i = 0, ni = links.size(); i < ni; ++i) { - Link &link = links[i]; - link.c3 = link.n[1]->q - link.n[0]->q; - link.c2 = 1 / (link.c3.length_squared() * link.c0); - } - - // Solve velocities. - for (i = 0, ni = nodes.size(); i < ni; ++i) { - Node &node = nodes[i]; - node.x = node.q + node.v * p_delta; - } - - // Solve positions. - for (int isolve = 0; isolve < iteration_count; ++isolve) { - const real_t ti = isolve / (real_t)iteration_count; - solve_links(1.0, ti); - } - const real_t vc = (1.0 - damping_coefficient) * inv_delta; - for (i = 0, ni = nodes.size(); i < ni; ++i) { - Node &node = nodes[i]; - - node.x += node.bv * p_delta; - node.bv = Vector3(); - - node.v = (node.x - node.q) * vc; - - node.q = node.x; - } - - update_normals_and_centroids(); -} - -void SoftBody3DSW::solve_links(real_t kst, real_t ti) { - for (uint32_t i = 0, ni = links.size(); i < ni; ++i) { - Link &link = links[i]; - if (link.c0 > 0) { - Node &node_a = *link.n[0]; - Node &node_b = *link.n[1]; - const Vector3 del = node_b.x - node_a.x; - const real_t len = del.length_squared(); - if (link.c1 + len > CMP_EPSILON) { - const real_t k = ((link.c1 - len) / (link.c0 * (link.c1 + len))) * kst; - node_a.x -= del * (k * node_a.im); - node_b.x += del * (k * node_b.im); - } - } - } -} - -struct AABBQueryResult { - const SoftBody3DSW *soft_body = nullptr; - void *userdata = nullptr; - SoftBody3DSW::QueryResultCallback result_callback = nullptr; - - _FORCE_INLINE_ bool operator()(void *p_data) { - return result_callback(soft_body->get_node_index(p_data), userdata); - }; -}; - -void SoftBody3DSW::query_aabb(const AABB &p_aabb, SoftBody3DSW::QueryResultCallback p_result_callback, void *p_userdata) { - AABBQueryResult query_result; - query_result.soft_body = this; - query_result.result_callback = p_result_callback; - query_result.userdata = p_userdata; - - node_tree.aabb_query(p_aabb, query_result); -} - -struct RayQueryResult { - const SoftBody3DSW *soft_body = nullptr; - void *userdata = nullptr; - SoftBody3DSW::QueryResultCallback result_callback = nullptr; - - _FORCE_INLINE_ bool operator()(void *p_data) { - return result_callback(soft_body->get_face_index(p_data), userdata); - }; -}; - -void SoftBody3DSW::query_ray(const Vector3 &p_from, const Vector3 &p_to, SoftBody3DSW::QueryResultCallback p_result_callback, void *p_userdata) { - if (face_tree.is_empty()) { - initialize_face_tree(); - } - - RayQueryResult query_result; - query_result.soft_body = this; - query_result.result_callback = p_result_callback; - query_result.userdata = p_userdata; - - face_tree.ray_query(p_from, p_to, query_result); -} - -void SoftBody3DSW::initialize_face_tree() { - face_tree.clear(); - for (uint32_t i = 0; i < faces.size(); ++i) { - Face &face = faces[i]; - - AABB face_aabb; - - face_aabb.position = face.n[0]->x; - face_aabb.expand_to(face.n[1]->x); - face_aabb.expand_to(face.n[2]->x); - - face_aabb.grow_by(collision_margin); - - face.leaf = face_tree.insert(face_aabb, &face); - } -} - -void SoftBody3DSW::update_face_tree(real_t p_delta) { - for (uint32_t i = 0; i < faces.size(); ++i) { - const Face &face = faces[i]; - - AABB face_aabb; - - const Node *node0 = face.n[0]; - face_aabb.position = node0->x; - face_aabb.expand_to(node0->x + node0->v * p_delta); - - const Node *node1 = face.n[1]; - face_aabb.expand_to(node1->x); - face_aabb.expand_to(node1->x + node1->v * p_delta); - - const Node *node2 = face.n[2]; - face_aabb.expand_to(node2->x); - face_aabb.expand_to(node2->x + node2->v * p_delta); - - face_aabb.grow_by(collision_margin); - - face_tree.update(face.leaf, face_aabb); - } -} - -void SoftBody3DSW::initialize_shape(bool p_force_move) { - if (get_shape_count() == 0) { - SoftBodyShape3DSW *soft_body_shape = memnew(SoftBodyShape3DSW(this)); - add_shape(soft_body_shape); - } else if (p_force_move) { - SoftBodyShape3DSW *soft_body_shape = static_cast(get_shape(0)); - soft_body_shape->update_bounds(); - } -} - -void SoftBody3DSW::deinitialize_shape() { - if (get_shape_count() > 0) { - Shape3DSW *shape = get_shape(0); - remove_shape(shape); - memdelete(shape); - } -} - -void SoftBody3DSW::destroy() { - soft_mesh = RID(); - - map_visual_to_physics.clear(); - - node_tree.clear(); - face_tree.clear(); - - nodes.clear(); - links.clear(); - faces.clear(); - - bounds = AABB(); - deinitialize_shape(); -} - -void SoftBodyShape3DSW::update_bounds() { - ERR_FAIL_COND(!soft_body); - - AABB collision_aabb = soft_body->get_bounds(); - collision_aabb.grow_by(soft_body->get_collision_margin()); - configure(collision_aabb); -} - -SoftBodyShape3DSW::SoftBodyShape3DSW(SoftBody3DSW *p_soft_body) { - soft_body = p_soft_body; - update_bounds(); -} - -struct _SoftBodyIntersectSegmentInfo { - const SoftBody3DSW *soft_body = nullptr; - Vector3 from; - Vector3 dir; - Vector3 hit_position; - uint32_t hit_face_index = -1; - real_t hit_dist_sq = INFINITY; - - static bool process_hit(uint32_t p_face_index, void *p_userdata) { - _SoftBodyIntersectSegmentInfo &query_info = *(_SoftBodyIntersectSegmentInfo *)(p_userdata); - - Vector3 points[3]; - query_info.soft_body->get_face_points(p_face_index, points[0], points[1], points[2]); - - Vector3 result; - if (Geometry3D::ray_intersects_triangle(query_info.from, query_info.dir, points[0], points[1], points[2], &result)) { - real_t dist_sq = query_info.from.distance_squared_to(result); - if (dist_sq < query_info.hit_dist_sq) { - query_info.hit_dist_sq = dist_sq; - query_info.hit_position = result; - query_info.hit_face_index = p_face_index; - } - } - - // Continue with the query. - return false; - } -}; - -bool SoftBodyShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - _SoftBodyIntersectSegmentInfo query_info; - query_info.soft_body = soft_body; - query_info.from = p_begin; - query_info.dir = (p_end - p_begin).normalized(); - - soft_body->query_ray(p_begin, p_end, _SoftBodyIntersectSegmentInfo::process_hit, &query_info); - - if (query_info.hit_dist_sq != INFINITY) { - r_result = query_info.hit_position; - r_normal = soft_body->get_face_normal(query_info.hit_face_index); - return true; - } - - return false; -} - -bool SoftBodyShape3DSW::intersect_point(const Vector3 &p_point) const { - return false; -} - -Vector3 SoftBodyShape3DSW::get_closest_point_to(const Vector3 &p_point) const { - ERR_FAIL_V_MSG(Vector3(), "Get closest point is not supported for soft bodies."); -} diff --git a/servers/physics_3d/soft_body_3d_sw.h b/servers/physics_3d/soft_body_3d_sw.h deleted file mode 100644 index 7d4b83d0ee..0000000000 --- a/servers/physics_3d/soft_body_3d_sw.h +++ /dev/null @@ -1,279 +0,0 @@ -/*************************************************************************/ -/* soft_body_3d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 SOFT_BODY_3D_SW_H -#define SOFT_BODY_3D_SW_H - -#include "area_3d_sw.h" -#include "collision_object_3d_sw.h" - -#include "core/math/aabb.h" -#include "core/math/dynamic_bvh.h" -#include "core/math/vector3.h" -#include "core/templates/local_vector.h" -#include "core/templates/set.h" -#include "core/templates/vset.h" - -class Constraint3DSW; - -class SoftBody3DSW : public CollisionObject3DSW { - RID soft_mesh; - - struct Node { - Vector3 s; // Source position - Vector3 x; // Position - Vector3 q; // Previous step position/Test position - Vector3 f; // Force accumulator - Vector3 v; // Velocity - Vector3 bv; // Biased Velocity - Vector3 n; // Normal - real_t area = 0.0; // Area - real_t im = 0.0; // 1/mass - DynamicBVH::ID leaf; // Leaf data - uint32_t index = 0; - }; - - struct Link { - Vector3 c3; // gradient - Node *n[2] = { nullptr, nullptr }; // Node pointers - real_t rl = 0.0; // Rest length - real_t c0 = 0.0; // (ima+imb)*kLST - real_t c1 = 0.0; // rl^2 - real_t c2 = 0.0; // |gradient|^2/c0 - }; - - struct Face { - Vector3 centroid; - Node *n[3] = { nullptr, nullptr, nullptr }; // Node pointers - Vector3 normal; // Normal - real_t ra = 0.0; // Rest area - DynamicBVH::ID leaf; // Leaf data - uint32_t index = 0; - }; - - LocalVector nodes; - LocalVector links; - LocalVector faces; - - DynamicBVH node_tree; - DynamicBVH face_tree; - - LocalVector map_visual_to_physics; - - AABB bounds; - - real_t collision_margin = 0.05; - - real_t total_mass = 1.0; - real_t inv_total_mass = 1.0; - - int iteration_count = 5; - real_t linear_stiffness = 0.5; // [0,1] - real_t pressure_coefficient = 0.0; // [-inf,+inf] - real_t damping_coefficient = 0.01; // [0,1] - real_t drag_coefficient = 0.0; // [0,1] - LocalVector pinned_vertices; - - Vector3 gravity; - - SelfList active_list; - - Set constraints; - - Vector areas; - - VSet exceptions; - - uint64_t island_step = 0; - - _FORCE_INLINE_ void _compute_area_gravity(const Area3DSW *p_area); - _FORCE_INLINE_ Vector3 _compute_area_windforce(const Area3DSW *p_area, const Face *p_face); - -public: - SoftBody3DSW(); - - const AABB &get_bounds() const { return bounds; } - - void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant); - Variant get_state(PhysicsServer3D::BodyState p_state) const; - - _FORCE_INLINE_ void add_constraint(Constraint3DSW *p_constraint) { constraints.insert(p_constraint); } - _FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraints.erase(p_constraint); } - _FORCE_INLINE_ const Set &get_constraints() const { return constraints; } - _FORCE_INLINE_ void clear_constraints() { constraints.clear(); } - - _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); } - _FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); } - _FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); } - _FORCE_INLINE_ const VSet &get_exceptions() const { return exceptions; } - - _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } - _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } - - _FORCE_INLINE_ void add_area(Area3DSW *p_area) { - int index = areas.find(AreaCMP(p_area)); - if (index > -1) { - areas.write[index].refCount += 1; - } else { - areas.ordered_insert(AreaCMP(p_area)); - } - } - - _FORCE_INLINE_ void remove_area(Area3DSW *p_area) { - int index = areas.find(AreaCMP(p_area)); - if (index > -1) { - areas.write[index].refCount -= 1; - if (areas[index].refCount < 1) { - areas.remove(index); - } - } - } - - virtual void set_space(Space3DSW *p_space); - - void set_mesh(RID p_mesh); - - void update_rendering_server(RenderingServerHandler *p_rendering_server_handler); - - Vector3 get_vertex_position(int p_index) const; - void set_vertex_position(int p_index, const Vector3 &p_position); - - void pin_vertex(int p_index); - void unpin_vertex(int p_index); - void unpin_all_vertices(); - bool is_vertex_pinned(int p_index) const; - - uint32_t get_node_count() const; - real_t get_node_inv_mass(uint32_t p_node_index) const; - Vector3 get_node_position(uint32_t p_node_index) const; - Vector3 get_node_velocity(uint32_t p_node_index) const; - Vector3 get_node_biased_velocity(uint32_t p_node_index) const; - void apply_node_impulse(uint32_t p_node_index, const Vector3 &p_impulse); - void apply_node_bias_impulse(uint32_t p_node_index, const Vector3 &p_impulse); - - uint32_t get_face_count() const; - void get_face_points(uint32_t p_face_index, Vector3 &r_point_1, Vector3 &r_point_2, Vector3 &r_point_3) const; - Vector3 get_face_normal(uint32_t p_face_index) const; - - void set_iteration_count(int p_val); - _FORCE_INLINE_ real_t get_iteration_count() const { return iteration_count; } - - void set_total_mass(real_t p_val); - _FORCE_INLINE_ real_t get_total_mass() const { return total_mass; } - _FORCE_INLINE_ real_t get_total_inv_mass() const { return inv_total_mass; } - - void set_collision_margin(real_t p_val); - _FORCE_INLINE_ real_t get_collision_margin() const { return collision_margin; } - - void set_linear_stiffness(real_t p_val); - _FORCE_INLINE_ real_t get_linear_stiffness() const { return linear_stiffness; } - - void set_pressure_coefficient(real_t p_val); - _FORCE_INLINE_ real_t get_pressure_coefficient() const { return pressure_coefficient; } - - void set_damping_coefficient(real_t p_val); - _FORCE_INLINE_ real_t get_damping_coefficient() const { return damping_coefficient; } - - void set_drag_coefficient(real_t p_val); - _FORCE_INLINE_ real_t get_drag_coefficient() const { return drag_coefficient; } - - void predict_motion(real_t p_delta); - void solve_constraints(real_t p_delta); - - _FORCE_INLINE_ uint32_t get_node_index(void *p_node) const { return ((Node *)p_node)->index; } - _FORCE_INLINE_ uint32_t get_face_index(void *p_face) const { return ((Face *)p_face)->index; } - - // Return true to stop the query. - // p_index is the node index for AABB query, face index for Ray query. - typedef bool (*QueryResultCallback)(uint32_t p_index, void *p_userdata); - - void query_aabb(const AABB &p_aabb, QueryResultCallback p_result_callback, void *p_userdata); - void query_ray(const Vector3 &p_from, const Vector3 &p_to, QueryResultCallback p_result_callback, void *p_userdata); - -protected: - virtual void _shapes_changed(); - -private: - void update_normals_and_centroids(); - void update_bounds(); - void update_constants(); - void update_area(); - void reset_link_rest_lengths(); - void update_link_constants(); - - void apply_nodes_transform(const Transform3D &p_transform); - - void add_velocity(const Vector3 &p_velocity); - - void apply_forces(bool p_has_wind_forces); - - bool create_from_trimesh(const Vector &p_indices, const Vector &p_vertices); - void generate_bending_constraints(int p_distance); - void reoptimize_link_order(); - void append_link(uint32_t p_node1, uint32_t p_node2); - void append_face(uint32_t p_node1, uint32_t p_node2, uint32_t p_node3); - - void solve_links(real_t kst, real_t ti); - - void initialize_face_tree(); - void update_face_tree(real_t p_delta); - - void initialize_shape(bool p_force_move = true); - void deinitialize_shape(); - - void destroy(); -}; - -class SoftBodyShape3DSW : public Shape3DSW { - SoftBody3DSW *soft_body = nullptr; - -public: - SoftBody3DSW *get_soft_body() const { return soft_body; } - - virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_SOFT_BODY; } - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { r_min = r_max = 0.0; } - virtual Vector3 get_support(const Vector3 &p_normal) const { return Vector3(); } - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; } - - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; - virtual Vector3 get_moment_of_inertia(real_t p_mass) const { return Vector3(); } - - virtual void set_data(const Variant &p_data) {} - virtual Variant get_data() const { return Variant(); } - - void update_bounds(); - - SoftBodyShape3DSW(SoftBody3DSW *p_soft_body); - ~SoftBodyShape3DSW() {} -}; - -#endif // SOFT_BODY_3D_SW_H diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp deleted file mode 100644 index c88747c017..0000000000 --- a/servers/physics_3d/space_3d_sw.cpp +++ /dev/null @@ -1,1234 +0,0 @@ -/*************************************************************************/ -/* space_3d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "space_3d_sw.h" - -#include "collision_solver_3d_sw.h" -#include "core/config/project_settings.h" -#include "physics_server_3d_sw.h" - -#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05 - -_FORCE_INLINE_ static bool _can_collide_with(CollisionObject3DSW *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - if (!(p_object->get_collision_layer() & p_collision_mask)) { - return false; - } - - if (p_object->get_type() == CollisionObject3DSW::TYPE_AREA && !p_collide_with_areas) { - return false; - } - - if (p_object->get_type() == CollisionObject3DSW::TYPE_BODY && !p_collide_with_bodies) { - return false; - } - - if (p_object->get_type() == CollisionObject3DSW::TYPE_SOFT_BODY && !p_collide_with_bodies) { - return false; - } - - return true; -} - -int PhysicsDirectSpaceState3DSW::intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - ERR_FAIL_COND_V(space->locked, false); - int amount = space->broadphase->cull_point(p_point, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - int cc = 0; - - //Transform3D ai = p_xform.affine_inverse(); - - for (int i = 0; i < amount; i++) { - if (cc >= p_result_max) { - break; - } - - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { - continue; - } - - //area can't be picked by ray (default) - - if (p_exclude.has(space->intersection_query_results[i]->get_self())) { - continue; - } - - const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; - int shape_idx = space->intersection_query_subindex_results[i]; - - Transform3D inv_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - inv_xform.affine_invert(); - - if (!col_obj->get_shape(shape_idx)->intersect_point(inv_xform.xform(p_point))) { - continue; - } - - r_results[cc].collider_id = col_obj->get_instance_id(); - if (r_results[cc].collider_id.is_valid()) { - r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id); - } else { - r_results[cc].collider = nullptr; - } - r_results[cc].rid = col_obj->get_self(); - r_results[cc].shape = shape_idx; - - cc++; - } - - return cc; -} - -bool PhysicsDirectSpaceState3DSW::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_ray) { - ERR_FAIL_COND_V(space->locked, false); - - Vector3 begin, end; - Vector3 normal; - begin = p_from; - end = p_to; - normal = (end - begin).normalized(); - - int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - //todo, create another array that references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision - - bool collided = false; - Vector3 res_point, res_normal; - int res_shape; - const CollisionObject3DSW *res_obj; - real_t min_d = 1e10; - - for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { - continue; - } - - if (p_pick_ray && !(space->intersection_query_results[i]->is_ray_pickable())) { - continue; - } - - if (p_exclude.has(space->intersection_query_results[i]->get_self())) { - continue; - } - - const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; - - int shape_idx = space->intersection_query_subindex_results[i]; - Transform3D inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform(); - - Vector3 local_from = inv_xform.xform(begin); - Vector3 local_to = inv_xform.xform(end); - - const Shape3DSW *shape = col_obj->get_shape(shape_idx); - - Vector3 shape_point, shape_normal; - - if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) { - Transform3D xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - shape_point = xform.xform(shape_point); - - real_t ld = normal.dot(shape_point); - - if (ld < min_d) { - min_d = ld; - res_point = shape_point; - res_normal = inv_xform.basis.xform_inv(shape_normal).normalized(); - res_shape = shape_idx; - res_obj = col_obj; - collided = true; - } - } - } - - if (!collided) { - return false; - } - - r_result.collider_id = res_obj->get_instance_id(); - if (r_result.collider_id.is_valid()) { - r_result.collider = ObjectDB::get_instance(r_result.collider_id); - } else { - r_result.collider = nullptr; - } - r_result.normal = res_normal; - r_result.position = res_point; - r_result.rid = res_obj->get_self(); - r_result.shape = res_shape; - - return true; -} - -int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Transform3D &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - if (p_result_max <= 0) { - return 0; - } - - Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.get_or_null(p_shape); - ERR_FAIL_COND_V(!shape, 0); - - AABB aabb = p_xform.xform(shape->get_aabb()); - - int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - int cc = 0; - - //Transform3D ai = p_xform.affine_inverse(); - - for (int i = 0; i < amount; i++) { - if (cc >= p_result_max) { - break; - } - - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { - continue; - } - - //area can't be picked by ray (default) - - if (p_exclude.has(space->intersection_query_results[i]->get_self())) { - continue; - } - - const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; - int shape_idx = space->intersection_query_subindex_results[i]; - - if (!CollisionSolver3DSW::solve_static(shape, p_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), nullptr, nullptr, nullptr, p_margin, 0)) { - continue; - } - - if (r_results) { - r_results[cc].collider_id = col_obj->get_instance_id(); - if (r_results[cc].collider_id.is_valid()) { - r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id); - } else { - r_results[cc].collider = nullptr; - } - r_results[cc].rid = col_obj->get_self(); - r_results[cc].shape = shape_idx; - } - - cc++; - } - - return cc; -} - -bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transform3D &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, ShapeRestInfo *r_info) { - Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.get_or_null(p_shape); - ERR_FAIL_COND_V(!shape, false); - - AABB aabb = p_xform.xform(shape->get_aabb()); - aabb = aabb.merge(AABB(aabb.position + p_motion, aabb.size)); //motion - aabb = aabb.grow(p_margin); - - int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - real_t best_safe = 1; - real_t best_unsafe = 1; - - Transform3D xform_inv = p_xform.affine_inverse(); - MotionShape3DSW mshape; - mshape.shape = shape; - mshape.motion = xform_inv.basis.xform(p_motion); - - bool best_first = true; - - Vector3 motion_normal = p_motion.normalized(); - - Vector3 closest_A, closest_B; - - for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { - continue; - } - - if (p_exclude.has(space->intersection_query_results[i]->get_self())) { - continue; //ignore excluded - } - - const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; - int shape_idx = space->intersection_query_subindex_results[i]; - - Vector3 point_A, point_B; - Vector3 sep_axis = motion_normal; - - Transform3D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - //test initial overlap, does it collide if going all the way? - if (CollisionSolver3DSW::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { - continue; - } - - //test initial overlap, ignore objects it's inside of. - sep_axis = motion_normal; - - if (!CollisionSolver3DSW::solve_distance(shape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { - continue; - } - - //just do kinematic solving - real_t low = 0.0; - real_t hi = 1.0; - real_t fraction_coeff = 0.5; - for (int j = 0; j < 8; j++) { //steps should be customizable.. - real_t fraction = low + (hi - low) * fraction_coeff; - - mshape.motion = xform_inv.basis.xform(p_motion * fraction); - - Vector3 lA, lB; - Vector3 sep = motion_normal; //important optimization for this to work fast enough - bool collided = !CollisionSolver3DSW::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, aabb, &sep); - - if (collided) { - hi = fraction; - if ((j == 0) || (low > 0.0)) { // Did it not collide before? - // When alternating or first iteration, use dichotomy. - fraction_coeff = 0.5; - } else { - // When colliding again, converge faster towards low fraction - // for more accurate results with long motions that collide near the start. - fraction_coeff = 0.25; - } - } else { - point_A = lA; - point_B = lB; - low = fraction; - if ((j == 0) || (hi < 1.0)) { // Did it collide before? - // When alternating or first iteration, use dichotomy. - fraction_coeff = 0.5; - } else { - // When not colliding again, converge faster towards high fraction - // for more accurate results with long motions that collide near the end. - fraction_coeff = 0.75; - } - } - } - - if (low < best_safe) { - best_first = true; //force reset - best_safe = low; - best_unsafe = hi; - } - - if (r_info && (best_first || (point_A.distance_squared_to(point_B) < closest_A.distance_squared_to(closest_B) && low <= best_safe))) { - closest_A = point_A; - closest_B = point_B; - r_info->collider_id = col_obj->get_instance_id(); - r_info->rid = col_obj->get_self(); - r_info->shape = shape_idx; - r_info->point = closest_B; - r_info->normal = (closest_A - closest_B).normalized(); - best_first = false; - if (col_obj->get_type() == CollisionObject3DSW::TYPE_BODY) { - const Body3DSW *body = static_cast(col_obj); - Vector3 rel_vec = closest_B - (body->get_transform().origin + body->get_center_of_mass()); - r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec); - } - } - } - - p_closest_safe = best_safe; - p_closest_unsafe = best_unsafe; - - return true; -} - -bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - if (p_result_max <= 0) { - return false; - } - - Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.get_or_null(p_shape); - ERR_FAIL_COND_V(!shape, 0); - - AABB aabb = p_shape_xform.xform(shape->get_aabb()); - aabb = aabb.grow(p_margin); - - int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - bool collided = false; - r_result_count = 0; - - PhysicsServer3DSW::CollCbkData cbk; - cbk.max = p_result_max; - cbk.amount = 0; - cbk.ptr = r_results; - CollisionSolver3DSW::CallbackResult cbkres = PhysicsServer3DSW::_shape_col_cbk; - - PhysicsServer3DSW::CollCbkData *cbkptr = &cbk; - - for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { - continue; - } - - const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; - - if (p_exclude.has(col_obj->get_self())) { - continue; - } - - int shape_idx = space->intersection_query_subindex_results[i]; - - if (CollisionSolver3DSW::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_margin)) { - collided = true; - } - } - - r_result_count = cbk.amount; - - return collided; -} - -struct _RestResultData { - const CollisionObject3DSW *object = nullptr; - int local_shape = 0; - int shape = 0; - Vector3 contact; - Vector3 normal; - real_t len = 0.0; -}; - -struct _RestCallbackData { - const CollisionObject3DSW *object = nullptr; - int local_shape = 0; - int shape = 0; - - real_t min_allowed_depth = 0.0; - - _RestResultData best_result; - - int max_results = 0; - int result_count = 0; - _RestResultData *other_results = nullptr; -}; - -static void _rest_cbk_result(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { - _RestCallbackData *rd = (_RestCallbackData *)p_userdata; - - Vector3 contact_rel = p_point_B - p_point_A; - real_t len = contact_rel.length(); - if (len < rd->min_allowed_depth) { - return; - } - - bool is_best_result = (len > rd->best_result.len); - - if (rd->other_results && rd->result_count > 0) { - // Consider as new result by default. - int prev_result_count = rd->result_count++; - - int result_index = 0; - real_t tested_len = is_best_result ? rd->best_result.len : len; - for (; result_index < prev_result_count - 1; ++result_index) { - if (tested_len > rd->other_results[result_index].len) { - // Re-using a previous result. - rd->result_count--; - break; - } - } - - if (result_index < rd->max_results - 1) { - _RestResultData &result = rd->other_results[result_index]; - - if (is_best_result) { - // Keep the previous best result as separate result. - result = rd->best_result; - } else { - // Keep this result as separate result. - result.len = len; - result.contact = p_point_B; - result.normal = contact_rel / len; - result.object = rd->object; - result.shape = rd->shape; - result.local_shape = rd->local_shape; - } - } else { - // Discarding this result. - rd->result_count--; - } - } else if (is_best_result) { - rd->result_count = 1; - } - - if (!is_best_result) { - return; - } - - rd->best_result.len = len; - rd->best_result.contact = p_point_B; - rd->best_result.normal = contact_rel / len; - rd->best_result.object = rd->object; - rd->best_result.shape = rd->shape; - rd->best_result.local_shape = rd->local_shape; -} - -bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.get_or_null(p_shape); - ERR_FAIL_COND_V(!shape, 0); - - real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; - - AABB aabb = p_shape_xform.xform(shape->get_aabb()); - aabb = aabb.grow(p_margin); - - int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - _RestCallbackData rcd; - rcd.min_allowed_depth = min_contact_depth; - - for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { - continue; - } - - const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; - - if (p_exclude.has(col_obj->get_self())) { - continue; - } - - int shape_idx = space->intersection_query_subindex_results[i]; - - rcd.object = col_obj; - rcd.shape = shape_idx; - bool sc = CollisionSolver3DSW::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin); - if (!sc) { - continue; - } - } - - if (rcd.best_result.len == 0 || !rcd.best_result.object) { - return false; - } - - r_info->collider_id = rcd.best_result.object->get_instance_id(); - r_info->shape = rcd.best_result.shape; - r_info->normal = rcd.best_result.normal; - r_info->point = rcd.best_result.contact; - r_info->rid = rcd.best_result.object->get_self(); - if (rcd.best_result.object->get_type() == CollisionObject3DSW::TYPE_BODY) { - const Body3DSW *body = static_cast(rcd.best_result.object); - Vector3 rel_vec = rcd.best_result.contact - (body->get_transform().origin + body->get_center_of_mass()); - r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec); - - } else { - r_info->linear_velocity = Vector3(); - } - - return true; -} - -Vector3 PhysicsDirectSpaceState3DSW::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const { - CollisionObject3DSW *obj = PhysicsServer3DSW::singletonsw->area_owner.get_or_null(p_object); - if (!obj) { - obj = PhysicsServer3DSW::singletonsw->body_owner.get_or_null(p_object); - } - ERR_FAIL_COND_V(!obj, Vector3()); - - ERR_FAIL_COND_V(obj->get_space() != space, Vector3()); - - real_t min_distance = 1e20; - Vector3 min_point; - - bool shapes_found = false; - - for (int i = 0; i < obj->get_shape_count(); i++) { - if (obj->is_shape_disabled(i)) { - continue; - } - - Transform3D shape_xform = obj->get_transform() * obj->get_shape_transform(i); - Shape3DSW *shape = obj->get_shape(i); - - Vector3 point = shape->get_closest_point_to(shape_xform.affine_inverse().xform(p_point)); - point = shape_xform.xform(point); - - real_t dist = point.distance_to(p_point); - if (dist < min_distance) { - min_distance = dist; - min_point = point; - } - shapes_found = true; - } - - if (!shapes_found) { - return obj->get_transform().origin; //no shapes found, use distance to origin. - } else { - return min_point; - } -} - -PhysicsDirectSpaceState3DSW::PhysicsDirectSpaceState3DSW() { - space = nullptr; -} - -//////////////////////////////////////////////////////////////////////////////////////////////////////////// - -int Space3DSW::_cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb) { - int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results); - - for (int i = 0; i < amount; i++) { - bool keep = true; - - if (intersection_query_results[i] == p_body) { - keep = false; - } else if (intersection_query_results[i]->get_type() == CollisionObject3DSW::TYPE_AREA) { - keep = false; - } else if (intersection_query_results[i]->get_type() == CollisionObject3DSW::TYPE_SOFT_BODY) { - keep = false; - } else if (!p_body->collides_with(static_cast(intersection_query_results[i]))) { - keep = false; - } else if (static_cast(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) { - keep = false; - } - - if (!keep) { - if (i < amount - 1) { - SWAP(intersection_query_results[i], intersection_query_results[amount - 1]); - SWAP(intersection_query_subindex_results[i], intersection_query_subindex_results[amount - 1]); - } - - amount--; - i--; - } - } - - return amount; -} - -bool Space3DSW::test_body_motion(Body3DSW *p_body, const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult *r_result) { - //give me back regular physics engine logic - //this is madness - //and most people using this function will think - //what it does is simpler than using physics - //this took about a week to get right.. - //but is it right? who knows at this point.. - - ERR_FAIL_INDEX_V(p_parameters.max_collisions, PhysicsServer3D::MotionResult::MAX_COLLISIONS, false); - - if (r_result) { - *r_result = PhysicsServer3D::MotionResult(); - } - - AABB body_aabb; - bool shapes_found = false; - - for (int i = 0; i < p_body->get_shape_count(); i++) { - if (p_body->is_shape_disabled(i)) { - continue; - } - - if (!shapes_found) { - body_aabb = p_body->get_shape_aabb(i); - shapes_found = true; - } else { - body_aabb = body_aabb.merge(p_body->get_shape_aabb(i)); - } - } - - if (!shapes_found) { - if (r_result) { - r_result->travel = p_parameters.motion; - } - - return false; - } - - // Undo the currently transform the physics server is aware of and apply the provided one - body_aabb = p_parameters.from.xform(p_body->get_inv_transform().xform(body_aabb)); - body_aabb = body_aabb.grow(p_parameters.margin); - - real_t min_contact_depth = p_parameters.margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; - - real_t motion_length = p_parameters.motion.length(); - Vector3 motion_normal = p_parameters.motion / motion_length; - - Transform3D body_transform = p_parameters.from; - - bool recovered = false; - - { - //STEP 1, FREE BODY IF STUCK - - const int max_results = 32; - int recover_attempts = 4; - Vector3 sr[max_results * 2]; - - do { - PhysicsServer3DSW::CollCbkData cbk; - cbk.max = max_results; - cbk.amount = 0; - cbk.ptr = sr; - - PhysicsServer3DSW::CollCbkData *cbkptr = &cbk; - CollisionSolver3DSW::CallbackResult cbkres = PhysicsServer3DSW::_shape_col_cbk; - - bool collided = false; - - int amount = _cull_aabb_for_body(p_body, body_aabb); - - for (int j = 0; j < p_body->get_shape_count(); j++) { - if (p_body->is_shape_disabled(j)) { - continue; - } - - Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j); - Shape3DSW *body_shape = p_body->get_shape(j); - - for (int i = 0; i < amount; i++) { - const CollisionObject3DSW *col_obj = intersection_query_results[i]; - if (p_parameters.exclude_bodies.has(col_obj->get_self())) { - continue; - } - if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { - continue; - } - - int shape_idx = intersection_query_subindex_results[i]; - - if (CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_parameters.margin)) { - collided = cbk.amount > 0; - } - } - } - - if (!collided) { - break; - } - - recovered = true; - - Vector3 recover_motion; - for (int i = 0; i < cbk.amount; i++) { - Vector3 a = sr[i * 2 + 0]; - Vector3 b = sr[i * 2 + 1]; - - // Compute plane on b towards a. - Vector3 n = (a - b).normalized(); - real_t d = n.dot(b); - - // Compute depth on recovered motion. - real_t depth = n.dot(a + recover_motion) - d; - if (depth > min_contact_depth + CMP_EPSILON) { - // Only recover if there is penetration. - recover_motion -= n * (depth - min_contact_depth) * 0.4; - } - } - - if (recover_motion == Vector3()) { - collided = false; - break; - } - - body_transform.origin += recover_motion; - body_aabb.position += recover_motion; - - recover_attempts--; - - } while (recover_attempts); - } - - real_t safe = 1.0; - real_t unsafe = 1.0; - int best_shape = -1; - - { - // STEP 2 ATTEMPT MOTION - - AABB motion_aabb = body_aabb; - motion_aabb.position += p_parameters.motion; - motion_aabb = motion_aabb.merge(body_aabb); - - int amount = _cull_aabb_for_body(p_body, motion_aabb); - - for (int j = 0; j < p_body->get_shape_count(); j++) { - if (p_body->is_shape_disabled(j)) { - continue; - } - - Shape3DSW *body_shape = p_body->get_shape(j); - - // Colliding separation rays allows to properly snap to the ground, - // otherwise it's not needed in regular motion. - if (!p_parameters.collide_separation_ray && (body_shape->get_type() == PhysicsServer3D::SHAPE_SEPARATION_RAY)) { - // When slide on slope is on, separation ray shape acts like a regular shape. - if (!static_cast(body_shape)->get_slide_on_slope()) { - continue; - } - } - - Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j); - - Transform3D body_shape_xform_inv = body_shape_xform.affine_inverse(); - MotionShape3DSW mshape; - mshape.shape = body_shape; - mshape.motion = body_shape_xform_inv.basis.xform(p_parameters.motion); - - bool stuck = false; - - real_t best_safe = 1; - real_t best_unsafe = 1; - - for (int i = 0; i < amount; i++) { - const CollisionObject3DSW *col_obj = intersection_query_results[i]; - if (p_parameters.exclude_bodies.has(col_obj->get_self())) { - continue; - } - if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { - continue; - } - - int shape_idx = intersection_query_subindex_results[i]; - - //test initial overlap, does it collide if going all the way? - Vector3 point_A, point_B; - Vector3 sep_axis = motion_normal; - - Transform3D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - //test initial overlap, does it collide if going all the way? - if (CollisionSolver3DSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { - continue; - } - sep_axis = motion_normal; - - if (!CollisionSolver3DSW::solve_distance(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { - stuck = true; - break; - } - - //just do kinematic solving - real_t low = 0.0; - real_t hi = 1.0; - real_t fraction_coeff = 0.5; - for (int k = 0; k < 8; k++) { //steps should be customizable.. - real_t fraction = low + (hi - low) * fraction_coeff; - - mshape.motion = body_shape_xform_inv.basis.xform(p_parameters.motion * fraction); - - Vector3 lA, lB; - Vector3 sep = motion_normal; //important optimization for this to work fast enough - bool collided = !CollisionSolver3DSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, motion_aabb, &sep); - - if (collided) { - hi = fraction; - if ((k == 0) || (low > 0.0)) { // Did it not collide before? - // When alternating or first iteration, use dichotomy. - fraction_coeff = 0.5; - } else { - // When colliding again, converge faster towards low fraction - // for more accurate results with long motions that collide near the start. - fraction_coeff = 0.25; - } - } else { - point_A = lA; - point_B = lB; - low = fraction; - if ((k == 0) || (hi < 1.0)) { // Did it collide before? - // When alternating or first iteration, use dichotomy. - fraction_coeff = 0.5; - } else { - // When not colliding again, converge faster towards high fraction - // for more accurate results with long motions that collide near the end. - fraction_coeff = 0.75; - } - } - } - - if (low < best_safe) { - best_safe = low; - best_unsafe = hi; - } - } - - if (stuck) { - safe = 0; - unsafe = 0; - best_shape = j; //sadly it's the best - break; - } - if (best_safe == 1.0) { - continue; - } - if (best_safe < safe) { - safe = best_safe; - unsafe = best_unsafe; - best_shape = j; - } - } - } - - bool collided = false; - if (recovered || (safe < 1)) { - if (safe >= 1) { - best_shape = -1; //no best shape with cast, reset to -1 - } - - //it collided, let's get the rest info in unsafe advance - Transform3D ugt = body_transform; - ugt.origin += p_parameters.motion * unsafe; - - _RestResultData results[PhysicsServer3D::MotionResult::MAX_COLLISIONS]; - - _RestCallbackData rcd; - if (p_parameters.max_collisions > 1) { - rcd.max_results = p_parameters.max_collisions; - rcd.other_results = results; - } - - // Allowed depth can't be lower than motion length, in order to handle contacts at low speed. - rcd.min_allowed_depth = MIN(motion_length, min_contact_depth); - - int from_shape = best_shape != -1 ? best_shape : 0; - int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count(); - - for (int j = from_shape; j < to_shape; j++) { - if (p_body->is_shape_disabled(j)) { - continue; - } - - Transform3D body_shape_xform = ugt * p_body->get_shape_transform(j); - Shape3DSW *body_shape = p_body->get_shape(j); - - body_aabb.position += p_parameters.motion * unsafe; - - int amount = _cull_aabb_for_body(p_body, body_aabb); - - for (int i = 0; i < amount; i++) { - const CollisionObject3DSW *col_obj = intersection_query_results[i]; - if (p_parameters.exclude_bodies.has(col_obj->get_self())) { - continue; - } - if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { - continue; - } - - int shape_idx = intersection_query_subindex_results[i]; - - rcd.object = col_obj; - rcd.shape = shape_idx; - bool sc = CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_parameters.margin); - if (!sc) { - continue; - } - } - } - - if (rcd.result_count > 0) { - if (r_result) { - for (int collision_index = 0; collision_index < rcd.result_count; ++collision_index) { - const _RestResultData &result = (collision_index > 0) ? rcd.other_results[collision_index - 1] : rcd.best_result; - - PhysicsServer3D::MotionCollision &collision = r_result->collisions[collision_index]; - - collision.collider = result.object->get_self(); - collision.collider_id = result.object->get_instance_id(); - collision.collider_shape = result.shape; - collision.local_shape = result.local_shape; - collision.normal = result.normal; - collision.position = result.contact; - collision.depth = result.len; - - const Body3DSW *body = static_cast(result.object); - - Vector3 rel_vec = result.contact - (body->get_transform().origin + body->get_center_of_mass()); - collision.collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec); - } - - r_result->travel = safe * p_parameters.motion; - r_result->remainder = p_parameters.motion - safe * p_parameters.motion; - r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin()); - - r_result->collision_safe_fraction = safe; - r_result->collision_unsafe_fraction = unsafe; - - r_result->collision_count = rcd.result_count; - } - - collided = true; - } - } - - if (!collided && r_result) { - r_result->travel = p_parameters.motion; - r_result->remainder = Vector3(); - r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin()); - - r_result->collision_safe_fraction = 1.0; - r_result->collision_unsafe_fraction = 1.0; - } - - return collided; -} - -void *Space3DSW::_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_self) { - if (!A->interacts_with(B)) { - return nullptr; - } - - CollisionObject3DSW::Type type_A = A->get_type(); - CollisionObject3DSW::Type type_B = B->get_type(); - if (type_A > type_B) { - SWAP(A, B); - SWAP(p_subindex_A, p_subindex_B); - SWAP(type_A, type_B); - } - - Space3DSW *self = (Space3DSW *)p_self; - - self->collision_pairs++; - - if (type_A == CollisionObject3DSW::TYPE_AREA) { - Area3DSW *area = static_cast(A); - if (type_B == CollisionObject3DSW::TYPE_AREA) { - Area3DSW *area_b = static_cast(B); - Area2Pair3DSW *area2_pair = memnew(Area2Pair3DSW(area_b, p_subindex_B, area, p_subindex_A)); - return area2_pair; - } else if (type_B == CollisionObject3DSW::TYPE_SOFT_BODY) { - SoftBody3DSW *softbody = static_cast(B); - AreaSoftBodyPair3DSW *soft_area_pair = memnew(AreaSoftBodyPair3DSW(softbody, p_subindex_B, area, p_subindex_A)); - return soft_area_pair; - } else { - Body3DSW *body = static_cast(B); - AreaPair3DSW *area_pair = memnew(AreaPair3DSW(body, p_subindex_B, area, p_subindex_A)); - return area_pair; - } - } else if (type_A == CollisionObject3DSW::TYPE_BODY) { - if (type_B == CollisionObject3DSW::TYPE_SOFT_BODY) { - BodySoftBodyPair3DSW *soft_pair = memnew(BodySoftBodyPair3DSW((Body3DSW *)A, p_subindex_A, (SoftBody3DSW *)B)); - return soft_pair; - } else { - BodyPair3DSW *b = memnew(BodyPair3DSW((Body3DSW *)A, p_subindex_A, (Body3DSW *)B, p_subindex_B)); - return b; - } - } else { - // Soft Body/Soft Body, not supported. - } - - return nullptr; -} - -void Space3DSW::_broadphase_unpair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_data, void *p_self) { - if (!p_data) { - return; - } - - Space3DSW *self = (Space3DSW *)p_self; - self->collision_pairs--; - Constraint3DSW *c = (Constraint3DSW *)p_data; - memdelete(c); -} - -const SelfList::List &Space3DSW::get_active_body_list() const { - return active_list; -} - -void Space3DSW::body_add_to_active_list(SelfList *p_body) { - active_list.add(p_body); -} - -void Space3DSW::body_remove_from_active_list(SelfList *p_body) { - active_list.remove(p_body); -} - -void Space3DSW::body_add_to_mass_properties_update_list(SelfList *p_body) { - mass_properties_update_list.add(p_body); -} - -void Space3DSW::body_remove_from_mass_properties_update_list(SelfList *p_body) { - mass_properties_update_list.remove(p_body); -} - -BroadPhase3DSW *Space3DSW::get_broadphase() { - return broadphase; -} - -void Space3DSW::add_object(CollisionObject3DSW *p_object) { - ERR_FAIL_COND(objects.has(p_object)); - objects.insert(p_object); -} - -void Space3DSW::remove_object(CollisionObject3DSW *p_object) { - ERR_FAIL_COND(!objects.has(p_object)); - objects.erase(p_object); -} - -const Set &Space3DSW::get_objects() const { - return objects; -} - -void Space3DSW::body_add_to_state_query_list(SelfList *p_body) { - state_query_list.add(p_body); -} - -void Space3DSW::body_remove_from_state_query_list(SelfList *p_body) { - state_query_list.remove(p_body); -} - -void Space3DSW::area_add_to_monitor_query_list(SelfList *p_area) { - monitor_query_list.add(p_area); -} - -void Space3DSW::area_remove_from_monitor_query_list(SelfList *p_area) { - monitor_query_list.remove(p_area); -} - -void Space3DSW::area_add_to_moved_list(SelfList *p_area) { - area_moved_list.add(p_area); -} - -void Space3DSW::area_remove_from_moved_list(SelfList *p_area) { - area_moved_list.remove(p_area); -} - -const SelfList::List &Space3DSW::get_moved_area_list() const { - return area_moved_list; -} - -const SelfList::List &Space3DSW::get_active_soft_body_list() const { - return active_soft_body_list; -} - -void Space3DSW::soft_body_add_to_active_list(SelfList *p_soft_body) { - active_soft_body_list.add(p_soft_body); -} - -void Space3DSW::soft_body_remove_from_active_list(SelfList *p_soft_body) { - active_soft_body_list.remove(p_soft_body); -} - -void Space3DSW::call_queries() { - while (state_query_list.first()) { - Body3DSW *b = state_query_list.first()->self(); - state_query_list.remove(state_query_list.first()); - b->call_queries(); - } - - while (monitor_query_list.first()) { - Area3DSW *a = monitor_query_list.first()->self(); - monitor_query_list.remove(monitor_query_list.first()); - a->call_queries(); - } -} - -void Space3DSW::setup() { - contact_debug_count = 0; - while (mass_properties_update_list.first()) { - mass_properties_update_list.first()->self()->update_mass_properties(); - mass_properties_update_list.remove(mass_properties_update_list.first()); - } -} - -void Space3DSW::update() { - broadphase->update(); -} - -void Space3DSW::set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_value) { - switch (p_param) { - case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: - contact_recycle_radius = p_value; - break; - case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION: - contact_max_separation = p_value; - break; - case PhysicsServer3D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: - contact_max_allowed_penetration = p_value; - break; - case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: - body_linear_velocity_sleep_threshold = p_value; - break; - case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: - body_angular_velocity_sleep_threshold = p_value; - break; - case PhysicsServer3D::SPACE_PARAM_BODY_TIME_TO_SLEEP: - body_time_to_sleep = p_value; - break; - case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: - body_angular_velocity_damp_ratio = p_value; - break; - case PhysicsServer3D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: - constraint_bias = p_value; - break; - } -} - -real_t Space3DSW::get_param(PhysicsServer3D::SpaceParameter p_param) const { - switch (p_param) { - case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: - return contact_recycle_radius; - case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION: - return contact_max_separation; - case PhysicsServer3D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: - return contact_max_allowed_penetration; - case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: - return body_linear_velocity_sleep_threshold; - case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: - return body_angular_velocity_sleep_threshold; - case PhysicsServer3D::SPACE_PARAM_BODY_TIME_TO_SLEEP: - return body_time_to_sleep; - case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: - return body_angular_velocity_damp_ratio; - case PhysicsServer3D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: - return constraint_bias; - } - return 0; -} - -void Space3DSW::lock() { - locked = true; -} - -void Space3DSW::unlock() { - locked = false; -} - -bool Space3DSW::is_locked() const { - return locked; -} - -PhysicsDirectSpaceState3DSW *Space3DSW::get_direct_state() { - return direct_access; -} - -Space3DSW::Space3DSW() { - body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_linear", 0.1); - body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_angular", Math::deg2rad(8.0)); - body_time_to_sleep = GLOBAL_DEF("physics/3d/time_before_sleep", 0.5); - ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/time_before_sleep", PropertyInfo(Variant::FLOAT, "physics/3d/time_before_sleep", PROPERTY_HINT_RANGE, "0,5,0.01,or_greater")); - body_angular_velocity_damp_ratio = 10; - - broadphase = BroadPhase3DSW::create_func(); - broadphase->set_pair_callback(_broadphase_pair, this); - broadphase->set_unpair_callback(_broadphase_unpair, this); - - direct_access = memnew(PhysicsDirectSpaceState3DSW); - direct_access->space = this; -} - -Space3DSW::~Space3DSW() { - memdelete(broadphase); - memdelete(direct_access); -} diff --git a/servers/physics_3d/space_3d_sw.h b/servers/physics_3d/space_3d_sw.h deleted file mode 100644 index 69cc3c4bbd..0000000000 --- a/servers/physics_3d/space_3d_sw.h +++ /dev/null @@ -1,216 +0,0 @@ -/*************************************************************************/ -/* space_3d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 SPACE_SW_H -#define SPACE_SW_H - -#include "area_3d_sw.h" -#include "area_pair_3d_sw.h" -#include "body_3d_sw.h" -#include "body_pair_3d_sw.h" -#include "broad_phase_3d_sw.h" -#include "collision_object_3d_sw.h" -#include "core/config/project_settings.h" -#include "core/templates/hash_map.h" -#include "core/typedefs.h" -#include "soft_body_3d_sw.h" - -class PhysicsDirectSpaceState3DSW : public PhysicsDirectSpaceState3D { - GDCLASS(PhysicsDirectSpaceState3DSW, PhysicsDirectSpaceState3D); - -public: - Space3DSW *space; - - virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_ray = false) override; - virtual int intersect_shape(const RID &p_shape, const Transform3D &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual bool cast_motion(const RID &p_shape, const Transform3D &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, ShapeRestInfo *r_info = nullptr) override; - virtual bool collide_shape(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual bool rest_info(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set &p_exclude = Set(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const override; - - PhysicsDirectSpaceState3DSW(); -}; - -class Space3DSW { -public: - enum ElapsedTime { - ELAPSED_TIME_INTEGRATE_FORCES, - ELAPSED_TIME_GENERATE_ISLANDS, - ELAPSED_TIME_SETUP_CONSTRAINTS, - ELAPSED_TIME_SOLVE_CONSTRAINTS, - ELAPSED_TIME_INTEGRATE_VELOCITIES, - ELAPSED_TIME_MAX - - }; - -private: - uint64_t elapsed_time[ELAPSED_TIME_MAX] = {}; - - PhysicsDirectSpaceState3DSW *direct_access; - RID self; - - BroadPhase3DSW *broadphase; - SelfList::List active_list; - SelfList::List mass_properties_update_list; - SelfList::List state_query_list; - SelfList::List monitor_query_list; - SelfList::List area_moved_list; - SelfList::List active_soft_body_list; - - static void *_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_self); - static void _broadphase_unpair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_data, void *p_self); - - Set objects; - - Area3DSW *area = nullptr; - - real_t contact_recycle_radius = 0.01; - real_t contact_max_separation = 0.05; - real_t contact_max_allowed_penetration = 0.01; - real_t constraint_bias = 0.01; - - enum { - INTERSECTION_QUERY_MAX = 2048 - }; - - CollisionObject3DSW *intersection_query_results[INTERSECTION_QUERY_MAX]; - int intersection_query_subindex_results[INTERSECTION_QUERY_MAX]; - - real_t body_linear_velocity_sleep_threshold; - real_t body_angular_velocity_sleep_threshold; - real_t body_time_to_sleep; - real_t body_angular_velocity_damp_ratio; - - bool locked = false; - - real_t last_step = 0.001; - - int island_count = 0; - int active_objects = 0; - int collision_pairs = 0; - - RID static_global_body; - - Vector contact_debug; - int contact_debug_count = 0; - - friend class PhysicsDirectSpaceState3DSW; - - int _cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb); - -public: - _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } - _FORCE_INLINE_ RID get_self() const { return self; } - - void set_default_area(Area3DSW *p_area) { area = p_area; } - Area3DSW *get_default_area() const { return area; } - - const SelfList::List &get_active_body_list() const; - void body_add_to_active_list(SelfList *p_body); - void body_remove_from_active_list(SelfList *p_body); - void body_add_to_mass_properties_update_list(SelfList *p_body); - void body_remove_from_mass_properties_update_list(SelfList *p_body); - - void body_add_to_state_query_list(SelfList *p_body); - void body_remove_from_state_query_list(SelfList *p_body); - - void area_add_to_monitor_query_list(SelfList *p_area); - void area_remove_from_monitor_query_list(SelfList *p_area); - void area_add_to_moved_list(SelfList *p_area); - void area_remove_from_moved_list(SelfList *p_area); - const SelfList::List &get_moved_area_list() const; - - const SelfList::List &get_active_soft_body_list() const; - void soft_body_add_to_active_list(SelfList *p_soft_body); - void soft_body_remove_from_active_list(SelfList *p_soft_body); - - BroadPhase3DSW *get_broadphase(); - - void add_object(CollisionObject3DSW *p_object); - void remove_object(CollisionObject3DSW *p_object); - const Set &get_objects() const; - - _FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; } - _FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; } - _FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; } - _FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; } - _FORCE_INLINE_ real_t get_body_linear_velocity_sleep_threshold() const { return body_linear_velocity_sleep_threshold; } - _FORCE_INLINE_ real_t get_body_angular_velocity_sleep_threshold() const { return body_angular_velocity_sleep_threshold; } - _FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; } - _FORCE_INLINE_ real_t get_body_angular_velocity_damp_ratio() const { return body_angular_velocity_damp_ratio; } - - void update(); - void setup(); - void call_queries(); - - bool is_locked() const; - void lock(); - void unlock(); - - real_t get_last_step() const { return last_step; } - void set_last_step(real_t p_step) { last_step = p_step; } - - void set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_value); - real_t get_param(PhysicsServer3D::SpaceParameter p_param) const; - - void set_island_count(int p_island_count) { island_count = p_island_count; } - int get_island_count() const { return island_count; } - - void set_active_objects(int p_active_objects) { active_objects = p_active_objects; } - int get_active_objects() const { return active_objects; } - - int get_collision_pairs() const { return collision_pairs; } - - PhysicsDirectSpaceState3DSW *get_direct_state(); - - void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); } - _FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.is_empty(); } - _FORCE_INLINE_ void add_debug_contact(const Vector3 &p_contact) { - if (contact_debug_count < contact_debug.size()) { - contact_debug.write[contact_debug_count++] = p_contact; - } - } - _FORCE_INLINE_ Vector get_debug_contacts() { return contact_debug; } - _FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; } - - void set_static_global_body(RID p_body) { static_global_body = p_body; } - RID get_static_global_body() { return static_global_body; } - - void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; } - uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; } - - bool test_body_motion(Body3DSW *p_body, const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult *r_result); - - Space3DSW(); - ~Space3DSW(); -}; - -#endif // SPACE__SW_H diff --git a/servers/physics_3d/step_3d_sw.cpp b/servers/physics_3d/step_3d_sw.cpp deleted file mode 100644 index 6572d58c91..0000000000 --- a/servers/physics_3d/step_3d_sw.cpp +++ /dev/null @@ -1,419 +0,0 @@ -/*************************************************************************/ -/* step_3d_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 "step_3d_sw.h" -#include "joints_3d_sw.h" - -#include "core/os/os.h" - -#define BODY_ISLAND_COUNT_RESERVE 128 -#define BODY_ISLAND_SIZE_RESERVE 512 -#define ISLAND_COUNT_RESERVE 128 -#define ISLAND_SIZE_RESERVE 512 -#define CONSTRAINT_COUNT_RESERVE 1024 - -void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector &p_body_island, LocalVector &p_constraint_island) { - p_body->set_island_step(_step); - - if (p_body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) { - // Only dynamic bodies are tested for activation. - p_body_island.push_back(p_body); - } - - for (const KeyValue &E : p_body->get_constraint_map()) { - Constraint3DSW *constraint = (Constraint3DSW *)E.key; - if (constraint->get_island_step() == _step) { - continue; // Already processed. - } - constraint->set_island_step(_step); - p_constraint_island.push_back(constraint); - - all_constraints.push_back(constraint); - - // Find connected rigid bodies. - for (int i = 0; i < constraint->get_body_count(); i++) { - if (i == E.value) { - continue; - } - Body3DSW *other_body = constraint->get_body_ptr()[i]; - if (other_body->get_island_step() == _step) { - continue; // Already processed. - } - if (other_body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) { - continue; // Static bodies don't connect islands. - } - _populate_island(other_body, p_body_island, p_constraint_island); - } - - // Find connected soft bodies. - for (int i = 0; i < constraint->get_soft_body_count(); i++) { - SoftBody3DSW *soft_body = constraint->get_soft_body_ptr(i); - if (soft_body->get_island_step() == _step) { - continue; // Already processed. - } - _populate_island_soft_body(soft_body, p_body_island, p_constraint_island); - } - } -} - -void Step3DSW::_populate_island_soft_body(SoftBody3DSW *p_soft_body, LocalVector &p_body_island, LocalVector &p_constraint_island) { - p_soft_body->set_island_step(_step); - - for (Set::Element *E = p_soft_body->get_constraints().front(); E; E = E->next()) { - Constraint3DSW *constraint = (Constraint3DSW *)E->get(); - if (constraint->get_island_step() == _step) { - continue; // Already processed. - } - constraint->set_island_step(_step); - p_constraint_island.push_back(constraint); - - all_constraints.push_back(constraint); - - // Find connected rigid bodies. - for (int i = 0; i < constraint->get_body_count(); i++) { - Body3DSW *body = constraint->get_body_ptr()[i]; - if (body->get_island_step() == _step) { - continue; // Already processed. - } - if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) { - continue; // Static bodies don't connect islands. - } - _populate_island(body, p_body_island, p_constraint_island); - } - } -} - -void Step3DSW::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) { - Constraint3DSW *constraint = all_constraints[p_constraint_index]; - constraint->setup(delta); -} - -void Step3DSW::_pre_solve_island(LocalVector &p_constraint_island) const { - uint32_t constraint_count = p_constraint_island.size(); - uint32_t valid_constraint_count = 0; - for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { - Constraint3DSW *constraint = p_constraint_island[constraint_index]; - if (p_constraint_island[constraint_index]->pre_solve(delta)) { - // Keep this constraint for solving. - p_constraint_island[valid_constraint_count++] = constraint; - } - } - p_constraint_island.resize(valid_constraint_count); -} - -void Step3DSW::_solve_island(uint32_t p_island_index, void *p_userdata) { - LocalVector &constraint_island = constraint_islands[p_island_index]; - - int current_priority = 1; - - uint32_t constraint_count = constraint_island.size(); - while (constraint_count > 0) { - for (int i = 0; i < iterations; i++) { - // Go through all iterations. - for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { - constraint_island[constraint_index]->solve(delta); - } - } - - // Check priority to keep only higher priority constraints. - uint32_t priority_constraint_count = 0; - ++current_priority; - for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { - Constraint3DSW *constraint = constraint_island[constraint_index]; - if (constraint->get_priority() >= current_priority) { - // Keep this constraint for the next iteration. - constraint_island[priority_constraint_count++] = constraint; - } - } - constraint_count = priority_constraint_count; - } -} - -void Step3DSW::_check_suspend(const LocalVector &p_body_island) const { - bool can_sleep = true; - - uint32_t body_count = p_body_island.size(); - for (uint32_t body_index = 0; body_index < body_count; ++body_index) { - Body3DSW *body = p_body_island[body_index]; - - if (!body->sleep_test(delta)) { - can_sleep = false; - } - } - - // Put all to sleep or wake up everyone. - for (uint32_t body_index = 0; body_index < body_count; ++body_index) { - Body3DSW *body = p_body_island[body_index]; - - bool active = body->is_active(); - - if (active == can_sleep) { - body->set_active(!can_sleep); - } - } -} - -void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { - p_space->lock(); // can't access space during this - - p_space->setup(); //update inertias, etc - - p_space->set_last_step(p_delta); - - iterations = p_iterations; - delta = p_delta; - - const SelfList::List *body_list = &p_space->get_active_body_list(); - - const SelfList::List *soft_body_list = &p_space->get_active_soft_body_list(); - - /* INTEGRATE FORCES */ - - uint64_t profile_begtime = OS::get_singleton()->get_ticks_usec(); - uint64_t profile_endtime = 0; - - int active_count = 0; - - const SelfList *b = body_list->first(); - while (b) { - b->self()->integrate_forces(p_delta); - b = b->next(); - active_count++; - } - - /* UPDATE SOFT BODY MOTION */ - - const SelfList *sb = soft_body_list->first(); - while (sb) { - sb->self()->predict_motion(p_delta); - sb = sb->next(); - active_count++; - } - - p_space->set_active_objects(active_count); - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime); - profile_begtime = profile_endtime; - } - - /* GENERATE CONSTRAINT ISLANDS FOR MOVING AREAS */ - - uint32_t island_count = 0; - - const SelfList::List &aml = p_space->get_moved_area_list(); - - while (aml.first()) { - for (const Set::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { - Constraint3DSW *constraint = E->get(); - if (constraint->get_island_step() == _step) { - continue; - } - constraint->set_island_step(_step); - - // Each constraint can be on a separate island for areas as there's no solving phase. - ++island_count; - if (constraint_islands.size() < island_count) { - constraint_islands.resize(island_count); - } - LocalVector &constraint_island = constraint_islands[island_count - 1]; - constraint_island.clear(); - - all_constraints.push_back(constraint); - constraint_island.push_back(constraint); - } - p_space->area_remove_from_moved_list((SelfList *)aml.first()); //faster to remove here - } - - /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */ - - b = body_list->first(); - - uint32_t body_island_count = 0; - - while (b) { - Body3DSW *body = b->self(); - - if (body->get_island_step() != _step) { - ++body_island_count; - if (body_islands.size() < body_island_count) { - body_islands.resize(body_island_count); - } - LocalVector &body_island = body_islands[body_island_count - 1]; - body_island.clear(); - body_island.reserve(BODY_ISLAND_SIZE_RESERVE); - - ++island_count; - if (constraint_islands.size() < island_count) { - constraint_islands.resize(island_count); - } - LocalVector &constraint_island = constraint_islands[island_count - 1]; - constraint_island.clear(); - constraint_island.reserve(ISLAND_SIZE_RESERVE); - - _populate_island(body, body_island, constraint_island); - - if (body_island.is_empty()) { - --body_island_count; - } - - if (constraint_island.is_empty()) { - --island_count; - } - } - b = b->next(); - } - - /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE SOFT BODIES */ - - sb = soft_body_list->first(); - while (sb) { - SoftBody3DSW *soft_body = sb->self(); - - if (soft_body->get_island_step() != _step) { - ++body_island_count; - if (body_islands.size() < body_island_count) { - body_islands.resize(body_island_count); - } - LocalVector &body_island = body_islands[body_island_count - 1]; - body_island.clear(); - body_island.reserve(BODY_ISLAND_SIZE_RESERVE); - - ++island_count; - if (constraint_islands.size() < island_count) { - constraint_islands.resize(island_count); - } - LocalVector &constraint_island = constraint_islands[island_count - 1]; - constraint_island.clear(); - constraint_island.reserve(ISLAND_SIZE_RESERVE); - - _populate_island_soft_body(soft_body, body_island, constraint_island); - - if (body_island.is_empty()) { - --body_island_count; - } - - if (constraint_island.is_empty()) { - --island_count; - } - } - sb = sb->next(); - } - - p_space->set_island_count((int)island_count); - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); - profile_begtime = profile_endtime; - } - - /* SETUP CONSTRAINTS / PROCESS COLLISIONS */ - - uint32_t total_contraint_count = all_constraints.size(); - work_pool.do_work(total_contraint_count, this, &Step3DSW::_setup_contraint, nullptr); - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime); - profile_begtime = profile_endtime; - } - - /* PRE-SOLVE CONSTRAINT ISLANDS */ - - // Warning: This doesn't run on threads, because it involves thread-unsafe processing. - for (uint32_t island_index = 0; island_index < island_count; ++island_index) { - _pre_solve_island(constraint_islands[island_index]); - } - - /* SOLVE CONSTRAINT ISLANDS */ - - // Warning: _solve_island modifies the constraint islands for optimization purpose, - // their content is not reliable after these calls and shouldn't be used anymore. - if (island_count > 1) { - work_pool.do_work(island_count, this, &Step3DSW::_solve_island, nullptr); - } else if (island_count > 0) { - _solve_island(0); - } - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime); - profile_begtime = profile_endtime; - } - - /* INTEGRATE VELOCITIES */ - - b = body_list->first(); - while (b) { - const SelfList *n = b->next(); - b->self()->integrate_velocities(p_delta); - b = n; - } - - /* SLEEP / WAKE UP ISLANDS */ - - for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) { - _check_suspend(body_islands[island_index]); - } - - /* UPDATE SOFT BODY CONSTRAINTS */ - - sb = soft_body_list->first(); - while (sb) { - sb->self()->solve_constraints(p_delta); - sb = sb->next(); - } - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime); - profile_begtime = profile_endtime; - } - - all_constraints.clear(); - - p_space->update(); - p_space->unlock(); - _step++; -} - -Step3DSW::Step3DSW() { - body_islands.reserve(BODY_ISLAND_COUNT_RESERVE); - constraint_islands.reserve(ISLAND_COUNT_RESERVE); - all_constraints.reserve(CONSTRAINT_COUNT_RESERVE); - - work_pool.init(); -} - -Step3DSW::~Step3DSW() { - work_pool.finish(); -} diff --git a/servers/physics_3d/step_3d_sw.h b/servers/physics_3d/step_3d_sw.h deleted file mode 100644 index f2f879104a..0000000000 --- a/servers/physics_3d/step_3d_sw.h +++ /dev/null @@ -1,64 +0,0 @@ -/*************************************************************************/ -/* step_3d_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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 STEP_SW_H -#define STEP_SW_H - -#include "space_3d_sw.h" - -#include "core/templates/local_vector.h" -#include "core/templates/thread_work_pool.h" - -class Step3DSW { - uint64_t _step = 1; - - int iterations = 0; - real_t delta = 0.0; - - ThreadWorkPool work_pool; - - LocalVector> body_islands; - LocalVector> constraint_islands; - LocalVector all_constraints; - - void _populate_island(Body3DSW *p_body, LocalVector &p_body_island, LocalVector &p_constraint_island); - void _populate_island_soft_body(SoftBody3DSW *p_soft_body, LocalVector &p_body_island, LocalVector &p_constraint_island); - void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr); - void _pre_solve_island(LocalVector &p_constraint_island) const; - void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr); - void _check_suspend(const LocalVector &p_body_island) const; - -public: - void step(Space3DSW *p_space, real_t p_delta, int p_iterations); - Step3DSW(); - ~Step3DSW(); -}; - -#endif // STEP__SW_H diff --git a/servers/physics_server_2d.h b/servers/physics_server_2d.h index 26e1411111..f83c57407d 100644 --- a/servers/physics_server_2d.h +++ b/servers/physics_server_2d.h @@ -28,8 +28,8 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#ifndef PHYSICS_2D_SERVER_H -#define PHYSICS_2D_SERVER_H +#ifndef PHYSICS_SERVER_2D_H +#define PHYSICS_SERVER_2D_H #include "core/io/resource.h" #include "core/object/class_db.h" @@ -701,4 +701,4 @@ VARIANT_ENUM_CAST(PhysicsServer2D::DampedSpringParam); VARIANT_ENUM_CAST(PhysicsServer2D::AreaBodyStatus); VARIANT_ENUM_CAST(PhysicsServer2D::ProcessInfo); -#endif +#endif // PHYSICS_SERVER_2D_H diff --git a/servers/physics_server_2d_wrap_mt.cpp b/servers/physics_server_2d_wrap_mt.cpp new file mode 100644 index 0000000000..33070bf42d --- /dev/null +++ b/servers/physics_server_2d_wrap_mt.cpp @@ -0,0 +1,137 @@ +/*************************************************************************/ +/* physics_server_2d_wrap_mt.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* 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 "physics_server_2d_wrap_mt.h" + +#include "core/os/os.h" + +void PhysicsServer2DWrapMT::thread_exit() { + exit.set(); +} + +void PhysicsServer2DWrapMT::thread_step(real_t p_delta) { + physics_2d_server->step(p_delta); + step_sem.post(); +} + +void PhysicsServer2DWrapMT::_thread_callback(void *_instance) { + PhysicsServer2DWrapMT *vsmt = reinterpret_cast(_instance); + + vsmt->thread_loop(); +} + +void PhysicsServer2DWrapMT::thread_loop() { + server_thread = Thread::get_caller_id(); + + physics_2d_server->init(); + + exit.clear(); + step_thread_up.set(); + while (!exit.is_set()) { + // flush commands one by one, until exit is requested + command_queue.wait_and_flush(); + } + + command_queue.flush_all(); // flush all + + physics_2d_server->finish(); +} + +/* EVENT QUEUING */ + +void PhysicsServer2DWrapMT::step(real_t p_step) { + if (create_thread) { + command_queue.push(this, &PhysicsServer2DWrapMT::thread_step, p_step); + } else { + command_queue.flush_all(); //flush all pending from other threads + physics_2d_server->step(p_step); + } +} + +void PhysicsServer2DWrapMT::sync() { + if (create_thread) { + if (first_frame) { + first_frame = false; + } else { + step_sem.wait(); //must not wait if a step was not issued + } + } + physics_2d_server->sync(); +} + +void PhysicsServer2DWrapMT::flush_queries() { + physics_2d_server->flush_queries(); +} + +void PhysicsServer2DWrapMT::end_sync() { + physics_2d_server->end_sync(); +} + +void PhysicsServer2DWrapMT::init() { + if (create_thread) { + //OS::get_singleton()->release_rendering_thread(); + thread.start(_thread_callback, this); + while (!step_thread_up.is_set()) { + OS::get_singleton()->delay_usec(1000); + } + } else { + physics_2d_server->init(); + } +} + +void PhysicsServer2DWrapMT::finish() { + if (thread.is_started()) { + command_queue.push(this, &PhysicsServer2DWrapMT::thread_exit); + thread.wait_to_finish(); + } else { + physics_2d_server->finish(); + } +} + +PhysicsServer2DWrapMT::PhysicsServer2DWrapMT(PhysicsServer2D *p_contained, bool p_create_thread) : + command_queue(p_create_thread) { + physics_2d_server = p_contained; + create_thread = p_create_thread; + + pool_max_size = GLOBAL_GET("memory/limits/multithreaded_server/rid_pool_prealloc"); + + if (!p_create_thread) { + server_thread = Thread::get_caller_id(); + } else { + server_thread = 0; + } + + main_thread = Thread::get_caller_id(); +} + +PhysicsServer2DWrapMT::~PhysicsServer2DWrapMT() { + memdelete(physics_2d_server); + //finish(); +} diff --git a/servers/physics_server_2d_wrap_mt.h b/servers/physics_server_2d_wrap_mt.h new file mode 100644 index 0000000000..927ef5d57c --- /dev/null +++ b/servers/physics_server_2d_wrap_mt.h @@ -0,0 +1,333 @@ +/*************************************************************************/ +/* physics_server_2d_wrap_mt.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* 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 PHYSICS_SERVER_2D_WRAP_MT_H +#define PHYSICS_SERVER_2D_WRAP_MT_H + +#include "core/config/project_settings.h" +#include "core/os/thread.h" +#include "core/templates/command_queue_mt.h" +#include "core/templates/safe_refcount.h" +#include "servers/physics_server_2d.h" + +#ifdef DEBUG_SYNC +#define SYNC_DEBUG print_line("sync on: " + String(__FUNCTION__)); +#else +#define SYNC_DEBUG +#endif + +class PhysicsServer2DWrapMT : public PhysicsServer2D { + mutable PhysicsServer2D *physics_2d_server; + + mutable CommandQueueMT command_queue; + + static void _thread_callback(void *_instance); + void thread_loop(); + + Thread::ID server_thread; + Thread::ID main_thread; + SafeFlag exit; + Thread thread; + SafeFlag step_thread_up; + bool create_thread = false; + + Semaphore step_sem; + int step_pending = 0; + void thread_step(real_t p_delta); + void thread_flush(); + + void thread_exit(); + + bool first_frame = true; + + Mutex alloc_mutex; + int pool_max_size = 0; + +public: +#define ServerName PhysicsServer2D +#define ServerNameWrapMT PhysicsServer2DWrapMT +#define server_name physics_2d_server +#define WRITE_ACTION + +#include "servers/server_wrap_mt_common.h" + + //FUNC1RID(shape,ShapeType); todo fix + FUNCRID(world_boundary_shape) + FUNCRID(separation_ray_shape) + FUNCRID(segment_shape) + FUNCRID(circle_shape) + FUNCRID(rectangle_shape) + FUNCRID(capsule_shape) + FUNCRID(convex_polygon_shape) + FUNCRID(concave_polygon_shape) + + FUNC2(shape_set_data, RID, const Variant &); + FUNC2(shape_set_custom_solver_bias, RID, real_t); + + FUNC1RC(ShapeType, shape_get_type, RID); + FUNC1RC(Variant, shape_get_data, RID); + FUNC1RC(real_t, shape_get_custom_solver_bias, RID); + + //these work well, but should be used from the main thread only + bool shape_collide(RID p_shape_A, const Transform2D &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Transform2D &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); + return physics_2d_server->shape_collide(p_shape_A, p_xform_A, p_motion_A, p_shape_B, p_xform_B, p_motion_B, r_results, p_result_max, r_result_count); + } + + /* SPACE API */ + + FUNCRID(space); + FUNC2(space_set_active, RID, bool); + FUNC1RC(bool, space_is_active, RID); + + FUNC3(space_set_param, RID, SpaceParameter, real_t); + FUNC2RC(real_t, space_get_param, RID, SpaceParameter); + + // this function only works on physics process, errors and returns null otherwise + PhysicsDirectSpaceState2D *space_get_direct_state(RID p_space) override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr); + return physics_2d_server->space_get_direct_state(p_space); + } + + FUNC2(space_set_debug_contacts, RID, int); + virtual Vector space_get_contacts(RID p_space) const override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), Vector()); + return physics_2d_server->space_get_contacts(p_space); + } + + virtual int space_get_contact_count(RID p_space) const override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), 0); + return physics_2d_server->space_get_contact_count(p_space); + } + + /* AREA API */ + + //FUNC0RID(area); + FUNCRID(area); + + FUNC2(area_set_space, RID, RID); + FUNC1RC(RID, area_get_space, RID); + + FUNC2(area_set_space_override_mode, RID, AreaSpaceOverrideMode); + FUNC1RC(AreaSpaceOverrideMode, area_get_space_override_mode, RID); + + FUNC4(area_add_shape, RID, RID, const Transform2D &, bool); + FUNC3(area_set_shape, RID, int, RID); + FUNC3(area_set_shape_transform, RID, int, const Transform2D &); + FUNC3(area_set_shape_disabled, RID, int, bool); + + FUNC1RC(int, area_get_shape_count, RID); + FUNC2RC(RID, area_get_shape, RID, int); + FUNC2RC(Transform2D, area_get_shape_transform, RID, int); + FUNC2(area_remove_shape, RID, int); + FUNC1(area_clear_shapes, RID); + + FUNC2(area_attach_object_instance_id, RID, ObjectID); + FUNC1RC(ObjectID, area_get_object_instance_id, RID); + + FUNC2(area_attach_canvas_instance_id, RID, ObjectID); + FUNC1RC(ObjectID, area_get_canvas_instance_id, RID); + + FUNC3(area_set_param, RID, AreaParameter, const Variant &); + FUNC2(area_set_transform, RID, const Transform2D &); + + FUNC2RC(Variant, area_get_param, RID, AreaParameter); + FUNC1RC(Transform2D, area_get_transform, RID); + + FUNC2(area_set_collision_mask, RID, uint32_t); + FUNC2(area_set_collision_layer, RID, uint32_t); + + FUNC2(area_set_monitorable, RID, bool); + FUNC2(area_set_pickable, RID, bool); + + FUNC3(area_set_monitor_callback, RID, Object *, const StringName &); + FUNC3(area_set_area_monitor_callback, RID, Object *, const StringName &); + + /* BODY API */ + + //FUNC2RID(body,BodyMode,bool); + FUNCRID(body) + + FUNC2(body_set_space, RID, RID); + FUNC1RC(RID, body_get_space, RID); + + FUNC2(body_set_mode, RID, BodyMode); + FUNC1RC(BodyMode, body_get_mode, RID); + + FUNC4(body_add_shape, RID, RID, const Transform2D &, bool); + FUNC3(body_set_shape, RID, int, RID); + FUNC3(body_set_shape_transform, RID, int, const Transform2D &); + + FUNC1RC(int, body_get_shape_count, RID); + FUNC2RC(Transform2D, body_get_shape_transform, RID, int); + FUNC2RC(RID, body_get_shape, RID, int); + + FUNC3(body_set_shape_disabled, RID, int, bool); + FUNC4(body_set_shape_as_one_way_collision, RID, int, bool, real_t); + + FUNC2(body_remove_shape, RID, int); + FUNC1(body_clear_shapes, RID); + + FUNC2(body_attach_object_instance_id, RID, ObjectID); + FUNC1RC(ObjectID, body_get_object_instance_id, RID); + + FUNC2(body_attach_canvas_instance_id, RID, ObjectID); + FUNC1RC(ObjectID, body_get_canvas_instance_id, RID); + + FUNC2(body_set_continuous_collision_detection_mode, RID, CCDMode); + FUNC1RC(CCDMode, body_get_continuous_collision_detection_mode, RID); + + FUNC2(body_set_collision_layer, RID, uint32_t); + FUNC1RC(uint32_t, body_get_collision_layer, RID); + + FUNC2(body_set_collision_mask, RID, uint32_t); + FUNC1RC(uint32_t, body_get_collision_mask, RID); + + FUNC3(body_set_param, RID, BodyParameter, const Variant &); + FUNC2RC(Variant, body_get_param, RID, BodyParameter); + + FUNC1(body_reset_mass_properties, RID); + + FUNC3(body_set_state, RID, BodyState, const Variant &); + FUNC2RC(Variant, body_get_state, RID, BodyState); + + FUNC2(body_set_applied_force, RID, const Vector2 &); + FUNC1RC(Vector2, body_get_applied_force, RID); + + FUNC2(body_set_applied_torque, RID, real_t); + FUNC1RC(real_t, body_get_applied_torque, RID); + + FUNC2(body_add_central_force, RID, const Vector2 &); + FUNC3(body_add_force, RID, const Vector2 &, const Vector2 &); + FUNC2(body_add_torque, RID, real_t); + FUNC2(body_apply_central_impulse, RID, const Vector2 &); + FUNC2(body_apply_torque_impulse, RID, real_t); + FUNC3(body_apply_impulse, RID, const Vector2 &, const Vector2 &); + FUNC2(body_set_axis_velocity, RID, const Vector2 &); + + FUNC2(body_add_collision_exception, RID, RID); + FUNC2(body_remove_collision_exception, RID, RID); + FUNC2S(body_get_collision_exceptions, RID, List *); + + FUNC2(body_set_max_contacts_reported, RID, int); + FUNC1RC(int, body_get_max_contacts_reported, RID); + + FUNC2(body_set_contacts_reported_depth_threshold, RID, real_t); + FUNC1RC(real_t, body_get_contacts_reported_depth_threshold, RID); + + FUNC2(body_set_omit_force_integration, RID, bool); + FUNC1RC(bool, body_is_omitting_force_integration, RID); + + FUNC3(body_set_state_sync_callback, RID, void *, BodyStateCallback); + FUNC3(body_set_force_integration_callback, RID, const Callable &, const Variant &); + + bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override { + return physics_2d_server->body_collide_shape(p_body, p_body_shape, p_shape, p_shape_xform, p_motion, r_results, p_result_max, r_result_count); + } + + FUNC2(body_set_pickable, RID, bool); + + bool body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result = nullptr) override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); + return physics_2d_server->body_test_motion(p_body, p_parameters, r_result); + } + + // this function only works on physics process, errors and returns null otherwise + PhysicsDirectBodyState2D *body_get_direct_state(RID p_body) override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr); + return physics_2d_server->body_get_direct_state(p_body); + } + + /* JOINT API */ + + FUNCRID(joint) + + FUNC1(joint_clear, RID) + + FUNC3(joint_set_param, RID, JointParam, real_t); + FUNC2RC(real_t, joint_get_param, RID, JointParam); + + FUNC2(joint_disable_collisions_between_bodies, RID, const bool); + FUNC1RC(bool, joint_is_disabled_collisions_between_bodies, RID); + + ///FUNC3RID(pin_joint,const Vector2&,RID,RID); + ///FUNC5RID(groove_joint,const Vector2&,const Vector2&,const Vector2&,RID,RID); + ///FUNC4RID(damped_spring_joint,const Vector2&,const Vector2&,RID,RID); + + //TODO need to convert this to FUNCRID, but it's a hassle.. + + FUNC4(joint_make_pin, RID, const Vector2 &, RID, RID); + FUNC6(joint_make_groove, RID, const Vector2 &, const Vector2 &, const Vector2 &, RID, RID); + FUNC5(joint_make_damped_spring, RID, const Vector2 &, const Vector2 &, RID, RID); + + FUNC3(pin_joint_set_param, RID, PinJointParam, real_t); + FUNC2RC(real_t, pin_joint_get_param, RID, PinJointParam); + + FUNC3(damped_spring_joint_set_param, RID, DampedSpringParam, real_t); + FUNC2RC(real_t, damped_spring_joint_get_param, RID, DampedSpringParam); + + FUNC1RC(JointType, joint_get_type, RID); + + /* MISC */ + + FUNC1(free, RID); + FUNC1(set_active, bool); + FUNC1(set_collision_iterations, int); + + virtual void init() override; + virtual void step(real_t p_step) override; + virtual void sync() override; + virtual void end_sync() override; + virtual void flush_queries() override; + virtual void finish() override; + + virtual bool is_flushing_queries() const override { + return physics_2d_server->is_flushing_queries(); + } + + int get_process_info(ProcessInfo p_info) override { + return physics_2d_server->get_process_info(p_info); + } + + PhysicsServer2DWrapMT(PhysicsServer2D *p_contained, bool p_create_thread); + ~PhysicsServer2DWrapMT(); + +#undef ServerNameWrapMT +#undef ServerName +#undef server_name +#undef WRITE_ACTION +}; + +#ifdef DEBUG_SYNC +#undef DEBUG_SYNC +#endif +#undef SYNC_DEBUG + +#endif // PHYSICS_SERVER_2D_WRAP_MT_H diff --git a/servers/physics_server_3d.h b/servers/physics_server_3d.h index c609afc11e..6f55e287c9 100644 --- a/servers/physics_server_3d.h +++ b/servers/physics_server_3d.h @@ -28,8 +28,8 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#ifndef PHYSICS_SERVER_H -#define PHYSICS_SERVER_H +#ifndef PHYSICS_SERVER_3D_H +#define PHYSICS_SERVER_3D_H #include "core/io/resource.h" #include "core/object/class_db.h" @@ -903,4 +903,4 @@ VARIANT_ENUM_CAST(PhysicsServer3D::G6DOFJointAxisFlag); VARIANT_ENUM_CAST(PhysicsServer3D::AreaBodyStatus); VARIANT_ENUM_CAST(PhysicsServer3D::ProcessInfo); -#endif +#endif // PHYSICS_SERVER_3D_H diff --git a/servers/physics_server_3d_wrap_mt.cpp b/servers/physics_server_3d_wrap_mt.cpp new file mode 100644 index 0000000000..c424100bba --- /dev/null +++ b/servers/physics_server_3d_wrap_mt.cpp @@ -0,0 +1,137 @@ +/*************************************************************************/ +/* physics_server_3d_wrap_mt.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* 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 "physics_server_3d_wrap_mt.h" + +#include "core/os/os.h" + +void PhysicsServer3DWrapMT::thread_exit() { + exit = true; +} + +void PhysicsServer3DWrapMT::thread_step(real_t p_delta) { + physics_3d_server->step(p_delta); + step_sem.post(); +} + +void PhysicsServer3DWrapMT::_thread_callback(void *_instance) { + PhysicsServer3DWrapMT *vsmt = reinterpret_cast(_instance); + + vsmt->thread_loop(); +} + +void PhysicsServer3DWrapMT::thread_loop() { + server_thread = Thread::get_caller_id(); + + physics_3d_server->init(); + + exit = false; + step_thread_up = true; + while (!exit) { + // flush commands one by one, until exit is requested + command_queue.wait_and_flush(); + } + + command_queue.flush_all(); // flush all + + physics_3d_server->finish(); +} + +/* EVENT QUEUING */ + +void PhysicsServer3DWrapMT::step(real_t p_step) { + if (create_thread) { + command_queue.push(this, &PhysicsServer3DWrapMT::thread_step, p_step); + } else { + command_queue.flush_all(); //flush all pending from other threads + physics_3d_server->step(p_step); + } +} + +void PhysicsServer3DWrapMT::sync() { + if (create_thread) { + if (first_frame) { + first_frame = false; + } else { + step_sem.wait(); //must not wait if a step was not issued + } + } + physics_3d_server->sync(); +} + +void PhysicsServer3DWrapMT::flush_queries() { + physics_3d_server->flush_queries(); +} + +void PhysicsServer3DWrapMT::end_sync() { + physics_3d_server->end_sync(); +} + +void PhysicsServer3DWrapMT::init() { + if (create_thread) { + //OS::get_singleton()->release_rendering_thread(); + thread.start(_thread_callback, this); + while (!step_thread_up) { + OS::get_singleton()->delay_usec(1000); + } + } else { + physics_3d_server->init(); + } +} + +void PhysicsServer3DWrapMT::finish() { + if (thread.is_started()) { + command_queue.push(this, &PhysicsServer3DWrapMT::thread_exit); + thread.wait_to_finish(); + } else { + physics_3d_server->finish(); + } +} + +PhysicsServer3DWrapMT::PhysicsServer3DWrapMT(PhysicsServer3D *p_contained, bool p_create_thread) : + command_queue(p_create_thread) { + physics_3d_server = p_contained; + create_thread = p_create_thread; + + pool_max_size = GLOBAL_GET("memory/limits/multithreaded_server/rid_pool_prealloc"); + + if (!p_create_thread) { + server_thread = Thread::get_caller_id(); + } else { + server_thread = 0; + } + + main_thread = Thread::get_caller_id(); +} + +PhysicsServer3DWrapMT::~PhysicsServer3DWrapMT() { + memdelete(physics_3d_server); + //finish(); +} diff --git a/servers/physics_server_3d_wrap_mt.h b/servers/physics_server_3d_wrap_mt.h new file mode 100644 index 0000000000..bf936fd0fc --- /dev/null +++ b/servers/physics_server_3d_wrap_mt.h @@ -0,0 +1,409 @@ +/*************************************************************************/ +/* physics_server_3d_wrap_mt.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* 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 PHYSICS_SERVER_3D_WRAP_MT_H +#define PHYSICS_SERVER_3D_WRAP_MT_H + +#include "core/config/project_settings.h" +#include "core/os/thread.h" +#include "core/templates/command_queue_mt.h" +#include "servers/physics_server_3d.h" + +#ifdef DEBUG_SYNC +#define SYNC_DEBUG print_line("sync on: " + String(__FUNCTION__)); +#else +#define SYNC_DEBUG +#endif + +class PhysicsServer3DWrapMT : public PhysicsServer3D { + mutable PhysicsServer3D *physics_3d_server; + + mutable CommandQueueMT command_queue; + + static void _thread_callback(void *_instance); + void thread_loop(); + + Thread::ID server_thread; + Thread::ID main_thread; + volatile bool exit = false; + Thread thread; + volatile bool step_thread_up = false; + bool create_thread = false; + + Semaphore step_sem; + int step_pending = 0; + void thread_step(real_t p_delta); + void thread_flush(); + + void thread_exit(); + + bool first_frame = true; + + Mutex alloc_mutex; + int pool_max_size = 0; + +public: +#define ServerName PhysicsServer3D +#define ServerNameWrapMT PhysicsServer3DWrapMT +#define server_name physics_3d_server +#define WRITE_ACTION + +#include "servers/server_wrap_mt_common.h" + + //FUNC1RID(shape,ShapeType); todo fix + FUNCRID(world_boundary_shape) + FUNCRID(separation_ray_shape) + FUNCRID(sphere_shape) + FUNCRID(box_shape) + FUNCRID(capsule_shape) + FUNCRID(cylinder_shape) + FUNCRID(convex_polygon_shape) + FUNCRID(concave_polygon_shape) + FUNCRID(heightmap_shape) + FUNCRID(custom_shape) + + FUNC2(shape_set_data, RID, const Variant &); + FUNC2(shape_set_custom_solver_bias, RID, real_t); + + FUNC2(shape_set_margin, RID, real_t) + FUNC1RC(real_t, shape_get_margin, RID) + + FUNC1RC(ShapeType, shape_get_type, RID); + FUNC1RC(Variant, shape_get_data, RID); + FUNC1RC(real_t, shape_get_custom_solver_bias, RID); +#if 0 + //these work well, but should be used from the main thread only + bool shape_collide(RID p_shape_A, const Transform &p_xform_A, const Vector3 &p_motion_A, RID p_shape_B, const Transform &p_xform_B, const Vector3 &p_motion_B, Vector3 *r_results, int p_result_max, int &r_result_count) { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); + return physics_3d_server->shape_collide(p_shape_A, p_xform_A, p_motion_A, p_shape_B, p_xform_B, p_motion_B, r_results, p_result_max, r_result_count); + } +#endif + /* SPACE API */ + + FUNCRID(space); + FUNC2(space_set_active, RID, bool); + FUNC1RC(bool, space_is_active, RID); + + FUNC3(space_set_param, RID, SpaceParameter, real_t); + FUNC2RC(real_t, space_get_param, RID, SpaceParameter); + + // this function only works on physics process, errors and returns null otherwise + PhysicsDirectSpaceState3D *space_get_direct_state(RID p_space) override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr); + return physics_3d_server->space_get_direct_state(p_space); + } + + FUNC2(space_set_debug_contacts, RID, int); + virtual Vector space_get_contacts(RID p_space) const override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), Vector()); + return physics_3d_server->space_get_contacts(p_space); + } + + virtual int space_get_contact_count(RID p_space) const override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), 0); + return physics_3d_server->space_get_contact_count(p_space); + } + + /* AREA API */ + + //FUNC0RID(area); + FUNCRID(area); + + FUNC2(area_set_space, RID, RID); + FUNC1RC(RID, area_get_space, RID); + + FUNC2(area_set_space_override_mode, RID, AreaSpaceOverrideMode); + FUNC1RC(AreaSpaceOverrideMode, area_get_space_override_mode, RID); + + FUNC4(area_add_shape, RID, RID, const Transform3D &, bool); + FUNC3(area_set_shape, RID, int, RID); + FUNC3(area_set_shape_transform, RID, int, const Transform3D &); + FUNC3(area_set_shape_disabled, RID, int, bool); + + FUNC1RC(int, area_get_shape_count, RID); + FUNC2RC(RID, area_get_shape, RID, int); + FUNC2RC(Transform3D, area_get_shape_transform, RID, int); + FUNC2(area_remove_shape, RID, int); + FUNC1(area_clear_shapes, RID); + + FUNC2(area_attach_object_instance_id, RID, ObjectID); + FUNC1RC(ObjectID, area_get_object_instance_id, RID); + + FUNC3(area_set_param, RID, AreaParameter, const Variant &); + FUNC2(area_set_transform, RID, const Transform3D &); + + FUNC2RC(Variant, area_get_param, RID, AreaParameter); + FUNC1RC(Transform3D, area_get_transform, RID); + + FUNC2(area_set_collision_mask, RID, uint32_t); + FUNC2(area_set_collision_layer, RID, uint32_t); + + FUNC2(area_set_monitorable, RID, bool); + FUNC2(area_set_ray_pickable, RID, bool); + + FUNC3(area_set_monitor_callback, RID, Object *, const StringName &); + FUNC3(area_set_area_monitor_callback, RID, Object *, const StringName &); + + /* BODY API */ + + //FUNC2RID(body,BodyMode,bool); + FUNCRID(body) + + FUNC2(body_set_space, RID, RID); + FUNC1RC(RID, body_get_space, RID); + + FUNC2(body_set_mode, RID, BodyMode); + FUNC1RC(BodyMode, body_get_mode, RID); + + FUNC4(body_add_shape, RID, RID, const Transform3D &, bool); + FUNC3(body_set_shape, RID, int, RID); + FUNC3(body_set_shape_transform, RID, int, const Transform3D &); + + FUNC1RC(int, body_get_shape_count, RID); + FUNC2RC(Transform3D, body_get_shape_transform, RID, int); + FUNC2RC(RID, body_get_shape, RID, int); + + FUNC3(body_set_shape_disabled, RID, int, bool); + + FUNC2(body_remove_shape, RID, int); + FUNC1(body_clear_shapes, RID); + + FUNC2(body_attach_object_instance_id, RID, ObjectID); + FUNC1RC(ObjectID, body_get_object_instance_id, RID); + + FUNC2(body_set_enable_continuous_collision_detection, RID, bool); + FUNC1RC(bool, body_is_continuous_collision_detection_enabled, RID); + + FUNC2(body_set_collision_layer, RID, uint32_t); + FUNC1RC(uint32_t, body_get_collision_layer, RID); + + FUNC2(body_set_collision_mask, RID, uint32_t); + FUNC1RC(uint32_t, body_get_collision_mask, RID); + + FUNC2(body_set_user_flags, RID, uint32_t); + FUNC1RC(uint32_t, body_get_user_flags, RID); + + FUNC3(body_set_param, RID, BodyParameter, const Variant &); + FUNC2RC(Variant, body_get_param, RID, BodyParameter); + + FUNC1(body_reset_mass_properties, RID); + + FUNC3(body_set_state, RID, BodyState, const Variant &); + FUNC2RC(Variant, body_get_state, RID, BodyState); + + FUNC2(body_set_applied_force, RID, const Vector3 &); + FUNC1RC(Vector3, body_get_applied_force, RID); + + FUNC2(body_set_applied_torque, RID, const Vector3 &); + FUNC1RC(Vector3, body_get_applied_torque, RID); + + FUNC2(body_add_central_force, RID, const Vector3 &); + FUNC3(body_add_force, RID, const Vector3 &, const Vector3 &); + FUNC2(body_add_torque, RID, const Vector3 &); + FUNC2(body_apply_torque_impulse, RID, const Vector3 &); + FUNC2(body_apply_central_impulse, RID, const Vector3 &); + FUNC3(body_apply_impulse, RID, const Vector3 &, const Vector3 &); + FUNC2(body_set_axis_velocity, RID, const Vector3 &); + + FUNC3(body_set_axis_lock, RID, BodyAxis, bool); + FUNC2RC(bool, body_is_axis_locked, RID, BodyAxis); + + FUNC2(body_add_collision_exception, RID, RID); + FUNC2(body_remove_collision_exception, RID, RID); + FUNC2S(body_get_collision_exceptions, RID, List *); + + FUNC2(body_set_max_contacts_reported, RID, int); + FUNC1RC(int, body_get_max_contacts_reported, RID); + + FUNC2(body_set_contacts_reported_depth_threshold, RID, real_t); + FUNC1RC(real_t, body_get_contacts_reported_depth_threshold, RID); + + FUNC2(body_set_omit_force_integration, RID, bool); + FUNC1RC(bool, body_is_omitting_force_integration, RID); + + FUNC3(body_set_state_sync_callback, RID, void *, BodyStateCallback); + FUNC3(body_set_force_integration_callback, RID, const Callable &, const Variant &); + + FUNC2(body_set_ray_pickable, RID, bool); + + bool body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result = nullptr) override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); + return physics_3d_server->body_test_motion(p_body, p_parameters, r_result); + } + + // this function only works on physics process, errors and returns null otherwise + PhysicsDirectBodyState3D *body_get_direct_state(RID p_body) override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr); + return physics_3d_server->body_get_direct_state(p_body); + } + + /* SOFT BODY API */ + + FUNCRID(soft_body) + + FUNC2(soft_body_update_rendering_server, RID, class RenderingServerHandler *) + + FUNC2(soft_body_set_space, RID, RID) + FUNC1RC(RID, soft_body_get_space, RID) + + FUNC2(soft_body_set_ray_pickable, RID, bool); + + FUNC2(soft_body_set_collision_layer, RID, uint32_t) + FUNC1RC(uint32_t, soft_body_get_collision_layer, RID) + + FUNC2(soft_body_set_collision_mask, RID, uint32_t) + FUNC1RC(uint32_t, soft_body_get_collision_mask, RID) + + FUNC2(soft_body_add_collision_exception, RID, RID) + FUNC2(soft_body_remove_collision_exception, RID, RID) + FUNC2S(soft_body_get_collision_exceptions, RID, List *) + + FUNC3(soft_body_set_state, RID, BodyState, const Variant &); + FUNC2RC(Variant, soft_body_get_state, RID, BodyState); + + FUNC2(soft_body_set_transform, RID, const Transform3D &); + + FUNC2(soft_body_set_simulation_precision, RID, int); + FUNC1RC(int, soft_body_get_simulation_precision, RID); + + FUNC2(soft_body_set_total_mass, RID, real_t); + FUNC1RC(real_t, soft_body_get_total_mass, RID); + + FUNC2(soft_body_set_linear_stiffness, RID, real_t); + FUNC1RC(real_t, soft_body_get_linear_stiffness, RID); + + FUNC2(soft_body_set_pressure_coefficient, RID, real_t); + FUNC1RC(real_t, soft_body_get_pressure_coefficient, RID); + + FUNC2(soft_body_set_damping_coefficient, RID, real_t); + FUNC1RC(real_t, soft_body_get_damping_coefficient, RID); + + FUNC2(soft_body_set_drag_coefficient, RID, real_t); + FUNC1RC(real_t, soft_body_get_drag_coefficient, RID); + + FUNC2(soft_body_set_mesh, RID, RID); + + FUNC1RC(AABB, soft_body_get_bounds, RID); + + FUNC3(soft_body_move_point, RID, int, const Vector3 &); + FUNC2RC(Vector3, soft_body_get_point_global_position, RID, int); + + FUNC1(soft_body_remove_all_pinned_points, RID); + FUNC3(soft_body_pin_point, RID, int, bool); + FUNC2RC(bool, soft_body_is_point_pinned, RID, int); + + /* JOINT API */ + + FUNCRID(joint) + + FUNC1(joint_clear, RID) + + FUNC5(joint_make_pin, RID, RID, const Vector3 &, RID, const Vector3 &) + + FUNC3(pin_joint_set_param, RID, PinJointParam, real_t) + FUNC2RC(real_t, pin_joint_get_param, RID, PinJointParam) + + FUNC2(pin_joint_set_local_a, RID, const Vector3 &) + FUNC1RC(Vector3, pin_joint_get_local_a, RID) + + FUNC2(pin_joint_set_local_b, RID, const Vector3 &) + FUNC1RC(Vector3, pin_joint_get_local_b, RID) + + FUNC5(joint_make_hinge, RID, RID, const Transform3D &, RID, const Transform3D &) + FUNC7(joint_make_hinge_simple, RID, RID, const Vector3 &, const Vector3 &, RID, const Vector3 &, const Vector3 &) + + FUNC3(hinge_joint_set_param, RID, HingeJointParam, real_t) + FUNC2RC(real_t, hinge_joint_get_param, RID, HingeJointParam) + + FUNC3(hinge_joint_set_flag, RID, HingeJointFlag, bool) + FUNC2RC(bool, hinge_joint_get_flag, RID, HingeJointFlag) + + FUNC5(joint_make_slider, RID, RID, const Transform3D &, RID, const Transform3D &) + + FUNC3(slider_joint_set_param, RID, SliderJointParam, real_t) + FUNC2RC(real_t, slider_joint_get_param, RID, SliderJointParam) + + FUNC5(joint_make_cone_twist, RID, RID, const Transform3D &, RID, const Transform3D &) + + FUNC3(cone_twist_joint_set_param, RID, ConeTwistJointParam, real_t) + FUNC2RC(real_t, cone_twist_joint_get_param, RID, ConeTwistJointParam) + + FUNC5(joint_make_generic_6dof, RID, RID, const Transform3D &, RID, const Transform3D &) + + FUNC4(generic_6dof_joint_set_param, RID, Vector3::Axis, G6DOFJointAxisParam, real_t) + FUNC3RC(real_t, generic_6dof_joint_get_param, RID, Vector3::Axis, G6DOFJointAxisParam) + + FUNC4(generic_6dof_joint_set_flag, RID, Vector3::Axis, G6DOFJointAxisFlag, bool) + FUNC3RC(bool, generic_6dof_joint_get_flag, RID, Vector3::Axis, G6DOFJointAxisFlag) + + FUNC1RC(JointType, joint_get_type, RID); + + FUNC2(joint_set_solver_priority, RID, int); + FUNC1RC(int, joint_get_solver_priority, RID); + + FUNC2(joint_disable_collisions_between_bodies, RID, const bool); + FUNC1RC(bool, joint_is_disabled_collisions_between_bodies, RID); + + /* MISC */ + + FUNC1(free, RID); + FUNC1(set_active, bool); + FUNC1(set_collision_iterations, int); + + virtual void init() override; + virtual void step(real_t p_step) override; + virtual void sync() override; + virtual void end_sync() override; + virtual void flush_queries() override; + virtual void finish() override; + + virtual bool is_flushing_queries() const override { + return physics_3d_server->is_flushing_queries(); + } + + int get_process_info(ProcessInfo p_info) override { + return physics_3d_server->get_process_info(p_info); + } + + PhysicsServer3DWrapMT(PhysicsServer3D *p_contained, bool p_create_thread); + ~PhysicsServer3DWrapMT(); + +#undef ServerNameWrapMT +#undef ServerName +#undef server_name +#undef WRITE_ACTION +}; + +#ifdef DEBUG_SYNC +#undef DEBUG_SYNC +#endif +#undef SYNC_DEBUG + +#endif // PHYSICS_SERVER_3D_WRAP_MT_H diff --git a/servers/register_server_types.cpp b/servers/register_server_types.cpp index 28cd8374c0..de64773026 100644 --- a/servers/register_server_types.cpp +++ b/servers/register_server_types.cpp @@ -59,12 +59,12 @@ #include "display_server.h" #include "navigation_server_2d.h" #include "navigation_server_3d.h" -#include "physics_2d/physics_server_2d_sw.h" -#include "physics_2d/physics_server_2d_wrap_mt.h" -#include "physics_3d/physics_server_3d_sw.h" -#include "physics_3d/physics_server_3d_wrap_mt.h" +#include "physics_2d/godot_physics_server_2d.h" +#include "physics_3d/godot_physics_server_3d.h" #include "physics_server_2d.h" +#include "physics_server_2d_wrap_mt.h" #include "physics_server_3d.h" +#include "physics_server_3d_wrap_mt.h" #include "rendering/renderer_compositor.h" #include "rendering/rendering_device.h" #include "rendering/rendering_device_binds.h" @@ -82,7 +82,7 @@ ShaderTypes *shader_types = nullptr; PhysicsServer3D *_createGodotPhysics3DCallback() { bool using_threads = GLOBAL_GET("physics/3d/run_on_thread"); - PhysicsServer3D *physics_server = memnew(PhysicsServer3DSW(using_threads)); + PhysicsServer3D *physics_server = memnew(GodotPhysicsServer3D(using_threads)); return memnew(PhysicsServer3DWrapMT(physics_server, using_threads)); } @@ -90,7 +90,7 @@ PhysicsServer3D *_createGodotPhysics3DCallback() { PhysicsServer2D *_createGodotPhysics2DCallback() { bool using_threads = GLOBAL_GET("physics/2d/run_on_thread"); - PhysicsServer2D *physics_server = memnew(PhysicsServer2DSW(using_threads)); + PhysicsServer2D *physics_server = memnew(GodotPhysicsServer2D(using_threads)); return memnew(PhysicsServer2DWrapMT(physics_server, using_threads)); } -- cgit v1.2.3