From 8adb27eb0fb41f67606deed20570d76fa5b4eb17 Mon Sep 17 00:00:00 2001 From: Relintai Date: Fri, 15 Dec 2023 09:24:57 +0100 Subject: [PATCH] Removed PhysicsServer. --- main/main.cpp | 18 - main/performance.cpp | 16 - main/performance.h | 3 - ...icbody3d_navigation_geometry_parser_3d.cpp | 188 -- ...aticbody3d_navigation_geometry_parser_3d.h | 43 - .../register_types.cpp | 2 - scene/3d/area.cpp | 644 ----- scene/3d/area.h | 201 -- scene/3d/audio_stream_player_3d.cpp | 277 +- scene/3d/camera.cpp | 213 -- scene/3d/camera.h | 58 - scene/3d/collision_object.cpp | 593 ----- scene/3d/collision_object.h | 148 -- scene/3d/collision_polygon.cpp | 221 -- scene/3d/collision_polygon.h | 83 - scene/3d/collision_shape.cpp | 227 -- scene/3d/collision_shape.h | 77 - scene/3d/mesh_instance.cpp | 601 +---- scene/3d/mesh_instance.h | 57 - scene/3d/navigation_obstacle.cpp | 609 ----- scene/3d/navigation_obstacle.h | 132 - scene/3d/physics_body.cpp | 1624 ------------ scene/3d/physics_body.h | 386 --- scene/3d/physics_joint.cpp | 1079 -------- scene/3d/physics_joint.h | 336 --- scene/3d/ray_cast.cpp | 526 ---- scene/3d/ray_cast.h | 124 - scene/3d/shape_cast.cpp | 638 ----- scene/3d/shape_cast.h | 148 -- scene/3d/soft_body.cpp | 856 ------ scene/3d/soft_body.h | 204 -- scene/3d/spring_arm.cpp | 174 -- scene/3d/spring_arm.h | 73 - scene/3d/vehicle_body.cpp | 965 ------- scene/3d/vehicle_body.h | 211 -- scene/3d/visibility_notifier.cpp | 17 - scene/main/scene_tree.cpp | 3 - scene/main/viewport.cpp | 146 -- scene/main/viewport.h | 3 - scene/register_scene_types.cpp | 64 +- scene/resources/mesh/importer_mesh.cpp | 21 - scene/resources/mesh/importer_mesh.h | 3 - scene/resources/mesh/mesh.cpp | 104 - scene/resources/mesh/mesh.h | 6 - scene/resources/physics_material.h | 1 - scene/resources/shapes/box_shape.cpp | 80 - scene/resources/shapes/box_shape.h | 54 - scene/resources/shapes/capsule_shape.cpp | 119 - scene/resources/shapes/capsule_shape.h | 57 - .../shapes/concave_polygon_shape.cpp | 96 - .../resources/shapes/concave_polygon_shape.h | 73 - .../resources/shapes/convex_polygon_shape.cpp | 90 - scene/resources/shapes/convex_polygon_shape.h | 54 - scene/resources/shapes/cylinder_shape.cpp | 112 - scene/resources/shapes/cylinder_shape.h | 56 - scene/resources/shapes/height_map_shape.cpp | 214 -- scene/resources/shapes/height_map_shape.h | 62 - scene/resources/shapes/plane_shape.cpp | 89 - scene/resources/shapes/plane_shape.h | 55 - scene/resources/shapes/ray_shape.cpp | 98 - scene/resources/shapes/ray_shape.h | 56 - scene/resources/shapes/shape.cpp | 122 - scene/resources/shapes/shape.h | 70 - scene/resources/shapes/sphere_shape.cpp | 87 - scene/resources/shapes/sphere_shape.h | 54 - scene/resources/world_3d.cpp | 16 +- scene/resources/world_3d.h | 3 - servers/physics/SCsub | 7 - servers/physics/area_pair_sw.cpp | 155 -- servers/physics/area_pair_sw.h | 69 - servers/physics/area_sw.cpp | 296 --- servers/physics/area_sw.h | 198 -- servers/physics/body_pair_sw.cpp | 468 ---- servers/physics/body_pair_sw.h | 95 - servers/physics/body_sw.cpp | 814 ------ servers/physics/body_sw.h | 494 ---- servers/physics/broad_phase_basic.cpp | 212 -- servers/physics/broad_phase_basic.h | 105 - servers/physics/broad_phase_bvh.cpp | 141 - servers/physics/broad_phase_bvh.h | 100 - servers/physics/broad_phase_octree.cpp | 128 - servers/physics/broad_phase_octree.h | 72 - servers/physics/broad_phase_sw.cpp | 36 - servers/physics/broad_phase_sw.h | 72 - servers/physics/collision_object_sw.cpp | 262 -- servers/physics/collision_object_sw.h | 184 -- servers/physics/collision_solver_sat.cpp | 2313 ----------------- servers/physics/collision_solver_sat.h | 37 - servers/physics/collision_solver_sw.cpp | 405 --- servers/physics/collision_solver_sw.h | 52 - servers/physics/constraint_sw.h | 83 - servers/physics/gjk_epa.cpp | 1035 -------- servers/physics/gjk_epa.h | 39 - servers/physics/joints/SCsub | 5 - .../physics/joints/cone_twist_joint_sw.cpp | 356 --- servers/physics/joints/cone_twist_joint_sw.h | 141 - .../physics/joints/generic_6dof_joint_sw.cpp | 663 ----- .../physics/joints/generic_6dof_joint_sw.h | 364 --- servers/physics/joints/hinge_joint_sw.cpp | 487 ---- servers/physics/joints/hinge_joint_sw.h | 118 - servers/physics/joints/jacobian_entry_sw.h | 168 -- servers/physics/joints/pin_joint_sw.cpp | 170 -- servers/physics/joints/pin_joint_sw.h | 94 - servers/physics/joints/slider_joint_sw.cpp | 513 ---- servers/physics/joints/slider_joint_sw.h | 251 -- servers/physics/joints_sw.h | 44 - servers/physics/physics_server_sw.cpp | 1451 ----------- servers/physics/physics_server_sw.h | 372 --- servers/physics/shape_sw.cpp | 2210 ---------------- servers/physics/shape_sw.h | 528 ---- servers/physics/space_sw.cpp | 1328 ---------- servers/physics/space_sw.h | 210 -- servers/physics/step_sw.cpp | 290 --- servers/physics/step_sw.h | 48 - servers/physics_server.cpp | 870 ------- servers/physics_server.h | 842 ------ servers/register_server_types.cpp | 20 - 117 files changed, 20 insertions(+), 33235 deletions(-) delete mode 100644 modules/navigation_geometry_parsers/geometry_parser_3d/staticbody3d_navigation_geometry_parser_3d.cpp delete mode 100644 modules/navigation_geometry_parsers/geometry_parser_3d/staticbody3d_navigation_geometry_parser_3d.h delete mode 100644 scene/3d/area.cpp delete mode 100644 scene/3d/area.h delete mode 100644 scene/3d/collision_object.cpp delete mode 100644 scene/3d/collision_object.h delete mode 100644 scene/3d/collision_polygon.cpp delete mode 100644 scene/3d/collision_polygon.h delete mode 100644 scene/3d/collision_shape.cpp delete mode 100644 scene/3d/collision_shape.h delete mode 100644 scene/3d/navigation_obstacle.cpp delete mode 100644 scene/3d/navigation_obstacle.h delete mode 100644 scene/3d/physics_body.cpp delete mode 100644 scene/3d/physics_body.h delete mode 100644 scene/3d/physics_joint.cpp delete mode 100644 scene/3d/physics_joint.h delete mode 100644 scene/3d/ray_cast.cpp delete mode 100644 scene/3d/ray_cast.h delete mode 100644 scene/3d/shape_cast.cpp delete mode 100644 scene/3d/shape_cast.h delete mode 100644 scene/3d/soft_body.cpp delete mode 100644 scene/3d/soft_body.h delete mode 100644 scene/3d/spring_arm.cpp delete mode 100644 scene/3d/spring_arm.h delete mode 100644 scene/3d/vehicle_body.cpp delete mode 100644 scene/3d/vehicle_body.h delete mode 100644 scene/resources/shapes/box_shape.cpp delete mode 100644 scene/resources/shapes/box_shape.h delete mode 100644 scene/resources/shapes/capsule_shape.cpp delete mode 100644 scene/resources/shapes/capsule_shape.h delete mode 100644 scene/resources/shapes/concave_polygon_shape.cpp delete mode 100644 scene/resources/shapes/concave_polygon_shape.h delete mode 100644 scene/resources/shapes/convex_polygon_shape.cpp delete mode 100644 scene/resources/shapes/convex_polygon_shape.h delete mode 100644 scene/resources/shapes/cylinder_shape.cpp delete mode 100644 scene/resources/shapes/cylinder_shape.h delete mode 100644 scene/resources/shapes/height_map_shape.cpp delete mode 100644 scene/resources/shapes/height_map_shape.h delete mode 100644 scene/resources/shapes/plane_shape.cpp delete mode 100644 scene/resources/shapes/plane_shape.h delete mode 100644 scene/resources/shapes/ray_shape.cpp delete mode 100644 scene/resources/shapes/ray_shape.h delete mode 100644 scene/resources/shapes/shape.cpp delete mode 100644 scene/resources/shapes/shape.h delete mode 100644 scene/resources/shapes/sphere_shape.cpp delete mode 100644 scene/resources/shapes/sphere_shape.h delete mode 100644 servers/physics/SCsub delete mode 100644 servers/physics/area_pair_sw.cpp delete mode 100644 servers/physics/area_pair_sw.h delete mode 100644 servers/physics/area_sw.cpp delete mode 100644 servers/physics/area_sw.h delete mode 100644 servers/physics/body_pair_sw.cpp delete mode 100644 servers/physics/body_pair_sw.h delete mode 100644 servers/physics/body_sw.cpp delete mode 100644 servers/physics/body_sw.h delete mode 100644 servers/physics/broad_phase_basic.cpp delete mode 100644 servers/physics/broad_phase_basic.h delete mode 100644 servers/physics/broad_phase_bvh.cpp delete mode 100644 servers/physics/broad_phase_bvh.h delete mode 100644 servers/physics/broad_phase_octree.cpp delete mode 100644 servers/physics/broad_phase_octree.h delete mode 100644 servers/physics/broad_phase_sw.cpp delete mode 100644 servers/physics/broad_phase_sw.h delete mode 100644 servers/physics/collision_object_sw.cpp delete mode 100644 servers/physics/collision_object_sw.h delete mode 100644 servers/physics/collision_solver_sat.cpp delete mode 100644 servers/physics/collision_solver_sat.h delete mode 100644 servers/physics/collision_solver_sw.cpp delete mode 100644 servers/physics/collision_solver_sw.h delete mode 100644 servers/physics/constraint_sw.h delete mode 100644 servers/physics/gjk_epa.cpp delete mode 100644 servers/physics/gjk_epa.h delete mode 100644 servers/physics/joints/SCsub delete mode 100644 servers/physics/joints/cone_twist_joint_sw.cpp delete mode 100644 servers/physics/joints/cone_twist_joint_sw.h delete mode 100644 servers/physics/joints/generic_6dof_joint_sw.cpp delete mode 100644 servers/physics/joints/generic_6dof_joint_sw.h delete mode 100644 servers/physics/joints/hinge_joint_sw.cpp delete mode 100644 servers/physics/joints/hinge_joint_sw.h delete mode 100644 servers/physics/joints/jacobian_entry_sw.h delete mode 100644 servers/physics/joints/pin_joint_sw.cpp delete mode 100644 servers/physics/joints/pin_joint_sw.h delete mode 100644 servers/physics/joints/slider_joint_sw.cpp delete mode 100644 servers/physics/joints/slider_joint_sw.h delete mode 100644 servers/physics/joints_sw.h delete mode 100644 servers/physics/physics_server_sw.cpp delete mode 100644 servers/physics/physics_server_sw.h delete mode 100644 servers/physics/shape_sw.cpp delete mode 100644 servers/physics/shape_sw.h delete mode 100644 servers/physics/space_sw.cpp delete mode 100644 servers/physics/space_sw.h delete mode 100644 servers/physics/step_sw.cpp delete mode 100644 servers/physics/step_sw.h delete mode 100644 servers/physics_server.cpp delete mode 100644 servers/physics_server.h diff --git a/main/main.cpp b/main/main.cpp index e8bddca..23717eb 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -66,7 +66,6 @@ #include "servers/navigation_2d_server.h" #include "servers/navigation_server.h" #include "servers/physics_2d_server.h" -#include "servers/physics_server.h" #include "servers/register_server_types.h" #include "servers/rendering_server_callbacks.h" @@ -111,7 +110,6 @@ static MessageQueue *message_queue = nullptr; // Initialized in setup2() static AudioServer *audio_server = nullptr; -static PhysicsServer *physics_server = nullptr; static Physics2DServer *physics_2d_server = nullptr; static RenderingServerCallbacks *rendering_server_callbacks = nullptr; static NavigationMeshGeneratorManager *navigation_mesh_generator_manager = nullptr; @@ -197,15 +195,6 @@ void initialize_physics() { GLOBAL_DEF("physics/3d/pandemonium_physics/bvh_collision_margin", 0.1); ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/pandemonium_physics/bvh_collision_margin", PropertyInfo(Variant::REAL, "physics/3d/pandemonium_physics/bvh_collision_margin", PROPERTY_HINT_RANGE, "0.0,2.0,0.01")); - /// 3D Physics Server - physics_server = PhysicsServerManager::new_server(ProjectSettings::get_singleton()->get(PhysicsServerManager::setting_property_name)); - if (!physics_server) { - // Physics server not found, Use the default physics - physics_server = PhysicsServerManager::new_default_server(); - } - ERR_FAIL_COND(!physics_server); - physics_server->init(); - /// 2D Physics server physics_2d_server = Physics2DServerManager::new_server(ProjectSettings::get_singleton()->get(Physics2DServerManager::setting_property_name)); if (!physics_2d_server) { @@ -217,9 +206,6 @@ void initialize_physics() { } void finalize_physics() { - physics_server->finish(); - memdelete(physics_server); - physics_2d_server->finish(); memdelete(physics_2d_server); } @@ -2286,8 +2272,6 @@ bool Main::iteration() { uint64_t physics_begin = OS::get_singleton()->get_ticks_usec(); - PhysicsServer::get_singleton()->flush_queries(); - // Prepare the fixed timestep interpolated nodes // BEFORE they are updated by the physics 2D, // otherwise the current and previous transforms @@ -2312,8 +2296,6 @@ bool Main::iteration() { message_queue->flush(); - PhysicsServer::get_singleton()->step(frame_slice * time_scale); - Physics2DServer::get_singleton()->end_sync(); Physics2DServer::get_singleton()->step(frame_slice * time_scale); diff --git a/main/performance.cpp b/main/performance.cpp index 5590315..1ff1b43 100644 --- a/main/performance.cpp +++ b/main/performance.cpp @@ -37,7 +37,6 @@ #include "servers/audio_server.h" #include "servers/navigation_server.h" #include "servers/physics_2d_server.h" -#include "servers/physics_server.h" #include "servers/rendering_server.h" Performance *Performance::singleton = nullptr; @@ -73,9 +72,6 @@ void Performance::_bind_methods() { BIND_ENUM_CONSTANT(PHYSICS_2D_ACTIVE_OBJECTS); BIND_ENUM_CONSTANT(PHYSICS_2D_COLLISION_PAIRS); BIND_ENUM_CONSTANT(PHYSICS_2D_ISLAND_COUNT); - BIND_ENUM_CONSTANT(PHYSICS_3D_ACTIVE_OBJECTS); - BIND_ENUM_CONSTANT(PHYSICS_3D_COLLISION_PAIRS); - BIND_ENUM_CONSTANT(PHYSICS_3D_ISLAND_COUNT); BIND_ENUM_CONSTANT(AUDIO_OUTPUT_LATENCY); BIND_ENUM_CONSTANT(NAVIGATION_ACTIVE_MAPS); BIND_ENUM_CONSTANT(NAVIGATION_REGION_COUNT); @@ -131,9 +127,6 @@ String Performance::get_monitor_name(Monitor p_monitor) const { "physics_2d/active_objects", "physics_2d/collision_pairs", "physics_2d/islands", - "physics_3d/active_objects", - "physics_3d/collision_pairs", - "physics_3d/islands", "audio/output_latency", "navigation/active_maps", "navigation/regions", @@ -208,12 +201,6 @@ float Performance::get_monitor(Monitor p_monitor) const { return Physics2DServer::get_singleton()->get_process_info(Physics2DServer::INFO_COLLISION_PAIRS); case PHYSICS_2D_ISLAND_COUNT: return Physics2DServer::get_singleton()->get_process_info(Physics2DServer::INFO_ISLAND_COUNT); - case PHYSICS_3D_ACTIVE_OBJECTS: - return PhysicsServer::get_singleton()->get_process_info(PhysicsServer::INFO_ACTIVE_OBJECTS); - case PHYSICS_3D_COLLISION_PAIRS: - return PhysicsServer::get_singleton()->get_process_info(PhysicsServer::INFO_COLLISION_PAIRS); - case PHYSICS_3D_ISLAND_COUNT: - return PhysicsServer::get_singleton()->get_process_info(PhysicsServer::INFO_ISLAND_COUNT); case AUDIO_OUTPUT_LATENCY: return AudioServer::get_singleton()->get_output_latency(); case NAVIGATION_ACTIVE_MAPS: @@ -275,9 +262,6 @@ Performance::MonitorType Performance::get_monitor_type(Monitor p_monitor) const MONITOR_TYPE_QUANTITY, MONITOR_TYPE_QUANTITY, MONITOR_TYPE_QUANTITY, - MONITOR_TYPE_QUANTITY, - MONITOR_TYPE_QUANTITY, - MONITOR_TYPE_QUANTITY, MONITOR_TYPE_TIME, MONITOR_TYPE_QUANTITY, MONITOR_TYPE_QUANTITY, diff --git a/main/performance.h b/main/performance.h index 2e1603d..d1da7d2 100644 --- a/main/performance.h +++ b/main/performance.h @@ -78,9 +78,6 @@ public: PHYSICS_2D_ACTIVE_OBJECTS, PHYSICS_2D_COLLISION_PAIRS, PHYSICS_2D_ISLAND_COUNT, - PHYSICS_3D_ACTIVE_OBJECTS, - PHYSICS_3D_COLLISION_PAIRS, - PHYSICS_3D_ISLAND_COUNT, AUDIO_OUTPUT_LATENCY, NAVIGATION_ACTIVE_MAPS, NAVIGATION_REGION_COUNT, diff --git a/modules/navigation_geometry_parsers/geometry_parser_3d/staticbody3d_navigation_geometry_parser_3d.cpp b/modules/navigation_geometry_parsers/geometry_parser_3d/staticbody3d_navigation_geometry_parser_3d.cpp deleted file mode 100644 index 4d272aa..0000000 --- a/modules/navigation_geometry_parsers/geometry_parser_3d/staticbody3d_navigation_geometry_parser_3d.cpp +++ /dev/null @@ -1,188 +0,0 @@ -/**************************************************************************/ -/* staticbody3d_navigation_geometry_parser_3d.cpp */ -/**************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/**************************************************************************/ -/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/**************************************************************************/ - -#include "staticbody3d_navigation_geometry_parser_3d.h" - -#include "core/math/convex_hull.h" -#include "scene/3d/mesh_instance.h" -#include "scene/3d/physics_body.h" -#include "scene/resources/shapes/box_shape.h" -#include "scene/resources/shapes/capsule_shape.h" -#include "scene/resources/shapes/concave_polygon_shape.h" -#include "scene/resources/shapes/convex_polygon_shape.h" -#include "scene/resources/shapes/cylinder_shape.h" -#include "scene/resources/shapes/height_map_shape.h" -#include "scene/resources/shapes/plane_shape.h" -#include "scene/resources/mesh/primitive_meshes.h" -#include "scene/resources/shapes/shape.h" -#include "scene/resources/shapes/sphere_shape.h" - -#include "scene/resources/navigation/navigation_mesh.h" -#include "scene/resources/navigation/navigation_mesh_source_geometry_data_3d.h" - -bool StaticBody3DNavigationGeometryParser3D::parses_node(Node *p_node) { - return (Object::cast_to(p_node) != nullptr); -} - -void StaticBody3DNavigationGeometryParser3D::parse_geometry(Node *p_node, Ref p_navigationmesh, Ref p_source_geometry) { - NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigationmesh->get_parsed_geometry_type(); - uint32_t navigationmesh_collision_mask = p_navigationmesh->get_collision_mask(); - - if (Object::cast_to(p_node) && parsed_geometry_type != NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES) { - StaticBody *static_body = Object::cast_to(p_node); - if (static_body->get_collision_layer() & navigationmesh_collision_mask) { - List shape_owners; - static_body->get_shape_owners(&shape_owners); - - for (List::Element *E = shape_owners.front(); E; E = E->next()) { - uint32_t shape_owner = E->get(); - - if (static_body->is_shape_owner_disabled(shape_owner)) { - continue; - } - - const int shape_count = static_body->shape_owner_get_shape_count(shape_owner); - for (int shape_index = 0; shape_index < shape_count; shape_index++) { - Ref s = static_body->shape_owner_get_shape(shape_owner, shape_index); - if (s.is_null()) { - continue; - } - - const Transform transform = static_body->get_global_transform() * static_body->shape_owner_get_transform(shape_owner); - - BoxShape *box = Object::cast_to(*s); - if (box) { - Array arr; - arr.resize(RS::ARRAY_MAX); - CubeMesh::create_mesh_array(arr, box->get_extents()); - p_source_geometry->add_mesh_array(arr, transform); - } - - CapsuleShape *capsule = Object::cast_to(*s); - if (capsule) { - Array arr; - arr.resize(RS::ARRAY_MAX); - CapsuleMesh::create_mesh_array(arr, capsule->get_radius(), capsule->get_height()); - p_source_geometry->add_mesh_array(arr, transform); - } - - CylinderShape *cylinder = Object::cast_to(*s); - if (cylinder) { - Array arr; - arr.resize(RS::ARRAY_MAX); - CylinderMesh::create_mesh_array(arr, cylinder->get_radius(), cylinder->get_radius(), cylinder->get_height()); - p_source_geometry->add_mesh_array(arr, transform); - } - - SphereShape *sphere = Object::cast_to(*s); - if (sphere) { - Array arr; - arr.resize(RS::ARRAY_MAX); - SphereMesh::create_mesh_array(arr, sphere->get_radius(), sphere->get_radius() * 2.0); - p_source_geometry->add_mesh_array(arr, transform); - } - - ConcavePolygonShape *concave_polygon = Object::cast_to(*s); - if (concave_polygon) { - p_source_geometry->add_faces(concave_polygon->get_faces(), transform); - } - - ConvexPolygonShape *convex_polygon = Object::cast_to(*s); - if (convex_polygon) { - Vector varr = Variant(convex_polygon->get_points()); - Geometry::MeshData md; - - Error err = ConvexHullComputer::convex_hull(varr, md); - - if (err == OK) { - PoolVector3Array faces; - - for (int i = 0; i < md.faces.size(); ++i) { - const Geometry::MeshData::Face &face = md.faces[i]; - - for (int k = 2; k < face.indices.size(); ++k) { - faces.push_back(md.vertices[face.indices[0]]); - faces.push_back(md.vertices[face.indices[k - 1]]); - faces.push_back(md.vertices[face.indices[k]]); - } - } - - p_source_geometry->add_faces(faces, transform); - } - } - - HeightMapShape *heightmap_shape = Object::cast_to(*s); - if (heightmap_shape) { - int heightmap_depth = heightmap_shape->get_map_depth(); - int heightmap_width = heightmap_shape->get_map_width(); - - if (heightmap_depth >= 2 && heightmap_width >= 2) { - const PoolVector &map_data = heightmap_shape->get_map_data(); - - Vector2 heightmap_gridsize(heightmap_width - 1, heightmap_depth - 1); - Vector2 start = heightmap_gridsize * -0.5; - - PoolVector vertex_array; - vertex_array.resize((heightmap_depth - 1) * (heightmap_width - 1) * 6); - int map_data_current_index = 0; - - for (int d = 0; d < heightmap_depth; d++) { - for (int w = 0; w < heightmap_width; w++) { - if (map_data_current_index + 1 + heightmap_depth < map_data.size()) { - float top_left_height = map_data[map_data_current_index]; - float top_right_height = map_data[map_data_current_index + 1]; - float bottom_left_height = map_data[map_data_current_index + heightmap_depth]; - float bottom_right_height = map_data[map_data_current_index + 1 + heightmap_depth]; - - Vector3 top_left = Vector3(start.x + w, top_left_height, start.y + d); - Vector3 top_right = Vector3(start.x + w + 1.0, top_right_height, start.y + d); - Vector3 bottom_left = Vector3(start.x + w, bottom_left_height, start.y + d + 1.0); - Vector3 bottom_right = Vector3(start.x + w + 1.0, bottom_right_height, start.y + d + 1.0); - - vertex_array.push_back(top_right); - vertex_array.push_back(bottom_left); - vertex_array.push_back(top_left); - vertex_array.push_back(top_right); - vertex_array.push_back(bottom_right); - vertex_array.push_back(bottom_left); - } - map_data_current_index += 1; - } - } - if (vertex_array.size() > 0) { - p_source_geometry->add_faces(vertex_array, transform); - } - } - } - } - } - } - } -} diff --git a/modules/navigation_geometry_parsers/geometry_parser_3d/staticbody3d_navigation_geometry_parser_3d.h b/modules/navigation_geometry_parsers/geometry_parser_3d/staticbody3d_navigation_geometry_parser_3d.h deleted file mode 100644 index 4c3e524..0000000 --- a/modules/navigation_geometry_parsers/geometry_parser_3d/staticbody3d_navigation_geometry_parser_3d.h +++ /dev/null @@ -1,43 +0,0 @@ -/**************************************************************************/ -/* staticbody3d_navigation_geometry_parser_3d.h */ -/**************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/**************************************************************************/ -/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/**************************************************************************/ - -#ifndef STATICBODY3D_NAVIGATION_GEOMETRY_PARSER_3D_H -#define STATICBODY3D_NAVIGATION_GEOMETRY_PARSER_3D_H - -#include "scene/3d/navigation_geometry_parser_3d.h" - -class StaticBody3DNavigationGeometryParser3D : public NavigationGeometryParser3D { -public: - virtual bool parses_node(Node *p_node) ; - - virtual void parse_geometry(Node *p_node, Ref p_navigationmesh, Ref p_source_geometry) ; -}; - -#endif // STATICBODY3D_NAVIGATION_GEOMETRY_PARSER_3D_H diff --git a/modules/navigation_geometry_parsers/register_types.cpp b/modules/navigation_geometry_parsers/register_types.cpp index 6e38401..7a4dd96 100644 --- a/modules/navigation_geometry_parsers/register_types.cpp +++ b/modules/navigation_geometry_parsers/register_types.cpp @@ -42,7 +42,6 @@ #ifndef _3D_DISABLED #include "geometry_parser_3d/meshinstance3d_navigation_geometry_parser_3d.h" #include "geometry_parser_3d/multimeshinstance3d_navigation_geometry_parser_3d.h" -#include "geometry_parser_3d/staticbody3d_navigation_geometry_parser_3d.h" #endif // _3D_DISABLED void register_navigation_geometry_parsers_types(ModuleRegistrationLevel p_level) { @@ -54,7 +53,6 @@ void register_navigation_geometry_parsers_types(ModuleRegistrationLevel p_level) #ifndef _3D_DISABLED NavigationMeshGenerator::get_singleton()->register_geometry_parser_3d(memnew(MeshInstance3DNavigationGeometryParser3D)); NavigationMeshGenerator::get_singleton()->register_geometry_parser_3d(memnew(MultiMeshInstance3DNavigationGeometryParser3D)); - NavigationMeshGenerator::get_singleton()->register_geometry_parser_3d(memnew(StaticBody3DNavigationGeometryParser3D)); #endif // _3D_DISABLED } } diff --git a/scene/3d/area.cpp b/scene/3d/area.cpp deleted file mode 100644 index c594b74..0000000 --- a/scene/3d/area.cpp +++ /dev/null @@ -1,644 +0,0 @@ -/*************************************************************************/ -/* area.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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.h" -#include "scene/resources/shapes/shape.h" -#include "scene/main/scene_string_names.h" -#include "servers/audio_server.h" -#include "servers/physics_server.h" - -void Area::set_space_override_mode(SpaceOverride p_mode) { - space_override = p_mode; - PhysicsServer::get_singleton()->area_set_space_override_mode(get_rid(), PhysicsServer::AreaSpaceOverrideMode(p_mode)); -} -Area::SpaceOverride Area::get_space_override_mode() const { - return space_override; -} - -void Area::set_gravity_is_point(bool p_enabled) { - gravity_is_point = p_enabled; - PhysicsServer::get_singleton()->area_set_param(get_rid(), PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT, p_enabled); -} -bool Area::is_gravity_a_point() const { - return gravity_is_point; -} - -void Area::set_gravity_distance_scale(real_t p_scale) { - gravity_distance_scale = p_scale; - PhysicsServer::get_singleton()->area_set_param(get_rid(), PhysicsServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE, p_scale); -} - -real_t Area::get_gravity_distance_scale() const { - return gravity_distance_scale; -} - -void Area::set_gravity_vector(const Vector3 &p_vec) { - gravity_vec = p_vec; - PhysicsServer::get_singleton()->area_set_param(get_rid(), PhysicsServer::AREA_PARAM_GRAVITY_VECTOR, p_vec); -} -Vector3 Area::get_gravity_vector() const { - return gravity_vec; -} - -void Area::set_gravity(real_t p_gravity) { - gravity = p_gravity; - PhysicsServer::get_singleton()->area_set_param(get_rid(), PhysicsServer::AREA_PARAM_GRAVITY, p_gravity); -} -real_t Area::get_gravity() const { - return gravity; -} -void Area::set_linear_damp(real_t p_linear_damp) { - linear_damp = p_linear_damp; - PhysicsServer::get_singleton()->area_set_param(get_rid(), PhysicsServer::AREA_PARAM_LINEAR_DAMP, p_linear_damp); -} -real_t Area::get_linear_damp() const { - return linear_damp; -} - -void Area::set_angular_damp(real_t p_angular_damp) { - angular_damp = p_angular_damp; - PhysicsServer::get_singleton()->area_set_param(get_rid(), PhysicsServer::AREA_PARAM_ANGULAR_DAMP, p_angular_damp); -} - -real_t Area::get_angular_damp() const { - return angular_damp; -} - -void Area::set_priority(real_t p_priority) { - priority = p_priority; - PhysicsServer::get_singleton()->area_set_param(get_rid(), PhysicsServer::AREA_PARAM_PRIORITY, p_priority); -} -real_t Area::get_priority() const { - return priority; -} - -void Area::_body_enter_tree(ObjectID p_id) { - Object *obj = ObjectDB::get_instance(p_id); - Node *node = Object::cast_to(obj); - ERR_FAIL_COND(!node); - - RBMap::Element *E = body_map.find(p_id); - ERR_FAIL_COND(!E); - ERR_FAIL_COND(E->get().in_tree); - - E->get().in_tree = true; - emit_signal(SceneStringNames::get_singleton()->body_entered, node); - for (int i = 0; i < E->get().shapes.size(); i++) { - emit_signal(SceneStringNames::get_singleton()->body_shape_entered, E->get().rid, node, E->get().shapes[i].body_shape, E->get().shapes[i].area_shape); - } -} - -void Area::_body_exit_tree(ObjectID p_id) { - Object *obj = ObjectDB::get_instance(p_id); - Node *node = Object::cast_to(obj); - ERR_FAIL_COND(!node); - RBMap::Element *E = body_map.find(p_id); - ERR_FAIL_COND(!E); - ERR_FAIL_COND(!E->get().in_tree); - E->get().in_tree = false; - emit_signal(SceneStringNames::get_singleton()->body_exited, node); - for (int i = 0; i < E->get().shapes.size(); i++) { - emit_signal(SceneStringNames::get_singleton()->body_shape_exited, E->get().rid, node, E->get().shapes[i].body_shape, E->get().shapes[i].area_shape); - } -} - -void Area::_body_inout(int p_status, const RID &p_body, int p_instance, int p_body_shape, int p_area_shape) { - bool body_in = p_status == PhysicsServer::AREA_BODY_ADDED; - ObjectID objid = p_instance; - - Object *obj = ObjectDB::get_instance(objid); - Node *node = Object::cast_to(obj); - - RBMap::Element *E = body_map.find(objid); - - if (!body_in && !E) { - return; //likely removed from the tree - } - - locked = true; - - if (body_in) { - if (!E) { - E = body_map.insert(objid, BodyState()); - E->get().rid = p_body; - E->get().rc = 0; - E->get().in_tree = node && node->is_inside_tree(); - if (node) { - node->connect(SceneStringNames::get_singleton()->tree_entered, this, SceneStringNames::get_singleton()->_body_enter_tree, make_binds(objid)); - node->connect(SceneStringNames::get_singleton()->tree_exiting, this, SceneStringNames::get_singleton()->_body_exit_tree, make_binds(objid)); - if (E->get().in_tree) { - emit_signal(SceneStringNames::get_singleton()->body_entered, node); - } - } - } - E->get().rc++; - if (node) { - E->get().shapes.insert(ShapePair(p_body_shape, p_area_shape)); - } - - if (E->get().in_tree) { - emit_signal(SceneStringNames::get_singleton()->body_shape_entered, p_body, node, p_body_shape, p_area_shape); - } - - } else { - E->get().rc--; - - if (node) { - E->get().shapes.erase(ShapePair(p_body_shape, p_area_shape)); - } - - bool in_tree = E->get().in_tree; - if (E->get().rc == 0) { - body_map.erase(E); - if (node) { - node->disconnect(SceneStringNames::get_singleton()->tree_entered, this, SceneStringNames::get_singleton()->_body_enter_tree); - node->disconnect(SceneStringNames::get_singleton()->tree_exiting, this, SceneStringNames::get_singleton()->_body_exit_tree); - if (in_tree) { - emit_signal(SceneStringNames::get_singleton()->body_exited, obj); - } - } - } - if (node && in_tree) { - emit_signal(SceneStringNames::get_singleton()->body_shape_exited, p_body, obj, p_body_shape, p_area_shape); - } - } - - locked = false; -} - -void Area::_clear_monitoring() { - ERR_FAIL_COND_MSG(locked, "This function can't be used during the in/out signal."); - - { - RBMap bmcopy = body_map; - body_map.clear(); - //disconnect all monitored stuff - - for (RBMap::Element *E = bmcopy.front(); E; E = E->next()) { - Object *obj = ObjectDB::get_instance(E->key()); - Node *node = Object::cast_to(obj); - - if (!node) { //node may have been deleted in previous frame or at other legiminate point - continue; - } - - node->disconnect(SceneStringNames::get_singleton()->tree_entered, this, SceneStringNames::get_singleton()->_body_enter_tree); - node->disconnect(SceneStringNames::get_singleton()->tree_exiting, this, SceneStringNames::get_singleton()->_body_exit_tree); - - if (!E->get().in_tree) { - continue; - } - - for (int i = 0; i < E->get().shapes.size(); i++) { - emit_signal(SceneStringNames::get_singleton()->body_shape_exited, E->get().rid, node, E->get().shapes[i].body_shape, E->get().shapes[i].area_shape); - } - - emit_signal(SceneStringNames::get_singleton()->body_exited, node); - } - } - - { - RBMap bmcopy = area_map; - area_map.clear(); - //disconnect all monitored stuff - - for (RBMap::Element *E = bmcopy.front(); E; E = E->next()) { - Object *obj = ObjectDB::get_instance(E->key()); - Node *node = Object::cast_to(obj); - - if (!node) { //node may have been deleted in previous frame or at other legiminate point - continue; - } - - node->disconnect(SceneStringNames::get_singleton()->tree_entered, this, SceneStringNames::get_singleton()->_area_enter_tree); - node->disconnect(SceneStringNames::get_singleton()->tree_exiting, this, SceneStringNames::get_singleton()->_area_exit_tree); - - if (!E->get().in_tree) { - continue; - } - - for (int i = 0; i < E->get().shapes.size(); i++) { - emit_signal(SceneStringNames::get_singleton()->area_shape_exited, E->get().rid, node, E->get().shapes[i].area_shape, E->get().shapes[i].self_shape); - } - - emit_signal(SceneStringNames::get_singleton()->area_exited, obj); - } - } -} -void Area::_notification(int p_what) { - if (p_what == NOTIFICATION_EXIT_TREE) { - _clear_monitoring(); - } -} - -void Area::set_monitoring(bool p_enable) { - ERR_FAIL_COND_MSG(locked, "Function blocked during in/out signal. Use set_deferred(\"monitoring\", true/false)."); - - if (p_enable == monitoring) { - return; - } - - monitoring = p_enable; - - if (monitoring) { - PhysicsServer::get_singleton()->area_set_monitor_callback(get_rid(), this, SceneStringNames::get_singleton()->_body_inout); - PhysicsServer::get_singleton()->area_set_area_monitor_callback(get_rid(), this, SceneStringNames::get_singleton()->_area_inout); - } else { - PhysicsServer::get_singleton()->area_set_monitor_callback(get_rid(), nullptr, StringName()); - PhysicsServer::get_singleton()->area_set_area_monitor_callback(get_rid(), nullptr, StringName()); - _clear_monitoring(); - } -} - -void Area::_area_enter_tree(ObjectID p_id) { - Object *obj = ObjectDB::get_instance(p_id); - Node *node = Object::cast_to(obj); - ERR_FAIL_COND(!node); - - RBMap::Element *E = area_map.find(p_id); - ERR_FAIL_COND(!E); - ERR_FAIL_COND(E->get().in_tree); - - E->get().in_tree = true; - emit_signal(SceneStringNames::get_singleton()->area_entered, node); - for (int i = 0; i < E->get().shapes.size(); i++) { - emit_signal(SceneStringNames::get_singleton()->area_shape_entered, E->get().rid, node, E->get().shapes[i].area_shape, E->get().shapes[i].self_shape); - } -} - -void Area::_area_exit_tree(ObjectID p_id) { - Object *obj = ObjectDB::get_instance(p_id); - Node *node = Object::cast_to(obj); - ERR_FAIL_COND(!node); - RBMap::Element *E = area_map.find(p_id); - ERR_FAIL_COND(!E); - ERR_FAIL_COND(!E->get().in_tree); - E->get().in_tree = false; - emit_signal(SceneStringNames::get_singleton()->area_exited, node); - for (int i = 0; i < E->get().shapes.size(); i++) { - emit_signal(SceneStringNames::get_singleton()->area_shape_exited, E->get().rid, node, E->get().shapes[i].area_shape, E->get().shapes[i].self_shape); - } -} - -void Area::_area_inout(int p_status, const RID &p_area, int p_instance, int p_area_shape, int p_self_shape) { - bool area_in = p_status == PhysicsServer::AREA_BODY_ADDED; - ObjectID objid = p_instance; - - Object *obj = ObjectDB::get_instance(objid); - Node *node = Object::cast_to(obj); - - RBMap::Element *E = area_map.find(objid); - - if (!area_in && !E) { - return; //likely removed from the tree - } - - locked = true; - - if (area_in) { - if (!E) { - E = area_map.insert(objid, AreaState()); - E->get().rid = p_area; - E->get().rc = 0; - E->get().in_tree = node && node->is_inside_tree(); - if (node) { - node->connect(SceneStringNames::get_singleton()->tree_entered, this, SceneStringNames::get_singleton()->_area_enter_tree, make_binds(objid)); - node->connect(SceneStringNames::get_singleton()->tree_exiting, this, SceneStringNames::get_singleton()->_area_exit_tree, make_binds(objid)); - if (E->get().in_tree) { - emit_signal(SceneStringNames::get_singleton()->area_entered, node); - } - } - } - E->get().rc++; - if (node) { - E->get().shapes.insert(AreaShapePair(p_area_shape, p_self_shape)); - } - - if (!node || E->get().in_tree) { - emit_signal(SceneStringNames::get_singleton()->area_shape_entered, p_area, node, p_area_shape, p_self_shape); - } - - } else { - E->get().rc--; - - if (node) { - E->get().shapes.erase(AreaShapePair(p_area_shape, p_self_shape)); - } - - bool in_tree = E->get().in_tree; - if (E->get().rc == 0) { - area_map.erase(E); - if (node) { - node->disconnect(SceneStringNames::get_singleton()->tree_entered, this, SceneStringNames::get_singleton()->_area_enter_tree); - node->disconnect(SceneStringNames::get_singleton()->tree_exiting, this, SceneStringNames::get_singleton()->_area_exit_tree); - if (in_tree) { - emit_signal(SceneStringNames::get_singleton()->area_exited, obj); - } - } - } - if (!node || in_tree) { - emit_signal(SceneStringNames::get_singleton()->area_shape_exited, p_area, obj, p_area_shape, p_self_shape); - } - } - - locked = false; -} - -bool Area::is_monitoring() const { - return monitoring; -} - -Array Area::get_overlapping_bodies() const { - ERR_FAIL_COND_V(!monitoring, Array()); - Array ret; - ret.resize(body_map.size()); - int idx = 0; - for (const RBMap::Element *E = body_map.front(); E; E = E->next()) { - Object *obj = ObjectDB::get_instance(E->key()); - if (!obj) { - ret.resize(ret.size() - 1); //ops - } else { - ret[idx++] = obj; - } - } - - return ret; -} - -void Area::set_monitorable(bool p_enable) { - ERR_FAIL_COND_MSG(locked || (is_inside_tree() && PhysicsServer::get_singleton()->is_flushing_queries()), "Function blocked during in/out signal. Use set_deferred(\"monitorable\", true/false)."); - - if (p_enable == monitorable) { - return; - } - - monitorable = p_enable; - - PhysicsServer::get_singleton()->area_set_monitorable(get_rid(), monitorable); -} - -bool Area::is_monitorable() const { - return monitorable; -} - -Array Area::get_overlapping_areas() const { - ERR_FAIL_COND_V(!monitoring, Array()); - Array ret; - ret.resize(area_map.size()); - int idx = 0; - for (const RBMap::Element *E = area_map.front(); E; E = E->next()) { - Object *obj = ObjectDB::get_instance(E->key()); - if (!obj) { - ret.resize(ret.size() - 1); //ops - } else { - ret[idx++] = obj; - } - } - - return ret; -} - -bool Area::overlaps_area(Node *p_area) const { - ERR_FAIL_NULL_V(p_area, false); - const RBMap::Element *E = area_map.find(p_area->get_instance_id()); - if (!E) { - return false; - } - return E->get().in_tree; -} - -bool Area::overlaps_body(Node *p_body) const { - ERR_FAIL_NULL_V(p_body, false); - const RBMap::Element *E = body_map.find(p_body->get_instance_id()); - if (!E) { - return false; - } - return E->get().in_tree; -} - -void Area::set_audio_bus_override(bool p_override) { - audio_bus_override = p_override; -} - -bool Area::is_overriding_audio_bus() const { - return audio_bus_override; -} - -void Area::set_audio_bus(const StringName &p_audio_bus) { - audio_bus = p_audio_bus; -} -StringName Area::get_audio_bus() const { - for (int i = 0; i < AudioServer::get_singleton()->get_bus_count(); i++) { - if (AudioServer::get_singleton()->get_bus_name(i) == audio_bus) { - return audio_bus; - } - } - return "Master"; -} - -void Area::set_use_reverb_bus(bool p_enable) { - use_reverb_bus = p_enable; -} -bool Area::is_using_reverb_bus() const { - return use_reverb_bus; -} - -void Area::set_reverb_bus(const StringName &p_audio_bus) { - reverb_bus = p_audio_bus; -} -StringName Area::get_reverb_bus() const { - for (int i = 0; i < AudioServer::get_singleton()->get_bus_count(); i++) { - if (AudioServer::get_singleton()->get_bus_name(i) == reverb_bus) { - return reverb_bus; - } - } - return "Master"; -} - -void Area::set_reverb_amount(float p_amount) { - reverb_amount = p_amount; -} -float Area::get_reverb_amount() const { - return reverb_amount; -} - -void Area::set_reverb_uniformity(float p_uniformity) { - reverb_uniformity = p_uniformity; -} -float Area::get_reverb_uniformity() const { - return reverb_uniformity; -} - -void Area::_validate_property(PropertyInfo &property) const { - if (property.name == "audio_bus_name" || property.name == "reverb_bus_name") { - String options; - for (int i = 0; i < AudioServer::get_singleton()->get_bus_count(); i++) { - if (i > 0) { - options += ","; - } - String name = AudioServer::get_singleton()->get_bus_name(i); - options += name; - } - - property.hint_string = options; - } -} - -void Area::_bind_methods() { - ClassDB::bind_method(D_METHOD("_body_enter_tree", "id"), &Area::_body_enter_tree); - ClassDB::bind_method(D_METHOD("_body_exit_tree", "id"), &Area::_body_exit_tree); - - ClassDB::bind_method(D_METHOD("_area_enter_tree", "id"), &Area::_area_enter_tree); - ClassDB::bind_method(D_METHOD("_area_exit_tree", "id"), &Area::_area_exit_tree); - - ClassDB::bind_method(D_METHOD("set_space_override_mode", "enable"), &Area::set_space_override_mode); - ClassDB::bind_method(D_METHOD("get_space_override_mode"), &Area::get_space_override_mode); - - ClassDB::bind_method(D_METHOD("set_gravity_is_point", "enable"), &Area::set_gravity_is_point); - ClassDB::bind_method(D_METHOD("is_gravity_a_point"), &Area::is_gravity_a_point); - - ClassDB::bind_method(D_METHOD("set_gravity_distance_scale", "distance_scale"), &Area::set_gravity_distance_scale); - ClassDB::bind_method(D_METHOD("get_gravity_distance_scale"), &Area::get_gravity_distance_scale); - - ClassDB::bind_method(D_METHOD("set_gravity_vector", "vector"), &Area::set_gravity_vector); - ClassDB::bind_method(D_METHOD("get_gravity_vector"), &Area::get_gravity_vector); - - ClassDB::bind_method(D_METHOD("set_gravity", "gravity"), &Area::set_gravity); - ClassDB::bind_method(D_METHOD("get_gravity"), &Area::get_gravity); - - ClassDB::bind_method(D_METHOD("set_angular_damp", "angular_damp"), &Area::set_angular_damp); - ClassDB::bind_method(D_METHOD("get_angular_damp"), &Area::get_angular_damp); - - ClassDB::bind_method(D_METHOD("set_linear_damp", "linear_damp"), &Area::set_linear_damp); - ClassDB::bind_method(D_METHOD("get_linear_damp"), &Area::get_linear_damp); - - ClassDB::bind_method(D_METHOD("set_priority", "priority"), &Area::set_priority); - ClassDB::bind_method(D_METHOD("get_priority"), &Area::get_priority); - - ClassDB::bind_method(D_METHOD("set_monitorable", "enable"), &Area::set_monitorable); - ClassDB::bind_method(D_METHOD("is_monitorable"), &Area::is_monitorable); - - ClassDB::bind_method(D_METHOD("set_monitoring", "enable"), &Area::set_monitoring); - ClassDB::bind_method(D_METHOD("is_monitoring"), &Area::is_monitoring); - - ClassDB::bind_method(D_METHOD("get_overlapping_bodies"), &Area::get_overlapping_bodies); - ClassDB::bind_method(D_METHOD("get_overlapping_areas"), &Area::get_overlapping_areas); - - ClassDB::bind_method(D_METHOD("overlaps_body", "body"), &Area::overlaps_body); - ClassDB::bind_method(D_METHOD("overlaps_area", "area"), &Area::overlaps_area); - - ClassDB::bind_method(D_METHOD("_body_inout"), &Area::_body_inout); - ClassDB::bind_method(D_METHOD("_area_inout"), &Area::_area_inout); - - ClassDB::bind_method(D_METHOD("set_audio_bus_override", "enable"), &Area::set_audio_bus_override); - ClassDB::bind_method(D_METHOD("is_overriding_audio_bus"), &Area::is_overriding_audio_bus); - - ClassDB::bind_method(D_METHOD("set_audio_bus", "name"), &Area::set_audio_bus); - ClassDB::bind_method(D_METHOD("get_audio_bus"), &Area::get_audio_bus); - - ClassDB::bind_method(D_METHOD("set_use_reverb_bus", "enable"), &Area::set_use_reverb_bus); - ClassDB::bind_method(D_METHOD("is_using_reverb_bus"), &Area::is_using_reverb_bus); - - ClassDB::bind_method(D_METHOD("set_reverb_bus", "name"), &Area::set_reverb_bus); - ClassDB::bind_method(D_METHOD("get_reverb_bus"), &Area::get_reverb_bus); - - ClassDB::bind_method(D_METHOD("set_reverb_amount", "amount"), &Area::set_reverb_amount); - ClassDB::bind_method(D_METHOD("get_reverb_amount"), &Area::get_reverb_amount); - - ClassDB::bind_method(D_METHOD("set_reverb_uniformity", "amount"), &Area::set_reverb_uniformity); - ClassDB::bind_method(D_METHOD("get_reverb_uniformity"), &Area::get_reverb_uniformity); - - ADD_SIGNAL(MethodInfo("body_shape_entered", PropertyInfo(Variant::RID, "body_rid"), PropertyInfo(Variant::OBJECT, "body", PROPERTY_HINT_RESOURCE_TYPE, "Node"), PropertyInfo(Variant::INT, "body_shape_index"), PropertyInfo(Variant::INT, "local_shape_index"))); - ADD_SIGNAL(MethodInfo("body_shape_exited", PropertyInfo(Variant::RID, "body_rid"), PropertyInfo(Variant::OBJECT, "body", PROPERTY_HINT_RESOURCE_TYPE, "Node"), PropertyInfo(Variant::INT, "body_shape_index"), PropertyInfo(Variant::INT, "local_shape_index"))); - ADD_SIGNAL(MethodInfo("body_entered", PropertyInfo(Variant::OBJECT, "body", PROPERTY_HINT_RESOURCE_TYPE, "Node"))); - ADD_SIGNAL(MethodInfo("body_exited", PropertyInfo(Variant::OBJECT, "body", PROPERTY_HINT_RESOURCE_TYPE, "Node"))); - - ADD_SIGNAL(MethodInfo("area_shape_entered", PropertyInfo(Variant::RID, "area_rid"), PropertyInfo(Variant::OBJECT, "area", PROPERTY_HINT_RESOURCE_TYPE, "Area"), PropertyInfo(Variant::INT, "area_shape_index"), PropertyInfo(Variant::INT, "local_shape_index"))); - ADD_SIGNAL(MethodInfo("area_shape_exited", PropertyInfo(Variant::RID, "area_rid"), PropertyInfo(Variant::OBJECT, "area", PROPERTY_HINT_RESOURCE_TYPE, "Area"), PropertyInfo(Variant::INT, "area_shape_index"), PropertyInfo(Variant::INT, "local_shape_index"))); - ADD_SIGNAL(MethodInfo("area_entered", PropertyInfo(Variant::OBJECT, "area", PROPERTY_HINT_RESOURCE_TYPE, "Area"))); - ADD_SIGNAL(MethodInfo("area_exited", PropertyInfo(Variant::OBJECT, "area", PROPERTY_HINT_RESOURCE_TYPE, "Area"))); - - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "monitoring"), "set_monitoring", "is_monitoring"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "monitorable"), "set_monitorable", "is_monitorable"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "priority", PROPERTY_HINT_RANGE, "0,128,1"), "set_priority", "get_priority"); - - ADD_GROUP("Physics Overrides", ""); - ADD_PROPERTY(PropertyInfo(Variant::INT, "space_override", PROPERTY_HINT_ENUM, "Disabled,Combine,Combine-Replace,Replace,Replace-Combine"), "set_space_override_mode", "get_space_override_mode"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "gravity_point"), "set_gravity_is_point", "is_gravity_a_point"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "gravity_distance_scale", PROPERTY_HINT_EXP_RANGE, "0,1024,0.001,or_greater"), "set_gravity_distance_scale", "get_gravity_distance_scale"); - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "gravity_vec"), "set_gravity_vector", "get_gravity_vector"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "gravity", PROPERTY_HINT_RANGE, "-32,32,0.001,or_lesser,or_greater"), "set_gravity", "get_gravity"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "linear_damp", PROPERTY_HINT_RANGE, "0,100,0.001,or_greater"), "set_linear_damp", "get_linear_damp"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "angular_damp", PROPERTY_HINT_RANGE, "0,100,0.001,or_greater"), "set_angular_damp", "get_angular_damp"); - - ADD_GROUP("Audio Bus", "audio_bus_"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "audio_bus_override"), "set_audio_bus_override", "is_overriding_audio_bus"); - ADD_PROPERTY(PropertyInfo(Variant::STRING_NAME, "audio_bus_name", PROPERTY_HINT_ENUM, ""), "set_audio_bus", "get_audio_bus"); - - ADD_GROUP("Reverb Bus", "reverb_bus_"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "reverb_bus_enable"), "set_use_reverb_bus", "is_using_reverb_bus"); - ADD_PROPERTY(PropertyInfo(Variant::STRING_NAME, "reverb_bus_name", PROPERTY_HINT_ENUM, ""), "set_reverb_bus", "get_reverb_bus"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "reverb_bus_amount", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_reverb_amount", "get_reverb_amount"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "reverb_bus_uniformity", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_reverb_uniformity", "get_reverb_uniformity"); - - BIND_ENUM_CONSTANT(SPACE_OVERRIDE_DISABLED); - BIND_ENUM_CONSTANT(SPACE_OVERRIDE_COMBINE); - BIND_ENUM_CONSTANT(SPACE_OVERRIDE_COMBINE_REPLACE); - BIND_ENUM_CONSTANT(SPACE_OVERRIDE_REPLACE); - BIND_ENUM_CONSTANT(SPACE_OVERRIDE_REPLACE_COMBINE); -} - -Area::Area() : - CollisionObject(RID_PRIME(PhysicsServer::get_singleton()->area_create()), true) { - space_override = SPACE_OVERRIDE_DISABLED; - set_gravity(9.8); - locked = false; - set_gravity_vector(Vector3(0, -1, 0)); - gravity_is_point = false; - gravity_distance_scale = 0; - linear_damp = 0.1; - angular_damp = 0.1; - priority = 0; - monitoring = false; - monitorable = false; - set_monitoring(true); - set_monitorable(true); - - audio_bus_override = false; - audio_bus = "Master"; - - use_reverb_bus = false; - reverb_bus = "Master"; - reverb_amount = 0.0; - reverb_uniformity = 0.0; -} - -Area::~Area() { -} diff --git a/scene/3d/area.h b/scene/3d/area.h deleted file mode 100644 index f111bfe..0000000 --- a/scene/3d/area.h +++ /dev/null @@ -1,201 +0,0 @@ -#ifndef AREA_H -#define AREA_H -/*************************************************************************/ -/* area.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "core/containers/vset.h" -#include "scene/3d/collision_object.h" - -class Area : public CollisionObject { - GDCLASS(Area, CollisionObject); - -public: - enum SpaceOverride { - SPACE_OVERRIDE_DISABLED, - SPACE_OVERRIDE_COMBINE, - SPACE_OVERRIDE_COMBINE_REPLACE, - SPACE_OVERRIDE_REPLACE, - SPACE_OVERRIDE_REPLACE_COMBINE - }; - -private: - SpaceOverride space_override; - Vector3 gravity_vec; - real_t gravity; - bool gravity_is_point; - real_t gravity_distance_scale; - real_t angular_damp; - real_t linear_damp; - int priority; - bool monitoring; - bool monitorable; - bool locked; - - void _body_inout(int p_status, const RID &p_body, int p_instance, int p_body_shape, int p_area_shape); - - void _body_enter_tree(ObjectID p_id); - void _body_exit_tree(ObjectID p_id); - - struct ShapePair { - int body_shape; - int area_shape; - bool operator<(const ShapePair &p_sp) const { - if (body_shape == p_sp.body_shape) { - return area_shape < p_sp.area_shape; - } else { - return body_shape < p_sp.body_shape; - } - } - - ShapePair() {} - ShapePair(int p_bs, int p_as) { - body_shape = p_bs; - area_shape = p_as; - } - }; - - struct BodyState { - RID rid; - int rc; - bool in_tree; - VSet shapes; - }; - - RBMap body_map; - - void _area_inout(int p_status, const RID &p_area, int p_instance, int p_area_shape, int p_self_shape); - - void _area_enter_tree(ObjectID p_id); - void _area_exit_tree(ObjectID p_id); - - struct AreaShapePair { - int area_shape; - int self_shape; - bool operator<(const AreaShapePair &p_sp) const { - if (area_shape == p_sp.area_shape) { - return self_shape < p_sp.self_shape; - } else { - return area_shape < p_sp.area_shape; - } - } - - AreaShapePair() {} - AreaShapePair(int p_bs, int p_as) { - area_shape = p_bs; - self_shape = p_as; - } - }; - - struct AreaState { - RID rid; - int rc; - bool in_tree; - VSet shapes; - }; - - RBMap area_map; - void _clear_monitoring(); - - bool audio_bus_override; - StringName audio_bus; - - bool use_reverb_bus; - StringName reverb_bus; - float reverb_amount; - float reverb_uniformity; - - void _validate_property(PropertyInfo &property) const; - -protected: - void _notification(int p_what); - static void _bind_methods(); - -public: - void set_space_override_mode(SpaceOverride p_mode); - SpaceOverride get_space_override_mode() const; - - void set_gravity_is_point(bool p_enabled); - bool is_gravity_a_point() const; - - void set_gravity_distance_scale(real_t p_scale); - real_t get_gravity_distance_scale() const; - - void set_gravity_vector(const Vector3 &p_vec); - Vector3 get_gravity_vector() const; - - void set_gravity(real_t p_gravity); - real_t get_gravity() const; - - void set_angular_damp(real_t p_angular_damp); - real_t get_angular_damp() const; - - void set_linear_damp(real_t p_linear_damp); - real_t get_linear_damp() const; - - void set_priority(real_t p_priority); - real_t get_priority() const; - - void set_monitoring(bool p_enable); - bool is_monitoring() const; - - void set_monitorable(bool p_enable); - bool is_monitorable() const; - - Array get_overlapping_bodies() const; - Array get_overlapping_areas() const; //function for script - - bool overlaps_area(Node *p_area) const; - bool overlaps_body(Node *p_body) const; - - void set_audio_bus_override(bool p_override); - bool is_overriding_audio_bus() const; - - void set_audio_bus(const StringName &p_audio_bus); - StringName get_audio_bus() const; - - void set_use_reverb_bus(bool p_enable); - bool is_using_reverb_bus() const; - - void set_reverb_bus(const StringName &p_audio_bus); - StringName get_reverb_bus() const; - - void set_reverb_amount(float p_amount); - float get_reverb_amount() const; - - void set_reverb_uniformity(float p_uniformity); - float get_reverb_uniformity() const; - - Area(); - ~Area(); -}; - -VARIANT_ENUM_CAST(Area::SpaceOverride); - -#endif // AREA__H diff --git a/scene/3d/audio_stream_player_3d.cpp b/scene/3d/audio_stream_player_3d.cpp index 1f643c4..514d9c9 100644 --- a/scene/3d/audio_stream_player_3d.cpp +++ b/scene/3d/audio_stream_player_3d.cpp @@ -31,15 +31,11 @@ #include "audio_stream_player_3d.h" #include "core/config/engine.h" #include "core/config/project_settings.h" -#include "scene/3d/area.h" #include "scene/3d/camera.h" #include "scene/3d/listener.h" #include "scene/3d/spatial_velocity_tracker.h" #include "scene/main/viewport.h" -#include "scene/resources/shapes/shape.h" -#include "scene/resources/world_3d.h" #include "servers/audio/audio_stream.h" -#include "servers/physics_server.h" // Based on "A Novel Multichannel Panning Method for Standard and Arbitrary Loudspeaker Configurations" by Ramy Sadek and Chris Kyriakakis (2004) // Speaker-Placement Correction Amplitude Panning (SPCAP) @@ -365,266 +361,21 @@ void AudioStreamPlayer3D::_notification(int p_what) { } } - if (p_what == NOTIFICATION_INTERNAL_PHYSICS_PROCESS) { - //update anything related to position first, if possible of course + //start playing if requested + if (setplay.get() >= 0.0) { + setseek.set(setplay.get()); + active.set(); + setplay.set(-1); + //do not update, this makes it easier to animate (will shut off otherwise) + ///_change_notify("playing"); //update property in editor + } - if (!output_ready.is_set()) { - Vector3 linear_velocity; - - //compute linear velocity for doppler - if (doppler_tracking != DOPPLER_TRACKING_DISABLED) { - linear_velocity = velocity_tracker->get_tracked_linear_velocity(); - } - - Ref world = get_world_3d(); - ERR_FAIL_COND(world.is_null()); - - int new_output_count = 0; - - Vector3 global_pos = get_global_transform().origin; - - int bus_index = AudioServer::get_singleton()->thread_find_bus_index(bus); - - //check if any area is diverting sound into a bus - - PhysicsDirectSpaceState *space_state = PhysicsServer::get_singleton()->space_get_direct_state(world->get_space()); - - PhysicsDirectSpaceState::ShapeResult sr[MAX_INTERSECT_AREAS]; - - int areas = space_state->intersect_point(global_pos, sr, MAX_INTERSECT_AREAS, RBSet(), area_mask, false, true); - Area *area = nullptr; - - for (int i = 0; i < areas; i++) { - if (!sr[i].collider) { - continue; - } - - Area *tarea = Object::cast_to(sr[i].collider); - if (!tarea) { - continue; - } - - if (!tarea->is_overriding_audio_bus() && !tarea->is_using_reverb_bus()) { - continue; - } - - area = tarea; - break; - } - - List cameras; - world->get_camera_list(&cameras); - - for (List::Element *E = cameras.front(); E; E = E->next()) { - Camera *camera = E->get(); - Viewport *vp = camera->get_viewport(); - if (!vp->is_audio_listener()) { - continue; - } - - bool listener_is_camera = true; - Spatial *listener_node = camera; - - Listener *listener = vp->get_listener(); - if (listener) { - listener_node = listener; - listener_is_camera = false; - } - - Vector3 local_pos = listener_node->get_global_transform().orthonormalized().affine_inverse().xform(global_pos); - - float dist = local_pos.length(); - - Vector3 area_sound_pos; - Vector3 listener_area_pos; - - if (area && area->is_using_reverb_bus() && area->get_reverb_uniformity() > 0) { - area_sound_pos = space_state->get_closest_point_to_object_volume(area->get_rid(), listener_node->get_global_transform().origin); - listener_area_pos = listener_node->get_global_transform().affine_inverse().xform(area_sound_pos); - } - - if (max_distance > 0) { - float total_max = max_distance; - - if (area && area->is_using_reverb_bus() && area->get_reverb_uniformity() > 0) { - total_max = MAX(total_max, listener_area_pos.length()); - } - if (total_max > max_distance) { - continue; //can't hear this sound in this listener - } - } - - float multiplier = Math::db2linear(_get_attenuation_db(dist)); - if (max_distance > 0) { - multiplier *= MAX(0, 1.0 - (dist / max_distance)); - } - - Output output; - output.bus_index = bus_index; - output.reverb_bus_index = -1; //no reverb by default - output.viewport = vp; - - float db_att = (1.0 - MIN(1.0, multiplier)) * attenuation_filter_db; - - if (emission_angle_enabled) { - Vector3 listenertopos = global_pos - listener_node->get_global_transform().origin; - float c = listenertopos.normalized().dot(get_global_transform().basis.get_axis(2).normalized()); //it's z negative - float angle = Math::rad2deg(Math::acos(c)); - if (angle > emission_angle) { - db_att -= -emission_angle_filter_attenuation_db; - } - } - - output.filter_gain = Math::db2linear(db_att); - - // Bake in a constant factor here to allow the project setting defaults for 2d and 3d to be normalized to 1.0. - float tightness = cached_global_panning_strength * 2.0f; - tightness *= panning_strength; - _calc_output_vol(local_pos.normalized(), tightness, output); - - unsigned int cc = AudioServer::get_singleton()->get_channel_count(); - for (unsigned int k = 0; k < cc; k++) { - output.vol[k] *= multiplier; - } - - bool filled_reverb = false; - int vol_index_max = AudioServer::get_singleton()->get_speaker_mode() + 1; - - if (area) { - if (area->is_overriding_audio_bus()) { - //override audio bus - StringName bus_name = area->get_audio_bus(); - output.bus_index = AudioServer::get_singleton()->thread_find_bus_index(bus_name); - } - - if (area->is_using_reverb_bus()) { - filled_reverb = true; - StringName bus_name = area->get_reverb_bus(); - output.reverb_bus_index = AudioServer::get_singleton()->thread_find_bus_index(bus_name); - - float uniformity = area->get_reverb_uniformity(); - float area_send = area->get_reverb_amount(); - - if (uniformity > 0.0) { - float distance = listener_area_pos.length(); - float attenuation = Math::db2linear(_get_attenuation_db(distance)); - - //float dist_att_db = -20 * Math::log(dist + 0.00001); //logarithmic attenuation, like in real life - - float center_val[3] = { 0.5f, 0.25f, 0.16666f }; - AudioFrame center_frame(center_val[vol_index_max - 1], center_val[vol_index_max - 1]); - - if (attenuation < 1.0) { - //pan the uniform sound - Vector3 rev_pos = listener_area_pos; - rev_pos.y = 0; - rev_pos.normalize(); - - if (cc >= 1) { - // Stereo pair - float c = rev_pos.x * 0.5 + 0.5; - output.reverb_vol[0].l = 1.0 - c; - output.reverb_vol[0].r = c; - } - - if (cc >= 3) { - // Center pair + Side pair - float xl = Vector3(-1, 0, -1).normalized().dot(rev_pos) * 0.5 + 0.5; - float xr = Vector3(1, 0, -1).normalized().dot(rev_pos) * 0.5 + 0.5; - - output.reverb_vol[1].l = xl; - output.reverb_vol[1].r = xr; - output.reverb_vol[2].l = 1.0 - xr; - output.reverb_vol[2].r = 1.0 - xl; - } - - if (cc >= 4) { - // Rear pair - // FIXME: Not sure what math should be done here - float c = rev_pos.x * 0.5 + 0.5; - output.reverb_vol[3].l = 1.0 - c; - output.reverb_vol[3].r = c; - } - - for (int i = 0; i < vol_index_max; i++) { - output.reverb_vol[i] = output.reverb_vol[i].linear_interpolate(center_frame, attenuation); - } - } else { - for (int i = 0; i < vol_index_max; i++) { - output.reverb_vol[i] = center_frame; - } - } - - for (int i = 0; i < vol_index_max; i++) { - output.reverb_vol[i] = output.vol[i].linear_interpolate(output.reverb_vol[i] * attenuation, uniformity); - output.reverb_vol[i] *= area_send; - } - - } else { - for (int i = 0; i < vol_index_max; i++) { - output.reverb_vol[i] = output.vol[i] * area_send; - } - } - } - } - - if (doppler_tracking != DOPPLER_TRACKING_DISABLED) { - Vector3 listener_velocity; - - if (listener_is_camera) { - listener_velocity = camera->get_doppler_tracked_velocity(); - } - - Vector3 local_velocity = listener_node->get_global_transform().orthonormalized().basis.xform_inv(linear_velocity - listener_velocity); - - if (local_velocity == Vector3()) { - output.pitch_scale = 1.0; - } else { - float approaching = local_pos.normalized().dot(local_velocity.normalized()); - float velocity = local_velocity.length(); - float speed_of_sound = 343.0; - - output.pitch_scale = speed_of_sound / (speed_of_sound + velocity * approaching); - output.pitch_scale = CLAMP(output.pitch_scale, (1 / 8.0), 8.0); //avoid crazy stuff - } - - } else { - output.pitch_scale = 1.0; - } - - if (!filled_reverb) { - for (int i = 0; i < vol_index_max; i++) { - output.reverb_vol[i] = AudioFrame(0, 0); - } - } - - outputs[new_output_count] = output; - new_output_count++; - if (new_output_count == MAX_OUTPUTS) { - break; - } - } - - output_count.set(new_output_count); - output_ready.set(); - } - - //start playing if requested - if (setplay.get() >= 0.0) { - setseek.set(setplay.get()); - active.set(); - setplay.set(-1); - //do not update, this makes it easier to animate (will shut off otherwise) - ///_change_notify("playing"); //update property in editor - } - - //stop playing if no longer active - if (!active.is_set()) { - set_physics_process_internal(false); - //do not update, this makes it easier to animate (will shut off otherwise) - //_change_notify("playing"); //update property in editor - emit_signal("finished"); - } + //stop playing if no longer active + if (!active.is_set()) { + set_physics_process_internal(false); + //do not update, this makes it easier to animate (will shut off otherwise) + //_change_notify("playing"); //update property in editor + emit_signal("finished"); } } diff --git a/scene/3d/camera.cpp b/scene/3d/camera.cpp index 3433ccf..9e03398 100644 --- a/scene/3d/camera.cpp +++ b/scene/3d/camera.cpp @@ -30,7 +30,6 @@ #include "camera.h" -#include "collision_object.h" #include "core/config/engine.h" #include "core/math/projection.h" #include "scene/3d/spatial_velocity_tracker.h" @@ -684,215 +683,3 @@ Camera::Camera() { Camera::~Camera() { RenderingServer::get_singleton()->free(camera); } - -//////////////////////////////////////// - -void ClippedCamera::set_margin(float p_margin) { - margin = p_margin; -} -float ClippedCamera::get_margin() const { - return margin; -} -void ClippedCamera::set_process_mode(ProcessMode p_mode) { - if (process_mode == p_mode) { - return; - } - process_mode = p_mode; - set_process_internal(process_mode == CLIP_PROCESS_IDLE); - set_physics_process_internal(process_mode == CLIP_PROCESS_PHYSICS); -} -ClippedCamera::ProcessMode ClippedCamera::get_process_mode() const { - return process_mode; -} - -Transform ClippedCamera::get_camera_transform() const { - Transform t = Camera::get_camera_transform(); - t.origin += -t.basis.get_axis(Vector3::AXIS_Z).normalized() * clip_offset; - return t; -} - -void ClippedCamera::_notification(int p_what) { - if (p_what == NOTIFICATION_INTERNAL_PROCESS || p_what == NOTIFICATION_INTERNAL_PHYSICS_PROCESS) { - Spatial *parent = Object::cast_to(get_parent()); - if (!parent) { - return; - } - - PhysicsDirectSpaceState *dspace = get_world_3d()->get_direct_space_state(); - ERR_FAIL_COND(!dspace); // most likely physics set to threads - - Vector3 cam_fw = -get_global_transform().basis.get_axis(Vector3::AXIS_Z).normalized(); - Vector3 cam_pos = get_global_transform().origin; - Vector3 parent_pos = parent->get_global_transform().origin; - - Plane parent_plane(parent_pos, cam_fw); - - if (parent_plane.is_point_over(cam_pos)) { - //cam is beyond parent plane - return; - } - - Vector3 ray_from = parent_plane.project(cam_pos); - - clip_offset = 0; //reset by defau;t - - { //check if points changed - Vector local_points = get_near_plane_points(); - - bool all_equal = true; - - for (int i = 0; i < 5; i++) { - if (points[i] != local_points[i]) { - all_equal = false; - break; - } - } - - if (!all_equal) { - PhysicsServer::get_singleton()->shape_set_data(pyramid_shape, local_points); - points = local_points; - } - } - - Transform xf = get_global_transform(); - xf.origin = ray_from; - xf.orthonormalize(); - - float closest_safe = 1.0f, closest_unsafe = 1.0f; - if (dspace->cast_motion(pyramid_shape, xf, cam_pos - ray_from, margin, closest_safe, closest_unsafe, exclude, collision_mask, clip_to_bodies, clip_to_areas)) { - clip_offset = cam_pos.distance_to(ray_from + (cam_pos - ray_from) * closest_safe); - } - - _update_camera(); - } - - if (p_what == NOTIFICATION_LOCAL_TRANSFORM_CHANGED) { - update_gizmos(); - } -} - -void ClippedCamera::set_collision_mask(uint32_t p_mask) { - collision_mask = p_mask; -} - -uint32_t ClippedCamera::get_collision_mask() const { - return collision_mask; -} - -void ClippedCamera::set_collision_mask_bit(int p_bit, bool p_value) { - ERR_FAIL_INDEX_MSG(p_bit, 32, "Collision layer bit must be between 0 and 31 inclusive."); - uint32_t mask = get_collision_mask(); - if (p_value) { - mask |= 1 << p_bit; - } else { - mask &= ~(1 << p_bit); - } - set_collision_mask(mask); -} - -bool ClippedCamera::get_collision_mask_bit(int p_bit) const { - ERR_FAIL_INDEX_V_MSG(p_bit, 32, false, "Collision mask bit must be between 0 and 31 inclusive."); - return get_collision_mask() & (1 << p_bit); -} - -void ClippedCamera::add_exception_rid(const RID &p_rid) { - exclude.insert(p_rid); -} - -void ClippedCamera::add_exception(const Object *p_object) { - ERR_FAIL_NULL(p_object); - const CollisionObject *co = Object::cast_to(p_object); - ERR_FAIL_COND_MSG(!co, "The passed Node must be an instance of CollisionObject."); - add_exception_rid(co->get_rid()); -} - -void ClippedCamera::remove_exception_rid(const RID &p_rid) { - exclude.erase(p_rid); -} - -void ClippedCamera::remove_exception(const Object *p_object) { - ERR_FAIL_NULL(p_object); - const CollisionObject *co = Object::cast_to(p_object); - ERR_FAIL_COND_MSG(!co, "The passed Node must be an instance of CollisionObject."); - remove_exception_rid(co->get_rid()); -} - -void ClippedCamera::clear_exceptions() { - exclude.clear(); -} - -float ClippedCamera::get_clip_offset() const { - return clip_offset; -} - -void ClippedCamera::set_clip_to_areas(bool p_clip) { - clip_to_areas = p_clip; -} - -bool ClippedCamera::is_clip_to_areas_enabled() const { - return clip_to_areas; -} - -void ClippedCamera::set_clip_to_bodies(bool p_clip) { - clip_to_bodies = p_clip; -} - -bool ClippedCamera::is_clip_to_bodies_enabled() const { - return clip_to_bodies; -} - -void ClippedCamera::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_margin", "margin"), &ClippedCamera::set_margin); - ClassDB::bind_method(D_METHOD("get_margin"), &ClippedCamera::get_margin); - - ClassDB::bind_method(D_METHOD("set_process_mode", "process_mode"), &ClippedCamera::set_process_mode); - ClassDB::bind_method(D_METHOD("get_process_mode"), &ClippedCamera::get_process_mode); - - ClassDB::bind_method(D_METHOD("set_collision_mask", "mask"), &ClippedCamera::set_collision_mask); - ClassDB::bind_method(D_METHOD("get_collision_mask"), &ClippedCamera::get_collision_mask); - - ClassDB::bind_method(D_METHOD("set_collision_mask_bit", "bit", "value"), &ClippedCamera::set_collision_mask_bit); - ClassDB::bind_method(D_METHOD("get_collision_mask_bit", "bit"), &ClippedCamera::get_collision_mask_bit); - - ClassDB::bind_method(D_METHOD("add_exception_rid", "rid"), &ClippedCamera::add_exception_rid); - ClassDB::bind_method(D_METHOD("add_exception", "node"), &ClippedCamera::add_exception); - - ClassDB::bind_method(D_METHOD("remove_exception_rid", "rid"), &ClippedCamera::remove_exception_rid); - ClassDB::bind_method(D_METHOD("remove_exception", "node"), &ClippedCamera::remove_exception); - - ClassDB::bind_method(D_METHOD("set_clip_to_areas", "enable"), &ClippedCamera::set_clip_to_areas); - ClassDB::bind_method(D_METHOD("is_clip_to_areas_enabled"), &ClippedCamera::is_clip_to_areas_enabled); - - ClassDB::bind_method(D_METHOD("get_clip_offset"), &ClippedCamera::get_clip_offset); - - ClassDB::bind_method(D_METHOD("set_clip_to_bodies", "enable"), &ClippedCamera::set_clip_to_bodies); - ClassDB::bind_method(D_METHOD("is_clip_to_bodies_enabled"), &ClippedCamera::is_clip_to_bodies_enabled); - - ClassDB::bind_method(D_METHOD("clear_exceptions"), &ClippedCamera::clear_exceptions); - - ADD_PROPERTY(PropertyInfo(Variant::REAL, "margin", PROPERTY_HINT_RANGE, "0,32,0.01"), "set_margin", "get_margin"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "process_mode", PROPERTY_HINT_ENUM, "Physics,Idle"), "set_process_mode", "get_process_mode"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "collision_mask", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_collision_mask", "get_collision_mask"); - - ADD_GROUP("Clip To", "clip_to"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "clip_to_areas", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_clip_to_areas", "is_clip_to_areas_enabled"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "clip_to_bodies", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_clip_to_bodies", "is_clip_to_bodies_enabled"); - - BIND_ENUM_CONSTANT(CLIP_PROCESS_PHYSICS); - BIND_ENUM_CONSTANT(CLIP_PROCESS_IDLE); -} -ClippedCamera::ClippedCamera() { - margin = 0; - clip_offset = 0; - process_mode = CLIP_PROCESS_PHYSICS; - set_physics_process_internal(true); - collision_mask = 1; - set_notify_local_transform(Engine::get_singleton()->is_editor_hint()); - points.resize(5); - pyramid_shape = RID_PRIME(PhysicsServer::get_singleton()->shape_create(PhysicsServer::SHAPE_CONVEX_POLYGON)); - clip_to_areas = false; - clip_to_bodies = true; -} -ClippedCamera::~ClippedCamera() { - PhysicsServer::get_singleton()->free(pyramid_shape); -} diff --git a/scene/3d/camera.h b/scene/3d/camera.h index fc2db27..0f42fb8 100644 --- a/scene/3d/camera.h +++ b/scene/3d/camera.h @@ -180,63 +180,5 @@ VARIANT_ENUM_CAST(Camera::ProjectionMode); VARIANT_ENUM_CAST(Camera::KeepAspect); VARIANT_ENUM_CAST(Camera::DopplerTracking); -class ClippedCamera : public Camera { - GDCLASS(ClippedCamera, Camera); -public: - enum ProcessMode { - CLIP_PROCESS_PHYSICS, - CLIP_PROCESS_IDLE, - }; - -private: - ProcessMode process_mode; - RID pyramid_shape; - float margin; - float clip_offset; - uint32_t collision_mask; - bool clip_to_areas; - bool clip_to_bodies; - - RBSet exclude; - - Vector points; - -protected: - void _notification(int p_what); - static void _bind_methods(); - virtual Transform get_camera_transform() const; - -public: - void set_clip_to_areas(bool p_clip); - bool is_clip_to_areas_enabled() const; - - void set_clip_to_bodies(bool p_clip); - bool is_clip_to_bodies_enabled() const; - - void set_margin(float p_margin); - float get_margin() const; - - void set_process_mode(ProcessMode p_mode); - ProcessMode get_process_mode() const; - - void set_collision_mask(uint32_t p_mask); - uint32_t get_collision_mask() const; - - void set_collision_mask_bit(int p_bit, bool p_value); - bool get_collision_mask_bit(int p_bit) const; - - void add_exception_rid(const RID &p_rid); - void add_exception(const Object *p_object); - void remove_exception_rid(const RID &p_rid); - void remove_exception(const Object *p_object); - void clear_exceptions(); - - float get_clip_offset() const; - - ClippedCamera(); - ~ClippedCamera(); -}; - -VARIANT_ENUM_CAST(ClippedCamera::ProcessMode); #endif diff --git a/scene/3d/collision_object.cpp b/scene/3d/collision_object.cpp deleted file mode 100644 index 086c86a..0000000 --- a/scene/3d/collision_object.cpp +++ /dev/null @@ -1,593 +0,0 @@ -/*************************************************************************/ -/* collision_object.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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.h" - -#include "core/config/engine.h" -#include "mesh_instance.h" -#include "scene/resources/mesh/mesh.h" -#include "scene/resources/world_3d.h" -#include "scene/main/scene_string_names.h" -#include "servers/physics_server.h" - -void CollisionObject::_notification(int p_what) { - switch (p_what) { - case NOTIFICATION_ENTER_TREE: { - if (_are_collision_shapes_visible()) { - debug_shape_old_transform = get_global_transform(); - for (RBMap::Element *E = shapes.front(); E; E = E->next()) { - debug_shapes_to_update.insert(E->key()); - } - _update_debug_shapes(); - } - } break; - - case NOTIFICATION_EXIT_TREE: { - if (debug_shapes_count > 0) { - _clear_debug_shapes(); - } - } break; - - case NOTIFICATION_ENTER_WORLD: { - if (area) { - PhysicsServer::get_singleton()->area_set_transform(rid, get_global_transform()); - } else { - PhysicsServer::get_singleton()->body_set_state(rid, PhysicsServer::BODY_STATE_TRANSFORM, get_global_transform()); - } - - Ref world_ref = get_world_3d(); - ERR_FAIL_COND(!world_ref.is_valid()); - RID space = world_ref->get_space(); - if (area) { - PhysicsServer::get_singleton()->area_set_space(rid, space); - } else { - PhysicsServer::get_singleton()->body_set_space(rid, space); - } - _update_pickable(); - //get space - } break; - - case NOTIFICATION_TRANSFORM_CHANGED: { - if (only_update_transform_changes) { - return; - } - - if (area) { - PhysicsServer::get_singleton()->area_set_transform(rid, get_global_transform()); - } else { - PhysicsServer::get_singleton()->body_set_state(rid, PhysicsServer::BODY_STATE_TRANSFORM, get_global_transform()); - } - - _on_transform_changed(); - - } break; - case NOTIFICATION_VISIBILITY_CHANGED: { - _update_pickable(); - - } break; - case NOTIFICATION_EXIT_WORLD: { - if (area) { - PhysicsServer::get_singleton()->area_set_space(rid, RID()); - } else { - PhysicsServer::get_singleton()->body_set_space(rid, RID()); - } - - } break; - } -} - -void CollisionObject::set_collision_layer(uint32_t p_layer) { - collision_layer = p_layer; - if (area) { - PhysicsServer::get_singleton()->area_set_collision_layer(get_rid(), p_layer); - } else { - PhysicsServer::get_singleton()->body_set_collision_layer(get_rid(), p_layer); - } -} - -uint32_t CollisionObject::get_collision_layer() const { - return collision_layer; -} - -void CollisionObject::set_collision_mask(uint32_t p_mask) { - collision_mask = p_mask; - if (area) { - PhysicsServer::get_singleton()->area_set_collision_mask(get_rid(), p_mask); - } else { - PhysicsServer::get_singleton()->body_set_collision_mask(get_rid(), p_mask); - } -} - -uint32_t CollisionObject::get_collision_mask() const { - return collision_mask; -} - -void CollisionObject::set_collision_layer_bit(int p_bit, bool p_value) { - ERR_FAIL_INDEX_MSG(p_bit, 32, "Collision layer bit must be between 0 and 31 inclusive."); - uint32_t collision_layer = get_collision_layer(); - if (p_value) { - collision_layer |= 1 << p_bit; - } else { - collision_layer &= ~(1 << p_bit); - } - set_collision_layer(collision_layer); -} - -bool CollisionObject::get_collision_layer_bit(int p_bit) const { - ERR_FAIL_INDEX_V_MSG(p_bit, 32, false, "Collision layer bit must be between 0 and 31 inclusive."); - return get_collision_layer() & (1 << p_bit); -} - -void CollisionObject::set_collision_mask_bit(int p_bit, bool p_value) { - ERR_FAIL_INDEX_MSG(p_bit, 32, "Collision mask bit must be between 0 and 31 inclusive."); - uint32_t mask = get_collision_mask(); - if (p_value) { - mask |= 1 << p_bit; - } else { - mask &= ~(1 << p_bit); - } - set_collision_mask(mask); -} - -bool CollisionObject::get_collision_mask_bit(int p_bit) const { - ERR_FAIL_INDEX_V_MSG(p_bit, 32, false, "Collision mask bit must be between 0 and 31 inclusive."); - return get_collision_mask() & (1 << p_bit); -} - -void CollisionObject::_input_event(Node *p_camera, const Ref &p_input_event, const Vector3 &p_pos, const Vector3 &p_normal, int p_shape) { - if (get_script_instance()) { - get_script_instance()->call(SceneStringNames::get_singleton()->_input_event, p_camera, p_input_event, p_pos, p_normal, p_shape); - } - emit_signal(SceneStringNames::get_singleton()->input_event, p_camera, p_input_event, p_pos, p_normal, p_shape); -} - -void CollisionObject::_mouse_enter() { - if (get_script_instance()) { - get_script_instance()->call(SceneStringNames::get_singleton()->_mouse_enter); - } - emit_signal(SceneStringNames::get_singleton()->mouse_entered); -} - -void CollisionObject::_mouse_exit() { - if (get_script_instance()) { - get_script_instance()->call(SceneStringNames::get_singleton()->_mouse_exit); - } - emit_signal(SceneStringNames::get_singleton()->mouse_exited); -} - -void CollisionObject::set_only_update_transform_changes(bool p_enable) { - only_update_transform_changes = p_enable; -} - -void CollisionObject::_update_pickable() { - if (!is_inside_tree()) { - return; - } - - bool pickable = ray_pickable && is_visible_in_tree(); - if (area) { - PhysicsServer::get_singleton()->area_set_ray_pickable(rid, pickable); - } else { - PhysicsServer::get_singleton()->body_set_ray_pickable(rid, pickable); - } -} - -bool CollisionObject::_are_collision_shapes_visible() { - return is_inside_tree() && get_tree()->is_debugging_collisions_hint() && !Engine::get_singleton()->is_editor_hint(); -} - -void CollisionObject::_update_shape_data(uint32_t p_owner) { - if (_are_collision_shapes_visible()) { - if (debug_shapes_to_update.empty()) { - call_deferred("_update_debug_shapes"); - } - debug_shapes_to_update.insert(p_owner); - } -} - -void CollisionObject::_shape_changed(const Ref &p_shape) { - for (RBMap::Element *E = shapes.front(); E; E = E->next()) { - ShapeData &shapedata = E->get(); - ShapeData::ShapeBase *shapes = shapedata.shapes.ptrw(); - for (int i = 0; i < shapedata.shapes.size(); i++) { - ShapeData::ShapeBase &s = shapes[i]; - if (s.shape == p_shape && s.debug_shape.is_valid()) { - Ref mesh = s.shape->get_debug_mesh(); - RS::get_singleton()->instance_set_base(s.debug_shape, mesh->get_rid()); - } - } - } -} - -void CollisionObject::_update_debug_shapes() { - if (!is_inside_tree()) { - debug_shapes_to_update.clear(); - return; - } - - for (RBSet::Element *shapedata_idx = debug_shapes_to_update.front(); shapedata_idx; shapedata_idx = shapedata_idx->next()) { - if (shapes.has(shapedata_idx->get())) { - ShapeData &shapedata = shapes[shapedata_idx->get()]; - ShapeData::ShapeBase *shapes = shapedata.shapes.ptrw(); - for (int i = 0; i < shapedata.shapes.size(); i++) { - ShapeData::ShapeBase &s = shapes[i]; - if (s.shape.is_null() || shapedata.disabled) { - if (s.debug_shape.is_valid()) { - RS::get_singleton()->free(s.debug_shape); - s.debug_shape = RID(); - --debug_shapes_count; - } - } - if (!s.debug_shape.is_valid()) { - s.debug_shape = RID_PRIME(RS::get_singleton()->instance_create()); - RS::get_singleton()->instance_set_scenario(s.debug_shape, get_world_3d()->get_scenario()); - - if (!s.shape->is_connected("changed", this, "_shape_changed")) { - s.shape->connect("changed", this, "_shape_changed", varray(s.shape), CONNECT_DEFERRED); - } - - ++debug_shapes_count; - } - - Ref mesh = s.shape->get_debug_mesh(); - RS::get_singleton()->instance_set_base(s.debug_shape, mesh->get_rid()); - RS::get_singleton()->instance_set_transform(s.debug_shape, get_global_transform() * shapedata.xform); - RS::get_singleton()->instance_set_portal_mode(s.debug_shape, RenderingServer::INSTANCE_PORTAL_MODE_GLOBAL); - } - } - } - debug_shapes_to_update.clear(); -} - -void CollisionObject::_clear_debug_shapes() { - for (RBMap::Element *E = shapes.front(); E; E = E->next()) { - ShapeData &shapedata = E->get(); - ShapeData::ShapeBase *shapes = shapedata.shapes.ptrw(); - for (int i = 0; i < shapedata.shapes.size(); i++) { - ShapeData::ShapeBase &s = shapes[i]; - if (s.debug_shape.is_valid()) { - RS::get_singleton()->free(s.debug_shape); - s.debug_shape = RID(); - if (s.shape.is_valid() && s.shape->is_connected("changed", this, "_shape_changed")) { - s.shape->disconnect("changed", this, "_shape_changed"); - } - } - } - } - - debug_shapes_count = 0; -} - -void CollisionObject::_on_transform_changed() { - if (debug_shapes_count > 0 && !debug_shape_old_transform.is_equal_approx(get_global_transform())) { - debug_shape_old_transform = get_global_transform(); - for (RBMap::Element *E = shapes.front(); E; E = E->next()) { - ShapeData &shapedata = E->get(); - const ShapeData::ShapeBase *shapes = shapedata.shapes.ptr(); - for (int i = 0; i < shapedata.shapes.size(); i++) { - RS::get_singleton()->instance_set_transform(shapes[i].debug_shape, debug_shape_old_transform * shapedata.xform); - } - } - } -} - -void CollisionObject::set_ray_pickable(bool p_ray_pickable) { - ray_pickable = p_ray_pickable; - _update_pickable(); -} - -bool CollisionObject::is_ray_pickable() const { - return ray_pickable; -} - -void CollisionObject::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_collision_layer", "layer"), &CollisionObject::set_collision_layer); - ClassDB::bind_method(D_METHOD("get_collision_layer"), &CollisionObject::get_collision_layer); - ClassDB::bind_method(D_METHOD("set_collision_mask", "mask"), &CollisionObject::set_collision_mask); - ClassDB::bind_method(D_METHOD("get_collision_mask"), &CollisionObject::get_collision_mask); - ClassDB::bind_method(D_METHOD("set_collision_layer_bit", "bit", "value"), &CollisionObject::set_collision_layer_bit); - ClassDB::bind_method(D_METHOD("get_collision_layer_bit", "bit"), &CollisionObject::get_collision_layer_bit); - ClassDB::bind_method(D_METHOD("set_collision_mask_bit", "bit", "value"), &CollisionObject::set_collision_mask_bit); - ClassDB::bind_method(D_METHOD("get_collision_mask_bit", "bit"), &CollisionObject::get_collision_mask_bit); - ClassDB::bind_method(D_METHOD("set_ray_pickable", "ray_pickable"), &CollisionObject::set_ray_pickable); - ClassDB::bind_method(D_METHOD("is_ray_pickable"), &CollisionObject::is_ray_pickable); - ClassDB::bind_method(D_METHOD("set_capture_input_on_drag", "enable"), &CollisionObject::set_capture_input_on_drag); - ClassDB::bind_method(D_METHOD("get_capture_input_on_drag"), &CollisionObject::get_capture_input_on_drag); - ClassDB::bind_method(D_METHOD("get_rid"), &CollisionObject::get_rid); - ClassDB::bind_method(D_METHOD("create_shape_owner", "owner"), &CollisionObject::create_shape_owner); - ClassDB::bind_method(D_METHOD("remove_shape_owner", "owner_id"), &CollisionObject::remove_shape_owner); - ClassDB::bind_method(D_METHOD("get_shape_owners"), &CollisionObject::_get_shape_owners); - ClassDB::bind_method(D_METHOD("shape_owner_set_transform", "owner_id", "transform"), &CollisionObject::shape_owner_set_transform); - ClassDB::bind_method(D_METHOD("shape_owner_get_transform", "owner_id"), &CollisionObject::shape_owner_get_transform); - ClassDB::bind_method(D_METHOD("shape_owner_get_owner", "owner_id"), &CollisionObject::shape_owner_get_owner); - ClassDB::bind_method(D_METHOD("shape_owner_set_disabled", "owner_id", "disabled"), &CollisionObject::shape_owner_set_disabled); - ClassDB::bind_method(D_METHOD("is_shape_owner_disabled", "owner_id"), &CollisionObject::is_shape_owner_disabled); - ClassDB::bind_method(D_METHOD("shape_owner_add_shape", "owner_id", "shape"), &CollisionObject::shape_owner_add_shape); - ClassDB::bind_method(D_METHOD("shape_owner_get_shape_count", "owner_id"), &CollisionObject::shape_owner_get_shape_count); - ClassDB::bind_method(D_METHOD("shape_owner_get_shape", "owner_id", "shape_id"), &CollisionObject::shape_owner_get_shape); - ClassDB::bind_method(D_METHOD("shape_owner_get_shape_index", "owner_id", "shape_id"), &CollisionObject::shape_owner_get_shape_index); - ClassDB::bind_method(D_METHOD("shape_owner_remove_shape", "owner_id", "shape_id"), &CollisionObject::shape_owner_remove_shape); - ClassDB::bind_method(D_METHOD("shape_owner_clear_shapes", "owner_id"), &CollisionObject::shape_owner_clear_shapes); - ClassDB::bind_method(D_METHOD("shape_find_owner", "shape_index"), &CollisionObject::shape_find_owner); - - ClassDB::bind_method(D_METHOD("_update_debug_shapes"), &CollisionObject::_update_debug_shapes); - ClassDB::bind_method(D_METHOD("_shape_changed", "shape"), &CollisionObject::_shape_changed); - - BIND_VMETHOD(MethodInfo("_input_event", PropertyInfo(Variant::OBJECT, "camera"), PropertyInfo(Variant::OBJECT, "event", PROPERTY_HINT_RESOURCE_TYPE, "InputEvent"), PropertyInfo(Variant::VECTOR3, "position"), PropertyInfo(Variant::VECTOR3, "normal"), PropertyInfo(Variant::INT, "shape_idx"))); - - ADD_SIGNAL(MethodInfo("input_event", PropertyInfo(Variant::OBJECT, "camera", PROPERTY_HINT_RESOURCE_TYPE, "Node"), PropertyInfo(Variant::OBJECT, "event", PROPERTY_HINT_RESOURCE_TYPE, "InputEvent"), PropertyInfo(Variant::VECTOR3, "position"), PropertyInfo(Variant::VECTOR3, "normal"), PropertyInfo(Variant::INT, "shape_idx"))); - ADD_SIGNAL(MethodInfo("mouse_entered")); - ADD_SIGNAL(MethodInfo("mouse_exited")); - - ADD_GROUP("Collision", "collision_"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "collision_layer", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_collision_layer", "get_collision_layer"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "collision_mask", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_collision_mask", "get_collision_mask"); - - ADD_GROUP("Input", "input_"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "input_ray_pickable"), "set_ray_pickable", "is_ray_pickable"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "input_capture_on_drag"), "set_capture_input_on_drag", "get_capture_input_on_drag"); -} - -uint32_t CollisionObject::create_shape_owner(Object *p_owner) { - ShapeData sd; - uint32_t id; - - if (shapes.size() == 0) { - id = 0; - } else { - id = shapes.back()->key() + 1; - } - - sd.owner_id = p_owner ? p_owner->get_instance_id() : 0; - - shapes[id] = sd; - - return id; -} - -void CollisionObject::remove_shape_owner(uint32_t owner) { - ERR_FAIL_COND(!shapes.has(owner)); - - shape_owner_clear_shapes(owner); - - shapes.erase(owner); -} - -void CollisionObject::shape_owner_set_disabled(uint32_t p_owner, bool p_disabled) { - ERR_FAIL_COND(!shapes.has(p_owner)); - - ShapeData &sd = shapes[p_owner]; - if (sd.disabled == p_disabled) { - return; - } - sd.disabled = p_disabled; - - for (int i = 0; i < sd.shapes.size(); i++) { - if (area) { - PhysicsServer::get_singleton()->area_set_shape_disabled(rid, sd.shapes[i].index, p_disabled); - } else { - PhysicsServer::get_singleton()->body_set_shape_disabled(rid, sd.shapes[i].index, p_disabled); - } - } - _update_shape_data(p_owner); -} - -bool CollisionObject::is_shape_owner_disabled(uint32_t p_owner) const { - ERR_FAIL_COND_V(!shapes.has(p_owner), false); - - return shapes[p_owner].disabled; -} - -void CollisionObject::get_shape_owners(List *r_owners) { - for (RBMap::Element *E = shapes.front(); E; E = E->next()) { - r_owners->push_back(E->key()); - } -} - -Array CollisionObject::_get_shape_owners() { - Array ret; - for (RBMap::Element *E = shapes.front(); E; E = E->next()) { - ret.push_back(E->key()); - } - - return ret; -} - -void CollisionObject::shape_owner_set_transform(uint32_t p_owner, const Transform &p_transform) { - ERR_FAIL_COND(!shapes.has(p_owner)); - - ShapeData &sd = shapes[p_owner]; - sd.xform = p_transform; - for (int i = 0; i < sd.shapes.size(); i++) { - if (area) { - PhysicsServer::get_singleton()->area_set_shape_transform(rid, sd.shapes[i].index, p_transform); - } else { - PhysicsServer::get_singleton()->body_set_shape_transform(rid, sd.shapes[i].index, p_transform); - } - } - - _update_shape_data(p_owner); -} -Transform CollisionObject::shape_owner_get_transform(uint32_t p_owner) const { - ERR_FAIL_COND_V(!shapes.has(p_owner), Transform()); - - return shapes[p_owner].xform; -} - -Object *CollisionObject::shape_owner_get_owner(uint32_t p_owner) const { - ERR_FAIL_COND_V(!shapes.has(p_owner), nullptr); - - return ObjectDB::get_instance(shapes[p_owner].owner_id); -} - -void CollisionObject::shape_owner_add_shape(uint32_t p_owner, const Ref &p_shape) { - ERR_FAIL_COND(!shapes.has(p_owner)); - ERR_FAIL_COND(p_shape.is_null()); - - ShapeData &sd = shapes[p_owner]; - ShapeData::ShapeBase s; - s.index = total_subshapes; - s.shape = p_shape; - - if (area) { - PhysicsServer::get_singleton()->area_add_shape(rid, p_shape->get_rid(), sd.xform, sd.disabled); - } else { - PhysicsServer::get_singleton()->body_add_shape(rid, p_shape->get_rid(), sd.xform, sd.disabled); - } - sd.shapes.push_back(s); - - total_subshapes++; - - _update_shape_data(p_owner); -} -int CollisionObject::shape_owner_get_shape_count(uint32_t p_owner) const { - ERR_FAIL_COND_V(!shapes.has(p_owner), 0); - - return shapes[p_owner].shapes.size(); -} -Ref CollisionObject::shape_owner_get_shape(uint32_t p_owner, int p_shape) const { - ERR_FAIL_COND_V(!shapes.has(p_owner), Ref()); - ERR_FAIL_INDEX_V(p_shape, shapes[p_owner].shapes.size(), Ref()); - - return shapes[p_owner].shapes[p_shape].shape; -} -int CollisionObject::shape_owner_get_shape_index(uint32_t p_owner, int p_shape) const { - ERR_FAIL_COND_V(!shapes.has(p_owner), -1); - ERR_FAIL_INDEX_V(p_shape, shapes[p_owner].shapes.size(), -1); - - return shapes[p_owner].shapes[p_shape].index; -} - -void CollisionObject::shape_owner_remove_shape(uint32_t p_owner, int p_shape) { - ERR_FAIL_COND(!shapes.has(p_owner)); - ERR_FAIL_INDEX(p_shape, shapes[p_owner].shapes.size()); - - ShapeData::ShapeBase &s = shapes[p_owner].shapes.write[p_shape]; - int index_to_remove = s.index; - - if (area) { - PhysicsServer::get_singleton()->area_remove_shape(rid, index_to_remove); - } else { - PhysicsServer::get_singleton()->body_remove_shape(rid, index_to_remove); - } - - if (s.debug_shape.is_valid()) { - RS::get_singleton()->free(s.debug_shape); - if (s.shape.is_valid() && s.shape->is_connected("changed", this, "_shape_changed")) { - s.shape->disconnect("changed", this, "_shape_changed"); - } - --debug_shapes_count; - } - - shapes[p_owner].shapes.remove(p_shape); - - for (RBMap::Element *E = shapes.front(); E; E = E->next()) { - for (int i = 0; i < E->get().shapes.size(); i++) { - if (E->get().shapes[i].index > index_to_remove) { - E->get().shapes.write[i].index -= 1; - } - } - } - - total_subshapes--; -} - -void CollisionObject::shape_owner_clear_shapes(uint32_t p_owner) { - ERR_FAIL_COND(!shapes.has(p_owner)); - - while (shape_owner_get_shape_count(p_owner) > 0) { - shape_owner_remove_shape(p_owner, 0); - } -} - -uint32_t CollisionObject::shape_find_owner(int p_shape_index) const { - ERR_FAIL_INDEX_V(p_shape_index, total_subshapes, UINT32_MAX); - - for (const RBMap::Element *E = shapes.front(); E; E = E->next()) { - for (int i = 0; i < E->get().shapes.size(); i++) { - if (E->get().shapes[i].index == p_shape_index) { - return E->key(); - } - } - } - - //in theory it should be unreachable - ERR_FAIL_V_MSG(UINT32_MAX, "Can't find owner for shape index " + itos(p_shape_index) + "."); -} - -CollisionObject::CollisionObject(RID p_rid, bool p_area) { - rid = p_rid; - area = p_area; - capture_input_on_drag = false; - ray_pickable = true; - set_notify_transform(true); - total_subshapes = 0; - - if (p_area) { - PhysicsServer::get_singleton()->area_attach_object_instance_id(rid, get_instance_id()); - } else { - PhysicsServer::get_singleton()->body_attach_object_instance_id(rid, get_instance_id()); - } - //set_transform_notify(true); -} - -void CollisionObject::set_capture_input_on_drag(bool p_capture) { - capture_input_on_drag = p_capture; -} - -bool CollisionObject::get_capture_input_on_drag() const { - return capture_input_on_drag; -} - -String CollisionObject::get_configuration_warning() const { - String warning = Spatial::get_configuration_warning(); - - if (shapes.empty()) { - if (!warning.empty()) { - warning += "\n\n"; - } - warning += TTR("This node has no shape, so it can't collide or interact with other objects.\nConsider adding a CollisionShape or CollisionPolygon as a child to define its shape."); - } - - return warning; -} - -CollisionObject::CollisionObject() { - capture_input_on_drag = false; - ray_pickable = true; - set_notify_transform(true); - //owner= - - //set_transform_notify(true); -} - -CollisionObject::~CollisionObject() { - PhysicsServer::get_singleton()->free(rid); -} diff --git a/scene/3d/collision_object.h b/scene/3d/collision_object.h deleted file mode 100644 index 9c041a9..0000000 --- a/scene/3d/collision_object.h +++ /dev/null @@ -1,148 +0,0 @@ -#ifndef COLLISION_OBJECT_H -#define COLLISION_OBJECT_H -/*************************************************************************/ -/* collision_object.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "core/object/reference.h" -#include "scene/main/spatial.h" -#include "scene/resources/shapes/shape.h" - -class CollisionObject : public Spatial { - GDCLASS(CollisionObject, Spatial); - - uint32_t collision_layer = 1; - uint32_t collision_mask = 1; - - bool area; - - RID rid; - - struct ShapeData { - ObjectID owner_id; - Transform xform; - struct ShapeBase { - RID debug_shape; - Ref shape; - int index; - }; - - Vector shapes; - bool disabled; - - ShapeData() { - disabled = false; - owner_id = 0; - } - }; - - int total_subshapes; - - RBMap shapes; - bool only_update_transform_changes = false; //this is used for sync physics in KinematicBody - - bool capture_input_on_drag; - bool ray_pickable; - - RBSet debug_shapes_to_update; - int debug_shapes_count = 0; - Transform debug_shape_old_transform; - - void _update_pickable(); - - bool _are_collision_shapes_visible(); - void _update_shape_data(uint32_t p_owner); - void _shape_changed(const Ref &p_shape); - void _update_debug_shapes(); - void _clear_debug_shapes(); - -protected: - CollisionObject(RID p_rid, bool p_area); - - void _notification(int p_what); - static void _bind_methods(); - friend class Viewport; - virtual void _input_event(Node *p_camera, const Ref &p_input_event, const Vector3 &p_pos, const Vector3 &p_normal, int p_shape); - virtual void _mouse_enter(); - virtual void _mouse_exit(); - - void set_only_update_transform_changes(bool p_enable); - - void _on_transform_changed(); - -public: - void set_collision_layer(uint32_t p_layer); - uint32_t get_collision_layer() const; - - void set_collision_mask(uint32_t p_mask); - uint32_t get_collision_mask() const; - - void set_collision_layer_bit(int p_bit, bool p_value); - bool get_collision_layer_bit(int p_bit) const; - - void set_collision_mask_bit(int p_bit, bool p_value); - bool get_collision_mask_bit(int p_bit) const; - - uint32_t create_shape_owner(Object *p_owner); - void remove_shape_owner(uint32_t owner); - void get_shape_owners(List *r_owners); - Array _get_shape_owners(); - - void shape_owner_set_transform(uint32_t p_owner, const Transform &p_transform); - Transform shape_owner_get_transform(uint32_t p_owner) const; - Object *shape_owner_get_owner(uint32_t p_owner) const; - - void shape_owner_set_disabled(uint32_t p_owner, bool p_disabled); - bool is_shape_owner_disabled(uint32_t p_owner) const; - - void shape_owner_add_shape(uint32_t p_owner, const Ref &p_shape); - int shape_owner_get_shape_count(uint32_t p_owner) const; - Ref shape_owner_get_shape(uint32_t p_owner, int p_shape) const; - int shape_owner_get_shape_index(uint32_t p_owner, int p_shape) const; - - void shape_owner_remove_shape(uint32_t p_owner, int p_shape); - void shape_owner_clear_shapes(uint32_t p_owner); - - uint32_t shape_find_owner(int p_shape_index) const; - - void set_ray_pickable(bool p_ray_pickable); - bool is_ray_pickable() const; - - void set_capture_input_on_drag(bool p_capture); - bool get_capture_input_on_drag() const; - - _FORCE_INLINE_ RID get_rid() const { return rid; } - - virtual String get_configuration_warning() const; - - CollisionObject(); - ~CollisionObject(); -}; - -#endif // COLLISION_OBJECT__H diff --git a/scene/3d/collision_polygon.cpp b/scene/3d/collision_polygon.cpp deleted file mode 100644 index 159c2a5..0000000 --- a/scene/3d/collision_polygon.cpp +++ /dev/null @@ -1,221 +0,0 @@ -/*************************************************************************/ -/* collision_polygon.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_polygon.h" - -#include "collision_object.h" -#include "core/math/geometry.h" -#include "scene/resources/shapes/concave_polygon_shape.h" -#include "scene/resources/shapes/convex_polygon_shape.h" -#include "scene/resources/shapes/shape.h" - -void CollisionPolygon::_build_polygon() { - if (!parent) { - return; - } - - parent->shape_owner_clear_shapes(owner_id); - - if (polygon.size() == 0) { - return; - } - - Vector> decomp = Geometry::decompose_polygon_in_convex(polygon); - if (decomp.size() == 0) { - return; - } - - //here comes the sun, lalalala - //decompose concave into multiple convex polygons and add them - - for (int i = 0; i < decomp.size(); i++) { - Ref convex = memnew(ConvexPolygonShape); - PoolVector cp; - int cs = decomp[i].size(); - cp.resize(cs * 2); - { - PoolVector::Write w = cp.write(); - int idx = 0; - for (int j = 0; j < cs; j++) { - Vector2 d = decomp[i][j]; - w[idx++] = Vector3(d.x, d.y, depth * 0.5); - w[idx++] = Vector3(d.x, d.y, -depth * 0.5); - } - } - - convex->set_points(cp); - convex->set_margin(margin); - parent->shape_owner_add_shape(owner_id, convex); - parent->shape_owner_set_disabled(owner_id, disabled); - } -} - -void CollisionPolygon::_update_in_shape_owner(bool p_xform_only) { - parent->shape_owner_set_transform(owner_id, get_transform()); - if (p_xform_only) { - return; - } - parent->shape_owner_set_disabled(owner_id, disabled); -} - -void CollisionPolygon::_notification(int p_what) { - switch (p_what) { - case NOTIFICATION_PARENTED: { - parent = Object::cast_to(get_parent()); - if (parent) { - owner_id = parent->create_shape_owner(this); - _build_polygon(); - _update_in_shape_owner(); - } - } break; - case NOTIFICATION_ENTER_TREE: { - if (parent) { - _update_in_shape_owner(); - } - - } break; - case NOTIFICATION_LOCAL_TRANSFORM_CHANGED: { - if (parent) { - _update_in_shape_owner(true); - } - - } break; - case NOTIFICATION_UNPARENTED: { - if (parent) { - parent->remove_shape_owner(owner_id); - } - owner_id = 0; - parent = nullptr; - } break; - } -} - -void CollisionPolygon::set_polygon(const Vector &p_polygon) { - polygon = p_polygon; - if (parent) { - _build_polygon(); - } - update_configuration_warning(); - update_gizmos(); -} - -Vector CollisionPolygon::get_polygon() const { - return polygon; -} - -AABB CollisionPolygon::get_item_rect() const { - return aabb; -} - -void CollisionPolygon::set_depth(float p_depth) { - depth = p_depth; - _build_polygon(); - update_gizmos(); -} - -float CollisionPolygon::get_depth() const { - return depth; -} - -void CollisionPolygon::set_disabled(bool p_disabled) { - disabled = p_disabled; - update_gizmos(); - - if (parent) { - parent->shape_owner_set_disabled(owner_id, p_disabled); - } -} - -bool CollisionPolygon::is_disabled() const { - return disabled; -} - -real_t CollisionPolygon::get_margin() const { - return margin; -} - -void CollisionPolygon::set_margin(real_t p_margin) { - margin = p_margin; - if (parent) { - _build_polygon(); - } -} - -String CollisionPolygon::get_configuration_warning() const { - String warning = Spatial::get_configuration_warning(); - if (!Object::cast_to(get_parent())) { - if (warning != String()) { - warning += "\n\n"; - } - warning += TTR("CollisionPolygon only serves to provide a collision shape to a CollisionObject derived node. Please only use it as a child of Area, StaticBody, RigidBody, KinematicBody, etc. to give them a shape."); - } - - if (polygon.empty()) { - if (warning != String()) { - warning += "\n\n"; - } - warning += TTR("An empty CollisionPolygon has no effect on collision."); - } - - return warning; -} - -bool CollisionPolygon::_is_editable_3d_polygon() const { - return true; -} -void CollisionPolygon::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_depth", "depth"), &CollisionPolygon::set_depth); - ClassDB::bind_method(D_METHOD("get_depth"), &CollisionPolygon::get_depth); - - ClassDB::bind_method(D_METHOD("set_polygon", "polygon"), &CollisionPolygon::set_polygon); - ClassDB::bind_method(D_METHOD("get_polygon"), &CollisionPolygon::get_polygon); - - ClassDB::bind_method(D_METHOD("set_disabled", "disabled"), &CollisionPolygon::set_disabled); - ClassDB::bind_method(D_METHOD("is_disabled"), &CollisionPolygon::is_disabled); - - ClassDB::bind_method(D_METHOD("set_margin", "margin"), &CollisionPolygon::set_margin); - ClassDB::bind_method(D_METHOD("get_margin"), &CollisionPolygon::get_margin); - - ClassDB::bind_method(D_METHOD("_is_editable_3d_polygon"), &CollisionPolygon::_is_editable_3d_polygon); - - ADD_PROPERTY(PropertyInfo(Variant::REAL, "depth"), "set_depth", "get_depth"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "disabled"), "set_disabled", "is_disabled"); - ADD_PROPERTY(PropertyInfo(Variant::POOL_VECTOR2_ARRAY, "polygon"), "set_polygon", "get_polygon"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "margin", PROPERTY_HINT_RANGE, "0.001,10,0.001"), "set_margin", "get_margin"); -} - -CollisionPolygon::CollisionPolygon() { - aabb = AABB(Vector3(-1, -1, -1), Vector3(2, 2, 2)); - depth = 1.0; - set_notify_local_transform(true); - parent = nullptr; - owner_id = 0; - disabled = false; -} diff --git a/scene/3d/collision_polygon.h b/scene/3d/collision_polygon.h deleted file mode 100644 index 59ef88e..0000000 --- a/scene/3d/collision_polygon.h +++ /dev/null @@ -1,83 +0,0 @@ -#ifndef COLLISION_POLYGON_H -#define COLLISION_POLYGON_H -/*************************************************************************/ -/* collision_polygon.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "core/object/reference.h" -#include "scene/main/spatial.h" - -class CollisionObject; -class Shape; - -class CollisionPolygon : public Spatial { - GDCLASS(CollisionPolygon, Spatial); - real_t margin = 0.04; - -protected: - float depth; - AABB aabb; - Vector polygon; - - uint32_t owner_id; - CollisionObject *parent; - - bool disabled; - - void _build_polygon(); - - void _update_in_shape_owner(bool p_xform_only = false); - - bool _is_editable_3d_polygon() const; - -protected: - void _notification(int p_what); - static void _bind_methods(); - -public: - void set_depth(float p_depth); - float get_depth() const; - - void set_polygon(const Vector &p_polygon); - Vector get_polygon() const; - - void set_disabled(bool p_disabled); - bool is_disabled() const; - - virtual AABB get_item_rect() const; - - real_t get_margin() const; - void set_margin(real_t p_margin); - - String get_configuration_warning() const; - - CollisionPolygon(); -}; - -#endif // COLLISION_POLYGON_H diff --git a/scene/3d/collision_shape.cpp b/scene/3d/collision_shape.cpp deleted file mode 100644 index 8f1cc06..0000000 --- a/scene/3d/collision_shape.cpp +++ /dev/null @@ -1,227 +0,0 @@ -/*************************************************************************/ -/* collision_shape.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_shape.h" - -#include "core/math/quick_hull.h" -#include "mesh_instance.h" -#include "physics_body.h" -#include "scene/resources/shapes/box_shape.h" -#include "scene/resources/shapes/capsule_shape.h" -#include "scene/resources/shapes/concave_polygon_shape.h" -#include "scene/resources/shapes/convex_polygon_shape.h" -#include "scene/resources/mesh/mesh.h" -#include "scene/resources/shapes/plane_shape.h" -#include "scene/resources/shapes/ray_shape.h" -#include "scene/resources/shapes/shape.h" -#include "scene/resources/shapes/sphere_shape.h" -#include "servers/rendering_server.h" - -void CollisionShape::make_convex_from_brothers() { - Node *p = get_parent(); - if (!p) { - return; - } - - for (int i = 0; i < p->get_child_count(); i++) { - Node *n = p->get_child(i); - MeshInstance *mi = Object::cast_to(n); - if (mi) { - Ref m = mi->get_mesh(); - if (m.is_valid()) { - Ref s = m->create_convex_shape(); - set_shape(s); - } - } - } -} - -void CollisionShape::_update_in_shape_owner(bool p_xform_only) { - parent->shape_owner_set_transform(owner_id, get_transform()); - if (p_xform_only) { - return; - } - parent->shape_owner_set_disabled(owner_id, disabled); -} - -void CollisionShape::_notification(int p_what) { - switch (p_what) { - case NOTIFICATION_PARENTED: { - parent = Object::cast_to(get_parent()); - if (parent) { - owner_id = parent->create_shape_owner(this); - if (shape.is_valid()) { - parent->shape_owner_add_shape(owner_id, shape); - } - _update_in_shape_owner(); - } - } break; - case NOTIFICATION_ENTER_TREE: { - if (parent) { - _update_in_shape_owner(); - } - } break; - case NOTIFICATION_LOCAL_TRANSFORM_CHANGED: { - if (parent) { - _update_in_shape_owner(true); - } - } break; - case NOTIFICATION_UNPARENTED: { - if (parent) { - parent->remove_shape_owner(owner_id); - } - owner_id = 0; - parent = nullptr; - } break; - } -} - -void CollisionShape::resource_changed(RES res) { - update_gizmos(); -} - -String CollisionShape::get_configuration_warning() const { - String warning = Spatial::get_configuration_warning(); - - if (!Object::cast_to(get_parent())) { - if (warning != String()) { - warning += "\n\n"; - } - warning += TTR("CollisionShape only serves to provide a collision shape to a CollisionObject derived node. Please only use it as a child of Area, StaticBody, RigidBody, KinematicBody, etc. to give them a shape."); - } - - if (!shape.is_valid()) { - if (warning != String()) { - warning += "\n\n"; - } - warning += TTR("A shape must be provided for CollisionShape to function. Please create a shape resource for it."); - } else { - if (shape->is_class("PlaneShape")) { - if (warning != String()) { - warning += "\n\n"; - } - warning += TTR("Plane shapes don't work well and will be removed in future versions. Please don't use them."); - } - - if (Object::cast_to(get_parent()) && - Object::cast_to(*shape) && - Object::cast_to(get_parent())->get_mode() != RigidBody::MODE_STATIC) { - if (warning != String()) { - warning += "\n\n"; - } - warning += TTR("ConcavePolygonShape doesn't support RigidBody in another mode than static."); - } - } - - return warning; -} - -#ifdef TOOLS_ENABLED -AABB CollisionShape::get_fallback_gizmo_aabb() const { - if (shape.is_null()) { - return Spatial::get_fallback_gizmo_aabb(); - } - - // get_debug_mesh() is not const because the mesh is lazy initialized and cached. - // It would be better if we can mark the cache mutable and make get_debug_mesh() const. - return const_cast(this)->shape->get_debug_mesh()->get_aabb(); -} -#endif - -void CollisionShape::_bind_methods() { - //not sure if this should do anything - ClassDB::bind_method(D_METHOD("resource_changed", "resource"), &CollisionShape::resource_changed); - ClassDB::bind_method(D_METHOD("set_shape", "shape"), &CollisionShape::set_shape); - ClassDB::bind_method(D_METHOD("get_shape"), &CollisionShape::get_shape); - ClassDB::bind_method(D_METHOD("set_disabled", "enable"), &CollisionShape::set_disabled); - ClassDB::bind_method(D_METHOD("is_disabled"), &CollisionShape::is_disabled); - ClassDB::bind_method(D_METHOD("make_convex_from_brothers"), &CollisionShape::make_convex_from_brothers); - ClassDB::set_method_flags("CollisionShape", "make_convex_from_brothers", METHOD_FLAGS_DEFAULT | METHOD_FLAG_EDITOR); - - ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "shape", PROPERTY_HINT_RESOURCE_TYPE, "Shape"), "set_shape", "get_shape"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "disabled"), "set_disabled", "is_disabled"); -} - -void CollisionShape::set_shape(const Ref &p_shape) { - if (p_shape == shape) { - return; - } - if (!shape.is_null()) { - shape->unregister_owner(this); - } - shape = p_shape; - if (!shape.is_null()) { - shape->register_owner(this); - } - update_gizmos(); - if (parent) { - parent->shape_owner_clear_shapes(owner_id); - if (shape.is_valid()) { - parent->shape_owner_add_shape(owner_id, shape); - } - } - - if (is_inside_tree() && parent) { - // If this is a heightfield shape our center may have changed - _update_in_shape_owner(true); - } - update_configuration_warning(); -} - -Ref CollisionShape::get_shape() const { - return shape; -} - -void CollisionShape::set_disabled(bool p_disabled) { - disabled = p_disabled; - update_gizmos(); - if (parent) { - parent->shape_owner_set_disabled(owner_id, p_disabled); - } -} - -bool CollisionShape::is_disabled() const { - return disabled; -} - -CollisionShape::CollisionShape() { - //indicator = RID_PRIME(RenderingServer::get_singleton()->mesh_create()); - disabled = false; - parent = nullptr; - owner_id = 0; - set_notify_local_transform(true); -} - -CollisionShape::~CollisionShape() { - if (!shape.is_null()) { - shape->unregister_owner(this); - } - //RenderingServer::get_singleton()->free(indicator); -} diff --git a/scene/3d/collision_shape.h b/scene/3d/collision_shape.h deleted file mode 100644 index ae301d8..0000000 --- a/scene/3d/collision_shape.h +++ /dev/null @@ -1,77 +0,0 @@ -#ifndef COLLISION_SHAPE_H -#define COLLISION_SHAPE_H -/*************************************************************************/ -/* collision_shape.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "core/object/reference.h" -#include "scene/main/spatial.h" - -class CollisionObject; -class Shape; - -class CollisionShape : public Spatial { - GDCLASS(CollisionShape, Spatial); - OBJ_CATEGORY("3D Physics Nodes"); - - Ref shape; - - uint32_t owner_id; - CollisionObject *parent; - - void resource_changed(RES res); - bool disabled; - -protected: - void _update_in_shape_owner(bool p_xform_only = false); - -protected: - void _notification(int p_what); - static void _bind_methods(); - -public: - void make_convex_from_brothers(); - - void set_shape(const Ref &p_shape); - Ref get_shape() const; - - void set_disabled(bool p_disabled); - bool is_disabled() const; - - String get_configuration_warning() const; - -#ifdef TOOLS_ENABLED - virtual AABB get_fallback_gizmo_aabb() const; -#endif - - CollisionShape(); - ~CollisionShape(); -}; - -#endif // BODY_VOLUME_H diff --git a/scene/3d/mesh_instance.cpp b/scene/3d/mesh_instance.cpp index 0bf9d83..32c8a12 100644 --- a/scene/3d/mesh_instance.cpp +++ b/scene/3d/mesh_instance.cpp @@ -30,23 +30,14 @@ #include "mesh_instance.h" -#include "collision_shape.h" #include "core/config/project_settings.h" #include "core/core_string_names.h" -#include "physics_body.h" #include "scene/resources/material/material.h" #include "scene/resources/material/spatial_material.h" #include "scene/resources/mesh/mesh.h" #include "scene/main/scene_string_names.h" #include "servers/rendering/rendering_server_globals.h" -#include "modules/modules_enabled.gen.h" - -#ifdef MODULE_SKELETON_3D_ENABLED -#include "modules/skeleton_3d/nodes/skeleton.h" -#include "modules/skeleton_3d/resources/skin.h" -#endif - bool MeshInstance::_set(const StringName &p_name, const Variant &p_value) { //this is not _too_ bad performance wise, really. it only arrives here if the property was not set anywhere else. //add to it that it's probably found on first call to _set anyway. @@ -125,20 +116,6 @@ void MeshInstance::set_mesh(const Ref &p_mesh) { mesh->disconnect(CoreStringNames::get_singleton()->changed, this, SceneStringNames::get_singleton()->_mesh_changed); } -#ifdef MODULE_SKELETON_3D_ENABLED - if (skin_ref.is_valid() && mesh.is_valid() && _is_software_skinning_enabled() && is_visible_in_tree()) { - ERR_FAIL_COND(!skin_ref->get_skeleton_node()); - skin_ref->get_skeleton_node()->disconnect("pose_updated", this, "_update_skinning"); - } -#endif - -#ifdef MODULE_SKELETON_3D_ENABLED - if (software_skinning) { - memdelete(software_skinning); - software_skinning = nullptr; - } -#endif - mesh = p_mesh; blend_shape_tracks.clear(); @@ -168,402 +145,6 @@ Ref MeshInstance::get_mesh() const { return mesh; } -#ifdef MODULE_SKELETON_3D_ENABLED -void MeshInstance::_resolve_skeleton_path() { - Ref new_skin_reference; - - if (!skeleton_path.is_empty()) { - Skeleton *skeleton = Object::cast_to(get_node(skeleton_path)); - if (skeleton) { - if (skin_internal.is_null()) { - new_skin_reference = skeleton->register_skin(skeleton->create_skin_from_rest_transforms()); - //a skin was created for us - skin_internal = new_skin_reference->get_skin(); - _change_notify(); - } else { - new_skin_reference = skeleton->register_skin(skin_internal); - } - } - } - - if (skin_ref.is_valid() && mesh.is_valid() && _is_software_skinning_enabled() && is_visible_in_tree()) { - ERR_FAIL_COND(!skin_ref->get_skeleton_node()); - skin_ref->get_skeleton_node()->disconnect("pose_updated", this, "_update_skinning"); - } - - skin_ref = new_skin_reference; - - software_skinning_flags &= ~SoftwareSkinning::FLAG_BONES_READY; - - _initialize_skinning(); -} - -bool MeshInstance::_is_global_software_skinning_enabled() { - // Check if forced in project settings. - if (GLOBAL_GET("rendering/quality/skinning/force_software_skinning")) { - return true; - } - - // Check if enabled in project settings. - if (!GLOBAL_GET("rendering/quality/skinning/software_skinning_fallback")) { - return false; - } - - // Check if requested by renderer settings. - return RSG::storage->has_os_feature("skinning_fallback"); -} - -bool MeshInstance::_is_software_skinning_enabled() const { - // Using static local variable which will be initialized only once, - // so _is_global_software_skinning_enabled can be only called once on first use. - static bool global_software_skinning = _is_global_software_skinning_enabled(); - return global_software_skinning; -} - -void MeshInstance::_initialize_skinning(bool p_force_reset, bool p_call_attach_skeleton) { - if (mesh.is_null()) { - return; - } - - RenderingServer *rendering_server = RenderingServer::get_singleton(); - - bool update_mesh = false; - - if (skin_ref.is_valid()) { - if (_is_software_skinning_enabled()) { - if (is_visible_in_tree()) { - ERR_FAIL_COND(!skin_ref->get_skeleton_node()); - if (!skin_ref->get_skeleton_node()->is_connected("pose_updated", this, "_update_skinning")) { - skin_ref->get_skeleton_node()->connect("pose_updated", this, "_update_skinning"); - } - } - - if (p_force_reset && software_skinning) { - memdelete(software_skinning); - software_skinning = nullptr; - } - - if (!software_skinning) { - software_skinning = memnew(SoftwareSkinning); - - if (mesh->get_blend_shape_count() > 0) { - ERR_PRINT("Blend shapes are not supported for software skinning."); - } - - Ref software_mesh; - software_mesh.instance(); - RID mesh_rid = software_mesh->get_rid(); - - // Initialize mesh for dynamic update. - int surface_count = mesh->get_surface_count(); - software_skinning->surface_data.resize(surface_count); - for (int surface_index = 0; surface_index < surface_count; ++surface_index) { - ERR_CONTINUE(Mesh::PRIMITIVE_TRIANGLES != mesh->surface_get_primitive_type(surface_index)); - - SoftwareSkinning::SurfaceData &surface_data = software_skinning->surface_data[surface_index]; - surface_data.transform_tangents = false; - surface_data.ensure_correct_normals = false; - - uint32_t format = mesh->surface_get_format(surface_index); - ERR_CONTINUE(0 == (format & Mesh::ARRAY_FORMAT_VERTEX)); - ERR_CONTINUE(0 == (format & Mesh::ARRAY_FORMAT_BONES)); - ERR_CONTINUE(0 == (format & Mesh::ARRAY_FORMAT_WEIGHTS)); - - format |= Mesh::ARRAY_FLAG_USE_DYNAMIC_UPDATE; - format &= ~Mesh::ARRAY_COMPRESS_VERTEX; - format &= ~Mesh::ARRAY_COMPRESS_WEIGHTS; - format &= ~Mesh::ARRAY_FLAG_USE_16_BIT_BONES; - - Array write_arrays = mesh->surface_get_arrays(surface_index); - Array read_arrays; - read_arrays.resize(Mesh::ARRAY_MAX); - - read_arrays[Mesh::ARRAY_VERTEX] = write_arrays[Mesh::ARRAY_VERTEX]; - read_arrays[Mesh::ARRAY_BONES] = write_arrays[Mesh::ARRAY_BONES]; - read_arrays[Mesh::ARRAY_WEIGHTS] = write_arrays[Mesh::ARRAY_WEIGHTS]; - - write_arrays[Mesh::ARRAY_BONES] = Variant(); - write_arrays[Mesh::ARRAY_WEIGHTS] = Variant(); - - if (software_skinning_flags & SoftwareSkinning::FLAG_TRANSFORM_NORMALS) { - ERR_CONTINUE(0 == (format & Mesh::ARRAY_FORMAT_NORMAL)); - format &= ~Mesh::ARRAY_COMPRESS_NORMAL; - - read_arrays[Mesh::ARRAY_NORMAL] = write_arrays[Mesh::ARRAY_NORMAL]; - - Ref mat = get_active_material(surface_index); - if (mat.is_valid()) { - Ref spatial_mat = mat; - if (spatial_mat.is_valid()) { - // Spatial material, check from material settings. - surface_data.transform_tangents = spatial_mat->get_feature(SpatialMaterial::FEATURE_NORMAL_MAPPING); - surface_data.ensure_correct_normals = spatial_mat->get_flag(SpatialMaterial::FLAG_ENSURE_CORRECT_NORMALS); - } else { - // Custom shader, must check for compiled flags. - surface_data.transform_tangents = RSG::storage->material_uses_tangents(mat->get_rid()); - surface_data.ensure_correct_normals = RSG::storage->material_uses_ensure_correct_normals(mat->get_rid()); - } - } - - if (surface_data.transform_tangents) { - ERR_CONTINUE(0 == (format & Mesh::ARRAY_FORMAT_TANGENT)); - format &= ~Mesh::ARRAY_COMPRESS_TANGENT; - - read_arrays[Mesh::ARRAY_TANGENT] = write_arrays[Mesh::ARRAY_TANGENT]; - } - } - - // 1. Temporarily add surface with bone data to create the read buffer. - software_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, read_arrays, Array(), format); - - PoolByteArray buffer_read = rendering_server->mesh_surface_get_array(mesh_rid, surface_index); - surface_data.source_buffer.append_array(buffer_read); - surface_data.source_format = software_mesh->surface_get_format(surface_index); - - software_mesh->surface_remove(surface_index); - - // 2. Create the surface again without the bone data for the write buffer. - software_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, write_arrays, Array(), format); - - Ref material = mesh->surface_get_material(surface_index); - software_mesh->surface_set_material(surface_index, material); - - surface_data.buffer = rendering_server->mesh_surface_get_array(mesh_rid, surface_index); - surface_data.buffer_write = surface_data.buffer.write(); - } - - software_skinning->mesh_instance = software_mesh; - update_mesh = true; - } - - if (p_call_attach_skeleton) { - rendering_server->instance_attach_skeleton(get_instance(), RID()); - } - - if (is_visible_in_tree() && (software_skinning_flags & SoftwareSkinning::FLAG_BONES_READY)) { - // Initialize from current skeleton pose. - _update_skinning(); - } - } else { - ERR_FAIL_COND(!skin_ref->get_skeleton_node()); - if (skin_ref->get_skeleton_node()->is_connected("pose_updated", this, "_update_skinning")) { - skin_ref->get_skeleton_node()->disconnect("pose_updated", this, "_update_skinning"); - } - - if (p_call_attach_skeleton) { - rendering_server->instance_attach_skeleton(get_instance(), skin_ref->get_skeleton()); - } - - if (software_skinning) { - memdelete(software_skinning); - software_skinning = nullptr; - update_mesh = true; - } - } - } else { - if (p_call_attach_skeleton) { - rendering_server->instance_attach_skeleton(get_instance(), RID()); - } - if (software_skinning) { - memdelete(software_skinning); - software_skinning = nullptr; - update_mesh = true; - } - } - - RID render_mesh = software_skinning ? software_skinning->mesh_instance->get_rid() : mesh->get_rid(); - if (update_mesh || (render_mesh != get_base())) { - set_base(render_mesh); - - // Update instance materials after switching mesh. - int surface_count = mesh->get_surface_count(); - for (int surface_index = 0; surface_index < surface_count; ++surface_index) { - if (materials[surface_index].is_valid()) { - rendering_server->instance_set_surface_material(get_instance(), surface_index, materials[surface_index]->get_rid()); - } - } - } -} - -void MeshInstance::_update_skinning() { - ERR_FAIL_COND(!_is_software_skinning_enabled()); -#if defined(TOOLS_ENABLED) && defined(DEBUG_ENABLED) - ERR_FAIL_COND(!is_visible_in_tree()); -#else - ERR_FAIL_COND(!is_visible()); -#endif - - ERR_FAIL_COND(!software_skinning); - Ref software_skinning_mesh = software_skinning->mesh_instance; - ERR_FAIL_COND(!software_skinning_mesh.is_valid()); - RID mesh_rid = software_skinning_mesh->get_rid(); - ERR_FAIL_COND(!mesh_rid.is_valid()); - - ERR_FAIL_COND(!mesh.is_valid()); - RID source_mesh_rid = mesh->get_rid(); - ERR_FAIL_COND(!source_mesh_rid.is_valid()); - - ERR_FAIL_COND(skin_ref.is_null()); - RID skeleton = skin_ref->get_skeleton(); - ERR_FAIL_COND(!skeleton.is_valid()); - - Vector3 aabb_min = Vector3(FLT_MAX, FLT_MAX, FLT_MAX); - Vector3 aabb_max = Vector3(-FLT_MAX, -FLT_MAX, -FLT_MAX); - - RenderingServer *rendering_server = RenderingServer::get_singleton(); - - // Prepare bone transforms. - const int num_bones = rendering_server->skeleton_get_bone_count(skeleton); - ERR_FAIL_COND(num_bones <= 0); - Transform *bone_transforms = (Transform *)alloca(sizeof(Transform) * num_bones); - for (int bone_index = 0; bone_index < num_bones; ++bone_index) { - bone_transforms[bone_index] = rendering_server->skeleton_bone_get_transform(skeleton, bone_index); - } - - // Apply skinning. - int surface_count = software_skinning_mesh->get_surface_count(); - for (int surface_index = 0; surface_index < surface_count; ++surface_index) { - ERR_CONTINUE((uint32_t)surface_index >= software_skinning->surface_data.size()); - const SoftwareSkinning::SurfaceData &surface_data = software_skinning->surface_data[surface_index]; - const bool transform_tangents = surface_data.transform_tangents; - const bool ensure_correct_normals = surface_data.ensure_correct_normals; - - const uint32_t format_write = software_skinning_mesh->surface_get_format(surface_index); - - const int vertex_count_write = software_skinning_mesh->surface_get_array_len(surface_index); - const int index_count_write = software_skinning_mesh->surface_get_array_index_len(surface_index); - - uint32_t array_offsets_write[Mesh::ARRAY_MAX]; - uint32_t array_strides_write[Mesh::ARRAY_MAX]; - rendering_server->mesh_surface_make_offsets_from_format(format_write, vertex_count_write, index_count_write, array_offsets_write, array_strides_write); - ERR_FAIL_COND(array_strides_write[Mesh::ARRAY_VERTEX] != array_strides_write[Mesh::ARRAY_NORMAL]); - const uint32_t stride_write = array_strides_write[Mesh::ARRAY_VERTEX]; - const uint32_t offset_vertices_write = array_offsets_write[Mesh::ARRAY_VERTEX]; - const uint32_t offset_normals_write = array_offsets_write[Mesh::ARRAY_NORMAL]; - const uint32_t offset_tangents_write = array_offsets_write[Mesh::ARRAY_TANGENT]; - - PoolByteArray buffer_source = surface_data.source_buffer; - PoolByteArray::Read buffer_read = buffer_source.read(); - - const uint32_t format_read = surface_data.source_format; - - ERR_CONTINUE(0 == (format_read & Mesh::ARRAY_FORMAT_BONES)); - ERR_CONTINUE(0 == (format_read & Mesh::ARRAY_FORMAT_WEIGHTS)); - - const int vertex_count = mesh->surface_get_array_len(surface_index); - const int index_count = mesh->surface_get_array_index_len(surface_index); - - ERR_CONTINUE(vertex_count != vertex_count_write); - - uint32_t array_offsets[Mesh::ARRAY_MAX]; - uint32_t array_strides[Mesh::ARRAY_MAX]; - rendering_server->mesh_surface_make_offsets_from_format(format_read, vertex_count, index_count, array_offsets, array_strides); - ERR_FAIL_COND(array_strides[Mesh::ARRAY_VERTEX] != array_strides[Mesh::ARRAY_NORMAL]); - const uint32_t stride = array_strides[Mesh::ARRAY_VERTEX]; - const uint32_t offset_vertices = array_offsets[Mesh::ARRAY_VERTEX]; - const uint32_t offset_normals = array_offsets[Mesh::ARRAY_NORMAL]; - const uint32_t offset_tangents = array_offsets[Mesh::ARRAY_TANGENT]; - const uint32_t offset_bones = array_offsets[Mesh::ARRAY_BONES]; - const uint32_t offset_weights = array_offsets[Mesh::ARRAY_WEIGHTS]; - - PoolByteArray buffer = surface_data.buffer; - PoolByteArray::Write buffer_write = surface_data.buffer_write; - - for (int vertex_index = 0; vertex_index < vertex_count; ++vertex_index) { - const uint32_t vertex_offset = vertex_index * stride; - const uint32_t vertex_offset_write = vertex_index * stride_write; - - float bone_weights[4]; - const float *weight_ptr = (const float *)(buffer_read.ptr() + offset_weights + vertex_offset); - bone_weights[0] = weight_ptr[0]; - bone_weights[1] = weight_ptr[1]; - bone_weights[2] = weight_ptr[2]; - bone_weights[3] = weight_ptr[3]; - - const uint8_t *bones_ptr = buffer_read.ptr() + offset_bones + vertex_offset; - const int b0 = bones_ptr[0]; - const int b1 = bones_ptr[1]; - const int b2 = bones_ptr[2]; - const int b3 = bones_ptr[3]; - - Transform transform; - transform.origin = - bone_weights[0] * bone_transforms[b0].origin + - bone_weights[1] * bone_transforms[b1].origin + - bone_weights[2] * bone_transforms[b2].origin + - bone_weights[3] * bone_transforms[b3].origin; - - transform.basis = - bone_transforms[b0].basis * bone_weights[0] + - bone_transforms[b1].basis * bone_weights[1] + - bone_transforms[b2].basis * bone_weights[2] + - bone_transforms[b3].basis * bone_weights[3]; - - const Vector3 &vertex_read = (const Vector3 &)buffer_read[vertex_offset + offset_vertices]; - Vector3 &vertex = (Vector3 &)buffer_write[vertex_offset_write + offset_vertices_write]; - vertex = transform.xform(vertex_read); - - if (software_skinning_flags & SoftwareSkinning::FLAG_TRANSFORM_NORMALS) { - if (ensure_correct_normals) { - transform.basis.invert(); - transform.basis.transpose(); - } - - const Vector3 &normal_read = (const Vector3 &)buffer_read[vertex_offset + offset_normals]; - Vector3 &normal = (Vector3 &)buffer_write[vertex_offset_write + offset_normals_write]; - normal = transform.basis.xform(normal_read); - - if (transform_tangents) { - const Vector3 &tangent_read = (const Vector3 &)buffer_read[vertex_offset + offset_tangents]; - Vector3 &tangent = (Vector3 &)buffer_write[vertex_offset_write + offset_tangents_write]; - tangent = transform.basis.xform(tangent_read); - } - } - - aabb_min.x = MIN(aabb_min.x, vertex.x); - aabb_min.y = MIN(aabb_min.y, vertex.y); - aabb_min.z = MIN(aabb_min.z, vertex.z); - aabb_max.x = MAX(aabb_max.x, vertex.x); - aabb_max.y = MAX(aabb_max.y, vertex.y); - aabb_max.z = MAX(aabb_max.z, vertex.z); - } - - rendering_server->mesh_surface_update_region(mesh_rid, surface_index, 0, buffer); - } - - rendering_server->mesh_set_custom_aabb(mesh_rid, AABB(aabb_min, aabb_max - aabb_min)); - - software_skinning_flags |= SoftwareSkinning::FLAG_BONES_READY; -} -#endif - -#ifdef MODULE_SKELETON_3D_ENABLED -void MeshInstance::set_skin(const Ref &p_skin) { - skin_internal = p_skin; - skin = p_skin; - if (!is_inside_tree()) { - return; - } - _resolve_skeleton_path(); -} - -Ref MeshInstance::get_skin() const { - return skin; -} - -void MeshInstance::set_skeleton_path(const NodePath &p_skeleton) { - skeleton_path = p_skeleton; - if (!is_inside_tree()) { - return; - } - _resolve_skeleton_path(); -} - -NodePath MeshInstance::get_skeleton_path() { - return skeleton_path; -} -#endif - AABB MeshInstance::get_aabb() const { if (!mesh.is_null()) { return mesh->get_aabb(); @@ -585,123 +166,27 @@ PoolVector MeshInstance::get_faces(uint32_t p_usage_flags) const { } Node *MeshInstance::create_trimesh_collision_node() { - if (mesh.is_null()) { - return nullptr; - } - - Ref shape = mesh->create_trimesh_shape(); - if (shape.is_null()) { - return nullptr; - } - - StaticBody *static_body = memnew(StaticBody); - CollisionShape *cshape = memnew(CollisionShape); - cshape->set_shape(shape); - static_body->add_child(cshape); - return static_body; + return NULL; } void MeshInstance::create_trimesh_collision() { - StaticBody *static_body = Object::cast_to(create_trimesh_collision_node()); - ERR_FAIL_COND(!static_body); - static_body->set_name(String(get_name()) + "_col"); - - add_child(static_body); - if (get_owner()) { - CollisionShape *cshape = Object::cast_to(static_body->get_child(0)); - static_body->set_owner(get_owner()); - cshape->set_owner(get_owner()); - } } Node *MeshInstance::create_multiple_convex_collisions_node() { - if (mesh.is_null()) { - return nullptr; - } - - Vector> shapes = mesh->convex_decompose(); - if (!shapes.size()) { - return nullptr; - } - - StaticBody *static_body = memnew(StaticBody); - for (int i = 0; i < shapes.size(); i++) { - CollisionShape *cshape = memnew(CollisionShape); - cshape->set_shape(shapes[i]); - static_body->add_child(cshape); - } - return static_body; + return NULL; } void MeshInstance::create_multiple_convex_collisions() { - StaticBody *static_body = Object::cast_to(create_multiple_convex_collisions_node()); - ERR_FAIL_COND(!static_body); - static_body->set_name(String(get_name()) + "_col"); - - add_child(static_body); - if (get_owner()) { - static_body->set_owner(get_owner()); - int count = static_body->get_child_count(); - for (int i = 0; i < count; i++) { - CollisionShape *cshape = Object::cast_to(static_body->get_child(i)); - cshape->set_owner(get_owner()); - } - } } Node *MeshInstance::create_convex_collision_node(bool p_clean, bool p_simplify) { - if (mesh.is_null()) { - return nullptr; - } - - Ref shape = mesh->create_convex_shape(p_clean, p_simplify); - if (shape.is_null()) { - return nullptr; - } - - StaticBody *static_body = memnew(StaticBody); - CollisionShape *cshape = memnew(CollisionShape); - cshape->set_shape(shape); - static_body->add_child(cshape); - return static_body; + return NULL; } void MeshInstance::create_convex_collision(bool p_clean, bool p_simplify) { - StaticBody *static_body = Object::cast_to(create_convex_collision_node(p_clean, p_simplify)); - ERR_FAIL_COND(!static_body); - static_body->set_name(String(get_name()) + "_col"); - - add_child(static_body); - if (get_owner()) { - CollisionShape *cshape = Object::cast_to(static_body->get_child(0)); - static_body->set_owner(get_owner()); - cshape->set_owner(get_owner()); - } } void MeshInstance::_notification(int p_what) { -#ifdef MODULE_SKELETON_3D_ENABLED - if (p_what == NOTIFICATION_ENTER_TREE) { - _resolve_skeleton_path(); - } - - if (p_what == NOTIFICATION_TRANSLATION_CHANGED) { - if (mesh.is_valid()) { - mesh->notification(NOTIFICATION_TRANSLATION_CHANGED); - } - } - - if (p_what == NOTIFICATION_VISIBILITY_CHANGED) { - if (skin_ref.is_valid() && mesh.is_valid() && _is_software_skinning_enabled()) { - ERR_FAIL_COND(!skin_ref->get_skeleton_node()); - if (is_visible_in_tree()) { - skin_ref->get_skeleton_node()->connect("pose_updated", this, "_update_skinning"); - } else { - skin_ref->get_skeleton_node()->disconnect("pose_updated", this, "_update_skinning"); - } - } - } -#endif } int MeshInstance::get_surface_material_count() const { @@ -718,12 +203,6 @@ void MeshInstance::set_surface_material(int p_surface, const Ref &p_ma } else { RS::get_singleton()->instance_set_surface_material(get_instance(), p_surface, RID()); } - -#ifdef MODULE_SKELETON_3D_ENABLED - if (software_skinning) { - _initialize_skinning(true); - } -#endif } Ref MeshInstance::get_surface_material(int p_surface) const { @@ -757,12 +236,6 @@ void MeshInstance::set_material_override(const Ref &p_material) { } GeometryInstance::set_material_override(p_material); - -#ifdef MODULE_SKELETON_3D_ENABLED - if (software_skinning) { - _initialize_skinning(true); - } -#endif } void MeshInstance::set_material_overlay(const Ref &p_material) { @@ -773,37 +246,9 @@ void MeshInstance::set_material_overlay(const Ref &p_material) { GeometryInstance::set_material_overlay(p_material); } -#ifdef MODULE_SKELETON_3D_ENABLED -void MeshInstance::set_software_skinning_transform_normals(bool p_enabled) { - if (p_enabled == is_software_skinning_transform_normals_enabled()) { - return; - } - - if (p_enabled) { - software_skinning_flags |= SoftwareSkinning::FLAG_TRANSFORM_NORMALS; - } else { - software_skinning_flags &= ~SoftwareSkinning::FLAG_TRANSFORM_NORMALS; - } - - if (software_skinning) { - _initialize_skinning(true); - } -} - -bool MeshInstance::is_software_skinning_transform_normals_enabled() const { - return 0 != (software_skinning_flags & SoftwareSkinning::FLAG_TRANSFORM_NORMALS); -} -#endif - void MeshInstance::_mesh_changed() { ERR_FAIL_COND(mesh.is_null()); materials.resize(mesh->get_surface_count()); - -#ifdef MODULE_SKELETON_3D_ENABLED - if (software_skinning) { - _initialize_skinning(true); - } -#endif } void MeshInstance::create_debug_tangents() { @@ -872,14 +317,6 @@ void MeshInstance::create_debug_tangents() { mi->set_mesh(am); mi->set_name("DebugTangents"); add_child(mi); -#ifdef TOOLS_ENABLED - - if (is_inside_tree() && this == get_tree()->get_edited_scene_root()) { - mi->set_owner(this); - } else { - mi->set_owner(get_owner()); - } -#endif } } @@ -1376,28 +813,11 @@ void MeshInstance::_bind_methods() { ClassDB::bind_method(D_METHOD("get_mesh"), &MeshInstance::get_mesh); ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "mesh", PROPERTY_HINT_RESOURCE_TYPE, "Mesh"), "set_mesh", "get_mesh"); -#ifdef MODULE_SKELETON_3D_ENABLED - ClassDB::bind_method(D_METHOD("set_skeleton_path", "skeleton_path"), &MeshInstance::set_skeleton_path); - ClassDB::bind_method(D_METHOD("get_skeleton_path"), &MeshInstance::get_skeleton_path); - ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "skeleton", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Skeleton"), "set_skeleton_path", "get_skeleton_path"); - - ClassDB::bind_method(D_METHOD("set_skin", "skin"), &MeshInstance::set_skin); - ClassDB::bind_method(D_METHOD("get_skin"), &MeshInstance::get_skin); - ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "skin", PROPERTY_HINT_RESOURCE_TYPE, "Skin"), "set_skin", "get_skin"); -#endif - ClassDB::bind_method(D_METHOD("get_surface_material_count"), &MeshInstance::get_surface_material_count); ClassDB::bind_method(D_METHOD("set_surface_material", "surface", "material"), &MeshInstance::set_surface_material); ClassDB::bind_method(D_METHOD("get_surface_material", "surface"), &MeshInstance::get_surface_material); ClassDB::bind_method(D_METHOD("get_active_material", "surface"), &MeshInstance::get_active_material); -#ifdef MODULE_SKELETON_3D_ENABLED - ADD_GROUP("Software Skinning", "software_skinning"); - ClassDB::bind_method(D_METHOD("set_software_skinning_transform_normals", "enabled"), &MeshInstance::set_software_skinning_transform_normals); - ClassDB::bind_method(D_METHOD("is_software_skinning_transform_normals_enabled"), &MeshInstance::is_software_skinning_transform_normals_enabled); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "software_skinning_transform_normals"), "set_software_skinning_transform_normals", "is_software_skinning_transform_normals_enabled"); -#endif - ClassDB::bind_method(D_METHOD("create_trimesh_collision"), &MeshInstance::create_trimesh_collision); ClassDB::set_method_flags("MeshInstance", "create_trimesh_collision", METHOD_FLAGS_DEFAULT); ClassDB::bind_method(D_METHOD("create_multiple_convex_collisions"), &MeshInstance::create_multiple_convex_collisions); @@ -1406,10 +826,6 @@ void MeshInstance::_bind_methods() { ClassDB::set_method_flags("MeshInstance", "create_convex_collision", METHOD_FLAGS_DEFAULT); ClassDB::bind_method(D_METHOD("_mesh_changed"), &MeshInstance::_mesh_changed); -#ifdef MODULE_SKELETON_3D_ENABLED - ClassDB::bind_method(D_METHOD("_update_skinning"), &MeshInstance::_update_skinning); -#endif - ClassDB::bind_method(D_METHOD("create_debug_tangents"), &MeshInstance::create_debug_tangents); ClassDB::set_method_flags("MeshInstance", "create_debug_tangents", METHOD_FLAGS_DEFAULT | METHOD_FLAG_EDITOR); @@ -1419,18 +835,7 @@ void MeshInstance::_bind_methods() { } MeshInstance::MeshInstance() { -#ifdef MODULE_SKELETON_3D_ENABLED - skeleton_path = NodePath(".."); - software_skinning = nullptr; - software_skinning_flags = SoftwareSkinning::FLAG_TRANSFORM_NORMALS; -#endif } MeshInstance::~MeshInstance() { -#ifdef MODULE_SKELETON_3D_ENABLED - if (software_skinning) { - memdelete(software_skinning); - software_skinning = nullptr; - } -#endif } diff --git a/scene/3d/mesh_instance.h b/scene/3d/mesh_instance.h index cff75b3..5f1d45d 100644 --- a/scene/3d/mesh_instance.h +++ b/scene/3d/mesh_instance.h @@ -35,13 +35,6 @@ #include "core/containers/local_vector.h" #include "core/object/reference.h" -#include "modules/modules_enabled.gen.h" - -#ifdef MODULE_SKELETON_3D_ENABLED -class Skin; -class SkinReference; -#endif - class Mesh; class NodePath; @@ -51,38 +44,6 @@ class MeshInstance : public GeometryInstance { protected: Ref mesh; -#ifdef MODULE_SKELETON_3D_ENABLED - Ref skin; - Ref skin_internal; - Ref skin_ref; - NodePath skeleton_path; - - struct SoftwareSkinning { - enum Flags { - // Data flags. - FLAG_TRANSFORM_NORMALS = 1 << 0, - - // Runtime flags. - FLAG_BONES_READY = 1 << 1, - }; - - struct SurfaceData { - PoolByteArray source_buffer; - uint32_t source_format; - PoolByteArray buffer; - PoolByteArray::Write buffer_write; - bool transform_tangents; - bool ensure_correct_normals; - }; - - Ref mesh_instance; - LocalVector surface_data; - }; - - SoftwareSkinning *software_skinning; - uint32_t software_skinning_flags; -#endif - struct BlendShapeTrack { int idx; float value; @@ -97,16 +58,6 @@ protected: void _mesh_changed(); -#ifdef MODULE_SKELETON_3D_ENABLED - void _resolve_skeleton_path(); - - bool _is_software_skinning_enabled() const; - static bool _is_global_software_skinning_enabled(); - - void _initialize_skinning(bool p_force_reset = false, bool p_call_attach_skeleton = true); - void _update_skinning(); -#endif - private: // merging bool _merge_meshes(Vector p_list, bool p_use_global_space, bool p_check_compatibility); @@ -129,14 +80,6 @@ public: void set_mesh(const Ref &p_mesh); Ref get_mesh() const; -#ifdef MODULE_SKELETON_3D_ENABLED - void set_skin(const Ref &p_skin); - Ref get_skin() const; - - void set_skeleton_path(const NodePath &p_skeleton); - NodePath get_skeleton_path(); -#endif - int get_surface_material_count() const; void set_surface_material(int p_surface, const Ref &p_material); Ref get_surface_material(int p_surface) const; diff --git a/scene/3d/navigation_obstacle.cpp b/scene/3d/navigation_obstacle.cpp deleted file mode 100644 index 2a71216..0000000 --- a/scene/3d/navigation_obstacle.cpp +++ /dev/null @@ -1,609 +0,0 @@ -/*************************************************************************/ -/* navigation_obstacle.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "navigation_obstacle.h" - -#include "core/config/engine.h" -#include "scene/3d/collision_shape.h" -#include "scene/3d/navigation.h" -#include "scene/3d/physics_body.h" -#include "scene/main/spatial.h" -#include "scene/resources/material/material.h" -#include "scene/resources/material/spatial_material.h" -#include "scene/resources/mesh/mesh.h" -#include "scene/resources/shapes/shape.h" -#include "scene/resources/world_3d.h" -#include "servers/navigation_server.h" -#include "servers/rendering_server.h" - -void NavigationObstacle::_bind_methods() { - ClassDB::bind_method(D_METHOD("get_rid"), &NavigationObstacle::get_rid); - - ClassDB::bind_method(D_METHOD("set_avoidance_enabled", "enabled"), &NavigationObstacle::set_avoidance_enabled); - ClassDB::bind_method(D_METHOD("get_avoidance_enabled"), &NavigationObstacle::get_avoidance_enabled); - - ClassDB::bind_method(D_METHOD("set_navigation_map", "navigation_map"), &NavigationObstacle::set_navigation_map); - ClassDB::bind_method(D_METHOD("get_navigation_map"), &NavigationObstacle::get_navigation_map); - - ClassDB::bind_method(D_METHOD("set_navigation", "navigation"), &NavigationObstacle::set_navigation_node); - ClassDB::bind_method(D_METHOD("get_navigation"), &NavigationObstacle::get_navigation_node); - - ClassDB::bind_method(D_METHOD("set_radius", "radius"), &NavigationObstacle::set_radius); - ClassDB::bind_method(D_METHOD("get_radius"), &NavigationObstacle::get_radius); - - ClassDB::bind_method(D_METHOD("set_height", "height"), &NavigationObstacle::set_height); - ClassDB::bind_method(D_METHOD("get_height"), &NavigationObstacle::get_height); - - ClassDB::bind_method(D_METHOD("set_velocity", "velocity"), &NavigationObstacle::set_velocity); - ClassDB::bind_method(D_METHOD("get_velocity"), &NavigationObstacle::get_velocity); - - ClassDB::bind_method(D_METHOD("set_vertices", "vertices"), &NavigationObstacle::set_vertices); - ClassDB::bind_method(D_METHOD("get_vertices"), &NavigationObstacle::get_vertices); - - ClassDB::bind_method(D_METHOD("set_avoidance_layers", "layers"), &NavigationObstacle::set_avoidance_layers); - ClassDB::bind_method(D_METHOD("get_avoidance_layers"), &NavigationObstacle::get_avoidance_layers); - - ClassDB::bind_method(D_METHOD("set_avoidance_layer_value", "layer_number", "value"), &NavigationObstacle::set_avoidance_layer_value); - ClassDB::bind_method(D_METHOD("get_avoidance_layer_value", "layer_number"), &NavigationObstacle::get_avoidance_layer_value); - - ClassDB::bind_method(D_METHOD("set_use_3d_avoidance", "enabled"), &NavigationObstacle::set_use_3d_avoidance); - ClassDB::bind_method(D_METHOD("get_use_3d_avoidance"), &NavigationObstacle::get_use_3d_avoidance); - -#ifdef DEBUG_ENABLED - ClassDB::bind_method(D_METHOD("_update_fake_agent_radius_debug"), &NavigationObstacle::_update_fake_agent_radius_debug); - ClassDB::bind_method(D_METHOD("_update_static_obstacle_debug"), &NavigationObstacle::_update_static_obstacle_debug); -#endif // DEBUG_ENABLED - - ADD_GROUP("Avoidance", ""); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "avoidance_enabled"), "set_avoidance_enabled", "get_avoidance_enabled"); - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "velocity", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR), "set_velocity", "get_velocity"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "radius", PROPERTY_HINT_RANGE, "0.0,100,0.01,suffix:m"), "set_radius", "get_radius"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "height", PROPERTY_HINT_RANGE, "0.0,100,0.01,suffix:m"), "set_height", "get_height"); - ADD_PROPERTY(PropertyInfo(Variant::POOL_VECTOR3_ARRAY, "vertices"), "set_vertices", "get_vertices"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "avoidance_layers", PROPERTY_HINT_LAYERS_AVOIDANCE), "set_avoidance_layers", "get_avoidance_layers"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "use_3d_avoidance"), "set_use_3d_avoidance", "get_use_3d_avoidance"); -} - -void NavigationObstacle::_notification(int p_what) { - switch (p_what) { - case NOTIFICATION_POST_ENTER_TREE: { - // Search the navigation node and set it - { - Navigation *nav = nullptr; - Node *p = get_parent(); - while (p != nullptr) { - nav = Object::cast_to(p); - if (nav != nullptr) { - p = nullptr; - } else { - p = p->get_parent(); - } - } - - set_navigation(nav); - } - - _update_map(get_navigation_map()); - - previous_transform = get_global_transform(); - // need to trigger map controlled agent assignment somehow for the fake_agent since obstacles use no callback like regular agents - NavigationServer::get_singleton()->obstacle_set_avoidance_enabled(obstacle, avoidance_enabled); - _update_position(get_global_transform().origin); - - set_physics_process_internal(true); - -#ifdef DEBUG_ENABLED - if ((NavigationServer::get_singleton()->get_debug_avoidance_enabled()) && - (NavigationServer::get_singleton()->get_debug_navigation_avoidance_enable_obstacles_radius())) { - _update_fake_agent_radius_debug(); - _update_static_obstacle_debug(); - } -#endif // DEBUG_ENABLED - } break; - case NOTIFICATION_PAUSED: { - if (!can_process()) { - map_before_pause = map_current; - _update_map(RID()); - } else if (can_process() && !(map_before_pause == RID())) { - _update_map(map_before_pause); - map_before_pause = RID(); - } - NavigationServer::get_singleton()->obstacle_set_paused(obstacle, !can_process()); - } break; - case NOTIFICATION_UNPAUSED: { - if (!can_process()) { - map_before_pause = map_current; - _update_map(RID()); - } else if (can_process() && !(map_before_pause == RID())) { - _update_map(map_before_pause); - map_before_pause = RID(); - } - NavigationServer::get_singleton()->obstacle_set_paused(obstacle, !can_process()); - } break; - case NOTIFICATION_EXIT_TREE: { - set_navigation(nullptr); - set_physics_process_internal(false); - request_ready(); // required to solve an issue with losing the navigation - - _update_map(RID()); -#ifdef DEBUG_ENABLED - if (fake_agent_radius_debug_instance.is_valid()) { - RS::get_singleton()->instance_set_visible(fake_agent_radius_debug_instance, false); - } - if (static_obstacle_debug_instance.is_valid()) { - RS::get_singleton()->instance_set_visible(static_obstacle_debug_instance, false); - } -#endif // DEBUG_ENABLED - } break; - case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: { - if (is_inside_tree()) { - _update_position(get_global_transform().origin); - - if (velocity_submitted) { - velocity_submitted = false; - // only update if there is a noticeable change, else the rvo agent preferred velocity stays the same - if (!previous_velocity.is_equal_approx(velocity)) { - NavigationServer::get_singleton()->obstacle_set_velocity(obstacle, velocity); - } - previous_velocity = velocity; - } -#ifdef DEBUG_ENABLED - if (fake_agent_radius_debug_instance.is_valid() && radius > 0.0) { - RS::get_singleton()->instance_set_transform(fake_agent_radius_debug_instance, get_global_transform()); - } - if (static_obstacle_debug_instance.is_valid() && get_vertices().size() > 0) { - RS::get_singleton()->instance_set_transform(static_obstacle_debug_instance, get_global_transform()); - } -#endif // DEBUG_ENABLED - } - - } break; - } -} - -NavigationObstacle::NavigationObstacle() { - navigation = nullptr; - - height = 1.0; - radius = 0.0; - - avoidance_layers = 1; - - use_3d_avoidance = false; - - velocity_submitted = false; - avoidance_enabled = true; - - obstacle = NavigationServer::get_singleton()->obstacle_create(); - - set_radius(radius); - set_height(height); - set_vertices(vertices); - set_avoidance_layers(avoidance_layers); - set_avoidance_enabled(avoidance_enabled); - set_use_3d_avoidance(use_3d_avoidance); - -#ifdef DEBUG_ENABLED - NavigationServer::get_singleton()->connect("avoidance_debug_changed", this, "_update_fake_agent_radius_debug"); - NavigationServer::get_singleton()->connect("avoidance_debug_changed", this, "_update_static_obstacle_debug"); - _update_fake_agent_radius_debug(); - _update_static_obstacle_debug(); -#endif // DEBUG_ENABLED -} - -NavigationObstacle::~NavigationObstacle() { - ERR_FAIL_NULL(NavigationServer::get_singleton()); - - NavigationServer::get_singleton()->free(obstacle); - obstacle = RID(); - -#ifdef DEBUG_ENABLED - NavigationServer::get_singleton()->disconnect("avoidance_debug_changed", this, "_update_fake_agent_radius_debug"); - NavigationServer::get_singleton()->disconnect("avoidance_debug_changed", this, "_update_static_obstacle_debug"); - - if (fake_agent_radius_debug_instance.is_valid()) { - RenderingServer::get_singleton()->free(fake_agent_radius_debug_instance); - } - if (fake_agent_radius_debug_mesh.is_valid()) { - RenderingServer::get_singleton()->free(fake_agent_radius_debug_mesh->get_rid()); - } - - if (static_obstacle_debug_instance.is_valid()) { - RenderingServer::get_singleton()->free(static_obstacle_debug_instance); - } - if (static_obstacle_debug_mesh.is_valid()) { - RenderingServer::get_singleton()->free(static_obstacle_debug_mesh->get_rid()); - } -#endif // DEBUG_ENABLED -} - -void NavigationObstacle::set_navigation(Navigation *p_nav) { - if (navigation == p_nav && navigation != nullptr) { - return; // Pointless - } - - navigation = p_nav; - - NavigationServer::get_singleton()->obstacle_set_map(obstacle, get_navigation_map()); -} - -void NavigationObstacle::set_navigation_node(Node *p_nav) { - Navigation *nav = Object::cast_to(p_nav); - ERR_FAIL_NULL(nav); - set_navigation(nav); -} - -Node *NavigationObstacle::get_navigation_node() const { - return Object::cast_to(navigation); -} - -void NavigationObstacle::set_navigation_map(RID p_navigation_map) { - if (map_override == p_navigation_map) { - return; - } - map_override = p_navigation_map; - _update_map(get_navigation_map()); -} - -RID NavigationObstacle::get_navigation_map() const { - if (navigation) { - navigation->get_rid(); - } else if (map_override.is_valid()) { - return map_override; - } else if (is_inside_tree()) { - return get_world_3d()->get_navigation_map(); - } - return RID(); -} - -void NavigationObstacle::set_vertices(const Vector &p_vertices) { - vertices = p_vertices; - NavigationServer::get_singleton()->obstacle_set_vertices(obstacle, vertices); -#ifdef DEBUG_ENABLED - _update_static_obstacle_debug(); -#endif // DEBUG_ENABLED -} - -void NavigationObstacle::set_radius(real_t p_radius) { - ERR_FAIL_COND_MSG(p_radius < 0.0, "Radius must be positive."); - if (Math::is_equal_approx(radius, p_radius)) { - return; - } - - radius = p_radius; - NavigationServer::get_singleton()->obstacle_set_radius(obstacle, radius); - -#ifdef DEBUG_ENABLED - _update_fake_agent_radius_debug(); -#endif // DEBUG_ENABLED -} - -void NavigationObstacle::set_height(real_t p_height) { - ERR_FAIL_COND_MSG(p_height < 0.0, "Height must be positive."); - if (Math::is_equal_approx(height, p_height)) { - return; - } - - height = p_height; - NavigationServer::get_singleton()->obstacle_set_height(obstacle, height); - -#ifdef DEBUG_ENABLED - _update_static_obstacle_debug(); -#endif // DEBUG_ENABLED -} - -void NavigationObstacle::set_avoidance_layers(uint32_t p_layers) { - avoidance_layers = p_layers; - NavigationServer::get_singleton()->obstacle_set_avoidance_layers(obstacle, avoidance_layers); -} - -uint32_t NavigationObstacle::get_avoidance_layers() const { - return avoidance_layers; -} - -void NavigationObstacle::set_avoidance_layer_value(int p_layer_number, bool p_value) { - ERR_FAIL_COND_MSG(p_layer_number < 1, "Avoidance layer number must be between 1 and 32 inclusive."); - ERR_FAIL_COND_MSG(p_layer_number > 32, "Avoidance layer number must be between 1 and 32 inclusive."); - uint32_t avoidance_layers_new = get_avoidance_layers(); - if (p_value) { - avoidance_layers_new |= 1 << (p_layer_number - 1); - } else { - avoidance_layers_new &= ~(1 << (p_layer_number - 1)); - } - set_avoidance_layers(avoidance_layers_new); -} - -bool NavigationObstacle::get_avoidance_layer_value(int p_layer_number) const { - ERR_FAIL_COND_V_MSG(p_layer_number < 1, false, "Avoidance layer number must be between 1 and 32 inclusive."); - ERR_FAIL_COND_V_MSG(p_layer_number > 32, false, "Avoidance layer number must be between 1 and 32 inclusive."); - return get_avoidance_layers() & (1 << (p_layer_number - 1)); -} - -void NavigationObstacle::set_avoidance_enabled(bool p_enabled) { - if (avoidance_enabled == p_enabled) { - return; - } - - avoidance_enabled = p_enabled; - NavigationServer::get_singleton()->obstacle_set_avoidance_enabled(obstacle, avoidance_enabled); -} - -bool NavigationObstacle::get_avoidance_enabled() const { - return avoidance_enabled; -} - -void NavigationObstacle::set_use_3d_avoidance(bool p_use_3d_avoidance) { - use_3d_avoidance = p_use_3d_avoidance; - _update_use_3d_avoidance(use_3d_avoidance); - property_list_changed_notify(); -} - -void NavigationObstacle::set_velocity(const Vector3 p_velocity) { - velocity = p_velocity; - velocity_submitted = true; -} - -void NavigationObstacle::_update_map(RID p_map) { - NavigationServer::get_singleton()->obstacle_set_map(obstacle, p_map); - map_current = p_map; -} - -void NavigationObstacle::_update_position(const Vector3 p_position) { - NavigationServer::get_singleton()->obstacle_set_position(obstacle, p_position); -} - -void NavigationObstacle::_update_use_3d_avoidance(bool p_use_3d_avoidance) { - NavigationServer::get_singleton()->obstacle_set_use_3d_avoidance(obstacle, use_3d_avoidance); - _update_map(map_current); -} - -#ifdef DEBUG_ENABLED -void NavigationObstacle::_update_fake_agent_radius_debug() { - bool is_debug_enabled = false; - if (Engine::get_singleton()->is_editor_hint()) { - is_debug_enabled = true; - } else if (NavigationServer::get_singleton()->get_debug_enabled() && - NavigationServer::get_singleton()->get_debug_avoidance_enabled() && - NavigationServer::get_singleton()->get_debug_navigation_avoidance_enable_obstacles_radius()) { - is_debug_enabled = true; - } - - if (is_debug_enabled == false) { - if (fake_agent_radius_debug_instance.is_valid()) { - RS::get_singleton()->instance_set_visible(fake_agent_radius_debug_instance, false); - } - return; - } - - if (!fake_agent_radius_debug_instance.is_valid()) { - fake_agent_radius_debug_instance = RenderingServer::get_singleton()->instance_create(); - } - if (!fake_agent_radius_debug_mesh.is_valid()) { - fake_agent_radius_debug_mesh = Ref(memnew(ArrayMesh)); - } - fake_agent_radius_debug_mesh->clear_surfaces(); - - Vector face_vertex_array; - Vector face_indices_array; - - int i, j, prevrow, thisrow, point; - float x, y, z; - - int rings = 16; - int radial_segments = 32; - - point = 0; - - thisrow = 0; - prevrow = 0; - for (j = 0; j <= (rings + 1); j++) { - float v = j; - float w; - - v /= (rings + 1); - w = sin(Math_PI * v); - y = (radius)*cos(Math_PI * v); - - for (i = 0; i <= radial_segments; i++) { - float u = i; - u /= radial_segments; - - x = sin(u * Math_TAU); - z = cos(u * Math_TAU); - - Vector3 p = Vector3(x * radius * w, y, z * radius * w); - face_vertex_array.push_back(p); - - point++; - - if (i > 0 && j > 0) { - face_indices_array.push_back(prevrow + i - 1); - face_indices_array.push_back(prevrow + i); - face_indices_array.push_back(thisrow + i - 1); - - face_indices_array.push_back(prevrow + i); - face_indices_array.push_back(thisrow + i); - face_indices_array.push_back(thisrow + i - 1); - }; - }; - - prevrow = thisrow; - thisrow = point; - }; - - Array face_mesh_array; - face_mesh_array.resize(Mesh::ARRAY_MAX); - face_mesh_array[Mesh::ARRAY_VERTEX] = face_vertex_array; - face_mesh_array[Mesh::ARRAY_INDEX] = face_indices_array; - - fake_agent_radius_debug_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, face_mesh_array); - Ref face_material = NavigationServer::get_singleton()->get_debug_navigation_avoidance_obstacles_radius_material(); - fake_agent_radius_debug_mesh->surface_set_material(0, face_material); - - RS::get_singleton()->instance_set_base(fake_agent_radius_debug_instance, fake_agent_radius_debug_mesh->get_rid()); - if (is_inside_tree()) { - RS::get_singleton()->instance_set_scenario(fake_agent_radius_debug_instance, get_world_3d()->get_scenario()); - RS::get_singleton()->instance_set_visible(fake_agent_radius_debug_instance, is_visible_in_tree()); - } -} -#endif // DEBUG_ENABLED - -#ifdef DEBUG_ENABLED -void NavigationObstacle::_update_static_obstacle_debug() { - bool is_debug_enabled = false; - if (Engine::get_singleton()->is_editor_hint()) { - is_debug_enabled = true; - } else if (NavigationServer::get_singleton()->get_debug_enabled() && - NavigationServer::get_singleton()->get_debug_avoidance_enabled() && - NavigationServer::get_singleton()->get_debug_navigation_avoidance_enable_obstacles_static()) { - is_debug_enabled = true; - } - - if (is_debug_enabled == false) { - if (static_obstacle_debug_instance.is_valid()) { - RS::get_singleton()->instance_set_visible(static_obstacle_debug_instance, false); - } - return; - } - - if (vertices.size() < 3) { - if (static_obstacle_debug_instance.is_valid()) { - RS::get_singleton()->instance_set_visible(static_obstacle_debug_instance, false); - } - return; - } - - if (!static_obstacle_debug_instance.is_valid()) { - static_obstacle_debug_instance = RenderingServer::get_singleton()->instance_create(); - } - if (!static_obstacle_debug_mesh.is_valid()) { - static_obstacle_debug_mesh = Ref(memnew(ArrayMesh)); - } - static_obstacle_debug_mesh->clear_surfaces(); - - Vector polygon_2d_vertices; - polygon_2d_vertices.resize(vertices.size()); - Vector2 *polygon_2d_vertices_ptr = polygon_2d_vertices.ptrw(); - - for (int i = 0; i < vertices.size(); ++i) { - Vector3 obstacle_vertex = vertices[i]; - Vector2 obstacle_vertex_2d = Vector2(obstacle_vertex.x, obstacle_vertex.z); - polygon_2d_vertices_ptr[i] = obstacle_vertex_2d; - } - - Vector triangulated_polygon_2d_indices = Geometry::triangulate_polygon(polygon_2d_vertices); - - if (triangulated_polygon_2d_indices.empty()) { - // failed triangulation - return; - } - - bool obstacle_pushes_inward = Geometry::is_polygon_clockwise(polygon_2d_vertices); - - Vector face_vertex_array; - Vector face_indices_array; - - face_vertex_array.resize(polygon_2d_vertices.size()); - face_indices_array.resize(triangulated_polygon_2d_indices.size()); - - Vector3 *face_vertex_array_ptr = face_vertex_array.ptrw(); - int *face_indices_array_ptr = face_indices_array.ptrw(); - - for (int i = 0; i < triangulated_polygon_2d_indices.size(); ++i) { - int vertex_index = triangulated_polygon_2d_indices[i]; - const Vector2 &vertex_2d = polygon_2d_vertices[vertex_index]; - Vector3 vertex_3d = Vector3(vertex_2d.x, 0.0, vertex_2d.y); - face_vertex_array_ptr[vertex_index] = vertex_3d; - face_indices_array_ptr[i] = vertex_index; - } - - Array face_mesh_array; - face_mesh_array.resize(Mesh::ARRAY_MAX); - face_mesh_array[Mesh::ARRAY_VERTEX] = face_vertex_array; - face_mesh_array[Mesh::ARRAY_INDEX] = face_indices_array; - - static_obstacle_debug_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, face_mesh_array); - - Vector edge_vertex_array; - - for (int i = 0; i < polygon_2d_vertices.size(); ++i) { - int from_index = i - 1; - int to_index = i; - - if (i == 0) { - from_index = polygon_2d_vertices.size() - 1; - } - - const Vector2 &vertex_2d_from = polygon_2d_vertices[from_index]; - const Vector2 &vertex_2d_to = polygon_2d_vertices[to_index]; - - Vector3 vertex_3d_ground_from = Vector3(vertex_2d_from.x, 0.0, vertex_2d_from.y); - Vector3 vertex_3d_ground_to = Vector3(vertex_2d_to.x, 0.0, vertex_2d_to.y); - - edge_vertex_array.push_back(vertex_3d_ground_from); - edge_vertex_array.push_back(vertex_3d_ground_to); - - Vector3 vertex_3d_height_from = Vector3(vertex_2d_from.x, height, vertex_2d_from.y); - Vector3 vertex_3d_height_to = Vector3(vertex_2d_to.x, height, vertex_2d_to.y); - - edge_vertex_array.push_back(vertex_3d_height_from); - edge_vertex_array.push_back(vertex_3d_height_to); - - edge_vertex_array.push_back(vertex_3d_ground_from); - edge_vertex_array.push_back(vertex_3d_height_from); - } - - Array edge_mesh_array; - edge_mesh_array.resize(Mesh::ARRAY_MAX); - edge_mesh_array[Mesh::ARRAY_VERTEX] = edge_vertex_array; - - static_obstacle_debug_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_LINES, edge_mesh_array); - - Ref face_material; - Ref edge_material; - - if (obstacle_pushes_inward) { - face_material = NavigationServer::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushin_face_material(); - edge_material = NavigationServer::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushin_edge_material(); - } else { - face_material = NavigationServer::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushout_face_material(); - edge_material = NavigationServer::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushout_edge_material(); - } - - static_obstacle_debug_mesh->surface_set_material(0, face_material); - static_obstacle_debug_mesh->surface_set_material(1, edge_material); - - RS::get_singleton()->instance_set_base(static_obstacle_debug_instance, static_obstacle_debug_mesh->get_rid()); - if (is_inside_tree()) { - RS::get_singleton()->instance_set_scenario(static_obstacle_debug_instance, get_world_3d()->get_scenario()); - RS::get_singleton()->instance_set_visible(static_obstacle_debug_instance, is_visible_in_tree()); - } -} -#endif // DEBUG_ENABLED diff --git a/scene/3d/navigation_obstacle.h b/scene/3d/navigation_obstacle.h deleted file mode 100644 index 09a338f..0000000 --- a/scene/3d/navigation_obstacle.h +++ /dev/null @@ -1,132 +0,0 @@ -#ifndef NAVIGATION_OBSTACLE_H -#define NAVIGATION_OBSTACLE_H - -/*************************************************************************/ -/* navigation_obstacle.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "scene/main/spatial.h" - -class Navigation; -class Spatial; - -class NavigationObstacle : public Spatial { - GDCLASS(NavigationObstacle, Spatial); - - Navigation *navigation; - - RID obstacle; - RID map_before_pause; - RID map_override; - RID map_current; - - real_t height; - real_t radius; - - Vector vertices; - - bool avoidance_enabled; - uint32_t avoidance_layers; - - bool use_3d_avoidance; - - Transform previous_transform; - - Vector3 velocity; - Vector3 previous_velocity; - bool velocity_submitted; - -#ifdef DEBUG_ENABLED - RID fake_agent_radius_debug_instance; - Ref fake_agent_radius_debug_mesh; - - RID static_obstacle_debug_instance; - Ref static_obstacle_debug_mesh; - -private: - void _update_fake_agent_radius_debug(); - void _update_static_obstacle_debug(); -#endif // DEBUG_ENABLED - -protected: - static void _bind_methods(); - void _notification(int p_what); - -public: - NavigationObstacle(); - virtual ~NavigationObstacle(); - - void set_navigation(Navigation *p_nav); - const Navigation *get_navigation() const { - return navigation; - } - - void set_navigation_node(Node *p_nav); - Node *get_navigation_node() const; - - RID get_rid() const { return obstacle; } - - void set_avoidance_enabled(bool p_enabled); - bool get_avoidance_enabled() const; - - void set_navigation_map(RID p_navigation_map); - RID get_navigation_map() const; - - void set_radius(real_t p_radius); - real_t get_radius() const { - return radius; - } - - void set_height(real_t p_height); - real_t get_height() const { return height; } - - void set_vertices(const Vector &p_vertices); - Vector get_vertices() const { return vertices; }; - - void set_avoidance_layers(uint32_t p_layers); - uint32_t get_avoidance_layers() const; - - void set_avoidance_layer_value(int p_layer_number, bool p_value); - bool get_avoidance_layer_value(int p_layer_number) const; - - void set_use_3d_avoidance(bool p_use_3d_avoidance); - bool get_use_3d_avoidance() const { return use_3d_avoidance; } - - void set_velocity(const Vector3 p_velocity); - Vector3 get_velocity() const { return velocity; }; - - void _avoidance_done(Vector3 p_new_velocity); // Dummy - -private: - void _update_map(RID p_map); - void _update_position(const Vector3 p_position); - void _update_use_3d_avoidance(bool p_use_3d_avoidance); -}; - -#endif diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp deleted file mode 100644 index 25513da..0000000 --- a/scene/3d/physics_body.cpp +++ /dev/null @@ -1,1624 +0,0 @@ -/*************************************************************************/ -/* physics_body.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_body.h" - -#include "core/config/engine.h" -#include "core/config/project_settings.h" -#include "core/containers/list.h" -#include "core/containers/rid.h" -#include "core/core_string_names.h" -#include "core/object/method_bind_ext.gen.inc" -#include "core/object/object.h" -#include "scene/resources/physics_material.h" -#include "scene/main/scene_string_names.h" -//#include "skeleton.h" - -#ifdef TOOLS_ENABLED -#include "editor/plugins/spatial_editor_plugin.h" -#endif - -void PhysicsBody::_notification(int p_what) { -} - -Vector3 PhysicsBody::get_linear_velocity() const { - return Vector3(); -} -Vector3 PhysicsBody::get_angular_velocity() const { - return Vector3(); -} - -float PhysicsBody::get_inverse_mass() const { - return 0; -} - -Array PhysicsBody::get_collision_exceptions() { - List exceptions; - PhysicsServer::get_singleton()->body_get_collision_exceptions(get_rid(), &exceptions); - Array ret; - for (List::Element *E = exceptions.front(); E; E = E->next()) { - RID body = E->get(); - ObjectID instance_id = PhysicsServer::get_singleton()->body_get_object_instance_id(body); - Object *obj = ObjectDB::get_instance(instance_id); - PhysicsBody *physics_body = Object::cast_to(obj); - ret.append(physics_body); - } - return ret; -} - -void PhysicsBody::add_collision_exception_with(Node *p_node) { - ERR_FAIL_NULL(p_node); - CollisionObject *collision_object = Object::cast_to(p_node); - ERR_FAIL_COND_MSG(!collision_object, "Collision exception only works between two nodes that inherit from CollisionObject (such as Area or PhysicsBody)."); - PhysicsServer::get_singleton()->body_add_collision_exception(get_rid(), collision_object->get_rid()); -} - -void PhysicsBody::remove_collision_exception_with(Node *p_node) { - ERR_FAIL_NULL(p_node); - CollisionObject *collision_object = Object::cast_to(p_node); - ERR_FAIL_COND_MSG(!collision_object, "Collision exception only works between two nodes that inherit from CollisionObject (such as Area or PhysicsBody)."); - PhysicsServer::get_singleton()->body_remove_collision_exception(get_rid(), collision_object->get_rid()); -} - -void PhysicsBody::_set_layers(uint32_t p_mask) { - set_collision_layer(p_mask); - set_collision_mask(p_mask); -} - -uint32_t PhysicsBody::_get_layers() const { - return get_collision_layer(); -} - -void PhysicsBody::_bind_methods() { - ClassDB::bind_method(D_METHOD("_set_layers", "mask"), &PhysicsBody::_set_layers); - ClassDB::bind_method(D_METHOD("_get_layers"), &PhysicsBody::_get_layers); -} - -PhysicsBody::PhysicsBody(PhysicsServer::BodyMode p_mode) : - CollisionObject(RID_PRIME(PhysicsServer::get_singleton()->body_create(p_mode)), false) { -} - -PhysicsBody::~PhysicsBody() { -} - -#ifndef DISABLE_DEPRECATED -void StaticBody::set_friction(real_t p_friction) { - if (p_friction == 1.0 && physics_material_override.is_null()) { // default value, don't create an override for that - return; - } - - WARN_DEPRECATED_MSG("The method set_friction has been deprecated and will be removed in the future, use physics material instead."); - - ERR_FAIL_COND_MSG(p_friction < 0 || p_friction > 1, "Friction must be between 0 and 1."); - - if (physics_material_override.is_null()) { - physics_material_override.instance(); - set_physics_material_override(physics_material_override); - } - physics_material_override->set_friction(p_friction); -} - -real_t StaticBody::get_friction() const { - WARN_DEPRECATED_MSG("The method get_friction has been deprecated and will be removed in the future, use physics material instead."); - - if (physics_material_override.is_null()) { - return 1; - } - - return physics_material_override->get_friction(); -} - -void StaticBody::set_bounce(real_t p_bounce) { - if (p_bounce == 0.0 && physics_material_override.is_null()) { // default value, don't create an override for that - return; - } - - WARN_DEPRECATED_MSG("The method set_bounce has been deprecated and will be removed in the future, use physics material instead."); - - ERR_FAIL_COND_MSG(p_bounce < 0 || p_bounce > 1, "Bounce must be between 0 and 1."); - - if (physics_material_override.is_null()) { - physics_material_override.instance(); - set_physics_material_override(physics_material_override); - } - physics_material_override->set_bounce(p_bounce); -} - -real_t StaticBody::get_bounce() const { - WARN_DEPRECATED_MSG("The method get_bounce has been deprecated and will be removed in the future, use physics material instead."); - - if (physics_material_override.is_null()) { - return 0; - } - - return physics_material_override->get_bounce(); -} -#endif - -void StaticBody::set_physics_material_override(const Ref &p_physics_material_override) { - if (physics_material_override.is_valid()) { - if (physics_material_override->is_connected(CoreStringNames::get_singleton()->changed, this, "_reload_physics_characteristics")) { - physics_material_override->disconnect(CoreStringNames::get_singleton()->changed, this, "_reload_physics_characteristics"); - } - } - - physics_material_override = p_physics_material_override; - - if (physics_material_override.is_valid()) { - physics_material_override->connect(CoreStringNames::get_singleton()->changed, this, "_reload_physics_characteristics"); - } - _reload_physics_characteristics(); -} - -Ref StaticBody::get_physics_material_override() const { - return physics_material_override; -} - -void StaticBody::set_constant_linear_velocity(const Vector3 &p_vel) { - constant_linear_velocity = p_vel; - PhysicsServer::get_singleton()->body_set_state(get_rid(), PhysicsServer::BODY_STATE_LINEAR_VELOCITY, constant_linear_velocity); -} - -void StaticBody::set_constant_angular_velocity(const Vector3 &p_vel) { - constant_angular_velocity = p_vel; - PhysicsServer::get_singleton()->body_set_state(get_rid(), PhysicsServer::BODY_STATE_ANGULAR_VELOCITY, constant_angular_velocity); -} - -Vector3 StaticBody::get_constant_linear_velocity() const { - return constant_linear_velocity; -} -Vector3 StaticBody::get_constant_angular_velocity() const { - return constant_angular_velocity; -} - -void StaticBody::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_constant_linear_velocity", "vel"), &StaticBody::set_constant_linear_velocity); - ClassDB::bind_method(D_METHOD("set_constant_angular_velocity", "vel"), &StaticBody::set_constant_angular_velocity); - ClassDB::bind_method(D_METHOD("get_constant_linear_velocity"), &StaticBody::get_constant_linear_velocity); - ClassDB::bind_method(D_METHOD("get_constant_angular_velocity"), &StaticBody::get_constant_angular_velocity); - -#ifndef DISABLE_DEPRECATED - ClassDB::bind_method(D_METHOD("set_friction", "friction"), &StaticBody::set_friction); - ClassDB::bind_method(D_METHOD("get_friction"), &StaticBody::get_friction); - - ClassDB::bind_method(D_METHOD("set_bounce", "bounce"), &StaticBody::set_bounce); - ClassDB::bind_method(D_METHOD("get_bounce"), &StaticBody::get_bounce); -#endif // DISABLE_DEPRECATED - - ClassDB::bind_method(D_METHOD("set_physics_material_override", "physics_material_override"), &StaticBody::set_physics_material_override); - ClassDB::bind_method(D_METHOD("get_physics_material_override"), &StaticBody::get_physics_material_override); - - ClassDB::bind_method(D_METHOD("_reload_physics_characteristics"), &StaticBody::_reload_physics_characteristics); - - ClassDB::bind_method(D_METHOD("get_collision_exceptions"), &PhysicsBody::get_collision_exceptions); - ClassDB::bind_method(D_METHOD("add_collision_exception_with", "body"), &PhysicsBody::add_collision_exception_with); - ClassDB::bind_method(D_METHOD("remove_collision_exception_with", "body"), &PhysicsBody::remove_collision_exception_with); - -#ifndef DISABLE_DEPRECATED - ADD_PROPERTY(PropertyInfo(Variant::REAL, "friction", PROPERTY_HINT_RANGE, "0,1,0.01", 0), "set_friction", "get_friction"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "bounce", PROPERTY_HINT_RANGE, "0,1,0.01", 0), "set_bounce", "get_bounce"); -#endif // DISABLE_DEPRECATED - ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "physics_material_override", PROPERTY_HINT_RESOURCE_TYPE, "PhysicsMaterial"), "set_physics_material_override", "get_physics_material_override"); - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "constant_linear_velocity"), "set_constant_linear_velocity", "get_constant_linear_velocity"); - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "constant_angular_velocity"), "set_constant_angular_velocity", "get_constant_angular_velocity"); -} - -StaticBody::StaticBody() : - PhysicsBody(PhysicsServer::BODY_MODE_STATIC) { -} - -StaticBody::~StaticBody() {} - -void StaticBody::_reload_physics_characteristics() { - if (physics_material_override.is_null()) { - PhysicsServer::get_singleton()->body_set_param(get_rid(), PhysicsServer::BODY_PARAM_BOUNCE, 0); - PhysicsServer::get_singleton()->body_set_param(get_rid(), PhysicsServer::BODY_PARAM_FRICTION, 1); - } else { - PhysicsServer::get_singleton()->body_set_param(get_rid(), PhysicsServer::BODY_PARAM_BOUNCE, physics_material_override->computed_bounce()); - PhysicsServer::get_singleton()->body_set_param(get_rid(), PhysicsServer::BODY_PARAM_FRICTION, physics_material_override->computed_friction()); - } -} - -void RigidBody::_body_enter_tree(ObjectID p_id) { - Object *obj = ObjectDB::get_instance(p_id); - Node *node = Object::cast_to(obj); - ERR_FAIL_COND(!node); - - ERR_FAIL_COND(!contact_monitor); - RBMap::Element *E = contact_monitor->body_map.find(p_id); - ERR_FAIL_COND(!E); - ERR_FAIL_COND(E->get().in_tree); - - E->get().in_tree = true; - - contact_monitor->locked = true; - - emit_signal(SceneStringNames::get_singleton()->body_entered, node); - - for (int i = 0; i < E->get().shapes.size(); i++) { - emit_signal(SceneStringNames::get_singleton()->body_shape_entered, E->get().rid, node, E->get().shapes[i].body_shape, E->get().shapes[i].local_shape); - } - - contact_monitor->locked = false; -} - -void RigidBody::_body_exit_tree(ObjectID p_id) { - Object *obj = ObjectDB::get_instance(p_id); - Node *node = Object::cast_to(obj); - ERR_FAIL_COND(!node); - ERR_FAIL_COND(!contact_monitor); - RBMap::Element *E = contact_monitor->body_map.find(p_id); - ERR_FAIL_COND(!E); - ERR_FAIL_COND(!E->get().in_tree); - E->get().in_tree = false; - - contact_monitor->locked = true; - - emit_signal(SceneStringNames::get_singleton()->body_exited, node); - - for (int i = 0; i < E->get().shapes.size(); i++) { - emit_signal(SceneStringNames::get_singleton()->body_shape_exited, E->get().rid, node, E->get().shapes[i].body_shape, E->get().shapes[i].local_shape); - } - - contact_monitor->locked = false; -} - -void RigidBody::_body_inout(int p_status, const RID &p_body, ObjectID p_instance, int p_body_shape, int p_local_shape) { - bool body_in = p_status == 1; - ObjectID objid = p_instance; - - Object *obj = ObjectDB::get_instance(objid); - Node *node = Object::cast_to(obj); - - ERR_FAIL_COND(!contact_monitor); - RBMap::Element *E = contact_monitor->body_map.find(objid); - - ERR_FAIL_COND(!body_in && !E); - - if (body_in) { - if (!E) { - E = contact_monitor->body_map.insert(objid, BodyState()); - E->get().rid = p_body; - //E->get().rc=0; - E->get().in_tree = node && node->is_inside_tree(); - if (node) { - node->connect(SceneStringNames::get_singleton()->tree_entered, this, SceneStringNames::get_singleton()->_body_enter_tree, make_binds(objid)); - node->connect(SceneStringNames::get_singleton()->tree_exiting, this, SceneStringNames::get_singleton()->_body_exit_tree, make_binds(objid)); - if (E->get().in_tree) { - emit_signal(SceneStringNames::get_singleton()->body_entered, node); - } - } - } - //E->get().rc++; - if (node) { - E->get().shapes.insert(ShapePair(p_body_shape, p_local_shape)); - } - - if (E->get().in_tree) { - emit_signal(SceneStringNames::get_singleton()->body_shape_entered, p_body, node, p_body_shape, p_local_shape); - } - - } else { - //E->get().rc--; - - if (node) { - E->get().shapes.erase(ShapePair(p_body_shape, p_local_shape)); - } - - bool in_tree = E->get().in_tree; - - if (E->get().shapes.empty()) { - if (node) { - node->disconnect(SceneStringNames::get_singleton()->tree_entered, this, SceneStringNames::get_singleton()->_body_enter_tree); - node->disconnect(SceneStringNames::get_singleton()->tree_exiting, this, SceneStringNames::get_singleton()->_body_exit_tree); - if (in_tree) { - emit_signal(SceneStringNames::get_singleton()->body_exited, node); - } - } - - contact_monitor->body_map.erase(E); - } - if (node && in_tree) { - emit_signal(SceneStringNames::get_singleton()->body_shape_exited, p_body, obj, p_body_shape, p_local_shape); - } - } -} - -struct _RigidBodyInOut { - RID rid; - ObjectID id; - int shape; - int local_shape; -}; - -void RigidBody::_direct_state_changed(Object *p_state) { - state = Object::cast_to(p_state); - ERR_FAIL_COND_MSG(!state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState object as argument"); - - set_ignore_transform_notification(true); - set_global_transform(state->get_transform()); - linear_velocity = state->get_linear_velocity(); - angular_velocity = state->get_angular_velocity(); - inverse_inertia_tensor = state->get_inverse_inertia_tensor(); - if (sleeping != state->is_sleeping()) { - sleeping = state->is_sleeping(); - emit_signal(SceneStringNames::get_singleton()->sleeping_state_changed); - } - if (get_script_instance()) { - get_script_instance()->call("_integrate_forces", state); - } - set_ignore_transform_notification(false); - _on_transform_changed(); - - if (contact_monitor) { - contact_monitor->locked = true; - - //untag all - int rc = 0; - for (RBMap::Element *E = contact_monitor->body_map.front(); E; E = E->next()) { - for (int i = 0; i < E->get().shapes.size(); i++) { - E->get().shapes[i].tagged = false; - rc++; - } - } - - _RigidBodyInOut *toadd = (_RigidBodyInOut *)alloca(state->get_contact_count() * sizeof(_RigidBodyInOut)); - int toadd_count = 0; //state->get_contact_count(); - RigidBody_RemoveAction *toremove = (RigidBody_RemoveAction *)alloca(rc * sizeof(RigidBody_RemoveAction)); - int toremove_count = 0; - - //put the ones to add - - for (int i = 0; i < state->get_contact_count(); i++) { - RID rid = state->get_contact_collider(i); - ObjectID obj = state->get_contact_collider_id(i); - int local_shape = state->get_contact_local_shape(i); - int shape = state->get_contact_collider_shape(i); - - //bool found=false; - - RBMap::Element *E = contact_monitor->body_map.find(obj); - if (!E) { - toadd[toadd_count].rid = rid; - toadd[toadd_count].local_shape = local_shape; - toadd[toadd_count].id = obj; - toadd[toadd_count].shape = shape; - toadd_count++; - continue; - } - - ShapePair sp(shape, local_shape); - int idx = E->get().shapes.find(sp); - if (idx == -1) { - toadd[toadd_count].rid = rid; - toadd[toadd_count].local_shape = local_shape; - toadd[toadd_count].id = obj; - toadd[toadd_count].shape = shape; - toadd_count++; - continue; - } - - E->get().shapes[idx].tagged = true; - } - - //put the ones to remove - - for (RBMap::Element *E = contact_monitor->body_map.front(); E; E = E->next()) { - for (int i = 0; i < E->get().shapes.size(); i++) { - if (!E->get().shapes[i].tagged) { - toremove[toremove_count].rid = E->get().rid; - toremove[toremove_count].body_id = E->key(); - toremove[toremove_count].pair = E->get().shapes[i]; - toremove_count++; - } - } - } - - //process remotions - - for (int i = 0; i < toremove_count; i++) { - _body_inout(0, toremove[i].rid, toremove[i].body_id, toremove[i].pair.body_shape, toremove[i].pair.local_shape); - } - - //process aditions - - for (int i = 0; i < toadd_count; i++) { - _body_inout(1, toadd[i].rid, toadd[i].id, toadd[i].shape, toadd[i].local_shape); - } - - contact_monitor->locked = false; - } - - state = nullptr; -} - -void RigidBody::_notification(int p_what) { -#ifdef TOOLS_ENABLED - if (p_what == NOTIFICATION_ENTER_TREE) { - if (Engine::get_singleton()->is_editor_hint()) { - set_notify_local_transform(true); //used for warnings and only in editor - } - } - - if (p_what == NOTIFICATION_LOCAL_TRANSFORM_CHANGED) { - if (Engine::get_singleton()->is_editor_hint()) { - update_configuration_warning(); - } - } - -#endif -} - -void RigidBody::set_mode(Mode p_mode) { - mode = p_mode; - switch (p_mode) { - case MODE_RIGID: { - PhysicsServer::get_singleton()->body_set_mode(get_rid(), PhysicsServer::BODY_MODE_RIGID); - } break; - case MODE_STATIC: { - PhysicsServer::get_singleton()->body_set_mode(get_rid(), PhysicsServer::BODY_MODE_STATIC); - - } break; - case MODE_CHARACTER: { - PhysicsServer::get_singleton()->body_set_mode(get_rid(), PhysicsServer::BODY_MODE_CHARACTER); - - } break; - case MODE_KINEMATIC: { - PhysicsServer::get_singleton()->body_set_mode(get_rid(), PhysicsServer::BODY_MODE_KINEMATIC); - } break; - } - update_configuration_warning(); -} - -RigidBody::Mode RigidBody::get_mode() const { - return mode; -} - -void RigidBody::set_mass(real_t p_mass) { - ERR_FAIL_COND(p_mass <= 0); - mass = p_mass; - _change_notify("mass"); - _change_notify("weight"); - PhysicsServer::get_singleton()->body_set_param(get_rid(), PhysicsServer::BODY_PARAM_MASS, mass); -} -real_t RigidBody::get_mass() const { - return mass; -} - -void RigidBody::set_weight(real_t p_weight) { - set_mass(p_weight / real_t(GLOBAL_DEF("physics/3d/default_gravity", 9.8))); -} -real_t RigidBody::get_weight() const { - return mass * real_t(GLOBAL_DEF("physics/3d/default_gravity", 9.8)); -} - -#ifndef DISABLE_DEPRECATED -void RigidBody::set_friction(real_t p_friction) { - if (p_friction == 1.0 && physics_material_override.is_null()) { // default value, don't create an override for that - return; - } - - WARN_DEPRECATED_MSG("The method set_friction has been deprecated and will be removed in the future, use physics material instead."); - - ERR_FAIL_COND(p_friction < 0 || p_friction > 1); - - if (physics_material_override.is_null()) { - physics_material_override.instance(); - set_physics_material_override(physics_material_override); - } - physics_material_override->set_friction(p_friction); -} -real_t RigidBody::get_friction() const { - WARN_DEPRECATED_MSG("The method get_friction has been deprecated and will be removed in the future, use physics material instead."); - - if (physics_material_override.is_null()) { - return 1; - } - - return physics_material_override->get_friction(); -} - -void RigidBody::set_bounce(real_t p_bounce) { - if (p_bounce == 0.0 && physics_material_override.is_null()) { // default value, don't create an override for that - return; - } - - WARN_DEPRECATED_MSG("The method set_bounce has been deprecated and will be removed in the future, use physics material instead."); - - ERR_FAIL_COND(p_bounce < 0 || p_bounce > 1); - - if (physics_material_override.is_null()) { - physics_material_override.instance(); - set_physics_material_override(physics_material_override); - } - physics_material_override->set_bounce(p_bounce); -} -real_t RigidBody::get_bounce() const { - WARN_DEPRECATED_MSG("The method get_bounce has been deprecated and will be removed in the future, use physics material instead."); - if (physics_material_override.is_null()) { - return 0; - } - - return physics_material_override->get_bounce(); -} -#endif // DISABLE_DEPRECATED - -void RigidBody::set_physics_material_override(const Ref &p_physics_material_override) { - if (physics_material_override.is_valid()) { - if (physics_material_override->is_connected(CoreStringNames::get_singleton()->changed, this, "_reload_physics_characteristics")) { - physics_material_override->disconnect(CoreStringNames::get_singleton()->changed, this, "_reload_physics_characteristics"); - } - } - - physics_material_override = p_physics_material_override; - - if (physics_material_override.is_valid()) { - physics_material_override->connect(CoreStringNames::get_singleton()->changed, this, "_reload_physics_characteristics"); - } - _reload_physics_characteristics(); -} - -Ref RigidBody::get_physics_material_override() const { - return physics_material_override; -} - -void RigidBody::set_gravity_scale(real_t p_gravity_scale) { - gravity_scale = p_gravity_scale; - PhysicsServer::get_singleton()->body_set_param(get_rid(), PhysicsServer::BODY_PARAM_GRAVITY_SCALE, gravity_scale); -} -real_t RigidBody::get_gravity_scale() const { - return gravity_scale; -} - -void RigidBody::set_linear_damp(real_t p_linear_damp) { - ERR_FAIL_COND(p_linear_damp < -1); - linear_damp = p_linear_damp; - PhysicsServer::get_singleton()->body_set_param(get_rid(), PhysicsServer::BODY_PARAM_LINEAR_DAMP, linear_damp); -} -real_t RigidBody::get_linear_damp() const { - return linear_damp; -} - -void RigidBody::set_angular_damp(real_t p_angular_damp) { - ERR_FAIL_COND(p_angular_damp < -1); - angular_damp = p_angular_damp; - PhysicsServer::get_singleton()->body_set_param(get_rid(), PhysicsServer::BODY_PARAM_ANGULAR_DAMP, angular_damp); -} -real_t RigidBody::get_angular_damp() const { - return angular_damp; -} - -void RigidBody::set_axis_velocity(const Vector3 &p_axis) { - Vector3 v = state ? state->get_linear_velocity() : linear_velocity; - Vector3 axis = p_axis.normalized(); - v -= axis * axis.dot(v); - v += p_axis; - if (state) { - set_linear_velocity(v); - } else { - PhysicsServer::get_singleton()->body_set_axis_velocity(get_rid(), p_axis); - linear_velocity = v; - } -} - -void RigidBody::set_linear_velocity(const Vector3 &p_velocity) { - linear_velocity = p_velocity; - if (state) { - state->set_linear_velocity(linear_velocity); - } else { - PhysicsServer::get_singleton()->body_set_state(get_rid(), PhysicsServer::BODY_STATE_LINEAR_VELOCITY, linear_velocity); - } -} - -Vector3 RigidBody::get_linear_velocity() const { - return linear_velocity; -} - -void RigidBody::set_angular_velocity(const Vector3 &p_velocity) { - angular_velocity = p_velocity; - if (state) { - state->set_angular_velocity(angular_velocity); - } else { - PhysicsServer::get_singleton()->body_set_state(get_rid(), PhysicsServer::BODY_STATE_ANGULAR_VELOCITY, angular_velocity); - } -} -Vector3 RigidBody::get_angular_velocity() const { - return angular_velocity; -} - -Basis RigidBody::get_inverse_inertia_tensor() { - return inverse_inertia_tensor; -} - -void RigidBody::set_use_custom_integrator(bool p_enable) { - if (custom_integrator == p_enable) { - return; - } - - custom_integrator = p_enable; - PhysicsServer::get_singleton()->body_set_omit_force_integration(get_rid(), p_enable); -} -bool RigidBody::is_using_custom_integrator() { - return custom_integrator; -} - -void RigidBody::set_sleeping(bool p_sleeping) { - sleeping = p_sleeping; - PhysicsServer::get_singleton()->body_set_state(get_rid(), PhysicsServer::BODY_STATE_SLEEPING, sleeping); -} - -void RigidBody::set_can_sleep(bool p_active) { - can_sleep = p_active; - PhysicsServer::get_singleton()->body_set_state(get_rid(), PhysicsServer::BODY_STATE_CAN_SLEEP, p_active); -} - -bool RigidBody::is_able_to_sleep() const { - return can_sleep; -} - -bool RigidBody::is_sleeping() const { - return sleeping; -} - -void RigidBody::set_max_contacts_reported(int p_amount) { - max_contacts_reported = p_amount; - PhysicsServer::get_singleton()->body_set_max_contacts_reported(get_rid(), p_amount); -} - -int RigidBody::get_max_contacts_reported() const { - return max_contacts_reported; -} - -void RigidBody::add_central_force(const Vector3 &p_force) { - PhysicsServer::get_singleton()->body_add_central_force(get_rid(), p_force); -} - -void RigidBody::add_force(const Vector3 &p_force, const Vector3 &p_pos) { - PhysicsServer::get_singleton()->body_add_force(get_rid(), p_force, p_pos); -} - -void RigidBody::add_torque(const Vector3 &p_torque) { - PhysicsServer::get_singleton()->body_add_torque(get_rid(), p_torque); -} - -void RigidBody::apply_central_impulse(const Vector3 &p_impulse) { - PhysicsServer::get_singleton()->body_apply_central_impulse(get_rid(), p_impulse); -} - -void RigidBody::apply_impulse(const Vector3 &p_pos, const Vector3 &p_impulse) { - PhysicsServer::get_singleton()->body_apply_impulse(get_rid(), p_pos, p_impulse); -} - -void RigidBody::apply_torque_impulse(const Vector3 &p_impulse) { - PhysicsServer::get_singleton()->body_apply_torque_impulse(get_rid(), p_impulse); -} - -void RigidBody::set_use_continuous_collision_detection(bool p_enable) { - ccd = p_enable; - PhysicsServer::get_singleton()->body_set_enable_continuous_collision_detection(get_rid(), p_enable); -} - -bool RigidBody::is_using_continuous_collision_detection() const { - return ccd; -} - -void RigidBody::set_contact_monitor(bool p_enabled) { - if (p_enabled == is_contact_monitor_enabled()) { - return; - } - - if (!p_enabled) { - ERR_FAIL_COND_MSG(contact_monitor->locked, "Can't disable contact monitoring during in/out callback. Use call_deferred(\"set_contact_monitor\", false) instead."); - - for (RBMap::Element *E = contact_monitor->body_map.front(); E; E = E->next()) { - //clean up mess - Object *obj = ObjectDB::get_instance(E->key()); - Node *node = Object::cast_to(obj); - - if (node) { - node->disconnect(SceneStringNames::get_singleton()->tree_entered, this, SceneStringNames::get_singleton()->_body_enter_tree); - node->disconnect(SceneStringNames::get_singleton()->tree_exiting, this, SceneStringNames::get_singleton()->_body_exit_tree); - } - } - - memdelete(contact_monitor); - contact_monitor = nullptr; - } else { - contact_monitor = memnew(ContactMonitor); - contact_monitor->locked = false; - } -} - -bool RigidBody::is_contact_monitor_enabled() const { - return contact_monitor != nullptr; -} - -void RigidBody::set_axis_lock(PhysicsServer::BodyAxis p_axis, bool p_lock) { - PhysicsServer::get_singleton()->body_set_axis_lock(get_rid(), p_axis, p_lock); -} - -bool RigidBody::get_axis_lock(PhysicsServer::BodyAxis p_axis) const { - return PhysicsServer::get_singleton()->body_is_axis_locked(get_rid(), p_axis); -} - -Array RigidBody::get_colliding_bodies() const { - ERR_FAIL_COND_V(!contact_monitor, Array()); - - Array ret; - ret.resize(contact_monitor->body_map.size()); - int idx = 0; - for (const RBMap::Element *E = contact_monitor->body_map.front(); E; E = E->next()) { - Object *obj = ObjectDB::get_instance(E->key()); - if (!obj) { - ret.resize(ret.size() - 1); //ops - } else { - ret[idx++] = obj; - } - } - - return ret; -} - -String RigidBody::get_configuration_warning() const { - Transform t = get_transform(); - - String warning = CollisionObject::get_configuration_warning(); - - if ((get_mode() == MODE_RIGID || get_mode() == MODE_CHARACTER) && (ABS(t.basis.get_axis(0).length() - 1.0) > 0.05 || ABS(t.basis.get_axis(1).length() - 1.0) > 0.05 || ABS(t.basis.get_axis(2).length() - 1.0) > 0.05)) { - if (warning != String()) { - warning += "\n\n"; - } - warning += TTR("Size changes to RigidBody (in character or rigid modes) will be overridden by the physics engine when running.\nChange the size in children collision shapes instead."); - } - - return warning; -} - -void RigidBody::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_mode", "mode"), &RigidBody::set_mode); - ClassDB::bind_method(D_METHOD("get_mode"), &RigidBody::get_mode); - - ClassDB::bind_method(D_METHOD("set_mass", "mass"), &RigidBody::set_mass); - ClassDB::bind_method(D_METHOD("get_mass"), &RigidBody::get_mass); - - ClassDB::bind_method(D_METHOD("set_weight", "weight"), &RigidBody::set_weight); - ClassDB::bind_method(D_METHOD("get_weight"), &RigidBody::get_weight); - -#ifndef DISABLE_DEPRECATED - ClassDB::bind_method(D_METHOD("set_friction", "friction"), &RigidBody::set_friction); - ClassDB::bind_method(D_METHOD("get_friction"), &RigidBody::get_friction); - - ClassDB::bind_method(D_METHOD("set_bounce", "bounce"), &RigidBody::set_bounce); - ClassDB::bind_method(D_METHOD("get_bounce"), &RigidBody::get_bounce); -#endif // DISABLE_DEPRECATED - - ClassDB::bind_method(D_METHOD("set_physics_material_override", "physics_material_override"), &RigidBody::set_physics_material_override); - ClassDB::bind_method(D_METHOD("get_physics_material_override"), &RigidBody::get_physics_material_override); - - ClassDB::bind_method(D_METHOD("_reload_physics_characteristics"), &RigidBody::_reload_physics_characteristics); - - ClassDB::bind_method(D_METHOD("set_linear_velocity", "linear_velocity"), &RigidBody::set_linear_velocity); - ClassDB::bind_method(D_METHOD("get_linear_velocity"), &RigidBody::get_linear_velocity); - - ClassDB::bind_method(D_METHOD("set_angular_velocity", "angular_velocity"), &RigidBody::set_angular_velocity); - ClassDB::bind_method(D_METHOD("get_angular_velocity"), &RigidBody::get_angular_velocity); - - ClassDB::bind_method(D_METHOD("get_inverse_inertia_tensor"), &RigidBody::get_inverse_inertia_tensor); - - ClassDB::bind_method(D_METHOD("set_gravity_scale", "gravity_scale"), &RigidBody::set_gravity_scale); - ClassDB::bind_method(D_METHOD("get_gravity_scale"), &RigidBody::get_gravity_scale); - - ClassDB::bind_method(D_METHOD("set_linear_damp", "linear_damp"), &RigidBody::set_linear_damp); - ClassDB::bind_method(D_METHOD("get_linear_damp"), &RigidBody::get_linear_damp); - - ClassDB::bind_method(D_METHOD("set_angular_damp", "angular_damp"), &RigidBody::set_angular_damp); - ClassDB::bind_method(D_METHOD("get_angular_damp"), &RigidBody::get_angular_damp); - - ClassDB::bind_method(D_METHOD("set_max_contacts_reported", "amount"), &RigidBody::set_max_contacts_reported); - ClassDB::bind_method(D_METHOD("get_max_contacts_reported"), &RigidBody::get_max_contacts_reported); - - ClassDB::bind_method(D_METHOD("set_use_custom_integrator", "enable"), &RigidBody::set_use_custom_integrator); - ClassDB::bind_method(D_METHOD("is_using_custom_integrator"), &RigidBody::is_using_custom_integrator); - - ClassDB::bind_method(D_METHOD("set_contact_monitor", "enabled"), &RigidBody::set_contact_monitor); - ClassDB::bind_method(D_METHOD("is_contact_monitor_enabled"), &RigidBody::is_contact_monitor_enabled); - - ClassDB::bind_method(D_METHOD("set_use_continuous_collision_detection", "enable"), &RigidBody::set_use_continuous_collision_detection); - ClassDB::bind_method(D_METHOD("is_using_continuous_collision_detection"), &RigidBody::is_using_continuous_collision_detection); - - ClassDB::bind_method(D_METHOD("set_axis_velocity", "axis_velocity"), &RigidBody::set_axis_velocity); - - ClassDB::bind_method(D_METHOD("add_central_force", "force"), &RigidBody::add_central_force); - ClassDB::bind_method(D_METHOD("add_force", "force", "position"), &RigidBody::add_force); - ClassDB::bind_method(D_METHOD("add_torque", "torque"), &RigidBody::add_torque); - - ClassDB::bind_method(D_METHOD("apply_central_impulse", "impulse"), &RigidBody::apply_central_impulse); - ClassDB::bind_method(D_METHOD("apply_impulse", "position", "impulse"), &RigidBody::apply_impulse); - ClassDB::bind_method(D_METHOD("apply_torque_impulse", "impulse"), &RigidBody::apply_torque_impulse); - - ClassDB::bind_method(D_METHOD("set_sleeping", "sleeping"), &RigidBody::set_sleeping); - ClassDB::bind_method(D_METHOD("is_sleeping"), &RigidBody::is_sleeping); - - ClassDB::bind_method(D_METHOD("set_can_sleep", "able_to_sleep"), &RigidBody::set_can_sleep); - ClassDB::bind_method(D_METHOD("is_able_to_sleep"), &RigidBody::is_able_to_sleep); - - ClassDB::bind_method(D_METHOD("_direct_state_changed"), &RigidBody::_direct_state_changed); - ClassDB::bind_method(D_METHOD("_body_enter_tree"), &RigidBody::_body_enter_tree); - ClassDB::bind_method(D_METHOD("_body_exit_tree"), &RigidBody::_body_exit_tree); - - ClassDB::bind_method(D_METHOD("set_axis_lock", "axis", "lock"), &RigidBody::set_axis_lock); - ClassDB::bind_method(D_METHOD("get_axis_lock", "axis"), &RigidBody::get_axis_lock); - - ClassDB::bind_method(D_METHOD("get_colliding_bodies"), &RigidBody::get_colliding_bodies); - - BIND_VMETHOD(MethodInfo("_integrate_forces", PropertyInfo(Variant::OBJECT, "state", PROPERTY_HINT_RESOURCE_TYPE, "PhysicsDirectBodyState"))); - - ADD_PROPERTY(PropertyInfo(Variant::INT, "mode", PROPERTY_HINT_ENUM, "Rigid,Static,Character,Kinematic"), "set_mode", "get_mode"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "mass", PROPERTY_HINT_EXP_RANGE, "0.01,65535,0.01,or_greater"), "set_mass", "get_mass"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "weight", PROPERTY_HINT_EXP_RANGE, "0.01,65535,0.01,or_greater", PROPERTY_USAGE_EDITOR), "set_weight", "get_weight"); -#ifndef DISABLE_DEPRECATED - ADD_PROPERTY(PropertyInfo(Variant::REAL, "friction", PROPERTY_HINT_RANGE, "0,1,0.01", 0), "set_friction", "get_friction"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "bounce", PROPERTY_HINT_RANGE, "0,1,0.01", 0), "set_bounce", "get_bounce"); -#endif // DISABLE_DEPRECATED - ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "physics_material_override", PROPERTY_HINT_RESOURCE_TYPE, "PhysicsMaterial"), "set_physics_material_override", "get_physics_material_override"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "gravity_scale", PROPERTY_HINT_RANGE, "-128,128,0.01"), "set_gravity_scale", "get_gravity_scale"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "custom_integrator"), "set_use_custom_integrator", "is_using_custom_integrator"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "continuous_cd"), "set_use_continuous_collision_detection", "is_using_continuous_collision_detection"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "contacts_reported", PROPERTY_HINT_RANGE, "0,64,1,or_greater"), "set_max_contacts_reported", "get_max_contacts_reported"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "contact_monitor"), "set_contact_monitor", "is_contact_monitor_enabled"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "sleeping"), "set_sleeping", "is_sleeping"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "can_sleep"), "set_can_sleep", "is_able_to_sleep"); - ADD_GROUP("Axis Lock", "axis_lock_"); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "axis_lock_linear_x"), "set_axis_lock", "get_axis_lock", PhysicsServer::BODY_AXIS_LINEAR_X); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "axis_lock_linear_y"), "set_axis_lock", "get_axis_lock", PhysicsServer::BODY_AXIS_LINEAR_Y); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "axis_lock_linear_z"), "set_axis_lock", "get_axis_lock", PhysicsServer::BODY_AXIS_LINEAR_Z); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "axis_lock_angular_x"), "set_axis_lock", "get_axis_lock", PhysicsServer::BODY_AXIS_ANGULAR_X); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "axis_lock_angular_y"), "set_axis_lock", "get_axis_lock", PhysicsServer::BODY_AXIS_ANGULAR_Y); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "axis_lock_angular_z"), "set_axis_lock", "get_axis_lock", PhysicsServer::BODY_AXIS_ANGULAR_Z); - ADD_GROUP("Linear", "linear_"); - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "linear_velocity"), "set_linear_velocity", "get_linear_velocity"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "linear_damp", PROPERTY_HINT_RANGE, "-1,100,0.001,or_greater"), "set_linear_damp", "get_linear_damp"); - ADD_GROUP("Angular", "angular_"); - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "angular_velocity"), "set_angular_velocity", "get_angular_velocity"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "angular_damp", PROPERTY_HINT_RANGE, "-1,100,0.001,or_greater"), "set_angular_damp", "get_angular_damp"); - - ADD_SIGNAL(MethodInfo("body_shape_entered", PropertyInfo(Variant::RID, "body_rid"), PropertyInfo(Variant::OBJECT, "body", PROPERTY_HINT_RESOURCE_TYPE, "Node"), PropertyInfo(Variant::INT, "body_shape_index"), PropertyInfo(Variant::INT, "local_shape_index"))); - ADD_SIGNAL(MethodInfo("body_shape_exited", PropertyInfo(Variant::RID, "body_rid"), PropertyInfo(Variant::OBJECT, "body", PROPERTY_HINT_RESOURCE_TYPE, "Node"), PropertyInfo(Variant::INT, "body_shape_index"), PropertyInfo(Variant::INT, "local_shape_index"))); - ADD_SIGNAL(MethodInfo("body_entered", PropertyInfo(Variant::OBJECT, "body", PROPERTY_HINT_RESOURCE_TYPE, "Node"))); - ADD_SIGNAL(MethodInfo("body_exited", PropertyInfo(Variant::OBJECT, "body", PROPERTY_HINT_RESOURCE_TYPE, "Node"))); - ADD_SIGNAL(MethodInfo("sleeping_state_changed")); - - BIND_ENUM_CONSTANT(MODE_RIGID); - BIND_ENUM_CONSTANT(MODE_STATIC); - BIND_ENUM_CONSTANT(MODE_CHARACTER); - BIND_ENUM_CONSTANT(MODE_KINEMATIC); -} - -RigidBody::RigidBody() : - PhysicsBody(PhysicsServer::BODY_MODE_RIGID) { - mode = MODE_RIGID; - - mass = 1; - max_contacts_reported = 0; - state = nullptr; - - gravity_scale = 1; - linear_damp = -1; - angular_damp = -1; - - //angular_velocity=0; - sleeping = false; - ccd = false; - - custom_integrator = false; - contact_monitor = nullptr; - can_sleep = true; - - PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(), this, "_direct_state_changed"); -} - -RigidBody::~RigidBody() { - if (contact_monitor) { - memdelete(contact_monitor); - } -} - -void RigidBody::_reload_physics_characteristics() { - if (physics_material_override.is_null()) { - PhysicsServer::get_singleton()->body_set_param(get_rid(), PhysicsServer::BODY_PARAM_BOUNCE, 0); - PhysicsServer::get_singleton()->body_set_param(get_rid(), PhysicsServer::BODY_PARAM_FRICTION, 1); - } else { - PhysicsServer::get_singleton()->body_set_param(get_rid(), PhysicsServer::BODY_PARAM_BOUNCE, physics_material_override->computed_bounce()); - PhysicsServer::get_singleton()->body_set_param(get_rid(), PhysicsServer::BODY_PARAM_FRICTION, physics_material_override->computed_friction()); - } -} - -////////////////////////////////////////////////////// -////////////////////////// - -Ref KinematicBody::_move(const Vector3 &p_motion, bool p_infinite_inertia, bool p_exclude_raycast_shapes, bool p_test_only) { - Collision col; - - bool collided = move_and_collide(p_motion, p_infinite_inertia, col, p_exclude_raycast_shapes, p_test_only); - - // Don't report collision when the whole motion is done. - if (collided && col.collision_safe_fraction < 1) { - // Create a new instance when the cached reference is invalid or still in use in script. - if (motion_cache.is_null() || motion_cache->reference_get_count() > 1) { - motion_cache.instance(); - motion_cache->owner = this; - } - - motion_cache->collision = col; - - return motion_cache; - } - - return Ref(); -} - -bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collision, bool p_exclude_raycast_shapes, bool p_test_only, bool p_cancel_sliding, const RBSet &p_exclude) { - if (sync_to_physics) { - ERR_PRINT("Functions move_and_slide and move_and_collide do not work together with 'sync to physics' option. Please read the documentation."); - } - - Transform gt = get_global_transform(); - PhysicsServer::MotionResult result; - bool colliding = PhysicsServer::get_singleton()->body_test_motion(get_rid(), gt, p_motion, p_infinite_inertia, &result, p_exclude_raycast_shapes, p_exclude); - - // Restore direction of motion to be along original motion, - // in order to avoid sliding due to recovery, - // but only if collision depth is low enough to avoid tunneling. - if (p_cancel_sliding) { - real_t motion_length = p_motion.length(); - real_t precision = 0.001; - - if (colliding) { - // Can't just use margin as a threshold because collision depth is calculated on unsafe motion, - // so even in normal resting cases the depth can be a bit more than the margin. - precision += motion_length * (result.collision_unsafe_fraction - result.collision_safe_fraction); - - if (result.collision_depth > (real_t)margin + precision) { - p_cancel_sliding = false; - } - } - - if (p_cancel_sliding) { - // When motion is null, recovery is the resulting motion. - Vector3 motion_normal; - if (motion_length > CMP_EPSILON) { - motion_normal = p_motion / motion_length; - } - - // Check depth of recovery. - real_t projected_length = result.motion.dot(motion_normal); - Vector3 recovery = result.motion - motion_normal * projected_length; - real_t recovery_length = recovery.length(); - // Fixes cases where canceling slide causes the motion to go too deep into the ground, - // because we're only taking rest information into account and not general recovery. - if (recovery_length < (real_t)margin + precision) { - // Apply adjustment to motion. - result.motion = motion_normal * projected_length; - result.remainder = p_motion - result.motion; - } - } - } - - if (colliding) { - r_collision.collider_metadata = result.collider_metadata; - r_collision.collider_shape = result.collider_shape; - r_collision.collider_vel = result.collider_velocity; - r_collision.collision = result.collision_point; - r_collision.normal = result.collision_normal; - r_collision.collider = result.collider_id; - r_collision.collider_rid = result.collider; - r_collision.travel = result.motion; - r_collision.remainder = result.remainder; - r_collision.local_shape = result.collision_local_shape; - r_collision.collision_safe_fraction = result.collision_safe_fraction; - } - - for (int i = 0; i < 3; i++) { - if (locked_axis & (1 << i)) { - result.motion[i] = 0; - } - } - - if (!p_test_only) { - gt.origin += result.motion; - set_global_transform(gt); - } - - return colliding; -} - -//so, if you pass 45 as limit, avoid numerical precision errors when angle is 45. -#define FLOOR_ANGLE_THRESHOLD 0.01 - -Vector3 KinematicBody::_move_and_slide_internal(const Vector3 &p_linear_velocity, const Vector3 &p_snap, const Vector3 &p_up_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) { - Vector3 body_velocity = p_linear_velocity; - Vector3 body_velocity_normal = body_velocity.normalized(); - Vector3 up_direction = p_up_direction.normalized(); - bool was_on_floor = on_floor; - - for (int i = 0; i < 3; i++) { - if (locked_axis & (1 << i)) { - body_velocity[i] = 0; - } - } - - // Hack in order to work with calling from _process as well as from _physics_process; calling from thread is risky - float delta = Engine::get_singleton()->is_in_physics_frame() ? get_physics_process_delta_time() : get_process_delta_time(); - - Vector3 current_floor_velocity = floor_velocity; - if (on_floor && on_floor_body.is_valid()) { - // This approach makes sure there is less delay between the actual body velocity and the one we saved. - PhysicsDirectBodyState *bs = PhysicsServer::get_singleton()->body_get_direct_state(on_floor_body); - if (bs) { - Transform gt = get_global_transform(); - Vector3 local_position = gt.origin - bs->get_transform().origin; - current_floor_velocity = bs->get_velocity_at_local_position(local_position); - } else { - // Body is removed or destroyed, invalidate floor. - current_floor_velocity = Vector3(); - on_floor_body = RID(); - } - } - - colliders.clear(); - on_floor = false; - on_ceiling = false; - on_wall = false; - floor_normal = Vector3(); - floor_velocity = Vector3(); - - if (current_floor_velocity != Vector3() && on_floor_body.is_valid()) { - Collision floor_collision; - RBSet exclude; - exclude.insert(on_floor_body); - if (move_and_collide(current_floor_velocity * delta, p_infinite_inertia, floor_collision, true, false, false, exclude)) { - colliders.push_back(floor_collision); - _set_collision_direction(floor_collision, up_direction, p_floor_max_angle); - } - } - - on_floor_body = RID(); - Vector3 motion = body_velocity * delta; - - // No sliding on first attempt to keep floor motion stable when possible, - // when stop on slope is enabled. - bool sliding_enabled = !p_stop_on_slope; - for (int iteration = 0; iteration < p_max_slides; ++iteration) { - Collision collision; - bool found_collision = false; - - for (int i = 0; i < 2; ++i) { - bool collided; - if (i == 0) { //collide - collided = move_and_collide(motion, p_infinite_inertia, collision, true, false, !sliding_enabled); - if (!collided) { - motion = Vector3(); //clear because no collision happened and motion completed - } - } else { //separate raycasts (if any) - collided = separate_raycast_shapes(p_infinite_inertia, collision); - if (collided) { - collision.remainder = motion; //keep - collision.travel = Vector3(); - } - } - - if (collided) { - found_collision = true; - - colliders.push_back(collision); - - _set_collision_direction(collision, up_direction, p_floor_max_angle); - - if (on_floor && p_stop_on_slope) { - if ((body_velocity_normal + up_direction).length() < 0.01) { - Transform gt = get_global_transform(); - if (collision.travel.length() > margin) { - gt.origin -= collision.travel.slide(up_direction); - } else { - gt.origin -= collision.travel; - } - set_global_transform(gt); - return Vector3(); - } - } - - if (sliding_enabled || !on_floor) { - motion = collision.remainder.slide(collision.normal); - body_velocity = body_velocity.slide(collision.normal); - - for (int j = 0; j < 3; j++) { - if (locked_axis & (1 << j)) { - body_velocity[j] = 0; - } - } - } else { - motion = collision.remainder; - } - } - - sliding_enabled = true; - } - - if (!found_collision || motion == Vector3()) { - break; - } - } - - if (was_on_floor && p_snap != Vector3() && !on_floor) { - // Apply snap. - Collision col; - Transform gt = get_global_transform(); - - if (move_and_collide(p_snap, p_infinite_inertia, col, false, true, false)) { - bool apply = true; - if (up_direction != Vector3()) { - if (Math::acos(col.normal.dot(up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { - on_floor = true; - floor_normal = col.normal; - on_floor_body = col.collider_rid; - floor_velocity = col.collider_vel; - if (p_stop_on_slope) { - // move and collide may stray the object a bit because of pre un-stucking, - // so only ensure that motion happens on floor direction in this case. - if (col.travel.length() > margin) { - col.travel = col.travel.project(up_direction); - } else { - col.travel = Vector3(); - } - } - } else { - apply = false; //snapped with floor direction, but did not snap to a floor, do not snap. - } - } - if (apply) { - gt.origin += col.travel; - set_global_transform(gt); - } - } - } - - if (moving_platform_apply_velocity_on_leave != PLATFORM_VEL_ON_LEAVE_NEVER) { - // Add last platform velocity when just left a moving platform. - if (!on_floor) { - if (moving_platform_apply_velocity_on_leave == PLATFORM_VEL_ON_LEAVE_UPWARD_ONLY && current_floor_velocity.dot(up_direction) < 0) { - current_floor_velocity = current_floor_velocity.slide(up_direction); - } - return body_velocity + current_floor_velocity; - } - } - - return body_velocity; -} - -Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Vector3 &p_up_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) { - return _move_and_slide_internal(p_linear_velocity, Vector3(), p_up_direction, p_stop_on_slope, p_max_slides, p_floor_max_angle, p_infinite_inertia); -} - -Vector3 KinematicBody::move_and_slide_with_snap(const Vector3 &p_linear_velocity, const Vector3 &p_snap, const Vector3 &p_up_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) { - return _move_and_slide_internal(p_linear_velocity, p_snap, p_up_direction, p_stop_on_slope, p_max_slides, p_floor_max_angle, p_infinite_inertia); -} - -void KinematicBody::_set_collision_direction(const Collision &p_collision, const Vector3 &p_up_direction, float p_floor_max_angle) { - if (p_up_direction == Vector3()) { - //all is a wall - on_wall = true; - } else { - if (Math::acos(p_collision.normal.dot(p_up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //floor - on_floor = true; - floor_normal = p_collision.normal; - on_floor_body = p_collision.collider_rid; - floor_velocity = p_collision.collider_vel; - } else if (Math::acos(p_collision.normal.dot(-p_up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //ceiling - on_ceiling = true; - } else { - on_wall = true; - } - } -} - -bool KinematicBody::is_on_floor() const { - return on_floor; -} - -bool KinematicBody::is_on_wall() const { - return on_wall; -} -bool KinematicBody::is_on_ceiling() const { - return on_ceiling; -} - -Vector3 KinematicBody::get_floor_normal() const { - return floor_normal; -} - -real_t KinematicBody::get_floor_angle(const Vector3 &p_up_direction) const { - ERR_FAIL_COND_V(p_up_direction == Vector3(), 0); - return Math::acos(floor_normal.dot(p_up_direction)); -} - -Vector3 KinematicBody::get_floor_velocity() const { - return floor_velocity; -} - -void KinematicBody::set_moving_platform_apply_velocity_on_leave(MovingPlatformApplyVelocityOnLeave p_on_leave_apply_velocity) { - moving_platform_apply_velocity_on_leave = p_on_leave_apply_velocity; -} - -KinematicBody::MovingPlatformApplyVelocityOnLeave KinematicBody::get_moving_platform_apply_velocity_on_leave() const { - return moving_platform_apply_velocity_on_leave; -} - -bool KinematicBody::test_move(const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia) { - ERR_FAIL_COND_V(!is_inside_tree(), false); - - PhysicsServer::MotionResult result; - bool colliding = PhysicsServer::get_singleton()->body_test_motion(get_rid(), p_from, p_motion, p_infinite_inertia, &result); - - if (colliding) { - // Don't report collision when the whole motion is done. - return (result.collision_safe_fraction < 1.0); - } else { - return false; - } -} - -bool KinematicBody::separate_raycast_shapes(bool p_infinite_inertia, Collision &r_collision) { - PhysicsServer::SeparationResult sep_res[8]; //max 8 rays - - Transform gt = get_global_transform(); - - Vector3 recover; - int hits = PhysicsServer::get_singleton()->body_test_ray_separation(get_rid(), gt, p_infinite_inertia, recover, sep_res, 8, margin); - int deepest = -1; - float deepest_depth; - for (int i = 0; i < hits; i++) { - if (deepest == -1 || sep_res[i].collision_depth > deepest_depth) { - deepest = i; - deepest_depth = sep_res[i].collision_depth; - } - } - - gt.origin += recover; - set_global_transform(gt); - - if (deepest != -1) { - r_collision.collider = sep_res[deepest].collider_id; - r_collision.collider_rid = sep_res[deepest].collider; - r_collision.collider_metadata = sep_res[deepest].collider_metadata; - r_collision.collider_shape = sep_res[deepest].collider_shape; - r_collision.collider_vel = sep_res[deepest].collider_velocity; - r_collision.collision = sep_res[deepest].collision_point; - r_collision.normal = sep_res[deepest].collision_normal; - r_collision.local_shape = sep_res[deepest].collision_local_shape; - r_collision.travel = recover; - r_collision.remainder = Vector3(); - - return true; - } else { - return false; - } -} - -void KinematicBody::set_axis_lock(PhysicsServer::BodyAxis p_axis, bool p_lock) { - if (p_lock) { - locked_axis |= p_axis; - } else { - locked_axis &= (~p_axis); - } - PhysicsServer::get_singleton()->body_set_axis_lock(get_rid(), p_axis, p_lock); -} - -bool KinematicBody::get_axis_lock(PhysicsServer::BodyAxis p_axis) const { - return PhysicsServer::get_singleton()->body_is_axis_locked(get_rid(), p_axis); -} - -void KinematicBody::set_safe_margin(float p_margin) { - margin = p_margin; - PhysicsServer::get_singleton()->body_set_kinematic_safe_margin(get_rid(), margin); -} - -float KinematicBody::get_safe_margin() const { - return margin; -} -int KinematicBody::get_slide_count() const { - return colliders.size(); -} - -KinematicBody::Collision KinematicBody::get_slide_collision(int p_bounce) const { - ERR_FAIL_INDEX_V(p_bounce, colliders.size(), Collision()); - return colliders[p_bounce]; -} - -Ref KinematicBody::_get_slide_collision(int p_bounce) { - ERR_FAIL_INDEX_V(p_bounce, colliders.size(), Ref()); - if (p_bounce >= slide_colliders.size()) { - slide_colliders.resize(p_bounce + 1); - } - - // Create a new instance when the cached reference is invalid or still in use in script. - if (slide_colliders[p_bounce].is_null() || slide_colliders[p_bounce]->reference_get_count() > 1) { - slide_colliders.write[p_bounce].instance(); - slide_colliders.write[p_bounce]->owner = this; - } - - slide_colliders.write[p_bounce]->collision = colliders[p_bounce]; - return slide_colliders[p_bounce]; -} - -Ref KinematicBody::_get_last_slide_collision() { - if (colliders.size() == 0) { - return Ref(); - } - return _get_slide_collision(colliders.size() - 1); -} - -void KinematicBody::set_sync_to_physics(bool p_enable) { - if (sync_to_physics == p_enable) { - return; - } - sync_to_physics = p_enable; - - if (Engine::get_singleton()->is_editor_hint()) { - return; - } - - if (p_enable) { - PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(), this, "_direct_state_changed"); - set_only_update_transform_changes(true); - set_notify_local_transform(true); - } else { - PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(), nullptr, ""); - set_only_update_transform_changes(false); - set_notify_local_transform(false); - } -} - -bool KinematicBody::is_sync_to_physics_enabled() const { - return sync_to_physics; -} - -void KinematicBody::_direct_state_changed(Object *p_state) { - if (!sync_to_physics) { - return; - } - - PhysicsDirectBodyState *state = Object::cast_to(p_state); - ERR_FAIL_COND_MSG(!state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState object as argument"); - - last_valid_transform = state->get_transform(); - set_notify_local_transform(false); - set_global_transform(last_valid_transform); - set_notify_local_transform(true); - _on_transform_changed(); -} - -void KinematicBody::_notification(int p_what) { - if (p_what == NOTIFICATION_ENTER_TREE) { - last_valid_transform = get_global_transform(); - - // Reset move_and_slide() data. - on_floor = false; - on_floor_body = RID(); - on_ceiling = false; - on_wall = false; - colliders.clear(); - floor_velocity = Vector3(); - } - - if (p_what == NOTIFICATION_LOCAL_TRANSFORM_CHANGED) { - //used by sync to physics, send the new transform to the physics - Transform new_transform = get_global_transform(); - PhysicsServer::get_singleton()->body_set_state(get_rid(), PhysicsServer::BODY_STATE_TRANSFORM, new_transform); - //but then revert changes - set_notify_local_transform(false); - set_global_transform(last_valid_transform); - set_notify_local_transform(true); - _on_transform_changed(); - } -} - -void KinematicBody::_bind_methods() { - ClassDB::bind_method(D_METHOD("move_and_collide", "rel_vec", "infinite_inertia", "exclude_raycast_shapes", "test_only"), &KinematicBody::_move, DEFVAL(true), DEFVAL(true), DEFVAL(false)); - ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity", "up_direction", "stop_on_slope", "max_slides", "floor_max_angle", "infinite_inertia"), &KinematicBody::move_and_slide, DEFVAL(Vector3(0, 0, 0)), DEFVAL(false), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)), DEFVAL(true)); - ClassDB::bind_method(D_METHOD("move_and_slide_with_snap", "linear_velocity", "snap", "up_direction", "stop_on_slope", "max_slides", "floor_max_angle", "infinite_inertia"), &KinematicBody::move_and_slide_with_snap, DEFVAL(Vector3(0, 0, 0)), DEFVAL(false), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)), DEFVAL(true)); - - ClassDB::bind_method(D_METHOD("test_move", "from", "rel_vec", "infinite_inertia"), &KinematicBody::test_move, DEFVAL(true)); - - ClassDB::bind_method(D_METHOD("is_on_floor"), &KinematicBody::is_on_floor); - ClassDB::bind_method(D_METHOD("is_on_ceiling"), &KinematicBody::is_on_ceiling); - ClassDB::bind_method(D_METHOD("is_on_wall"), &KinematicBody::is_on_wall); - ClassDB::bind_method(D_METHOD("get_floor_normal"), &KinematicBody::get_floor_normal); - ClassDB::bind_method(D_METHOD("get_floor_angle", "up_direction"), &KinematicBody::get_floor_angle, DEFVAL(Vector3(0.0, 1.0, 0.0))); - ClassDB::bind_method(D_METHOD("get_floor_velocity"), &KinematicBody::get_floor_velocity); - - ClassDB::bind_method(D_METHOD("set_axis_lock", "axis", "lock"), &KinematicBody::set_axis_lock); - ClassDB::bind_method(D_METHOD("get_axis_lock", "axis"), &KinematicBody::get_axis_lock); - - ClassDB::bind_method(D_METHOD("set_safe_margin", "pixels"), &KinematicBody::set_safe_margin); - ClassDB::bind_method(D_METHOD("get_safe_margin"), &KinematicBody::get_safe_margin); - - ClassDB::bind_method(D_METHOD("set_moving_platform_apply_velocity_on_leave", "on_leave_apply_velocity"), &KinematicBody::set_moving_platform_apply_velocity_on_leave); - ClassDB::bind_method(D_METHOD("get_moving_platform_apply_velocity_on_leave"), &KinematicBody::get_moving_platform_apply_velocity_on_leave); - - ClassDB::bind_method(D_METHOD("get_slide_count"), &KinematicBody::get_slide_count); - ClassDB::bind_method(D_METHOD("get_slide_collision", "slide_idx"), &KinematicBody::_get_slide_collision); - ClassDB::bind_method(D_METHOD("get_last_slide_collision"), &KinematicBody::_get_last_slide_collision); - - ClassDB::bind_method(D_METHOD("set_sync_to_physics", "enable"), &KinematicBody::set_sync_to_physics); - ClassDB::bind_method(D_METHOD("is_sync_to_physics_enabled"), &KinematicBody::is_sync_to_physics_enabled); - - ClassDB::bind_method(D_METHOD("_direct_state_changed"), &KinematicBody::_direct_state_changed); - - ADD_GROUP("Axis Lock", "axis_lock_"); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "axis_lock_motion_x"), "set_axis_lock", "get_axis_lock", PhysicsServer::BODY_AXIS_LINEAR_X); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "axis_lock_motion_y"), "set_axis_lock", "get_axis_lock", PhysicsServer::BODY_AXIS_LINEAR_Y); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "axis_lock_motion_z"), "set_axis_lock", "get_axis_lock", PhysicsServer::BODY_AXIS_LINEAR_Z); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "move_lock_x", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR), "set_axis_lock", "get_axis_lock", PhysicsServer::BODY_AXIS_LINEAR_X); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "move_lock_y", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR), "set_axis_lock", "get_axis_lock", PhysicsServer::BODY_AXIS_LINEAR_Y); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "move_lock_z", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR), "set_axis_lock", "get_axis_lock", PhysicsServer::BODY_AXIS_LINEAR_Z); - - ADD_PROPERTY(PropertyInfo(Variant::REAL, "collision/safe_margin", PROPERTY_HINT_RANGE, "0.001,256,0.001"), "set_safe_margin", "get_safe_margin"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "motion/sync_to_physics"), "set_sync_to_physics", "is_sync_to_physics_enabled"); - ADD_GROUP("Moving platform", "moving_platform"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "moving_platform_apply_velocity_on_leave", PROPERTY_HINT_ENUM, "Always,Upward Only,Never", PROPERTY_USAGE_DEFAULT), "set_moving_platform_apply_velocity_on_leave", "get_moving_platform_apply_velocity_on_leave"); - - BIND_ENUM_CONSTANT(PLATFORM_VEL_ON_LEAVE_ALWAYS); - BIND_ENUM_CONSTANT(PLATFORM_VEL_ON_LEAVE_UPWARD_ONLY); - BIND_ENUM_CONSTANT(PLATFORM_VEL_ON_LEAVE_NEVER); -} - -KinematicBody::KinematicBody() : - PhysicsBody(PhysicsServer::BODY_MODE_KINEMATIC) { - locked_axis = 0; - on_floor = false; - on_ceiling = false; - on_wall = false; - - set_safe_margin(0.001); -} - -KinematicBody::~KinematicBody() { - if (motion_cache.is_valid()) { - motion_cache->owner = nullptr; - } - - for (int i = 0; i < slide_colliders.size(); i++) { - if (slide_colliders[i].is_valid()) { - slide_colliders.write[i]->owner = nullptr; - } - } -} - -/////////////////////////////////////// - -Vector3 KinematicCollision::get_position() const { - return collision.collision; -} -Vector3 KinematicCollision::get_normal() const { - return collision.normal; -} -Vector3 KinematicCollision::get_travel() const { - return collision.travel; -} -Vector3 KinematicCollision::get_remainder() const { - return collision.remainder; -} - -real_t KinematicCollision::get_angle(const Vector3 &p_up_direction) const { - ERR_FAIL_COND_V(p_up_direction == Vector3(), 0); - return collision.get_angle(p_up_direction); -} - -Object *KinematicCollision::get_local_shape() const { - if (!owner) { - return nullptr; - } - uint32_t ownerid = owner->shape_find_owner(collision.local_shape); - return owner->shape_owner_get_owner(ownerid); -} - -Object *KinematicCollision::get_collider() const { - if (collision.collider) { - return ObjectDB::get_instance(collision.collider); - } - - return nullptr; -} -ObjectID KinematicCollision::get_collider_id() const { - return collision.collider; -} -RID KinematicCollision::get_collider_rid() const { - return collision.collider_rid; -} -Object *KinematicCollision::get_collider_shape() const { - Object *collider = get_collider(); - if (collider) { - CollisionObject *obj2d = Object::cast_to(collider); - if (obj2d) { - uint32_t ownerid = obj2d->shape_find_owner(collision.collider_shape); - return obj2d->shape_owner_get_owner(ownerid); - } - } - - return nullptr; -} -int KinematicCollision::get_collider_shape_index() const { - return collision.collider_shape; -} -Vector3 KinematicCollision::get_collider_velocity() const { - return collision.collider_vel; -} -Variant KinematicCollision::get_collider_metadata() const { - return Variant(); -} - -void KinematicCollision::_bind_methods() { - ClassDB::bind_method(D_METHOD("get_position"), &KinematicCollision::get_position); - ClassDB::bind_method(D_METHOD("get_normal"), &KinematicCollision::get_normal); - ClassDB::bind_method(D_METHOD("get_travel"), &KinematicCollision::get_travel); - ClassDB::bind_method(D_METHOD("get_remainder"), &KinematicCollision::get_remainder); - ClassDB::bind_method(D_METHOD("get_angle", "up_direction"), &KinematicCollision::get_angle, DEFVAL(Vector3(0.0, 1.0, 0.0))); - ClassDB::bind_method(D_METHOD("get_local_shape"), &KinematicCollision::get_local_shape); - ClassDB::bind_method(D_METHOD("get_collider"), &KinematicCollision::get_collider); - ClassDB::bind_method(D_METHOD("get_collider_id"), &KinematicCollision::get_collider_id); - ClassDB::bind_method(D_METHOD("get_collider_rid"), &KinematicCollision::get_collider_rid); - ClassDB::bind_method(D_METHOD("get_collider_shape"), &KinematicCollision::get_collider_shape); - ClassDB::bind_method(D_METHOD("get_collider_shape_index"), &KinematicCollision::get_collider_shape_index); - ClassDB::bind_method(D_METHOD("get_collider_velocity"), &KinematicCollision::get_collider_velocity); - ClassDB::bind_method(D_METHOD("get_collider_metadata"), &KinematicCollision::get_collider_metadata); - - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "position"), "", "get_position"); - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "normal"), "", "get_normal"); - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "travel"), "", "get_travel"); - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "remainder"), "", "get_remainder"); - ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "local_shape"), "", "get_local_shape"); - ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "collider"), "", "get_collider"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "collider_id"), "", "get_collider_id"); - ADD_PROPERTY(PropertyInfo(Variant::RID, "collider_rid"), "", "get_collider_rid"); - ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "collider_shape"), "", "get_collider_shape"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "collider_shape_index"), "", "get_collider_shape_index"); - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "collider_velocity"), "", "get_collider_velocity"); - ADD_PROPERTY(PropertyInfo(Variant::NIL, "collider_metadata", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NIL_IS_VARIANT), "", "get_collider_metadata"); -} - -KinematicCollision::KinematicCollision() { - collision.collider = 0; - collision.collider_shape = 0; - collision.local_shape = 0; - owner = nullptr; -} - -KinematicCollision::~KinematicCollision() { -} diff --git a/scene/3d/physics_body.h b/scene/3d/physics_body.h deleted file mode 100644 index 95faf86..0000000 --- a/scene/3d/physics_body.h +++ /dev/null @@ -1,386 +0,0 @@ -#ifndef PHYSICS_BODY__H -#define PHYSICS_BODY__H -/*************************************************************************/ -/* physics_body.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "core/containers/vset.h" -#include "core/object/reference.h" -#include "scene/3d/collision_object.h" -#include "servers/physics_server.h" - -class PhysicsMaterial; -class Skeleton; - -class PhysicsBody : public CollisionObject { - GDCLASS(PhysicsBody, CollisionObject); - - void _set_layers(uint32_t p_mask); - uint32_t _get_layers() const; - -protected: - static void _bind_methods(); - void _notification(int p_what); - PhysicsBody(PhysicsServer::BodyMode p_mode); - ~PhysicsBody(); - -public: - virtual Vector3 get_linear_velocity() const; - virtual Vector3 get_angular_velocity() const; - virtual float get_inverse_mass() const; - - Array get_collision_exceptions(); - void add_collision_exception_with(Node *p_node); //must be physicsbody - void remove_collision_exception_with(Node *p_node); -}; - -class StaticBody : public PhysicsBody { - GDCLASS(StaticBody, PhysicsBody); - - Vector3 constant_linear_velocity; - Vector3 constant_angular_velocity; - - Ref physics_material_override; - -protected: - static void _bind_methods(); - -public: -#ifndef DISABLE_DEPRECATED - void set_friction(real_t p_friction); - real_t get_friction() const; - - void set_bounce(real_t p_bounce); - real_t get_bounce() const; -#endif - - void set_physics_material_override(const Ref &p_physics_material_override); - Ref get_physics_material_override() const; - - void set_constant_linear_velocity(const Vector3 &p_vel); - void set_constant_angular_velocity(const Vector3 &p_vel); - - Vector3 get_constant_linear_velocity() const; - Vector3 get_constant_angular_velocity() const; - - StaticBody(); - ~StaticBody(); - -private: - void _reload_physics_characteristics(); -}; - -class RigidBody : public PhysicsBody { - GDCLASS(RigidBody, PhysicsBody); - -public: - enum Mode { - MODE_RIGID, - MODE_STATIC, - MODE_CHARACTER, - MODE_KINEMATIC, - }; - -protected: - bool can_sleep; - PhysicsDirectBodyState *state; - Mode mode; - - real_t mass; - Ref physics_material_override; - - Vector3 linear_velocity; - Vector3 angular_velocity; - Basis inverse_inertia_tensor; - real_t gravity_scale; - real_t linear_damp; - real_t angular_damp; - - bool sleeping; - bool ccd; - - int max_contacts_reported; - - bool custom_integrator; - - struct ShapePair { - int body_shape; - int local_shape; - bool tagged; - bool operator<(const ShapePair &p_sp) const { - if (body_shape == p_sp.body_shape) { - return local_shape < p_sp.local_shape; - } else { - return body_shape < p_sp.body_shape; - } - } - - ShapePair() {} - ShapePair(int p_bs, int p_ls) { - body_shape = p_bs; - local_shape = p_ls; - tagged = false; - } - }; - struct RigidBody_RemoveAction { - RID rid; - ObjectID body_id; - ShapePair pair; - }; - struct BodyState { - RID rid; - bool in_tree; - VSet shapes; - }; - - struct ContactMonitor { - bool locked; - RBMap body_map; - }; - - ContactMonitor *contact_monitor; - void _body_enter_tree(ObjectID p_id); - void _body_exit_tree(ObjectID p_id); - - void _body_inout(int p_status, const RID &p_body, ObjectID p_instance, int p_body_shape, int p_local_shape); - virtual void _direct_state_changed(Object *p_state); - - void _notification(int p_what); - static void _bind_methods(); - -public: - void set_mode(Mode p_mode); - Mode get_mode() const; - - void set_mass(real_t p_mass); - real_t get_mass() const; - - virtual float get_inverse_mass() const { return 1.0 / mass; } - - void set_weight(real_t p_weight); - real_t get_weight() const; - -#ifndef DISABLE_DEPRECATED - void set_friction(real_t p_friction); - real_t get_friction() const; - - void set_bounce(real_t p_bounce); - real_t get_bounce() const; -#endif - - void set_physics_material_override(const Ref &p_physics_material_override); - Ref get_physics_material_override() const; - - void set_linear_velocity(const Vector3 &p_velocity); - Vector3 get_linear_velocity() const; - - void set_axis_velocity(const Vector3 &p_axis); - - void set_angular_velocity(const Vector3 &p_velocity); - Vector3 get_angular_velocity() const; - - Basis get_inverse_inertia_tensor(); - - void set_gravity_scale(real_t p_gravity_scale); - real_t get_gravity_scale() const; - - void set_linear_damp(real_t p_linear_damp); - real_t get_linear_damp() const; - - void set_angular_damp(real_t p_angular_damp); - real_t get_angular_damp() const; - - void set_use_custom_integrator(bool p_enable); - bool is_using_custom_integrator(); - - void set_sleeping(bool p_sleeping); - bool is_sleeping() const; - - void set_can_sleep(bool p_active); - bool is_able_to_sleep() const; - - void set_contact_monitor(bool p_enabled); - bool is_contact_monitor_enabled() const; - - void set_max_contacts_reported(int p_amount); - int get_max_contacts_reported() const; - - void set_use_continuous_collision_detection(bool p_enable); - bool is_using_continuous_collision_detection() const; - - void set_axis_lock(PhysicsServer::BodyAxis p_axis, bool p_lock); - bool get_axis_lock(PhysicsServer::BodyAxis p_axis) const; - - Array get_colliding_bodies() const; - - void add_central_force(const Vector3 &p_force); - void add_force(const Vector3 &p_force, const Vector3 &p_pos); - void add_torque(const Vector3 &p_torque); - - void apply_central_impulse(const Vector3 &p_impulse); - void apply_impulse(const Vector3 &p_pos, const Vector3 &p_impulse); - void apply_torque_impulse(const Vector3 &p_impulse); - - virtual String get_configuration_warning() const; - - RigidBody(); - ~RigidBody(); - -private: - void _reload_physics_characteristics(); -}; - -VARIANT_ENUM_CAST(RigidBody::Mode); - -class KinematicCollision; - -class KinematicBody : public PhysicsBody { - GDCLASS(KinematicBody, PhysicsBody); - -public: - enum MovingPlatformApplyVelocityOnLeave { - PLATFORM_VEL_ON_LEAVE_ALWAYS, - PLATFORM_VEL_ON_LEAVE_UPWARD_ONLY, - PLATFORM_VEL_ON_LEAVE_NEVER, - }; - struct Collision { - Vector3 collision; - Vector3 normal; - Vector3 collider_vel; - ObjectID collider; - RID collider_rid; - int collider_shape; - Variant collider_metadata; - Vector3 remainder; - Vector3 travel; - int local_shape; - real_t collision_safe_fraction; - - real_t get_angle(const Vector3 &p_up_direction) const { - return Math::acos(normal.dot(p_up_direction)); - } - }; - -private: - uint16_t locked_axis; - - float margin; - - Vector3 floor_normal; - Vector3 floor_velocity; - RID on_floor_body; - bool on_floor; - bool on_ceiling; - bool on_wall; - bool sync_to_physics = false; - MovingPlatformApplyVelocityOnLeave moving_platform_apply_velocity_on_leave = PLATFORM_VEL_ON_LEAVE_ALWAYS; - - Vector colliders; - Vector> slide_colliders; - Ref motion_cache; - - Ref _move(const Vector3 &p_motion, bool p_infinite_inertia = true, bool p_exclude_raycast_shapes = true, bool p_test_only = false); - Ref _get_slide_collision(int p_bounce); - Ref _get_last_slide_collision(); - - Transform last_valid_transform; - void _direct_state_changed(Object *p_state); - - Vector3 _move_and_slide_internal(const Vector3 &p_linear_velocity, const Vector3 &p_snap, const Vector3 &p_up_direction = Vector3(0, 0, 0), bool p_stop_on_slope = false, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45), bool p_infinite_inertia = true); - void _set_collision_direction(const Collision &p_collision, const Vector3 &p_up_direction, float p_floor_max_angle); - void set_moving_platform_apply_velocity_on_leave(MovingPlatformApplyVelocityOnLeave p_on_leave_velocity); - MovingPlatformApplyVelocityOnLeave get_moving_platform_apply_velocity_on_leave() const; - -protected: - void _notification(int p_what); - static void _bind_methods(); - -public: - bool move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collision, bool p_exclude_raycast_shapes = true, bool p_test_only = false, bool p_cancel_sliding = true, const RBSet &p_exclude = RBSet()); - bool test_move(const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia); - - bool separate_raycast_shapes(bool p_infinite_inertia, Collision &r_collision); - - void set_axis_lock(PhysicsServer::BodyAxis p_axis, bool p_lock); - bool get_axis_lock(PhysicsServer::BodyAxis p_axis) const; - - void set_safe_margin(float p_margin); - float get_safe_margin() const; - - Vector3 move_and_slide(const Vector3 &p_linear_velocity, const Vector3 &p_up_direction = Vector3(0, 0, 0), bool p_stop_on_slope = false, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45), bool p_infinite_inertia = true); - Vector3 move_and_slide_with_snap(const Vector3 &p_linear_velocity, const Vector3 &p_snap, const Vector3 &p_up_direction = Vector3(0, 0, 0), bool p_stop_on_slope = false, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45), bool p_infinite_inertia = true); - bool is_on_floor() const; - bool is_on_wall() const; - bool is_on_ceiling() const; - Vector3 get_floor_normal() const; - real_t get_floor_angle(const Vector3 &p_up_direction = Vector3(0.0, 1.0, 0.0)) const; - Vector3 get_floor_velocity() const; - - int get_slide_count() const; - Collision get_slide_collision(int p_bounce) const; - - void set_sync_to_physics(bool p_enable); - bool is_sync_to_physics_enabled() const; - - KinematicBody(); - ~KinematicBody(); -}; - -VARIANT_ENUM_CAST(KinematicBody::MovingPlatformApplyVelocityOnLeave); - -class KinematicCollision : public Reference { - GDCLASS(KinematicCollision, Reference); - - KinematicBody *owner; - friend class KinematicBody; - KinematicBody::Collision collision; - -protected: - static void _bind_methods(); - -public: - Vector3 get_position() const; - Vector3 get_normal() const; - Vector3 get_travel() const; - Vector3 get_remainder() const; - real_t get_angle(const Vector3 &p_up_direction = Vector3(0.0, 1.0, 0.0)) const; - Object *get_local_shape() const; - Object *get_collider() const; - ObjectID get_collider_id() const; - RID get_collider_rid() const; - Object *get_collider_shape() const; - int get_collider_shape_index() const; - Vector3 get_collider_velocity() const; - Variant get_collider_metadata() const; - - KinematicCollision(); - ~KinematicCollision(); -}; - -#endif // PHYSICS_BODY__H diff --git a/scene/3d/physics_joint.cpp b/scene/3d/physics_joint.cpp deleted file mode 100644 index b8f3b74..0000000 --- a/scene/3d/physics_joint.cpp +++ /dev/null @@ -1,1079 +0,0 @@ -/*************************************************************************/ -/* physics_joint.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_joint.h" - -#include "scene/3d/physics_body.h" -#include "scene/resources/shapes/shape.h" -#include "scene/main/scene_string_names.h" - -void Joint::_disconnect_signals() { - Node *node_a = get_node_or_null(a); - PhysicsBody *body_a = Object::cast_to(node_a); - if (body_a) { - body_a->disconnect(SceneStringNames::get_singleton()->tree_exiting, this, SceneStringNames::get_singleton()->_body_exit_tree); - } - - Node *node_b = get_node_or_null(b); - PhysicsBody *body_b = Object::cast_to(node_b); - if (body_b) { - body_b->disconnect(SceneStringNames::get_singleton()->tree_exiting, this, SceneStringNames::get_singleton()->_body_exit_tree); - } -} - -void Joint::_body_exit_tree() { - _disconnect_signals(); - _update_joint(true); -} - -void Joint::_update_joint(bool p_only_free) { - if (joint.is_valid()) { - if (ba.is_valid() && bb.is_valid()) { - PhysicsServer::get_singleton()->body_remove_collision_exception(ba, bb); - PhysicsServer::get_singleton()->body_remove_collision_exception(bb, ba); - } - - PhysicsServer::get_singleton()->free(joint); - joint = RID(); - ba = RID(); - bb = RID(); - } - - if (p_only_free || !is_inside_tree()) { - warning = String(); - return; - } - - Node *node_a = get_node_or_null(a); - Node *node_b = get_node_or_null(b); - - PhysicsBody *body_a = Object::cast_to(node_a); - PhysicsBody *body_b = Object::cast_to(node_b); - - if (node_a && !body_a && node_b && !body_b) { - warning = TTR("Node A and Node B must be PhysicsBodies"); - update_configuration_warning(); - return; - } - - if (node_a && !body_a) { - warning = TTR("Node A must be a PhysicsBody"); - update_configuration_warning(); - return; - } - - if (node_b && !body_b) { - warning = TTR("Node B must be a PhysicsBody"); - update_configuration_warning(); - return; - } - - if (!body_a && !body_b) { - warning = TTR("Joint is not connected to any PhysicsBodies"); - update_configuration_warning(); - return; - } - - if (body_a == body_b) { - warning = TTR("Node A and Node B must be different PhysicsBodies"); - update_configuration_warning(); - return; - } - - warning = String(); - update_configuration_warning(); - - if (body_a) { - joint = _configure_joint(body_a, body_b); - } else if (body_b) { - joint = _configure_joint(body_b, nullptr); - } - - ERR_FAIL_COND_MSG(!joint.is_valid(), "Failed to configure the joint."); - - PhysicsServer::get_singleton()->joint_set_solver_priority(joint, solver_priority); - - if (body_a) { - ba = body_a->get_rid(); - body_a->connect(SceneStringNames::get_singleton()->tree_exiting, this, SceneStringNames::get_singleton()->_body_exit_tree); - } - - if (body_b) { - bb = body_b->get_rid(); - body_b->connect(SceneStringNames::get_singleton()->tree_exiting, this, SceneStringNames::get_singleton()->_body_exit_tree); - } - - PhysicsServer::get_singleton()->joint_disable_collisions_between_bodies(joint, exclude_from_collision); -} - -void Joint::set_node_a(const NodePath &p_node_a) { - if (a == p_node_a) { - return; - } - - if (joint.is_valid()) { - _disconnect_signals(); - } - - a = p_node_a; - _update_joint(); -} - -NodePath Joint::get_node_a() const { - return a; -} - -void Joint::set_node_b(const NodePath &p_node_b) { - if (b == p_node_b) { - return; - } - - if (joint.is_valid()) { - _disconnect_signals(); - } - - b = p_node_b; - _update_joint(); -} - -NodePath Joint::get_node_b() const { - return b; -} - -void Joint::set_solver_priority(int p_priority) { - solver_priority = p_priority; - if (joint.is_valid()) { - PhysicsServer::get_singleton()->joint_set_solver_priority(joint, solver_priority); - } -} - -int Joint::get_solver_priority() const { - return solver_priority; -} - -void Joint::_notification(int p_what) { - switch (p_what) { - case NOTIFICATION_POST_ENTER_TREE: { - if (joint.is_valid()) { - _disconnect_signals(); - } - _update_joint(); - } break; - case NOTIFICATION_EXIT_TREE: { - if (joint.is_valid()) { - _disconnect_signals(); - } - _update_joint(true); - } break; - } -} - -void Joint::set_exclude_nodes_from_collision(bool p_enable) { - if (exclude_from_collision == p_enable) { - return; - } - if (joint.is_valid()) { - _disconnect_signals(); - } - _update_joint(true); - exclude_from_collision = p_enable; - _update_joint(); -} - -bool Joint::get_exclude_nodes_from_collision() const { - return exclude_from_collision; -} - -String Joint::get_configuration_warning() const { - String node_warning = Node::get_configuration_warning(); - - if (!warning.empty()) { - if (!node_warning.empty()) { - node_warning += "\n\n"; - } - node_warning += warning; - } - - return node_warning; -} - -void Joint::_bind_methods() { - ClassDB::bind_method(D_METHOD("_body_exit_tree"), &Joint::_body_exit_tree); - - ClassDB::bind_method(D_METHOD("set_node_a", "node"), &Joint::set_node_a); - ClassDB::bind_method(D_METHOD("get_node_a"), &Joint::get_node_a); - - ClassDB::bind_method(D_METHOD("set_node_b", "node"), &Joint::set_node_b); - ClassDB::bind_method(D_METHOD("get_node_b"), &Joint::get_node_b); - - ClassDB::bind_method(D_METHOD("set_solver_priority", "priority"), &Joint::set_solver_priority); - ClassDB::bind_method(D_METHOD("get_solver_priority"), &Joint::get_solver_priority); - - ClassDB::bind_method(D_METHOD("set_exclude_nodes_from_collision", "enable"), &Joint::set_exclude_nodes_from_collision); - ClassDB::bind_method(D_METHOD("get_exclude_nodes_from_collision"), &Joint::get_exclude_nodes_from_collision); - - ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "nodes/node_a", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "PhysicsBody"), "set_node_a", "get_node_a"); - ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "nodes/node_b", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "PhysicsBody"), "set_node_b", "get_node_b"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "solver/priority", PROPERTY_HINT_RANGE, "1,8,1"), "set_solver_priority", "get_solver_priority"); - - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "collision/exclude_nodes"), "set_exclude_nodes_from_collision", "get_exclude_nodes_from_collision"); -} - -Joint::Joint() { - exclude_from_collision = true; - solver_priority = 1; - set_notify_transform(true); -} - -/////////////////////////////////// - -void PinJoint::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_param", "param", "value"), &PinJoint::set_param); - ClassDB::bind_method(D_METHOD("get_param", "param"), &PinJoint::get_param); - - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "params/bias", PROPERTY_HINT_RANGE, "0.01,0.99,0.01"), "set_param", "get_param", PARAM_BIAS); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "params/damping", PROPERTY_HINT_RANGE, "0.01,8.0,0.01"), "set_param", "get_param", PARAM_DAMPING); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "params/impulse_clamp", PROPERTY_HINT_RANGE, "0.0,64.0,0.01"), "set_param", "get_param", PARAM_IMPULSE_CLAMP); - - BIND_ENUM_CONSTANT(PARAM_BIAS); - BIND_ENUM_CONSTANT(PARAM_DAMPING); - BIND_ENUM_CONSTANT(PARAM_IMPULSE_CLAMP); -} - -void PinJoint::set_param(Param p_param, float p_value) { - ERR_FAIL_INDEX(p_param, 3); - params[p_param] = p_value; - if (get_joint().is_valid()) { - PhysicsServer::get_singleton()->pin_joint_set_param(get_joint(), PhysicsServer::PinJointParam(p_param), p_value); - } -} -float PinJoint::get_param(Param p_param) const { - ERR_FAIL_INDEX_V(p_param, 3, 0); - return params[p_param]; -} - -RID PinJoint::_configure_joint(PhysicsBody *body_a, PhysicsBody *body_b) { - Vector3 pinpos = get_global_transform().origin; - Vector3 local_a = body_a->get_global_transform().affine_inverse().xform(pinpos); - Vector3 local_b; - - if (body_b) { - local_b = body_b->get_global_transform().affine_inverse().xform(pinpos); - } else { - local_b = pinpos; - } - - RID j = PhysicsServer::get_singleton()->joint_create_pin(body_a->get_rid(), local_a, body_b ? body_b->get_rid() : RID(), local_b); - for (int i = 0; i < 3; i++) { - PhysicsServer::get_singleton()->pin_joint_set_param(j, PhysicsServer::PinJointParam(i), params[i]); - } - return j; -} - -PinJoint::PinJoint() { - params[PARAM_BIAS] = 0.3; - params[PARAM_DAMPING] = 1; - params[PARAM_IMPULSE_CLAMP] = 0; -} - -///////////////////////////////////////////////// - -/////////////////////////////////// - -void HingeJoint::_set_upper_limit(float p_limit) { - set_param(PARAM_LIMIT_UPPER, Math::deg2rad(p_limit)); -} - -float HingeJoint::_get_upper_limit() const { - return Math::rad2deg(get_param(PARAM_LIMIT_UPPER)); -} - -void HingeJoint::_set_lower_limit(float p_limit) { - set_param(PARAM_LIMIT_LOWER, Math::deg2rad(p_limit)); -} - -float HingeJoint::_get_lower_limit() const { - return Math::rad2deg(get_param(PARAM_LIMIT_LOWER)); -} - -void HingeJoint::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_param", "param", "value"), &HingeJoint::set_param); - ClassDB::bind_method(D_METHOD("get_param", "param"), &HingeJoint::get_param); - - ClassDB::bind_method(D_METHOD("set_flag", "flag", "enabled"), &HingeJoint::set_flag); - ClassDB::bind_method(D_METHOD("get_flag", "flag"), &HingeJoint::get_flag); - - ClassDB::bind_method(D_METHOD("_set_upper_limit", "upper_limit"), &HingeJoint::_set_upper_limit); - ClassDB::bind_method(D_METHOD("_get_upper_limit"), &HingeJoint::_get_upper_limit); - - ClassDB::bind_method(D_METHOD("_set_lower_limit", "lower_limit"), &HingeJoint::_set_lower_limit); - ClassDB::bind_method(D_METHOD("_get_lower_limit"), &HingeJoint::_get_lower_limit); - - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "params/bias", PROPERTY_HINT_RANGE, "0.00,0.99,0.01"), "set_param", "get_param", PARAM_BIAS); - - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "angular_limit/enable"), "set_flag", "get_flag", FLAG_USE_LIMIT); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "angular_limit/upper", PROPERTY_HINT_RANGE, "-180,180,0.1"), "_set_upper_limit", "_get_upper_limit"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "angular_limit/lower", PROPERTY_HINT_RANGE, "-180,180,0.1"), "_set_lower_limit", "_get_lower_limit"); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_limit/bias", PROPERTY_HINT_RANGE, "0.01,0.99,0.01"), "set_param", "get_param", PARAM_LIMIT_BIAS); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_limit/softness", PROPERTY_HINT_RANGE, "0.01,16,0.01"), "set_param", "get_param", PARAM_LIMIT_SOFTNESS); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_limit/relaxation", PROPERTY_HINT_RANGE, "0.01,16,0.01"), "set_param", "get_param", PARAM_LIMIT_RELAXATION); - - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "motor/enable"), "set_flag", "get_flag", FLAG_ENABLE_MOTOR); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "motor/target_velocity", PROPERTY_HINT_RANGE, "-200,200,0.01,or_greater,or_lesser"), "set_param", "get_param", PARAM_MOTOR_TARGET_VELOCITY); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "motor/max_impulse", PROPERTY_HINT_RANGE, "0.01,1024,0.01"), "set_param", "get_param", PARAM_MOTOR_MAX_IMPULSE); - - BIND_ENUM_CONSTANT(PARAM_BIAS); - BIND_ENUM_CONSTANT(PARAM_LIMIT_UPPER); - BIND_ENUM_CONSTANT(PARAM_LIMIT_LOWER); - BIND_ENUM_CONSTANT(PARAM_LIMIT_BIAS); - BIND_ENUM_CONSTANT(PARAM_LIMIT_SOFTNESS); - BIND_ENUM_CONSTANT(PARAM_LIMIT_RELAXATION); - BIND_ENUM_CONSTANT(PARAM_MOTOR_TARGET_VELOCITY); - BIND_ENUM_CONSTANT(PARAM_MOTOR_MAX_IMPULSE); - BIND_ENUM_CONSTANT(PARAM_MAX); - - BIND_ENUM_CONSTANT(FLAG_USE_LIMIT); - BIND_ENUM_CONSTANT(FLAG_ENABLE_MOTOR); - BIND_ENUM_CONSTANT(FLAG_MAX); -} - -void HingeJoint::set_param(Param p_param, float p_value) { - ERR_FAIL_INDEX(p_param, PARAM_MAX); - params[p_param] = p_value; - if (get_joint().is_valid()) { - PhysicsServer::get_singleton()->hinge_joint_set_param(get_joint(), PhysicsServer::HingeJointParam(p_param), p_value); - } - - update_gizmos(); -} -float HingeJoint::get_param(Param p_param) const { - ERR_FAIL_INDEX_V(p_param, PARAM_MAX, 0); - return params[p_param]; -} - -void HingeJoint::set_flag(Flag p_flag, bool p_value) { - ERR_FAIL_INDEX(p_flag, FLAG_MAX); - flags[p_flag] = p_value; - if (get_joint().is_valid()) { - PhysicsServer::get_singleton()->hinge_joint_set_flag(get_joint(), PhysicsServer::HingeJointFlag(p_flag), p_value); - } - - update_gizmos(); -} -bool HingeJoint::get_flag(Flag p_flag) const { - ERR_FAIL_INDEX_V(p_flag, FLAG_MAX, false); - return flags[p_flag]; -} - -RID HingeJoint::_configure_joint(PhysicsBody *body_a, PhysicsBody *body_b) { - Transform gt = get_global_transform(); - Transform ainv = body_a->get_global_transform().affine_inverse(); - - Transform local_a = ainv * gt; - local_a.orthonormalize(); - Transform local_b = gt; - - if (body_b) { - Transform binv = body_b->get_global_transform().affine_inverse(); - local_b = binv * gt; - } - - local_b.orthonormalize(); - - RID j = RID_PRIME(PhysicsServer::get_singleton()->joint_create_hinge(body_a->get_rid(), local_a, body_b ? body_b->get_rid() : RID(), local_b)); - for (int i = 0; i < PARAM_MAX; i++) { - PhysicsServer::get_singleton()->hinge_joint_set_param(j, PhysicsServer::HingeJointParam(i), params[i]); - } - for (int i = 0; i < FLAG_MAX; i++) { - set_flag(Flag(i), flags[i]); - PhysicsServer::get_singleton()->hinge_joint_set_flag(j, PhysicsServer::HingeJointFlag(i), flags[i]); - } - return j; -} - -HingeJoint::HingeJoint() { - params[PARAM_BIAS] = 0.3; - params[PARAM_LIMIT_UPPER] = Math_PI * 0.5; - params[PARAM_LIMIT_LOWER] = -Math_PI * 0.5; - params[PARAM_LIMIT_BIAS] = 0.3; - params[PARAM_LIMIT_SOFTNESS] = 0.9; - params[PARAM_LIMIT_RELAXATION] = 1.0; - params[PARAM_MOTOR_TARGET_VELOCITY] = 1; - params[PARAM_MOTOR_MAX_IMPULSE] = 1; - - flags[FLAG_USE_LIMIT] = false; - flags[FLAG_ENABLE_MOTOR] = false; -} - -///////////////////////////////////////////////// - -////////////////////////////////// - -void SliderJoint::_set_upper_limit_angular(float p_limit_angular) { - set_param(PARAM_ANGULAR_LIMIT_UPPER, Math::deg2rad(p_limit_angular)); -} - -float SliderJoint::_get_upper_limit_angular() const { - return Math::rad2deg(get_param(PARAM_ANGULAR_LIMIT_UPPER)); -} - -void SliderJoint::_set_lower_limit_angular(float p_limit_angular) { - set_param(PARAM_ANGULAR_LIMIT_LOWER, Math::deg2rad(p_limit_angular)); -} - -float SliderJoint::_get_lower_limit_angular() const { - return Math::rad2deg(get_param(PARAM_ANGULAR_LIMIT_LOWER)); -} - -void SliderJoint::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_param", "param", "value"), &SliderJoint::set_param); - ClassDB::bind_method(D_METHOD("get_param", "param"), &SliderJoint::get_param); - - ClassDB::bind_method(D_METHOD("_set_upper_limit_angular", "upper_limit_angular"), &SliderJoint::_set_upper_limit_angular); - ClassDB::bind_method(D_METHOD("_get_upper_limit_angular"), &SliderJoint::_get_upper_limit_angular); - - ClassDB::bind_method(D_METHOD("_set_lower_limit_angular", "lower_limit_angular"), &SliderJoint::_set_lower_limit_angular); - ClassDB::bind_method(D_METHOD("_get_lower_limit_angular"), &SliderJoint::_get_lower_limit_angular); - - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_limit/upper_distance", PROPERTY_HINT_RANGE, "-1024,1024,0.01"), "set_param", "get_param", PARAM_LINEAR_LIMIT_UPPER); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_limit/lower_distance", PROPERTY_HINT_RANGE, "-1024,1024,0.01"), "set_param", "get_param", PARAM_LINEAR_LIMIT_LOWER); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_limit/softness", PROPERTY_HINT_RANGE, "0.01,16.0,0.01"), "set_param", "get_param", PARAM_LINEAR_LIMIT_SOFTNESS); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_limit/restitution", PROPERTY_HINT_RANGE, "0.01,16.0,0.01"), "set_param", "get_param", PARAM_LINEAR_LIMIT_RESTITUTION); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_limit/damping", PROPERTY_HINT_RANGE, "0,16.0,0.01"), "set_param", "get_param", PARAM_LINEAR_LIMIT_DAMPING); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_motion/softness", PROPERTY_HINT_RANGE, "0.01,16.0,0.01"), "set_param", "get_param", PARAM_LINEAR_MOTION_SOFTNESS); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_motion/restitution", PROPERTY_HINT_RANGE, "0.01,16.0,0.01"), "set_param", "get_param", PARAM_LINEAR_MOTION_RESTITUTION); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_motion/damping", PROPERTY_HINT_RANGE, "0,16.0,0.01"), "set_param", "get_param", PARAM_LINEAR_MOTION_DAMPING); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_ortho/softness", PROPERTY_HINT_RANGE, "0.01,16.0,0.01"), "set_param", "get_param", PARAM_LINEAR_ORTHOGONAL_SOFTNESS); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_ortho/restitution", PROPERTY_HINT_RANGE, "0.01,16.0,0.01"), "set_param", "get_param", PARAM_LINEAR_ORTHOGONAL_RESTITUTION); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_ortho/damping", PROPERTY_HINT_RANGE, "0,16.0,0.01"), "set_param", "get_param", PARAM_LINEAR_ORTHOGONAL_DAMPING); - - ADD_PROPERTY(PropertyInfo(Variant::REAL, "angular_limit/upper_angle", PROPERTY_HINT_RANGE, "-180,180,0.1"), "_set_upper_limit_angular", "_get_upper_limit_angular"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "angular_limit/lower_angle", PROPERTY_HINT_RANGE, "-180,180,0.1"), "_set_lower_limit_angular", "_get_lower_limit_angular"); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_limit/softness", PROPERTY_HINT_RANGE, "0.01,16.0,0.01"), "set_param", "get_param", PARAM_ANGULAR_LIMIT_SOFTNESS); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_limit/restitution", PROPERTY_HINT_RANGE, "0.01,16.0,0.01"), "set_param", "get_param", PARAM_ANGULAR_LIMIT_RESTITUTION); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_limit/damping", PROPERTY_HINT_RANGE, "0,16.0,0.01"), "set_param", "get_param", PARAM_ANGULAR_LIMIT_DAMPING); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_motion/softness", PROPERTY_HINT_RANGE, "0.01,16.0,0.01"), "set_param", "get_param", PARAM_ANGULAR_MOTION_SOFTNESS); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_motion/restitution", PROPERTY_HINT_RANGE, "0.01,16.0,0.01"), "set_param", "get_param", PARAM_ANGULAR_MOTION_RESTITUTION); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_motion/damping", PROPERTY_HINT_RANGE, "0,16.0,0.01"), "set_param", "get_param", PARAM_ANGULAR_MOTION_DAMPING); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_ortho/softness", PROPERTY_HINT_RANGE, "0.01,16.0,0.01"), "set_param", "get_param", PARAM_ANGULAR_ORTHOGONAL_SOFTNESS); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_ortho/restitution", PROPERTY_HINT_RANGE, "0.01,16.0,0.01"), "set_param", "get_param", PARAM_ANGULAR_ORTHOGONAL_RESTITUTION); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_ortho/damping", PROPERTY_HINT_RANGE, "0,16.0,0.01"), "set_param", "get_param", PARAM_ANGULAR_ORTHOGONAL_DAMPING); - - BIND_ENUM_CONSTANT(PARAM_LINEAR_LIMIT_UPPER); - BIND_ENUM_CONSTANT(PARAM_LINEAR_LIMIT_LOWER); - BIND_ENUM_CONSTANT(PARAM_LINEAR_LIMIT_SOFTNESS); - BIND_ENUM_CONSTANT(PARAM_LINEAR_LIMIT_RESTITUTION); - BIND_ENUM_CONSTANT(PARAM_LINEAR_LIMIT_DAMPING); - BIND_ENUM_CONSTANT(PARAM_LINEAR_MOTION_SOFTNESS); - BIND_ENUM_CONSTANT(PARAM_LINEAR_MOTION_RESTITUTION); - BIND_ENUM_CONSTANT(PARAM_LINEAR_MOTION_DAMPING); - BIND_ENUM_CONSTANT(PARAM_LINEAR_ORTHOGONAL_SOFTNESS); - BIND_ENUM_CONSTANT(PARAM_LINEAR_ORTHOGONAL_RESTITUTION); - BIND_ENUM_CONSTANT(PARAM_LINEAR_ORTHOGONAL_DAMPING); - - BIND_ENUM_CONSTANT(PARAM_ANGULAR_LIMIT_UPPER); - BIND_ENUM_CONSTANT(PARAM_ANGULAR_LIMIT_LOWER); - BIND_ENUM_CONSTANT(PARAM_ANGULAR_LIMIT_SOFTNESS); - BIND_ENUM_CONSTANT(PARAM_ANGULAR_LIMIT_RESTITUTION); - BIND_ENUM_CONSTANT(PARAM_ANGULAR_LIMIT_DAMPING); - BIND_ENUM_CONSTANT(PARAM_ANGULAR_MOTION_SOFTNESS); - BIND_ENUM_CONSTANT(PARAM_ANGULAR_MOTION_RESTITUTION); - BIND_ENUM_CONSTANT(PARAM_ANGULAR_MOTION_DAMPING); - BIND_ENUM_CONSTANT(PARAM_ANGULAR_ORTHOGONAL_SOFTNESS); - BIND_ENUM_CONSTANT(PARAM_ANGULAR_ORTHOGONAL_RESTITUTION); - BIND_ENUM_CONSTANT(PARAM_ANGULAR_ORTHOGONAL_DAMPING); - - BIND_ENUM_CONSTANT(PARAM_MAX); -} - -void SliderJoint::set_param(Param p_param, float p_value) { - ERR_FAIL_INDEX(p_param, PARAM_MAX); - params[p_param] = p_value; - if (get_joint().is_valid()) { - PhysicsServer::get_singleton()->slider_joint_set_param(get_joint(), PhysicsServer::SliderJointParam(p_param), p_value); - } - update_gizmos(); -} -float SliderJoint::get_param(Param p_param) const { - ERR_FAIL_INDEX_V(p_param, PARAM_MAX, 0); - return params[p_param]; -} - -RID SliderJoint::_configure_joint(PhysicsBody *body_a, PhysicsBody *body_b) { - Transform gt = get_global_transform(); - Transform ainv = body_a->get_global_transform().affine_inverse(); - - Transform local_a = ainv * gt; - local_a.orthonormalize(); - Transform local_b = gt; - - if (body_b) { - Transform binv = body_b->get_global_transform().affine_inverse(); - local_b = binv * gt; - } - - local_b.orthonormalize(); - - RID j = PhysicsServer::get_singleton()->joint_create_slider(body_a->get_rid(), local_a, body_b ? body_b->get_rid() : RID(), local_b); - for (int i = 0; i < PARAM_MAX; i++) { - PhysicsServer::get_singleton()->slider_joint_set_param(j, PhysicsServer::SliderJointParam(i), params[i]); - } - - return j; -} - -SliderJoint::SliderJoint() { - params[PARAM_LINEAR_LIMIT_UPPER] = 1.0; - params[PARAM_LINEAR_LIMIT_LOWER] = -1.0; - params[PARAM_LINEAR_LIMIT_SOFTNESS] = 1.0; - params[PARAM_LINEAR_LIMIT_RESTITUTION] = 0.7; - params[PARAM_LINEAR_LIMIT_DAMPING] = 1.0; - params[PARAM_LINEAR_MOTION_SOFTNESS] = 1.0; - params[PARAM_LINEAR_MOTION_RESTITUTION] = 0.7; - params[PARAM_LINEAR_MOTION_DAMPING] = 0; //1.0; - params[PARAM_LINEAR_ORTHOGONAL_SOFTNESS] = 1.0; - params[PARAM_LINEAR_ORTHOGONAL_RESTITUTION] = 0.7; - params[PARAM_LINEAR_ORTHOGONAL_DAMPING] = 1.0; - - params[PARAM_ANGULAR_LIMIT_UPPER] = 0; - params[PARAM_ANGULAR_LIMIT_LOWER] = 0; - params[PARAM_ANGULAR_LIMIT_SOFTNESS] = 1.0; - params[PARAM_ANGULAR_LIMIT_RESTITUTION] = 0.7; - params[PARAM_ANGULAR_LIMIT_DAMPING] = 0; //1.0; - params[PARAM_ANGULAR_MOTION_SOFTNESS] = 1.0; - params[PARAM_ANGULAR_MOTION_RESTITUTION] = 0.7; - params[PARAM_ANGULAR_MOTION_DAMPING] = 1.0; - params[PARAM_ANGULAR_ORTHOGONAL_SOFTNESS] = 1.0; - params[PARAM_ANGULAR_ORTHOGONAL_RESTITUTION] = 0.7; - params[PARAM_ANGULAR_ORTHOGONAL_DAMPING] = 1.0; -} - -////////////////////////////////// - -void ConeTwistJoint::_set_swing_span(float p_limit_angular) { - set_param(PARAM_SWING_SPAN, Math::deg2rad(p_limit_angular)); -} - -float ConeTwistJoint::_get_swing_span() const { - return Math::rad2deg(get_param(PARAM_SWING_SPAN)); -} - -void ConeTwistJoint::_set_twist_span(float p_limit_angular) { - set_param(PARAM_TWIST_SPAN, Math::deg2rad(p_limit_angular)); -} - -float ConeTwistJoint::_get_twist_span() const { - return Math::rad2deg(get_param(PARAM_TWIST_SPAN)); -} - -void ConeTwistJoint::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_param", "param", "value"), &ConeTwistJoint::set_param); - ClassDB::bind_method(D_METHOD("get_param", "param"), &ConeTwistJoint::get_param); - - ClassDB::bind_method(D_METHOD("_set_swing_span", "swing_span"), &ConeTwistJoint::_set_swing_span); - ClassDB::bind_method(D_METHOD("_get_swing_span"), &ConeTwistJoint::_get_swing_span); - - ClassDB::bind_method(D_METHOD("_set_twist_span", "twist_span"), &ConeTwistJoint::_set_twist_span); - ClassDB::bind_method(D_METHOD("_get_twist_span"), &ConeTwistJoint::_get_twist_span); - - ADD_PROPERTY(PropertyInfo(Variant::REAL, "swing_span", PROPERTY_HINT_RANGE, "-180,180,0.1"), "_set_swing_span", "_get_swing_span"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "twist_span", PROPERTY_HINT_RANGE, "-40000,40000,0.1"), "_set_twist_span", "_get_twist_span"); - - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "bias", PROPERTY_HINT_RANGE, "0.01,16.0,0.01"), "set_param", "get_param", PARAM_BIAS); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "softness", PROPERTY_HINT_RANGE, "0.01,16.0,0.01"), "set_param", "get_param", PARAM_SOFTNESS); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "relaxation", PROPERTY_HINT_RANGE, "0.01,16.0,0.01"), "set_param", "get_param", PARAM_RELAXATION); - - BIND_ENUM_CONSTANT(PARAM_SWING_SPAN); - BIND_ENUM_CONSTANT(PARAM_TWIST_SPAN); - BIND_ENUM_CONSTANT(PARAM_BIAS); - BIND_ENUM_CONSTANT(PARAM_SOFTNESS); - BIND_ENUM_CONSTANT(PARAM_RELAXATION); - BIND_ENUM_CONSTANT(PARAM_MAX); -} - -void ConeTwistJoint::set_param(Param p_param, float p_value) { - ERR_FAIL_INDEX(p_param, PARAM_MAX); - params[p_param] = p_value; - if (get_joint().is_valid()) { - PhysicsServer::get_singleton()->cone_twist_joint_set_param(get_joint(), PhysicsServer::ConeTwistJointParam(p_param), p_value); - } - - update_gizmos(); -} -float ConeTwistJoint::get_param(Param p_param) const { - ERR_FAIL_INDEX_V(p_param, PARAM_MAX, 0); - return params[p_param]; -} - -RID ConeTwistJoint::_configure_joint(PhysicsBody *body_a, PhysicsBody *body_b) { - Transform gt = get_global_transform(); - //Vector3 cone_twistpos = gt.origin; - //Vector3 cone_twistdir = gt.basis.get_axis(2); - - Transform ainv = body_a->get_global_transform().affine_inverse(); - - Transform local_a = ainv * gt; - local_a.orthonormalize(); - Transform local_b = gt; - - if (body_b) { - Transform binv = body_b->get_global_transform().affine_inverse(); - local_b = binv * gt; - } - - local_b.orthonormalize(); - - RID j = PhysicsServer::get_singleton()->joint_create_cone_twist(body_a->get_rid(), local_a, body_b ? body_b->get_rid() : RID(), local_b); - for (int i = 0; i < PARAM_MAX; i++) { - PhysicsServer::get_singleton()->cone_twist_joint_set_param(j, PhysicsServer::ConeTwistJointParam(i), params[i]); - } - - return j; -} - -ConeTwistJoint::ConeTwistJoint() { - params[PARAM_SWING_SPAN] = Math_PI * 0.25; - params[PARAM_TWIST_SPAN] = Math_PI; - params[PARAM_BIAS] = 0.3; - params[PARAM_SOFTNESS] = 0.8; - params[PARAM_RELAXATION] = 1.0; -} - -///////////////////////////////////////////////////////////////////// - -void Generic6DOFJoint::_set_angular_hi_limit_x(float p_limit_angular) { - set_param_x(PARAM_ANGULAR_UPPER_LIMIT, Math::deg2rad(p_limit_angular)); -} - -float Generic6DOFJoint::_get_angular_hi_limit_x() const { - return Math::rad2deg(get_param_x(PARAM_ANGULAR_UPPER_LIMIT)); -} - -void Generic6DOFJoint::_set_angular_lo_limit_x(float p_limit_angular) { - set_param_x(PARAM_ANGULAR_LOWER_LIMIT, Math::deg2rad(p_limit_angular)); -} - -float Generic6DOFJoint::_get_angular_lo_limit_x() const { - return Math::rad2deg(get_param_x(PARAM_ANGULAR_LOWER_LIMIT)); -} - -void Generic6DOFJoint::_set_angular_hi_limit_y(float p_limit_angular) { - set_param_y(PARAM_ANGULAR_UPPER_LIMIT, Math::deg2rad(p_limit_angular)); -} - -float Generic6DOFJoint::_get_angular_hi_limit_y() const { - return Math::rad2deg(get_param_y(PARAM_ANGULAR_UPPER_LIMIT)); -} - -void Generic6DOFJoint::_set_angular_lo_limit_y(float p_limit_angular) { - set_param_y(PARAM_ANGULAR_LOWER_LIMIT, Math::deg2rad(p_limit_angular)); -} - -float Generic6DOFJoint::_get_angular_lo_limit_y() const { - return Math::rad2deg(get_param_y(PARAM_ANGULAR_LOWER_LIMIT)); -} - -void Generic6DOFJoint::_set_angular_hi_limit_z(float p_limit_angular) { - set_param_z(PARAM_ANGULAR_UPPER_LIMIT, Math::deg2rad(p_limit_angular)); -} - -float Generic6DOFJoint::_get_angular_hi_limit_z() const { - return Math::rad2deg(get_param_z(PARAM_ANGULAR_UPPER_LIMIT)); -} - -void Generic6DOFJoint::_set_angular_lo_limit_z(float p_limit_angular) { - set_param_z(PARAM_ANGULAR_LOWER_LIMIT, Math::deg2rad(p_limit_angular)); -} - -float Generic6DOFJoint::_get_angular_lo_limit_z() const { - return Math::rad2deg(get_param_z(PARAM_ANGULAR_LOWER_LIMIT)); -} - -void Generic6DOFJoint::_bind_methods() { - ClassDB::bind_method(D_METHOD("_set_angular_hi_limit_x", "angle"), &Generic6DOFJoint::_set_angular_hi_limit_x); - ClassDB::bind_method(D_METHOD("_get_angular_hi_limit_x"), &Generic6DOFJoint::_get_angular_hi_limit_x); - - ClassDB::bind_method(D_METHOD("_set_angular_lo_limit_x", "angle"), &Generic6DOFJoint::_set_angular_lo_limit_x); - ClassDB::bind_method(D_METHOD("_get_angular_lo_limit_x"), &Generic6DOFJoint::_get_angular_lo_limit_x); - - ClassDB::bind_method(D_METHOD("_set_angular_hi_limit_y", "angle"), &Generic6DOFJoint::_set_angular_hi_limit_y); - ClassDB::bind_method(D_METHOD("_get_angular_hi_limit_y"), &Generic6DOFJoint::_get_angular_hi_limit_y); - - ClassDB::bind_method(D_METHOD("_set_angular_lo_limit_y", "angle"), &Generic6DOFJoint::_set_angular_lo_limit_y); - ClassDB::bind_method(D_METHOD("_get_angular_lo_limit_y"), &Generic6DOFJoint::_get_angular_lo_limit_y); - - ClassDB::bind_method(D_METHOD("_set_angular_hi_limit_z", "angle"), &Generic6DOFJoint::_set_angular_hi_limit_z); - ClassDB::bind_method(D_METHOD("_get_angular_hi_limit_z"), &Generic6DOFJoint::_get_angular_hi_limit_z); - - ClassDB::bind_method(D_METHOD("_set_angular_lo_limit_z", "angle"), &Generic6DOFJoint::_set_angular_lo_limit_z); - ClassDB::bind_method(D_METHOD("_get_angular_lo_limit_z"), &Generic6DOFJoint::_get_angular_lo_limit_z); - - ClassDB::bind_method(D_METHOD("set_param_x", "param", "value"), &Generic6DOFJoint::set_param_x); - ClassDB::bind_method(D_METHOD("get_param_x", "param"), &Generic6DOFJoint::get_param_x); - - ClassDB::bind_method(D_METHOD("set_param_y", "param", "value"), &Generic6DOFJoint::set_param_y); - ClassDB::bind_method(D_METHOD("get_param_y", "param"), &Generic6DOFJoint::get_param_y); - - ClassDB::bind_method(D_METHOD("set_param_z", "param", "value"), &Generic6DOFJoint::set_param_z); - ClassDB::bind_method(D_METHOD("get_param_z", "param"), &Generic6DOFJoint::get_param_z); - - ClassDB::bind_method(D_METHOD("set_flag_x", "flag", "value"), &Generic6DOFJoint::set_flag_x); - ClassDB::bind_method(D_METHOD("get_flag_x", "flag"), &Generic6DOFJoint::get_flag_x); - - ClassDB::bind_method(D_METHOD("set_flag_y", "flag", "value"), &Generic6DOFJoint::set_flag_y); - ClassDB::bind_method(D_METHOD("get_flag_y", "flag"), &Generic6DOFJoint::get_flag_y); - - ClassDB::bind_method(D_METHOD("set_flag_z", "flag", "value"), &Generic6DOFJoint::set_flag_z); - ClassDB::bind_method(D_METHOD("get_flag_z", "flag"), &Generic6DOFJoint::get_flag_z); - - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "linear_limit_x/enabled"), "set_flag_x", "get_flag_x", FLAG_ENABLE_LINEAR_LIMIT); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_limit_x/upper_distance"), "set_param_x", "get_param_x", PARAM_LINEAR_UPPER_LIMIT); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_limit_x/lower_distance"), "set_param_x", "get_param_x", PARAM_LINEAR_LOWER_LIMIT); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_limit_x/softness", PROPERTY_HINT_RANGE, "0.01,16,0.01"), "set_param_x", "get_param_x", PARAM_LINEAR_LIMIT_SOFTNESS); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_limit_x/restitution", PROPERTY_HINT_RANGE, "0.01,16,0.01"), "set_param_x", "get_param_x", PARAM_LINEAR_RESTITUTION); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_limit_x/damping", PROPERTY_HINT_RANGE, "0.01,16,0.01"), "set_param_x", "get_param_x", PARAM_LINEAR_DAMPING); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "linear_motor_x/enabled"), "set_flag_x", "get_flag_x", FLAG_ENABLE_LINEAR_MOTOR); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_motor_x/target_velocity"), "set_param_x", "get_param_x", PARAM_LINEAR_MOTOR_TARGET_VELOCITY); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_motor_x/force_limit"), "set_param_x", "get_param_x", PARAM_LINEAR_MOTOR_FORCE_LIMIT); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "linear_spring_x/enabled"), "set_flag_x", "get_flag_x", FLAG_ENABLE_LINEAR_SPRING); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_spring_x/stiffness"), "set_param_x", "get_param_x", PARAM_LINEAR_SPRING_STIFFNESS); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_spring_x/damping"), "set_param_x", "get_param_x", PARAM_LINEAR_SPRING_DAMPING); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_spring_x/equilibrium_point"), "set_param_x", "get_param_x", PARAM_LINEAR_SPRING_EQUILIBRIUM_POINT); - - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "angular_limit_x/enabled"), "set_flag_x", "get_flag_x", FLAG_ENABLE_ANGULAR_LIMIT); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "angular_limit_x/upper_angle", PROPERTY_HINT_RANGE, "-180,180,0.01"), "_set_angular_hi_limit_x", "_get_angular_hi_limit_x"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "angular_limit_x/lower_angle", PROPERTY_HINT_RANGE, "-180,180,0.01"), "_set_angular_lo_limit_x", "_get_angular_lo_limit_x"); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_limit_x/softness", PROPERTY_HINT_RANGE, "0.01,16,0.01"), "set_param_x", "get_param_x", PARAM_ANGULAR_LIMIT_SOFTNESS); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_limit_x/restitution", PROPERTY_HINT_RANGE, "0.01,16,0.01"), "set_param_x", "get_param_x", PARAM_ANGULAR_RESTITUTION); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_limit_x/damping", PROPERTY_HINT_RANGE, "0.01,16,0.01"), "set_param_x", "get_param_x", PARAM_ANGULAR_DAMPING); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_limit_x/force_limit"), "set_param_x", "get_param_x", PARAM_ANGULAR_FORCE_LIMIT); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_limit_x/erp"), "set_param_x", "get_param_x", PARAM_ANGULAR_ERP); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "angular_motor_x/enabled"), "set_flag_x", "get_flag_x", FLAG_ENABLE_MOTOR); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_motor_x/target_velocity"), "set_param_x", "get_param_x", PARAM_ANGULAR_MOTOR_TARGET_VELOCITY); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_motor_x/force_limit"), "set_param_x", "get_param_x", PARAM_ANGULAR_MOTOR_FORCE_LIMIT); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "angular_spring_x/enabled"), "set_flag_x", "get_flag_x", FLAG_ENABLE_ANGULAR_SPRING); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_spring_x/stiffness"), "set_param_x", "get_param_x", PARAM_ANGULAR_SPRING_STIFFNESS); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_spring_x/damping"), "set_param_x", "get_param_x", PARAM_ANGULAR_SPRING_DAMPING); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_spring_x/equilibrium_point"), "set_param_x", "get_param_x", PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT); - - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "linear_limit_y/enabled"), "set_flag_y", "get_flag_y", FLAG_ENABLE_LINEAR_LIMIT); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_limit_y/upper_distance"), "set_param_y", "get_param_y", PARAM_LINEAR_UPPER_LIMIT); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_limit_y/lower_distance"), "set_param_y", "get_param_y", PARAM_LINEAR_LOWER_LIMIT); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_limit_y/softness", PROPERTY_HINT_RANGE, "0.01,16,0.01"), "set_param_y", "get_param_y", PARAM_LINEAR_LIMIT_SOFTNESS); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_limit_y/restitution", PROPERTY_HINT_RANGE, "0.01,16,0.01"), "set_param_y", "get_param_y", PARAM_LINEAR_RESTITUTION); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_limit_y/damping", PROPERTY_HINT_RANGE, "0.01,16,0.01"), "set_param_y", "get_param_y", PARAM_LINEAR_DAMPING); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "linear_motor_y/enabled"), "set_flag_y", "get_flag_y", FLAG_ENABLE_LINEAR_MOTOR); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_motor_y/target_velocity"), "set_param_y", "get_param_y", PARAM_LINEAR_MOTOR_TARGET_VELOCITY); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_motor_y/force_limit"), "set_param_y", "get_param_y", PARAM_LINEAR_MOTOR_FORCE_LIMIT); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "linear_spring_y/enabled"), "set_flag_y", "get_flag_y", FLAG_ENABLE_LINEAR_SPRING); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_spring_y/stiffness"), "set_param_y", "get_param_y", PARAM_LINEAR_SPRING_STIFFNESS); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_spring_y/damping"), "set_param_y", "get_param_y", PARAM_LINEAR_SPRING_DAMPING); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_spring_y/equilibrium_point"), "set_param_y", "get_param_y", PARAM_LINEAR_SPRING_EQUILIBRIUM_POINT); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "angular_limit_y/enabled"), "set_flag_y", "get_flag_y", FLAG_ENABLE_ANGULAR_LIMIT); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "angular_limit_y/upper_angle", PROPERTY_HINT_RANGE, "-180,180,0.01"), "_set_angular_hi_limit_y", "_get_angular_hi_limit_y"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "angular_limit_y/lower_angle", PROPERTY_HINT_RANGE, "-180,180,0.01"), "_set_angular_lo_limit_y", "_get_angular_lo_limit_y"); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_limit_y/softness", PROPERTY_HINT_RANGE, "0.01,16,0.01"), "set_param_y", "get_param_y", PARAM_ANGULAR_LIMIT_SOFTNESS); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_limit_y/restitution", PROPERTY_HINT_RANGE, "0.01,16,0.01"), "set_param_y", "get_param_y", PARAM_ANGULAR_RESTITUTION); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_limit_y/damping", PROPERTY_HINT_RANGE, "0.01,16,0.01"), "set_param_y", "get_param_y", PARAM_ANGULAR_DAMPING); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_limit_y/force_limit"), "set_param_y", "get_param_y", PARAM_ANGULAR_FORCE_LIMIT); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_limit_y/erp"), "set_param_y", "get_param_y", PARAM_ANGULAR_ERP); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "angular_motor_y/enabled"), "set_flag_y", "get_flag_y", FLAG_ENABLE_MOTOR); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_motor_y/target_velocity"), "set_param_y", "get_param_y", PARAM_ANGULAR_MOTOR_TARGET_VELOCITY); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_motor_y/force_limit"), "set_param_y", "get_param_y", PARAM_ANGULAR_MOTOR_FORCE_LIMIT); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "angular_spring_y/enabled"), "set_flag_y", "get_flag_y", FLAG_ENABLE_ANGULAR_SPRING); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_spring_y/stiffness"), "set_param_y", "get_param_y", PARAM_ANGULAR_SPRING_STIFFNESS); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_spring_y/damping"), "set_param_y", "get_param_y", PARAM_ANGULAR_SPRING_DAMPING); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_spring_y/equilibrium_point"), "set_param_y", "get_param_y", PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT); - - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "linear_limit_z/enabled"), "set_flag_z", "get_flag_z", FLAG_ENABLE_LINEAR_LIMIT); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_limit_z/upper_distance"), "set_param_z", "get_param_z", PARAM_LINEAR_UPPER_LIMIT); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_limit_z/lower_distance"), "set_param_z", "get_param_z", PARAM_LINEAR_LOWER_LIMIT); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_limit_z/softness", PROPERTY_HINT_RANGE, "0.01,16,0.01"), "set_param_z", "get_param_z", PARAM_LINEAR_LIMIT_SOFTNESS); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_limit_z/restitution", PROPERTY_HINT_RANGE, "0.01,16,0.01"), "set_param_z", "get_param_z", PARAM_LINEAR_RESTITUTION); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_limit_z/damping", PROPERTY_HINT_RANGE, "0.01,16,0.01"), "set_param_z", "get_param_z", PARAM_LINEAR_DAMPING); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "linear_motor_z/enabled"), "set_flag_z", "get_flag_z", FLAG_ENABLE_LINEAR_MOTOR); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_motor_z/target_velocity"), "set_param_z", "get_param_z", PARAM_LINEAR_MOTOR_TARGET_VELOCITY); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_motor_z/force_limit"), "set_param_z", "get_param_z", PARAM_LINEAR_MOTOR_FORCE_LIMIT); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "linear_spring_z/enabled"), "set_flag_z", "get_flag_z", FLAG_ENABLE_LINEAR_SPRING); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_spring_z/stiffness"), "set_param_z", "get_param_z", PARAM_LINEAR_SPRING_STIFFNESS); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_spring_z/damping"), "set_param_z", "get_param_z", PARAM_LINEAR_SPRING_DAMPING); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "linear_spring_z/equilibrium_point"), "set_param_z", "get_param_z", PARAM_LINEAR_SPRING_EQUILIBRIUM_POINT); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "angular_limit_z/enabled"), "set_flag_z", "get_flag_z", FLAG_ENABLE_ANGULAR_LIMIT); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "angular_limit_z/upper_angle", PROPERTY_HINT_RANGE, "-180,180,0.01"), "_set_angular_hi_limit_z", "_get_angular_hi_limit_z"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "angular_limit_z/lower_angle", PROPERTY_HINT_RANGE, "-180,180,0.01"), "_set_angular_lo_limit_z", "_get_angular_lo_limit_z"); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_limit_z/softness", PROPERTY_HINT_RANGE, "0.01,16,0.01"), "set_param_z", "get_param_z", PARAM_ANGULAR_LIMIT_SOFTNESS); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_limit_z/restitution", PROPERTY_HINT_RANGE, "0.01,16,0.01"), "set_param_z", "get_param_z", PARAM_ANGULAR_RESTITUTION); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_limit_z/damping", PROPERTY_HINT_RANGE, "0.01,16,0.01"), "set_param_z", "get_param_z", PARAM_ANGULAR_DAMPING); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_limit_z/force_limit"), "set_param_z", "get_param_z", PARAM_ANGULAR_FORCE_LIMIT); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_limit_z/erp"), "set_param_z", "get_param_z", PARAM_ANGULAR_ERP); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "angular_motor_z/enabled"), "set_flag_z", "get_flag_z", FLAG_ENABLE_MOTOR); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_motor_z/target_velocity"), "set_param_z", "get_param_z", PARAM_ANGULAR_MOTOR_TARGET_VELOCITY); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_motor_z/force_limit"), "set_param_z", "get_param_z", PARAM_ANGULAR_MOTOR_FORCE_LIMIT); - ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "angular_spring_z/enabled"), "set_flag_z", "get_flag_z", FLAG_ENABLE_ANGULAR_SPRING); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_spring_z/stiffness"), "set_param_z", "get_param_z", PARAM_ANGULAR_SPRING_STIFFNESS); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_spring_z/damping"), "set_param_z", "get_param_z", PARAM_ANGULAR_SPRING_DAMPING); - ADD_PROPERTYI(PropertyInfo(Variant::REAL, "angular_spring_z/equilibrium_point"), "set_param_z", "get_param_z", PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT); - - BIND_ENUM_CONSTANT(PARAM_LINEAR_LOWER_LIMIT); - BIND_ENUM_CONSTANT(PARAM_LINEAR_UPPER_LIMIT); - BIND_ENUM_CONSTANT(PARAM_LINEAR_LIMIT_SOFTNESS); - BIND_ENUM_CONSTANT(PARAM_LINEAR_RESTITUTION); - BIND_ENUM_CONSTANT(PARAM_LINEAR_DAMPING); - BIND_ENUM_CONSTANT(PARAM_LINEAR_MOTOR_TARGET_VELOCITY); - BIND_ENUM_CONSTANT(PARAM_LINEAR_MOTOR_FORCE_LIMIT); - BIND_ENUM_CONSTANT(PARAM_LINEAR_SPRING_STIFFNESS); - BIND_ENUM_CONSTANT(PARAM_LINEAR_SPRING_DAMPING); - BIND_ENUM_CONSTANT(PARAM_LINEAR_SPRING_EQUILIBRIUM_POINT); - BIND_ENUM_CONSTANT(PARAM_ANGULAR_LOWER_LIMIT); - BIND_ENUM_CONSTANT(PARAM_ANGULAR_UPPER_LIMIT); - BIND_ENUM_CONSTANT(PARAM_ANGULAR_LIMIT_SOFTNESS); - BIND_ENUM_CONSTANT(PARAM_ANGULAR_DAMPING); - BIND_ENUM_CONSTANT(PARAM_ANGULAR_RESTITUTION); - BIND_ENUM_CONSTANT(PARAM_ANGULAR_FORCE_LIMIT); - BIND_ENUM_CONSTANT(PARAM_ANGULAR_ERP); - BIND_ENUM_CONSTANT(PARAM_ANGULAR_MOTOR_TARGET_VELOCITY); - BIND_ENUM_CONSTANT(PARAM_ANGULAR_MOTOR_FORCE_LIMIT); - BIND_ENUM_CONSTANT(PARAM_ANGULAR_SPRING_STIFFNESS); - BIND_ENUM_CONSTANT(PARAM_ANGULAR_SPRING_DAMPING); - BIND_ENUM_CONSTANT(PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT); - BIND_ENUM_CONSTANT(PARAM_MAX); - - BIND_ENUM_CONSTANT(FLAG_ENABLE_LINEAR_LIMIT); - BIND_ENUM_CONSTANT(FLAG_ENABLE_ANGULAR_LIMIT); - BIND_ENUM_CONSTANT(FLAG_ENABLE_LINEAR_SPRING); - BIND_ENUM_CONSTANT(FLAG_ENABLE_ANGULAR_SPRING); - BIND_ENUM_CONSTANT(FLAG_ENABLE_MOTOR); - BIND_ENUM_CONSTANT(FLAG_ENABLE_LINEAR_MOTOR); - BIND_ENUM_CONSTANT(FLAG_MAX); -} - -void Generic6DOFJoint::set_param_x(Param p_param, float p_value) { - ERR_FAIL_INDEX(p_param, PARAM_MAX); - params_x[p_param] = p_value; - if (get_joint().is_valid()) { - PhysicsServer::get_singleton()->generic_6dof_joint_set_param(get_joint(), Vector3::AXIS_X, PhysicsServer::G6DOFJointAxisParam(p_param), p_value); - } - - update_gizmos(); -} -float Generic6DOFJoint::get_param_x(Param p_param) const { - ERR_FAIL_INDEX_V(p_param, PARAM_MAX, 0); - return params_x[p_param]; -} - -void Generic6DOFJoint::set_param_y(Param p_param, float p_value) { - ERR_FAIL_INDEX(p_param, PARAM_MAX); - params_y[p_param] = p_value; - if (get_joint().is_valid()) { - PhysicsServer::get_singleton()->generic_6dof_joint_set_param(get_joint(), Vector3::AXIS_Y, PhysicsServer::G6DOFJointAxisParam(p_param), p_value); - } - update_gizmos(); -} -float Generic6DOFJoint::get_param_y(Param p_param) const { - ERR_FAIL_INDEX_V(p_param, PARAM_MAX, 0); - return params_y[p_param]; -} - -void Generic6DOFJoint::set_param_z(Param p_param, float p_value) { - ERR_FAIL_INDEX(p_param, PARAM_MAX); - params_z[p_param] = p_value; - if (get_joint().is_valid()) { - PhysicsServer::get_singleton()->generic_6dof_joint_set_param(get_joint(), Vector3::AXIS_Z, PhysicsServer::G6DOFJointAxisParam(p_param), p_value); - } - update_gizmos(); -} -float Generic6DOFJoint::get_param_z(Param p_param) const { - ERR_FAIL_INDEX_V(p_param, PARAM_MAX, 0); - return params_z[p_param]; -} - -void Generic6DOFJoint::set_flag_x(Flag p_flag, bool p_enabled) { - ERR_FAIL_INDEX(p_flag, FLAG_MAX); - flags_x[p_flag] = p_enabled; - if (get_joint().is_valid()) { - PhysicsServer::get_singleton()->generic_6dof_joint_set_flag(get_joint(), Vector3::AXIS_X, PhysicsServer::G6DOFJointAxisFlag(p_flag), p_enabled); - } - update_gizmos(); -} -bool Generic6DOFJoint::get_flag_x(Flag p_flag) const { - ERR_FAIL_INDEX_V(p_flag, FLAG_MAX, false); - return flags_x[p_flag]; -} - -void Generic6DOFJoint::set_flag_y(Flag p_flag, bool p_enabled) { - ERR_FAIL_INDEX(p_flag, FLAG_MAX); - flags_y[p_flag] = p_enabled; - if (get_joint().is_valid()) { - PhysicsServer::get_singleton()->generic_6dof_joint_set_flag(get_joint(), Vector3::AXIS_Y, PhysicsServer::G6DOFJointAxisFlag(p_flag), p_enabled); - } - update_gizmos(); -} -bool Generic6DOFJoint::get_flag_y(Flag p_flag) const { - ERR_FAIL_INDEX_V(p_flag, FLAG_MAX, false); - return flags_y[p_flag]; -} - -void Generic6DOFJoint::set_flag_z(Flag p_flag, bool p_enabled) { - ERR_FAIL_INDEX(p_flag, FLAG_MAX); - flags_z[p_flag] = p_enabled; - if (get_joint().is_valid()) { - PhysicsServer::get_singleton()->generic_6dof_joint_set_flag(get_joint(), Vector3::AXIS_Z, PhysicsServer::G6DOFJointAxisFlag(p_flag), p_enabled); - } - update_gizmos(); -} -bool Generic6DOFJoint::get_flag_z(Flag p_flag) const { - ERR_FAIL_INDEX_V(p_flag, FLAG_MAX, false); - return flags_z[p_flag]; -} - -RID Generic6DOFJoint::_configure_joint(PhysicsBody *body_a, PhysicsBody *body_b) { - Transform gt = get_global_transform(); - //Vector3 cone_twistpos = gt.origin; - //Vector3 cone_twistdir = gt.basis.get_axis(2); - - Transform ainv = body_a->get_global_transform().affine_inverse(); - - Transform local_a = ainv * gt; - local_a.orthonormalize(); - Transform local_b = gt; - - if (body_b) { - Transform binv = body_b->get_global_transform().affine_inverse(); - local_b = binv * gt; - } - - local_b.orthonormalize(); - - RID j = PhysicsServer::get_singleton()->joint_create_generic_6dof(body_a->get_rid(), local_a, body_b ? body_b->get_rid() : RID(), local_b); - for (int i = 0; i < PARAM_MAX; i++) { - PhysicsServer::get_singleton()->generic_6dof_joint_set_param(j, Vector3::AXIS_X, PhysicsServer::G6DOFJointAxisParam(i), params_x[i]); - PhysicsServer::get_singleton()->generic_6dof_joint_set_param(j, Vector3::AXIS_Y, PhysicsServer::G6DOFJointAxisParam(i), params_y[i]); - PhysicsServer::get_singleton()->generic_6dof_joint_set_param(j, Vector3::AXIS_Z, PhysicsServer::G6DOFJointAxisParam(i), params_z[i]); - } - for (int i = 0; i < FLAG_MAX; i++) { - PhysicsServer::get_singleton()->generic_6dof_joint_set_flag(j, Vector3::AXIS_X, PhysicsServer::G6DOFJointAxisFlag(i), flags_x[i]); - PhysicsServer::get_singleton()->generic_6dof_joint_set_flag(j, Vector3::AXIS_Y, PhysicsServer::G6DOFJointAxisFlag(i), flags_y[i]); - PhysicsServer::get_singleton()->generic_6dof_joint_set_flag(j, Vector3::AXIS_Z, PhysicsServer::G6DOFJointAxisFlag(i), flags_z[i]); - } - - return j; -} - -Generic6DOFJoint::Generic6DOFJoint() { - set_param_x(PARAM_LINEAR_LOWER_LIMIT, 0); - set_param_x(PARAM_LINEAR_UPPER_LIMIT, 0); - set_param_x(PARAM_LINEAR_LIMIT_SOFTNESS, 0.7); - set_param_x(PARAM_LINEAR_RESTITUTION, 0.5); - set_param_x(PARAM_LINEAR_DAMPING, 1.0); - set_param_x(PARAM_LINEAR_MOTOR_TARGET_VELOCITY, 0); - set_param_x(PARAM_LINEAR_MOTOR_FORCE_LIMIT, 0); - set_param_x(PARAM_LINEAR_SPRING_STIFFNESS, 0.01); - set_param_x(PARAM_LINEAR_SPRING_DAMPING, 0.01); - set_param_x(PARAM_LINEAR_SPRING_EQUILIBRIUM_POINT, 0.0); - set_param_x(PARAM_ANGULAR_LOWER_LIMIT, 0); - set_param_x(PARAM_ANGULAR_UPPER_LIMIT, 0); - set_param_x(PARAM_ANGULAR_LIMIT_SOFTNESS, 0.5f); - set_param_x(PARAM_ANGULAR_DAMPING, 1.0f); - set_param_x(PARAM_ANGULAR_RESTITUTION, 0); - set_param_x(PARAM_ANGULAR_FORCE_LIMIT, 0); - set_param_x(PARAM_ANGULAR_ERP, 0.5); - set_param_x(PARAM_ANGULAR_MOTOR_TARGET_VELOCITY, 0); - set_param_x(PARAM_ANGULAR_MOTOR_FORCE_LIMIT, 300); - set_param_x(PARAM_ANGULAR_SPRING_STIFFNESS, 0); - set_param_x(PARAM_ANGULAR_SPRING_DAMPING, 0); - set_param_x(PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT, 0); - - set_flag_x(FLAG_ENABLE_ANGULAR_LIMIT, true); - set_flag_x(FLAG_ENABLE_LINEAR_LIMIT, true); - set_flag_x(FLAG_ENABLE_ANGULAR_SPRING, false); - set_flag_x(FLAG_ENABLE_LINEAR_SPRING, false); - set_flag_x(FLAG_ENABLE_MOTOR, false); - set_flag_x(FLAG_ENABLE_LINEAR_MOTOR, false); - - set_param_y(PARAM_LINEAR_LOWER_LIMIT, 0); - set_param_y(PARAM_LINEAR_UPPER_LIMIT, 0); - set_param_y(PARAM_LINEAR_LIMIT_SOFTNESS, 0.7); - set_param_y(PARAM_LINEAR_RESTITUTION, 0.5); - set_param_y(PARAM_LINEAR_DAMPING, 1.0); - set_param_y(PARAM_LINEAR_MOTOR_TARGET_VELOCITY, 0); - set_param_y(PARAM_LINEAR_MOTOR_FORCE_LIMIT, 0); - set_param_y(PARAM_LINEAR_SPRING_STIFFNESS, 0.01); - set_param_y(PARAM_LINEAR_SPRING_DAMPING, 0.01); - set_param_y(PARAM_LINEAR_SPRING_EQUILIBRIUM_POINT, 0.0); - set_param_y(PARAM_ANGULAR_LOWER_LIMIT, 0); - set_param_y(PARAM_ANGULAR_UPPER_LIMIT, 0); - set_param_y(PARAM_ANGULAR_LIMIT_SOFTNESS, 0.5f); - set_param_y(PARAM_ANGULAR_DAMPING, 1.0f); - set_param_y(PARAM_ANGULAR_RESTITUTION, 0); - set_param_y(PARAM_ANGULAR_FORCE_LIMIT, 0); - set_param_y(PARAM_ANGULAR_ERP, 0.5); - set_param_y(PARAM_ANGULAR_MOTOR_TARGET_VELOCITY, 0); - set_param_y(PARAM_ANGULAR_MOTOR_FORCE_LIMIT, 300); - set_param_y(PARAM_ANGULAR_SPRING_STIFFNESS, 0); - set_param_y(PARAM_ANGULAR_SPRING_DAMPING, 0); - set_param_y(PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT, 0); - - set_flag_y(FLAG_ENABLE_ANGULAR_LIMIT, true); - set_flag_y(FLAG_ENABLE_LINEAR_LIMIT, true); - set_flag_y(FLAG_ENABLE_ANGULAR_SPRING, false); - set_flag_y(FLAG_ENABLE_LINEAR_SPRING, false); - set_flag_y(FLAG_ENABLE_MOTOR, false); - set_flag_y(FLAG_ENABLE_LINEAR_MOTOR, false); - - set_param_z(PARAM_LINEAR_LOWER_LIMIT, 0); - set_param_z(PARAM_LINEAR_UPPER_LIMIT, 0); - set_param_z(PARAM_LINEAR_LIMIT_SOFTNESS, 0.7); - set_param_z(PARAM_LINEAR_RESTITUTION, 0.5); - set_param_z(PARAM_LINEAR_DAMPING, 1.0); - set_param_z(PARAM_LINEAR_MOTOR_TARGET_VELOCITY, 0); - set_param_z(PARAM_LINEAR_MOTOR_FORCE_LIMIT, 0); - set_param_z(PARAM_LINEAR_SPRING_STIFFNESS, 0.01); - set_param_z(PARAM_LINEAR_SPRING_DAMPING, 0.01); - set_param_z(PARAM_LINEAR_SPRING_EQUILIBRIUM_POINT, 0.0); - set_param_z(PARAM_ANGULAR_LOWER_LIMIT, 0); - set_param_z(PARAM_ANGULAR_UPPER_LIMIT, 0); - set_param_z(PARAM_ANGULAR_LIMIT_SOFTNESS, 0.5f); - set_param_z(PARAM_ANGULAR_DAMPING, 1.0f); - set_param_z(PARAM_ANGULAR_RESTITUTION, 0); - set_param_z(PARAM_ANGULAR_FORCE_LIMIT, 0); - set_param_z(PARAM_ANGULAR_ERP, 0.5); - set_param_z(PARAM_ANGULAR_MOTOR_TARGET_VELOCITY, 0); - set_param_z(PARAM_ANGULAR_MOTOR_FORCE_LIMIT, 300); - set_param_z(PARAM_ANGULAR_SPRING_STIFFNESS, 0); - set_param_z(PARAM_ANGULAR_SPRING_DAMPING, 0); - set_param_z(PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT, 0); - - set_flag_z(FLAG_ENABLE_ANGULAR_LIMIT, true); - set_flag_z(FLAG_ENABLE_LINEAR_LIMIT, true); - set_flag_z(FLAG_ENABLE_ANGULAR_SPRING, false); - set_flag_z(FLAG_ENABLE_LINEAR_SPRING, false); - set_flag_z(FLAG_ENABLE_MOTOR, false); - set_flag_z(FLAG_ENABLE_LINEAR_MOTOR, false); -} diff --git a/scene/3d/physics_joint.h b/scene/3d/physics_joint.h deleted file mode 100644 index 45b9318..0000000 --- a/scene/3d/physics_joint.h +++ /dev/null @@ -1,336 +0,0 @@ -#ifndef PHYSICS_JOINT_H -#define PHYSICS_JOINT_H -/*************************************************************************/ -/* physics_joint.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "scene/main/spatial.h" -#include "servers/physics_server.h" - -class PhysicsBody; - -class Joint : public Spatial { - GDCLASS(Joint, Spatial); - - RID ba, bb; - - RID joint; - - NodePath a; - NodePath b; - - int solver_priority; - bool exclude_from_collision; - String warning; - -protected: - void _disconnect_signals(); - void _body_exit_tree(); - void _update_joint(bool p_only_free = false); - - void _notification(int p_what); - - virtual RID _configure_joint(PhysicsBody *body_a, PhysicsBody *body_b) = 0; - - static void _bind_methods(); - -public: - virtual String get_configuration_warning() const; - - void set_node_a(const NodePath &p_node_a); - NodePath get_node_a() const; - - void set_node_b(const NodePath &p_node_b); - NodePath get_node_b() const; - - void set_solver_priority(int p_priority); - int get_solver_priority() const; - - void set_exclude_nodes_from_collision(bool p_enable); - bool get_exclude_nodes_from_collision() const; - - RID get_joint() const { return joint; } - Joint(); -}; - -/////////////////////////////////////////// - -class PinJoint : public Joint { - GDCLASS(PinJoint, Joint); - -public: - enum Param { - PARAM_BIAS = PhysicsServer::PIN_JOINT_BIAS, - PARAM_DAMPING = PhysicsServer::PIN_JOINT_DAMPING, - PARAM_IMPULSE_CLAMP = PhysicsServer::PIN_JOINT_IMPULSE_CLAMP - }; - -protected: - float params[3]; - virtual RID _configure_joint(PhysicsBody *body_a, PhysicsBody *body_b); - static void _bind_methods(); - -public: - void set_param(Param p_param, float p_value); - float get_param(Param p_param) const; - - PinJoint(); -}; - -VARIANT_ENUM_CAST(PinJoint::Param); - -class HingeJoint : public Joint { - GDCLASS(HingeJoint, Joint); - -public: - enum Param { - PARAM_BIAS = PhysicsServer::HINGE_JOINT_BIAS, - PARAM_LIMIT_UPPER = PhysicsServer::HINGE_JOINT_LIMIT_UPPER, - PARAM_LIMIT_LOWER = PhysicsServer::HINGE_JOINT_LIMIT_LOWER, - PARAM_LIMIT_BIAS = PhysicsServer::HINGE_JOINT_LIMIT_BIAS, - PARAM_LIMIT_SOFTNESS = PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS, - PARAM_LIMIT_RELAXATION = PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION, - PARAM_MOTOR_TARGET_VELOCITY = PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY, - PARAM_MOTOR_MAX_IMPULSE = PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE, - PARAM_MAX = PhysicsServer::HINGE_JOINT_MAX - }; - - enum Flag { - FLAG_USE_LIMIT = PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT, - FLAG_ENABLE_MOTOR = PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR, - FLAG_MAX = PhysicsServer::HINGE_JOINT_FLAG_MAX - }; - -protected: - float params[PARAM_MAX]; - bool flags[FLAG_MAX]; - virtual RID _configure_joint(PhysicsBody *body_a, PhysicsBody *body_b); - static void _bind_methods(); - - void _set_upper_limit(float p_limit); - float _get_upper_limit() const; - - void _set_lower_limit(float p_limit); - float _get_lower_limit() const; - -public: - void set_param(Param p_param, float p_value); - float get_param(Param p_param) const; - - void set_flag(Flag p_flag, bool p_value); - bool get_flag(Flag p_flag) const; - - HingeJoint(); -}; - -VARIANT_ENUM_CAST(HingeJoint::Param); -VARIANT_ENUM_CAST(HingeJoint::Flag); - -class SliderJoint : public Joint { - GDCLASS(SliderJoint, Joint); - -public: - enum Param { - PARAM_LINEAR_LIMIT_UPPER = PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER, - PARAM_LINEAR_LIMIT_LOWER = PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER, - PARAM_LINEAR_LIMIT_SOFTNESS = PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS, - PARAM_LINEAR_LIMIT_RESTITUTION = PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION, - PARAM_LINEAR_LIMIT_DAMPING = PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING, - PARAM_LINEAR_MOTION_SOFTNESS = PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS, - PARAM_LINEAR_MOTION_RESTITUTION = PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION, - PARAM_LINEAR_MOTION_DAMPING = PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING, - PARAM_LINEAR_ORTHOGONAL_SOFTNESS = PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS, - PARAM_LINEAR_ORTHOGONAL_RESTITUTION = PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION, - PARAM_LINEAR_ORTHOGONAL_DAMPING = PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING, - - PARAM_ANGULAR_LIMIT_UPPER = PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER, - PARAM_ANGULAR_LIMIT_LOWER = PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER, - PARAM_ANGULAR_LIMIT_SOFTNESS = PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS, - PARAM_ANGULAR_LIMIT_RESTITUTION = PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION, - PARAM_ANGULAR_LIMIT_DAMPING = PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING, - PARAM_ANGULAR_MOTION_SOFTNESS = PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS, - PARAM_ANGULAR_MOTION_RESTITUTION = PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION, - PARAM_ANGULAR_MOTION_DAMPING = PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING, - PARAM_ANGULAR_ORTHOGONAL_SOFTNESS = PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS, - PARAM_ANGULAR_ORTHOGONAL_RESTITUTION = PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION, - PARAM_ANGULAR_ORTHOGONAL_DAMPING = PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING, - PARAM_MAX = PhysicsServer::SLIDER_JOINT_MAX - - }; - -protected: - void _set_upper_limit_angular(float p_limit_angular); - float _get_upper_limit_angular() const; - - void _set_lower_limit_angular(float p_limit_angular); - float _get_lower_limit_angular() const; - - float params[PARAM_MAX]; - virtual RID _configure_joint(PhysicsBody *body_a, PhysicsBody *body_b); - static void _bind_methods(); - -public: - void set_param(Param p_param, float p_value); - float get_param(Param p_param) const; - - SliderJoint(); -}; - -VARIANT_ENUM_CAST(SliderJoint::Param); - -class ConeTwistJoint : public Joint { - GDCLASS(ConeTwistJoint, Joint); - -public: - enum Param { - - PARAM_SWING_SPAN, - PARAM_TWIST_SPAN, - PARAM_BIAS, - PARAM_SOFTNESS, - PARAM_RELAXATION, - PARAM_MAX - }; - -protected: - void _set_swing_span(float p_limit_angular); - float _get_swing_span() const; - - void _set_twist_span(float p_limit_angular); - float _get_twist_span() const; - - float params[PARAM_MAX]; - virtual RID _configure_joint(PhysicsBody *body_a, PhysicsBody *body_b); - static void _bind_methods(); - -public: - void set_param(Param p_param, float p_value); - float get_param(Param p_param) const; - - ConeTwistJoint(); -}; - -VARIANT_ENUM_CAST(ConeTwistJoint::Param); - -class Generic6DOFJoint : public Joint { - GDCLASS(Generic6DOFJoint, Joint); - -public: - enum Param { - - PARAM_LINEAR_LOWER_LIMIT = PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT, - PARAM_LINEAR_UPPER_LIMIT = PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT, - PARAM_LINEAR_LIMIT_SOFTNESS = PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS, - PARAM_LINEAR_RESTITUTION = PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION, - PARAM_LINEAR_DAMPING = PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING, - PARAM_LINEAR_MOTOR_TARGET_VELOCITY = PhysicsServer::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY, - PARAM_LINEAR_MOTOR_FORCE_LIMIT = PhysicsServer::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT, - PARAM_LINEAR_SPRING_STIFFNESS = PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS, - PARAM_LINEAR_SPRING_DAMPING = PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_DAMPING, - PARAM_LINEAR_SPRING_EQUILIBRIUM_POINT = PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT, - PARAM_ANGULAR_LOWER_LIMIT = PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT, - PARAM_ANGULAR_UPPER_LIMIT = PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT, - PARAM_ANGULAR_LIMIT_SOFTNESS = PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS, - PARAM_ANGULAR_DAMPING = PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING, - PARAM_ANGULAR_RESTITUTION = PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION, - PARAM_ANGULAR_FORCE_LIMIT = PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT, - PARAM_ANGULAR_ERP = PhysicsServer::G6DOF_JOINT_ANGULAR_ERP, - PARAM_ANGULAR_MOTOR_TARGET_VELOCITY = PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY, - PARAM_ANGULAR_MOTOR_FORCE_LIMIT = PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT, - PARAM_ANGULAR_SPRING_STIFFNESS = PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS, - PARAM_ANGULAR_SPRING_DAMPING = PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_DAMPING, - PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT = PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT, - PARAM_MAX = PhysicsServer::G6DOF_JOINT_MAX, - }; - - enum Flag { - FLAG_ENABLE_LINEAR_LIMIT = PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT, - FLAG_ENABLE_ANGULAR_LIMIT = PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT, - FLAG_ENABLE_LINEAR_SPRING = PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING, - FLAG_ENABLE_ANGULAR_SPRING = PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING, - FLAG_ENABLE_MOTOR = PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR, - FLAG_ENABLE_LINEAR_MOTOR = PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR, - FLAG_MAX = PhysicsServer::G6DOF_JOINT_FLAG_MAX - }; - -protected: - void _set_angular_hi_limit_x(float p_limit_angular); - float _get_angular_hi_limit_x() const; - - void _set_angular_hi_limit_y(float p_limit_angular); - float _get_angular_hi_limit_y() const; - - void _set_angular_hi_limit_z(float p_limit_angular); - float _get_angular_hi_limit_z() const; - - void _set_angular_lo_limit_x(float p_limit_angular); - float _get_angular_lo_limit_x() const; - - void _set_angular_lo_limit_y(float p_limit_angular); - float _get_angular_lo_limit_y() const; - - void _set_angular_lo_limit_z(float p_limit_angular); - float _get_angular_lo_limit_z() const; - - float params_x[PARAM_MAX]; - bool flags_x[FLAG_MAX]; - float params_y[PARAM_MAX]; - bool flags_y[FLAG_MAX]; - float params_z[PARAM_MAX]; - bool flags_z[FLAG_MAX]; - - virtual RID _configure_joint(PhysicsBody *body_a, PhysicsBody *body_b); - static void _bind_methods(); - -public: - void set_param_x(Param p_param, float p_value); - float get_param_x(Param p_param) const; - - void set_param_y(Param p_param, float p_value); - float get_param_y(Param p_param) const; - - void set_param_z(Param p_param, float p_value); - float get_param_z(Param p_param) const; - - void set_flag_x(Flag p_flag, bool p_enabled); - bool get_flag_x(Flag p_flag) const; - - void set_flag_y(Flag p_flag, bool p_enabled); - bool get_flag_y(Flag p_flag) const; - - void set_flag_z(Flag p_flag, bool p_enabled); - bool get_flag_z(Flag p_flag) const; - - Generic6DOFJoint(); -}; - -VARIANT_ENUM_CAST(Generic6DOFJoint::Param); -VARIANT_ENUM_CAST(Generic6DOFJoint::Flag); - -#endif // PHYSICS_JOINT_H diff --git a/scene/3d/ray_cast.cpp b/scene/3d/ray_cast.cpp deleted file mode 100644 index 0bccd8c..0000000 --- a/scene/3d/ray_cast.cpp +++ /dev/null @@ -1,526 +0,0 @@ -/*************************************************************************/ -/* ray_cast.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "ray_cast.h" - -#include "collision_object.h" -#include "core/config/engine.h" -#include "mesh_instance.h" -#include "scene/resources/material/material.h" -#include "scene/resources/material/spatial_material.h" -#include "scene/resources/mesh/mesh.h" -#include "scene/resources/world_3d.h" -#include "servers/physics_server.h" - -void RayCast::set_cast_to(const Vector3 &p_point) { - cast_to = p_point; - update_gizmos(); - - if (Engine::get_singleton()->is_editor_hint()) { - if (is_inside_tree()) { - _update_debug_shape_vertices(); - } - } else if (debug_shape) { - _update_debug_shape(); - } -} - -Vector3 RayCast::get_cast_to() const { - return cast_to; -} - -void RayCast::set_collision_mask(uint32_t p_mask) { - collision_mask = p_mask; -} - -uint32_t RayCast::get_collision_mask() const { - return collision_mask; -} - -void RayCast::set_collision_mask_bit(int p_bit, bool p_value) { - ERR_FAIL_INDEX_MSG(p_bit, 32, "Collision mask bit must be between 0 and 31 inclusive."); - uint32_t mask = get_collision_mask(); - if (p_value) { - mask |= 1 << p_bit; - } else { - mask &= ~(1 << p_bit); - } - set_collision_mask(mask); -} - -bool RayCast::get_collision_mask_bit(int p_bit) const { - ERR_FAIL_INDEX_V_MSG(p_bit, 32, false, "Collision mask bit must be between 0 and 31 inclusive."); - return get_collision_mask() & (1 << p_bit); -} - -bool RayCast::is_colliding() const { - return collided; -} -Object *RayCast::get_collider() const { - if (against == 0) { - return nullptr; - } - - return ObjectDB::get_instance(against); -} - -int RayCast::get_collider_shape() const { - return against_shape; -} -Vector3 RayCast::get_collision_point() const { - return collision_point; -} -Vector3 RayCast::get_collision_normal() const { - return collision_normal; -} - -void RayCast::set_enabled(bool p_enabled) { - enabled = p_enabled; - update_gizmos(); - - if (is_inside_tree() && !Engine::get_singleton()->is_editor_hint()) { - set_physics_process_internal(p_enabled); - } - if (!p_enabled) { - collided = false; - } - - if (is_inside_tree() && get_tree()->is_debugging_collisions_hint()) { - if (p_enabled) { - _update_debug_shape(); - } else { - _clear_debug_shape(); - } - } -} - -bool RayCast::is_enabled() const { - return enabled; -} - -void RayCast::set_exclude_parent_body(bool p_exclude_parent_body) { - if (exclude_parent_body == p_exclude_parent_body) { - return; - } - - exclude_parent_body = p_exclude_parent_body; - - if (!is_inside_tree()) { - return; - } - - if (Object::cast_to(get_parent())) { - if (exclude_parent_body) { - exclude.insert(Object::cast_to(get_parent())->get_rid()); - } else { - exclude.erase(Object::cast_to(get_parent())->get_rid()); - } - } -} - -bool RayCast::get_exclude_parent_body() const { - return exclude_parent_body; -} - -void RayCast::_notification(int p_what) { - switch (p_what) { - case NOTIFICATION_ENTER_TREE: { - if (Engine::get_singleton()->is_editor_hint()) { - _update_debug_shape_vertices(); - } - if (enabled && !Engine::get_singleton()->is_editor_hint()) { - set_physics_process_internal(true); - } else { - set_physics_process_internal(false); - } - - if (get_tree()->is_debugging_collisions_hint()) { - _update_debug_shape(); - } - - if (Object::cast_to(get_parent())) { - if (exclude_parent_body) { - exclude.insert(Object::cast_to(get_parent())->get_rid()); - } else { - exclude.erase(Object::cast_to(get_parent())->get_rid()); - } - } - - } break; - case NOTIFICATION_EXIT_TREE: { - if (enabled) { - set_physics_process_internal(false); - } - - if (debug_shape) { - _clear_debug_shape(); - } - - } break; - case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: { - if (!enabled) { - break; - } - - bool prev_collision_state = collided; - _update_raycast_state(); - if (prev_collision_state != collided && get_tree()->is_debugging_collisions_hint()) { - _update_debug_shape_material(true); - } - - } break; - } -} - -void RayCast::_update_raycast_state() { - Ref w3d = get_world_3d(); - ERR_FAIL_COND(w3d.is_null()); - - PhysicsDirectSpaceState *dss = PhysicsServer::get_singleton()->space_get_direct_state(w3d->get_space()); - ERR_FAIL_COND(!dss); - - Transform gt = get_global_transform(); - - Vector3 to = cast_to; - if (to == Vector3()) { - to = Vector3(0, 0.01, 0); - } - - PhysicsDirectSpaceState::RayResult rr; - - if (dss->intersect_ray(gt.get_origin(), gt.xform(to), rr, exclude, collision_mask, collide_with_bodies, collide_with_areas)) { - collided = true; - against = rr.collider_id; - collision_point = rr.position; - collision_normal = rr.normal; - against_shape = rr.shape; - } else { - collided = false; - against = 0; - against_shape = 0; - } -} - -void RayCast::force_raycast_update() { - _update_raycast_state(); -} - -void RayCast::add_exception_rid(const RID &p_rid) { - exclude.insert(p_rid); -} - -void RayCast::add_exception(const Object *p_object) { - ERR_FAIL_NULL(p_object); - const CollisionObject *co = Object::cast_to(p_object); - ERR_FAIL_COND_MSG(!co, "The passed Node must be an instance of CollisionObject."); - add_exception_rid(co->get_rid()); -} - -void RayCast::remove_exception_rid(const RID &p_rid) { - exclude.erase(p_rid); -} - -void RayCast::remove_exception(const Object *p_object) { - ERR_FAIL_NULL(p_object); - const CollisionObject *co = Object::cast_to(p_object); - ERR_FAIL_COND_MSG(!co, "The passed Node must be an instance of CollisionObject."); - remove_exception_rid(co->get_rid()); -} - -void RayCast::clear_exceptions() { - exclude.clear(); - - if (exclude_parent_body && is_inside_tree()) { - CollisionObject *parent = Object::cast_to(get_parent()); - if (parent) { - exclude.insert(parent->get_rid()); - } - } -} - -void RayCast::set_collide_with_areas(bool p_clip) { - collide_with_areas = p_clip; -} - -bool RayCast::is_collide_with_areas_enabled() const { - return collide_with_areas; -} - -void RayCast::set_collide_with_bodies(bool p_clip) { - collide_with_bodies = p_clip; -} - -bool RayCast::is_collide_with_bodies_enabled() const { - return collide_with_bodies; -} - -void RayCast::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_enabled", "enabled"), &RayCast::set_enabled); - ClassDB::bind_method(D_METHOD("is_enabled"), &RayCast::is_enabled); - - ClassDB::bind_method(D_METHOD("set_cast_to", "local_point"), &RayCast::set_cast_to); - ClassDB::bind_method(D_METHOD("get_cast_to"), &RayCast::get_cast_to); - - ClassDB::bind_method(D_METHOD("is_colliding"), &RayCast::is_colliding); - ClassDB::bind_method(D_METHOD("force_raycast_update"), &RayCast::force_raycast_update); - - ClassDB::bind_method(D_METHOD("get_collider"), &RayCast::get_collider); - ClassDB::bind_method(D_METHOD("get_collider_shape"), &RayCast::get_collider_shape); - ClassDB::bind_method(D_METHOD("get_collision_point"), &RayCast::get_collision_point); - ClassDB::bind_method(D_METHOD("get_collision_normal"), &RayCast::get_collision_normal); - - ClassDB::bind_method(D_METHOD("add_exception_rid", "rid"), &RayCast::add_exception_rid); - ClassDB::bind_method(D_METHOD("add_exception", "node"), &RayCast::add_exception); - - ClassDB::bind_method(D_METHOD("remove_exception_rid", "rid"), &RayCast::remove_exception_rid); - ClassDB::bind_method(D_METHOD("remove_exception", "node"), &RayCast::remove_exception); - - ClassDB::bind_method(D_METHOD("clear_exceptions"), &RayCast::clear_exceptions); - - ClassDB::bind_method(D_METHOD("set_collision_mask", "mask"), &RayCast::set_collision_mask); - ClassDB::bind_method(D_METHOD("get_collision_mask"), &RayCast::get_collision_mask); - - ClassDB::bind_method(D_METHOD("set_collision_mask_bit", "bit", "value"), &RayCast::set_collision_mask_bit); - ClassDB::bind_method(D_METHOD("get_collision_mask_bit", "bit"), &RayCast::get_collision_mask_bit); - - ClassDB::bind_method(D_METHOD("set_exclude_parent_body", "mask"), &RayCast::set_exclude_parent_body); - ClassDB::bind_method(D_METHOD("get_exclude_parent_body"), &RayCast::get_exclude_parent_body); - - ClassDB::bind_method(D_METHOD("set_collide_with_areas", "enable"), &RayCast::set_collide_with_areas); - ClassDB::bind_method(D_METHOD("is_collide_with_areas_enabled"), &RayCast::is_collide_with_areas_enabled); - - ClassDB::bind_method(D_METHOD("set_collide_with_bodies", "enable"), &RayCast::set_collide_with_bodies); - ClassDB::bind_method(D_METHOD("is_collide_with_bodies_enabled"), &RayCast::is_collide_with_bodies_enabled); - - ClassDB::bind_method(D_METHOD("set_debug_shape_custom_color", "debug_shape_custom_color"), &RayCast::set_debug_shape_custom_color); - ClassDB::bind_method(D_METHOD("get_debug_shape_custom_color"), &RayCast::get_debug_shape_custom_color); - - ClassDB::bind_method(D_METHOD("set_debug_shape_thickness", "debug_shape_thickness"), &RayCast::set_debug_shape_thickness); - ClassDB::bind_method(D_METHOD("get_debug_shape_thickness"), &RayCast::get_debug_shape_thickness); - - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "enabled"), "set_enabled", "is_enabled"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "exclude_parent"), "set_exclude_parent_body", "get_exclude_parent_body"); - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "cast_to"), "set_cast_to", "get_cast_to"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "collision_mask", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_collision_mask", "get_collision_mask"); - - ADD_GROUP("Collide With", "collide_with"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "collide_with_areas", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_collide_with_areas", "is_collide_with_areas_enabled"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "collide_with_bodies", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_collide_with_bodies", "is_collide_with_bodies_enabled"); - - ADD_GROUP("Debug Shape", "debug_shape"); - ADD_PROPERTY(PropertyInfo(Variant::COLOR, "debug_shape_custom_color"), "set_debug_shape_custom_color", "get_debug_shape_custom_color"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "debug_shape_thickness", PROPERTY_HINT_RANGE, "1,5"), "set_debug_shape_thickness", "get_debug_shape_thickness"); -} - -int RayCast::get_debug_shape_thickness() const { - return debug_shape_thickness; -} - -void RayCast::_update_debug_shape_vertices() { - debug_shape_vertices.clear(); - debug_line_vertices.clear(); - - if (cast_to == Vector3()) { - return; - } - - debug_line_vertices.push_back(Vector3()); - debug_line_vertices.push_back(cast_to); - - if (debug_shape_thickness > 1) { - float scale_factor = 100.0; - Vector3 dir = Vector3(cast_to).normalized(); - // Draw truncated pyramid - Vector3 normal = (fabs(dir.x) + fabs(dir.y) > CMP_EPSILON) ? Vector3(-dir.y, dir.x, 0).normalized() : Vector3(0, -dir.z, dir.y).normalized(); - normal *= debug_shape_thickness / scale_factor; - int vertices_strip_order[14] = { 4, 5, 0, 1, 2, 5, 6, 4, 7, 0, 3, 2, 7, 6 }; - for (int v = 0; v < 14; v++) { - Vector3 vertex = vertices_strip_order[v] < 4 ? normal : normal / 3.0 + cast_to; - debug_shape_vertices.push_back(vertex.rotated(dir, Math_PI * (0.5 * (vertices_strip_order[v] % 4) + 0.25))); - } - } -} - -void RayCast::set_debug_shape_thickness(const int p_debug_shape_thickness) { - debug_shape_thickness = p_debug_shape_thickness; - update_gizmos(); - - if (Engine::get_singleton()->is_editor_hint()) { - if (is_inside_tree()) { - _update_debug_shape_vertices(); - } - } else if (debug_shape) { - _update_debug_shape(); - } -} - -const Vector &RayCast::get_debug_shape_vertices() const { - return debug_shape_vertices; -} - -const Vector &RayCast::get_debug_line_vertices() const { - return debug_line_vertices; -} - -void RayCast::set_debug_shape_custom_color(const Color &p_color) { - debug_shape_custom_color = p_color; - if (debug_material.is_valid()) { - _update_debug_shape_material(); - } -} - -Ref RayCast::get_debug_material() { - _update_debug_shape_material(); - return debug_material; -} - -const Color &RayCast::get_debug_shape_custom_color() const { - return debug_shape_custom_color; -} - -void RayCast::_create_debug_shape() { - _update_debug_shape_material(); - - Ref mesh = memnew(ArrayMesh); - - MeshInstance *mi = memnew(MeshInstance); -#ifdef TOOLS_ENABLED - // This enables the debug helper to show up in editor runs. - // However it should not show up during export, because global mode - // can slow the portal system, and this should only be used for debugging. - mi->set_portal_mode(CullInstance::PORTAL_MODE_GLOBAL); -#endif - mi->set_mesh(mesh); - add_child(mi); - - debug_shape = mi; -} - -void RayCast::_update_debug_shape_material(bool p_check_collision) { - if (!debug_material.is_valid()) { - Ref material = memnew(SpatialMaterial); - debug_material = material; - - material->set_flag(SpatialMaterial::FLAG_UNSHADED, true); - material->set_feature(SpatialMaterial::FEATURE_TRANSPARENT, true); - // Use double-sided rendering so that the RayCast can be seen if the camera is inside. - material->set_cull_mode(SpatialMaterial::CULL_DISABLED); - } - - Color color = debug_shape_custom_color; - if (color == Color(0.0, 0.0, 0.0)) { - // Use the default debug shape color defined in the Project Settings. - color = get_tree()->get_debug_collisions_color(); - } - - if (p_check_collision && collided) { - if ((color.get_h() < 0.055 || color.get_h() > 0.945) && color.get_s() > 0.5 && color.get_v() > 0.5) { - // If base color is already quite reddish, highlight collision with green color - color = Color(0.0, 1.0, 0.0, color.a); - } else { - // Else, highlight collision with red color - color = Color(1.0, 0, 0, color.a); - } - } - - Ref material = static_cast>(debug_material); - material->set_albedo(color); -} - -void RayCast::_update_debug_shape() { - if (!enabled) { - return; - } - - if (!debug_shape) { - _create_debug_shape(); - } - - MeshInstance *mi = static_cast(debug_shape); - Ref mesh = mi->get_mesh(); - if (!mesh.is_valid()) { - return; - } - - _update_debug_shape_vertices(); - - mesh->clear_surfaces(); - - Array a; - a.resize(Mesh::ARRAY_MAX); - - uint32_t flags = 0; - int surface_count = 0; - - if (!debug_line_vertices.empty()) { - a[Mesh::ARRAY_VERTEX] = debug_line_vertices; - mesh->add_surface_from_arrays(Mesh::PRIMITIVE_LINES, a, Array(), flags); - mesh->surface_set_material(surface_count, debug_material); - ++surface_count; - } - - if (!debug_shape_vertices.empty()) { - a[Mesh::ARRAY_VERTEX] = debug_shape_vertices; - mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLE_STRIP, a, Array(), flags); - mesh->surface_set_material(surface_count, debug_material); - ++surface_count; - } -} - -void RayCast::_clear_debug_shape() { - if (!debug_shape) { - return; - } - - MeshInstance *mi = static_cast(debug_shape); - if (mi->is_inside_tree()) { - mi->queue_delete(); - } else { - memdelete(mi); - } - - debug_shape = nullptr; -} - -RayCast::RayCast() { - enabled = false; - against = 0; - collided = false; - against_shape = 0; - collision_mask = 1; - cast_to = Vector3(0, -1, 0); - debug_shape = nullptr; - exclude_parent_body = true; - collide_with_areas = false; - collide_with_bodies = true; -} - -RayCast::~RayCast() { -} diff --git a/scene/3d/ray_cast.h b/scene/3d/ray_cast.h deleted file mode 100644 index ef2ed5d..0000000 --- a/scene/3d/ray_cast.h +++ /dev/null @@ -1,124 +0,0 @@ -#ifndef RAY_CAST_H -#define RAY_CAST_H -/*************************************************************************/ -/* ray_cast.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "scene/main/spatial.h" - -class SpatialMaterial; - -class RayCast : public Spatial { - GDCLASS(RayCast, Spatial); - - bool enabled; - bool collided; - ObjectID against; - int against_shape; - Vector3 collision_point; - Vector3 collision_normal; - - Vector3 cast_to; - RBSet exclude; - - uint32_t collision_mask; - bool exclude_parent_body; - - Node *debug_shape; - Ref debug_material; - Color debug_shape_custom_color = Color(0.0, 0.0, 0.0); - int debug_shape_thickness = 2; - Vector debug_shape_vertices; - Vector debug_line_vertices; - - void _create_debug_shape(); - void _update_debug_shape(); - void _update_debug_shape_material(bool p_check_collision = false); - void _update_debug_shape_vertices(); - void _clear_debug_shape(); - - bool collide_with_areas; - bool collide_with_bodies; - -protected: - void _notification(int p_what); - void _update_raycast_state(); - static void _bind_methods(); - -public: - void set_collide_with_areas(bool p_clip); - bool is_collide_with_areas_enabled() const; - - void set_collide_with_bodies(bool p_clip); - bool is_collide_with_bodies_enabled() const; - - void set_enabled(bool p_enabled); - bool is_enabled() const; - - void set_cast_to(const Vector3 &p_point); - Vector3 get_cast_to() const; - - void set_collision_mask(uint32_t p_mask); - uint32_t get_collision_mask() const; - - void set_collision_mask_bit(int p_bit, bool p_value); - bool get_collision_mask_bit(int p_bit) const; - - void set_exclude_parent_body(bool p_exclude_parent_body); - bool get_exclude_parent_body() const; - - const Color &get_debug_shape_custom_color() const; - void set_debug_shape_custom_color(const Color &p_color); - - const Vector &get_debug_shape_vertices() const; - const Vector &get_debug_line_vertices() const; - - Ref get_debug_material(); - - int get_debug_shape_thickness() const; - void set_debug_shape_thickness(const int p_debug_thickness); - - void force_raycast_update(); - bool is_colliding() const; - Object *get_collider() const; - int get_collider_shape() const; - Vector3 get_collision_point() const; - Vector3 get_collision_normal() const; - - void add_exception_rid(const RID &p_rid); - void add_exception(const Object *p_object); - void remove_exception_rid(const RID &p_rid); - void remove_exception(const Object *p_object); - void clear_exceptions(); - - RayCast(); - ~RayCast(); -}; - -#endif // RAY_CAST_H diff --git a/scene/3d/shape_cast.cpp b/scene/3d/shape_cast.cpp deleted file mode 100644 index 76f3c9f..0000000 --- a/scene/3d/shape_cast.cpp +++ /dev/null @@ -1,638 +0,0 @@ -/*************************************************************************/ -/* shape_cast.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_cast.h" - -#include "collision_object.h" -#include "core/config/engine.h" -#include "mesh_instance.h" -#include "scene/resources/shapes/concave_polygon_shape.h" -#include "scene/resources/material/material.h" -#include "scene/resources/material/spatial_material.h" -#include "scene/resources/mesh/mesh.h" -#include "scene/resources/world_3d.h" - -void ShapeCast::_notification(int p_what) { - switch (p_what) { - case NOTIFICATION_ENTER_TREE: { - if (Engine::get_singleton()->is_editor_hint()) { - _update_debug_shape_vertices(); - } - if (enabled && !Engine::get_singleton()->is_editor_hint()) { - set_physics_process_internal(true); - } else { - set_physics_process_internal(false); - } - - if (get_tree()->is_debugging_collisions_hint()) { - _update_debug_shape(); - } - - if (Object::cast_to(get_parent())) { - if (exclude_parent_body) { - exclude.insert(Object::cast_to(get_parent())->get_rid()); - } else { - exclude.erase(Object::cast_to(get_parent())->get_rid()); - } - } - } break; - - case NOTIFICATION_EXIT_TREE: { - if (enabled) { - set_physics_process_internal(false); - } - - if (debug_shape) { - _clear_debug_shape(); - } - } break; - - case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: { - if (!enabled) { - break; - } - - bool prev_collision_state = collided; - _update_shapecast_state(); - if (get_tree()->is_debugging_collisions_hint()) { - if (prev_collision_state != collided) { - _update_debug_shape_material(true); - } - if (collided) { - _update_debug_shape(); - } - if (prev_collision_state == collided && !collided) { - _update_debug_shape(); - } - } - } break; - } -} - -void ShapeCast::_bind_methods() { - ClassDB::bind_method(D_METHOD("resource_changed", "resource"), &ShapeCast::resource_changed); - - ClassDB::bind_method(D_METHOD("set_enabled", "enabled"), &ShapeCast::set_enabled); - ClassDB::bind_method(D_METHOD("is_enabled"), &ShapeCast::is_enabled); - - ClassDB::bind_method(D_METHOD("set_shape", "shape"), &ShapeCast::set_shape); - ClassDB::bind_method(D_METHOD("get_shape"), &ShapeCast::get_shape); - - ClassDB::bind_method(D_METHOD("set_target_position", "local_point"), &ShapeCast::set_target_position); - ClassDB::bind_method(D_METHOD("get_target_position"), &ShapeCast::get_target_position); - - ClassDB::bind_method(D_METHOD("set_margin", "margin"), &ShapeCast::set_margin); - ClassDB::bind_method(D_METHOD("get_margin"), &ShapeCast::get_margin); - - ClassDB::bind_method(D_METHOD("set_max_results", "max_results"), &ShapeCast::set_max_results); - ClassDB::bind_method(D_METHOD("get_max_results"), &ShapeCast::get_max_results); - - ClassDB::bind_method(D_METHOD("is_colliding"), &ShapeCast::is_colliding); - ClassDB::bind_method(D_METHOD("get_collision_count"), &ShapeCast::get_collision_count); - - ClassDB::bind_method(D_METHOD("force_shapecast_update"), &ShapeCast::force_shapecast_update); - - ClassDB::bind_method(D_METHOD("get_collider", "index"), &ShapeCast::get_collider); - ClassDB::bind_method(D_METHOD("get_collider_rid", "index"), &ShapeCast::get_collider_rid); - ClassDB::bind_method(D_METHOD("get_collider_shape", "index"), &ShapeCast::get_collider_shape); - ClassDB::bind_method(D_METHOD("get_collision_point", "index"), &ShapeCast::get_collision_point); - ClassDB::bind_method(D_METHOD("get_collision_normal", "index"), &ShapeCast::get_collision_normal); - - ClassDB::bind_method(D_METHOD("get_closest_collision_safe_fraction"), &ShapeCast::get_closest_collision_safe_fraction); - ClassDB::bind_method(D_METHOD("get_closest_collision_unsafe_fraction"), &ShapeCast::get_closest_collision_unsafe_fraction); - - ClassDB::bind_method(D_METHOD("add_exception_rid", "rid"), &ShapeCast::add_exception_rid); - ClassDB::bind_method(D_METHOD("add_exception", "node"), &ShapeCast::add_exception); - - ClassDB::bind_method(D_METHOD("remove_exception_rid", "rid"), &ShapeCast::remove_exception_rid); - ClassDB::bind_method(D_METHOD("remove_exception", "node"), &ShapeCast::remove_exception); - - ClassDB::bind_method(D_METHOD("clear_exceptions"), &ShapeCast::clear_exceptions); - - ClassDB::bind_method(D_METHOD("set_collision_mask", "mask"), &ShapeCast::set_collision_mask); - ClassDB::bind_method(D_METHOD("get_collision_mask"), &ShapeCast::get_collision_mask); - - ClassDB::bind_method(D_METHOD("set_collision_mask_value", "layer_number", "value"), &ShapeCast::set_collision_mask_value); - ClassDB::bind_method(D_METHOD("get_collision_mask_value", "layer_number"), &ShapeCast::get_collision_mask_value); - - ClassDB::bind_method(D_METHOD("set_exclude_parent_body", "mask"), &ShapeCast::set_exclude_parent_body); - ClassDB::bind_method(D_METHOD("get_exclude_parent_body"), &ShapeCast::get_exclude_parent_body); - - ClassDB::bind_method(D_METHOD("set_collide_with_areas", "enable"), &ShapeCast::set_collide_with_areas); - ClassDB::bind_method(D_METHOD("is_collide_with_areas_enabled"), &ShapeCast::is_collide_with_areas_enabled); - - ClassDB::bind_method(D_METHOD("set_collide_with_bodies", "enable"), &ShapeCast::set_collide_with_bodies); - ClassDB::bind_method(D_METHOD("is_collide_with_bodies_enabled"), &ShapeCast::is_collide_with_bodies_enabled); - - ClassDB::bind_method(D_METHOD("_get_collision_result"), &ShapeCast::_get_collision_result); - - ClassDB::bind_method(D_METHOD("set_debug_shape_custom_color", "debug_shape_custom_color"), &ShapeCast::set_debug_shape_custom_color); - ClassDB::bind_method(D_METHOD("get_debug_shape_custom_color"), &ShapeCast::get_debug_shape_custom_color); - - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "enabled"), "set_enabled", "is_enabled"); - ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "shape", PROPERTY_HINT_RESOURCE_TYPE, "Shape"), "set_shape", "get_shape"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "exclude_parent"), "set_exclude_parent_body", "get_exclude_parent_body"); - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "target_position", PROPERTY_HINT_NONE), "set_target_position", "get_target_position"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "margin", PROPERTY_HINT_RANGE, "0,100,0.01"), "set_margin", "get_margin"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "max_results"), "set_max_results", "get_max_results"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "collision_mask", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_collision_mask", "get_collision_mask"); - ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "collision_result", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_SCRIPT_VARIABLE), "", "_get_collision_result"); - - ADD_GROUP("Collide With", "collide_with"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "collide_with_areas", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_collide_with_areas", "is_collide_with_areas_enabled"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "collide_with_bodies", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_collide_with_bodies", "is_collide_with_bodies_enabled"); - - ADD_GROUP("Debug Shape", "debug_shape"); - ADD_PROPERTY(PropertyInfo(Variant::COLOR, "debug_shape_custom_color"), "set_debug_shape_custom_color", "get_debug_shape_custom_color"); -} - -String ShapeCast::get_configuration_warning() const { - String warning = Spatial::get_configuration_warning(); - - if (shape.is_null()) { - if (warning != String()) { - warning += "\n\n"; - } - warning += TTR("This node cannot interact with other objects unless a Shape is assigned."); - } - - if (shape.is_valid() && Object::cast_to(*shape)) { - if (warning != String()) { - warning += "\n\n"; - } - warning += TTR("ShapeCast does not support ConcavePolygonShapes. Collisions will not be reported."); - } - - return warning; -} - -void ShapeCast::set_enabled(bool p_enabled) { - enabled = p_enabled; - update_gizmos(); - - if (is_inside_tree() && !Engine::get_singleton()->is_editor_hint()) { - set_physics_process_internal(p_enabled); - } - if (!p_enabled) { - collided = false; - } - - if (is_inside_tree() && get_tree()->is_debugging_collisions_hint()) { - if (p_enabled) { - _update_debug_shape(); - } else { - _clear_debug_shape(); - } - } -} - -bool ShapeCast::is_enabled() const { - return enabled; -} - -void ShapeCast::set_target_position(const Vector3 &p_point) { - target_position = p_point; - if (is_inside_tree()) { - _update_debug_shape(); - } - update_gizmos(); - - if (Engine::get_singleton()->is_editor_hint()) { - if (is_inside_tree()) { - _update_debug_shape_vertices(); - } - } else if (debug_shape) { - _update_debug_shape(); - } -} - -Vector3 ShapeCast::get_target_position() const { - return target_position; -} - -void ShapeCast::set_margin(real_t p_margin) { - margin = p_margin; -} - -real_t ShapeCast::get_margin() const { - return margin; -} - -void ShapeCast::set_max_results(int p_max_results) { - max_results = p_max_results; -} - -int ShapeCast::get_max_results() const { - return max_results; -} - -void ShapeCast::set_collision_mask(uint32_t p_mask) { - collision_mask = p_mask; -} - -uint32_t ShapeCast::get_collision_mask() const { - return collision_mask; -} - -void ShapeCast::set_collision_mask_value(int p_layer_number, bool p_value) { - ERR_FAIL_COND_MSG(p_layer_number < 1, "Collision layer number must be between 1 and 32 inclusive."); - ERR_FAIL_COND_MSG(p_layer_number > 32, "Collision layer number must be between 1 and 32 inclusive."); - uint32_t mask = get_collision_mask(); - if (p_value) { - mask |= 1 << (p_layer_number - 1); - } else { - mask &= ~(1 << (p_layer_number - 1)); - } - set_collision_mask(mask); -} - -bool ShapeCast::get_collision_mask_value(int p_layer_number) const { - ERR_FAIL_COND_V_MSG(p_layer_number < 1, false, "Collision layer number must be between 1 and 32 inclusive."); - ERR_FAIL_COND_V_MSG(p_layer_number > 32, false, "Collision layer number must be between 1 and 32 inclusive."); - return get_collision_mask() & (1 << (p_layer_number - 1)); -} - -int ShapeCast::get_collision_count() const { - return result.size(); -} - -bool ShapeCast::is_colliding() const { - return collided; -} - -Object *ShapeCast::get_collider(int p_idx) const { - ERR_FAIL_INDEX_V_MSG(p_idx, result.size(), nullptr, "No collider found."); - - if (result[p_idx].collider_id == 0) { - return nullptr; - } - return ObjectDB::get_instance(result[p_idx].collider_id); -} - -RID ShapeCast::get_collider_rid(int p_idx) const { - ERR_FAIL_INDEX_V_MSG(p_idx, result.size(), RID(), "No collider RID found."); - return result[p_idx].rid; -} - -int ShapeCast::get_collider_shape(int p_idx) const { - ERR_FAIL_INDEX_V_MSG(p_idx, result.size(), -1, "No collider shape found."); - return result[p_idx].shape; -} - -Vector3 ShapeCast::get_collision_point(int p_idx) const { - ERR_FAIL_INDEX_V_MSG(p_idx, result.size(), Vector3(), "No collision point found."); - return result[p_idx].point; -} - -Vector3 ShapeCast::get_collision_normal(int p_idx) const { - ERR_FAIL_INDEX_V_MSG(p_idx, result.size(), Vector3(), "No collision normal found."); - return result[p_idx].normal; -} - -real_t ShapeCast::get_closest_collision_safe_fraction() const { - return collision_safe_fraction; -} - -real_t ShapeCast::get_closest_collision_unsafe_fraction() const { - return collision_unsafe_fraction; -} - -void ShapeCast::resource_changed(Ref p_res) { - if (is_inside_tree()) { - _update_debug_shape(); - } - update_gizmos(); -} - -void ShapeCast::set_shape(const Ref &p_shape) { - if (p_shape == shape) { - return; - } - if (!shape.is_null()) { - shape->unregister_owner(this); - } - shape = p_shape; - if (!shape.is_null()) { - shape->register_owner(this); - } - if (p_shape.is_valid()) { - shape_rid = shape->get_rid(); - } - - if (is_inside_tree()) { - _update_debug_shape(); - } - - update_gizmos(); - update_configuration_warning(); -} - -Ref ShapeCast::get_shape() const { - return shape; -} - -void ShapeCast::set_exclude_parent_body(bool p_exclude_parent_body) { - if (exclude_parent_body == p_exclude_parent_body) { - return; - } - exclude_parent_body = p_exclude_parent_body; - - if (!is_inside_tree()) { - return; - } - if (Object::cast_to(get_parent())) { - if (exclude_parent_body) { - exclude.insert(Object::cast_to(get_parent())->get_rid()); - } else { - exclude.erase(Object::cast_to(get_parent())->get_rid()); - } - } -} - -bool ShapeCast::get_exclude_parent_body() const { - return exclude_parent_body; -} - -void ShapeCast::_update_shapecast_state() { - result.clear(); - - ERR_FAIL_COND_MSG(shape.is_null(), "Null reference to shape. ShapeCast requires a Shape3D to sweep for collisions."); - - Ref w3d = get_world_3d(); - ERR_FAIL_COND(w3d.is_null()); - - PhysicsDirectSpaceState *dss = PhysicsServer::get_singleton()->space_get_direct_state(w3d->get_space()); - ERR_FAIL_COND(!dss); - - Transform gt = get_global_transform(); - - collision_safe_fraction = 0.0; - collision_unsafe_fraction = 0.0; - - if (target_position != Vector3()) { - dss->cast_motion(shape_rid, gt, target_position, margin, collision_safe_fraction, collision_unsafe_fraction, exclude, collision_mask, collide_with_bodies, collide_with_areas); - if (collision_unsafe_fraction < 1.0) { - // Move shape transform to the point of impact, - // so we can collect contact info at that point. - gt.set_origin(gt.get_origin() + target_position * (collision_unsafe_fraction + CMP_EPSILON)); - } - } - - // Regardless of whether the shape is stuck or it's moved along - // the motion vector, we'll only consider static collisions from now on. - bool intersected = true; - RBSet intersected_objects = exclude; - while (intersected && result.size() < max_results) { - PhysicsDirectSpaceState::ShapeRestInfo info; - intersected = dss->rest_info(shape_rid, gt, margin, &info, intersected_objects, collision_mask, collide_with_bodies, collide_with_areas); - if (intersected) { - result.push_back(info); - intersected_objects.insert(info.rid); - } - } - collided = !result.empty(); -} - -void ShapeCast::force_shapecast_update() { - _update_shapecast_state(); -} - -void ShapeCast::add_exception_rid(const RID &p_rid) { - exclude.insert(p_rid); -} - -void ShapeCast::add_exception(const Object *p_object) { - ERR_FAIL_NULL(p_object); - const CollisionObject *co = Object::cast_to(p_object); - ERR_FAIL_COND_MSG(!co, "The passed Node must be an instance of CollisionObject."); - add_exception_rid(co->get_rid()); -} - -void ShapeCast::remove_exception_rid(const RID &p_rid) { - exclude.erase(p_rid); -} - -void ShapeCast::remove_exception(const Object *p_object) { - ERR_FAIL_NULL(p_object); - const CollisionObject *co = Object::cast_to(p_object); - ERR_FAIL_COND_MSG(!co, "The passed Node must be an instance of CollisionObject."); - remove_exception_rid(co->get_rid()); -} - -void ShapeCast::clear_exceptions() { - exclude.clear(); -} - -void ShapeCast::set_collide_with_areas(bool p_clip) { - collide_with_areas = p_clip; -} - -bool ShapeCast::is_collide_with_areas_enabled() const { - return collide_with_areas; -} - -void ShapeCast::set_collide_with_bodies(bool p_clip) { - collide_with_bodies = p_clip; -} - -bool ShapeCast::is_collide_with_bodies_enabled() const { - return collide_with_bodies; -} - -Array ShapeCast::_get_collision_result() const { - Array ret; - - for (int i = 0; i < result.size(); ++i) { - const PhysicsDirectSpaceState::ShapeRestInfo &sri = result[i]; - - Dictionary col; - col["point"] = sri.point; - col["normal"] = sri.normal; - col["rid"] = sri.rid; - col["collider"] = ObjectDB::get_instance(sri.collider_id); - col["collider_id"] = sri.collider_id; - col["shape"] = sri.shape; - col["linear_velocity"] = sri.linear_velocity; - - ret.push_back(col); - } - return ret; -} - -void ShapeCast::_update_debug_shape_vertices() { - debug_shape_vertices.clear(); - debug_line_vertices.clear(); - - if (!shape.is_null()) { - debug_shape_vertices.append_array(shape->get_debug_mesh_lines()); - for (int i = 0; i < debug_shape_vertices.size(); i++) { - debug_shape_vertices.set(i, debug_shape_vertices[i] + Vector3(target_position * get_closest_collision_safe_fraction())); - } - } - - if (target_position == Vector3()) { - return; - } - - debug_line_vertices.push_back(Vector3()); - debug_line_vertices.push_back(target_position); -} - -const Vector &ShapeCast::get_debug_shape_vertices() const { - return debug_shape_vertices; -} - -const Vector &ShapeCast::get_debug_line_vertices() const { - return debug_line_vertices; -} - -void ShapeCast::set_debug_shape_custom_color(const Color &p_color) { - debug_shape_custom_color = p_color; - if (debug_material.is_valid()) { - _update_debug_shape_material(); - } -} - -Ref ShapeCast::get_debug_material() { - _update_debug_shape_material(); - return debug_material; -} - -const Color &ShapeCast::get_debug_shape_custom_color() const { - return debug_shape_custom_color; -} - -void ShapeCast::_create_debug_shape() { - _update_debug_shape_material(); - - Ref mesh = memnew(ArrayMesh); - - MeshInstance *mi = memnew(MeshInstance); - mi->set_mesh(mesh); - - add_child(mi); - debug_shape = mi; -} - -void ShapeCast::_update_debug_shape_material(bool p_check_collision) { - if (!debug_material.is_valid()) { - Ref material = memnew(SpatialMaterial); - debug_material = material; - - material->set_flag(SpatialMaterial::FLAG_UNSHADED, true); - material->set_feature(SpatialMaterial::FEATURE_TRANSPARENT, true); - // Use double-sided rendering so that the RayCast can be seen if the camera is inside. - material->set_cull_mode(SpatialMaterial::CULL_DISABLED); - } - - Color color = debug_shape_custom_color; - if (color == Color(0.0, 0.0, 0.0)) { - // Use the default debug shape color defined in the Project Settings. - color = get_tree()->get_debug_collisions_color(); - } - - if (p_check_collision && collided) { - if ((color.get_h() < 0.055 || color.get_h() > 0.945) && color.get_s() > 0.5 && color.get_v() > 0.5) { - // If base color is already quite reddish, highlight collision with green color - color = Color(0.0, 1.0, 0.0, color.a); - } else { - // Else, highlight collision with red color - color = Color(1.0, 0, 0, color.a); - } - } - - Ref material = static_cast>(debug_material); - material->set_albedo(color); -} - -void ShapeCast::_update_debug_shape() { - if (!enabled) { - return; - } - - if (!debug_shape) { - _create_debug_shape(); - } - - _update_debug_shape_vertices(); - - if (Engine::get_singleton()->is_editor_hint()) { - return; - } - - MeshInstance *mi = static_cast(debug_shape); - Ref mesh = mi->get_mesh(); - if (!mesh.is_valid()) { - return; - } - - mesh->clear_surfaces(); - - Array a; - a.resize(Mesh::ARRAY_MAX); - - uint32_t flags = 0; - int surface_count = 0; - - if (!debug_shape_vertices.empty()) { - a[Mesh::ARRAY_VERTEX] = debug_shape_vertices; - mesh->add_surface_from_arrays(Mesh::PRIMITIVE_LINES, a, Array(), flags); - mesh->surface_set_material(surface_count, debug_material); - ++surface_count; - } - - if (!debug_line_vertices.empty()) { - a[Mesh::ARRAY_VERTEX] = debug_line_vertices; - mesh->add_surface_from_arrays(Mesh::PRIMITIVE_LINES, a, Array(), flags); - mesh->surface_set_material(surface_count, debug_material); - ++surface_count; - } -} - -void ShapeCast::_clear_debug_shape() { - if (!debug_shape) { - return; - } - - MeshInstance *mi = static_cast(debug_shape); - if (mi->is_inside_tree()) { - mi->queue_delete(); - } else { - memdelete(mi); - } - - debug_shape = nullptr; -} - -ShapeCast::~ShapeCast() { - if (!shape.is_null()) { - shape->unregister_owner(this); - } -} diff --git a/scene/3d/shape_cast.h b/scene/3d/shape_cast.h deleted file mode 100644 index 1407eef..0000000 --- a/scene/3d/shape_cast.h +++ /dev/null @@ -1,148 +0,0 @@ -/*************************************************************************/ -/* shape_cast.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_CAST_H -#define SHAPE_CAST_H - -#include "core/math/color.h" - -#include "scene/main/spatial.h" -#include "scene/resources/shapes/shape.h" -#include "servers/physics_server.h" - -class SpatialMaterial; - -class ShapeCast : public Spatial { - GDCLASS(ShapeCast, Spatial); - - bool enabled = true; - void resource_changed(Ref p_res); - - Ref shape; - RID shape_rid; - Vector3 target_position = Vector3(0, -1, 0); - - RBSet exclude; - real_t margin = 0.0; - uint32_t collision_mask = 1; - bool exclude_parent_body = true; - bool collide_with_areas = false; - bool collide_with_bodies = true; - - Node *debug_shape = nullptr; - Ref debug_material; - Color debug_shape_custom_color = Color(0.0, 0.0, 0.0); - Vector debug_shape_vertices; - Vector debug_line_vertices; - - void _create_debug_shape(); - void _update_debug_shape(); - void _update_debug_shape_material(bool p_check_collision = false); - void _update_debug_shape_vertices(); - void _clear_debug_shape(); - - // Result - int max_results = 32; - Vector result; - bool collided = false; - real_t collision_safe_fraction = 1.0; - real_t collision_unsafe_fraction = 1.0; - - Array _get_collision_result() const; - - ~ShapeCast(); - -protected: - void _notification(int p_what); - void _update_shapecast_state(); - static void _bind_methods(); - -public: - void set_collide_with_areas(bool p_clip); - bool is_collide_with_areas_enabled() const; - - void set_collide_with_bodies(bool p_clip); - bool is_collide_with_bodies_enabled() const; - - void set_enabled(bool p_enabled); - bool is_enabled() const; - - void set_shape(const Ref &p_shape); - Ref get_shape() const; - - void set_target_position(const Vector3 &p_point); - Vector3 get_target_position() const; - - void set_margin(real_t p_margin); - real_t get_margin() const; - - void set_max_results(int p_max_results); - int get_max_results() const; - - void set_collision_mask(uint32_t p_mask); - uint32_t get_collision_mask() const; - - void set_collision_mask_value(int p_layer_number, bool p_value); - bool get_collision_mask_value(int p_layer_number) const; - - void set_exclude_parent_body(bool p_exclude_parent_body); - bool get_exclude_parent_body() const; - - const Color &get_debug_shape_custom_color() const; - void set_debug_shape_custom_color(const Color &p_color); - - const Vector &get_debug_shape_vertices() const; - const Vector &get_debug_line_vertices() const; - - Ref get_debug_material(); - - int get_collision_count() const; - Object *get_collider(int p_idx) const; - RID get_collider_rid(int p_idx) const; - int get_collider_shape(int p_idx) const; - Vector3 get_collision_point(int p_idx) const; - Vector3 get_collision_normal(int p_idx) const; - - real_t get_closest_collision_safe_fraction() const; - real_t get_closest_collision_unsafe_fraction() const; - - void force_shapecast_update(); - bool is_colliding() const; - - void add_exception_rid(const RID &p_rid); - void add_exception(const Object *p_object); - void remove_exception_rid(const RID &p_rid); - void remove_exception(const Object *p_object); - void clear_exceptions(); - - virtual String get_configuration_warning() const; -}; - -#endif // SHAPE_CAST_H diff --git a/scene/3d/soft_body.cpp b/scene/3d/soft_body.cpp deleted file mode 100644 index c35ee9a..0000000 --- a/scene/3d/soft_body.cpp +++ /dev/null @@ -1,856 +0,0 @@ -/*************************************************************************/ -/* soft_body.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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.h" -#include "core/containers/list.h" -#include "core/object/object.h" -#include "core/os/os.h" -#include "core/containers/rid.h" -#include "scene/3d/collision_object.h" -#include "scene/3d/physics_body.h" -#include "scene/resources/mesh/mesh.h" -#include "scene/resources/world_3d.h" -#include "servers/physics_server.h" - -SoftBodyRenderingServerHandler::SoftBodyRenderingServerHandler() {} - -void SoftBodyRenderingServerHandler::prepare(RID p_mesh, int p_surface) { - clear(); - - ERR_FAIL_COND(!p_mesh.is_valid()); - - mesh = p_mesh; - surface = p_surface; - - const uint32_t surface_format = RS::get_singleton()->mesh_surface_get_format(mesh, surface); - const int surface_vertex_len = RS::get_singleton()->mesh_surface_get_array_len(mesh, p_surface); - const int surface_index_len = RS::get_singleton()->mesh_surface_get_array_index_len(mesh, p_surface); - uint32_t surface_offsets[RS::ARRAY_MAX]; - uint32_t surface_strides[RS::ARRAY_MAX]; - - buffer = RS::get_singleton()->mesh_surface_get_array(mesh, surface); - RS::get_singleton()->mesh_surface_make_offsets_from_format(surface_format, surface_vertex_len, surface_index_len, surface_offsets, surface_strides); - ERR_FAIL_COND(surface_strides[RS::ARRAY_VERTEX] != surface_strides[RS::ARRAY_NORMAL]); - stride = surface_strides[RS::ARRAY_VERTEX]; - offset_vertices = surface_offsets[RS::ARRAY_VERTEX]; - offset_normal = surface_offsets[RS::ARRAY_NORMAL]; -} - -void SoftBodyRenderingServerHandler::clear() { - if (mesh.is_valid()) { - buffer.resize(0); - } - - mesh = RID(); -} - -void SoftBodyRenderingServerHandler::open() { - write_buffer = buffer.write(); -} - -void SoftBodyRenderingServerHandler::close() { - write_buffer.release(); -} - -void SoftBodyRenderingServerHandler::commit_changes() { - RS::get_singleton()->mesh_surface_update_region(mesh, surface, 0, buffer); -} - -void SoftBodyRenderingServerHandler::set_vertex(int p_vertex_id, const void *p_vector3) { - memcpy(&write_buffer[p_vertex_id * stride + offset_vertices], p_vector3, sizeof(float) * 3); -} - -void SoftBodyRenderingServerHandler::set_normal(int p_vertex_id, const void *p_vector3) { - Vector2 normal_oct = RenderingServer::get_singleton()->norm_to_oct(*(Vector3 *)p_vector3); - int16_t v_normal[2] = { - (int16_t)CLAMP(normal_oct.x * 32767, -32768, 32767), - (int16_t)CLAMP(normal_oct.y * 32767, -32768, 32767), - }; - memcpy(&write_buffer[p_vertex_id * stride + offset_normal], v_normal, sizeof(uint16_t) * 2); -} - -void SoftBodyRenderingServerHandler::set_aabb(const AABB &p_aabb) { - RS::get_singleton()->mesh_set_custom_aabb(mesh, p_aabb); -} - -SoftBody::PinnedPoint::PinnedPoint() : - point_index(-1), - spatial_attachment(nullptr) { -} - -SoftBody::PinnedPoint::PinnedPoint(const PinnedPoint &obj_tocopy) { - point_index = obj_tocopy.point_index; - spatial_attachment_path = obj_tocopy.spatial_attachment_path; - spatial_attachment = obj_tocopy.spatial_attachment; - offset = obj_tocopy.offset; -} - -SoftBody::PinnedPoint SoftBody::PinnedPoint::operator=(const PinnedPoint &obj) { - point_index = obj.point_index; - spatial_attachment_path = obj.spatial_attachment_path; - spatial_attachment = obj.spatial_attachment; - offset = obj.offset; - return *this; -} - -void SoftBody::_update_pickable() { - if (!is_inside_tree()) { - return; - } - bool pickable = ray_pickable && is_visible_in_tree(); - PhysicsServer::get_singleton()->soft_body_set_ray_pickable(physics_rid, pickable); -} - -bool SoftBody::_set(const StringName &p_name, const Variant &p_value) { - String name = p_name; - String which = name.get_slicec('/', 0); - - if ("pinned_points" == which) { - return _set_property_pinned_points_indices(p_value); - - } else if ("attachments" == which) { - int idx = name.get_slicec('/', 1).to_int(); - String what = name.get_slicec('/', 2); - - return _set_property_pinned_points_attachment(idx, what, p_value); - } - - return false; -} - -bool SoftBody::_get(const StringName &p_name, Variant &r_ret) const { - String name = p_name; - String which = name.get_slicec('/', 0); - - if ("pinned_points" == which) { - Array arr_ret; - const int pinned_points_indices_size = pinned_points.size(); - PoolVector::Read r = pinned_points.read(); - arr_ret.resize(pinned_points_indices_size); - - for (int i = 0; i < pinned_points_indices_size; ++i) { - arr_ret[i] = r[i].point_index; - } - - r_ret = arr_ret; - return true; - - } else if ("attachments" == which) { - int idx = name.get_slicec('/', 1).to_int(); - String what = name.get_slicec('/', 2); - - return _get_property_pinned_points(idx, what, r_ret); - } - - return false; -} - -void SoftBody::_get_property_list(List *p_list) const { - const int pinned_points_indices_size = pinned_points.size(); - - p_list->push_back(PropertyInfo(Variant::POOL_INT_ARRAY, "pinned_points")); - - for (int i = 0; i < pinned_points_indices_size; ++i) { - p_list->push_back(PropertyInfo(Variant::INT, "attachments/" + itos(i) + "/point_index")); - p_list->push_back(PropertyInfo(Variant::NODE_PATH, "attachments/" + itos(i) + "/spatial_attachment_path")); - p_list->push_back(PropertyInfo(Variant::VECTOR3, "attachments/" + itos(i) + "/offset")); - } -} - -bool SoftBody::_set_property_pinned_points_indices(const Array &p_indices) { - const int p_indices_size = p_indices.size(); - - { // Remove the pined points on physics server that will be removed by resize - PoolVector::Read r = pinned_points.read(); - if (p_indices_size < pinned_points.size()) { - for (int i = pinned_points.size() - 1; i >= p_indices_size; --i) { - pin_point(r[i].point_index, false); - } - } - } - - pinned_points.resize(p_indices_size); - - PoolVector::Write w = pinned_points.write(); - int point_index; - for (int i = 0; i < p_indices_size; ++i) { - point_index = p_indices.get(i); - if (w[i].point_index != point_index) { - if (-1 != w[i].point_index) { - pin_point(w[i].point_index, false); - } - w[i].point_index = point_index; - pin_point(w[i].point_index, true); - } - } - return true; -} - -bool SoftBody::_set_property_pinned_points_attachment(int p_item, const String &p_what, const Variant &p_value) { - if (pinned_points.size() <= p_item) { - return false; - } - - if ("spatial_attachment_path" == p_what) { - PoolVector::Write w = pinned_points.write(); - pin_point(w[p_item].point_index, true, p_value); - _make_cache_dirty(); - } else if ("offset" == p_what) { - PoolVector::Write w = pinned_points.write(); - w[p_item].offset = p_value; - } else { - return false; - } - - return true; -} - -bool SoftBody::_get_property_pinned_points(int p_item, const String &p_what, Variant &r_ret) const { - if (pinned_points.size() <= p_item) { - return false; - } - PoolVector::Read r = pinned_points.read(); - - if ("point_index" == p_what) { - r_ret = r[p_item].point_index; - } else if ("spatial_attachment_path" == p_what) { - r_ret = r[p_item].spatial_attachment_path; - } else if ("offset" == p_what) { - r_ret = r[p_item].offset; - } else { - return false; - } - - return true; -} - -void SoftBody::_changed_callback(Object *p_changed, const char *p_prop) { - _prepare_physics_server(); - _reset_points_offsets(); -#ifdef TOOLS_ENABLED - if (p_changed == this) { - update_configuration_warning(); - } -#endif -} - -void SoftBody::_notification(int p_what) { - switch (p_what) { - case NOTIFICATION_ENTER_WORLD: { - if (Engine::get_singleton()->is_editor_hint()) { - add_change_receptor(this); - } - - RID space = get_world_3d()->get_space(); - PhysicsServer::get_singleton()->soft_body_set_space(physics_rid, space); - _prepare_physics_server(); - } break; - case NOTIFICATION_READY: { - if (!parent_collision_ignore.is_empty()) { - add_collision_exception_with(get_node(parent_collision_ignore)); - } - - } break; - case NOTIFICATION_TRANSFORM_CHANGED: { - if (Engine::get_singleton()->is_editor_hint()) { - _reset_points_offsets(); - return; - } - - PhysicsServer::get_singleton()->soft_body_set_transform(physics_rid, get_global_transform()); - - set_notify_transform(false); - // Required to be top level with Transform at center of world in order to modify RenderingServer only to support custom Transform - set_as_toplevel(true); - set_transform(Transform()); - set_notify_transform(true); - - } break; - case NOTIFICATION_VISIBILITY_CHANGED: { - _update_pickable(); - - } break; - case NOTIFICATION_EXIT_WORLD: { - PhysicsServer::get_singleton()->soft_body_set_space(physics_rid, RID()); - - } break; - } - -#ifdef TOOLS_ENABLED - - if (p_what == NOTIFICATION_LOCAL_TRANSFORM_CHANGED) { - if (Engine::get_singleton()->is_editor_hint()) { - update_configuration_warning(); - } - } - -#endif -} - -void SoftBody::_bind_methods() { - ClassDB::bind_method(D_METHOD("_draw_soft_mesh"), &SoftBody::_draw_soft_mesh); - - ClassDB::bind_method(D_METHOD("set_physics_enabled", "enabled"), &SoftBody::set_physics_enabled); - ClassDB::bind_method(D_METHOD("is_physics_enabled"), &SoftBody::is_physics_enabled); - - ClassDB::bind_method(D_METHOD("set_collision_mask", "collision_mask"), &SoftBody::set_collision_mask); - ClassDB::bind_method(D_METHOD("get_collision_mask"), &SoftBody::get_collision_mask); - - ClassDB::bind_method(D_METHOD("set_collision_layer", "collision_layer"), &SoftBody::set_collision_layer); - ClassDB::bind_method(D_METHOD("get_collision_layer"), &SoftBody::get_collision_layer); - - ClassDB::bind_method(D_METHOD("set_collision_mask_bit", "bit", "value"), &SoftBody::set_collision_mask_bit); - ClassDB::bind_method(D_METHOD("get_collision_mask_bit", "bit"), &SoftBody::get_collision_mask_bit); - - ClassDB::bind_method(D_METHOD("set_collision_layer_bit", "bit", "value"), &SoftBody::set_collision_layer_bit); - ClassDB::bind_method(D_METHOD("get_collision_layer_bit", "bit"), &SoftBody::get_collision_layer_bit); - - ClassDB::bind_method(D_METHOD("set_parent_collision_ignore", "parent_collision_ignore"), &SoftBody::set_parent_collision_ignore); - ClassDB::bind_method(D_METHOD("get_parent_collision_ignore"), &SoftBody::get_parent_collision_ignore); - - ClassDB::bind_method(D_METHOD("get_collision_exceptions"), &SoftBody::get_collision_exceptions); - ClassDB::bind_method(D_METHOD("add_collision_exception_with", "body"), &SoftBody::add_collision_exception_with); - ClassDB::bind_method(D_METHOD("remove_collision_exception_with", "body"), &SoftBody::remove_collision_exception_with); - - ClassDB::bind_method(D_METHOD("set_simulation_precision", "simulation_precision"), &SoftBody::set_simulation_precision); - ClassDB::bind_method(D_METHOD("get_simulation_precision"), &SoftBody::get_simulation_precision); - - ClassDB::bind_method(D_METHOD("set_total_mass", "mass"), &SoftBody::set_total_mass); - ClassDB::bind_method(D_METHOD("get_total_mass"), &SoftBody::get_total_mass); - - ClassDB::bind_method(D_METHOD("set_linear_stiffness", "linear_stiffness"), &SoftBody::set_linear_stiffness); - ClassDB::bind_method(D_METHOD("get_linear_stiffness"), &SoftBody::get_linear_stiffness); - - ClassDB::bind_method(D_METHOD("set_areaAngular_stiffness", "areaAngular_stiffness"), &SoftBody::set_areaAngular_stiffness); - ClassDB::bind_method(D_METHOD("get_areaAngular_stiffness"), &SoftBody::get_areaAngular_stiffness); - - ClassDB::bind_method(D_METHOD("set_volume_stiffness", "volume_stiffness"), &SoftBody::set_volume_stiffness); - ClassDB::bind_method(D_METHOD("get_volume_stiffness"), &SoftBody::get_volume_stiffness); - - ClassDB::bind_method(D_METHOD("set_pressure_coefficient", "pressure_coefficient"), &SoftBody::set_pressure_coefficient); - ClassDB::bind_method(D_METHOD("get_pressure_coefficient"), &SoftBody::get_pressure_coefficient); - - ClassDB::bind_method(D_METHOD("set_pose_matching_coefficient", "pose_matching_coefficient"), &SoftBody::set_pose_matching_coefficient); - ClassDB::bind_method(D_METHOD("get_pose_matching_coefficient"), &SoftBody::get_pose_matching_coefficient); - - ClassDB::bind_method(D_METHOD("set_damping_coefficient", "damping_coefficient"), &SoftBody::set_damping_coefficient); - ClassDB::bind_method(D_METHOD("get_damping_coefficient"), &SoftBody::get_damping_coefficient); - - ClassDB::bind_method(D_METHOD("set_drag_coefficient", "drag_coefficient"), &SoftBody::set_drag_coefficient); - ClassDB::bind_method(D_METHOD("get_drag_coefficient"), &SoftBody::get_drag_coefficient); - - ClassDB::bind_method(D_METHOD("get_point_transform", "point_index"), &SoftBody::get_point_transform); - - ClassDB::bind_method(D_METHOD("set_point_pinned", "point_index", "pinned", "attachment_path"), &SoftBody::pin_point, DEFVAL(NodePath())); - ClassDB::bind_method(D_METHOD("is_point_pinned", "point_index"), &SoftBody::is_point_pinned); - - ClassDB::bind_method(D_METHOD("set_ray_pickable", "ray_pickable"), &SoftBody::set_ray_pickable); - ClassDB::bind_method(D_METHOD("is_ray_pickable"), &SoftBody::is_ray_pickable); - - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "physics_enabled"), "set_physics_enabled", "is_physics_enabled"); - - ADD_GROUP("Collision", "collision_"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "collision_layer", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_collision_layer", "get_collision_layer"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "collision_mask", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_collision_mask", "get_collision_mask"); - - ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "parent_collision_ignore", PROPERTY_HINT_PROPERTY_OF_VARIANT_TYPE, "Parent collision object"), "set_parent_collision_ignore", "get_parent_collision_ignore"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "simulation_precision", PROPERTY_HINT_RANGE, "1,100,1"), "set_simulation_precision", "get_simulation_precision"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "total_mass", PROPERTY_HINT_RANGE, "0.01,10000,1"), "set_total_mass", "get_total_mass"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "linear_stiffness", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_linear_stiffness", "get_linear_stiffness"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "areaAngular_stiffness", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_areaAngular_stiffness", "get_areaAngular_stiffness"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "volume_stiffness", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_volume_stiffness", "get_volume_stiffness"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "pressure_coefficient"), "set_pressure_coefficient", "get_pressure_coefficient"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "damping_coefficient", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_damping_coefficient", "get_damping_coefficient"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "drag_coefficient", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_drag_coefficient", "get_drag_coefficient"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "pose_matching_coefficient", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_pose_matching_coefficient", "get_pose_matching_coefficient"); - - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "ray_pickable"), "set_ray_pickable", "is_ray_pickable"); -} - -String SoftBody::get_configuration_warning() const { - String warning = MeshInstance::get_configuration_warning(); - - if (mesh.is_null()) { - if (!warning.empty()) { - warning += "\n\n"; - } - - warning += TTR("This body will be ignored until you set a mesh."); - } - - Transform t = get_transform(); - if ((ABS(t.basis.get_axis(0).length() - 1.0) > 0.05 || ABS(t.basis.get_axis(1).length() - 1.0) > 0.05 || ABS(t.basis.get_axis(2).length() - 1.0) > 0.05)) { - if (!warning.empty()) { - warning += "\n\n"; - } - - warning += TTR("Size changes to SoftBody will be overridden by the physics engine when running.\nChange the size in children collision shapes instead."); - } - - return warning; -} - -void SoftBody::_update_physics_server() { - if (!simulation_started) { - return; - } - - _update_cache_pin_points_datas(); - // Submit bone attachment - const int pinned_points_indices_size = pinned_points.size(); - PoolVector::Read r = pinned_points.read(); - for (int i = 0; i < pinned_points_indices_size; ++i) { - if (r[i].spatial_attachment) { - PhysicsServer::get_singleton()->soft_body_move_point(physics_rid, r[i].point_index, r[i].spatial_attachment->get_global_transform().xform(r[i].offset)); - } - } -} - -void SoftBody::_draw_soft_mesh() { - if (mesh.is_null()) { - return; - } - - RID mesh_rid = mesh->get_rid(); - if (owned_mesh != mesh_rid) { - _become_mesh_owner(); - mesh_rid = mesh->get_rid(); - PhysicsServer::get_singleton()->soft_body_set_mesh(physics_rid, mesh); - } - - if (!rendering_server_handler.is_ready(mesh_rid)) { - rendering_server_handler.prepare(mesh_rid, 0); - - /// Necessary in order to render the mesh correctly (Soft body nodes are in global space) - simulation_started = true; - call_deferred("set_as_toplevel", true); - call_deferred("set_transform", Transform()); - } - - _update_physics_server(); - - rendering_server_handler.open(); - PhysicsServer::get_singleton()->soft_body_update_rendering_server(physics_rid, &rendering_server_handler); - rendering_server_handler.close(); - - rendering_server_handler.commit_changes(); -} - -void SoftBody::_prepare_physics_server() { - if (Engine::get_singleton()->is_editor_hint()) { - if (mesh.is_valid()) { - PhysicsServer::get_singleton()->soft_body_set_mesh(physics_rid, mesh); - } else { - PhysicsServer::get_singleton()->soft_body_set_mesh(physics_rid, nullptr); - } - - return; - } - - if (mesh.is_valid() && physics_enabled) { - if (owned_mesh != mesh->get_rid()) { - _become_mesh_owner(); - } - PhysicsServer::get_singleton()->soft_body_set_mesh(physics_rid, mesh); - RS::get_singleton()->connect("frame_pre_draw", this, "_draw_soft_mesh"); - } else { - PhysicsServer::get_singleton()->soft_body_set_mesh(physics_rid, nullptr); - if (RS::get_singleton()->is_connected("frame_pre_draw", this, "_draw_soft_mesh")) { - RS::get_singleton()->disconnect("frame_pre_draw", this, "_draw_soft_mesh"); - } - } -} - -void SoftBody::_become_mesh_owner() { - Vector> copy_materials; - copy_materials.append_array(materials); - - ERR_FAIL_COND(!mesh->get_surface_count()); - - // Get current mesh array and create new mesh array with necessary flag for softbody - Array surface_arrays = mesh->surface_get_arrays(0); - Array surface_blend_arrays = mesh->surface_get_blend_shape_arrays(0); - uint32_t surface_format = mesh->surface_get_format(0); - - surface_format &= ~(Mesh::ARRAY_COMPRESS_VERTEX | Mesh::ARRAY_COMPRESS_NORMAL); - surface_format |= Mesh::ARRAY_FLAG_USE_DYNAMIC_UPDATE; - - Ref soft_mesh; - soft_mesh.instance(); - soft_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, surface_arrays, surface_blend_arrays, surface_format); - soft_mesh->surface_set_material(0, mesh->surface_get_material(0)); - - set_mesh(soft_mesh); - - for (int i = copy_materials.size() - 1; 0 <= i; --i) { - set_surface_material(i, copy_materials[i]); - } - - owned_mesh = soft_mesh->get_rid(); -} - -void SoftBody::set_collision_mask(uint32_t p_mask) { - collision_mask = p_mask; - PhysicsServer::get_singleton()->soft_body_set_collision_mask(physics_rid, p_mask); -} - -uint32_t SoftBody::get_collision_mask() const { - return collision_mask; -} -void SoftBody::set_collision_layer(uint32_t p_layer) { - collision_layer = p_layer; - PhysicsServer::get_singleton()->soft_body_set_collision_layer(physics_rid, p_layer); -} - -uint32_t SoftBody::get_collision_layer() const { - return collision_layer; -} - -void SoftBody::set_collision_mask_bit(int p_bit, bool p_value) { - ERR_FAIL_INDEX_MSG(p_bit, 32, "Collision mask bit must be between 0 and 31 inclusive."); - uint32_t mask = get_collision_mask(); - if (p_value) { - mask |= 1 << p_bit; - } else { - mask &= ~(1 << p_bit); - } - set_collision_mask(mask); -} - -bool SoftBody::get_collision_mask_bit(int p_bit) const { - ERR_FAIL_INDEX_V_MSG(p_bit, 32, false, "Collision mask bit must be between 0 and 31 inclusive."); - return get_collision_mask() & (1 << p_bit); -} - -void SoftBody::set_collision_layer_bit(int p_bit, bool p_value) { - ERR_FAIL_INDEX_MSG(p_bit, 32, "Collision layer bit must be between 0 and 31 inclusive."); - uint32_t layer = get_collision_layer(); - if (p_value) { - layer |= 1 << p_bit; - } else { - layer &= ~(1 << p_bit); - } - set_collision_layer(layer); -} - -bool SoftBody::get_collision_layer_bit(int p_bit) const { - ERR_FAIL_INDEX_V_MSG(p_bit, 32, false, "Collision layer bit must be between 0 and 31 inclusive."); - return get_collision_layer() & (1 << p_bit); -} - -void SoftBody::set_parent_collision_ignore(const NodePath &p_parent_collision_ignore) { - parent_collision_ignore = p_parent_collision_ignore; -} - -const NodePath &SoftBody::get_parent_collision_ignore() const { - return parent_collision_ignore; -} - -void SoftBody::set_physics_enabled(bool p_enabled) { - if (p_enabled == physics_enabled) { - return; - } - - physics_enabled = p_enabled; - - if (is_inside_tree()) { - _prepare_physics_server(); - } -} - -bool SoftBody::is_physics_enabled() const { - return physics_enabled; -} - -void SoftBody::set_pinned_points_indices(PoolVector p_pinned_points_indices) { - pinned_points = p_pinned_points_indices; - PoolVector::Read w = pinned_points.read(); - for (int i = pinned_points.size() - 1; 0 <= i; --i) { - pin_point(p_pinned_points_indices[i].point_index, true); - } -} - -PoolVector SoftBody::get_pinned_points_indices() { - return pinned_points; -} - -Array SoftBody::get_collision_exceptions() { - List exceptions; - PhysicsServer::get_singleton()->soft_body_get_collision_exceptions(physics_rid, &exceptions); - Array ret; - for (List::Element *E = exceptions.front(); E; E = E->next()) { - RID body = E->get(); - ObjectID instance_id = PhysicsServer::get_singleton()->body_get_object_instance_id(body); - Object *obj = ObjectDB::get_instance(instance_id); - PhysicsBody *physics_body = Object::cast_to(obj); - ret.append(physics_body); - } - return ret; -} - -void SoftBody::add_collision_exception_with(Node *p_node) { - ERR_FAIL_NULL(p_node); - CollisionObject *collision_object = Object::cast_to(p_node); - ERR_FAIL_COND_MSG(!collision_object, "Collision exception only works between two nodes that inherit from CollisionObject (such as Area or PhysicsBody)."); - PhysicsServer::get_singleton()->soft_body_add_collision_exception(physics_rid, collision_object->get_rid()); -} - -void SoftBody::remove_collision_exception_with(Node *p_node) { - ERR_FAIL_NULL(p_node); - CollisionObject *collision_object = Object::cast_to(p_node); - ERR_FAIL_COND_MSG(!collision_object, "Collision exception only works between two nodes that inherit from CollisionObject (such as Area or PhysicsBody)."); - PhysicsServer::get_singleton()->soft_body_remove_collision_exception(physics_rid, collision_object->get_rid()); -} - -int SoftBody::get_simulation_precision() { - return PhysicsServer::get_singleton()->soft_body_get_simulation_precision(physics_rid); -} - -void SoftBody::set_simulation_precision(int p_simulation_precision) { - PhysicsServer::get_singleton()->soft_body_set_simulation_precision(physics_rid, p_simulation_precision); -} - -real_t SoftBody::get_total_mass() { - return PhysicsServer::get_singleton()->soft_body_get_total_mass(physics_rid); -} - -void SoftBody::set_total_mass(real_t p_total_mass) { - PhysicsServer::get_singleton()->soft_body_set_total_mass(physics_rid, p_total_mass); -} - -void SoftBody::set_linear_stiffness(real_t p_linear_stiffness) { - PhysicsServer::get_singleton()->soft_body_set_linear_stiffness(physics_rid, p_linear_stiffness); -} - -real_t SoftBody::get_linear_stiffness() { - return PhysicsServer::get_singleton()->soft_body_get_linear_stiffness(physics_rid); -} - -void SoftBody::set_areaAngular_stiffness(real_t p_areaAngular_stiffness) { - PhysicsServer::get_singleton()->soft_body_set_areaAngular_stiffness(physics_rid, p_areaAngular_stiffness); -} - -real_t SoftBody::get_areaAngular_stiffness() { - return PhysicsServer::get_singleton()->soft_body_get_areaAngular_stiffness(physics_rid); -} - -void SoftBody::set_volume_stiffness(real_t p_volume_stiffness) { - PhysicsServer::get_singleton()->soft_body_set_volume_stiffness(physics_rid, p_volume_stiffness); -} - -real_t SoftBody::get_volume_stiffness() { - return PhysicsServer::get_singleton()->soft_body_get_volume_stiffness(physics_rid); -} - -real_t SoftBody::get_pressure_coefficient() { - return PhysicsServer::get_singleton()->soft_body_get_pressure_coefficient(physics_rid); -} - -void SoftBody::set_pose_matching_coefficient(real_t p_pose_matching_coefficient) { - PhysicsServer::get_singleton()->soft_body_set_pose_matching_coefficient(physics_rid, p_pose_matching_coefficient); -} - -real_t SoftBody::get_pose_matching_coefficient() { - return PhysicsServer::get_singleton()->soft_body_get_pose_matching_coefficient(physics_rid); -} - -void SoftBody::set_pressure_coefficient(real_t p_pressure_coefficient) { - PhysicsServer::get_singleton()->soft_body_set_pressure_coefficient(physics_rid, p_pressure_coefficient); -} - -real_t SoftBody::get_damping_coefficient() { - return PhysicsServer::get_singleton()->soft_body_get_damping_coefficient(physics_rid); -} - -void SoftBody::set_damping_coefficient(real_t p_damping_coefficient) { - PhysicsServer::get_singleton()->soft_body_set_damping_coefficient(physics_rid, p_damping_coefficient); -} - -real_t SoftBody::get_drag_coefficient() { - return PhysicsServer::get_singleton()->soft_body_get_drag_coefficient(physics_rid); -} - -void SoftBody::set_drag_coefficient(real_t p_drag_coefficient) { - PhysicsServer::get_singleton()->soft_body_set_drag_coefficient(physics_rid, p_drag_coefficient); -} - -Vector3 SoftBody::get_point_transform(int p_point_index) { - return PhysicsServer::get_singleton()->soft_body_get_point_global_position(physics_rid, p_point_index); -} - -void SoftBody::pin_point_toggle(int p_point_index) { - pin_point(p_point_index, !(-1 != _has_pinned_point(p_point_index))); -} - -void SoftBody::pin_point(int p_point_index, bool pin, const NodePath &p_spatial_attachment_path) { - _pin_point_on_physics_server(p_point_index, pin); - if (pin) { - _add_pinned_point(p_point_index, p_spatial_attachment_path); - } else { - _remove_pinned_point(p_point_index); - } -} - -bool SoftBody::is_point_pinned(int p_point_index) const { - return -1 != _has_pinned_point(p_point_index); -} - -void SoftBody::set_ray_pickable(bool p_ray_pickable) { - ray_pickable = p_ray_pickable; - _update_pickable(); -} - -bool SoftBody::is_ray_pickable() const { - return ray_pickable; -} - -SoftBody::SoftBody() : - physics_rid(RID_PRIME(PhysicsServer::get_singleton()->soft_body_create())), - collision_mask(1), - collision_layer(1), - simulation_started(false), - pinned_points_cache_dirty(true), - ray_pickable(true) { - PhysicsServer::get_singleton()->body_attach_object_instance_id(physics_rid, get_instance_id()); -} - -SoftBody::~SoftBody() { - PhysicsServer::get_singleton()->free(physics_rid); -} - -void SoftBody::reset_softbody_pin() { - PhysicsServer::get_singleton()->soft_body_remove_all_pinned_points(physics_rid); - PoolVector::Read pps = pinned_points.read(); - for (int i = pinned_points.size() - 1; 0 < i; --i) { - PhysicsServer::get_singleton()->soft_body_pin_point(physics_rid, pps[i].point_index, true); - } -} - -void SoftBody::_make_cache_dirty() { - pinned_points_cache_dirty = true; -} - -void SoftBody::_update_cache_pin_points_datas() { - if (!pinned_points_cache_dirty) { - return; - } - - pinned_points_cache_dirty = false; - - PoolVector::Write w = pinned_points.write(); - for (int i = pinned_points.size() - 1; 0 <= i; --i) { - if (!w[i].spatial_attachment_path.is_empty()) { - w[i].spatial_attachment = Object::cast_to(get_node(w[i].spatial_attachment_path)); - } - if (!w[i].spatial_attachment) { - ERR_PRINT("Spatial node not defined in the pinned point, this is undefined behavior for SoftBody!"); - } - } -} - -void SoftBody::_pin_point_on_physics_server(int p_point_index, bool pin) { - PhysicsServer::get_singleton()->soft_body_pin_point(physics_rid, p_point_index, pin); -} - -void SoftBody::_add_pinned_point(int p_point_index, const NodePath &p_spatial_attachment_path) { - SoftBody::PinnedPoint *pinned_point; - if (-1 == _get_pinned_point(p_point_index, pinned_point)) { - // Create new - PinnedPoint pp; - pp.point_index = p_point_index; - pp.spatial_attachment_path = p_spatial_attachment_path; - - if (!p_spatial_attachment_path.is_empty() && has_node(p_spatial_attachment_path)) { - pp.spatial_attachment = Object::cast_to(get_node(p_spatial_attachment_path)); - pp.offset = (pp.spatial_attachment->get_global_transform().affine_inverse() * get_global_transform()).xform(PhysicsServer::get_singleton()->soft_body_get_point_global_position(physics_rid, pp.point_index)); - } - - pinned_points.push_back(pp); - - } else { - pinned_point->point_index = p_point_index; - pinned_point->spatial_attachment_path = p_spatial_attachment_path; - - if (!p_spatial_attachment_path.is_empty() && has_node(p_spatial_attachment_path)) { - pinned_point->spatial_attachment = Object::cast_to(get_node(p_spatial_attachment_path)); - pinned_point->offset = (pinned_point->spatial_attachment->get_global_transform().affine_inverse() * get_global_transform()).xform(PhysicsServer::get_singleton()->soft_body_get_point_global_position(physics_rid, pinned_point->point_index)); - } - } -} - -void SoftBody::_reset_points_offsets() { - if (!Engine::get_singleton()->is_editor_hint()) { - return; - } - - PoolVector::Read r = pinned_points.read(); - PoolVector::Write w = pinned_points.write(); - for (int i = pinned_points.size() - 1; 0 <= i; --i) { - if (!r[i].spatial_attachment) { - if (!r[i].spatial_attachment_path.is_empty() && has_node(r[i].spatial_attachment_path)) { - w[i].spatial_attachment = Object::cast_to(get_node(r[i].spatial_attachment_path)); - } - } - - if (!r[i].spatial_attachment) { - continue; - } - - w[i].offset = (r[i].spatial_attachment->get_global_transform().affine_inverse() * get_global_transform()).xform(PhysicsServer::get_singleton()->soft_body_get_point_global_position(physics_rid, r[i].point_index)); - } -} - -void SoftBody::_remove_pinned_point(int p_point_index) { - const int id(_has_pinned_point(p_point_index)); - if (-1 != id) { - pinned_points.remove(id); - } -} - -int SoftBody::_get_pinned_point(int p_point_index, SoftBody::PinnedPoint *&r_point) const { - const int id = _has_pinned_point(p_point_index); - if (-1 == id) { - r_point = nullptr; - return -1; - } else { - r_point = const_cast(&pinned_points.read()[id]); - return id; - } -} - -int SoftBody::_has_pinned_point(int p_point_index) const { - PoolVector::Read r = pinned_points.read(); - for (int i = pinned_points.size() - 1; 0 <= i; --i) { - if (p_point_index == r[i].point_index) { - return i; - } - } - return -1; -} diff --git a/scene/3d/soft_body.h b/scene/3d/soft_body.h deleted file mode 100644 index 90c9dd9..0000000 --- a/scene/3d/soft_body.h +++ /dev/null @@ -1,204 +0,0 @@ -#ifndef SOFT_PHYSICS_BODY_H -#define SOFT_PHYSICS_BODY_H -/*************************************************************************/ -/* soft_body.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "scene/3d/mesh_instance.h" - -class SoftBody; - -class SoftBodyRenderingServerHandler { - friend class SoftBody; - - RID mesh; - int surface; - PoolVector buffer; - uint32_t stride; - uint32_t offset_vertices; - uint32_t offset_normal; - - PoolVector::Write write_buffer; - -private: - SoftBodyRenderingServerHandler(); - bool is_ready(RID p_mesh_rid) const { return mesh.is_valid() && mesh == p_mesh_rid; } - void prepare(RID p_mesh_rid, int p_surface); - void clear(); - void open(); - void close(); - void commit_changes(); - -public: - void set_vertex(int p_vertex_id, const void *p_vector3); - void set_normal(int p_vertex_id, const void *p_vector3); - void set_aabb(const AABB &p_aabb); -}; - -class SoftBody : public MeshInstance { - GDCLASS(SoftBody, MeshInstance); - -public: - struct PinnedPoint { - int point_index; - NodePath spatial_attachment_path; - Spatial *spatial_attachment; // Cache - Vector3 offset; - - PinnedPoint(); - PinnedPoint(const PinnedPoint &obj_tocopy); - PinnedPoint operator=(const PinnedPoint &obj); - }; - -private: - SoftBodyRenderingServerHandler rendering_server_handler; - - RID physics_rid; - - bool physics_enabled = true; - - RID owned_mesh; - uint32_t collision_mask; - uint32_t collision_layer; - NodePath parent_collision_ignore; - PoolVector pinned_points; - bool simulation_started; - bool pinned_points_cache_dirty; - - Ref debug_mesh_cache; - class MeshInstance *debug_mesh; - - bool capture_input_on_drag; - bool ray_pickable; - - void _update_pickable(); - - void _update_physics_server(); - void _draw_soft_mesh(); - - void _prepare_physics_server(); - void _become_mesh_owner(); - -protected: - bool _set(const StringName &p_name, const Variant &p_value); - bool _get(const StringName &p_name, Variant &r_ret) const; - void _get_property_list(List *p_list) const; - - bool _set_property_pinned_points_indices(const Array &p_indices); - bool _set_property_pinned_points_attachment(int p_item, const String &p_what, const Variant &p_value); - bool _get_property_pinned_points(int p_item, const String &p_what, Variant &r_ret) const; - - virtual void _changed_callback(Object *p_changed, const char *p_prop); - - void _notification(int p_what); - static void _bind_methods(); - - virtual String get_configuration_warning() const; - -public: - void set_collision_mask(uint32_t p_mask); - uint32_t get_collision_mask() const; - - void set_collision_layer(uint32_t p_layer); - uint32_t get_collision_layer() const; - - void set_collision_mask_bit(int p_bit, bool p_value); - bool get_collision_mask_bit(int p_bit) const; - - void set_collision_layer_bit(int p_bit, bool p_value); - bool get_collision_layer_bit(int p_bit) const; - - void set_parent_collision_ignore(const NodePath &p_parent_collision_ignore); - const NodePath &get_parent_collision_ignore() const; - - void set_physics_enabled(bool p_enabled); - bool is_physics_enabled() const; - - void set_pinned_points_indices(PoolVector p_pinned_points_indices); - PoolVector get_pinned_points_indices(); - - void set_simulation_precision(int p_simulation_precision); - int get_simulation_precision(); - - void set_total_mass(real_t p_total_mass); - real_t get_total_mass(); - - void set_linear_stiffness(real_t p_linear_stiffness); - real_t get_linear_stiffness(); - - void set_areaAngular_stiffness(real_t p_areaAngular_stiffness); - real_t get_areaAngular_stiffness(); - - void set_volume_stiffness(real_t p_volume_stiffness); - real_t get_volume_stiffness(); - - void set_pressure_coefficient(real_t p_pressure_coefficient); - real_t get_pressure_coefficient(); - - void set_pose_matching_coefficient(real_t p_pose_matching_coefficient); - real_t get_pose_matching_coefficient(); - - void set_damping_coefficient(real_t p_damping_coefficient); - real_t get_damping_coefficient(); - - void set_drag_coefficient(real_t p_drag_coefficient); - real_t get_drag_coefficient(); - - Array get_collision_exceptions(); - void add_collision_exception_with(Node *p_node); - void remove_collision_exception_with(Node *p_node); - - Vector3 get_point_transform(int p_point_index); - - void pin_point_toggle(int p_point_index); - void pin_point(int p_point_index, bool pin, const NodePath &p_spatial_attachment_path = NodePath()); - bool is_point_pinned(int p_point_index) const; - - void set_ray_pickable(bool p_ray_pickable); - bool is_ray_pickable() const; - - SoftBody(); - ~SoftBody(); - -private: - void reset_softbody_pin(); - - void _make_cache_dirty(); - void _update_cache_pin_points_datas(); - - void _pin_point_on_physics_server(int p_point_index, bool pin); - void _add_pinned_point(int p_point_index, const NodePath &p_spatial_attachment_path); - void _reset_points_offsets(); - - void _remove_pinned_point(int p_point_index); - int _get_pinned_point(int p_point_index, PinnedPoint *&r_point) const; - int _has_pinned_point(int p_point_index) const; -}; - -#endif // SOFT_PHYSICS_BODY_H diff --git a/scene/3d/spring_arm.cpp b/scene/3d/spring_arm.cpp deleted file mode 100644 index b04edc3..0000000 --- a/scene/3d/spring_arm.cpp +++ /dev/null @@ -1,174 +0,0 @@ -/*************************************************************************/ -/* spring_arm.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "spring_arm.h" -#include "core/config/engine.h" -#include "scene/3d/collision_object.h" -#include "scene/resources/shapes/shape.h" -#include "scene/resources/shapes/sphere_shape.h" -#include "scene/resources/world_3d.h" -#include "servers/physics_server.h" - -SpringArm::SpringArm() : - spring_length(1), - current_spring_length(0), - keep_child_basis(false), - mask(1), - margin(0.01) {} - -void SpringArm::_notification(int p_what) { - switch (p_what) { - case NOTIFICATION_ENTER_TREE: - if (!Engine::get_singleton()->is_editor_hint()) { - set_physics_process_internal(true); - } - break; - case NOTIFICATION_EXIT_TREE: - if (!Engine::get_singleton()->is_editor_hint()) { - set_physics_process_internal(false); - } - break; - case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: - process_spring(); - break; - } -} - -void SpringArm::_bind_methods() { - ClassDB::bind_method(D_METHOD("get_hit_length"), &SpringArm::get_hit_length); - - ClassDB::bind_method(D_METHOD("set_length", "length"), &SpringArm::set_length); - ClassDB::bind_method(D_METHOD("get_length"), &SpringArm::get_length); - - ClassDB::bind_method(D_METHOD("set_shape", "shape"), &SpringArm::set_shape); - ClassDB::bind_method(D_METHOD("get_shape"), &SpringArm::get_shape); - - ClassDB::bind_method(D_METHOD("add_excluded_object", "RID"), &SpringArm::add_excluded_object); - ClassDB::bind_method(D_METHOD("remove_excluded_object", "RID"), &SpringArm::remove_excluded_object); - ClassDB::bind_method(D_METHOD("clear_excluded_objects"), &SpringArm::clear_excluded_objects); - - ClassDB::bind_method(D_METHOD("set_collision_mask", "mask"), &SpringArm::set_mask); - ClassDB::bind_method(D_METHOD("get_collision_mask"), &SpringArm::get_mask); - - ClassDB::bind_method(D_METHOD("set_margin", "margin"), &SpringArm::set_margin); - ClassDB::bind_method(D_METHOD("get_margin"), &SpringArm::get_margin); - - ADD_PROPERTY(PropertyInfo(Variant::INT, "collision_mask", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_collision_mask", "get_collision_mask"); - ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "shape", PROPERTY_HINT_RESOURCE_TYPE, "Shape"), "set_shape", "get_shape"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "spring_length"), "set_length", "get_length"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "margin"), "set_margin", "get_margin"); -} - -float SpringArm::get_length() const { - return spring_length; -} - -void SpringArm::set_length(float p_length) { - if (is_inside_tree() && (Engine::get_singleton()->is_editor_hint() || get_tree()->is_debugging_collisions_hint())) { - update_gizmos(); - } - - spring_length = p_length; -} - -void SpringArm::set_shape(Ref p_shape) { - shape = p_shape; -} - -Ref SpringArm::get_shape() const { - return shape; -} - -void SpringArm::set_mask(uint32_t p_mask) { - mask = p_mask; -} - -uint32_t SpringArm::get_mask() { - return mask; -} - -float SpringArm::get_margin() { - return margin; -} - -void SpringArm::set_margin(float p_margin) { - margin = p_margin; -} - -void SpringArm::add_excluded_object(RID p_rid) { - excluded_objects.insert(p_rid); -} - -bool SpringArm::remove_excluded_object(RID p_rid) { - return excluded_objects.erase(p_rid); -} - -void SpringArm::clear_excluded_objects() { - excluded_objects.clear(); -} - -float SpringArm::get_hit_length() { - return current_spring_length; -} - -void SpringArm::process_spring() { - // From - real_t motion_delta(1); - real_t motion_delta_unsafe(1); - - Vector3 motion; - const Vector3 cast_direction(get_global_transform().basis.xform(Vector3(0, 0, 1))); - - if (shape.is_null()) { - motion = Vector3(cast_direction * (spring_length)); - PhysicsDirectSpaceState::RayResult r; - bool intersected = get_world_3d()->get_direct_space_state()->intersect_ray(get_global_transform().origin, get_global_transform().origin + motion, r, excluded_objects, mask); - if (intersected) { - float dist = get_global_transform().origin.distance_to(r.position); - dist -= margin; - motion_delta = dist / (spring_length); - } - } else { - motion = Vector3(cast_direction * spring_length); - get_world_3d()->get_direct_space_state()->cast_motion(shape->get_rid(), get_global_transform(), motion, 0, motion_delta, motion_delta_unsafe, excluded_objects, mask); - } - - current_spring_length = spring_length * motion_delta; - Transform childs_transform; - childs_transform.origin = get_global_transform().origin + cast_direction * (spring_length * motion_delta); - - for (int i = get_child_count() - 1; 0 <= i; --i) { - Spatial *child = Object::cast_to(get_child(i)); - if (child) { - childs_transform.basis = child->get_global_transform().basis; - child->set_global_transform(childs_transform); - } - } -} diff --git a/scene/3d/spring_arm.h b/scene/3d/spring_arm.h deleted file mode 100644 index 71cb1cc..0000000 --- a/scene/3d/spring_arm.h +++ /dev/null @@ -1,73 +0,0 @@ -#ifndef SPRING_ARM_H -#define SPRING_ARM_H -/*************************************************************************/ -/* spring_arm.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "core/object/reference.h" -#include "scene/main/spatial.h" - -class Shape; - -class SpringArm : public Spatial { - GDCLASS(SpringArm, Spatial); - - Ref shape; - RBSet excluded_objects; - float spring_length; - float current_spring_length; - bool keep_child_basis; - uint32_t mask; - float margin; - -protected: - void _notification(int p_what); - static void _bind_methods(); - -public: - void set_length(float p_length); - float get_length() const; - void set_shape(Ref p_shape); - Ref get_shape() const; - void set_mask(uint32_t p_mask); - uint32_t get_mask(); - void add_excluded_object(RID p_rid); - bool remove_excluded_object(RID p_rid); - void clear_excluded_objects(); - float get_hit_length(); - void set_margin(float p_margin); - float get_margin(); - - SpringArm(); - -private: - void process_spring(); -}; - -#endif diff --git a/scene/3d/vehicle_body.cpp b/scene/3d/vehicle_body.cpp deleted file mode 100644 index 11e3215..0000000 --- a/scene/3d/vehicle_body.cpp +++ /dev/null @@ -1,965 +0,0 @@ -/*************************************************************************/ -/* vehicle_body.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "vehicle_body.h" - -#include "scene/resources/shapes/shape.h" - -#define ROLLING_INFLUENCE_FIX - -class btVehicleJacobianEntry { -public: - 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; - - real_t getDiagonal() const { return m_Adiag; } - - btVehicleJacobianEntry(){}; - //constraint between two different rigidbodies - btVehicleJacobianEntry( - 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); - - //btAssert(m_Adiag > real_t(0.0)); - } - - 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; - } -}; - -void VehicleWheel::_notification(int p_what) { - if (p_what == NOTIFICATION_ENTER_TREE) { - VehicleBody *cb = Object::cast_to(get_parent()); - if (!cb) { - return; - } - body = cb; - local_xform = get_transform(); - cb->wheels.push_back(this); - - m_chassisConnectionPointCS = get_transform().origin; - m_wheelDirectionCS = -get_transform().basis.get_axis(Vector3::AXIS_Y).normalized(); - m_wheelAxleCS = get_transform().basis.get_axis(Vector3::AXIS_X).normalized(); - } - if (p_what == NOTIFICATION_EXIT_TREE) { - VehicleBody *cb = Object::cast_to(get_parent()); - if (!cb) { - return; - } - cb->wheels.erase(this); - body = nullptr; - } -} - -String VehicleWheel::get_configuration_warning() const { - String warning = Spatial::get_configuration_warning(); - if (!Object::cast_to(get_parent())) { - if (warning != String()) { - warning += "\n\n"; - } - warning += TTR("VehicleWheel serves to provide a wheel system to a VehicleBody. Please use it as a child of a VehicleBody."); - } - - return warning; -} - -void VehicleWheel::_update(PhysicsDirectBodyState *s) { - if (m_raycastInfo.m_isInContact) - - { - real_t project = m_raycastInfo.m_contactNormalWS.dot(m_raycastInfo.m_wheelDirectionWS); - Vector3 chassis_velocity_at_contactPoint; - Vector3 relpos = m_raycastInfo.m_contactPointWS - s->get_transform().origin; - - chassis_velocity_at_contactPoint = s->get_linear_velocity() + - (s->get_angular_velocity()).cross(relpos); // * mPos); - - real_t projVel = m_raycastInfo.m_contactNormalWS.dot(chassis_velocity_at_contactPoint); - if (project >= real_t(-0.1)) { - m_suspensionRelativeVelocity = real_t(0.0); - m_clippedInvContactDotSuspension = real_t(1.0) / real_t(0.1); - } else { - real_t inv = real_t(-1.) / project; - m_suspensionRelativeVelocity = projVel * inv; - m_clippedInvContactDotSuspension = inv; - } - - } - - else // Not in contact : position wheel in a nice (rest length) position - { - m_raycastInfo.m_suspensionLength = m_suspensionRestLength; - m_suspensionRelativeVelocity = real_t(0.0); - m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; - m_clippedInvContactDotSuspension = real_t(1.0); - } -} - -void VehicleWheel::set_radius(float p_radius) { - m_wheelRadius = p_radius; - update_gizmos(); -} - -float VehicleWheel::get_radius() const { - return m_wheelRadius; -} - -void VehicleWheel::set_suspension_rest_length(float p_length) { - m_suspensionRestLength = p_length; - update_gizmos(); -} -float VehicleWheel::get_suspension_rest_length() const { - return m_suspensionRestLength; -} - -void VehicleWheel::set_suspension_travel(float p_length) { - m_maxSuspensionTravelCm = p_length / 0.01; -} -float VehicleWheel::get_suspension_travel() const { - return m_maxSuspensionTravelCm * 0.01; -} - -void VehicleWheel::set_suspension_stiffness(float p_value) { - m_suspensionStiffness = p_value; -} -float VehicleWheel::get_suspension_stiffness() const { - return m_suspensionStiffness; -} - -void VehicleWheel::set_suspension_max_force(float p_value) { - m_maxSuspensionForce = p_value; -} -float VehicleWheel::get_suspension_max_force() const { - return m_maxSuspensionForce; -} - -void VehicleWheel::set_damping_compression(float p_value) { - m_wheelsDampingCompression = p_value; -} -float VehicleWheel::get_damping_compression() const { - return m_wheelsDampingCompression; -} - -void VehicleWheel::set_damping_relaxation(float p_value) { - m_wheelsDampingRelaxation = p_value; -} -float VehicleWheel::get_damping_relaxation() const { - return m_wheelsDampingRelaxation; -} - -void VehicleWheel::set_friction_slip(float p_value) { - m_frictionSlip = p_value; -} -float VehicleWheel::get_friction_slip() const { - return m_frictionSlip; -} - -void VehicleWheel::set_roll_influence(float p_value) { - m_rollInfluence = p_value; -} - -float VehicleWheel::get_roll_influence() const { - return m_rollInfluence; -} - -bool VehicleWheel::is_in_contact() const { - return m_raycastInfo.m_isInContact; -} - -Spatial *VehicleWheel::get_contact_body() const { - return m_raycastInfo.m_groundObject; -} - -void VehicleWheel::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_radius", "length"), &VehicleWheel::set_radius); - ClassDB::bind_method(D_METHOD("get_radius"), &VehicleWheel::get_radius); - - ClassDB::bind_method(D_METHOD("set_suspension_rest_length", "length"), &VehicleWheel::set_suspension_rest_length); - ClassDB::bind_method(D_METHOD("get_suspension_rest_length"), &VehicleWheel::get_suspension_rest_length); - - ClassDB::bind_method(D_METHOD("set_suspension_travel", "length"), &VehicleWheel::set_suspension_travel); - ClassDB::bind_method(D_METHOD("get_suspension_travel"), &VehicleWheel::get_suspension_travel); - - ClassDB::bind_method(D_METHOD("set_suspension_stiffness", "length"), &VehicleWheel::set_suspension_stiffness); - ClassDB::bind_method(D_METHOD("get_suspension_stiffness"), &VehicleWheel::get_suspension_stiffness); - - ClassDB::bind_method(D_METHOD("set_suspension_max_force", "length"), &VehicleWheel::set_suspension_max_force); - ClassDB::bind_method(D_METHOD("get_suspension_max_force"), &VehicleWheel::get_suspension_max_force); - - ClassDB::bind_method(D_METHOD("set_damping_compression", "length"), &VehicleWheel::set_damping_compression); - ClassDB::bind_method(D_METHOD("get_damping_compression"), &VehicleWheel::get_damping_compression); - - ClassDB::bind_method(D_METHOD("set_damping_relaxation", "length"), &VehicleWheel::set_damping_relaxation); - ClassDB::bind_method(D_METHOD("get_damping_relaxation"), &VehicleWheel::get_damping_relaxation); - - ClassDB::bind_method(D_METHOD("set_use_as_traction", "enable"), &VehicleWheel::set_use_as_traction); - ClassDB::bind_method(D_METHOD("is_used_as_traction"), &VehicleWheel::is_used_as_traction); - - ClassDB::bind_method(D_METHOD("set_use_as_steering", "enable"), &VehicleWheel::set_use_as_steering); - ClassDB::bind_method(D_METHOD("is_used_as_steering"), &VehicleWheel::is_used_as_steering); - - ClassDB::bind_method(D_METHOD("set_friction_slip", "length"), &VehicleWheel::set_friction_slip); - ClassDB::bind_method(D_METHOD("get_friction_slip"), &VehicleWheel::get_friction_slip); - - ClassDB::bind_method(D_METHOD("is_in_contact"), &VehicleWheel::is_in_contact); - ClassDB::bind_method(D_METHOD("get_contact_body"), &VehicleWheel::get_contact_body); - - ClassDB::bind_method(D_METHOD("set_roll_influence", "roll_influence"), &VehicleWheel::set_roll_influence); - ClassDB::bind_method(D_METHOD("get_roll_influence"), &VehicleWheel::get_roll_influence); - - ClassDB::bind_method(D_METHOD("get_skidinfo"), &VehicleWheel::get_skidinfo); - - ClassDB::bind_method(D_METHOD("get_rpm"), &VehicleWheel::get_rpm); - - ClassDB::bind_method(D_METHOD("set_engine_force", "engine_force"), &VehicleWheel::set_engine_force); - ClassDB::bind_method(D_METHOD("get_engine_force"), &VehicleWheel::get_engine_force); - - ClassDB::bind_method(D_METHOD("set_brake", "brake"), &VehicleWheel::set_brake); - ClassDB::bind_method(D_METHOD("get_brake"), &VehicleWheel::get_brake); - - ClassDB::bind_method(D_METHOD("set_steering", "steering"), &VehicleWheel::set_steering); - ClassDB::bind_method(D_METHOD("get_steering"), &VehicleWheel::get_steering); - - ADD_GROUP("Per-Wheel Motion", ""); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "engine_force", PROPERTY_HINT_RANGE, "-1024,1024.0,0.01,or_greater"), "set_engine_force", "get_engine_force"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "brake", PROPERTY_HINT_RANGE, "0.0,1.0,0.01"), "set_brake", "get_brake"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "steering", PROPERTY_HINT_RANGE, "-180,180.0,0.01"), "set_steering", "get_steering"); - ADD_GROUP("VehicleBody Motion", ""); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "use_as_traction"), "set_use_as_traction", "is_used_as_traction"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "use_as_steering"), "set_use_as_steering", "is_used_as_steering"); - ADD_GROUP("Wheel", "wheel_"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "wheel_roll_influence"), "set_roll_influence", "get_roll_influence"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "wheel_radius"), "set_radius", "get_radius"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "wheel_rest_length"), "set_suspension_rest_length", "get_suspension_rest_length"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "wheel_friction_slip"), "set_friction_slip", "get_friction_slip"); - ADD_GROUP("Suspension", "suspension_"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "suspension_travel"), "set_suspension_travel", "get_suspension_travel"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "suspension_stiffness"), "set_suspension_stiffness", "get_suspension_stiffness"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "suspension_max_force"), "set_suspension_max_force", "get_suspension_max_force"); - ADD_GROUP("Damping", "damping_"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "damping_compression"), "set_damping_compression", "get_damping_compression"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "damping_relaxation"), "set_damping_relaxation", "get_damping_relaxation"); -} - -void VehicleWheel::set_engine_force(float p_engine_force) { - m_engineForce = p_engine_force; -} - -float VehicleWheel::get_engine_force() const { - return m_engineForce; -} - -void VehicleWheel::set_brake(float p_brake) { - m_brake = p_brake; -} -float VehicleWheel::get_brake() const { - return m_brake; -} - -void VehicleWheel::set_steering(float p_steering) { - m_steering = p_steering; -} -float VehicleWheel::get_steering() const { - return m_steering; -} - -void VehicleWheel::set_use_as_traction(bool p_enable) { - engine_traction = p_enable; -} - -bool VehicleWheel::is_used_as_traction() const { - return engine_traction; -} - -void VehicleWheel::set_use_as_steering(bool p_enabled) { - steers = p_enabled; -} - -bool VehicleWheel::is_used_as_steering() const { - return steers; -} - -float VehicleWheel::get_skidinfo() const { - return m_skidInfo; -} - -float VehicleWheel::get_rpm() const { - return m_rpm; -} - -VehicleWheel::VehicleWheel() { - steers = false; - engine_traction = false; - - m_steering = real_t(0.); - m_engineForce = real_t(0.); - m_rotation = real_t(0.); - m_deltaRotation = real_t(0.); - m_brake = real_t(0.); - m_rollInfluence = real_t(0.1); - - m_suspensionRestLength = 0.15; - m_wheelRadius = 0.5; //0.28; - m_suspensionStiffness = 5.88; - m_wheelsDampingCompression = 0.83; - m_wheelsDampingRelaxation = 0.88; - m_frictionSlip = 10.5; - m_bIsFrontWheel = false; - m_maxSuspensionTravelCm = 500; - m_maxSuspensionForce = 6000; - - m_suspensionRelativeVelocity = 0; - m_clippedInvContactDotSuspension = 1.0; - m_raycastInfo.m_isInContact = false; - m_raycastInfo.m_groundObject = nullptr; - m_raycastInfo.m_suspensionLength = 0.0; - - body = nullptr; -} - -void VehicleBody::_update_wheel_transform(VehicleWheel &wheel, PhysicsDirectBodyState *s) { - wheel.m_raycastInfo.m_isInContact = false; - - Transform chassisTrans = s->get_transform(); - /* - if (interpolatedTransform && (getRigidBody()->getMotionState())) { - getRigidBody()->getMotionState()->getWorldTransform(chassisTrans); - } - */ - - wheel.m_raycastInfo.m_hardPointWS = chassisTrans.xform(wheel.m_chassisConnectionPointCS); - //wheel.m_raycastInfo.m_hardPointWS+=s->get_linear_velocity()*s->get_step(); - wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.get_basis().xform(wheel.m_wheelDirectionCS).normalized(); - wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.get_basis().xform(wheel.m_wheelAxleCS).normalized(); -} - -void VehicleBody::_update_wheel(int p_idx, PhysicsDirectBodyState *s) { - VehicleWheel &wheel = *wheels[p_idx]; - _update_wheel_transform(wheel, s); - - Vector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS; - const Vector3 &right = wheel.m_raycastInfo.m_wheelAxleWS; - Vector3 fwd = up.cross(right); - fwd = fwd.normalized(); - - Basis steeringMat(up, wheel.m_steering); - - Basis rotatingMat(right, wheel.m_rotation); - - Basis basis2( - right[0], up[0], fwd[0], - right[1], up[1], fwd[1], - right[2], up[2], fwd[2]); - - wheel.m_worldTransform.set_basis(steeringMat * rotatingMat * basis2); - //wheel.m_worldTransform.set_basis(basis2 * (steeringMat * rotatingMat)); - wheel.m_worldTransform.set_origin( - wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength); -} - -real_t VehicleBody::_ray_cast(int p_idx, PhysicsDirectBodyState *s) { - VehicleWheel &wheel = *wheels[p_idx]; - - _update_wheel_transform(wheel, s); - - real_t depth = -1; - - real_t raylen = wheel.m_suspensionRestLength + wheel.m_wheelRadius; - - Vector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen); - Vector3 source = wheel.m_raycastInfo.m_hardPointWS; - wheel.m_raycastInfo.m_contactPointWS = source + rayvector; - const Vector3 &target = wheel.m_raycastInfo.m_contactPointWS; - source -= wheel.m_wheelRadius * wheel.m_raycastInfo.m_wheelDirectionWS; - - real_t param = real_t(0.); - - PhysicsDirectSpaceState::RayResult rr; - - PhysicsDirectSpaceState *ss = s->get_space_state(); - - wheel.m_raycastInfo.m_groundObject = nullptr; - bool col = ss->intersect_ray(source, target, rr, exclude, get_collision_mask()); - - if (col) { - param = source.distance_to(rr.position) / source.distance_to(target); - depth = raylen * param; - wheel.m_raycastInfo.m_contactNormalWS = rr.normal; - - wheel.m_raycastInfo.m_isInContact = true; - if (rr.collider) { - wheel.m_raycastInfo.m_groundObject = Object::cast_to(rr.collider); - } - - real_t hitDistance = param * raylen; - wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelRadius; - //clamp on max suspension travel - - real_t minSuspensionLength = wheel.m_suspensionRestLength - wheel.m_maxSuspensionTravelCm * real_t(0.01); - real_t maxSuspensionLength = wheel.m_suspensionRestLength + wheel.m_maxSuspensionTravelCm * real_t(0.01); - if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength) { - wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength; - } - if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength) { - wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength; - } - - wheel.m_raycastInfo.m_contactPointWS = rr.position; - - real_t denominator = wheel.m_raycastInfo.m_contactNormalWS.dot(wheel.m_raycastInfo.m_wheelDirectionWS); - - Vector3 chassis_velocity_at_contactPoint; - //Vector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition(); - - //chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos); - - chassis_velocity_at_contactPoint = s->get_linear_velocity() + - (s->get_angular_velocity()).cross(wheel.m_raycastInfo.m_contactPointWS - s->get_transform().origin); // * mPos); - - real_t projVel = wheel.m_raycastInfo.m_contactNormalWS.dot(chassis_velocity_at_contactPoint); - - if (denominator >= real_t(-0.1)) { - wheel.m_suspensionRelativeVelocity = real_t(0.0); - wheel.m_clippedInvContactDotSuspension = real_t(1.0) / real_t(0.1); - } else { - real_t inv = real_t(-1.) / denominator; - wheel.m_suspensionRelativeVelocity = projVel * inv; - wheel.m_clippedInvContactDotSuspension = inv; - } - - } else { - wheel.m_raycastInfo.m_isInContact = false; - //put wheel info as in rest position - wheel.m_raycastInfo.m_suspensionLength = wheel.m_suspensionRestLength; - wheel.m_suspensionRelativeVelocity = real_t(0.0); - wheel.m_raycastInfo.m_contactNormalWS = -wheel.m_raycastInfo.m_wheelDirectionWS; - wheel.m_clippedInvContactDotSuspension = real_t(1.0); - } - - return depth; -} - -void VehicleBody::_update_suspension(PhysicsDirectBodyState *s) { - real_t chassisMass = mass; - - for (int w_it = 0; w_it < wheels.size(); w_it++) { - VehicleWheel &wheel_info = *wheels[w_it]; - - if (wheel_info.m_raycastInfo.m_isInContact) { - real_t force; - //Spring - { - real_t susp_length = wheel_info.m_suspensionRestLength; - real_t current_length = wheel_info.m_raycastInfo.m_suspensionLength; - - real_t length_diff = (susp_length - current_length); - - force = wheel_info.m_suspensionStiffness * length_diff * wheel_info.m_clippedInvContactDotSuspension; - } - - // Damper - { - real_t projected_rel_vel = wheel_info.m_suspensionRelativeVelocity; - { - real_t susp_damping; - if (projected_rel_vel < real_t(0.0)) { - susp_damping = wheel_info.m_wheelsDampingCompression; - } else { - susp_damping = wheel_info.m_wheelsDampingRelaxation; - } - force -= susp_damping * projected_rel_vel; - } - } - - // RESULT - wheel_info.m_wheelsSuspensionForce = force * chassisMass; - if (wheel_info.m_wheelsSuspensionForce < real_t(0.)) { - wheel_info.m_wheelsSuspensionForce = real_t(0.); - } - } else { - wheel_info.m_wheelsSuspensionForce = real_t(0.0); - } - } -} - -//bilateral constraint between two dynamic objects -void VehicleBody::_resolve_single_bilateral(PhysicsDirectBodyState *s, const Vector3 &pos1, - PhysicsBody *body2, const Vector3 &pos2, const Vector3 &normal, real_t &impulse, const real_t p_rollInfluence) { - real_t normalLenSqr = normal.length_squared(); - //ERR_FAIL_COND( normalLenSqr < real_t(1.1)); - - if (normalLenSqr > real_t(1.1)) { - impulse = real_t(0.); - return; - } - - Vector3 rel_pos1 = pos1 - s->get_transform().origin; - Vector3 rel_pos2; - if (body2) { - rel_pos2 = pos2 - body2->get_global_transform().origin; - } - //this jacobian entry could be re-used for all iterations - - Vector3 vel1 = s->get_linear_velocity() + (s->get_angular_velocity()).cross(rel_pos1); // * mPos); - Vector3 vel2; - - if (body2) { - vel2 = body2->get_linear_velocity() + body2->get_angular_velocity().cross(rel_pos2); - } - - Vector3 vel = vel1 - vel2; - - Basis b2trans; - float b2invmass = 0; - Vector3 b2lv; - Vector3 b2av; - Vector3 b2invinertia; //todo - - if (body2) { - b2trans = body2->get_global_transform().basis.transposed(); - b2invmass = body2->get_inverse_mass(); - b2lv = body2->get_linear_velocity(); - b2av = body2->get_angular_velocity(); - } - - btVehicleJacobianEntry jac(s->get_transform().basis.transposed(), - b2trans, - rel_pos1, - rel_pos2, - normal, - s->get_inverse_inertia_tensor().get_main_diagonal(), - 1.0 / mass, - b2invinertia, - b2invmass); - - // FIXME: rel_vel assignment here is overwritten by the following assignment. - // What seems to be intended in the next next assignment is: rel_vel = normal.dot(rel_vel); - // Investigate why. - real_t rel_vel = jac.getRelativeVelocity( - s->get_linear_velocity(), - s->get_transform().basis.transposed().xform(s->get_angular_velocity()), - b2lv, - b2trans.xform(b2av)); - - rel_vel = normal.dot(vel); - - // !BAS! We had this set to 0.4, in bullet its 0.2 - real_t contactDamping = real_t(0.2); - - if (p_rollInfluence > 0.0) { - // !BAS! But seeing we apply this frame by frame, makes more sense to me to make this time based - // keeping in mind our anti roll factor if it is set - contactDamping = MIN(contactDamping, s->get_step() / p_rollInfluence); - } - -#define ONLY_USE_LINEAR_MASS -#ifdef ONLY_USE_LINEAR_MASS - real_t massTerm = real_t(1.) / ((1.0 / mass) + b2invmass); - impulse = -contactDamping * rel_vel * massTerm; -#else - real_t velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; - impulse = velocityImpulse; -#endif -} - -VehicleBody::btVehicleWheelContactPoint::btVehicleWheelContactPoint(PhysicsDirectBodyState *s, PhysicsBody *body1, const Vector3 &frictionPosWorld, const Vector3 &frictionDirectionWorld, real_t maxImpulse) : - m_s(s), - m_body1(body1), - m_frictionPositionWorld(frictionPosWorld), - m_frictionDirectionWorld(frictionDirectionWorld), - m_maxImpulse(maxImpulse) { - float denom0 = 0; - float denom1 = 0; - - { - Vector3 r0 = frictionPosWorld - s->get_transform().origin; - Vector3 c0 = (r0).cross(frictionDirectionWorld); - Vector3 vec = s->get_inverse_inertia_tensor().xform_inv(c0).cross(r0); - denom0 = s->get_inverse_mass() + frictionDirectionWorld.dot(vec); - } - - /* TODO: Why is this code unused? - if (body1) { - - Vector3 r0 = frictionPosWorld - body1->get_global_transform().origin; - Vector3 c0 = (r0).cross(frictionDirectionWorld); - Vector3 vec = s->get_inverse_inertia_tensor().xform_inv(c0).cross(r0); - //denom1= body1->get_inverse_mass() + frictionDirectionWorld.dot(vec); - - } - */ - - real_t relaxation = 1.f; - m_jacDiagABInv = relaxation / (denom0 + denom1); -} - -real_t VehicleBody::_calc_rolling_friction(btVehicleWheelContactPoint &contactPoint) { - real_t j1 = 0.f; - - const Vector3 &contactPosWorld = contactPoint.m_frictionPositionWorld; - - Vector3 rel_pos1 = contactPosWorld - contactPoint.m_s->get_transform().origin; - Vector3 rel_pos2; - if (contactPoint.m_body1) { - rel_pos2 = contactPosWorld - contactPoint.m_body1->get_global_transform().origin; - } - - real_t maxImpulse = contactPoint.m_maxImpulse; - - Vector3 vel1 = contactPoint.m_s->get_linear_velocity() + (contactPoint.m_s->get_angular_velocity()).cross(rel_pos1); // * mPos); - - Vector3 vel2; - if (contactPoint.m_body1) { - vel2 = contactPoint.m_body1->get_linear_velocity() + contactPoint.m_body1->get_angular_velocity().cross(rel_pos2); - } - - Vector3 vel = vel1 - vel2; - - real_t vrel = contactPoint.m_frictionDirectionWorld.dot(vel); - - // calculate j that moves us to zero relative velocity - j1 = -vrel * contactPoint.m_jacDiagABInv; - - return CLAMP(j1, -maxImpulse, maxImpulse); -} - -static const real_t sideFrictionStiffness2 = real_t(1.0); -void VehicleBody::_update_friction(PhysicsDirectBodyState *s) { - //calculate the impulse, so that the wheels don't move sidewards - int numWheel = wheels.size(); - if (!numWheel) { - return; - } - - m_forwardWS.resize(numWheel); - m_axle.resize(numWheel); - m_forwardImpulse.resize(numWheel); - m_sideImpulse.resize(numWheel); - - //collapse all those loops into one! - for (int i = 0; i < wheels.size(); i++) { - m_sideImpulse.write[i] = real_t(0.); - m_forwardImpulse.write[i] = real_t(0.); - } - - { - for (int i = 0; i < wheels.size(); i++) { - VehicleWheel &wheelInfo = *wheels[i]; - - if (wheelInfo.m_raycastInfo.m_isInContact) { - //const btTransform& wheelTrans = getWheelTransformWS( i ); - - Basis wheelBasis0 = wheelInfo.m_worldTransform.basis; //get_global_transform().basis; - - m_axle.write[i] = wheelBasis0.get_axis(Vector3::AXIS_X); - //m_axle[i] = wheelInfo.m_raycastInfo.m_wheelAxleWS; - - const Vector3 &surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS; - real_t proj = m_axle[i].dot(surfNormalWS); - m_axle.write[i] -= surfNormalWS * proj; - m_axle.write[i] = m_axle[i].normalized(); - - m_forwardWS.write[i] = surfNormalWS.cross(m_axle[i]); - m_forwardWS.write[i].normalize(); - - _resolve_single_bilateral(s, wheelInfo.m_raycastInfo.m_contactPointWS, - wheelInfo.m_raycastInfo.m_groundObject, wheelInfo.m_raycastInfo.m_contactPointWS, - m_axle[i], m_sideImpulse.write[i], wheelInfo.m_rollInfluence); - - m_sideImpulse.write[i] *= sideFrictionStiffness2; - } - } - } - - real_t sideFactor = real_t(1.); - real_t fwdFactor = 0.5; - - bool sliding = false; - { - for (int wheel = 0; wheel < wheels.size(); wheel++) { - VehicleWheel &wheelInfo = *wheels[wheel]; - - //class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject; - - real_t rollingFriction = 0.f; - - if (wheelInfo.m_raycastInfo.m_isInContact) { - if (wheelInfo.m_engineForce != 0.f) { - rollingFriction = -wheelInfo.m_engineForce * s->get_step(); - } else { - real_t defaultRollingFrictionImpulse = 0.f; - real_t maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse; - btVehicleWheelContactPoint contactPt(s, wheelInfo.m_raycastInfo.m_groundObject, wheelInfo.m_raycastInfo.m_contactPointWS, m_forwardWS[wheel], maxImpulse); - rollingFriction = _calc_rolling_friction(contactPt); - } - } - - //switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break) - - m_forwardImpulse.write[wheel] = real_t(0.); - wheelInfo.m_skidInfo = real_t(1.); - - if (wheelInfo.m_raycastInfo.m_isInContact) { - wheelInfo.m_skidInfo = real_t(1.); - - real_t maximp = wheelInfo.m_wheelsSuspensionForce * s->get_step() * wheelInfo.m_frictionSlip; - real_t maximpSide = maximp; - - real_t maximpSquared = maximp * maximpSide; - - m_forwardImpulse.write[wheel] = rollingFriction; //wheelInfo.m_engineForce* timeStep; - - real_t x = (m_forwardImpulse[wheel]) * fwdFactor; - real_t y = (m_sideImpulse[wheel]) * sideFactor; - - real_t impulseSquared = (x * x + y * y); - - if (impulseSquared > maximpSquared) { - sliding = true; - - real_t factor = maximp / Math::sqrt(impulseSquared); - - wheelInfo.m_skidInfo *= factor; - } - } - } - } - - if (sliding) { - for (int wheel = 0; wheel < wheels.size(); wheel++) { - if (m_sideImpulse[wheel] != real_t(0.)) { - if (wheels[wheel]->m_skidInfo < real_t(1.)) { - m_forwardImpulse.write[wheel] *= wheels[wheel]->m_skidInfo; - m_sideImpulse.write[wheel] *= wheels[wheel]->m_skidInfo; - } - } - } - } - - // apply the impulses - { - for (int wheel = 0; wheel < wheels.size(); wheel++) { - VehicleWheel &wheelInfo = *wheels[wheel]; - - Vector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - - s->get_transform().origin; - - if (m_forwardImpulse[wheel] != real_t(0.)) { - s->apply_impulse(rel_pos, m_forwardWS[wheel] * (m_forwardImpulse[wheel])); - } - if (m_sideImpulse[wheel] != real_t(0.)) { - PhysicsBody *groundObject = wheelInfo.m_raycastInfo.m_groundObject; - - Vector3 rel_pos2; - if (groundObject) { - rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - groundObject->get_global_transform().origin; - } - - Vector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel]; - -#if defined ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT. - Vector3 vChassisWorldUp = s->get_transform().basis.transposed()[1]; //getRigidBody()->getCenterOfMassTransform().getBasis().getColumn(m_indexUpAxis); - rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f - wheelInfo.m_rollInfluence)); -#else - rel_pos[1] *= wheelInfo.m_rollInfluence; //? -#endif - s->apply_impulse(rel_pos, sideImp); - - //apply friction impulse on the ground - //todo - //groundObject->applyImpulse(-sideImp,rel_pos2); - } - } - } -} - -void VehicleBody::_direct_state_changed(Object *p_state) { - RigidBody::_direct_state_changed(p_state); - - state = Object::cast_to(p_state); - ERR_FAIL_COND_MSG(!state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState object as argument"); - - float step = state->get_step(); - - for (int i = 0; i < wheels.size(); i++) { - _update_wheel(i, state); - } - - for (int i = 0; i < wheels.size(); i++) { - _ray_cast(i, state); - wheels[i]->set_transform(state->get_transform().inverse() * wheels[i]->m_worldTransform); - } - - _update_suspension(state); - - for (int i = 0; i < wheels.size(); i++) { - //apply suspension force - VehicleWheel &wheel = *wheels[i]; - - real_t suspensionForce = wheel.m_wheelsSuspensionForce; - - if (suspensionForce > wheel.m_maxSuspensionForce) { - suspensionForce = wheel.m_maxSuspensionForce; - } - Vector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step; - Vector3 relpos = wheel.m_raycastInfo.m_contactPointWS - state->get_transform().origin; - - state->apply_impulse(relpos, impulse); - //getRigidBody()->applyImpulse(impulse, relpos); - } - - _update_friction(state); - - for (int i = 0; i < wheels.size(); i++) { - VehicleWheel &wheel = *wheels[i]; - Vector3 relpos = wheel.m_raycastInfo.m_hardPointWS - state->get_transform().origin; - Vector3 vel = state->get_linear_velocity() + (state->get_angular_velocity()).cross(relpos); // * mPos); - - if (wheel.m_raycastInfo.m_isInContact) { - const Transform &chassisWorldTransform = state->get_transform(); - - Vector3 fwd( - chassisWorldTransform.basis[0][Vector3::AXIS_Z], - chassisWorldTransform.basis[1][Vector3::AXIS_Z], - chassisWorldTransform.basis[2][Vector3::AXIS_Z]); - - real_t proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS); - fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj; - - real_t proj2 = fwd.dot(vel); - - wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelRadius); - } - - wheel.m_rotation += wheel.m_deltaRotation; - wheel.m_rpm = ((wheel.m_deltaRotation / step) * 60) / Math_TAU; - - wheel.m_deltaRotation *= real_t(0.99); //damping of rotation when not in contact - } - - state = nullptr; -} - -void VehicleBody::set_engine_force(float p_engine_force) { - engine_force = p_engine_force; - for (int i = 0; i < wheels.size(); i++) { - VehicleWheel &wheelInfo = *wheels[i]; - if (wheelInfo.engine_traction) { - wheelInfo.m_engineForce = p_engine_force; - } - } -} - -float VehicleBody::get_engine_force() const { - return engine_force; -} - -void VehicleBody::set_brake(float p_brake) { - brake = p_brake; - for (int i = 0; i < wheels.size(); i++) { - VehicleWheel &wheelInfo = *wheels[i]; - wheelInfo.m_brake = p_brake; - } -} -float VehicleBody::get_brake() const { - return brake; -} - -void VehicleBody::set_steering(float p_steering) { - m_steeringValue = p_steering; - for (int i = 0; i < wheels.size(); i++) { - VehicleWheel &wheelInfo = *wheels[i]; - if (wheelInfo.steers) { - wheelInfo.m_steering = p_steering; - } - } -} -float VehicleBody::get_steering() const { - return m_steeringValue; -} - -void VehicleBody::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_engine_force", "engine_force"), &VehicleBody::set_engine_force); - ClassDB::bind_method(D_METHOD("get_engine_force"), &VehicleBody::get_engine_force); - - ClassDB::bind_method(D_METHOD("set_brake", "brake"), &VehicleBody::set_brake); - ClassDB::bind_method(D_METHOD("get_brake"), &VehicleBody::get_brake); - - ClassDB::bind_method(D_METHOD("set_steering", "steering"), &VehicleBody::set_steering); - ClassDB::bind_method(D_METHOD("get_steering"), &VehicleBody::get_steering); - - ADD_GROUP("Motion", ""); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "engine_force", PROPERTY_HINT_RANGE, "-1024,1024.0,0.01,or_greater"), "set_engine_force", "get_engine_force"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "brake", PROPERTY_HINT_RANGE, "0.0,1.0,0.01"), "set_brake", "get_brake"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "steering", PROPERTY_HINT_RANGE, "-180,180.0,0.01"), "set_steering", "get_steering"); -} - -VehicleBody::VehicleBody() { - m_pitchControl = 0; - m_currentVehicleSpeedKmHour = real_t(0.); - m_steeringValue = real_t(0.); - - engine_force = 0; - brake = 0; - - state = nullptr; - ccd = false; - - exclude.insert(get_rid()); - //PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(), this, "_direct_state_changed"); - - set_mass(40); -} diff --git a/scene/3d/vehicle_body.h b/scene/3d/vehicle_body.h deleted file mode 100644 index 04e4b48..0000000 --- a/scene/3d/vehicle_body.h +++ /dev/null @@ -1,211 +0,0 @@ -#ifndef VEHICLE_BODY_H -#define VEHICLE_BODY_H -/*************************************************************************/ -/* vehicle_body.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "scene/3d/physics_body.h" - -class VehicleBody; - -class VehicleWheel : public Spatial { - GDCLASS(VehicleWheel, Spatial); - - friend class VehicleBody; - - Transform m_worldTransform; - Transform local_xform; - bool engine_traction; - bool steers; - - Vector3 m_chassisConnectionPointCS; //const - Vector3 m_wheelDirectionCS; //const - Vector3 m_wheelAxleCS; // const or modified by steering - - real_t m_suspensionRestLength; - real_t m_maxSuspensionTravelCm; - real_t m_wheelRadius; - - real_t m_suspensionStiffness; - real_t m_wheelsDampingCompression; - real_t m_wheelsDampingRelaxation; - real_t m_frictionSlip; - real_t m_maxSuspensionForce; - bool m_bIsFrontWheel; - - VehicleBody *body; - - //btVector3 m_wheelAxleCS; // const or modified by steering ? - - real_t m_steering; - real_t m_rotation; - real_t m_deltaRotation; - real_t m_rpm; - real_t m_rollInfluence; - real_t m_engineForce; - real_t m_brake; - - real_t m_clippedInvContactDotSuspension; - real_t m_suspensionRelativeVelocity; - //calculated by suspension - real_t m_wheelsSuspensionForce; - real_t m_skidInfo; - - struct RaycastInfo { - //set by raycaster - Vector3 m_contactNormalWS; //contactnormal - Vector3 m_contactPointWS; //raycast hitpoint - real_t m_suspensionLength; - Vector3 m_hardPointWS; //raycast starting point - Vector3 m_wheelDirectionWS; //direction in worldspace - Vector3 m_wheelAxleWS; // axle in worldspace - bool m_isInContact; - PhysicsBody *m_groundObject; //could be general void* ptr - } m_raycastInfo; - - void _update(PhysicsDirectBodyState *s); - -protected: - void _notification(int p_what); - static void _bind_methods(); - -public: - void set_radius(float p_radius); - float get_radius() const; - - void set_suspension_rest_length(float p_length); - float get_suspension_rest_length() const; - - void set_suspension_travel(float p_length); - float get_suspension_travel() const; - - void set_suspension_stiffness(float p_value); - float get_suspension_stiffness() const; - - void set_suspension_max_force(float p_value); - float get_suspension_max_force() const; - - void set_damping_compression(float p_value); - float get_damping_compression() const; - - void set_damping_relaxation(float p_value); - float get_damping_relaxation() const; - - void set_friction_slip(float p_value); - float get_friction_slip() const; - - void set_use_as_traction(bool p_enable); - bool is_used_as_traction() const; - - void set_use_as_steering(bool p_enabled); - bool is_used_as_steering() const; - - bool is_in_contact() const; - - Spatial *get_contact_body() const; - - void set_roll_influence(float p_value); - float get_roll_influence() const; - - float get_skidinfo() const; - - float get_rpm() const; - - void set_engine_force(float p_engine_force); - float get_engine_force() const; - - void set_brake(float p_brake); - float get_brake() const; - - void set_steering(float p_steering); - float get_steering() const; - - String get_configuration_warning() const; - - VehicleWheel(); -}; - -class VehicleBody : public RigidBody { - GDCLASS(VehicleBody, RigidBody); - - float engine_force; - float brake; - - real_t m_pitchControl; - real_t m_steeringValue; - real_t m_currentVehicleSpeedKmHour; - - RBSet exclude; - - Vector m_forwardWS; - Vector m_axle; - Vector m_forwardImpulse; - Vector m_sideImpulse; - - struct btVehicleWheelContactPoint { - PhysicsDirectBodyState *m_s; - PhysicsBody *m_body1; - Vector3 m_frictionPositionWorld; - Vector3 m_frictionDirectionWorld; - real_t m_jacDiagABInv; - real_t m_maxImpulse; - - btVehicleWheelContactPoint(PhysicsDirectBodyState *s, PhysicsBody *body1, const Vector3 &frictionPosWorld, const Vector3 &frictionDirectionWorld, real_t maxImpulse); - }; - - void _resolve_single_bilateral(PhysicsDirectBodyState *s, const Vector3 &pos1, PhysicsBody *body2, const Vector3 &pos2, const Vector3 &normal, real_t &impulse, const real_t p_rollInfluence); - real_t _calc_rolling_friction(btVehicleWheelContactPoint &contactPoint); - - void _update_friction(PhysicsDirectBodyState *s); - void _update_suspension(PhysicsDirectBodyState *s); - real_t _ray_cast(int p_idx, PhysicsDirectBodyState *s); - void _update_wheel_transform(VehicleWheel &wheel, PhysicsDirectBodyState *s); - void _update_wheel(int p_idx, PhysicsDirectBodyState *s); - - friend class VehicleWheel; - Vector wheels; - - static void _bind_methods(); - - void _direct_state_changed(Object *p_state); - -public: - void set_engine_force(float p_engine_force); - float get_engine_force() const; - - void set_brake(float p_brake); - float get_brake() const; - - void set_steering(float p_steering); - float get_steering() const; - - VehicleBody(); -}; - -#endif // VEHICLE_BODY_H diff --git a/scene/3d/visibility_notifier.cpp b/scene/3d/visibility_notifier.cpp index 76f9e1d..eefe3bf 100644 --- a/scene/3d/visibility_notifier.cpp +++ b/scene/3d/visibility_notifier.cpp @@ -32,10 +32,8 @@ #include "core/config/engine.h" #include "scene/3d/camera.h" -#include "scene/3d/physics_body.h" #include "scene/animation/animation_player.h" #include "scene/animation/animation_tree.h" -#include "scene/resources/shapes/shape.h" #include "scene/resources/world_3d.h" #include "scene/main/scene_string_names.h" @@ -269,14 +267,6 @@ void VisibilityEnabler::_find_nodes(Node *p_node) { bool add = false; Variant meta; - { - RigidBody *rb = Object::cast_to(p_node); - if (rb && ((rb->get_mode() == RigidBody::MODE_CHARACTER || rb->get_mode() == RigidBody::MODE_RIGID))) { - add = true; - meta = rb->get_mode(); - } - } - if (Object::cast_to(p_node) || Object::cast_to(p_node)) { add = true; } @@ -342,13 +332,6 @@ void VisibilityEnabler::_notification(int p_what) { void VisibilityEnabler::_change_node_state(Node *p_node, bool p_enabled) { ERR_FAIL_COND(!nodes.has(p_node)); - if (enabler[ENABLER_FREEZE_BODIES]) { - RigidBody *rb = Object::cast_to(p_node); - if (rb) { - rb->set_sleeping(!p_enabled); - } - } - if (enabler[ENABLER_PAUSE_ANIMATIONS]) { AnimationPlayer *ap = Object::cast_to(p_node); if (ap) { diff --git a/scene/main/scene_tree.cpp b/scene/main/scene_tree.cpp index d48dd72..6a7bde5 100644 --- a/scene/main/scene_tree.cpp +++ b/scene/main/scene_tree.cpp @@ -56,9 +56,7 @@ #include "servers/audio_server.h" #include "servers/navigation_server.h" #include "servers/physics_2d_server.h" -#include "servers/physics_server.h" #include "viewport.h" - #include "modules/modules_enabled.gen.h" // For freetype. #include "scene/resources/mesh/mesh.h" #include "scene/resources/world_2d.h" @@ -1133,7 +1131,6 @@ void SceneTree::set_pause(bool p_enabled) { } pause = p_enabled; - PhysicsServer::get_singleton()->set_active(!p_enabled); Physics2DServer::get_singleton()->set_active(!p_enabled); if (get_root()) { diff --git a/scene/main/viewport.cpp b/scene/main/viewport.cpp index f846844..07cdbeb 100644 --- a/scene/main/viewport.cpp +++ b/scene/main/viewport.cpp @@ -40,7 +40,6 @@ #include "scene/2d/collision_object_2d.h" #include "scene/2d/listener_2d.h" #include "scene/3d/camera.h" -#include "scene/3d/collision_object.h" #include "scene/3d/listener.h" #include "scene/main/spatial.h" #include "scene/main/control.h" @@ -217,24 +216,6 @@ void Viewport::_update_stretch_transform() { _update_global_transform(); } -void Viewport::_collision_object_input_event(CollisionObject *p_object, Camera *p_camera, const Ref &p_input_event, const Vector3 &p_pos, const Vector3 &p_normal, int p_shape) { - Transform object_transform = p_object->get_global_transform(); - Transform camera_transform = p_camera->get_global_transform(); - ObjectID id = p_object->get_instance_id(); - - //avoid sending the fake event unnecessarily if nothing really changed in the context - if (object_transform == physics_last_object_transform && camera_transform == physics_last_camera_transform && physics_last_id == id) { - Ref mm = p_input_event; - if (mm.is_valid() && mm->get_device() == InputEvent::DEVICE_ID_INTERNAL) { - return; //discarded - } - } - p_object->_input_event(camera, p_input_event, p_pos, p_normal, p_shape); - physics_last_object_transform = object_transform; - physics_last_camera_transform = camera_transform; - physics_last_id = id; -} - void Viewport::_own_world_3d_changed() { ERR_FAIL_COND(world_3d.is_null()); ERR_FAIL_COND(own_world_3d.is_null()); @@ -372,19 +353,6 @@ void Viewport::_notification(int p_what) { } } - if (get_tree()->is_debugging_collisions_hint() && contact_3d_debug_multimesh.is_valid()) { - Vector points = PhysicsServer::get_singleton()->space_get_contacts(find_world_3d()->get_space()); - int point_count = PhysicsServer::get_singleton()->space_get_contact_count(find_world_3d()->get_space()); - - RS::get_singleton()->multimesh_set_visible_instances(contact_3d_debug_multimesh, point_count); - - for (int i = 0; i < point_count; i++) { - Transform point_transform; - point_transform.origin = points[i]; - RS::get_singleton()->multimesh_instance_set_transform(contact_3d_debug_multimesh, i, point_transform); - } - } - if (!GLOBAL_GET("physics/common/enable_pause_aware_picking")) { _process_picking(false); } @@ -423,12 +391,6 @@ void Viewport::_process_picking(bool p_ignore_paused) { _drop_physics_mouseover(true); } -#ifndef _3D_DISABLED - Vector2 last_pos(1e20, 1e20); - CollisionObject *last_object = nullptr; - ObjectID last_id = 0; -#endif - PhysicsDirectSpaceState::RayResult result; Physics2DDirectSpaceState *ss2d = Physics2DServer::get_singleton()->space_get_direct_state(find_world_2d()->get_space()); if (physics_has_last_mousepos) { @@ -600,81 +562,6 @@ void Viewport::_process_picking(bool p_ignore_paused) { } } -#ifndef _3D_DISABLED - bool captured = false; - - if (physics_object_capture != 0) { - CollisionObject *co = Object::cast_to(ObjectDB::get_instance(physics_object_capture)); - if (co && camera) { - _collision_object_input_event(co, camera, ev, Vector3(), Vector3(), 0); - captured = true; - if (mb.is_valid() && mb->get_button_index() == 1 && !mb->is_pressed()) { - physics_object_capture = 0; - } - - } else { - physics_object_capture = 0; - } - } - - if (captured) { - //none - } else if (pos == last_pos) { - if (last_id) { - if (ObjectDB::get_instance(last_id) && last_object) { - //good, exists - _collision_object_input_event(last_object, camera, ev, result.position, result.normal, result.shape); - if (last_object->get_capture_input_on_drag() && mb.is_valid() && mb->get_button_index() == 1 && mb->is_pressed()) { - physics_object_capture = last_id; - } - } - } - } else { - if (camera) { - Vector3 from = camera->project_ray_origin(pos); - Vector3 dir = camera->project_ray_normal(pos); - float far = camera->far; - - PhysicsDirectSpaceState *space = PhysicsServer::get_singleton()->space_get_direct_state(find_world_3d()->get_space()); - if (space) { - bool col = space->intersect_ray(from, from + dir * far, result, RBSet(), 0xFFFFFFFF, true, true, true); - ObjectID new_collider = 0; - if (col) { - CollisionObject *co = Object::cast_to(result.collider); - if (co && (!p_ignore_paused || co->can_process())) { - _collision_object_input_event(co, camera, ev, result.position, result.normal, result.shape); - last_object = co; - last_id = result.collider_id; - new_collider = last_id; - if (co->get_capture_input_on_drag() && mb.is_valid() && mb->get_button_index() == 1 && mb->is_pressed()) { - physics_object_capture = last_id; - } - } - } - - if (is_mouse && new_collider != physics_object_over) { - if (physics_object_over) { - CollisionObject *co = Object::cast_to(ObjectDB::get_instance(physics_object_over)); - if (co) { - co->_mouse_exit(); - } - } - - if (new_collider) { - CollisionObject *co = Object::cast_to(ObjectDB::get_instance(new_collider)); - if (co) { - co->_mouse_enter(); - } - } - - physics_object_over = new_collider; - } - } - - last_pos = pos; - } - } -#endif } } @@ -826,25 +713,6 @@ void Viewport::_on_after_world_override_changed() { } RenderingServer::get_singleton()->canvas_item_set_parent(contact_2d_debug, find_world_2d()->get_canvas()); - - //3D - PhysicsServer::get_singleton()->space_set_debug_contacts(find_world_3d()->get_space(), get_tree()->get_collision_debug_contact_count()); - - if (contact_3d_debug_multimesh != RID()) { - contact_3d_debug_multimesh = RID_PRIME(RenderingServer::get_singleton()->multimesh_create()); - } - - RenderingServer::get_singleton()->multimesh_allocate(contact_3d_debug_multimesh, get_tree()->get_collision_debug_contact_count(), RS::MULTIMESH_TRANSFORM_3D, RS::MULTIMESH_COLOR_8BIT); - RenderingServer::get_singleton()->multimesh_set_visible_instances(contact_3d_debug_multimesh, 0); - RenderingServer::get_singleton()->multimesh_set_mesh(contact_3d_debug_multimesh, get_tree()->get_debug_contact_mesh()->get_rid()); - - if (contact_3d_debug_instance != RID()) { - contact_3d_debug_instance = RID_PRIME(RenderingServer::get_singleton()->instance_create()); - } - - RenderingServer::get_singleton()->instance_set_base(contact_3d_debug_instance, contact_3d_debug_multimesh); - RenderingServer::get_singleton()->instance_set_scenario(contact_3d_debug_instance, find_world_3d()->get_scenario()); - //RenderingServer::get_singleton()->instance_geometry_set_flag(contact_3d_debug_instance, RS::INSTANCE_FLAG_VISIBLE_IN_ALL_ROOMS, true); } } @@ -2714,20 +2582,6 @@ void Viewport::_drop_physics_mouseover(bool p_paused_only) { physics_2d_mouseover.erase(to_erase.front()->get()); to_erase.pop_front(); } - -#ifndef _3D_DISABLED - if (physics_object_over) { - CollisionObject *co = Object::cast_to(ObjectDB::get_instance(physics_object_over)); - if (co) { - if (!co->is_inside_tree()) { - physics_object_over = physics_object_capture = 0; - } else if (!(p_paused_only && co->can_process())) { - co->_mouse_exit(); - physics_object_over = physics_object_capture = 0; - } - } - } -#endif } List::Element *Viewport::_gui_show_modal(Control *p_control) { diff --git a/scene/main/viewport.h b/scene/main/viewport.h index 8bf5632..64e11e4 100644 --- a/scene/main/viewport.h +++ b/scene/main/viewport.h @@ -49,7 +49,6 @@ class Panel; class Label; class Timer; class Viewport; -class CollisionObject; class SceneTreeTimer; class ViewportTexture : public Texture { @@ -407,8 +406,6 @@ private: } physics_last_mouse_state; - void _collision_object_input_event(CollisionObject *p_object, Camera *p_camera, const Ref &p_input_event, const Vector3 &p_pos, const Vector3 &p_normal, int p_shape); - bool handle_input_locally; bool local_input_handled; diff --git a/scene/register_scene_types.cpp b/scene/register_scene_types.cpp index dd6d622..d273446 100644 --- a/scene/register_scene_types.cpp +++ b/scene/register_scene_types.cpp @@ -140,19 +140,13 @@ #include "scene/main/world.h" #include "scene/audio/audio_stream_sample.h" #include "scene/resources/bit_map.h" -#include "scene/resources/shapes/box_shape.h" -#include "scene/resources/shapes/capsule_shape.h" #include "scene/resources/shapes_2d/capsule_shape_2d.h" #include "scene/resources/shapes_2d/circle_shape_2d.h" -#include "scene/resources/shapes/concave_polygon_shape.h" #include "scene/resources/shapes_2d/concave_polygon_shape_2d.h" -#include "scene/resources/shapes/convex_polygon_shape.h" #include "scene/resources/shapes_2d/convex_polygon_shape_2d.h" -#include "scene/resources/shapes/cylinder_shape.h" #include "scene/resources/default_theme/default_theme.h" #include "scene/resources/font/dynamic_font.h" #include "scene/resources/gradient.h" -#include "scene/resources/shapes/height_map_shape.h" #include "scene/resources/mesh/immediate_mesh.h" #include "scene/resources/shapes_2d/line_shape_2d.h" #include "scene/resources/material/material.h" @@ -167,17 +161,15 @@ #include "scene/resources/navigation_2d/navigation_polygon.h" #include "scene/resources/packed_scene.h" #include "scene/resources/material/particles_material.h" -#include "scene/resources/physics_material.h" -#include "scene/resources/shapes/plane_shape.h" #include "scene/resources/mesh/polygon_path_finder.h" #include "scene/resources/mesh/primitive_meshes.h" -#include "scene/resources/shapes/ray_shape.h" #include "scene/resources/shapes_2d/rectangle_shape_2d.h" #include "scene/resources/resource_format_text.h" #include "scene/resources/shapes_2d/segment_shape_2d.h" +#include "scene/resources/physics_material.h" + #include "scene/resources/sky.h" -#include "scene/resources/shapes/sphere_shape.h" #include "scene/resources/mesh/surface_tool.h" #include "scene/gui/resources/syntax_highlighter.h" #include "scene/resources/text_file.h" @@ -188,11 +180,8 @@ #include "scene/main/scene_string_names.h" #ifndef _3D_DISABLED -#include "scene/3d/area.h" #include "scene/3d/audio_stream_player_3d.h" #include "scene/3d/camera.h" -#include "scene/3d/collision_polygon.h" -#include "scene/3d/collision_shape.h" #include "scene/3d/cpu_particles.h" #include "scene/3d/immediate_geometry.h" #include "scene/3d/interpolated_camera.h" @@ -205,26 +194,18 @@ #include "scene/3d/navigation_geometry_parser_3d.h" #include "scene/3d/navigation_link_3d.h" #include "scene/3d/navigation_mesh_instance.h" -#include "scene/3d/navigation_obstacle.h" #include "scene/3d/occluder.h" #include "scene/3d/path.h" -#include "scene/3d/physics_body.h" -#include "scene/3d/physics_joint.h" #include "scene/3d/portal.h" #include "scene/3d/position_3d.h" #include "scene/3d/proximity_group.h" -#include "scene/3d/ray_cast.h" #include "scene/3d/reflection_probe.h" #include "scene/3d/remote_transform.h" #include "scene/3d/room.h" #include "scene/3d/room_group.h" #include "scene/3d/room_manager.h" -#include "scene/3d/shape_cast.h" -#include "scene/3d/soft_body.h" #include "scene/3d/spatial_velocity_tracker.h" -#include "scene/3d/spring_arm.h" #include "scene/3d/sprite_3d.h" -#include "scene/3d/vehicle_body.h" #include "scene/3d/visibility_notifier.h" #include "scene/resources/environment_3d.h" #include "scene/resources/mesh/multimesh.h" @@ -438,7 +419,6 @@ void register_scene_types() { ClassDB::register_virtual_class(); ClassDB::register_virtual_class(); ClassDB::register_class(); - ClassDB::register_class(); ClassDB::register_class(); ClassDB::register_class(); ClassDB::register_class(); @@ -468,24 +448,7 @@ void register_scene_types() { OS::get_singleton()->yield(); //may take time to init - ClassDB::register_virtual_class(); - ClassDB::register_virtual_class(); - ClassDB::register_class(); - ClassDB::register_class(); - ClassDB::register_class(); - ClassDB::register_class(); - ClassDB::register_class(); - - ClassDB::register_class(); - - ClassDB::register_class(); - ClassDB::register_class(); - ClassDB::register_class(); ClassDB::register_class(); - ClassDB::register_class(); - ClassDB::register_class(); - ClassDB::register_class(); - ClassDB::register_class(); ClassDB::register_class(); ClassDB::register_class(); @@ -496,17 +459,10 @@ void register_scene_types() { ClassDB::register_class(); ClassDB::register_class(); - ClassDB::register_virtual_class(); - ClassDB::register_class(); - ClassDB::register_class(); - ClassDB::register_class(); - ClassDB::register_class(); - ClassDB::register_class(); ClassDB::register_class(); ClassDB::register_class(); ClassDB::register_class(); - ClassDB::register_class(); ClassDB::register_class(); OS::get_singleton()->yield(); //may take time to init @@ -601,22 +557,6 @@ void register_scene_types() { OS::get_singleton()->yield(); //may take time to init - ClassDB::register_virtual_class(); - ClassDB::register_class(); - ClassDB::register_class(); - ClassDB::register_class(); - ClassDB::register_class(); - ClassDB::register_class(); - ClassDB::register_class(); - ClassDB::register_class(); - ClassDB::register_class(); - ClassDB::register_class(); - ClassDB::register_virtual_class(); - ClassDB::register_class(); - ClassDB::register_class(); - - OS::get_singleton()->yield(); //may take time to init - ClassDB::register_class(); #endif diff --git a/scene/resources/mesh/importer_mesh.cpp b/scene/resources/mesh/importer_mesh.cpp index d02c36e..c2d66ad 100644 --- a/scene/resources/mesh/importer_mesh.cpp +++ b/scene/resources/mesh/importer_mesh.cpp @@ -422,27 +422,6 @@ Vector ImporterMesh::get_faces() const { return faces; } -Ref ImporterMesh::create_trimesh_shape() const { - Vector faces = get_faces(); - if (faces.size() == 0) { - return Ref(); - } - - PoolVector face_points; - face_points.resize(faces.size() * 3); - - for (int i = 0; i < face_points.size(); i += 3) { - Face3 f = faces.get(i / 3); - face_points.set(i, f.vertex[0]); - face_points.set(i + 1, f.vertex[1]); - face_points.set(i + 2, f.vertex[2]); - } - - Ref shape = memnew(ConcavePolygonShape); - shape->set_faces(face_points); - return shape; -} - Ref ImporterMesh::create_navigation_mesh() { Vector faces = get_faces(); if (faces.size() == 0) { diff --git a/scene/resources/mesh/importer_mesh.h b/scene/resources/mesh/importer_mesh.h index 2a6bab5..13e9a28 100644 --- a/scene/resources/mesh/importer_mesh.h +++ b/scene/resources/mesh/importer_mesh.h @@ -33,8 +33,6 @@ #include "core/object/resource.h" #include "core/containers/local_vector.h" -#include "scene/resources/shapes/concave_polygon_shape.h" -#include "scene/resources/shapes/convex_polygon_shape.h" #include "scene/resources/mesh/mesh.h" #include "scene/resources/navigation/navigation_mesh.h" @@ -116,7 +114,6 @@ public: void set_surface_material(int p_surface, const Ref &p_material); Vector get_faces() const; - Ref create_trimesh_shape() const; Ref create_navigation_mesh(); bool has_mesh() const; diff --git a/scene/resources/mesh/mesh.cpp b/scene/resources/mesh/mesh.cpp index 314379e..7c3f557 100644 --- a/scene/resources/mesh/mesh.cpp +++ b/scene/resources/mesh/mesh.cpp @@ -34,8 +34,6 @@ #include "core/containers/local_vector.h" #include "core/math/convex_hull.h" #include "core/containers/pair.h" -#include "scene/resources/shapes/concave_polygon_shape.h" -#include "scene/resources/shapes/convex_polygon_shape.h" #include "surface_tool.h" #include @@ -202,68 +200,6 @@ PoolVector Mesh::get_faces() const { return PoolVector(); } -Ref Mesh::create_convex_shape(bool p_clean, bool p_simplify) const { - if (p_simplify) { - Vector> decomposed = convex_decompose(1); - if (decomposed.size() == 1) { - return decomposed[0]; - } else { - ERR_PRINT("Convex shape simplification failed, falling back to simpler process."); - } - } - - PoolVector vertices; - for (int i = 0; i < get_surface_count(); i++) { - Array a = surface_get_arrays(i); - ERR_FAIL_COND_V(a.empty(), Ref()); - PoolVector v = a[ARRAY_VERTEX]; - vertices.append_array(v); - } - - Ref shape = memnew(ConvexPolygonShape); - - if (p_clean) { - Geometry::MeshData md; - Error err = ConvexHullComputer::convex_hull(vertices, md); - if (err == OK) { - int vertex_count = md.vertices.size(); - vertices.resize(vertex_count); - { - PoolVector::Write w = vertices.write(); - for (int idx = 0; idx < vertex_count; ++idx) { - w[idx] = md.vertices[idx]; - } - } - } else { - ERR_PRINT("Convex shape cleaning failed, falling back to simpler process."); - } - } - - shape->set_points(vertices); - return shape; -} - -Ref Mesh::create_trimesh_shape() const { - PoolVector faces = get_faces(); - if (faces.size() == 0) { - return Ref(); - } - - PoolVector face_points; - face_points.resize(faces.size() * 3); - - for (int i = 0; i < face_points.size(); i += 3) { - Face3 f = faces.get(i / 3); - face_points.set(i, f.vertex[0]); - face_points.set(i + 1, f.vertex[1]); - face_points.set(i + 2, f.vertex[2]); - } - - Ref shape = memnew(ConcavePolygonShape); - shape->set_faces(face_points); - return shape; -} - Ref Mesh::create_outline(float p_margin) const { Array arrays; int index_accum = 0; @@ -527,44 +463,6 @@ void Mesh::clear_cache() const { debug_lines.clear(); } -Vector> Mesh::convex_decompose(int p_max_convex_hulls) const { - ERR_FAIL_COND_V(!convex_decomposition_function, Vector>()); - - Ref tm = generate_triangle_mesh(); - ERR_FAIL_COND_V(!tm.is_valid(), Vector>()); - - const PoolVector &triangles = tm->get_triangles(); - int triangle_count = triangles.size(); - - PoolVector indices; - { - indices.resize(triangle_count * 3); - PoolVector::Write w = indices.write(); - PoolVector::Read triangles_read = triangles.read(); - for (int i = 0; i < triangle_count; i++) { - for (int j = 0; j < 3; j++) { - w[i * 3 + j] = triangles_read[i].indices[j]; - } - } - } - - const PoolVector &vertices = tm->get_vertices(); - int vertex_count = vertices.size(); - - Vector> decomposed = convex_decomposition_function((real_t *)vertices.read().ptr(), vertex_count, indices.read().ptr(), triangle_count, p_max_convex_hulls, nullptr); - - Vector> ret; - - for (int i = 0; i < decomposed.size(); i++) { - Ref shape; - shape.instance(); - shape->set_points(decomposed[i]); - ret.push_back(shape); - } - - return ret; -} - Mesh::Mesh() { } @@ -1081,8 +979,6 @@ void ArrayMesh::_bind_methods() { ClassDB::bind_method(D_METHOD("surface_find_by_name", "name"), &ArrayMesh::surface_find_by_name); ClassDB::bind_method(D_METHOD("surface_set_name", "surf_idx", "name"), &ArrayMesh::surface_set_name); ClassDB::bind_method(D_METHOD("surface_get_name", "surf_idx"), &ArrayMesh::surface_get_name); - ClassDB::bind_method(D_METHOD("create_trimesh_shape"), &ArrayMesh::create_trimesh_shape); - ClassDB::bind_method(D_METHOD("create_convex_shape", "clean", "simplify"), &ArrayMesh::create_convex_shape, DEFVAL(true), DEFVAL(false)); ClassDB::bind_method(D_METHOD("create_outline", "margin"), &ArrayMesh::create_outline); ClassDB::bind_method(D_METHOD("regen_normalmaps"), &ArrayMesh::regen_normalmaps); ClassDB::set_method_flags(get_class_static(), _scs_create("regen_normalmaps"), METHOD_FLAGS_DEFAULT | METHOD_FLAG_EDITOR); diff --git a/scene/resources/mesh/mesh.h b/scene/resources/mesh/mesh.h index df5d812..4b7c8c6 100644 --- a/scene/resources/mesh/mesh.h +++ b/scene/resources/mesh/mesh.h @@ -35,7 +35,6 @@ #include "core/object/resource.h" #include "scene/resources/material/material.h" -#include "scene/resources/shapes/shape.h" #include "servers/rendering_server.h" class Mesh : public Resource { @@ -136,9 +135,6 @@ public: void generate_debug_mesh_lines(Vector &r_lines); void generate_debug_mesh_indices(Vector &r_points); - Ref create_trimesh_shape() const; - Ref create_convex_shape(bool p_clean = true, bool p_simplify = false) const; - Ref create_outline(float p_margin) const; virtual AABB get_aabb() const = 0; @@ -149,8 +145,6 @@ public: static ConvexDecompositionFunc convex_decomposition_function; - Vector> convex_decompose(int p_max_convex_hulls = -1) const; - Mesh(); }; diff --git a/scene/resources/physics_material.h b/scene/resources/physics_material.h index 3626b6a..124193d 100644 --- a/scene/resources/physics_material.h +++ b/scene/resources/physics_material.h @@ -31,7 +31,6 @@ /*************************************************************************/ #include "core/object/resource.h" -#include "servers/physics_server.h" class PhysicsMaterial : public Resource { GDCLASS(PhysicsMaterial, Resource); diff --git a/scene/resources/shapes/box_shape.cpp b/scene/resources/shapes/box_shape.cpp deleted file mode 100644 index bd050ce..0000000 --- a/scene/resources/shapes/box_shape.cpp +++ /dev/null @@ -1,80 +0,0 @@ -/*************************************************************************/ -/* box_shape.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "box_shape.h" -#include "servers/physics_server.h" - -Vector BoxShape::get_debug_mesh_lines() { - Vector lines; - AABB aabb; - aabb.position = -get_extents(); - aabb.size = aabb.position * -2; - - for (int i = 0; i < 12; i++) { - Vector3 a, b; - aabb.get_edge(i, a, b); - lines.push_back(a); - lines.push_back(b); - } - - return lines; -} - -real_t BoxShape::get_enclosing_radius() const { - return extents.length(); -} - -void BoxShape::_update_shape() { - PhysicsServer::get_singleton()->shape_set_data(get_shape(), extents); - Shape::_update_shape(); -} - -void BoxShape::set_extents(const Vector3 &p_extents) { - extents = p_extents; - _update_shape(); - notify_change_to_owners(); - _change_notify("extents"); -} - -Vector3 BoxShape::get_extents() const { - return extents; -} - -void BoxShape::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_extents", "extents"), &BoxShape::set_extents); - ClassDB::bind_method(D_METHOD("get_extents"), &BoxShape::get_extents); - - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "extents"), "set_extents", "get_extents"); -} - -BoxShape::BoxShape() : - Shape(RID_PRIME(PhysicsServer::get_singleton()->shape_create(PhysicsServer::SHAPE_BOX))) { - set_extents(Vector3(1, 1, 1)); -} diff --git a/scene/resources/shapes/box_shape.h b/scene/resources/shapes/box_shape.h deleted file mode 100644 index 7a90f4e..0000000 --- a/scene/resources/shapes/box_shape.h +++ /dev/null @@ -1,54 +0,0 @@ -#ifndef BOX_SHAPE_H -#define BOX_SHAPE_H -/*************************************************************************/ -/* box_shape.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "scene/resources/shapes/shape.h" - -class BoxShape : public Shape { - GDCLASS(BoxShape, Shape); - Vector3 extents; - -protected: - static void _bind_methods(); - - virtual void _update_shape(); - -public: - void set_extents(const Vector3 &p_extents); - Vector3 get_extents() const; - - virtual Vector get_debug_mesh_lines(); - virtual real_t get_enclosing_radius() const; - - BoxShape(); -}; - -#endif // BOX_SHAPE_H diff --git a/scene/resources/shapes/capsule_shape.cpp b/scene/resources/shapes/capsule_shape.cpp deleted file mode 100644 index ef3dda3..0000000 --- a/scene/resources/shapes/capsule_shape.cpp +++ /dev/null @@ -1,119 +0,0 @@ -/*************************************************************************/ -/* capsule_shape.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "capsule_shape.h" - -#include "servers/physics_server.h" - -Vector CapsuleShape::get_debug_mesh_lines() { - float radius = get_radius(); - float height = get_height(); - - Vector points; - - Vector3 d(0, 0, height * 0.5); - for (int i = 0; i < 360; i++) { - float ra = Math::deg2rad((float)i); - float rb = Math::deg2rad((float)i + 1); - Point2 a = Vector2(Math::sin(ra), Math::cos(ra)) * radius; - Point2 b = Vector2(Math::sin(rb), Math::cos(rb)) * radius; - - points.push_back(Vector3(a.x, a.y, 0) + d); - points.push_back(Vector3(b.x, b.y, 0) + d); - - points.push_back(Vector3(a.x, a.y, 0) - d); - points.push_back(Vector3(b.x, b.y, 0) - d); - - if (i % 90 == 0) { - points.push_back(Vector3(a.x, a.y, 0) + d); - points.push_back(Vector3(a.x, a.y, 0) - d); - } - - Vector3 dud = i < 180 ? d : -d; - - points.push_back(Vector3(0, a.y, a.x) + dud); - points.push_back(Vector3(0, b.y, b.x) + dud); - points.push_back(Vector3(a.y, 0, a.x) + dud); - points.push_back(Vector3(b.y, 0, b.x) + dud); - } - - return points; -} - -real_t CapsuleShape::get_enclosing_radius() const { - return radius + height * 0.5; -} - -void CapsuleShape::_update_shape() { - Dictionary d; - d["radius"] = radius; - d["height"] = height; - PhysicsServer::get_singleton()->shape_set_data(get_shape(), d); - Shape::_update_shape(); -} - -void CapsuleShape::set_radius(float p_radius) { - radius = p_radius; - _update_shape(); - notify_change_to_owners(); - _change_notify("radius"); -} - -float CapsuleShape::get_radius() const { - return radius; -} - -void CapsuleShape::set_height(float p_height) { - height = p_height; - _update_shape(); - notify_change_to_owners(); - _change_notify("height"); -} - -float CapsuleShape::get_height() const { - return height; -} - -void CapsuleShape::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_radius", "radius"), &CapsuleShape::set_radius); - ClassDB::bind_method(D_METHOD("get_radius"), &CapsuleShape::get_radius); - ClassDB::bind_method(D_METHOD("set_height", "height"), &CapsuleShape::set_height); - ClassDB::bind_method(D_METHOD("get_height"), &CapsuleShape::get_height); - - ADD_PROPERTY(PropertyInfo(Variant::REAL, "radius", PROPERTY_HINT_RANGE, "0.001,100,0.001,or_greater"), "set_radius", "get_radius"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "height", PROPERTY_HINT_RANGE, "0.001,100,0.001,or_greater"), "set_height", "get_height"); -} - -CapsuleShape::CapsuleShape() : - Shape(RID_PRIME(PhysicsServer::get_singleton()->shape_create(PhysicsServer::SHAPE_CAPSULE))) { - radius = 1.0; - height = 1.0; - _update_shape(); -} diff --git a/scene/resources/shapes/capsule_shape.h b/scene/resources/shapes/capsule_shape.h deleted file mode 100644 index 6d17403..0000000 --- a/scene/resources/shapes/capsule_shape.h +++ /dev/null @@ -1,57 +0,0 @@ -#ifndef CAPSULE_SHAPE_H -#define CAPSULE_SHAPE_H -/*************************************************************************/ -/* capsule_shape.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "scene/resources/shapes/shape.h" - -class CapsuleShape : public Shape { - GDCLASS(CapsuleShape, Shape); - float radius; - float height; - -protected: - static void _bind_methods(); - - virtual void _update_shape(); - -public: - void set_radius(float p_radius); - float get_radius() const; - void set_height(float p_height); - float get_height() const; - - virtual Vector get_debug_mesh_lines(); - virtual real_t get_enclosing_radius() const; - - CapsuleShape(); -}; - -#endif // CAPSULE_SHAPE_H diff --git a/scene/resources/shapes/concave_polygon_shape.cpp b/scene/resources/shapes/concave_polygon_shape.cpp deleted file mode 100644 index fab9534..0000000 --- a/scene/resources/shapes/concave_polygon_shape.cpp +++ /dev/null @@ -1,96 +0,0 @@ -/*************************************************************************/ -/* concave_polygon_shape.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "concave_polygon_shape.h" - -#include "servers/physics_server.h" - -Vector ConcavePolygonShape::get_debug_mesh_lines() { - RBSet edges; - - PoolVector data = get_faces(); - int datalen = data.size(); - ERR_FAIL_COND_V((datalen % 3) != 0, Vector()); - - PoolVector::Read r = data.read(); - - for (int i = 0; i < datalen; i += 3) { - for (int j = 0; j < 3; j++) { - DrawEdge de(r[i + j], r[i + ((j + 1) % 3)]); - edges.insert(de); - } - } - - Vector points; - points.resize(edges.size() * 2); - int idx = 0; - for (RBSet::Element *E = edges.front(); E; E = E->next()) { - points.write[idx + 0] = E->get().a; - points.write[idx + 1] = E->get().b; - idx += 2; - } - - return points; -} - -real_t ConcavePolygonShape::get_enclosing_radius() const { - PoolVector data = get_faces(); - PoolVector::Read read = data.read(); - real_t r = 0; - for (int i(0); i < data.size(); i++) { - r = MAX(read[i].length_squared(), r); - } - return Math::sqrt(r); -} - -void ConcavePolygonShape::_update_shape() { - Shape::_update_shape(); -} - -void ConcavePolygonShape::set_faces(const PoolVector &p_faces) { - PhysicsServer::get_singleton()->shape_set_data(get_shape(), p_faces); - _update_shape(); - notify_change_to_owners(); -} - -PoolVector ConcavePolygonShape::get_faces() const { - return PhysicsServer::get_singleton()->shape_get_data(get_shape()); -} - -void ConcavePolygonShape::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_faces", "faces"), &ConcavePolygonShape::set_faces); - ClassDB::bind_method(D_METHOD("get_faces"), &ConcavePolygonShape::get_faces); - ADD_PROPERTY(PropertyInfo(Variant::POOL_VECTOR3_ARRAY, "data", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR | PROPERTY_USAGE_INTERNAL), "set_faces", "get_faces"); -} - -ConcavePolygonShape::ConcavePolygonShape() : - Shape(RID_PRIME(PhysicsServer::get_singleton()->shape_create(PhysicsServer::SHAPE_CONCAVE_POLYGON))) { - //set_planes(Vector3(1,1,1)); -} diff --git a/scene/resources/shapes/concave_polygon_shape.h b/scene/resources/shapes/concave_polygon_shape.h deleted file mode 100644 index 1919158..0000000 --- a/scene/resources/shapes/concave_polygon_shape.h +++ /dev/null @@ -1,73 +0,0 @@ -#ifndef CONCAVE_POLYGON_SHAPE_H -#define CONCAVE_POLYGON_SHAPE_H -/*************************************************************************/ -/* concave_polygon_shape.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "scene/resources/shapes/shape.h" - -class ConcavePolygonShape : public Shape { - GDCLASS(ConcavePolygonShape, Shape); - - struct DrawEdge { - Vector3 a; - Vector3 b; - bool operator<(const DrawEdge &p_edge) const { - if (a == p_edge.a) { - return b < p_edge.b; - } else { - return a < p_edge.a; - } - } - - DrawEdge(const Vector3 &p_a = Vector3(), const Vector3 &p_b = Vector3()) { - a = p_a; - b = p_b; - if (a < b) { - SWAP(a, b); - } - } - }; - -protected: - static void _bind_methods(); - - virtual void _update_shape(); - -public: - void set_faces(const PoolVector &p_faces); - PoolVector get_faces() const; - - Vector get_debug_mesh_lines(); - virtual real_t get_enclosing_radius() const; - - ConcavePolygonShape(); -}; - -#endif // CONCAVE_POLYGON_SHAPE_H diff --git a/scene/resources/shapes/convex_polygon_shape.cpp b/scene/resources/shapes/convex_polygon_shape.cpp deleted file mode 100644 index 7bb8521..0000000 --- a/scene/resources/shapes/convex_polygon_shape.cpp +++ /dev/null @@ -1,90 +0,0 @@ -/*************************************************************************/ -/* convex_polygon_shape.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "convex_polygon_shape.h" -#include "core/math/convex_hull.h" -#include "servers/physics_server.h" - -Vector ConvexPolygonShape::get_debug_mesh_lines() { - PoolVector points = get_points(); - - if (points.size() > 3) { - Vector varr = Variant(points); - Geometry::MeshData md; - Error err = ConvexHullComputer::convex_hull(varr, md); - if (err == OK) { - Vector lines; - lines.resize(md.edges.size() * 2); - for (int i = 0; i < md.edges.size(); i++) { - lines.write[i * 2 + 0] = md.vertices[md.edges[i].a]; - lines.write[i * 2 + 1] = md.vertices[md.edges[i].b]; - } - return lines; - } - } - - return Vector(); -} - -real_t ConvexPolygonShape::get_enclosing_radius() const { - PoolVector data = get_points(); - PoolVector::Read read = data.read(); - real_t r = 0; - for (int i(0); i < data.size(); i++) { - r = MAX(read[i].length_squared(), r); - } - return Math::sqrt(r); -} - -void ConvexPolygonShape::_update_shape() { - PhysicsServer::get_singleton()->shape_set_data(get_shape(), points); - Shape::_update_shape(); -} - -void ConvexPolygonShape::set_points(const PoolVector &p_points) { - points = p_points; - _update_shape(); - notify_change_to_owners(); -} - -PoolVector ConvexPolygonShape::get_points() const { - return points; -} - -void ConvexPolygonShape::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_points", "points"), &ConvexPolygonShape::set_points); - ClassDB::bind_method(D_METHOD("get_points"), &ConvexPolygonShape::get_points); - - ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "points"), "set_points", "get_points"); -} - -ConvexPolygonShape::ConvexPolygonShape() : - Shape(RID_PRIME(PhysicsServer::get_singleton()->shape_create(PhysicsServer::SHAPE_CONVEX_POLYGON))) { -} diff --git a/scene/resources/shapes/convex_polygon_shape.h b/scene/resources/shapes/convex_polygon_shape.h deleted file mode 100644 index c994cda..0000000 --- a/scene/resources/shapes/convex_polygon_shape.h +++ /dev/null @@ -1,54 +0,0 @@ -#ifndef CONVEX_POLYGON_SHAPE_H -#define CONVEX_POLYGON_SHAPE_H -/*************************************************************************/ -/* convex_polygon_shape.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "scene/resources/shapes/shape.h" - -class ConvexPolygonShape : public Shape { - GDCLASS(ConvexPolygonShape, Shape); - PoolVector points; - -protected: - static void _bind_methods(); - - virtual void _update_shape(); - -public: - void set_points(const PoolVector &p_points); - PoolVector get_points() const; - - virtual Vector get_debug_mesh_lines(); - virtual real_t get_enclosing_radius() const; - - ConvexPolygonShape(); -}; - -#endif // CONVEX_POLYGON_SHAPE_H diff --git a/scene/resources/shapes/cylinder_shape.cpp b/scene/resources/shapes/cylinder_shape.cpp deleted file mode 100644 index 45a05ab..0000000 --- a/scene/resources/shapes/cylinder_shape.cpp +++ /dev/null @@ -1,112 +0,0 @@ -/*************************************************************************/ -/* cylinder_shape.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "cylinder_shape.h" - -#include "servers/physics_server.h" - -Vector CylinderShape::get_debug_mesh_lines() { - float radius = get_radius(); - float height = get_height(); - - Vector points; - - Vector3 d(0, height * 0.5, 0); - for (int i = 0; i < 360; i++) { - float ra = Math::deg2rad((float)i); - float rb = Math::deg2rad((float)i + 1); - Point2 a = Vector2(Math::sin(ra), Math::cos(ra)) * radius; - Point2 b = Vector2(Math::sin(rb), Math::cos(rb)) * radius; - - points.push_back(Vector3(a.x, 0, a.y) + d); - points.push_back(Vector3(b.x, 0, b.y) + d); - - points.push_back(Vector3(a.x, 0, a.y) - d); - points.push_back(Vector3(b.x, 0, b.y) - d); - - if (i % 90 == 0) { - points.push_back(Vector3(a.x, 0, a.y) + d); - points.push_back(Vector3(a.x, 0, a.y) - d); - } - } - - return points; -} - -real_t CylinderShape::get_enclosing_radius() const { - return Vector2(radius, height * 0.5).length(); -} - -void CylinderShape::_update_shape() { - Dictionary d; - d["radius"] = radius; - d["height"] = height; - PhysicsServer::get_singleton()->shape_set_data(get_shape(), d); - Shape::_update_shape(); -} - -void CylinderShape::set_radius(float p_radius) { - radius = p_radius; - _update_shape(); - notify_change_to_owners(); - _change_notify("radius"); -} - -float CylinderShape::get_radius() const { - return radius; -} - -void CylinderShape::set_height(float p_height) { - height = p_height; - _update_shape(); - notify_change_to_owners(); - _change_notify("height"); -} - -float CylinderShape::get_height() const { - return height; -} - -void CylinderShape::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_radius", "radius"), &CylinderShape::set_radius); - ClassDB::bind_method(D_METHOD("get_radius"), &CylinderShape::get_radius); - ClassDB::bind_method(D_METHOD("set_height", "height"), &CylinderShape::set_height); - ClassDB::bind_method(D_METHOD("get_height"), &CylinderShape::get_height); - - ADD_PROPERTY(PropertyInfo(Variant::REAL, "height", PROPERTY_HINT_RANGE, "0.001,100,0.001,or_greater"), "set_height", "get_height"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "radius", PROPERTY_HINT_RANGE, "0.001,100,0.001,or_greater"), "set_radius", "get_radius"); -} - -CylinderShape::CylinderShape() : - Shape(RID_PRIME(PhysicsServer::get_singleton()->shape_create(PhysicsServer::SHAPE_CYLINDER))) { - radius = 1.0; - height = 2.0; - _update_shape(); -} diff --git a/scene/resources/shapes/cylinder_shape.h b/scene/resources/shapes/cylinder_shape.h deleted file mode 100644 index 2ae7348..0000000 --- a/scene/resources/shapes/cylinder_shape.h +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef CYLINDER_SHAPE_H -#define CYLINDER_SHAPE_H -/*************************************************************************/ -/* cylinder_shape.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "scene/resources/shapes/shape.h" - -class CylinderShape : public Shape { - GDCLASS(CylinderShape, Shape); - float radius; - float height; - -protected: - static void _bind_methods(); - virtual void _update_shape(); - -public: - void set_radius(float p_radius); - float get_radius() const; - void set_height(float p_height); - float get_height() const; - - virtual Vector get_debug_mesh_lines(); - virtual real_t get_enclosing_radius() const; - - CylinderShape(); -}; - -#endif // CYLINDER_SHAPE_H diff --git a/scene/resources/shapes/height_map_shape.cpp b/scene/resources/shapes/height_map_shape.cpp deleted file mode 100644 index 6b3f25b..0000000 --- a/scene/resources/shapes/height_map_shape.cpp +++ /dev/null @@ -1,214 +0,0 @@ -/*************************************************************************/ -/* height_map_shape.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "height_map_shape.h" - -#include "servers/physics_server.h" - -Vector HeightMapShape::get_debug_mesh_lines() { - Vector points; - - if ((map_width != 0) && (map_depth != 0)) { - // This will be slow for large maps... - // also we'll have to figure out how well bullet centers this shape... - - Vector2 size(map_width - 1, map_depth - 1); - Vector2 start = size * -0.5; - - PoolRealArray::Read r = map_data.read(); - - // reserve some memory for our points.. - points.resize(((map_width - 1) * map_depth * 2) + (map_width * (map_depth - 1) * 2) + (map_width - 1) * (map_depth - 1) * 2); - - // now set our points - int r_offset = 0; - int w_offset = 0; - for (int d = 0; d < map_depth; d++) { - Vector3 height(start.x, 0.0, start.y); - - for (int w = 0; w < map_width; w++) { - height.y = r[r_offset++]; - - if (w != map_width - 1) { - points.write[w_offset++] = height; - points.write[w_offset++] = Vector3(height.x + 1.0, r[r_offset], height.z); - } - - if (d != map_depth - 1) { - points.write[w_offset++] = height; - points.write[w_offset++] = Vector3(height.x, r[r_offset + map_width - 1], height.z + 1.0); - } - - if ((w != map_width - 1) && (d != map_depth - 1)) { - points.write[w_offset++] = Vector3(height.x + 1.0, r[r_offset], height.z); - points.write[w_offset++] = Vector3(height.x, r[r_offset + map_width - 1], height.z + 1.0); - } - - height.x += 1.0; - } - - start.y += 1.0; - } - } - - return points; -} - -real_t HeightMapShape::get_enclosing_radius() const { - return Vector3(real_t(map_width), max_height - min_height, real_t(map_depth)).length(); -} - -void HeightMapShape::_update_shape() { - Dictionary d; - d["width"] = map_width; - d["depth"] = map_depth; - d["heights"] = map_data; - d["min_height"] = min_height; - d["max_height"] = max_height; - PhysicsServer::get_singleton()->shape_set_data(get_shape(), d); - Shape::_update_shape(); -} - -void HeightMapShape::set_map_width(int p_new) { - if (p_new < 1) { - // ignore - } else if (map_width != p_new) { - int was_size = map_width * map_depth; - map_width = p_new; - - int new_size = map_width * map_depth; - map_data.resize(map_width * map_depth); - - PoolRealArray::Write w = map_data.write(); - while (was_size < new_size) { - w[was_size++] = 0.0; - } - - _update_shape(); - notify_change_to_owners(); - _change_notify("map_width"); - _change_notify("map_data"); - } -} - -int HeightMapShape::get_map_width() const { - return map_width; -} - -void HeightMapShape::set_map_depth(int p_new) { - if (p_new < 1) { - // ignore - } else if (map_depth != p_new) { - int was_size = map_width * map_depth; - map_depth = p_new; - - int new_size = map_width * map_depth; - map_data.resize(new_size); - - PoolRealArray::Write w = map_data.write(); - while (was_size < new_size) { - w[was_size++] = 0.0; - } - - _update_shape(); - notify_change_to_owners(); - _change_notify("map_depth"); - _change_notify("map_data"); - } -} - -int HeightMapShape::get_map_depth() const { - return map_depth; -} - -void HeightMapShape::set_map_data(PoolRealArray p_new) { - int size = (map_width * map_depth); - if (p_new.size() != size) { - // fail - return; - } - - // copy - PoolRealArray::Write w = map_data.write(); - PoolRealArray::Read r = p_new.read(); - for (int i = 0; i < size; i++) { - float val = r[i]; - w[i] = val; - if (i == 0) { - min_height = val; - max_height = val; - } else { - if (min_height > val) { - min_height = val; - } - - if (max_height < val) { - max_height = val; - } - } - } - - _update_shape(); - notify_change_to_owners(); - _change_notify("map_data"); -} - -PoolRealArray HeightMapShape::get_map_data() const { - return map_data; -} - -void HeightMapShape::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_map_width", "width"), &HeightMapShape::set_map_width); - ClassDB::bind_method(D_METHOD("get_map_width"), &HeightMapShape::get_map_width); - ClassDB::bind_method(D_METHOD("set_map_depth", "height"), &HeightMapShape::set_map_depth); - ClassDB::bind_method(D_METHOD("get_map_depth"), &HeightMapShape::get_map_depth); - ClassDB::bind_method(D_METHOD("set_map_data", "data"), &HeightMapShape::set_map_data); - ClassDB::bind_method(D_METHOD("get_map_data"), &HeightMapShape::get_map_data); - - ADD_PROPERTY(PropertyInfo(Variant::INT, "map_width", PROPERTY_HINT_RANGE, "0.001,100,0.001,or_greater"), "set_map_width", "get_map_width"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "map_depth", PROPERTY_HINT_RANGE, "0.001,100,0.001,or_greater"), "set_map_depth", "get_map_depth"); - ADD_PROPERTY(PropertyInfo(Variant::POOL_REAL_ARRAY, "map_data"), "set_map_data", "get_map_data"); -} - -HeightMapShape::HeightMapShape() : - Shape(RID_PRIME(PhysicsServer::get_singleton()->shape_create(PhysicsServer::SHAPE_HEIGHTMAP))) { - map_width = 2; - map_depth = 2; - map_data.resize(map_width * map_depth); - PoolRealArray::Write w = map_data.write(); - w[0] = 0.0; - w[1] = 0.0; - w[2] = 0.0; - w[3] = 0.0; - min_height = 0.0; - max_height = 0.0; - - _update_shape(); -} diff --git a/scene/resources/shapes/height_map_shape.h b/scene/resources/shapes/height_map_shape.h deleted file mode 100644 index af63b42..0000000 --- a/scene/resources/shapes/height_map_shape.h +++ /dev/null @@ -1,62 +0,0 @@ -#ifndef HEIGHT_MAP_SHAPE_H -#define HEIGHT_MAP_SHAPE_H -/*************************************************************************/ -/* height_map_shape.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "scene/resources/shapes/shape.h" - -class HeightMapShape : public Shape { - GDCLASS(HeightMapShape, Shape); - - int map_width; - int map_depth; - PoolRealArray map_data; - float min_height; - float max_height; - -protected: - static void _bind_methods(); - virtual void _update_shape(); - -public: - void set_map_width(int p_new); - int get_map_width() const; - void set_map_depth(int p_new); - int get_map_depth() const; - void set_map_data(PoolRealArray p_new); - PoolRealArray get_map_data() const; - - virtual Vector get_debug_mesh_lines(); - virtual real_t get_enclosing_radius() const; - - HeightMapShape(); -}; - -#endif /* !HEIGHT_MAP_SHAPE_H */ diff --git a/scene/resources/shapes/plane_shape.cpp b/scene/resources/shapes/plane_shape.cpp deleted file mode 100644 index b0d0740..0000000 --- a/scene/resources/shapes/plane_shape.cpp +++ /dev/null @@ -1,89 +0,0 @@ -/*************************************************************************/ -/* plane_shape.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "plane_shape.h" - -#include "servers/physics_server.h" - -Vector PlaneShape::get_debug_mesh_lines() { - Plane p = get_plane(); - Vector points; - - Vector3 n1 = p.get_any_perpendicular_normal(); - Vector3 n2 = p.normal.cross(n1).normalized(); - - Vector3 pface[4] = { - p.normal * p.d + n1 * 10.0 + n2 * 10.0, - p.normal * p.d + n1 * 10.0 + n2 * -10.0, - p.normal * p.d + n1 * -10.0 + n2 * -10.0, - p.normal * p.d + n1 * -10.0 + n2 * 10.0, - }; - - points.push_back(pface[0]); - points.push_back(pface[1]); - points.push_back(pface[1]); - points.push_back(pface[2]); - points.push_back(pface[2]); - points.push_back(pface[3]); - points.push_back(pface[3]); - points.push_back(pface[0]); - points.push_back(p.normal * p.d); - points.push_back(p.normal * p.d + p.normal * 3); - - return points; -} - -void PlaneShape::_update_shape() { - PhysicsServer::get_singleton()->shape_set_data(get_shape(), plane); - Shape::_update_shape(); -} - -void PlaneShape::set_plane(Plane p_plane) { - plane = p_plane; - _update_shape(); - notify_change_to_owners(); - _change_notify("plane"); -} - -Plane PlaneShape::get_plane() const { - return plane; -} - -void PlaneShape::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_plane", "plane"), &PlaneShape::set_plane); - ClassDB::bind_method(D_METHOD("get_plane"), &PlaneShape::get_plane); - - ADD_PROPERTY(PropertyInfo(Variant::PLANE, "plane"), "set_plane", "get_plane"); -} - -PlaneShape::PlaneShape() : - Shape(RID_PRIME(PhysicsServer::get_singleton()->shape_create(PhysicsServer::SHAPE_PLANE))) { - set_plane(Plane(0, 1, 0, 0)); -} diff --git a/scene/resources/shapes/plane_shape.h b/scene/resources/shapes/plane_shape.h deleted file mode 100644 index bcf2c99..0000000 --- a/scene/resources/shapes/plane_shape.h +++ /dev/null @@ -1,55 +0,0 @@ -#ifndef PLANE_SHAPE_H -#define PLANE_SHAPE_H -/*************************************************************************/ -/* plane_shape.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "scene/resources/shapes/shape.h" - -class PlaneShape : public Shape { - GDCLASS(PlaneShape, Shape); - Plane plane; - -protected: - static void _bind_methods(); - virtual void _update_shape(); - -public: - void set_plane(Plane p_plane); - Plane get_plane() const; - - virtual Vector get_debug_mesh_lines(); - virtual real_t get_enclosing_radius() const { - // Should be infinite? - return 0; - }; - - PlaneShape(); -}; -#endif // PLANE_SHAPE_H diff --git a/scene/resources/shapes/ray_shape.cpp b/scene/resources/shapes/ray_shape.cpp deleted file mode 100644 index 965a5e0..0000000 --- a/scene/resources/shapes/ray_shape.cpp +++ /dev/null @@ -1,98 +0,0 @@ -/*************************************************************************/ -/* ray_shape.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "ray_shape.h" - -#include "servers/physics_server.h" - -Vector RayShape::get_debug_mesh_lines() { - Vector points; - points.push_back(Vector3()); - points.push_back(Vector3(0, 0, get_length())); - - return points; -} - -real_t RayShape::get_enclosing_radius() const { - return length; -} - -void RayShape::_update_shape() { - Dictionary d; - d["length"] = length; - d["slips_on_slope"] = slips_on_slope; - PhysicsServer::get_singleton()->shape_set_data(get_shape(), d); - Shape::_update_shape(); -} - -void RayShape::set_length(float p_length) { - length = p_length; - _update_shape(); - notify_change_to_owners(); - _change_notify("length"); -} - -float RayShape::get_length() const { - return length; -} - -void RayShape::set_slips_on_slope(bool p_active) { - slips_on_slope = p_active; - _update_shape(); - notify_change_to_owners(); - _change_notify("slips_on_slope"); -} - -bool RayShape::get_slips_on_slope() const { - return slips_on_slope; -} - -void RayShape::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_length", "length"), &RayShape::set_length); - ClassDB::bind_method(D_METHOD("get_length"), &RayShape::get_length); - - ClassDB::bind_method(D_METHOD("set_slips_on_slope", "active"), &RayShape::set_slips_on_slope); - ClassDB::bind_method(D_METHOD("get_slips_on_slope"), &RayShape::get_slips_on_slope); - - ADD_PROPERTY(PropertyInfo(Variant::REAL, "length", PROPERTY_HINT_RANGE, "0.001,100,0.001,or_greater"), "set_length", "get_length"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "slips_on_slope"), "set_slips_on_slope", "get_slips_on_slope"); -} - -RayShape::RayShape() : - Shape(RID_PRIME(PhysicsServer::get_singleton()->shape_create(PhysicsServer::SHAPE_RAY))) { - length = 1.0; - slips_on_slope = false; - - /* Code copied from setters to prevent the use of uninitialized variables */ - _update_shape(); - notify_change_to_owners(); - _change_notify("length"); - _change_notify("slips_on_slope"); -} diff --git a/scene/resources/shapes/ray_shape.h b/scene/resources/shapes/ray_shape.h deleted file mode 100644 index 2c154cc..0000000 --- a/scene/resources/shapes/ray_shape.h +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef RAY_SHAPE_H -#define RAY_SHAPE_H -/*************************************************************************/ -/* ray_shape.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "scene/resources/shapes/shape.h" - -class RayShape : public Shape { - GDCLASS(RayShape, Shape); - float length; - bool slips_on_slope; - -protected: - static void _bind_methods(); - virtual void _update_shape(); - -public: - void set_length(float p_length); - float get_length() const; - - void set_slips_on_slope(bool p_active); - bool get_slips_on_slope() const; - - virtual Vector get_debug_mesh_lines(); - virtual real_t get_enclosing_radius() const; - - RayShape(); -}; -#endif // RAY_SHAPE_H diff --git a/scene/resources/shapes/shape.cpp b/scene/resources/shapes/shape.cpp deleted file mode 100644 index 761dc1b..0000000 --- a/scene/resources/shapes/shape.cpp +++ /dev/null @@ -1,122 +0,0 @@ -/*************************************************************************/ -/* shape.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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.h" - -#include "core/os/os.h" -#include "scene/main/scene_tree.h" -#include "scene/resources/mesh/mesh.h" -#include "servers/physics_server.h" - -void Shape::add_vertices_to_array(PoolVector &array, const Transform &p_xform) { - Vector toadd = get_debug_mesh_lines(); - - if (toadd.size()) { - int base = array.size(); - array.resize(base + toadd.size()); - PoolVector::Write w = array.write(); - for (int i = 0; i < toadd.size(); i++) { - w[i + base] = p_xform.xform(toadd[i]); - } - } -} - -real_t Shape::get_margin() const { - return margin; -} - -void Shape::set_margin(real_t p_margin) { - margin = p_margin; - PhysicsServer::get_singleton()->shape_set_margin(shape, margin); -} - -Ref Shape::get_debug_mesh() { - if (debug_mesh_cache.is_valid()) { - return debug_mesh_cache; - } - - Vector lines = get_debug_mesh_lines(); - - debug_mesh_cache = Ref(memnew(ArrayMesh)); - - if (!lines.empty()) { - //make mesh - PoolVector array; - array.resize(lines.size()); - { - PoolVector::Write w = array.write(); - for (int i = 0; i < lines.size(); i++) { - w[i] = lines[i]; - } - } - - Array arr; - arr.resize(Mesh::ARRAY_MAX); - arr[Mesh::ARRAY_VERTEX] = array; - - SceneTree *st = Object::cast_to(OS::get_singleton()->get_main_loop()); - - debug_mesh_cache->add_surface_from_arrays(Mesh::PRIMITIVE_LINES, arr); - - if (st) { - debug_mesh_cache->surface_set_material(0, st->get_debug_collision_material()); - } - } - - return debug_mesh_cache; -} - -void Shape::_update_shape() { - emit_changed(); - debug_mesh_cache.unref(); -} - -void Shape::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_margin", "margin"), &Shape::set_margin); - ClassDB::bind_method(D_METHOD("get_margin"), &Shape::get_margin); - - ClassDB::bind_method(D_METHOD("get_debug_mesh"), &Shape::get_debug_mesh); - - ADD_PROPERTY(PropertyInfo(Variant::REAL, "margin", PROPERTY_HINT_RANGE, "0.001,10,0.001"), "set_margin", "get_margin"); -} - -Shape::Shape() : - margin(0.04) { - ERR_PRINT("Constructor must not be called!"); -} - -Shape::Shape(RID p_shape) : - margin(0.04) { - shape = p_shape; -} - -Shape::~Shape() { - PhysicsServer::get_singleton()->free(shape); -} diff --git a/scene/resources/shapes/shape.h b/scene/resources/shapes/shape.h deleted file mode 100644 index 8f6b93f..0000000 --- a/scene/resources/shapes/shape.h +++ /dev/null @@ -1,70 +0,0 @@ -#ifndef SHAPE_H -#define SHAPE_H -/*************************************************************************/ -/* shape.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "core/object/resource.h" -class ArrayMesh; - -class Shape : public Resource { - GDCLASS(Shape, Resource); - OBJ_SAVE_TYPE(Shape); - RES_BASE_EXTENSION("shape"); - RID shape; - real_t margin; - - Ref debug_mesh_cache; - -protected: - static void _bind_methods(); - - _FORCE_INLINE_ RID get_shape() const { return shape; } - Shape(RID p_shape); - - virtual void _update_shape(); - -public: - virtual RID get_rid() const { return shape; } - - Ref get_debug_mesh(); - virtual Vector get_debug_mesh_lines() = 0; // { return Vector(); } - /// Returns the radius of a sphere that fully enclose this shape - virtual real_t get_enclosing_radius() const = 0; - - void add_vertices_to_array(PoolVector &array, const Transform &p_xform); - - real_t get_margin() const; - void set_margin(real_t p_margin); - - Shape(); - ~Shape(); -}; - -#endif // SHAPE_H diff --git a/scene/resources/shapes/sphere_shape.cpp b/scene/resources/shapes/sphere_shape.cpp deleted file mode 100644 index e2f9ba1..0000000 --- a/scene/resources/shapes/sphere_shape.cpp +++ /dev/null @@ -1,87 +0,0 @@ -/*************************************************************************/ -/* sphere_shape.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "sphere_shape.h" - -#include "servers/physics_server.h" - -Vector SphereShape::get_debug_mesh_lines() { - float r = get_radius(); - - Vector points; - - for (int i = 0; i <= 360; i++) { - float ra = Math::deg2rad((float)i); - float rb = Math::deg2rad((float)i + 1); - Point2 a = Vector2(Math::sin(ra), Math::cos(ra)) * r; - Point2 b = Vector2(Math::sin(rb), Math::cos(rb)) * r; - - points.push_back(Vector3(a.x, 0, a.y)); - points.push_back(Vector3(b.x, 0, b.y)); - points.push_back(Vector3(0, a.x, a.y)); - points.push_back(Vector3(0, b.x, b.y)); - points.push_back(Vector3(a.x, a.y, 0)); - points.push_back(Vector3(b.x, b.y, 0)); - } - - return points; -} - -real_t SphereShape::get_enclosing_radius() const { - return radius; -} - -void SphereShape::_update_shape() { - PhysicsServer::get_singleton()->shape_set_data(get_shape(), radius); - Shape::_update_shape(); -} - -void SphereShape::set_radius(float p_radius) { - radius = p_radius; - _update_shape(); - notify_change_to_owners(); - _change_notify("radius"); -} - -float SphereShape::get_radius() const { - return radius; -} - -void SphereShape::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_radius", "radius"), &SphereShape::set_radius); - ClassDB::bind_method(D_METHOD("get_radius"), &SphereShape::get_radius); - - ADD_PROPERTY(PropertyInfo(Variant::REAL, "radius", PROPERTY_HINT_RANGE, "0.001,100,0.001,or_greater"), "set_radius", "get_radius"); -} - -SphereShape::SphereShape() : - Shape(RID_PRIME(PhysicsServer::get_singleton()->shape_create(PhysicsServer::SHAPE_SPHERE))) { - set_radius(1.0); -} diff --git a/scene/resources/shapes/sphere_shape.h b/scene/resources/shapes/sphere_shape.h deleted file mode 100644 index c304f2e..0000000 --- a/scene/resources/shapes/sphere_shape.h +++ /dev/null @@ -1,54 +0,0 @@ -#ifndef SPHERE_SHAPE_H -#define SPHERE_SHAPE_H -/*************************************************************************/ -/* sphere_shape.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "scene/resources/shapes/shape.h" - -class SphereShape : public Shape { - GDCLASS(SphereShape, Shape); - float radius; - -protected: - static void _bind_methods(); - - virtual void _update_shape(); - -public: - void set_radius(float p_radius); - float get_radius() const; - - virtual Vector get_debug_mesh_lines(); - virtual real_t get_enclosing_radius() const; - - SphereShape(); -}; - -#endif // SPHERE_SHAPE_H diff --git a/scene/resources/world_3d.cpp b/scene/resources/world_3d.cpp index 7a20cb3..230b63a 100644 --- a/scene/resources/world_3d.cpp +++ b/scene/resources/world_3d.cpp @@ -304,10 +304,6 @@ Ref World3D::get_fallback_environment() const { return fallback_environment; } -PhysicsDirectSpaceState *World3D::get_direct_space_state() { - return PhysicsServer::get_singleton()->space_get_direct_state(space); -} - void World3D::get_camera_list(List *r_cameras) { for (RBMap::Element *E = indexer->cameras.front(); E; E = E->next()) { r_cameras->push_back(E->key()); @@ -336,7 +332,7 @@ void World3D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_environment"), &World3D::get_environment); ClassDB::bind_method(D_METHOD("set_fallback_environment", "env"), &World3D::set_fallback_environment); ClassDB::bind_method(D_METHOD("get_fallback_environment"), &World3D::get_fallback_environment); - ClassDB::bind_method(D_METHOD("get_direct_space_state"), &World3D::get_direct_space_state); + ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "environment", PROPERTY_HINT_RESOURCE_TYPE, "Environment3D"), "set_environment", "get_environment"); ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "fallback_environment", PROPERTY_HINT_RESOURCE_TYPE, "Environment3D"), "set_fallback_environment", "get_fallback_environment"); ADD_PROPERTY(PropertyInfo(Variant::RID, "space", PROPERTY_HINT_NONE, "", 0), "", "get_space"); @@ -346,17 +342,8 @@ void World3D::_bind_methods() { } World3D::World3D() { - space = RID_PRIME(PhysicsServer::get_singleton()->space_create()); scenario = RID_PRIME(RenderingServer::get_singleton()->scenario_create()); - PhysicsServer::get_singleton()->space_set_active(space, true); - PhysicsServer::get_singleton()->area_set_param(space, PhysicsServer::AREA_PARAM_GRAVITY, GLOBAL_DEF("physics/3d/default_gravity", 9.8)); - PhysicsServer::get_singleton()->area_set_param(space, PhysicsServer::AREA_PARAM_GRAVITY_VECTOR, GLOBAL_DEF("physics/3d/default_gravity_vector", Vector3(0, -1, 0))); - PhysicsServer::get_singleton()->area_set_param(space, PhysicsServer::AREA_PARAM_LINEAR_DAMP, GLOBAL_DEF("physics/3d/default_linear_damp", 0.1)); - ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/default_linear_damp", PropertyInfo(Variant::REAL, "physics/3d/default_linear_damp", PROPERTY_HINT_RANGE, "-1,100,0.001,or_greater")); - PhysicsServer::get_singleton()->area_set_param(space, PhysicsServer::AREA_PARAM_ANGULAR_DAMP, GLOBAL_DEF("physics/3d/default_angular_damp", 0.1)); - ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/default_angular_damp", PropertyInfo(Variant::REAL, "physics/3d/default_angular_damp", PROPERTY_HINT_RANGE, "-1,100,0.001,or_greater")); - // Create default navigation map navigation_map = NavigationServer::get_singleton()->map_create(); NavigationServer::get_singleton()->map_set_active(navigation_map, true); @@ -375,7 +362,6 @@ World3D::World3D() { } World3D::~World3D() { - PhysicsServer::get_singleton()->free(space); RenderingServer::get_singleton()->free(scenario); NavigationServer::get_singleton()->free(navigation_map); diff --git a/scene/resources/world_3d.h b/scene/resources/world_3d.h index 61d26c6..8d8f443 100644 --- a/scene/resources/world_3d.h +++ b/scene/resources/world_3d.h @@ -33,7 +33,6 @@ #include "core/object/resource.h" #include "scene/resources/environment_3d.h" -#include "servers/physics_server.h" #include "servers/rendering_server.h" class Camera; @@ -82,8 +81,6 @@ public: void get_camera_list(List *r_cameras); - PhysicsDirectSpaceState *get_direct_space_state(); - void move_cameras_into(Ref target); World3D(); diff --git a/servers/physics/SCsub b/servers/physics/SCsub deleted file mode 100644 index df7b521..0000000 --- a/servers/physics/SCsub +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env python - -Import("env") - -env.add_source_files(env.servers_sources, "*.cpp") - -SConscript("joints/SCsub") diff --git a/servers/physics/area_pair_sw.cpp b/servers/physics/area_pair_sw.cpp deleted file mode 100644 index 1aacfff..0000000 --- a/servers/physics/area_pair_sw.cpp +++ /dev/null @@ -1,155 +0,0 @@ -/*************************************************************************/ -/* area_pair_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" -#include "collision_solver_sw.h" - -bool AreaPairSW::setup(real_t p_step) { - bool result = false; - if (area->test_collision_mask(body) && CollisionSolverSW::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; - } - - if (result != colliding) { - if (result) { - if (area->get_space_override_mode() != PhysicsServer::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() != PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED) { - body->remove_area(area); - } - if (area->has_monitor_callback()) { - area->remove_body_from_query(body, body_shape, area_shape); - } - } - - colliding = result; - } - - return false; //never do any post solving -} - -void AreaPairSW::solve(real_t p_step) { -} - -AreaPairSW::AreaPairSW(BodySW *p_body, int p_body_shape, AreaSW *p_area, int p_area_shape) { - body = p_body; - area = p_area; - body_shape = p_body_shape; - area_shape = p_area_shape; - colliding = false; - body->add_constraint(this, 0); - area->add_constraint(this); - if (p_body->get_mode() == PhysicsServer::BODY_MODE_KINEMATIC) { - p_body->set_active(true); - } -} - -AreaPairSW::~AreaPairSW() { - if (colliding) { - if (area->get_space_override_mode() != PhysicsServer::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 Area2PairSW::setup(real_t p_step) { - bool result = false; - if (area_a->test_collision_mask(area_b) && CollisionSolverSW::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 = true; - } - - if (result != colliding) { - if (result) { - if (area_b->has_area_monitor_callback() && area_a_monitorable) { - area_b->add_area_to_query(area_a, shape_a, shape_b); - } - - if (area_a->has_area_monitor_callback() && area_b_monitorable) { - area_a->add_area_to_query(area_b, shape_b, shape_a); - } - - } else { - if (area_b->has_area_monitor_callback() && area_a_monitorable) { - area_b->remove_area_from_query(area_a, shape_a, shape_b); - } - - if (area_a->has_area_monitor_callback() && area_b_monitorable) { - area_a->remove_area_from_query(area_b, shape_b, shape_a); - } - } - - colliding = result; - } - - return false; //never do any post solving -} - -void Area2PairSW::solve(real_t p_step) { -} - -Area2PairSW::Area2PairSW(AreaSW *p_area_a, int p_shape_a, AreaSW *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; - colliding = false; - area_a_monitorable = area_a->is_monitorable(); - area_b_monitorable = area_b->is_monitorable(); - area_a->add_constraint(this); - area_b->add_constraint(this); -} - -Area2PairSW::~Area2PairSW() { - if (colliding) { - if (area_b->has_area_monitor_callback() && area_a_monitorable) { - area_b->remove_area_from_query(area_a, shape_a, shape_b); - } - - if (area_a->has_area_monitor_callback() && area_b_monitorable) { - area_a->remove_area_from_query(area_b, shape_b, shape_a); - } - } - - area_a->remove_constraint(this); - area_b->remove_constraint(this); -} diff --git a/servers/physics/area_pair_sw.h b/servers/physics/area_pair_sw.h deleted file mode 100644 index 1246efd..0000000 --- a/servers/physics/area_pair_sw.h +++ /dev/null @@ -1,69 +0,0 @@ -#ifndef AREA_PAIR_SW_H -#define AREA_PAIR_SW_H -/*************************************************************************/ -/* area_pair_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" -#include "body_sw.h" -#include "constraint_sw.h" - -class AreaPairSW : public ConstraintSW { - BodySW *body; - AreaSW *area; - int body_shape; - int area_shape; - bool colliding; - -public: - bool setup(real_t p_step); - void solve(real_t p_step); - - AreaPairSW(BodySW *p_body, int p_body_shape, AreaSW *p_area, int p_area_shape); - ~AreaPairSW(); -}; - -class Area2PairSW : public ConstraintSW { - AreaSW *area_a; - AreaSW *area_b; - int shape_a; - int shape_b; - bool colliding; - bool area_a_monitorable; - bool area_b_monitorable; - -public: - bool setup(real_t p_step); - void solve(real_t p_step); - - Area2PairSW(AreaSW *p_area_a, int p_shape_a, AreaSW *p_area_b, int p_shape_b); - ~Area2PairSW(); -}; - -#endif // AREA_PAIR__SW_H diff --git a/servers/physics/area_sw.cpp b/servers/physics/area_sw.cpp deleted file mode 100644 index a01f5e1..0000000 --- a/servers/physics/area_sw.cpp +++ /dev/null @@ -1,296 +0,0 @@ -/*************************************************************************/ -/* area_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" -#include "body_sw.h" -#include "space_sw.h" - -AreaSW::BodyKey::BodyKey(BodySW *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; -} -AreaSW::BodyKey::BodyKey(AreaSW *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 AreaSW::_shapes_changed() { - if (!moved_list.in_list() && get_space()) { - get_space()->area_add_to_moved_list(&moved_list); - } -} - -void AreaSW::set_transform(const Transform &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 AreaSW::set_space(SpaceSW *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 AreaSW::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 AreaSW::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 AreaSW::set_space_override_mode(PhysicsServer::AreaSpaceOverrideMode p_mode) { - bool do_override = p_mode != PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED; - if (do_override == (space_override_mode != PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED)) { - return; - } - _unregister_shapes(); - space_override_mode = p_mode; - _shape_changed(); -} - -void AreaSW::set_param(PhysicsServer::AreaParameter p_param, const Variant &p_value) { - switch (p_param) { - case PhysicsServer::AREA_PARAM_GRAVITY: - gravity = p_value; - break; - case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR: - gravity_vector = p_value; - break; - case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT: - gravity_is_point = p_value; - break; - case PhysicsServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE: - gravity_distance_scale = p_value; - break; - case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: - point_attenuation = p_value; - break; - case PhysicsServer::AREA_PARAM_LINEAR_DAMP: - linear_damp = p_value; - break; - case PhysicsServer::AREA_PARAM_ANGULAR_DAMP: - angular_damp = p_value; - break; - case PhysicsServer::AREA_PARAM_PRIORITY: - priority = p_value; - break; - } -} - -Variant AreaSW::get_param(PhysicsServer::AreaParameter p_param) const { - switch (p_param) { - case PhysicsServer::AREA_PARAM_GRAVITY: - return gravity; - case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR: - return gravity_vector; - case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT: - return gravity_is_point; - case PhysicsServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE: - return gravity_distance_scale; - case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: - return point_attenuation; - case PhysicsServer::AREA_PARAM_LINEAR_DAMP: - return linear_damp; - case PhysicsServer::AREA_PARAM_ANGULAR_DAMP: - return angular_damp; - case PhysicsServer::AREA_PARAM_PRIORITY: - return priority; - } - - return Variant(); -} - -void AreaSW::_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 AreaSW::set_monitorable(bool p_monitorable) { - if (monitorable == p_monitorable) { - return; - } - - monitorable = p_monitorable; - _set_static(!monitorable); - _shapes_changed(); -} - -void AreaSW::call_queries() { - if (monitor_callback_id && !monitored_bodies.empty()) { - Object *obj = ObjectDB::get_instance(monitor_callback_id); - if (obj) { - Variant res[5]; - Variant *resptr[5]; - for (int i = 0; i < 5; i++) { - resptr[i] = &res[i]; - } - - for (RBMap::Element *E = monitored_bodies.front(); E;) { - if (E->get().state == 0) { // Nothing happened - RBMap::Element *next = E->next(); - monitored_bodies.erase(E); - E = next; - continue; - } - - res[0] = E->get().state > 0 ? PhysicsServer::AREA_BODY_ADDED : PhysicsServer::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; - - RBMap::Element *next = E->next(); - monitored_bodies.erase(E); - E = next; - - Variant::CallError ce; - obj->call(monitor_callback_method, (const Variant **)resptr, 5, ce); - } - } else { - monitored_bodies.clear(); - monitor_callback_id = 0; - } - } - - if (area_monitor_callback_id && !monitored_areas.empty()) { - Object *obj = ObjectDB::get_instance(area_monitor_callback_id); - if (obj) { - Variant res[5]; - Variant *resptr[5]; - for (int i = 0; i < 5; i++) { - resptr[i] = &res[i]; - } - - for (RBMap::Element *E = monitored_areas.front(); E;) { - if (E->get().state == 0) { // Nothing happened - RBMap::Element *next = E->next(); - monitored_areas.erase(E); - E = next; - continue; - } - - res[0] = E->get().state > 0 ? PhysicsServer::AREA_BODY_ADDED : PhysicsServer::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; - - RBMap::Element *next = E->next(); - monitored_areas.erase(E); - E = next; - - Variant::CallError ce; - obj->call(area_monitor_callback_method, (const Variant **)resptr, 5, ce); - } - } else { - monitored_areas.clear(); - area_monitor_callback_id = 0; - } - } -} - -AreaSW::AreaSW() : - CollisionObjectSW(TYPE_AREA), - monitor_query_list(this), - moved_list(this) { - _set_static(true); //areas are never active - space_override_mode = PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED; - gravity = 9.80665; - gravity_vector = Vector3(0, -1, 0); - gravity_is_point = false; - gravity_distance_scale = 0; - point_attenuation = 1; - angular_damp = 0.1; - linear_damp = 0.1; - priority = 0; - set_ray_pickable(false); - monitor_callback_id = 0; - area_monitor_callback_id = 0; - monitorable = false; -} - -AreaSW::~AreaSW() { -} diff --git a/servers/physics/area_sw.h b/servers/physics/area_sw.h deleted file mode 100644 index 4f9973e..0000000 --- a/servers/physics/area_sw.h +++ /dev/null @@ -1,198 +0,0 @@ -#ifndef AREA_SW_H -#define AREA_SW_H -/*************************************************************************/ -/* area_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" -#include "core/containers/self_list.h" -#include "servers/physics_server.h" -//#include "servers/physics/query_sw.h" - -class SpaceSW; -class BodySW; -class ConstraintSW; - -class AreaSW : public CollisionObjectSW { - PhysicsServer::AreaSpaceOverrideMode space_override_mode; - real_t gravity; - Vector3 gravity_vector; - bool gravity_is_point; - real_t gravity_distance_scale; - real_t point_attenuation; - real_t linear_damp; - real_t angular_damp; - int priority; - bool monitorable; - - 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; - uint32_t area_shape; - - _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(BodySW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); - BodyKey(AreaSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); - }; - - struct BodyState { - int state; - _FORCE_INLINE_ void inc() { state++; } - _FORCE_INLINE_ void dec() { state--; } - _FORCE_INLINE_ BodyState() { state = 0; } - }; - - RBMap monitored_bodies; - RBMap monitored_areas; - - //virtual void shape_changed_notify(ShapeSW *p_shape); - //virtual void shape_deleted_notify(ShapeSW *p_shape); - - RBSet constraints; - - virtual void _shapes_changed(); - void _queue_monitor_update(); - -public: - //_FORCE_INLINE_ const Transform& get_inverse_transform() const { return inverse_transform; } - //_FORCE_INLINE_ SpaceSW* get_owner() { return owner; } - - void set_monitor_callback(ObjectID p_id, const StringName &p_method); - _FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id; } - - 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; } - - _FORCE_INLINE_ void add_body_to_query(BodySW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); - _FORCE_INLINE_ void remove_body_from_query(BodySW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); - - _FORCE_INLINE_ void add_area_to_query(AreaSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape); - _FORCE_INLINE_ void remove_area_from_query(AreaSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape); - - void set_param(PhysicsServer::AreaParameter p_param, const Variant &p_value); - Variant get_param(PhysicsServer::AreaParameter p_param) const; - - void set_space_override_mode(PhysicsServer::AreaSpaceOverrideMode p_mode); - PhysicsServer::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 add_constraint(ConstraintSW *p_constraint) { constraints.insert(p_constraint); } - _FORCE_INLINE_ void remove_constraint(ConstraintSW *p_constraint) { constraints.erase(p_constraint); } - _FORCE_INLINE_ const RBSet &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 Transform &p_transform); - - void set_space(SpaceSW *p_space); - - void call_queries(); - - AreaSW(); - ~AreaSW(); -}; - -void AreaSW::add_body_to_query(BodySW *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 AreaSW::remove_body_from_query(BodySW *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 AreaSW::add_area_to_query(AreaSW *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 AreaSW::remove_area_from_query(AreaSW *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__SW_H diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp deleted file mode 100644 index 51a00fb..0000000 --- a/servers/physics/body_pair_sw.cpp +++ /dev/null @@ -1,468 +0,0 @@ -/*************************************************************************/ -/* body_pair_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" - -#include "collision_solver_sw.h" -#include "core/os/os.h" -#include "space_sw.h" - -#define MIN_VELOCITY 0.0001 -#define MAX_BIAS_ROTATION (Math_PI / 8) - -void BodyPairSW::_contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) { - BodyPairSW *pair = (BodyPairSW *)p_userdata; - pair->contact_added_callback(p_point_A, p_point_B); -} - -void BodyPairSW::contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_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.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 BodyPairSW::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 BodyPairSW::_test_ccd(real_t p_step, BodySW *p_A, int p_shape_A, const Transform &p_xform_A, BodySW *p_B, int p_shape_B, const Transform &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; - - Transform 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(BodySW *A, BodySW *B) { - return CLAMP(A->get_bounce() + B->get_bounce(), 0, 1); -} - -real_t combine_friction(BodySW *A, BodySW *B) { - return ABS(MIN(A->get_friction(), B->get_friction())); -} - -bool BodyPairSW::setup(real_t p_step) { - //cannot collide - if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) { - collided = false; - return false; - } - - bool report_contacts_only = false; - if ((A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC)) { - 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(); - - Vector3 offset_A = A->get_transform().get_origin(); - Transform xform_Au = Transform(A->get_transform().basis, Vector3()); - Transform xform_A = xform_Au * A->get_shape_transform(shape_A); - - Transform xform_Bu = B->get_transform(); - xform_Bu.origin -= offset_A; - Transform xform_B = xform_Bu * B->get_shape_transform(shape_B); - - ShapeSW *shape_A_ptr = A->get_shape(shape_A); - ShapeSW *shape_B_ptr = B->get_shape(shape_B); - - bool collided = CollisionSolverSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); - this->collided = collided; - - if (!collided) { - //test ccd (currently just a raycast) - - if (A->is_continuous_collision_detection_enabled() && A->get_mode() > PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC) { - _test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B); - } - - if (B->is_continuous_collision_detection_enabled() && B->get_mode() > PhysicsServer::BODY_MODE_KINEMATIC && A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC) { - _test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A); - } - - return false; - } - - real_t max_penetration = space->get_contact_max_allowed_penetration(); - - real_t bias = (real_t)0.3; - - 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; - - for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; - c.active = false; - - Vector3 global_A = xform_Au.xform(c.local_A); - Vector3 global_B = xform_Bu.xform(c.local_B); - - real_t depth = c.normal.dot(global_A - global_B); - - if (depth <= 0) { - continue; - } - -#ifdef DEBUG_ENABLED - - if (space->is_debugging_contacts()) { - space->add_debug_contact(global_A + offset_A); - space->add_debug_contact(global_B + offset_A); - } -#endif - - c.rA = global_A - A->get_center_of_mass(); - c.rB = global_B - B->get_center_of_mass() - offset_B; - - // 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; - - // Precompute normal mass, tangent mass, and bias. - Vector3 inertia_A = A->get_inv_inertia_tensor().xform(c.rA.cross(c.normal)); - Vector3 inertia_B = B->get_inv_inertia_tensor().xform(c.rB.cross(c.normal)); - real_t kNormal = A->get_inv_mass() + B->get_inv_mass(); - 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; - A->apply_impulse(c.rA + A->get_center_of_mass(), -j_vec); - B->apply_impulse(c.rB + B->get_center_of_mass(), j_vec); - 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_prev_angular_velocity().cross(c.rA); - Vector3 crB = B->get_prev_angular_velocity().cross(c.rB); - Vector3 dv = B->get_prev_linear_velocity() + crB - A->get_prev_linear_velocity() - crA; - //normal impule - c.bounce = c.bounce * dv.dot(c.normal); - } - } - - return true; -} - -void BodyPairSW::solve(real_t p_step) { - if (!collided) { - return; - } - - 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); - - A->apply_bias_impulse(c.rA + A->get_center_of_mass(), -jb, MAX_BIAS_ROTATION / p_step); - B->apply_bias_impulse(c.rB + B->get_center_of_mass(), jb, MAX_BIAS_ROTATION / p_step); - - 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) / (A->get_inv_mass() + B->get_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); - - A->apply_bias_impulse(A->get_center_of_mass(), -jb_com, 0.0f); - B->apply_bias_impulse(B->get_center_of_mass(), jb_com, 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); - - A->apply_impulse(c.rA + A->get_center_of_mass(), -j); - B->apply_impulse(c.rB + B->get_center_of_mass(), j); - - c.active = true; - } - - //friction impulse - - real_t friction = combine_friction(A, B); - - Vector3 lvA = A->get_linear_velocity() + A->get_angular_velocity().cross(c.rA); - Vector3 lvB = B->get_linear_velocity() + B->get_angular_velocity().cross(c.rB); - - Vector3 dtv = lvB - lvA; - real_t tn = c.normal.dot(dtv); - - // tangential velocity - Vector3 tv = dtv - c.normal * tn; - real_t tvl = tv.length(); - - if (tvl > MIN_VELOCITY) { - tv /= tvl; - - Vector3 temp1 = A->get_inv_inertia_tensor().xform(c.rA.cross(tv)); - Vector3 temp2 = B->get_inv_inertia_tensor().xform(c.rB.cross(tv)); - - real_t t = -tvl / (A->get_inv_mass() + B->get_inv_mass() + 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; - - A->apply_impulse(c.rA + A->get_center_of_mass(), -jt); - B->apply_impulse(c.rB + B->get_center_of_mass(), jt); - - c.active = true; - } - } -} - -BodyPairSW::BodyPairSW(BodySW *p_A, int p_shape_A, BodySW *p_B, int p_shape_B) : - ConstraintSW(_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); - contact_count = 0; - collided = false; -} - -BodyPairSW::~BodyPairSW() { - A->remove_constraint(this); - B->remove_constraint(this); -} diff --git a/servers/physics/body_pair_sw.h b/servers/physics/body_pair_sw.h deleted file mode 100644 index bb1f96d..0000000 --- a/servers/physics/body_pair_sw.h +++ /dev/null @@ -1,95 +0,0 @@ -#ifndef BODY_PAIR_SW_H -#define BODY_PAIR_SW_H -/*************************************************************************/ -/* body_pair_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" -#include "constraint_sw.h" - -class BodyPairSW : public ConstraintSW { - enum { - - MAX_CONTACTS = 4 - }; - - union { - struct { - BodySW *A; - BodySW *B; - }; - - BodySW *_arr[2]; - }; - - int shape_A; - int shape_B; - - struct Contact { - Vector3 position; - Vector3 normal; - Vector3 local_A, local_B; - real_t acc_normal_impulse; // accumulated normal impulse (Pn) - Vector3 acc_tangent_impulse; // accumulated tangent impulse (Pt) - real_t acc_bias_impulse; // accumulated normal impulse for position bias (Pnb) - real_t acc_bias_impulse_center_of_mass; // accumulated normal impulse for position bias applied to com - real_t mass_normal; - real_t bias; - real_t bounce; - - real_t depth; - bool active; - Vector3 rA, rB; // Offset in world orientation with respect to center of mass - }; - - Vector3 offset_B; //use local A coordinates to avoid numerical issues on collision detection - - Vector3 sep_axis; - Contact contacts[MAX_CONTACTS]; - int contact_count; - bool collided; - - static void _contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata); - - void contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B); - - void validate_contacts(); - bool _test_ccd(real_t p_step, BodySW *p_A, int p_shape_A, const Transform &p_xform_A, BodySW *p_B, int p_shape_B, const Transform &p_xform_B); - - SpaceSW *space; - -public: - bool setup(real_t p_step); - void solve(real_t p_step); - - BodyPairSW(BodySW *p_A, int p_shape_A, BodySW *p_B, int p_shape_B); - ~BodyPairSW(); -}; - -#endif // BODY_PAIR__SW_H diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp deleted file mode 100644 index fddde24..0000000 --- a/servers/physics/body_sw.cpp +++ /dev/null @@ -1,814 +0,0 @@ -/*************************************************************************/ -/* body_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" -#include "area_sw.h" -#include "space_sw.h" - -void BodySW::_update_inertia() { - if (get_space() && !inertia_update_list.in_list()) { - get_space()->body_add_to_inertia_update_list(&inertia_update_list); - } -} - -void BodySW::_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 BodySW::update_inertias() { - // Update shapes and motions. - - switch (mode) { - case PhysicsServer::BODY_MODE_RIGID: { - // Update tensor for all shapes, not the best way but should be somehow OK. (inspired from bullet) - 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); - } - - // 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; - } - - // 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 ShapeSW *shape = get_shape(i); - - real_t mass = area * this->mass / total_area; - - Basis shape_inertia_tensor = shape->get_moment_of_inertia(mass).to_diagonal_matrix(); - Transform 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.set_diagonal(Vector3(1.0, 1.0, 1.0)); - } - - // 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 PhysicsServer::BODY_MODE_KINEMATIC: - case PhysicsServer::BODY_MODE_STATIC: { - _inv_inertia_tensor.set_zero(); - _inv_mass = 0; - } break; - case PhysicsServer::BODY_MODE_CHARACTER: { - _inv_inertia_tensor.set_zero(); - _inv_mass = 1.0 / mass; - - } break; - } - - //_update_shapes(); - - _update_transform_dependant(); -} - -void BodySW::set_active(bool p_active) { - if (active == p_active) { - return; - } - - active = p_active; - if (!p_active) { - if (get_space()) { - get_space()->body_remove_from_active_list(&active_list); - } - } else { - if (mode == PhysicsServer::BODY_MODE_STATIC) { - return; //static bodies can't become active - } - if (get_space()) { - get_space()->body_add_to_active_list(&active_list); - } - - //still_time=0; - } - /* - if (!space) - return; - - for(int i=0;i0) { - get_space()->get_broadphase()->set_active(s.bpid,active); - } - } -*/ -} - -void BodySW::set_param(PhysicsServer::BodyParameter p_param, real_t p_value) { - switch (p_param) { - case PhysicsServer::BODY_PARAM_BOUNCE: { - bounce = p_value; - } break; - case PhysicsServer::BODY_PARAM_FRICTION: { - friction = p_value; - } break; - case PhysicsServer::BODY_PARAM_MASS: { - ERR_FAIL_COND(p_value <= 0); - mass = p_value; - _update_inertia(); - - } break; - case PhysicsServer::BODY_PARAM_GRAVITY_SCALE: { - gravity_scale = p_value; - } break; - case PhysicsServer::BODY_PARAM_LINEAR_DAMP: { - linear_damp = p_value; - } break; - case PhysicsServer::BODY_PARAM_ANGULAR_DAMP: { - angular_damp = p_value; - } break; - default: { - } - } -} - -real_t BodySW::get_param(PhysicsServer::BodyParameter p_param) const { - switch (p_param) { - case PhysicsServer::BODY_PARAM_BOUNCE: { - return bounce; - } break; - case PhysicsServer::BODY_PARAM_FRICTION: { - return friction; - } break; - case PhysicsServer::BODY_PARAM_MASS: { - return mass; - } break; - case PhysicsServer::BODY_PARAM_GRAVITY_SCALE: { - return gravity_scale; - } break; - case PhysicsServer::BODY_PARAM_LINEAR_DAMP: { - return linear_damp; - } break; - case PhysicsServer::BODY_PARAM_ANGULAR_DAMP: { - return angular_damp; - } break; - - default: { - } - } - - return 0; -} - -void BodySW::set_mode(PhysicsServer::BodyMode p_mode) { - PhysicsServer::BodyMode prev = mode; - mode = p_mode; - - switch (p_mode) { - //CLEAR UP EVERYTHING IN CASE IT NOT WORKS! - case PhysicsServer::BODY_MODE_STATIC: - case PhysicsServer::BODY_MODE_KINEMATIC: { - _set_inv_transform(get_transform().affine_inverse()); - _inv_mass = 0; - _set_static(p_mode == PhysicsServer::BODY_MODE_STATIC); - //set_active(p_mode==PhysicsServer::BODY_MODE_KINEMATIC); - set_active(p_mode == PhysicsServer::BODY_MODE_KINEMATIC && contacts.size()); - linear_velocity = Vector3(); - angular_velocity = Vector3(); - if (mode == PhysicsServer::BODY_MODE_KINEMATIC && prev != mode) { - first_time_kinematic = true; - } - - } break; - case PhysicsServer::BODY_MODE_RIGID: { - _inv_mass = mass > 0 ? (1.0 / mass) : 0; - _set_static(false); - set_active(true); - - } break; - case PhysicsServer::BODY_MODE_CHARACTER: { - _inv_mass = mass > 0 ? (1.0 / mass) : 0; - _set_static(false); - set_active(true); - angular_velocity = Vector3(); - } break; - } - - _update_inertia(); - /* - if (get_space()) - _update_queries(); - */ -} -PhysicsServer::BodyMode BodySW::get_mode() const { - return mode; -} - -void BodySW::_shapes_changed() { - _update_inertia(); - wakeup(); - wakeup_neighbours(); -} - -void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant &p_variant) { - switch (p_state) { - case PhysicsServer::BODY_STATE_TRANSFORM: { - if (mode == PhysicsServer::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 == PhysicsServer::BODY_MODE_STATIC) { - _set_transform(p_variant); - _set_inv_transform(get_transform().affine_inverse()); - wakeup_neighbours(); - } else { - Transform 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 PhysicsServer::BODY_STATE_LINEAR_VELOCITY: { - /* - if (mode==PhysicsServer::BODY_MODE_STATIC) - break; - */ - linear_velocity = p_variant; - wakeup(); - } break; - case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: { - /* - if (mode!=PhysicsServer::BODY_MODE_RIGID) - break; - */ - angular_velocity = p_variant; - wakeup(); - - } break; - case PhysicsServer::BODY_STATE_SLEEPING: { - //? - if (mode == PhysicsServer::BODY_MODE_STATIC || mode == PhysicsServer::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 PhysicsServer::BODY_STATE_CAN_SLEEP: { - can_sleep = p_variant; - if (mode == PhysicsServer::BODY_MODE_RIGID && !active && !can_sleep) { - set_active(true); - } - - } break; - } -} -Variant BodySW::get_state(PhysicsServer::BodyState p_state) const { - switch (p_state) { - case PhysicsServer::BODY_STATE_TRANSFORM: { - return get_transform(); - } break; - case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: { - return linear_velocity; - } break; - case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: { - return angular_velocity; - } break; - case PhysicsServer::BODY_STATE_SLEEPING: { - return !is_active(); - } break; - case PhysicsServer::BODY_STATE_CAN_SLEEP: { - return can_sleep; - } break; - } - - return Variant(); -} - -void BodySW::set_space(SpaceSW *p_space) { - if (get_space()) { - if (inertia_update_list.in_list()) { - get_space()->body_remove_from_inertia_update_list(&inertia_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()) { - _update_inertia(); - if (active) { - get_space()->body_add_to_active_list(&active_list); - } - /* - _update_queries(); - if (is_active()) { - active=false; - set_active(true); - } - */ - } - - first_integration = true; -} - -void BodySW::_compute_area_gravity_and_dampenings(const AreaSW *p_area) { - if (p_area->is_gravity_point()) { - if (p_area->get_gravity_distance_scale() > 0) { - Vector3 v = p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin(); - gravity += v.normalized() * (p_area->get_gravity() / Math::pow(v.length() * p_area->get_gravity_distance_scale() + 1, 2)); - } else { - gravity += (p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin()).normalized() * p_area->get_gravity(); - } - } else { - gravity += p_area->get_gravity_vector() * p_area->get_gravity(); - } - - area_linear_damp += p_area->get_linear_damp(); - area_angular_damp += p_area->get_angular_damp(); -} - -void BodySW::set_axis_lock(PhysicsServer::BodyAxis p_axis, bool lock) { - if (lock) { - locked_axis |= p_axis; - } else { - locked_axis &= ~p_axis; - } -} - -bool BodySW::is_axis_locked(PhysicsServer::BodyAxis p_axis) const { - return locked_axis & p_axis; -} - -void BodySW::integrate_forces(real_t p_step) { - if (mode == PhysicsServer::BODY_MODE_STATIC) { - return; - } - - AreaSW *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--) { - PhysicsServer::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode(); - switch (mode) { - case PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE: - case PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { - _compute_area_gravity_and_dampenings(aa[i].area); - stopped = mode == PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; - } break; - case PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE: - case PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { - gravity = Vector3(0, 0, 0); - area_angular_damp = 0; - area_linear_damp = 0; - _compute_area_gravity_and_dampenings(aa[i].area); - stopped = mode == PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE; - } break; - default: { - } - } - } - } - - if (!stopped) { - _compute_area_gravity_and_dampenings(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(); - */ - - prev_linear_velocity = linear_velocity; - prev_angular_velocity = angular_velocity; - - Vector3 motion; - bool do_motion = false; - - if (mode == PhysicsServer::BODY_MODE_KINEMATIC) { - //compute motion, angular and etc. velocities from prev transform - motion = new_transform.origin - get_transform().origin; - do_motion = true; - 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 = axis * (angle / p_step); - } else { - if (!omit_force_integration && !first_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(); - first_integration = false; - - //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 BodySW::integrate_velocities(real_t p_step) { - if (mode == PhysicsServer::BODY_MODE_STATIC) { - return; - } - - if (fi_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((PhysicsServer::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((PhysicsServer::BodyAxis)(1 << (i + 3)))) { - angular_velocity[i] = 0; - biased_angular_velocity[i] = 0; - } - } - - if (mode == PhysicsServer::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(); - Transform 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); - */ -} - -/* -void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) { - - Transform inv_xform = p_xform.affine_inverse(); - if (!get_space()) { - _set_transform(p_xform); - _set_inv_transform(inv_xform); - - return; - } - - //compute a FAKE linear velocity - this is easy - - linear_velocity=(p_xform.origin - get_transform().origin)/p_step; - - //compute a FAKE angular velocity, not so easy - Basis rot=get_transform().basis.orthonormalized().transposed() * p_xform.basis.orthonormalized(); - Vector3 axis; - real_t angle; - - rot.get_axis_angle(axis,angle); - axis.normalize(); - angular_velocity=axis.normalized() * (angle/p_step); - linear_velocity = (p_xform.origin - get_transform().origin)/p_step; - - if (!direct_state_query_list.in_list())// - callalways, so lv and av are cleared && (state_query || direct_state_query)) - get_space()->body_add_to_state_query_list(&direct_state_query_list); - simulated_motion=true; - _set_transform(p_xform); - - -} -*/ - -void BodySW::wakeup_neighbours() { - for (RBMap::Element *E = constraint_map.front(); E; E = E->next()) { - const ConstraintSW *c = E->key(); - BodySW **n = c->get_body_ptr(); - int bc = c->get_body_count(); - - for (int i = 0; i < bc; i++) { - if (i == E->get()) { - continue; - } - BodySW *b = n[i]; - if (b->mode != PhysicsServer::BODY_MODE_RIGID) { - continue; - } - - if (!b->is_active()) { - b->set_active(true); - } - } - } -} - -void BodySW::call_queries() { - if (fi_callback) { - Variant v = direct_access; - - Object *obj = ObjectDB::get_instance(fi_callback->id); - if (!obj) { - set_force_integration_callback(0, StringName()); - } else { - const Variant *vp[2] = { &v, &fi_callback->udata }; - - Variant::CallError ce; - int argc = (fi_callback->udata.get_type() == Variant::NIL) ? 1 : 2; - obj->call(fi_callback->method, vp, argc, ce); - } - } -} - -bool BodySW::sleep_test(real_t p_step) { - if (mode == PhysicsServer::BODY_MODE_STATIC || mode == PhysicsServer::BODY_MODE_KINEMATIC) { - return true; // - } else if (mode == PhysicsServer::BODY_MODE_CHARACTER) { - return !active; // characters don't sleep unless asked to sleep - } 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 BodySW::set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata) { - if (fi_callback) { - memdelete(fi_callback); - fi_callback = nullptr; - } - - if (p_id != 0) { - fi_callback = memnew(ForceIntegrationCallback); - fi_callback->id = p_id; - fi_callback->method = p_method; - fi_callback->udata = p_udata; - } -} - -void BodySW::set_kinematic_margin(real_t p_margin) { - kinematic_safe_margin = p_margin; -} - -BodySW::BodySW() : - CollisionObjectSW(TYPE_BODY), - locked_axis(0), - active_list(this), - inertia_update_list(this), - direct_state_query_list(this) { - mode = PhysicsServer::BODY_MODE_RIGID; - active = true; - - mass = 1; - kinematic_safe_margin = 0.001; - //_inv_inertia=Transform(); - _inv_mass = 1; - bounce = 0; - friction = 1; - omit_force_integration = false; - //applied_torque=0; - island_step = 0; - island_next = nullptr; - island_list_next = nullptr; - first_time_kinematic = false; - first_integration = false; - _set_static(false); - - contact_count = 0; - gravity_scale = 1.0; - linear_damp = -1; - angular_damp = -1; - area_angular_damp = 0; - area_linear_damp = 0; - - still_time = 0; - continuous_cd = false; - can_sleep = true; - fi_callback = nullptr; - - direct_access = memnew(PhysicsDirectBodyStateSW); - direct_access->body = this; -} - -BodySW::~BodySW() { - memdelete(direct_access); - if (fi_callback) { - memdelete(fi_callback); - } -} - -PhysicsDirectSpaceState *PhysicsDirectBodyStateSW::get_space_state() { - return body->get_space()->get_direct_state(); -} - -real_t PhysicsDirectBodyStateSW::get_step() const { - return body->get_space()->get_step(); -} diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h deleted file mode 100644 index 2bcc8a9..0000000 --- a/servers/physics/body_sw.h +++ /dev/null @@ -1,494 +0,0 @@ -#ifndef BODY_SW_H -#define BODY_SW_H -/*************************************************************************/ -/* body_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" -#include "collision_object_sw.h" -#include "core/containers/vset.h" - -class ConstraintSW; -class PhysicsDirectBodyStateSW; - -class BodySW : public CollisionObjectSW { - PhysicsServer::BodyMode mode; - - Vector3 linear_velocity; - Vector3 angular_velocity; - - Vector3 prev_linear_velocity; - Vector3 prev_angular_velocity; - - Vector3 biased_linear_velocity; - Vector3 biased_angular_velocity; - real_t mass; - real_t bounce; - real_t friction; - - real_t linear_damp; - real_t angular_damp; - real_t gravity_scale; - - uint16_t locked_axis; - - real_t kinematic_safe_margin; - real_t _inv_mass; - 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; - - Vector3 gravity; - - real_t still_time; - - Vector3 applied_force; - Vector3 applied_torque; - - real_t area_angular_damp; - real_t area_linear_damp; - - SelfList active_list; - SelfList inertia_update_list; - SelfList direct_state_query_list; - - VSet exceptions; - bool omit_force_integration; - bool active; - - bool first_integration; - - bool continuous_cd; - bool can_sleep; - bool first_time_kinematic; - void _update_inertia(); - virtual void _shapes_changed(); - Transform new_transform; - - RBMap constraint_map; - - struct AreaCMP { - AreaSW *area; - int refCount; - _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(AreaSW *p_area) { - area = p_area; - refCount = 1; - } - }; - - Vector areas; - - struct Contact { - Vector3 local_pos; - Vector3 local_normal; - real_t depth; - int local_shape; - Vector3 collider_pos; - int collider_shape; - ObjectID collider_instance_id; - RID collider; - Vector3 collider_velocity_at_pos; - }; - - Vector contacts; //no contacts by default - int contact_count; - - struct ForceIntegrationCallback { - ObjectID id; - StringName method; - Variant udata; - }; - - ForceIntegrationCallback *fi_callback; - - uint64_t island_step; - BodySW *island_next; - BodySW *island_list_next; - - _FORCE_INLINE_ void _compute_area_gravity_and_dampenings(const AreaSW *p_area); - - _FORCE_INLINE_ void _update_transform_dependant(); - - PhysicsDirectBodyStateSW *direct_access = nullptr; - friend class PhysicsDirectBodyStateSW; // i give up, too many functions to expose - -public: - void set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant()); - - void set_kinematic_margin(real_t p_margin); - _FORCE_INLINE_ real_t get_kinematic_margin() { return kinematic_safe_margin; } - - _FORCE_INLINE_ void add_area(AreaSW *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(AreaSW *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 == PhysicsServer::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.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_ BodySW *get_island_next() const { return island_next; } - _FORCE_INLINE_ void set_island_next(BodySW *p_next) { island_next = p_next; } - - _FORCE_INLINE_ BodySW *get_island_list_next() const { return island_list_next; } - _FORCE_INLINE_ void set_island_list_next(BodySW *p_next) { island_list_next = p_next; } - - _FORCE_INLINE_ void add_constraint(ConstraintSW *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; } - _FORCE_INLINE_ void remove_constraint(ConstraintSW *p_constraint) { constraint_map.erase(p_constraint); } - const RBMap &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_ Vector3 get_prev_linear_velocity() const { return prev_linear_velocity; } - _FORCE_INLINE_ Vector3 get_prev_angular_velocity() const { return prev_angular_velocity; } - - _FORCE_INLINE_ const Vector3 &get_biased_linear_velocity() const { return biased_linear_velocity; } - _FORCE_INLINE_ const Vector3 &get_biased_angular_velocity() const { return biased_angular_velocity; } - - _FORCE_INLINE_ void apply_central_impulse(const Vector3 &p_j) { - linear_velocity += p_j * _inv_mass; - } - - _FORCE_INLINE_ void apply_impulse(const Vector3 &p_pos, const Vector3 &p_j) { - linear_velocity += p_j * _inv_mass; - angular_velocity += _inv_inertia_tensor.xform((p_pos - center_of_mass).cross(p_j)); - } - - _FORCE_INLINE_ void apply_torque_impulse(const Vector3 &p_j) { - angular_velocity += _inv_inertia_tensor.xform(p_j); - } - - _FORCE_INLINE_ void apply_bias_impulse(const Vector3 &p_pos, const Vector3 &p_j, real_t p_max_delta_av = -1.0) { - biased_linear_velocity += p_j * _inv_mass; - if (p_max_delta_av != 0.0) { - Vector3 delta_av = _inv_inertia_tensor.xform((p_pos - center_of_mass).cross(p_j)); - 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_j) { - biased_angular_velocity += _inv_inertia_tensor.xform(p_j); - } - - _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_pos) { - applied_force += p_force; - applied_torque += p_pos.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 == PhysicsServer::BODY_MODE_STATIC || mode == PhysicsServer::BODY_MODE_KINEMATIC) { - return; - } - set_active(true); - } - - void set_param(PhysicsServer::BodyParameter p_param, real_t); - real_t get_param(PhysicsServer::BodyParameter p_param) const; - - void set_mode(PhysicsServer::BodyMode p_mode); - PhysicsServer::BodyMode get_mode() const; - - void set_state(PhysicsServer::BodyState p_state, const Variant &p_variant); - Variant get_state(PhysicsServer::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(SpaceSW *p_space); - - void update_inertias(); - - _FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; } - _FORCE_INLINE_ Vector3 get_inv_inertia() const { return _inv_inertia; } - _FORCE_INLINE_ Basis get_inv_inertia_tensor() const { return _inv_inertia_tensor; } - _FORCE_INLINE_ real_t get_friction() const { return friction; } - _FORCE_INLINE_ Vector3 get_gravity() const { return gravity; } - _FORCE_INLINE_ real_t get_bounce() const { return bounce; } - - void set_axis_lock(PhysicsServer::BodyAxis p_axis, bool lock); - bool is_axis_locked(PhysicsServer::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 Transform& p_xform,real_t p_step); - void call_queries(); - void wakeup_neighbours(); - - bool sleep_test(real_t p_step); - - PhysicsDirectBodyStateSW *get_direct_state() const { return direct_access; } - - BodySW(); - ~BodySW(); -}; - -//add contact inline - -void BodySW::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; -} - -class PhysicsDirectBodyStateSW : public PhysicsDirectBodyState { - GDCLASS(PhysicsDirectBodyStateSW, PhysicsDirectBodyState); - -public: - BodySW *body = nullptr; - - virtual Vector3 get_total_gravity() const { return body->gravity; } // get gravity vector working on this body space/area - virtual real_t get_total_angular_damp() const { return body->area_angular_damp; } // get density of this body space/area - virtual real_t get_total_linear_damp() const { return body->area_linear_damp; } // get density of this body space/area - - virtual Vector3 get_center_of_mass() const { return body->get_center_of_mass(); } - virtual Basis get_principal_inertia_axes() const { return body->get_principal_inertia_axes(); } - - virtual real_t get_inverse_mass() const { return body->get_inv_mass(); } // get the mass - virtual Vector3 get_inverse_inertia() const { return body->get_inv_inertia(); } // get density of this body space - virtual Basis get_inverse_inertia_tensor() const { return body->get_inv_inertia_tensor(); } // get density of this body space - - virtual void set_linear_velocity(const Vector3 &p_velocity) { - body->wakeup(); - body->set_linear_velocity(p_velocity); - } - virtual Vector3 get_linear_velocity() const { return body->get_linear_velocity(); } - - virtual void set_angular_velocity(const Vector3 &p_velocity) { - body->wakeup(); - body->set_angular_velocity(p_velocity); - } - virtual Vector3 get_angular_velocity() const { return body->get_angular_velocity(); } - - virtual void set_transform(const Transform &p_transform) { body->set_state(PhysicsServer::BODY_STATE_TRANSFORM, p_transform); } - virtual Transform get_transform() const { return body->get_transform(); } - - virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const { return body->get_velocity_in_local_point(p_position); } - - virtual void add_central_force(const Vector3 &p_force) { - body->wakeup(); - body->add_central_force(p_force); - } - virtual void add_force(const Vector3 &p_force, const Vector3 &p_pos) { - body->wakeup(); - body->add_force(p_force, p_pos); - } - virtual void add_torque(const Vector3 &p_torque) { - body->wakeup(); - body->add_torque(p_torque); - } - virtual void apply_central_impulse(const Vector3 &p_j) { - body->wakeup(); - body->apply_central_impulse(p_j); - } - virtual void apply_impulse(const Vector3 &p_pos, const Vector3 &p_j) { - body->wakeup(); - body->apply_impulse(p_pos, p_j); - } - virtual void apply_torque_impulse(const Vector3 &p_j) { - body->wakeup(); - body->apply_torque_impulse(p_j); - } - - virtual void set_sleep_state(bool p_enable) { body->set_active(!p_enable); } - virtual bool is_sleeping() const { return !body->is_active(); } - - virtual int get_contact_count() const { return body->contact_count; } - - virtual Vector3 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; - } - virtual Vector3 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; - } - virtual float get_contact_impulse(int p_contact_idx) const { - return 0.0f; // Only implemented for bullet - } - virtual int 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; - } - - virtual RID 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; - } - virtual Vector3 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; - } - virtual ObjectID get_contact_collider_id(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0); - return body->contacts[p_contact_idx].collider_instance_id; - } - virtual int 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; - } - virtual Vector3 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; - } - - virtual PhysicsDirectSpaceState *get_space_state(); - - virtual real_t get_step() const; - - PhysicsDirectBodyStateSW() {} -}; - -#endif // BODY__SW_H diff --git a/servers/physics/broad_phase_basic.cpp b/servers/physics/broad_phase_basic.cpp deleted file mode 100644 index fa3d232..0000000 --- a/servers/physics/broad_phase_basic.cpp +++ /dev/null @@ -1,212 +0,0 @@ -/*************************************************************************/ -/* broad_phase_basic.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_basic.h" -#include "core/containers/list.h" -#include "core/string/print_string.h" - -BroadPhaseSW::ID BroadPhaseBasic::create(CollisionObjectSW *p_object, int p_subindex, const AABB &p_aabb, bool p_static) { - ERR_FAIL_COND_V(p_object == nullptr, 0); - - current++; - - Element e; - e.owner = p_object; - e._static = false; - e.subindex = p_subindex; - - element_map[current] = e; - return current; -} - -void BroadPhaseBasic::move(ID p_id, const AABB &p_aabb) { - RBMap::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - E->get().aabb = p_aabb; -} - -void BroadPhaseBasic::recheck_pairs(ID p_id) { - // Not supported. -} - -void BroadPhaseBasic::set_static(ID p_id, bool p_static) { - RBMap::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - E->get()._static = p_static; -} - -void BroadPhaseBasic::remove(ID p_id) { - RBMap::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - List to_erase; - //unpair must be done immediately on removal to avoid potential invalid pointers - for (RBMap::Element *F = pair_map.front(); F; F = F->next()) { - if (F->key().a == p_id || F->key().b == p_id) { - if (unpair_callback) { - Element *elem_A = &element_map[F->key().a]; - Element *elem_B = &element_map[F->key().b]; - unpair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, F->get(), unpair_userdata); - } - to_erase.push_back(F->key()); - } - } - while (to_erase.size()) { - pair_map.erase(to_erase.front()->get()); - to_erase.pop_front(); - } - element_map.erase(E); -} - -CollisionObjectSW *BroadPhaseBasic::get_object(ID p_id) const { - const RBMap::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, nullptr); - return E->get().owner; -} -bool BroadPhaseBasic::is_static(ID p_id) const { - const RBMap::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, false); - return E->get()._static; -} -int BroadPhaseBasic::get_subindex(ID p_id) const { - const RBMap::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, -1); - return E->get().subindex; -} - -int BroadPhaseBasic::cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { - int rc = 0; - - for (RBMap::Element *E = element_map.front(); E; E = E->next()) { - const AABB aabb = E->get().aabb; - if (aabb.has_point(p_point)) { - p_results[rc] = E->get().owner; - p_result_indices[rc] = E->get().subindex; - rc++; - if (rc >= p_max_results) { - break; - } - } - } - - return rc; -} - -int BroadPhaseBasic::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { - int rc = 0; - - for (RBMap::Element *E = element_map.front(); E; E = E->next()) { - const AABB aabb = E->get().aabb; - if (aabb.intersects_segment(p_from, p_to)) { - p_results[rc] = E->get().owner; - p_result_indices[rc] = E->get().subindex; - rc++; - if (rc >= p_max_results) { - break; - } - } - } - - return rc; -} -int BroadPhaseBasic::cull_aabb(const AABB &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { - int rc = 0; - - for (RBMap::Element *E = element_map.front(); E; E = E->next()) { - const AABB aabb = E->get().aabb; - if (aabb.intersects(p_aabb)) { - p_results[rc] = E->get().owner; - p_result_indices[rc] = E->get().subindex; - rc++; - if (rc >= p_max_results) { - break; - } - } - } - - return rc; -} - -void BroadPhaseBasic::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { - pair_userdata = p_userdata; - pair_callback = p_pair_callback; -} -void BroadPhaseBasic::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { - unpair_userdata = p_userdata; - unpair_callback = p_unpair_callback; -} - -void BroadPhaseBasic::update() { - // recompute pairs - for (RBMap::Element *I = element_map.front(); I; I = I->next()) { - for (RBMap::Element *J = I->next(); J; J = J->next()) { - Element *elem_A = &I->get(); - Element *elem_B = &J->get(); - - if (elem_A->owner == elem_B->owner) { - continue; - } - - bool pair_ok = elem_A->aabb.intersects(elem_B->aabb) && (!elem_A->_static || !elem_B->_static); - - PairKey key(I->key(), J->key()); - - RBMap::Element *E = pair_map.find(key); - - if (!pair_ok && E) { - if (unpair_callback) { - unpair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, E->get(), unpair_userdata); - } - pair_map.erase(key); - } - - if (pair_ok && !E) { - void *data = nullptr; - if (pair_callback) { - data = pair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, nullptr, unpair_userdata); - if (data) { - pair_map.insert(key, data); - } - } - } - } - } -} - -BroadPhaseSW *BroadPhaseBasic::_create() { - return memnew(BroadPhaseBasic); -} - -BroadPhaseBasic::BroadPhaseBasic() { - current = 1; - unpair_callback = nullptr; - unpair_userdata = nullptr; - pair_callback = nullptr; - pair_userdata = nullptr; -} diff --git a/servers/physics/broad_phase_basic.h b/servers/physics/broad_phase_basic.h deleted file mode 100644 index dfca4a5..0000000 --- a/servers/physics/broad_phase_basic.h +++ /dev/null @@ -1,105 +0,0 @@ -#ifndef BROAD_PHASE_BASIC_H -#define BROAD_PHASE_BASIC_H -/*************************************************************************/ -/* broad_phase_basic.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" -#include "core/containers/rb_map.h" - -class BroadPhaseBasic : public BroadPhaseSW { - struct Element { - CollisionObjectSW *owner; - bool _static; - AABB aabb; - int subindex; - }; - - RBMap element_map; - - ID current; - - struct PairKey { - union { - struct { - ID a; - ID b; - }; - uint64_t key; - }; - - _FORCE_INLINE_ bool operator<(const PairKey &p_key) const { - return key < p_key.key; - } - - PairKey() { key = 0; } - PairKey(ID p_a, ID p_b) { - if (p_a > p_b) { - a = p_b; - b = p_a; - } else { - a = p_a; - b = p_b; - } - } - }; - - RBMap pair_map; - - PairCallback pair_callback; - void *pair_userdata; - UnpairCallback unpair_callback; - void *unpair_userdata; - -public: - // 0 is an invalid ID - virtual ID create(CollisionObjectSW *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 recheck_pairs(ID p_id); - virtual void set_static(ID p_id, bool p_static); - virtual void remove(ID p_id); - - virtual CollisionObjectSW *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, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = nullptr); - virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = nullptr); - virtual int cull_aabb(const AABB &p_aabb, CollisionObjectSW **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 BroadPhaseSW *_create(); - BroadPhaseBasic(); -}; - -#endif // BROAD_PHASE_BASIC_H diff --git a/servers/physics/broad_phase_bvh.cpp b/servers/physics/broad_phase_bvh.cpp deleted file mode 100644 index e21489d..0000000 --- a/servers/physics/broad_phase_bvh.cpp +++ /dev/null @@ -1,141 +0,0 @@ -/*************************************************************************/ -/* broad_phase_bvh.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_bvh.h" -#include "collision_object_sw.h" -#include "core/config/project_settings.h" - -BroadPhaseSW::ID BroadPhaseBVH::create(CollisionObjectSW *p_object, int p_subindex, const AABB &p_aabb, bool p_static) { - uint32_t tree_id = p_static ? TREE_STATIC : TREE_DYNAMIC; - uint32_t tree_collision_mask = p_static ? TREE_FLAG_DYNAMIC : (TREE_FLAG_STATIC | TREE_FLAG_DYNAMIC); - ID oid = bvh.create(p_object, true, tree_id, tree_collision_mask, p_aabb, p_subindex); // Pair everything, don't care? - return oid + 1; -} - -void BroadPhaseBVH::move(ID p_id, const AABB &p_aabb) { - bvh.move(p_id - 1, p_aabb); -} - -void BroadPhaseBVH::recheck_pairs(ID p_id) { - bvh.recheck_pairs(p_id - 1); -} - -void BroadPhaseBVH::set_static(ID p_id, bool p_static) { - uint32_t tree_id = p_static ? TREE_STATIC : TREE_DYNAMIC; - uint32_t tree_collision_mask = p_static ? TREE_FLAG_DYNAMIC : (TREE_FLAG_STATIC | TREE_FLAG_DYNAMIC); - bvh.set_tree(p_id - 1, tree_id, tree_collision_mask, false); -} - -void BroadPhaseBVH::remove(ID p_id) { - bvh.erase(p_id - 1); -} - -CollisionObjectSW *BroadPhaseBVH::get_object(ID p_id) const { - CollisionObjectSW *it = bvh.get(p_id - 1); - ERR_FAIL_COND_V(!it, nullptr); - return it; -} - -bool BroadPhaseBVH::is_static(ID p_id) const { - uint32_t tree_id = bvh.get_tree_id(p_id - 1); - return tree_id == 0; -} - -int BroadPhaseBVH::get_subindex(ID p_id) const { - return bvh.get_subindex(p_id - 1); -} - -int BroadPhaseBVH::cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { - return bvh.cull_point(p_point, p_results, p_max_results, nullptr, 0xFFFFFFFF, p_result_indices); -} - -int BroadPhaseBVH::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { - return bvh.cull_segment(p_from, p_to, p_results, p_max_results, nullptr, 0xFFFFFFFF, p_result_indices); -} - -int BroadPhaseBVH::cull_aabb(const AABB &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { - return bvh.cull_aabb(p_aabb, p_results, p_max_results, nullptr, 0xFFFFFFFF, p_result_indices); -} - -void *BroadPhaseBVH::_pair_callback(void *p_self, uint32_t p_id_A, CollisionObjectSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObjectSW *p_object_B, int p_subindex_B) { - BroadPhaseBVH *bpo = (BroadPhaseBVH *)(p_self); - if (!bpo->pair_callback) { - return nullptr; - } - - return bpo->pair_callback(p_object_A, p_subindex_A, p_object_B, p_subindex_B, nullptr, bpo->pair_userdata); -} - -void BroadPhaseBVH::_unpair_callback(void *p_self, uint32_t p_id_A, CollisionObjectSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data) { - BroadPhaseBVH *bpo = (BroadPhaseBVH *)(p_self); - if (!bpo->unpair_callback) { - return; - } - - bpo->unpair_callback(p_object_A, p_subindex_A, p_object_B, p_subindex_B, p_pair_data, bpo->unpair_userdata); -} - -void *BroadPhaseBVH::_check_pair_callback(void *p_self, uint32_t p_id_A, CollisionObjectSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data) { - BroadPhaseBVH *bpo = (BroadPhaseBVH *)(p_self); - if (!bpo->pair_callback) { - return nullptr; - } - - return bpo->pair_callback(p_object_A, p_subindex_A, p_object_B, p_subindex_B, p_pair_data, bpo->pair_userdata); -} - -void BroadPhaseBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { - pair_callback = p_pair_callback; - pair_userdata = p_userdata; -} - -void BroadPhaseBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { - unpair_callback = p_unpair_callback; - unpair_userdata = p_userdata; -} - -void BroadPhaseBVH::update() { - bvh.update(); -} - -BroadPhaseSW *BroadPhaseBVH::_create() { - return memnew(BroadPhaseBVH); -} - -BroadPhaseBVH::BroadPhaseBVH() { - bvh.params_set_thread_safe(GLOBAL_GET("rendering/threads/thread_safe_bvh")); - bvh.params_set_pairing_expansion(GLOBAL_GET("physics/3d/pandemonium_physics/bvh_collision_margin")); - bvh.set_pair_callback(_pair_callback, this); - bvh.set_unpair_callback(_unpair_callback, this); - bvh.set_check_pair_callback(_check_pair_callback, this); - pair_callback = nullptr; - pair_userdata = nullptr; - unpair_userdata = nullptr; -} diff --git a/servers/physics/broad_phase_bvh.h b/servers/physics/broad_phase_bvh.h deleted file mode 100644 index 346ec51..0000000 --- a/servers/physics/broad_phase_bvh.h +++ /dev/null @@ -1,100 +0,0 @@ -#ifndef BROAD_PHASE_BVH_H -#define BROAD_PHASE_BVH_H -/*************************************************************************/ -/* broad_phase_bvh.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" -#include "core/math/bvh.h" - -class BroadPhaseBVH : public BroadPhaseSW { - template - class UserPairTestFunction { - public: - static bool user_pair_check(const T *p_a, const T *p_b) { - // return false if no collision, decided by masks etc - return p_a->test_collision_mask(p_b); - } - }; - - template - class UserCullTestFunction { - public: - static bool user_cull_check(const T *p_a, const T *p_b) { - return true; - } - }; - - enum Tree { - TREE_STATIC = 0, - TREE_DYNAMIC = 1, - }; - - enum TreeFlag { - TREE_FLAG_STATIC = 1 << TREE_STATIC, - TREE_FLAG_DYNAMIC = 1 << TREE_DYNAMIC, - }; - - BVH_Manager, UserCullTestFunction> bvh; - - static void *_pair_callback(void *p_self, uint32_t p_id_A, CollisionObjectSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObjectSW *p_object_B, int p_subindex_B); - static void _unpair_callback(void *p_self, uint32_t p_id_A, CollisionObjectSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data); - static void *_check_pair_callback(void *p_self, uint32_t p_id_A, CollisionObjectSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data); - - PairCallback pair_callback; - void *pair_userdata; - UnpairCallback unpair_callback; - void *unpair_userdata; - -public: - // 0 is an invalid ID - virtual ID create(CollisionObjectSW *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 recheck_pairs(ID p_id); - virtual void set_static(ID p_id, bool p_static); - virtual void remove(ID p_id); - - virtual CollisionObjectSW *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, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = nullptr); - virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = nullptr); - virtual int cull_aabb(const AABB &p_aabb, CollisionObjectSW **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 BroadPhaseSW *_create(); - BroadPhaseBVH(); -}; - -#endif // BROAD_PHASE_BVH_H diff --git a/servers/physics/broad_phase_octree.cpp b/servers/physics/broad_phase_octree.cpp deleted file mode 100644 index 14fc839..0000000 --- a/servers/physics/broad_phase_octree.cpp +++ /dev/null @@ -1,128 +0,0 @@ -/*************************************************************************/ -/* broad_phase_octree.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_octree.h" -#include "collision_object_sw.h" - -BroadPhaseSW::ID BroadPhaseOctree::create(CollisionObjectSW *p_object, int p_subindex, const AABB &p_aabb, bool p_static) { - ID oid = octree.create(p_object, AABB(), p_subindex, false, 1 << p_object->get_type(), 0); - return oid; -} - -void BroadPhaseOctree::move(ID p_id, const AABB &p_aabb) { - octree.move(p_id, p_aabb); -} - -void BroadPhaseOctree::recheck_pairs(ID p_id) { - AABB aabb = octree.get_aabb(p_id); - octree.move(p_id, aabb); -} - -void BroadPhaseOctree::set_static(ID p_id, bool p_static) { - CollisionObjectSW *it = octree.get(p_id); - octree.set_pairable(p_id, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF); //pair everything, don't care 1? -} - -void BroadPhaseOctree::remove(ID p_id) { - octree.erase(p_id); -} - -CollisionObjectSW *BroadPhaseOctree::get_object(ID p_id) const { - CollisionObjectSW *it = octree.get(p_id); - ERR_FAIL_COND_V(!it, nullptr); - return it; -} -bool BroadPhaseOctree::is_static(ID p_id) const { - return !octree.is_pairable(p_id); -} -int BroadPhaseOctree::get_subindex(ID p_id) const { - return octree.get_subindex(p_id); -} - -int BroadPhaseOctree::cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { - return octree.cull_point(p_point, p_results, p_max_results, p_result_indices); -} - -int BroadPhaseOctree::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { - return octree.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices); -} - -int BroadPhaseOctree::cull_aabb(const AABB &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { - return octree.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices); -} - -void *BroadPhaseOctree::_pair_callback(void *self, OctreeElementID p_A, CollisionObjectSW *p_object_A, int subindex_A, OctreeElementID p_B, CollisionObjectSW *p_object_B, int subindex_B) { - BroadPhaseOctree *bpo = (BroadPhaseOctree *)(self); - if (!bpo->pair_callback) { - return nullptr; - } - - bool valid_collision_pair = p_object_A->test_collision_mask(p_object_B); - - if (!valid_collision_pair) { - return nullptr; - } - - return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, nullptr, bpo->pair_userdata); -} - -void BroadPhaseOctree::_unpair_callback(void *self, OctreeElementID p_A, CollisionObjectSW *p_object_A, int subindex_A, OctreeElementID p_B, CollisionObjectSW *p_object_B, int subindex_B, void *pairdata) { - BroadPhaseOctree *bpo = (BroadPhaseOctree *)(self); - if (!bpo->unpair_callback) { - return; - } - - bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata); -} - -void BroadPhaseOctree::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { - pair_callback = p_pair_callback; - pair_userdata = p_userdata; -} -void BroadPhaseOctree::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { - unpair_callback = p_unpair_callback; - unpair_userdata = p_userdata; -} - -void BroadPhaseOctree::update() { - // does.. not? -} - -BroadPhaseSW *BroadPhaseOctree::_create() { - return memnew(BroadPhaseOctree); -} - -BroadPhaseOctree::BroadPhaseOctree() { - octree.set_pair_callback(_pair_callback, this); - octree.set_unpair_callback(_unpair_callback, this); - pair_callback = nullptr; - pair_userdata = nullptr; - unpair_userdata = nullptr; -} diff --git a/servers/physics/broad_phase_octree.h b/servers/physics/broad_phase_octree.h deleted file mode 100644 index 1612cb1..0000000 --- a/servers/physics/broad_phase_octree.h +++ /dev/null @@ -1,72 +0,0 @@ -#ifndef BROAD_PHASE_OCTREE_H -#define BROAD_PHASE_OCTREE_H -/*************************************************************************/ -/* broad_phase_octree.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" -#include "core/math/octree.h" - -class BroadPhaseOctree : public BroadPhaseSW { - Octree octree; - - static void *_pair_callback(void *, OctreeElementID, CollisionObjectSW *, int, OctreeElementID, CollisionObjectSW *, int); - static void _unpair_callback(void *, OctreeElementID, CollisionObjectSW *, int, OctreeElementID, CollisionObjectSW *, int, void *); - - PairCallback pair_callback; - void *pair_userdata; - UnpairCallback unpair_callback; - void *unpair_userdata; - -public: - // 0 is an invalid ID - virtual ID create(CollisionObjectSW *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 recheck_pairs(ID p_id); - virtual void set_static(ID p_id, bool p_static); - virtual void remove(ID p_id); - - virtual CollisionObjectSW *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, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = nullptr); - virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = nullptr); - virtual int cull_aabb(const AABB &p_aabb, CollisionObjectSW **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 BroadPhaseSW *_create(); - BroadPhaseOctree(); -}; - -#endif // BROAD_PHASE_OCTREE_H diff --git a/servers/physics/broad_phase_sw.cpp b/servers/physics/broad_phase_sw.cpp deleted file mode 100644 index b088323..0000000 --- a/servers/physics/broad_phase_sw.cpp +++ /dev/null @@ -1,36 +0,0 @@ -/*************************************************************************/ -/* broad_phase_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" - -BroadPhaseSW::CreateFunction BroadPhaseSW::create_func = nullptr; - -BroadPhaseSW::~BroadPhaseSW() { -} diff --git a/servers/physics/broad_phase_sw.h b/servers/physics/broad_phase_sw.h deleted file mode 100644 index 72c1e99..0000000 --- a/servers/physics/broad_phase_sw.h +++ /dev/null @@ -1,72 +0,0 @@ -#ifndef BROAD_PHASE_SW_H -#define BROAD_PHASE_SW_H -/*************************************************************************/ -/* broad_phase_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "core/math/aabb.h" -#include "core/math/math_funcs.h" - -class CollisionObjectSW; - -class BroadPhaseSW { -public: - typedef BroadPhaseSW *(*CreateFunction)(); - - static CreateFunction create_func; - - typedef uint32_t ID; - - typedef void *(*PairCallback)(CollisionObjectSW *p_object_A, int p_subindex_A, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_user_data); - typedef void (*UnpairCallback)(CollisionObjectSW *p_object_A, int p_subindex_A, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_user_data); - - // 0 is an invalid ID - virtual ID create(CollisionObjectSW *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 recheck_pairs(ID p_id) = 0; - virtual void set_static(ID p_id, bool p_static) = 0; - virtual void remove(ID p_id) = 0; - - virtual CollisionObjectSW *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, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; - virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; - virtual int cull_aabb(const AABB &p_aabb, CollisionObjectSW **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 ~BroadPhaseSW(); -}; - -#endif // BROAD_PHASE__SW_H diff --git a/servers/physics/collision_object_sw.cpp b/servers/physics/collision_object_sw.cpp deleted file mode 100644 index b870380..0000000 --- a/servers/physics/collision_object_sw.cpp +++ /dev/null @@ -1,262 +0,0 @@ -/*************************************************************************/ -/* collision_object_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" -#include "servers/physics/physics_server_sw.h" -#include "space_sw.h" - -void CollisionObjectSW::add_shape(ShapeSW *p_shape, const Transform &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()) { - PhysicsServerSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); - } -} - -void CollisionObjectSW::set_shape(int p_index, ShapeSW *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()) { - PhysicsServerSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); - } -} -void CollisionObjectSW::set_shape_transform(int p_index, const Transform &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()) { - PhysicsServerSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); - } -} - -void CollisionObjectSW::set_shape_disabled(int p_idx, bool p_disabled) { - ERR_FAIL_INDEX(p_idx, shapes.size()); - - CollisionObjectSW::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()) { - PhysicsServerSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); - } - } else if (!p_disabled && shape.bpid == 0) { - if (!pending_shape_update_list.in_list()) { - PhysicsServerSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); - } - } -} - -void CollisionObjectSW::remove_shape(ShapeSW *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 CollisionObjectSW::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()) { - PhysicsServerSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); - } -} - -void CollisionObjectSW::_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 CollisionObjectSW::_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 CollisionObjectSW::_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(); - Transform 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 CollisionObjectSW::_recheck_shapes() { - if (!space) { - return; - } - - for (int i = 0; i < shapes.size(); i++) { - Shape &s = shapes.write[i]; - if (s.disabled) { - continue; - } - - if (s.bpid != 0) { - space->get_broadphase()->recheck_pairs(s.bpid); - } - } -} - -void CollisionObjectSW::_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(); - Transform 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 CollisionObjectSW::_set_space(SpaceSW *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 CollisionObjectSW::_shape_changed() { - _update_shapes(); - _shapes_changed(); -} - -CollisionObjectSW::CollisionObjectSW(Type p_type) : - pending_shape_update_list(this) { - _static = true; - type = p_type; - space = nullptr; - instance_id = 0; - collision_layer = 1; - collision_mask = 1; - ray_pickable = true; -} diff --git a/servers/physics/collision_object_sw.h b/servers/physics/collision_object_sw.h deleted file mode 100644 index 90244af..0000000 --- a/servers/physics/collision_object_sw.h +++ /dev/null @@ -1,184 +0,0 @@ -#ifndef COLLISION_OBJECT_SW_H -#define COLLISION_OBJECT_SW_H -/*************************************************************************/ -/* collision_object_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" -#include "core/containers/self_list.h" -#include "servers/physics_server.h" -#include "shape_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 SpaceSW; - -class CollisionObjectSW : public ShapeOwnerSW { -public: - enum Type { - TYPE_AREA, - TYPE_BODY - }; - -private: - Type type; - RID self; - ObjectID instance_id; - uint32_t collision_layer; - uint32_t collision_mask; - - struct Shape { - Transform xform; - Transform xform_inv; - BroadPhaseSW::ID bpid; - AABB aabb_cache; //for rayqueries - real_t area_cache; - ShapeSW *shape; - bool disabled; - - Shape() { disabled = false; } - }; - - Vector shapes; - SpaceSW *space; - Transform transform; - Transform inv_transform; - bool _static; - - SelfList pending_shape_update_list; - - void _update_shapes(); - void _recheck_shapes(); - -protected: - void _update_shapes_with_motion(const Vector3 &p_motion); - void _unregister_shapes(); - - _FORCE_INLINE_ void _set_transform(const Transform &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 Transform &p_transform) { inv_transform = p_transform; } - void _set_static(bool p_static); - - virtual void _shapes_changed() = 0; - void _set_space(SpaceSW *p_space); - - bool ray_pickable; - - CollisionObjectSW(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(ShapeSW *p_shape, const Transform &p_transform = Transform(), bool p_disabled = false); - void set_shape(int p_index, ShapeSW *p_shape); - void set_shape_transform(int p_index, const Transform &p_transform); - _FORCE_INLINE_ int get_shape_count() const { return shapes.size(); } - _FORCE_INLINE_ ShapeSW *get_shape(int p_index) const { - CRASH_BAD_INDEX(p_index, shapes.size()); - return shapes[p_index].shape; - } - _FORCE_INLINE_ const Transform &get_shape_transform(int p_index) const { - CRASH_BAD_INDEX(p_index, shapes.size()); - return shapes[p_index].xform; - } - _FORCE_INLINE_ const Transform &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_ Transform get_transform() const { return transform; } - _FORCE_INLINE_ Transform get_inv_transform() const { return inv_transform; } - _FORCE_INLINE_ SpaceSW *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; - _recheck_shapes(); - _shapes_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; - _recheck_shapes(); - _shapes_changed(); - } - _FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; } - - _FORCE_INLINE_ bool test_collision_mask(const CollisionObjectSW *p_other) const { - return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask; - } - - void remove_shape(ShapeSW *p_shape); - void remove_shape(int p_index); - - virtual void set_space(SpaceSW *p_space) = 0; - - _FORCE_INLINE_ bool is_static() const { return _static; } - - virtual ~CollisionObjectSW() {} -}; - -#endif // COLLISION_OBJECT_SW_H diff --git a/servers/physics/collision_solver_sat.cpp b/servers/physics/collision_solver_sat.cpp deleted file mode 100644 index d39cc14..0000000 --- a/servers/physics/collision_solver_sat.cpp +++ /dev/null @@ -1,2313 +0,0 @@ -/*************************************************************************/ -/* collision_solver_sat.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sat.h" -#include "core/math/geometry.h" - -#include "gjk_epa.h" - -#define fallback_collision_solver gjk_epa_calculate_penetration - -// Cylinder SAT analytic methods and circle-face 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 { - CollisionSolverSW::CallbackResult callback; - void *userdata; - bool swap; - bool collided; - Vector3 normal; - Vector3 *prev_axis; - - _FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B) { - if (swap) { - callback(p_point_B, p_point_A, userdata); - } else { - callback(p_point_A, p_point_B, 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 = Geometry::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 = Geometry::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_pos, circle_B_normal); - - 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(edge0_B, clip_normal); - // 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_pos, circle_B_normal); - - 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_pos, circle_B_normal); - - // 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, ShapeSW::FeatureType p_feature_type_A, const Vector3 *p_points_B, int p_point_count_B, ShapeSW::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; - const ShapeB *shape_B; - const Transform *transform_A; - const Transform *transform_B; - real_t best_depth; - Vector3 best_axis; - _CollectorCallback *callback; - real_t margin_A; - real_t margin_B; - 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) { - Vector3 axis = p_axis; - - if (Math::abs(axis.x) < CMP_EPSILON && - Math::abs(axis.y) < CMP_EPSILON && - Math::abs(axis.z) < CMP_EPSILON) { - // 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 - 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, const Vector3 &p_point_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; - ShapeSW::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; - ShapeSW::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 Transform &p_transform_A, const ShapeB *p_shape_B, const Transform &p_transform_B, _CollectorCallback *p_callback, real_t p_margin_A = 0, real_t p_margin_B = 0) { - best_depth = 1e15; - 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 ShapeSW *, const Transform &, const ShapeSW *, const Transform &, _CollectorCallback *p_callback, real_t, real_t); - -template -static void _collision_sphere_sphere(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShapeSW *sphere_A = static_cast(p_a); - const SphereShapeSW *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 ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShapeSW *sphere_A = static_cast(p_a); - const BoxShapeSW *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 ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShapeSW *sphere_A = static_cast(p_a); - const CapsuleShapeSW *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(2) * (capsule_B->get_height() * 0.5); - - 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 ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShapeSW *sphere_A = static_cast(p_a); - const CylinderShapeSW *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 ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShapeSW *sphere_A = static_cast(p_a); - const ConvexPolygonShapeSW *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 Geometry::MeshData &mesh = convex_polygon_B->get_mesh(); - - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); - int face_count = mesh.faces.size(); - const Geometry::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 nx_b = p_transform_b.basis.get_normal_xform_basis(); - - // faces of B - for (int i = 0; i < face_count; i++) { - Vector3 axis = nx_b.xform_normal_fast(faces[i].plane.normal); - - 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 ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShapeSW *sphere_A = static_cast(p_a); - const FaceShapeSW *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]), - }; - - if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) { - return; - } - - // edges and points of B - for (int i = 0; i < 3; i++) { - Vector3 n1 = vertex[i] - p_transform_a.origin; - - if (!separator.test_axis(n1.normalized())) { - return; - } - - Vector3 n2 = vertex[(i + 1) % 3] - vertex[i]; - - Vector3 axis = n1.cross(n2).cross(n2).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - - separator.generate_contacts(); -} - -template -static void _collision_box_box(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const BoxShapeSW *box_A = static_cast(p_a); - const BoxShapeSW *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 ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const BoxShapeSW *box_A = static_cast(p_a); - const CapsuleShapeSW *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(2).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, 0).project(point).normalized(); - - if (!separator.test_axis(axis)) { - return; - } - } - } - } - - // capsule balls, edges of A - Transform transform_a_inverse = p_transform_a.affine_inverse(); - - for (int i = 0; i < 2; i++) { - Vector3 capsule_axis = p_transform_b.basis.get_axis(2) * (capsule_B->get_height() * 0.5); - - Vector3 sphere_pos = p_transform_b.origin + ((i == 0) ? capsule_axis : -capsule_axis); - - Vector3 cnormal = transform_a_inverse.xform(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 ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const BoxShapeSW *box_A = static_cast(p_a); - const CylinderShapeSW *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, 0).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 ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const BoxShapeSW *box_A = static_cast(p_a); - const ConvexPolygonShapeSW *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 Geometry::MeshData &mesh = convex_polygon_B->get_mesh(); - - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); - int face_count = mesh.faces.size(); - const Geometry::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 nx_b = p_transform_b.basis.get_normal_xform_basis(); - - // faces of B - for (int i = 0; i < face_count; i++) { - Vector3 axis = nx_b.xform_normal_fast(faces[i].plane.normal); - - 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 ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const BoxShapeSW *box_A = static_cast(p_a); - const FaceShapeSW *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]), - }; - - if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) { - 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; - } - } - - // 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 = p_transform_a.basis.get_axis(j); - - if (!separator.test_axis(e.cross(axis).normalized())) { - 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 (!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, 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); - - if (!separator.test_axis((point - p2).cross(n).cross(n).normalized())) { - return; - } - } - } - } - } - } - - separator.generate_contacts(); -} - -template -static void _collision_capsule_capsule(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const CapsuleShapeSW *capsule_A = static_cast(p_a); - const CapsuleShapeSW *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(2) * (capsule_A->get_height() * 0.5); - Vector3 capsule_B_axis = p_transform_b.basis.get_axis(2) * (capsule_B->get_height() * 0.5); - - 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 ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const CapsuleShapeSW *capsule_A = static_cast(p_a); - const CylinderShapeSW *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(2); - - Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis * (capsule_A->get_height() * 0.5); - Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis * (capsule_A->get_height() * 0.5); - - 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; - } - - CollisionSolverSW::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 ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const CapsuleShapeSW *capsule_A = static_cast(p_a); - const ConvexPolygonShapeSW *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 Geometry::MeshData &mesh = convex_polygon_B->get_mesh(); - - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); - int face_count = mesh.faces.size(); - const Geometry::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 nx_b = p_transform_b.basis.get_normal_xform_basis(); - - // faces of B - for (int i = 0; i < face_count; i++) { - Vector3 axis = nx_b.xform_normal_fast(faces[i].plane.normal); - - 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(2)).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(2) * (capsule_A->get_height() * 0.5); - - 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 ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const CapsuleShapeSW *capsule_A = static_cast(p_a); - const FaceShapeSW *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]), - }; - - if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) { - return; - } - - // edges of B, capsule cylinder - - Vector3 capsule_axis = p_transform_a.basis.get_axis(2) * (capsule_A->get_height() * 0.5); - - 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 (!separator.test_axis(axis)) { - return; - } - - if (!separator.test_axis((p_transform_a.origin - vertex[i]).cross(capsule_axis).cross(capsule_axis).normalized())) { - 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 (!separator.test_axis(n1.normalized())) { - return; - } - - Vector3 n2 = edge_axis; - - axis = n1.cross(n2).cross(n2); - - if (!separator.test_axis(axis.normalized())) { - return; - } - } - } - - separator.generate_contacts(); -} - -template -static void _collision_cylinder_cylinder(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const CylinderShapeSW *cylinder_A = static_cast(p_a); - const CylinderShapeSW *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; - } - - CollisionSolverSW::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 ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const CylinderShapeSW *cylinder_A = static_cast(p_a); - const ConvexPolygonShapeSW *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); - - CollisionSolverSW::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 ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const CylinderShapeSW *cylinder_A = static_cast(p_a); - const FaceShapeSW *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]), - }; - - // Face B normal. - if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) { - return; - } - - Vector3 cyl_axis = p_transform_a.basis.get_axis(1).normalized(); - - // 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 (!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, 0).project(point).normalized(); - - 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 (!separator.test_axis(axis.normalized())) { - return; - } - } - } - - separator.generate_contacts(); -} - -template -static void _collision_convex_polygon_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const ConvexPolygonShapeSW *convex_polygon_A = static_cast(p_a); - const ConvexPolygonShapeSW *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 Geometry::MeshData &mesh_A = convex_polygon_A->get_mesh(); - - const Geometry::MeshData::Face *faces_A = mesh_A.faces.ptr(); - int face_count_A = mesh_A.faces.size(); - const Geometry::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 Geometry::MeshData &mesh_B = convex_polygon_B->get_mesh(); - - const Geometry::MeshData::Face *faces_B = mesh_B.faces.ptr(); - int face_count_B = mesh_B.faces.size(); - const Geometry::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 nx_a = p_transform_a.basis.get_normal_xform_basis(); - - // faces of A - for (int i = 0; i < face_count_A; i++) { - Vector3 axis = nx_a.xform_normal_fast(faces_A[i].plane.normal); - - if (!separator.test_axis(axis)) { - return; - } - } - - // Precalculating this makes the transforms faster. - Basis nx_b = p_transform_b.basis.get_normal_xform_basis(); - - // faces of B - for (int i = 0; i < face_count_B; i++) { - Vector3 axis = nx_b.xform_normal_fast(faces_B[i].plane.normal); - - 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 ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const ConvexPolygonShapeSW *convex_polygon_A = static_cast(p_a); - const FaceShapeSW *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 Geometry::MeshData &mesh = convex_polygon_A->get_mesh(); - - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); - int face_count = mesh.faces.size(); - const Geometry::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]), - }; - - if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) { - 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 (!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 (!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++) { - if (!separator.test_axis((va - vertex[j]).normalized())) { - 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]; - - if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) { - 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]); - - if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) { - return; - } - } - } - } - - separator.generate_contacts(); -} - -bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector3 *r_prev_axis, real_t p_margin_a, real_t p_margin_b) { - PhysicsServer::ShapeType type_A = p_shape_A->get_type(); - - ERR_FAIL_COND_V(type_A == PhysicsServer::SHAPE_PLANE, false); - ERR_FAIL_COND_V(type_A == PhysicsServer::SHAPE_RAY, false); - ERR_FAIL_COND_V(p_shape_A->is_concave(), false); - - PhysicsServer::ShapeType type_B = p_shape_B->get_type(); - - ERR_FAIL_COND_V(type_B == PhysicsServer::SHAPE_PLANE, false); - ERR_FAIL_COND_V(type_B == PhysicsServer::SHAPE_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 ShapeSW *A = p_shape_A; - const ShapeSW *B = p_shape_B; - const Transform *transform_A = &p_transform_A; - const Transform *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/collision_solver_sat.h b/servers/physics/collision_solver_sat.h deleted file mode 100644 index 7614b58..0000000 --- a/servers/physics/collision_solver_sat.h +++ /dev/null @@ -1,37 +0,0 @@ -#ifndef COLLISION_SOLVER_SAT_H -#define COLLISION_SOLVER_SAT_H -/*************************************************************************/ -/* collision_solver_sat.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" - -bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::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/collision_solver_sw.cpp b/servers/physics/collision_solver_sw.cpp deleted file mode 100644 index 8641f1a..0000000 --- a/servers/physics/collision_solver_sw.cpp +++ /dev/null @@ -1,405 +0,0 @@ -/*************************************************************************/ -/* collision_solver_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" -#include "collision_solver_sat.h" - -#include "gjk_epa.h" - -#define collision_solver sat_calculate_penetration -//#define collision_solver gjk_epa_calculate_penetration - -bool CollisionSolverSW::solve_static_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { - const PlaneShapeSW *plane = static_cast(p_shape_A); - if (p_shape_B->get_type() == PhysicsServer::SHAPE_PLANE) { - return false; - } - Plane p = p_transform_A.xform(plane->get_plane()); - - static const int max_supports = 16; - Vector3 supports[max_supports]; - int support_count; - ShapeSW::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 == ShapeSW::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], support_A, p_userdata); - } else { - p_result_callback(support_A, supports[i], p_userdata); - } - } - } - - return found; -} - -bool CollisionSolverSW::solve_ray(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) { - const RayShapeSW *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; - - Transform 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; - } - - Vector3 support_B = p_transform_B.xform(p); - if (ray->get_slips_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, support_A, p_userdata); - } else { - p_result_callback(support_A, support_B, p_userdata); - } - } - return true; -} - -struct _ConcaveCollisionInfo { - const Transform *transform_A; - const ShapeSW *shape_A; - const Transform *transform_B; - CollisionSolverSW::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 CollisionSolverSW::concave_callback(void *p_userdata, ShapeSW *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 CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &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 ConcaveShapeSW *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; - - Transform 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 CollisionSolverSW::solve_static(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { - PhysicsServer::ShapeType type_A = p_shape_A->get_type(); - PhysicsServer::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 == PhysicsServer::SHAPE_PLANE) { - if (type_B == PhysicsServer::SHAPE_PLANE) { - return false; - } - if (type_B == PhysicsServer::SHAPE_RAY) { - return false; - } - - if (swap) { - return solve_static_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); - } else { - return solve_static_plane(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); - } - - } else if (type_A == PhysicsServer::SHAPE_RAY) { - if (type_B == PhysicsServer::SHAPE_RAY) { - return false; - } - - if (swap) { - return solve_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_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, p_margin_A); - } - - } 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 CollisionSolverSW::concave_distance_callback(void *p_userdata, ShapeSW *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 CollisionSolverSW::solve_distance_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) { - const PlaneShapeSW *plane = static_cast(p_shape_A); - if (p_shape_B->get_type() == PhysicsServer::SHAPE_PLANE) { - return false; - } - Plane p = p_transform_A.xform(plane->get_plane()); - - static const int max_supports = 16; - Vector3 supports[max_supports]; - int support_count; - ShapeSW::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 == ShapeSW::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 CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &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() == PhysicsServer::SHAPE_PLANE) { - Vector3 a, b; - bool col = solve_distance_plane(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 ConcaveShapeSW *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; - - Transform 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, 0), 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/collision_solver_sw.h b/servers/physics/collision_solver_sw.h deleted file mode 100644 index 9041889..0000000 --- a/servers/physics/collision_solver_sw.h +++ /dev/null @@ -1,52 +0,0 @@ -#ifndef COLLISION_SOLVER_SW_H -#define COLLISION_SOLVER_SW_H -/*************************************************************************/ -/* collision_solver_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" - -class CollisionSolverSW { -public: - typedef void (*CallbackResult)(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata); - -private: - static bool concave_callback(void *p_userdata, ShapeSW *p_convex); - static bool solve_static_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); - static bool solve_ray(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0); - static bool solve_concave(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &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, ShapeSW *p_convex); - static bool solve_distance_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B); - -public: - static bool solve_static(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &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 ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &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/constraint_sw.h b/servers/physics/constraint_sw.h deleted file mode 100644 index cfaaf6d..0000000 --- a/servers/physics/constraint_sw.h +++ /dev/null @@ -1,83 +0,0 @@ -#ifndef CONSTRAINT_SW_H -#define CONSTRAINT_SW_H -/*************************************************************************/ -/* constraint_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" - -class ConstraintSW : public RID_Data { - BodySW **_body_ptr; - int _body_count; - uint64_t island_step; - ConstraintSW *island_next; - ConstraintSW *island_list_next; - int priority; - bool disabled_collisions_between_bodies; - - RID self; - -protected: - ConstraintSW(BodySW **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_ ConstraintSW *get_island_next() const { return island_next; } - _FORCE_INLINE_ void set_island_next(ConstraintSW *p_next) { island_next = p_next; } - - _FORCE_INLINE_ ConstraintSW *get_island_list_next() const { return island_list_next; } - _FORCE_INLINE_ void set_island_list_next(ConstraintSW *p_next) { island_list_next = p_next; } - - _FORCE_INLINE_ BodySW **get_body_ptr() const { return _body_ptr; } - _FORCE_INLINE_ int get_body_count() const { return _body_count; } - - _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 void solve(real_t p_step) = 0; - - virtual ~ConstraintSW() {} -}; - -#endif // CONSTRAINT__SW_H diff --git a/servers/physics/gjk_epa.cpp b/servers/physics/gjk_epa.cpp deleted file mode 100644 index b6f6290..0000000 --- a/servers/physics/gjk_epa.cpp +++ /dev/null @@ -1,1035 +0,0 @@ -/*************************************************************************/ -/* gjk_epa.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* PANDEMONIUM ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "gjk_epa.h" - -/* Disabling formatting for thirdparty code snippet */ -/* clang-format off */ - -/*************** Bullet's GJK-EPA2 IMPLEMENTATION *******************/ - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the -use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it -freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not -claim that you wrote the original software. If you use this software in a -product, an acknowledgment in the product documentation would be appreciated -but is not required. -2. Altered source versions must be plainly marked as such, and must not be -misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -GJK-EPA collision solver by Nathanael Presson, 2008 -*/ - - // Config - -/* GJK */ -#define GJK_MAX_ITERATIONS 128 -#define GJK_ACCURACY ((real_t)0.0001) -#define GJK_MIN_DISTANCE ((real_t)0.0001) -#define GJK_DUPLICATED_EPS ((real_t)0.0001) -#define GJK_SIMPLEX2_EPS ((real_t)0.0) -#define GJK_SIMPLEX3_EPS ((real_t)0.0) -#define GJK_SIMPLEX4_EPS ((real_t)0.0) - -/* EPA */ -#define EPA_MAX_VERTICES 128 -#define EPA_MAX_FACES (EPA_MAX_VERTICES*2) -#define EPA_MAX_ITERATIONS 255 -// -- PANDEMONIUM start -- -//#define EPA_ACCURACY ((real_t)0.0001) -#define EPA_ACCURACY ((real_t)0.00001) -// -- PANDEMONIUM end -- -#define EPA_FALLBACK (10*EPA_ACCURACY) -#define EPA_PLANE_EPS ((real_t)0.00001) -#define EPA_INSIDE_EPS ((real_t)0.01) - -namespace GjkEpa2 { - - -struct sResults { - enum eStatus { - Separated, /* Shapes doesn't penetrate */ - Penetrating, /* Shapes are penetrating */ - GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ - EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */ - } status; - - Vector3 witnesses[2]; - Vector3 normal; - real_t distance; -}; - -// Shorthands -typedef unsigned int U; -typedef unsigned char U1; - -// MinkowskiDiff -struct MinkowskiDiff { - - const ShapeSW* m_shapes[2]; - - Transform transform_A; - Transform transform_B; - - real_t margin_A = 0.0; - real_t margin_B = 0.0; - - Vector3 (*get_support)(const ShapeSW*, const Vector3&, real_t); - - void Initialize(const ShapeSW* shape0, const Transform& wtrs0, const real_t margin0, - const ShapeSW* shape1, const Transform& wtrs1, const real_t margin1) { - m_shapes[0] = shape0; - m_shapes[1] = shape1; - transform_A = wtrs0; - transform_B = wtrs1; - margin_A = margin0; - margin_B = margin1; - - if ((margin0 > 0.0) || (margin1 > 0.0)) { - get_support = get_support_with_margin; - } else { - get_support = get_support_without_margin; - } - } - - static Vector3 get_support_without_margin(const ShapeSW* 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 ShapeSW* p_shape, const Vector3& p_dir, real_t p_margin) { - Vector3 local_dir_norm = p_dir; - if (local_dir_norm.length_squared() < CMP_EPSILON2) { - local_dir_norm = Vector3(-1.0, -1.0, -1.0); - } - local_dir_norm.normalize(); - - return p_shape->get_support(local_dir_norm) + p_margin * local_dir_norm; - } - - // i wonder how this could be sped up... if it can - _FORCE_INLINE_ Vector3 Support0(const Vector3& d) const { - return transform_A.xform(get_support(m_shapes[0], transform_A.basis.xform_inv(d), margin_A)); - } - - _FORCE_INLINE_ Vector3 Support1(const Vector3& d) const { - return transform_B.xform(get_support(m_shapes[1], transform_B.basis.xform_inv(d), margin_B)); - } - - _FORCE_INLINE_ Vector3 Support (const Vector3& d) const { - return (Support0(d) - Support1(-d)); - } - - _FORCE_INLINE_ Vector3 Support(const Vector3& d, U index) const { - if (index) { - return Support1(d); - } else { - return Support0(d); - } - } -}; - -typedef MinkowskiDiff tShape; - - -// GJK -struct GJK -{ - /* Types */ - struct sSV - { - Vector3 d,w; - }; - struct sSimplex - { - sSV* c[4]; - real_t p[4]; - U rank; - }; - struct eStatus { enum _ { - Valid, - Inside, - Failed };}; - /* Fields */ - tShape m_shape; - Vector3 m_ray; - real_t m_distance; - sSimplex m_simplices[2]; - sSV m_store[4]; - sSV* m_free[4]; - U m_nfree; - U m_current; - sSimplex* m_simplex; - eStatus::_ m_status; - /* Methods */ - GJK() - { - Initialize(); - } - void Initialize() - { - m_ray = Vector3(0,0,0); - m_nfree = 0; - m_status = eStatus::Failed; - m_current = 0; - m_distance = 0; - } - eStatus::_ Evaluate(const tShape& shapearg,const Vector3& guess) - { - U iterations=0; - real_t sqdist=0; - real_t alpha=0; - Vector3 lastw[4]; - U clastw=0; - /* Initialize solver */ - m_free[0] = &m_store[0]; - m_free[1] = &m_store[1]; - m_free[2] = &m_store[2]; - m_free[3] = &m_store[3]; - m_nfree = 4; - m_current = 0; - m_status = eStatus::Valid; - m_shape = shapearg; - m_distance = 0; - /* Initialize simplex */ - m_simplices[0].rank = 0; - m_ray = guess; - const real_t sqrl= m_ray.length_squared(); - appendvertice(m_simplices[0],sqrl>0?-m_ray:Vector3(1,0,0)); - m_simplices[0].p[0] = 1; - m_ray = m_simplices[0].c[0]->w; - sqdist = sqrl; - lastw[0] = - lastw[1] = - lastw[2] = - lastw[3] = m_ray; - /* Loop */ - do { - const U next=1-m_current; - sSimplex& cs=m_simplices[m_current]; - sSimplex& ns=m_simplices[next]; - /* Check zero */ - const real_t rl=m_ray.length(); - if(rlw; - bool found=false; - for(U i=0;i<4;++i) - { - if((w-lastw[i]).length_squared()w, - cs.c[1]->w, - weights,mask);break; - case 3: sqdist=projectorigin( cs.c[0]->w, - cs.c[1]->w, - cs.c[2]->w, - weights,mask);break; - case 4: sqdist=projectorigin( cs.c[0]->w, - cs.c[1]->w, - cs.c[2]->w, - cs.c[3]->w, - weights,mask);break; - } - if(sqdist>=0) - {/* Valid */ - ns.rank = 0; - m_ray = Vector3(0,0,0); - m_current = next; - for(U i=0,ni=cs.rank;iw*weights[i]; - } - else - { - m_free[m_nfree++] = cs.c[i]; - } - } - if(mask==15) { m_status=eStatus::Inside; -} - } - else - {/* Return old simplex */ - removevertice(m_simplices[m_current]); - break; - } - m_status=((++iterations)rank) - { - case 1: - { - for(U i=0;i<3;++i) - { - Vector3 axis=Vector3(0,0,0); - axis[i]=1; - appendvertice(*m_simplex, axis); - if(EncloseOrigin()) { return(true); -} - removevertice(*m_simplex); - appendvertice(*m_simplex,-axis); - if(EncloseOrigin()) { return(true); -} - removevertice(*m_simplex); - } - } - break; - case 2: - { - const Vector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; - for(U i=0;i<3;++i) - { - Vector3 axis=Vector3(0,0,0); - axis[i]=1; - const Vector3 p=vec3_cross(d,axis); - if(p.length_squared()>0) - { - appendvertice(*m_simplex, p); - if(EncloseOrigin()) { return(true); -} - removevertice(*m_simplex); - appendvertice(*m_simplex,-p); - if(EncloseOrigin()) { return(true); -} - removevertice(*m_simplex); - } - } - } - break; - case 3: - { - const Vector3 n=vec3_cross(m_simplex->c[1]->w-m_simplex->c[0]->w, - m_simplex->c[2]->w-m_simplex->c[0]->w); - if(n.length_squared()>0) - { - appendvertice(*m_simplex,n); - if(EncloseOrigin()) { return(true); -} - removevertice(*m_simplex); - appendvertice(*m_simplex,-n); - if(EncloseOrigin()) { return(true); -} - removevertice(*m_simplex); - } - } - break; - case 4: - { - if(Math::abs(det( m_simplex->c[0]->w-m_simplex->c[3]->w, - m_simplex->c[1]->w-m_simplex->c[3]->w, - m_simplex->c[2]->w-m_simplex->c[3]->w))>0) { - return(true); -} - } - break; - } - return(false); - } - /* Internals */ - void getsupport(const Vector3& d,sSV& sv) const - { - sv.d = d/d.length(); - sv.w = m_shape.Support(sv.d); - } - void removevertice(sSimplex& simplex) - { - m_free[m_nfree++]=simplex.c[--simplex.rank]; - } - void appendvertice(sSimplex& simplex,const Vector3& v) - { - simplex.p[simplex.rank]=0; - simplex.c[simplex.rank]=m_free[--m_nfree]; - getsupport(v,*simplex.c[simplex.rank++]); - } - static real_t det(const Vector3& a,const Vector3& b,const Vector3& c) - { - return( a.y*b.z*c.x+a.z*b.x*c.y- - a.x*b.z*c.y-a.y*b.x*c.z+ - a.x*b.y*c.z-a.z*b.y*c.x); - } - static real_t projectorigin( const Vector3& a, - const Vector3& b, - real_t* w,U& m) - { - const Vector3 d=b-a; - const real_t l=d.length_squared(); - if(l>GJK_SIMPLEX2_EPS) - { - const real_t t(l>0?-vec3_dot(a,d)/l:0); - if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length_squared()); } - else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length_squared()); } - else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length_squared()); } - } - return(-1); - } - static real_t projectorigin( const Vector3& a, - const Vector3& b, - const Vector3& c, - real_t* w,U& m) - { - static const U imd3[]={1,2,0}; - const Vector3* vt[]={&a,&b,&c}; - const Vector3 dl[]={a-b,b-c,c-a}; - const Vector3 n=vec3_cross(dl[0],dl[1]); - const real_t l=n.length_squared(); - if(l>GJK_SIMPLEX3_EPS) - { - real_t mindist=-1; - real_t subw[2] = { 0 , 0}; - U subm = 0; - for(U i=0;i<3;++i) - { - if(vec3_dot(*vt[i],vec3_cross(dl[i],n))>0) - { - const U j=imd3[i]; - const real_t subd(projectorigin(*vt[i],*vt[j],subw,subm)); - if((mindist<0)||(subd(((subm&1)?1<GJK_SIMPLEX4_EPS)) - { - real_t mindist=-1; - real_t subw[3] = {0.f, 0.f, 0.f}; - U subm=0; - for(U i=0;i<3;++i) - { - const U j=imd3[i]; - const real_t s=vl*vec3_dot(d,vec3_cross(dl[i],dl[j])); - if(s>0) - { - const real_t subd=projectorigin(*vt[i],*vt[j],d,subw,subm); - if((mindist<0)||(subd((subm&1?1<e[ea]=(U1)eb;fa->f[ea]=fb; - fb->e[eb]=(U1)ea;fb->f[eb]=fa; - } - static inline void append(sList& list,sFace* face) - { - face->l[0] = nullptr; - face->l[1] = list.root; - if(list.root) { list.root->l[0]=face; -} - list.root = face; - ++list.count; - } - static inline void remove(sList& list,sFace* face) - { - if(face->l[1]) { face->l[1]->l[0]=face->l[0]; -} - if(face->l[0]) { face->l[0]->l[1]=face->l[1]; -} - if(face==list.root) { list.root=face->l[1]; -} - --list.count; - } - - - void Initialize() - { - m_status = eStatus::Failed; - m_normal = Vector3(0,0,0); - m_depth = 0; - m_nextsv = 0; - for(U i=0;i1)&&gjk.EncloseOrigin()) - { - - /* Clean up */ - while(m_hull.root) - { - sFace* f = m_hull.root; - remove(m_hull,f); - append(m_stock,f); - } - m_status = eStatus::Valid; - m_nextsv = 0; - /* Orient simplex */ - if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, - simplex.c[1]->w-simplex.c[3]->w, - simplex.c[2]->w-simplex.c[3]->w)<0) - { - SWAP(simplex.c[0],simplex.c[1]); - SWAP(simplex.p[0],simplex.p[1]); - } - /* Build initial hull */ - sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), - newface(simplex.c[1],simplex.c[0],simplex.c[3],true), - newface(simplex.c[2],simplex.c[1],simplex.c[3],true), - newface(simplex.c[0],simplex.c[2],simplex.c[3],true)}; - if(m_hull.count==4) - { - sFace* best=findbest(); - sFace outer=*best; - U pass=0; - U iterations=0; - bind(tetra[0],0,tetra[1],0); - bind(tetra[0],1,tetra[2],0); - bind(tetra[0],2,tetra[3],0); - bind(tetra[1],1,tetra[3],2); - bind(tetra[1],2,tetra[2],1); - bind(tetra[2],2,tetra[3],1); - m_status=eStatus::Valid; - for(;iterationspass = (U1)(++pass); - gjk.getsupport(best->n,*w); - const real_t wdist=vec3_dot(best->n,w->w)-best->d; - if(wdist>EPA_ACCURACY) - { - for(U j=0;(j<3)&&valid;++j) - { - valid&=expand( pass,w, - best->f[j],best->e[j], - horizon); - } - if(valid&&(horizon.nf>=3)) - { - bind(horizon.cf,1,horizon.ff,2); - remove(m_hull,best); - append(m_stock,best); - best=findbest(); - outer=*best; - } else { m_status=eStatus::InvalidHull;break; } - } else { m_status=eStatus::AccuraryReached;break; } - } else { m_status=eStatus::OutOfVertices;break; } - } - const Vector3 projection=outer.n*outer.d; - m_normal = outer.n; - m_depth = outer.d; - m_result.rank = 3; - m_result.c[0] = outer.c[0]; - m_result.c[1] = outer.c[1]; - m_result.c[2] = outer.c[2]; - m_result.p[0] = vec3_cross( outer.c[1]->w-projection, - outer.c[2]->w-projection).length(); - m_result.p[1] = vec3_cross( outer.c[2]->w-projection, - outer.c[0]->w-projection).length(); - m_result.p[2] = vec3_cross( outer.c[0]->w-projection, - outer.c[1]->w-projection).length(); - const real_t sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; - m_result.p[0] /= sum; - m_result.p[1] /= sum; - m_result.p[2] /= sum; - return(m_status); - } - } - /* Fallback */ - m_status = eStatus::FallBack; - m_normal = -guess; - const real_t nl=m_normal.length(); - if(nl>0) { - m_normal = m_normal/nl; - } else { - m_normal = Vector3(1,0,0); -} - m_depth = 0; - m_result.rank=1; - m_result.c[0]=simplex.c[0]; - m_result.p[0]=1; - return(m_status); - } - - bool getedgedist(sFace* face, sSV* a, sSV* b, real_t& dist) - { - const Vector3 ba = b->w - a->w; - const Vector3 n_ab = vec3_cross(ba, face->n); // Outward facing edge normal direction, on triangle plane - const real_t a_dot_nab = vec3_dot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required - - if (a_dot_nab < 0) - { - // Outside of edge a->b - - const real_t ba_l2 = ba.length_squared(); - const real_t a_dot_ba = vec3_dot(a->w, ba); - const real_t b_dot_ba = vec3_dot(b->w, ba); - - if (a_dot_ba > 0) - { - // Pick distance vertex a - dist = a->w.length(); - } - else if (b_dot_ba < 0) - { - // Pick distance vertex b - dist = b->w.length(); - } - else - { - // Pick distance to edge a->b - const real_t a_dot_b = vec3_dot(a->w, b->w); - dist = Math::sqrt(MAX((a->w.length_squared() * b->w.length_squared() - a_dot_b * a_dot_b) / ba_l2, 0.0)); - } - - return true; - } - - return false; - } - - sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) - { - if(m_stock.root) - { - sFace* face=m_stock.root; - remove(m_stock,face); - append(m_hull,face); - face->pass = 0; - face->c[0] = a; - face->c[1] = b; - face->c[2] = c; - face->n = vec3_cross(b->w-a->w,c->w-a->w); - const real_t l=face->n.length(); - const bool v=l>EPA_ACCURACY; - if(v) - { - if (!(getedgedist(face, a, b, face->d) || - getedgedist(face, b, c, face->d) || - getedgedist(face, c, a, face->d))) - { - // Origin projects to the interior of the triangle - // Use distance to triangle plane - face->d = vec3_dot(a->w, face->n) / l; - } - face->n /= l; - if(forced||(face->d>=-EPA_PLANE_EPS)) - { - return(face); - } else { m_status=eStatus::NonConvex; -} - } else { m_status=eStatus::Degenerated; -} - remove(m_hull,face); - append(m_stock,face); - return(nullptr); - } - // -- PANDEMONIUM start -- - //m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces; - m_status=eStatus::OutOfFaces; - // -- PANDEMONIUM end -- - return(nullptr); - } - sFace* findbest() - { - sFace* minf=m_hull.root; - real_t mind=minf->d*minf->d; - for(sFace* f=minf->l[1];f;f=f->l[1]) - { - const real_t sqd=f->d*f->d; - if(sqdpass!=pass) - { - const U e1=i1m3[e]; - if((vec3_dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) - { - sFace* nf=newface(f->c[e1],f->c[e],w,false); - if(nf) - { - bind(nf,0,f,e); - if(horizon.cf) { bind(horizon.cf,1,nf,2); } else { horizon.ff=nf; -} - horizon.cf=nf; - ++horizon.nf; - return(true); - } - } - else - { - const U e2=i2m3[e]; - f->pass = (U1)pass; - if( expand(pass,w,f->f[e1],f->e[e1],horizon)&& - expand(pass,w,f->f[e2],f->e[e2],horizon)) - { - remove(m_hull,f); - append(m_stock,f); - return(true); - } - } - } - return(false); - } - - }; - - // - static void Initialize( const ShapeSW* shape0, const Transform& wtrs0, real_t margin0, - const ShapeSW* shape1, const Transform& wtrs1, real_t margin1, - sResults& results, - tShape& shape) - { - /* Results */ - results.witnesses[0] = Vector3(0,0,0); - results.witnesses[1] = Vector3(0,0,0); - results.status = sResults::Separated; - /* Shape */ - shape.Initialize(shape0, wtrs0, margin0, shape1, wtrs1, margin1); - - } - - - -// -// Api -// - -// - -// -bool Distance( const ShapeSW* shape0, - const Transform& wtrs0, - real_t margin0, - const ShapeSW* shape1, - const Transform& wtrs1, - real_t margin1, - const Vector3& guess, - sResults& results) -{ - tShape shape; - Initialize(shape0, wtrs0, margin0, shape1, wtrs1, margin1, results, shape); - GJK gjk; - GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); - if(gjk_status==GJK::eStatus::Valid) - { - Vector3 w0=Vector3(0,0,0); - Vector3 w1=Vector3(0,0,0); - for(U i=0;irank;++i) - { - const real_t p=gjk.m_simplex->p[i]; - w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; - w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; - } - results.witnesses[0] = w0; - results.witnesses[1] = w1; - results.normal = w0-w1; - results.distance = results.normal.length(); - results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1; - return(true); - } - else - { - results.status = gjk_status==GJK::eStatus::Inside? - sResults::Penetrating : - sResults::GJK_Failed ; - return(false); - } -} - -// -bool Penetration( const ShapeSW* shape0, - const Transform& wtrs0, - real_t margin0, - const ShapeSW* shape1, - const Transform& wtrs1, - real_t margin1, - const Vector3& guess, - sResults& results - ) -{ - tShape shape; - Initialize(shape0, wtrs0, margin0, shape1, wtrs1, margin1, results, shape); - GJK gjk; - GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess); - switch(gjk_status) - { - case GJK::eStatus::Inside: - { - EPA epa; - EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess); - if(epa_status!=EPA::eStatus::Failed) - { - Vector3 w0=Vector3(0,0,0); - for(U i=0;id,0)*epa.m_result.p[i]; - } - results.status = sResults::Penetrating; - results.witnesses[0] = w0; - results.witnesses[1] = w0-epa.m_normal*epa.m_depth; - results.normal = -epa.m_normal; - results.distance = -epa.m_depth; - return(true); - } else { results.status=sResults::EPA_Failed; -} - } - break; - case GJK::eStatus::Failed: - results.status=sResults::GJK_Failed; - break; - default: {} - } - return(false); -} - - -/* Symbols cleanup */ - -#undef GJK_MAX_ITERATIONS -#undef GJK_ACCURARY -#undef GJK_MIN_DISTANCE -#undef GJK_DUPLICATED_EPS -#undef GJK_SIMPLEX2_EPS -#undef GJK_SIMPLEX3_EPS -#undef GJK_SIMPLEX4_EPS - -#undef EPA_MAX_VERTICES -#undef EPA_MAX_FACES -#undef EPA_MAX_ITERATIONS -#undef EPA_ACCURACY -#undef EPA_FALLBACK -#undef EPA_PLANE_EPS -#undef EPA_INSIDE_EPS - - -} // end of namespace - -/* clang-format on */ - -bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B) { - GjkEpa2::sResults res; - - if (GjkEpa2::Distance(p_shape_A, p_transform_A, 0.0, p_shape_B, p_transform_B, 0.0, p_transform_B.origin - p_transform_A.origin, res)) { - r_result_A = res.witnesses[0]; - r_result_B = res.witnesses[1]; - return true; - } - - return false; -} - -bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, real_t p_margin_A, real_t p_margin_B) { - GjkEpa2::sResults res; - - if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_margin_A, p_shape_B, p_transform_B, p_margin_B, p_transform_B.origin - p_transform_A.origin, res)) { - if (p_result_callback) { - if (p_swap) { - p_result_callback(res.witnesses[1], res.witnesses[0], p_userdata); - } else { - p_result_callback(res.witnesses[0], res.witnesses[1], p_userdata); - } - } - return true; - } - - return false; -} diff --git a/servers/physics/gjk_epa.h b/servers/physics/gjk_epa.h deleted file mode 100644 index e4aa69b..0000000 --- a/servers/physics/gjk_epa.h +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef GJK_EPA_H -#define GJK_EPA_H -/*************************************************************************/ -/* gjk_epa.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" -#include "shape_sw.h" - -bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::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 ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B); - -#endif diff --git a/servers/physics/joints/SCsub b/servers/physics/joints/SCsub deleted file mode 100644 index 86681f9..0000000 --- a/servers/physics/joints/SCsub +++ /dev/null @@ -1,5 +0,0 @@ -#!/usr/bin/env python - -Import("env") - -env.add_source_files(env.servers_sources, "*.cpp") diff --git a/servers/physics/joints/cone_twist_joint_sw.cpp b/servers/physics/joints/cone_twist_joint_sw.cpp deleted file mode 100644 index 3edd646..0000000 --- a/servers/physics/joints/cone_twist_joint_sw.cpp +++ /dev/null @@ -1,356 +0,0 @@ -/*************************************************************************/ -/* cone_twist_joint_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 Pandemonium 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_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; -} - -ConeTwistJointSW::ConeTwistJointSW(BodySW *rbA, BodySW *rbB, const Transform &rbAFrame, const Transform &rbBFrame) : - JointSW(_arr, 2) { - A = rbA; - B = rbB; - - m_rbAFrame = rbAFrame; - m_rbBFrame = rbBFrame; - - m_swingSpan1 = Math_PI / 4.0; - m_swingSpan2 = Math_PI / 4.0; - m_twistSpan = Math_PI * 2; - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; - - m_angularOnly = false; - m_solveTwistLimit = false; - m_solveSwingLimit = false; - - A->add_constraint(this, 0); - B->add_constraint(this, 1); - - m_appliedImpulse = 0; -} - -bool ConeTwistJointSW::setup(real_t p_timestep) { - if ((A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC)) { - 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], - JacobianEntrySW( - 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 ConeTwistJointSW::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; - A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector); - B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector); - } - } - - { - ///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; - - A->apply_torque_impulse(impulse); - 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; - - A->apply_torque_impulse(impulse); - B->apply_torque_impulse(-impulse); - } - } -} - -void ConeTwistJointSW::set_param(PhysicsServer::ConeTwistJointParam p_param, real_t p_value) { - switch (p_param) { - case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN: { - m_swingSpan1 = p_value; - m_swingSpan2 = p_value; - } break; - case PhysicsServer::CONE_TWIST_JOINT_TWIST_SPAN: { - m_twistSpan = p_value; - } break; - case PhysicsServer::CONE_TWIST_JOINT_BIAS: { - m_biasFactor = p_value; - } break; - case PhysicsServer::CONE_TWIST_JOINT_SOFTNESS: { - m_limitSoftness = p_value; - } break; - case PhysicsServer::CONE_TWIST_JOINT_RELAXATION: { - m_relaxationFactor = p_value; - } break; - case PhysicsServer::CONE_TWIST_MAX: - break; // Can't happen, but silences warning - } -} - -real_t ConeTwistJointSW::get_param(PhysicsServer::ConeTwistJointParam p_param) const { - switch (p_param) { - case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN: { - return m_swingSpan1; - } break; - case PhysicsServer::CONE_TWIST_JOINT_TWIST_SPAN: { - return m_twistSpan; - } break; - case PhysicsServer::CONE_TWIST_JOINT_BIAS: { - return m_biasFactor; - } break; - case PhysicsServer::CONE_TWIST_JOINT_SOFTNESS: { - return m_limitSoftness; - } break; - case PhysicsServer::CONE_TWIST_JOINT_RELAXATION: { - return m_relaxationFactor; - } break; - case PhysicsServer::CONE_TWIST_MAX: - break; // Can't happen, but silences warning - } - - return 0; -} diff --git a/servers/physics/joints/cone_twist_joint_sw.h b/servers/physics/joints/cone_twist_joint_sw.h deleted file mode 100644 index 942d1e4..0000000 --- a/servers/physics/joints/cone_twist_joint_sw.h +++ /dev/null @@ -1,141 +0,0 @@ -#ifndef CONE_TWIST_JOINT_SW_H -#define CONE_TWIST_JOINT_SW_H -/*************************************************************************/ -/* cone_twist_joint_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 Pandemonium 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 "servers/physics/joints/jacobian_entry_sw.h" -#include "servers/physics/joints_sw.h" - -///ConeTwistJointSW can be used to simulate ragdoll joints (upper arm, leg etc) -class ConeTwistJointSW : public JointSW { -#ifdef IN_PARALLELL_SOLVER -public: -#endif - - union { - struct { - BodySW *A; - BodySW *B; - }; - - BodySW *_arr[2]; - }; - - JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints - - real_t m_appliedImpulse; - Transform m_rbAFrame; - Transform m_rbBFrame; - - real_t m_limitSoftness; - real_t m_biasFactor; - real_t m_relaxationFactor; - - real_t m_swingSpan1; - real_t m_swingSpan2; - real_t m_twistSpan; - - Vector3 m_swingAxis; - Vector3 m_twistAxis; - - real_t m_kSwing; - real_t m_kTwist; - - real_t m_twistLimitSign; - real_t m_swingCorrection; - real_t m_twistCorrection; - - real_t m_accSwingLimitImpulse; - real_t m_accTwistLimitImpulse; - - bool m_angularOnly; - bool m_solveTwistLimit; - bool m_solveSwingLimit; - -public: - virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_CONE_TWIST; } - - virtual bool setup(real_t p_timestep); - virtual void solve(real_t p_timestep); - - ConeTwistJointSW(BodySW *rbA, BodySW *rbB, const Transform &rbAFrame, const Transform &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(PhysicsServer::ConeTwistJointParam p_param, real_t p_value); - real_t get_param(PhysicsServer::ConeTwistJointParam p_param) const; -}; - -#endif // CONE_TWIST_JOINT_SW_H diff --git a/servers/physics/joints/generic_6dof_joint_sw.cpp b/servers/physics/joints/generic_6dof_joint_sw.cpp deleted file mode 100644 index ca3c445..0000000 --- a/servers/physics/joints/generic_6dof_joint_sw.cpp +++ /dev/null @@ -1,663 +0,0 @@ -/*************************************************************************/ -/* generic_6dof_joint_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 Pandemonium 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_sw.h" - -#define GENERIC_D6_DISABLE_WARMSTARTING 1 - -//////////////////////////// G6DOFRotationalLimitMotorSW //////////////////////////////////// - -int G6DOFRotationalLimitMotorSW::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 G6DOFRotationalLimitMotorSW::solveAngularLimits( - real_t timeStep, Vector3 &axis, real_t jacDiagABInv, - BodySW *body0, BodySW *body1) { - 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; - - body0->apply_torque_impulse(motorImp); - if (body1) { - body1->apply_torque_impulse(-motorImp); - } - - return clippedMotorImpulse; -} - -//////////////////////////// End G6DOFRotationalLimitMotorSW //////////////////////////////////// - -//////////////////////////// G6DOFTranslationalLimitMotorSW //////////////////////////////////// -real_t G6DOFTranslationalLimitMotorSW::solveLinearAxis( - real_t timeStep, - real_t jacDiagABInv, - BodySW *body1, const Vector3 &pointInA, - BodySW *body2, const Vector3 &pointInB, - 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; - body1->apply_impulse(rel_pos1, impulse_vector); - body2->apply_impulse(rel_pos2, -impulse_vector); - return normalImpulse; -} - -//////////////////////////// G6DOFTranslationalLimitMotorSW //////////////////////////////////// - -Generic6DOFJointSW::Generic6DOFJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA) : - JointSW(_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 Generic6DOFJointSW::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 Generic6DOFJointSW::calculateTransforms() { - m_calculatedTransformA = A->get_transform() * m_frameInA; - m_calculatedTransformB = B->get_transform() * m_frameInB; - - calculateAngleInfo(); -} - -void Generic6DOFJointSW::buildLinearJacobian( - JacobianEntrySW &jacLinear, const Vector3 &normalWorld, - const Vector3 &pivotAInW, const Vector3 &pivotBInW) { - memnew_placement( - &jacLinear, - JacobianEntrySW( - 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 Generic6DOFJointSW::buildAngularJacobian( - JacobianEntrySW &jacAngular, const Vector3 &jointAxisW) { - memnew_placement( - &jacAngular, - JacobianEntrySW( - jointAxisW, - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia())); -} - -bool Generic6DOFJointSW::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 Generic6DOFJointSW::setup(real_t p_timestep) { - if ((A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC)) { - 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 Generic6DOFJointSW::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, - 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); - } - } -} - -void Generic6DOFJointSW::updateRHS(real_t timeStep) { - (void)timeStep; -} - -Vector3 Generic6DOFJointSW::getAxis(int axis_index) const { - return m_calculatedAxis[axis_index]; -} - -real_t Generic6DOFJointSW::getAngle(int axis_index) const { - return m_calculatedAxisAngleDiff[axis_index]; -} - -void Generic6DOFJointSW::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 Generic6DOFJointSW::set_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param, real_t p_value) { - ERR_FAIL_INDEX(p_axis, 3); - switch (p_param) { - case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { - m_linearLimits.m_lowerLimit[p_axis] = p_value; - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT: { - m_linearLimits.m_upperLimit[p_axis] = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: { - m_linearLimits.m_limitSoftness[p_axis] = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION: { - m_linearLimits.m_restitution[p_axis] = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING: { - m_linearLimits.m_damping[p_axis] = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: { - m_angularLimits[p_axis].m_loLimit = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: { - m_angularLimits[p_axis].m_hiLimit = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: { - m_angularLimits[p_axis].m_limitSoftness = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING: { - m_angularLimits[p_axis].m_damping = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION: { - m_angularLimits[p_axis].m_bounce = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: { - m_angularLimits[p_axis].m_maxLimitForce = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP: { - m_angularLimits[p_axis].m_ERP = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: { - m_angularLimits[p_axis].m_targetVelocity = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: { - m_angularLimits[p_axis].m_maxLimitForce = p_value; - - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: { - // Not implemented in PandemoniumPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: { - // Not implemented in PandemoniumPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: { - // Not implemented in PandemoniumPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_DAMPING: { - // Not implemented in PandemoniumPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: { - // Not implemented in PandemoniumPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: { - // Not implemented in PandemoniumPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: { - // Not implemented in PandemoniumPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: { - // Not implemented in PandemoniumPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_MAX: - break; // Can't happen, but silences warning - } -} - -real_t Generic6DOFJointSW::get_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param) const { - ERR_FAIL_INDEX_V(p_axis, 3, 0); - switch (p_param) { - case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { - return m_linearLimits.m_lowerLimit[p_axis]; - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT: { - return m_linearLimits.m_upperLimit[p_axis]; - - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: { - return m_linearLimits.m_limitSoftness[p_axis]; - - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION: { - return m_linearLimits.m_restitution[p_axis]; - - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING: { - return m_linearLimits.m_damping[p_axis]; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: { - return m_angularLimits[p_axis].m_loLimit; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: { - return m_angularLimits[p_axis].m_hiLimit; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: { - return m_angularLimits[p_axis].m_limitSoftness; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING: { - return m_angularLimits[p_axis].m_damping; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION: { - return m_angularLimits[p_axis].m_bounce; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: { - return m_angularLimits[p_axis].m_maxLimitForce; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP: { - return m_angularLimits[p_axis].m_ERP; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: { - return m_angularLimits[p_axis].m_targetVelocity; - - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: { - return m_angularLimits[p_axis].m_maxMotorForce; - - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: { - // Not implemented in PandemoniumPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: { - // Not implemented in PandemoniumPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: { - // Not implemented in PandemoniumPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_DAMPING: { - // Not implemented in PandemoniumPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: { - // Not implemented in PandemoniumPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: { - // Not implemented in PandemoniumPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: { - // Not implemented in PandemoniumPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: { - // Not implemented in PandemoniumPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_MAX: - break; // Can't happen, but silences warning - } - return 0; -} - -void Generic6DOFJointSW::set_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value) { - ERR_FAIL_INDEX(p_axis, 3); - - switch (p_flag) { - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: { - m_linearLimits.enable_limit[p_axis] = p_value; - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: { - m_angularLimits[p_axis].m_enableLimit = p_value; - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR: { - m_angularLimits[p_axis].m_enableMotor = p_value; - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: { - // Not implemented in PandemoniumPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: { - // Not implemented in PandemoniumPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: { - // Not implemented in PandemoniumPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_MAX: - break; // Can't happen, but silences warning - } -} -bool Generic6DOFJointSW::get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag) const { - ERR_FAIL_INDEX_V(p_axis, 3, 0); - switch (p_flag) { - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: { - return m_linearLimits.enable_limit[p_axis]; - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: { - return m_angularLimits[p_axis].m_enableLimit; - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR: { - return m_angularLimits[p_axis].m_enableMotor; - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: { - // Not implemented in PandemoniumPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: { - // Not implemented in PandemoniumPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: { - // Not implemented in PandemoniumPhysics backend - } break; - case PhysicsServer::G6DOF_JOINT_FLAG_MAX: - break; // Can't happen, but silences warning - } - - return false; -} diff --git a/servers/physics/joints/generic_6dof_joint_sw.h b/servers/physics/joints/generic_6dof_joint_sw.h deleted file mode 100644 index affaeec..0000000 --- a/servers/physics/joints/generic_6dof_joint_sw.h +++ /dev/null @@ -1,364 +0,0 @@ -#ifndef GENERIC_6DOF_JOINT_SW_H -#define GENERIC_6DOF_JOINT_SW_H -/*************************************************************************/ -/* generic_6dof_joint_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 Pandemonium from the Bullet library. -*/ - -#include "servers/physics/joints/jacobian_entry_sw.h" -#include "servers/physics/joints_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 G6DOFRotationalLimitMotorSW { -public: - //! limit_parameters - //!@{ - real_t m_loLimit; //!< joint limit - real_t m_hiLimit; //!< joint limit - real_t m_targetVelocity; //!< target motor velocity - real_t m_maxMotorForce; //!< max force on motor - real_t m_maxLimitForce; //!< max force on limit - real_t m_damping; //!< Damping. - real_t m_limitSoftness; //! Relaxation factor - real_t m_ERP; //!< Error tolerance factor when joint is at limit - real_t m_bounce; //!< restitution factor - bool m_enableMotor; - bool m_enableLimit; - - //!@} - - //! temp_variables - //!@{ - real_t m_currentLimitError; //!< How much is violated this limit - int m_currentLimit; //!< 0=free, 1=at lo limit, 2=at hi limit - real_t m_accumulatedImpulse; - //!@} - - G6DOFRotationalLimitMotorSW() { - m_accumulatedImpulse = 0.f; - m_targetVelocity = 0; - m_maxMotorForce = 0.1f; - m_maxLimitForce = 300.0f; - m_loLimit = -1e30; - m_hiLimit = 1e30; - m_ERP = 0.5f; - m_bounce = 0.0f; - m_damping = 1.0f; - m_limitSoftness = 0.5f; - m_currentLimit = 0; - m_currentLimitError = 0; - m_enableMotor = false; - m_enableLimit = false; - } - - //! 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, BodySW *body0, BodySW *body1); -}; - -class G6DOFTranslationalLimitMotorSW { -public: - Vector3 m_lowerLimit; //!< the constraint lower limits - Vector3 m_upperLimit; //!< the constraint upper limits - Vector3 m_accumulatedImpulse; - //! Linear_Limit_parameters - //!@{ - Vector3 m_limitSoftness; //!< Softness for linear limit - Vector3 m_damping; //!< Damping for linear limit - Vector3 m_restitution; //! Bounce parameter for linear limit - //!@} - bool enable_limit[3]; - - G6DOFTranslationalLimitMotorSW() { - m_lowerLimit = Vector3(0.f, 0.f, 0.f); - m_upperLimit = Vector3(0.f, 0.f, 0.f); - m_accumulatedImpulse = Vector3(0.f, 0.f, 0.f); - - m_limitSoftness = Vector3(1, 1, 1) * 0.7f; - m_damping = Vector3(1, 1, 1) * real_t(1.0f); - m_restitution = Vector3(1, 1, 1) * real_t(0.5f); - - enable_limit[0] = true; - enable_limit[1] = true; - enable_limit[2] = 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, - BodySW *body1, const Vector3 &pointInA, - BodySW *body2, const Vector3 &pointInB, - int limit_index, - const Vector3 &axis_normal_on_a, - const Vector3 &anchorPos); -}; - -class Generic6DOFJointSW : public JointSW { -protected: - union { - struct { - BodySW *A; - BodySW *B; - }; - - BodySW *_arr[2]; - }; - - //! relative_frames - //!@{ - Transform m_frameInA; //!< the constraint space w.r.t body A - Transform m_frameInB; //!< the constraint space w.r.t body B - //!@} - - //! Jacobians - //!@{ - JacobianEntrySW m_jacLinear[3]; //!< 3 orthogonal linear constraints - JacobianEntrySW m_jacAng[3]; //!< 3 orthogonal angular constraints - //!@} - - //! Linear_Limit_parameters - //!@{ - G6DOFTranslationalLimitMotorSW m_linearLimits; - //!@} - - //! hinge_parameters - //!@{ - G6DOFRotationalLimitMotorSW m_angularLimits[3]; - //!@} - -protected: - //! temporal variables - //!@{ - real_t m_timeStep; - Transform m_calculatedTransformA; - Transform 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; - - //!@} - - Generic6DOFJointSW(Generic6DOFJointSW const &) = delete; - void operator=(Generic6DOFJointSW const &) = delete; - - void buildLinearJacobian( - JacobianEntrySW &jacLinear, const Vector3 &normalWorld, - const Vector3 &pivotAInW, const Vector3 &pivotBInW); - - void buildAngularJacobian(JacobianEntrySW &jacAngular, const Vector3 &jointAxisW); - - //! calcs the euler angles between the two bodies. - void calculateAngleInfo(); - -public: - Generic6DOFJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA); - - virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_6DOF; } - - virtual bool setup(real_t p_timestep); - virtual void solve(real_t p_timestep); - - // 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 Transform &getCalculatedTransformA() const { - return m_calculatedTransformA; - } - - // Gets the global transform of the offset for body B. - const Transform &getCalculatedTransformB() const { - return m_calculatedTransformB; - } - - const Transform &getFrameOffsetA() const { - return m_frameInA; - } - - const Transform &getFrameOffsetB() const { - return m_frameInB; - } - - Transform &getFrameOffsetA() { - return m_frameInA; - } - - Transform &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 - G6DOFRotationalLimitMotorSW *getRotationalLimitMotor(int index) { - return &m_angularLimits[index]; - } - - //! Retrieves the limit information - G6DOFTranslationalLimitMotorSW *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 BodySW *getRigidBodyA() const { - return A; - } - const BodySW *getRigidBodyB() const { - return B; - } - - virtual void calcAnchorPos(); // overridable - - void set_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param, real_t p_value); - real_t get_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param) const; - - void set_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value); - bool get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag) const; -}; - -#endif // GENERIC_6DOF_JOINT_SW_H diff --git a/servers/physics/joints/hinge_joint_sw.cpp b/servers/physics/joints/hinge_joint_sw.cpp deleted file mode 100644 index 151e2ce..0000000 --- a/servers/physics/joints/hinge_joint_sw.cpp +++ /dev/null @@ -1,487 +0,0 @@ -/*************************************************************************/ -/* hinge_joint_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 Pandemonium 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_sw.h" - -void HingeJointSW::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); - } -} - -HingeJointSW::HingeJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameA, const Transform &frameB) : - JointSW(_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.); - - //start with free - m_lowerLimit = Math_PI; - m_upperLimit = -Math_PI; - - m_useLimit = false; - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; - m_limitSoftness = 0.9f; - m_solveLimit = false; - - tau = 0.3; - - m_angularOnly = false; - m_enableAngularMotor = false; - - A->add_constraint(this, 0); - B->add_constraint(this, 1); -} - -HingeJointSW::HingeJointSW(BodySW *rbA, BodySW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, - const Vector3 &axisInA, const Vector3 &axisInB) : - JointSW(_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); - - //start with free - m_lowerLimit = Math_PI; - m_upperLimit = -Math_PI; - - m_useLimit = false; - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; - m_limitSoftness = 0.9f; - m_solveLimit = false; - - tau = 0.3; - - m_angularOnly = false; - m_enableAngularMotor = false; - - A->add_constraint(this, 0); - B->add_constraint(this, 1); -} - -bool HingeJointSW::setup(real_t p_step) { - if ((A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC)) { - 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], - JacobianEntrySW( - 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], - JacobianEntrySW( - 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], - JacobianEntrySW( - 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], - JacobianEntrySW( - 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 HingeJointSW::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; - A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector); - B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector); - } - } - - { - ///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; - } - - A->apply_torque_impulse(-velrelOrthog + angularError); - 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; - A->apply_torque_impulse(impulse); - 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; - - A->apply_torque_impulse(motorImp + angularLimit); - B->apply_torque_impulse(-motorImp - angularLimit); - } - } -} -/* -void HingeJointSW::updateRHS(real_t timeStep) -{ - (void)timeStep; - -} -*/ - -real_t HingeJointSW::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 HingeJointSW::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 HingeJointSW::set_param(PhysicsServer::HingeJointParam p_param, real_t p_value) { - switch (p_param) { - case PhysicsServer::HINGE_JOINT_BIAS: - tau = p_value; - break; - case PhysicsServer::HINGE_JOINT_LIMIT_UPPER: - m_upperLimit = p_value; - break; - case PhysicsServer::HINGE_JOINT_LIMIT_LOWER: - m_lowerLimit = p_value; - break; - case PhysicsServer::HINGE_JOINT_LIMIT_BIAS: - m_biasFactor = p_value; - break; - case PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS: - m_limitSoftness = p_value; - break; - case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION: - m_relaxationFactor = p_value; - break; - case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY: - m_motorTargetVelocity = p_value; - break; - case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE: - m_maxMotorImpulse = p_value; - break; - case PhysicsServer::HINGE_JOINT_MAX: - break; // Can't happen, but silences warning - } -} - -real_t HingeJointSW::get_param(PhysicsServer::HingeJointParam p_param) const { - switch (p_param) { - case PhysicsServer::HINGE_JOINT_BIAS: - return tau; - case PhysicsServer::HINGE_JOINT_LIMIT_UPPER: - return m_upperLimit; - case PhysicsServer::HINGE_JOINT_LIMIT_LOWER: - return m_lowerLimit; - case PhysicsServer::HINGE_JOINT_LIMIT_BIAS: - return m_biasFactor; - case PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS: - return m_limitSoftness; - case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION: - return m_relaxationFactor; - case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY: - return m_motorTargetVelocity; - case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE: - return m_maxMotorImpulse; - case PhysicsServer::HINGE_JOINT_MAX: - break; // Can't happen, but silences warning - } - - return 0; -} - -void HingeJointSW::set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value) { - switch (p_flag) { - case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT: - m_useLimit = p_value; - break; - case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR: - m_enableAngularMotor = p_value; - break; - case PhysicsServer::HINGE_JOINT_FLAG_MAX: - break; // Can't happen, but silences warning - } -} -bool HingeJointSW::get_flag(PhysicsServer::HingeJointFlag p_flag) const { - switch (p_flag) { - case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT: - return m_useLimit; - case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR: - return m_enableAngularMotor; - case PhysicsServer::HINGE_JOINT_FLAG_MAX: - break; // Can't happen, but silences warning - } - - return false; -} diff --git a/servers/physics/joints/hinge_joint_sw.h b/servers/physics/joints/hinge_joint_sw.h deleted file mode 100644 index 45d8837..0000000 --- a/servers/physics/joints/hinge_joint_sw.h +++ /dev/null @@ -1,118 +0,0 @@ -#ifndef HINGE_JOINT_SW_H -#define HINGE_JOINT_SW_H -/*************************************************************************/ -/* hinge_joint_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 Pandemonium from the Bullet library. -*/ - -#include "servers/physics/joints/jacobian_entry_sw.h" -#include "servers/physics/joints_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 HingeJointSW : public JointSW { - union { - struct { - BodySW *A; - BodySW *B; - }; - - BodySW *_arr[2]; - }; - - JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints - JacobianEntrySW m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor - - Transform m_rbAFrame; // constraint axii. Assumes z is hinge axis. - Transform m_rbBFrame; - - real_t m_motorTargetVelocity; - real_t m_maxMotorImpulse; - - real_t m_limitSoftness; - real_t m_biasFactor; - real_t m_relaxationFactor; - - real_t m_lowerLimit; - real_t m_upperLimit; - - real_t m_kHinge; - - real_t m_limitSign; - real_t m_correction; - - real_t m_accLimitImpulse; - - real_t tau; - - bool m_useLimit; - bool m_angularOnly; - bool m_enableAngularMotor; - bool m_solveLimit; - - real_t m_appliedImpulse; - - void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q); - _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x); - -public: - virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_HINGE; } - - virtual bool setup(real_t p_step); - virtual void solve(real_t p_step); - - real_t get_hinge_angle(); - - void set_param(PhysicsServer::HingeJointParam p_param, real_t p_value); - real_t get_param(PhysicsServer::HingeJointParam p_param) const; - - void set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value); - bool get_flag(PhysicsServer::HingeJointFlag p_flag) const; - - HingeJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameA, const Transform &frameB); - HingeJointSW(BodySW *rbA, BodySW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB); -}; - -#endif // HINGE_JOINT_SW_H diff --git a/servers/physics/joints/jacobian_entry_sw.h b/servers/physics/joints/jacobian_entry_sw.h deleted file mode 100644 index ff18d74..0000000 --- a/servers/physics/joints/jacobian_entry_sw.h +++ /dev/null @@ -1,168 +0,0 @@ -#ifndef JACOBIAN_ENTRY_SW_H -#define JACOBIAN_ENTRY_SW_H -/*************************************************************************/ -/* jacobian_entry_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 Pandemonium from the Bullet library. -*/ - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "core/math/transform.h" - -class JacobianEntrySW { -public: - JacobianEntrySW(){}; - //constraint between two different rigidbodies - JacobianEntrySW( - 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 - JacobianEntrySW(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 - JacobianEntrySW(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 - JacobianEntrySW( - 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 JacobianEntrySW &jacB, const real_t massInvA) const { - const JacobianEntrySW &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 JacobianEntrySW &jacB, const real_t massInvA, const real_t massInvB) const { - const JacobianEntrySW &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; -}; - -#endif // JACOBIAN_ENTRY_SW_H diff --git a/servers/physics/joints/pin_joint_sw.cpp b/servers/physics/joints/pin_joint_sw.cpp deleted file mode 100644 index fb4f19d..0000000 --- a/servers/physics/joints/pin_joint_sw.cpp +++ /dev/null @@ -1,170 +0,0 @@ -/*************************************************************************/ -/* pin_joint_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 Pandemonium 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_sw.h" - -bool PinJointSW::setup(real_t p_step) { - if ((A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC)) { - 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], - JacobianEntrySW( - 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 PinJointSW::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); - - 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); - - //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; - A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector); - B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector); - - normal[i] = 0; - } -} - -void PinJointSW::set_param(PhysicsServer::PinJointParam p_param, real_t p_value) { - switch (p_param) { - case PhysicsServer::PIN_JOINT_BIAS: - m_tau = p_value; - break; - case PhysicsServer::PIN_JOINT_DAMPING: - m_damping = p_value; - break; - case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP: - m_impulseClamp = p_value; - break; - } -} - -real_t PinJointSW::get_param(PhysicsServer::PinJointParam p_param) const { - switch (p_param) { - case PhysicsServer::PIN_JOINT_BIAS: - return m_tau; - case PhysicsServer::PIN_JOINT_DAMPING: - return m_damping; - case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP: - return m_impulseClamp; - } - - return 0; -} - -PinJointSW::PinJointSW(BodySW *p_body_a, const Vector3 &p_pos_a, BodySW *p_body_b, const Vector3 &p_pos_b) : - JointSW(_arr, 2) { - A = p_body_a; - B = p_body_b; - m_pivotInA = p_pos_a; - m_pivotInB = p_pos_b; - - m_tau = 0.3; - m_damping = 1; - m_impulseClamp = 0; - m_appliedImpulse = 0; - - A->add_constraint(this, 0); - B->add_constraint(this, 1); -} - -PinJointSW::~PinJointSW() { -} diff --git a/servers/physics/joints/pin_joint_sw.h b/servers/physics/joints/pin_joint_sw.h deleted file mode 100644 index 93de160..0000000 --- a/servers/physics/joints/pin_joint_sw.h +++ /dev/null @@ -1,94 +0,0 @@ -#ifndef PIN_JOINT_SW_H -#define PIN_JOINT_SW_H -/*************************************************************************/ -/* pin_joint_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 Pandemonium from the Bullet library. -*/ - -#include "servers/physics/joints/jacobian_entry_sw.h" -#include "servers/physics/joints_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 PinJointSW : public JointSW { - union { - struct { - BodySW *A; - BodySW *B; - }; - - BodySW *_arr[2]; - }; - - real_t m_tau; //bias - real_t m_damping; - real_t m_impulseClamp; - real_t m_appliedImpulse; - - JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints - - Vector3 m_pivotInA; - Vector3 m_pivotInB; - -public: - virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_PIN; } - - virtual bool setup(real_t p_step); - virtual void solve(real_t p_step); - - void set_param(PhysicsServer::PinJointParam p_param, real_t p_value); - real_t get_param(PhysicsServer::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; } - - PinJointSW(BodySW *p_body_a, const Vector3 &p_pos_a, BodySW *p_body_b, const Vector3 &p_pos_b); - ~PinJointSW(); -}; - -#endif // PIN_JOINT_SW_H diff --git a/servers/physics/joints/slider_joint_sw.cpp b/servers/physics/joints/slider_joint_sw.cpp deleted file mode 100644 index 7e20408..0000000 --- a/servers/physics/joints/slider_joint_sw.cpp +++ /dev/null @@ -1,513 +0,0 @@ -/*************************************************************************/ -/* slider_joint_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 Pandemonium 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_sw.h" - -//----------------------------------------------------------------------------- - -real_t SliderJointSW::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; -} - -void SliderJointSW::initParams() { - m_lowerLinLimit = real_t(1.0); - m_upperLinLimit = real_t(-1.0); - m_lowerAngLimit = real_t(0.); - m_upperAngLimit = real_t(0.); - m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingDirLin = real_t(0.); - m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingDirAng = real_t(0.); - m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING; - m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING; - m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING; - m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING; - - m_poweredLinMotor = false; - m_targetLinMotorVelocity = real_t(0.); - m_maxLinMotorForce = real_t(0.); - m_accumulatedLinMotorImpulse = real_t(0.0); - - m_poweredAngMotor = false; - m_targetAngMotorVelocity = real_t(0.); - m_maxAngMotorForce = real_t(0.); - m_accumulatedAngMotorImpulse = real_t(0.0); - -} // SliderJointSW::initParams() - -//----------------------------------------------------------------------------- - -//----------------------------------------------------------------------------- - -SliderJointSW::SliderJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB) : - JointSW(_arr, 2), - m_frameInA(frameInA), - m_frameInB(frameInB) { - A = rbA; - B = rbB; - - A->add_constraint(this, 0); - B->add_constraint(this, 1); - - initParams(); -} // SliderJointSW::SliderJointSW() - -//----------------------------------------------------------------------------- - -bool SliderJointSW::setup(real_t p_step) { - if ((A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC)) { - 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], - JacobianEntrySW( - 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], - JacobianEntrySW( - 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 SliderJointSW::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; - A->apply_impulse(m_relPosA, impulse_vector); - B->apply_impulse(m_relPosB, -impulse_vector); - 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; - A->apply_impulse(m_relPosA, impulse_vector); - B->apply_impulse(m_relPosB, -impulse_vector); - } - } - } - // 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 - A->apply_torque_impulse(-velrelOrthog + angularError); - 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; - A->apply_torque_impulse(impulse); - 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; - A->apply_torque_impulse(motorImp); - B->apply_torque_impulse(-motorImp); - } - } -} // SliderJointSW::solveConstraint() - -//----------------------------------------------------------------------------- - -//----------------------------------------------------------------------------- - -void SliderJointSW::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 SliderJointSW::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 SliderJointSW::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 SliderJointSW::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 SliderJointSW::getAncorInB() { - Vector3 ancorInB; - ancorInB = m_frameInB.origin; - return ancorInB; -} // SliderJointSW::getAncorInB(); - -void SliderJointSW::set_param(PhysicsServer::SliderJointParam p_param, real_t p_value) { - switch (p_param) { - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: - m_upperLinLimit = p_value; - break; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: - m_lowerLinLimit = p_value; - break; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: - m_softnessLimLin = p_value; - break; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: - m_restitutionLimLin = p_value; - break; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: - m_dampingLimLin = p_value; - break; - case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: - m_softnessDirLin = p_value; - break; - case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: - m_restitutionDirLin = p_value; - break; - case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING: - m_dampingDirLin = p_value; - break; - case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: - m_softnessOrthoLin = p_value; - break; - case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: - m_restitutionOrthoLin = p_value; - break; - case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: - m_dampingOrthoLin = p_value; - break; - - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: - m_upperAngLimit = p_value; - break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: - m_lowerAngLimit = p_value; - break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: - m_softnessLimAng = p_value; - break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: - m_restitutionLimAng = p_value; - break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: - m_dampingLimAng = p_value; - break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: - m_softnessDirAng = p_value; - break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: - m_restitutionDirAng = p_value; - break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: - m_dampingDirAng = p_value; - break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: - m_softnessOrthoAng = p_value; - break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: - m_restitutionOrthoAng = p_value; - break; - case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: - m_dampingOrthoAng = p_value; - break; - - case PhysicsServer::SLIDER_JOINT_MAX: - break; // Can't happen, but silences warning - } -} - -real_t SliderJointSW::get_param(PhysicsServer::SliderJointParam p_param) const { - switch (p_param) { - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: - return m_upperLinLimit; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: - return m_lowerLinLimit; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: - return m_softnessLimLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: - return m_restitutionLimLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: - return m_dampingLimLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: - return m_softnessDirLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: - return m_restitutionDirLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING: - return m_dampingDirLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: - return m_softnessOrthoLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: - return m_restitutionOrthoLin; - case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: - return m_dampingOrthoLin; - - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: - return m_upperAngLimit; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: - return m_lowerAngLimit; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: - return m_softnessLimAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: - return m_restitutionLimAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: - return m_dampingLimAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: - return m_softnessDirAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: - return m_restitutionDirAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: - return m_dampingDirAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: - return m_softnessOrthoAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: - return m_restitutionOrthoAng; - case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: - return m_dampingOrthoAng; - - case PhysicsServer::SLIDER_JOINT_MAX: - break; // Can't happen, but silences warning - } - - return 0; -} diff --git a/servers/physics/joints/slider_joint_sw.h b/servers/physics/joints/slider_joint_sw.h deleted file mode 100644 index 801854c..0000000 --- a/servers/physics/joints/slider_joint_sw.h +++ /dev/null @@ -1,251 +0,0 @@ -#ifndef SLIDER_JOINT_SW_H -#define SLIDER_JOINT_SW_H -/*************************************************************************/ -/* slider_joint_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 Pandemonium from the Bullet library. -*/ - -#include "servers/physics/joints/jacobian_entry_sw.h" -#include "servers/physics/joints_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 SliderJointSW : public JointSW { -protected: - union { - struct { - BodySW *A; - BodySW *B; - }; - - BodySW *_arr[2]; - }; - - Transform m_frameInA; - Transform m_frameInB; - - // linear limits - real_t m_lowerLinLimit; - real_t m_upperLinLimit; - // angular limits - real_t m_lowerAngLimit; - real_t m_upperAngLimit; - // 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; - real_t m_restitutionDirLin; - real_t m_dampingDirLin; - real_t m_softnessDirAng; - real_t m_restitutionDirAng; - real_t m_dampingDirAng; - real_t m_softnessLimLin; - real_t m_restitutionLimLin; - real_t m_dampingLimLin; - real_t m_softnessLimAng; - real_t m_restitutionLimAng; - real_t m_dampingLimAng; - real_t m_softnessOrthoLin; - real_t m_restitutionOrthoLin; - real_t m_dampingOrthoLin; - real_t m_softnessOrthoAng; - real_t m_restitutionOrthoAng; - real_t m_dampingOrthoAng; - - // for interlal use - bool m_solveLinLim; - bool m_solveAngLim; - - JacobianEntrySW m_jacLin[3]; - real_t m_jacLinDiagABInv[3]; - - JacobianEntrySW m_jacAng[3]; - - real_t m_timeStep; - Transform m_calculatedTransformA; - Transform 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; - - real_t m_angDepth; - real_t m_kAngle; - - bool m_poweredLinMotor; - real_t m_targetLinMotorVelocity; - real_t m_maxLinMotorForce; - real_t m_accumulatedLinMotorImpulse; - - bool m_poweredAngMotor; - real_t m_targetAngMotorVelocity; - real_t m_maxAngMotorForce; - real_t m_accumulatedAngMotorImpulse; - - //------------------------ - void initParams(); - -private: - _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x); - -public: - // constructors - SliderJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB); - //SliderJointSW(); - // overrides - - // access - const BodySW *getRigidBodyA() const { return A; } - const BodySW *getRigidBodyB() const { return B; } - const Transform &getCalculatedTransformA() const { return m_calculatedTransformA; } - const Transform &getCalculatedTransformB() const { return m_calculatedTransformB; } - const Transform &getFrameOffsetA() const { return m_frameInA; } - const Transform &getFrameOffsetB() const { return m_frameInB; } - Transform &getFrameOffsetA() { return m_frameInA; } - Transform &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(PhysicsServer::SliderJointParam p_param, real_t p_value); - real_t get_param(PhysicsServer::SliderJointParam p_param) const; - - bool setup(real_t p_step); - void solve(real_t p_step); - - virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_SLIDER; } -}; - -#endif // SLIDER_JOINT_SW_H diff --git a/servers/physics/joints_sw.h b/servers/physics/joints_sw.h deleted file mode 100644 index 9785d31..0000000 --- a/servers/physics/joints_sw.h +++ /dev/null @@ -1,44 +0,0 @@ -#ifndef JOINTS_SW_H -#define JOINTS_SW_H -/*************************************************************************/ -/* joints_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" -#include "constraint_sw.h" - -class JointSW : public ConstraintSW { -public: - virtual PhysicsServer::JointType get_type() const = 0; - _FORCE_INLINE_ JointSW(BodySW **p_body_ptr = nullptr, int p_body_count = 0) : - ConstraintSW(p_body_ptr, p_body_count) { - } -}; - -#endif // JOINTS_SW_H diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp deleted file mode 100644 index 7a6af60..0000000 --- a/servers/physics/physics_server_sw.cpp +++ /dev/null @@ -1,1451 +0,0 @@ -/*************************************************************************/ -/* physics_server_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" - -#include "broad_phase_basic.h" -#include "broad_phase_bvh.h" -#include "broad_phase_octree.h" -#include "core/config/project_settings.h" -#include "core/object/script_language.h" -#include "core/os/os.h" -#include "joints/cone_twist_joint_sw.h" -#include "joints/generic_6dof_joint_sw.h" -#include "joints/hinge_joint_sw.h" -#include "joints/pin_joint_sw.h" -#include "joints/slider_joint_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 PhysicsServerSW::shape_create(ShapeType p_shape) { - ShapeSW *shape = nullptr; - switch (p_shape) { - case SHAPE_PLANE: { - shape = memnew(PlaneShapeSW); - } break; - case SHAPE_RAY: { - shape = memnew(RayShapeSW); - } break; - case SHAPE_SPHERE: { - shape = memnew(SphereShapeSW); - } break; - case SHAPE_BOX: { - shape = memnew(BoxShapeSW); - } break; - case SHAPE_CAPSULE: { - shape = memnew(CapsuleShapeSW); - } break; - case SHAPE_CYLINDER: { - shape = memnew(CylinderShapeSW); - } break; - case SHAPE_CONVEX_POLYGON: { - shape = memnew(ConvexPolygonShapeSW); - } break; - case SHAPE_CONCAVE_POLYGON: { - shape = memnew(ConcavePolygonShapeSW); - } break; - case SHAPE_HEIGHTMAP: { - shape = memnew(HeightMapShapeSW); - } break; - case SHAPE_CUSTOM: { - ERR_FAIL_V(RID()); - - } break; - } - - RID id = shape_owner.make_rid(shape); - shape->set_self(id); - - return id; -}; - -void PhysicsServerSW::shape_set_data(RID p_shape, const Variant &p_data) { - ShapeSW *shape = shape_owner.get(p_shape); - ERR_FAIL_COND(!shape); - shape->set_data(p_data); -}; - -void PhysicsServerSW::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) { - ShapeSW *shape = shape_owner.get(p_shape); - ERR_FAIL_COND(!shape); - shape->set_custom_bias(p_bias); -} - -PhysicsServer::ShapeType PhysicsServerSW::shape_get_type(RID p_shape) const { - const ShapeSW *shape = shape_owner.get(p_shape); - ERR_FAIL_COND_V(!shape, SHAPE_CUSTOM); - return shape->get_type(); -}; - -Variant PhysicsServerSW::shape_get_data(RID p_shape) const { - const ShapeSW *shape = shape_owner.get(p_shape); - ERR_FAIL_COND_V(!shape, Variant()); - ERR_FAIL_COND_V(!shape->is_configured(), Variant()); - return shape->get_data(); -}; - -void PhysicsServerSW::shape_set_margin(RID p_shape, real_t p_margin) { -} - -real_t PhysicsServerSW::shape_get_margin(RID p_shape) const { - return 0.0; -} - -real_t PhysicsServerSW::shape_get_custom_solver_bias(RID p_shape) const { - const ShapeSW *shape = shape_owner.get(p_shape); - ERR_FAIL_COND_V(!shape, 0); - return shape->get_custom_bias(); -} - -RID PhysicsServerSW::space_create() { - SpaceSW *space = memnew(SpaceSW); - RID id = space_owner.make_rid(space); - space->set_self(id); - RID area_id = RID_PRIME(area_create()); - AreaSW *area = area_owner.get(area_id); - ERR_FAIL_COND_V(!area, RID()); - space->set_default_area(area); - area->set_space(space); - area->set_priority(-1); - RID sgb = RID_PRIME(body_create()); - body_set_space(sgb, id); - body_set_mode(sgb, BODY_MODE_STATIC); - space->set_static_global_body(sgb); - - return id; -}; - -void PhysicsServerSW::space_set_active(RID p_space, bool p_active) { - SpaceSW *space = space_owner.get(p_space); - ERR_FAIL_COND(!space); - if (p_active) { - active_spaces.insert(space); - } else { - active_spaces.erase(space); - } -} - -bool PhysicsServerSW::space_is_active(RID p_space) const { - const SpaceSW *space = space_owner.get(p_space); - ERR_FAIL_COND_V(!space, false); - - return active_spaces.has(space); -} - -void PhysicsServerSW::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) { - SpaceSW *space = space_owner.get(p_space); - ERR_FAIL_COND(!space); - - space->set_param(p_param, p_value); -} - -real_t PhysicsServerSW::space_get_param(RID p_space, SpaceParameter p_param) const { - const SpaceSW *space = space_owner.get(p_space); - ERR_FAIL_COND_V(!space, 0); - return space->get_param(p_param); -} - -PhysicsDirectSpaceState *PhysicsServerSW::space_get_direct_state(RID p_space) { - SpaceSW *space = space_owner.get(p_space); - ERR_FAIL_COND_V(!space, nullptr); - ERR_FAIL_COND_V_MSG(space->is_locked(), nullptr, "Space state is inaccessible right now, wait for iteration or physics process notification."); - - return space->get_direct_state(); -} - -void PhysicsServerSW::space_set_debug_contacts(RID p_space, int p_max_contacts) { - SpaceSW *space = space_owner.get(p_space); - ERR_FAIL_COND(!space); - space->set_debug_contacts(p_max_contacts); -} - -Vector PhysicsServerSW::space_get_contacts(RID p_space) const { - SpaceSW *space = space_owner.get(p_space); - ERR_FAIL_COND_V(!space, Vector()); - return space->get_debug_contacts(); -} - -int PhysicsServerSW::space_get_contact_count(RID p_space) const { - SpaceSW *space = space_owner.get(p_space); - ERR_FAIL_COND_V(!space, 0); - return space->get_debug_contact_count(); -} - -RID PhysicsServerSW::area_create() { - AreaSW *area = memnew(AreaSW); - RID rid = area_owner.make_rid(area); - area->set_self(rid); - return rid; -}; - -void PhysicsServerSW::area_set_space(RID p_area, RID p_space) { - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND(!area); - - SpaceSW *space = nullptr; - if (p_space.is_valid()) { - space = space_owner.get(p_space); - ERR_FAIL_COND(!space); - } - - if (area->get_space() == space) { - return; //pointless - } - - area->clear_constraints(); - area->set_space(space); -}; - -RID PhysicsServerSW::area_get_space(RID p_area) const { - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND_V(!area, RID()); - - SpaceSW *space = area->get_space(); - if (!space) { - return RID(); - } - return space->get_self(); -}; - -void PhysicsServerSW::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) { - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND(!area); - - area->set_space_override_mode(p_mode); -} - -PhysicsServer::AreaSpaceOverrideMode PhysicsServerSW::area_get_space_override_mode(RID p_area) const { - const AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND_V(!area, AREA_SPACE_OVERRIDE_DISABLED); - - return area->get_space_override_mode(); -} - -void PhysicsServerSW::area_add_shape(RID p_area, RID p_shape, const Transform &p_transform, bool p_disabled) { - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND(!area); - - ShapeSW *shape = shape_owner.get(p_shape); - ERR_FAIL_COND(!shape); - - area->add_shape(shape, p_transform, p_disabled); -} - -void PhysicsServerSW::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) { - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND(!area); - - ShapeSW *shape = shape_owner.get(p_shape); - ERR_FAIL_COND(!shape); - ERR_FAIL_COND(!shape->is_configured()); - - area->set_shape(p_shape_idx, shape); -} - -void PhysicsServerSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform) { - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND(!area); - - area->set_shape_transform(p_shape_idx, p_transform); -} - -int PhysicsServerSW::area_get_shape_count(RID p_area) const { - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND_V(!area, -1); - - return area->get_shape_count(); -} -RID PhysicsServerSW::area_get_shape(RID p_area, int p_shape_idx) const { - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND_V(!area, RID()); - - ShapeSW *shape = area->get_shape(p_shape_idx); - ERR_FAIL_COND_V(!shape, RID()); - - return shape->get_self(); -} -Transform PhysicsServerSW::area_get_shape_transform(RID p_area, int p_shape_idx) const { - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND_V(!area, Transform()); - - return area->get_shape_transform(p_shape_idx); -} - -void PhysicsServerSW::area_remove_shape(RID p_area, int p_shape_idx) { - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND(!area); - - area->remove_shape(p_shape_idx); -} - -void PhysicsServerSW::area_clear_shapes(RID p_area) { - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND(!area); - - while (area->get_shape_count()) { - area->remove_shape(0); - } -} - -void PhysicsServerSW::area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) { - AreaSW *area = area_owner.get(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 PhysicsServerSW::area_attach_object_instance_id(RID p_area, ObjectID p_id) { - if (space_owner.owns(p_area)) { - SpaceSW *space = space_owner.get(p_area); - p_area = space->get_default_area()->get_self(); - } - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND(!area); - area->set_instance_id(p_id); -} -ObjectID PhysicsServerSW::area_get_object_instance_id(RID p_area) const { - if (space_owner.owns(p_area)) { - SpaceSW *space = space_owner.get(p_area); - p_area = space->get_default_area()->get_self(); - } - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND_V(!area, 0); - return area->get_instance_id(); -} - -void PhysicsServerSW::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) { - if (space_owner.owns(p_area)) { - SpaceSW *space = space_owner.get(p_area); - p_area = space->get_default_area()->get_self(); - } - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND(!area); - area->set_param(p_param, p_value); -}; - -void PhysicsServerSW::area_set_transform(RID p_area, const Transform &p_transform) { - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND(!area); - area->set_transform(p_transform); -}; - -Variant PhysicsServerSW::area_get_param(RID p_area, AreaParameter p_param) const { - if (space_owner.owns(p_area)) { - SpaceSW *space = space_owner.get(p_area); - p_area = space->get_default_area()->get_self(); - } - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND_V(!area, Variant()); - - return area->get_param(p_param); -}; - -Transform PhysicsServerSW::area_get_transform(RID p_area) const { - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND_V(!area, Transform()); - - return area->get_transform(); -}; - -void PhysicsServerSW::area_set_collision_layer(RID p_area, uint32_t p_layer) { - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND(!area); - - area->set_collision_layer(p_layer); -} - -void PhysicsServerSW::area_set_collision_mask(RID p_area, uint32_t p_mask) { - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND(!area); - - area->set_collision_mask(p_mask); -} - -void PhysicsServerSW::area_set_monitorable(RID p_area, bool p_monitorable) { - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND(!area); - FLUSH_QUERY_CHECK(area); - - area->set_monitorable(p_monitorable); -} - -void PhysicsServerSW::area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND(!area); - - area->set_monitor_callback(p_receiver ? p_receiver->get_instance_id() : 0, p_method); -} - -void PhysicsServerSW::area_set_ray_pickable(RID p_area, bool p_enable) { - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND(!area); - - area->set_ray_pickable(p_enable); -} - -bool PhysicsServerSW::area_is_ray_pickable(RID p_area) const { - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND_V(!area, false); - - return area->is_ray_pickable(); -} - -void PhysicsServerSW::area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { - AreaSW *area = area_owner.get(p_area); - ERR_FAIL_COND(!area); - - area->set_area_monitor_callback(p_receiver ? p_receiver->get_instance_id() : 0, p_method); -} - -/* BODY API */ - -RID PhysicsServerSW::body_create(BodyMode p_mode, bool p_init_sleeping) { - BodySW *body = memnew(BodySW); - if (p_mode != BODY_MODE_RIGID) { - body->set_mode(p_mode); - } - if (p_init_sleeping) { - body->set_state(BODY_STATE_SLEEPING, p_init_sleeping); - } - RID rid = body_owner.make_rid(body); - body->set_self(rid); - return rid; -}; - -void PhysicsServerSW::body_set_space(RID p_body, RID p_space) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - SpaceSW *space = nullptr; - if (p_space.is_valid()) { - space = space_owner.get(p_space); - ERR_FAIL_COND(!space); - } - - if (body->get_space() == space) { - return; //pointless - } - - body->clear_constraint_map(); - body->set_space(space); -}; - -RID PhysicsServerSW::body_get_space(RID p_body) const { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body, RID()); - - SpaceSW *space = body->get_space(); - if (!space) { - return RID(); - } - return space->get_self(); -}; - -void PhysicsServerSW::body_set_mode(RID p_body, BodyMode p_mode) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - body->set_mode(p_mode); -}; - -PhysicsServer::BodyMode PhysicsServerSW::body_get_mode(RID p_body) const { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body, BODY_MODE_STATIC); - - return body->get_mode(); -}; - -void PhysicsServerSW::body_add_shape(RID p_body, RID p_shape, const Transform &p_transform, bool p_disabled) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - ShapeSW *shape = shape_owner.get(p_shape); - ERR_FAIL_COND(!shape); - - body->add_shape(shape, p_transform, p_disabled); -} - -void PhysicsServerSW::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - ShapeSW *shape = shape_owner.get(p_shape); - ERR_FAIL_COND(!shape); - ERR_FAIL_COND(!shape->is_configured()); - - body->set_shape(p_shape_idx, shape); -} -void PhysicsServerSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - body->set_shape_transform(p_shape_idx, p_transform); -} - -int PhysicsServerSW::body_get_shape_count(RID p_body) const { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body, -1); - - return body->get_shape_count(); -} -RID PhysicsServerSW::body_get_shape(RID p_body, int p_shape_idx) const { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body, RID()); - - ShapeSW *shape = body->get_shape(p_shape_idx); - ERR_FAIL_COND_V(!shape, RID()); - - return shape->get_self(); -} - -void PhysicsServerSW::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) { - BodySW *body = body_owner.get(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); -} - -Transform PhysicsServerSW::body_get_shape_transform(RID p_body, int p_shape_idx) const { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body, Transform()); - - return body->get_shape_transform(p_shape_idx); -} - -void PhysicsServerSW::body_remove_shape(RID p_body, int p_shape_idx) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - body->remove_shape(p_shape_idx); -} - -void PhysicsServerSW::body_clear_shapes(RID p_body) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - while (body->get_shape_count()) { - body->remove_shape(0); - } -} - -void PhysicsServerSW::body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - body->set_continuous_collision_detection(p_enable); -} - -bool PhysicsServerSW::body_is_continuous_collision_detection_enabled(RID p_body) const { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body, false); - - return body->is_continuous_collision_detection_enabled(); -} - -void PhysicsServerSW::body_set_collision_layer(RID p_body, uint32_t p_layer) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - body->set_collision_layer(p_layer); -} - -uint32_t PhysicsServerSW::body_get_collision_layer(RID p_body) const { - const BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body, 0); - - return body->get_collision_layer(); -} - -void PhysicsServerSW::body_set_collision_mask(RID p_body, uint32_t p_mask) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - body->set_collision_mask(p_mask); -} - -uint32_t PhysicsServerSW::body_get_collision_mask(RID p_body) const { - const BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body, 0); - - return body->get_collision_mask(); -} - -void PhysicsServerSW::body_attach_object_instance_id(RID p_body, uint32_t p_id) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - body->set_instance_id(p_id); -}; - -uint32_t PhysicsServerSW::body_get_object_instance_id(RID p_body) const { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body, 0); - - return body->get_instance_id(); -}; - -void PhysicsServerSW::body_set_user_flags(RID p_body, uint32_t p_flags) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); -}; - -uint32_t PhysicsServerSW::body_get_user_flags(RID p_body) const { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body, 0); - - return 0; -}; - -void PhysicsServerSW::body_set_param(RID p_body, BodyParameter p_param, real_t p_value) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - body->set_param(p_param, p_value); -}; - -real_t PhysicsServerSW::body_get_param(RID p_body, BodyParameter p_param) const { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body, 0); - - return body->get_param(p_param); -}; - -void PhysicsServerSW::body_set_kinematic_safe_margin(RID p_body, real_t p_margin) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - body->set_kinematic_margin(p_margin); -} - -real_t PhysicsServerSW::body_get_kinematic_safe_margin(RID p_body) const { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body, 0); - - return body->get_kinematic_margin(); -} - -void PhysicsServerSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - body->set_state(p_state, p_variant); -}; - -Variant PhysicsServerSW::body_get_state(RID p_body, BodyState p_state) const { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body, Variant()); - - return body->get_state(p_state); -}; - -void PhysicsServerSW::body_set_applied_force(RID p_body, const Vector3 &p_force) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - body->set_applied_force(p_force); - body->wakeup(); -}; - -Vector3 PhysicsServerSW::body_get_applied_force(RID p_body) const { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body, Vector3()); - return body->get_applied_force(); -}; - -void PhysicsServerSW::body_set_applied_torque(RID p_body, const Vector3 &p_torque) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - body->set_applied_torque(p_torque); - body->wakeup(); -}; - -Vector3 PhysicsServerSW::body_get_applied_torque(RID p_body) const { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body, Vector3()); - - return body->get_applied_torque(); -}; - -void PhysicsServerSW::body_add_central_force(RID p_body, const Vector3 &p_force) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - body->add_central_force(p_force); - body->wakeup(); -} - -void PhysicsServerSW::body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_pos) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - body->add_force(p_force, p_pos); - body->wakeup(); -}; - -void PhysicsServerSW::body_add_torque(RID p_body, const Vector3 &p_torque) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - body->add_torque(p_torque); - body->wakeup(); -}; - -void PhysicsServerSW::body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - _update_shapes(); - - body->apply_central_impulse(p_impulse); - body->wakeup(); -} - -void PhysicsServerSW::body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - _update_shapes(); - - body->apply_impulse(p_pos, p_impulse); - body->wakeup(); -}; - -void PhysicsServerSW::body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - _update_shapes(); - - body->apply_torque_impulse(p_impulse); - body->wakeup(); -}; - -void PhysicsServerSW::body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) { - BodySW *body = body_owner.get(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 PhysicsServerSW::body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - body->set_axis_lock(p_axis, p_lock); - body->wakeup(); -} - -bool PhysicsServerSW::body_is_axis_locked(RID p_body, BodyAxis p_axis) const { - const BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body, 0); - return body->is_axis_locked(p_axis); -} - -void PhysicsServerSW::body_add_collision_exception(RID p_body, RID p_body_b) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - body->add_exception(p_body_b); - body->wakeup(); -}; - -void PhysicsServerSW::body_remove_collision_exception(RID p_body, RID p_body_b) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - body->remove_exception(p_body_b); - body->wakeup(); -}; - -void PhysicsServerSW::body_get_collision_exceptions(RID p_body, List *p_exceptions) { - BodySW *body = body_owner.get(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 PhysicsServerSW::body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); -}; - -real_t PhysicsServerSW::body_get_contacts_reported_depth_threshold(RID p_body) const { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body, 0); - return 0; -}; - -void PhysicsServerSW::body_set_omit_force_integration(RID p_body, bool p_omit) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - body->set_omit_force_integration(p_omit); -}; - -bool PhysicsServerSW::body_is_omitting_force_integration(RID p_body) const { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body, false); - return body->get_omit_force_integration(); -}; - -void PhysicsServerSW::body_set_max_contacts_reported(RID p_body, int p_contacts) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - body->set_max_contacts_reported(p_contacts); -} - -int PhysicsServerSW::body_get_max_contacts_reported(RID p_body) const { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body, -1); - return body->get_max_contacts_reported(); -} - -void PhysicsServerSW::body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - body->set_force_integration_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(0), p_method, p_udata); -} - -void PhysicsServerSW::body_set_ray_pickable(RID p_body, bool p_enable) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - body->set_ray_pickable(p_enable); -} - -bool PhysicsServerSW::body_is_ray_pickable(RID p_body) const { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body, false); - return body->is_ray_pickable(); -} - -bool PhysicsServerSW::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result, bool p_exclude_raycast_shapes, const RBSet &p_exclude) { - BodySW *body = body_owner.get(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_from, p_motion, p_infinite_inertia, body->get_kinematic_margin(), r_result, p_exclude_raycast_shapes, p_exclude); -} - -int PhysicsServerSW::body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin) { - BodySW *body = body_owner.get(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_ray_separation(body, p_transform, p_infinite_inertia, r_recover_motion, r_results, p_result_max, p_margin); -} - -PhysicsDirectBodyState *PhysicsServerSW::body_get_direct_state(RID p_body) { - if (!body_owner.owns(p_body)) { - return nullptr; - } - - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V_MSG(!body, nullptr, "Body with RID " + itos(p_body.get_id()) + " not owned by this server."); - - if (!body->get_space()) { - return nullptr; - } - - ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); - return body->get_direct_state(); -} - -/* JOINT API */ - -RID PhysicsServerSW::joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) { - BodySW *body_A = body_owner.get(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); - - if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); - p_body_B = body_A->get_space()->get_static_global_body(); - } - - BodySW *body_B = body_owner.get(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); - - ERR_FAIL_COND_V(body_A == body_B, RID()); - - JointSW *joint = memnew(PinJointSW(body_A, p_local_A, body_B, p_local_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; -} - -void PhysicsServerSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) { - JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_PIN); - PinJointSW *pin_joint = static_cast(joint); - pin_joint->set_param(p_param, p_value); -} -real_t PhysicsServerSW::pin_joint_get_param(RID p_joint, PinJointParam p_param) const { - JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, 0); - PinJointSW *pin_joint = static_cast(joint); - return pin_joint->get_param(p_param); -} - -void PhysicsServerSW::pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) { - JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_PIN); - PinJointSW *pin_joint = static_cast(joint); - pin_joint->set_pos_a(p_A); -} -Vector3 PhysicsServerSW::pin_joint_get_local_a(RID p_joint) const { - JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!joint, Vector3()); - ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3()); - PinJointSW *pin_joint = static_cast(joint); - return pin_joint->get_position_a(); -} - -void PhysicsServerSW::pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) { - JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_PIN); - PinJointSW *pin_joint = static_cast(joint); - pin_joint->set_pos_b(p_B); -} -Vector3 PhysicsServerSW::pin_joint_get_local_b(RID p_joint) const { - JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!joint, Vector3()); - ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3()); - PinJointSW *pin_joint = static_cast(joint); - return pin_joint->get_position_b(); -} - -RID PhysicsServerSW::joint_create_hinge(RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B) { - BodySW *body_A = body_owner.get(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); - - if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); - p_body_B = body_A->get_space()->get_static_global_body(); - } - - BodySW *body_B = body_owner.get(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); - - ERR_FAIL_COND_V(body_A == body_B, RID()); - - JointSW *joint = memnew(HingeJointSW(body_A, body_B, p_frame_A, p_frame_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; -} - -RID PhysicsServerSW::joint_create_hinge_simple(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) { - BodySW *body_A = body_owner.get(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); - - if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); - p_body_B = body_A->get_space()->get_static_global_body(); - } - - BodySW *body_B = body_owner.get(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); - - ERR_FAIL_COND_V(body_A == body_B, RID()); - - JointSW *joint = memnew(HingeJointSW(body_A, body_B, p_pivot_A, p_pivot_B, p_axis_A, p_axis_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; -} - -void PhysicsServerSW::hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) { - JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_HINGE); - HingeJointSW *hinge_joint = static_cast(joint); - hinge_joint->set_param(p_param, p_value); -} -real_t PhysicsServerSW::hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const { - JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, 0); - HingeJointSW *hinge_joint = static_cast(joint); - return hinge_joint->get_param(p_param); -} - -void PhysicsServerSW::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) { - JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_HINGE); - HingeJointSW *hinge_joint = static_cast(joint); - hinge_joint->set_flag(p_flag, p_value); -} -bool PhysicsServerSW::hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const { - JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!joint, false); - ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, false); - HingeJointSW *hinge_joint = static_cast(joint); - return hinge_joint->get_flag(p_flag); -} - -void PhysicsServerSW::joint_set_solver_priority(RID p_joint, int p_priority) { - JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND(!joint); - joint->set_priority(p_priority); -} - -int PhysicsServerSW::joint_get_solver_priority(RID p_joint) const { - JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!joint, 0); - return joint->get_priority(); -} - -void PhysicsServerSW::joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) { - JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND(!joint); - - joint->disable_collisions_between_bodies(p_disable); - - if (2 == joint->get_body_count()) { - BodySW *body_a = *joint->get_body_ptr(); - BodySW *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 PhysicsServerSW::joint_is_disabled_collisions_between_bodies(RID p_joint) const { - JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!joint, true); - - return joint->is_disabled_collisions_between_bodies(); -} - -PhysicsServerSW::JointType PhysicsServerSW::joint_get_type(RID p_joint) const { - JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!joint, JOINT_PIN); - return joint->get_type(); -} - -RID PhysicsServerSW::joint_create_slider(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { - BodySW *body_A = body_owner.get(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); - - if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); - p_body_B = body_A->get_space()->get_static_global_body(); - } - - BodySW *body_B = body_owner.get(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); - - ERR_FAIL_COND_V(body_A == body_B, RID()); - - JointSW *joint = memnew(SliderJointSW(body_A, body_B, p_local_frame_A, p_local_frame_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; -} - -void PhysicsServerSW::slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) { - JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_SLIDER); - SliderJointSW *slider_joint = static_cast(joint); - slider_joint->set_param(p_param, p_value); -} -real_t PhysicsServerSW::slider_joint_get_param(RID p_joint, SliderJointParam p_param) const { - JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_CONE_TWIST, 0); - SliderJointSW *slider_joint = static_cast(joint); - return slider_joint->get_param(p_param); -} - -RID PhysicsServerSW::joint_create_cone_twist(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { - BodySW *body_A = body_owner.get(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); - - if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); - p_body_B = body_A->get_space()->get_static_global_body(); - } - - BodySW *body_B = body_owner.get(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); - - ERR_FAIL_COND_V(body_A == body_B, RID()); - - JointSW *joint = memnew(ConeTwistJointSW(body_A, body_B, p_local_frame_A, p_local_frame_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; -} - -void PhysicsServerSW::cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) { - JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_CONE_TWIST); - ConeTwistJointSW *cone_twist_joint = static_cast(joint); - cone_twist_joint->set_param(p_param, p_value); -} -real_t PhysicsServerSW::cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const { - JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_CONE_TWIST, 0); - ConeTwistJointSW *cone_twist_joint = static_cast(joint); - return cone_twist_joint->get_param(p_param); -} - -RID PhysicsServerSW::joint_create_generic_6dof(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { - BodySW *body_A = body_owner.get(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); - - if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); - p_body_B = body_A->get_space()->get_static_global_body(); - } - - BodySW *body_B = body_owner.get(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); - - ERR_FAIL_COND_V(body_A == body_B, RID()); - - JointSW *joint = memnew(Generic6DOFJointSW(body_A, body_B, p_local_frame_A, p_local_frame_B, true)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; -} - -void PhysicsServerSW::generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param, real_t p_value) { - JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_6DOF); - Generic6DOFJointSW *generic_6dof_joint = static_cast(joint); - generic_6dof_joint->set_param(p_axis, p_param, p_value); -} -real_t PhysicsServerSW::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param) { - JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, 0); - Generic6DOFJointSW *generic_6dof_joint = static_cast(joint); - return generic_6dof_joint->get_param(p_axis, p_param); -} - -void PhysicsServerSW::generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag, bool p_enable) { - JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_6DOF); - Generic6DOFJointSW *generic_6dof_joint = static_cast(joint); - generic_6dof_joint->set_flag(p_axis, p_flag, p_enable); -} -bool PhysicsServerSW::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag) { - JointSW *joint = joint_owner.get(p_joint); - ERR_FAIL_COND_V(!joint, false); - ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, false); - Generic6DOFJointSW *generic_6dof_joint = static_cast(joint); - return generic_6dof_joint->get_flag(p_axis, p_flag); -} - -void PhysicsServerSW::free(RID p_rid) { - if (!p_rid.is_valid()) { - ERR_FAIL_MSG("Invalid RID."); - return; - } - - _update_shapes(); //just in case - - if (shape_owner.owns(p_rid)) { - ShapeSW *shape = shape_owner.get(p_rid); - - while (shape->get_owners().size()) { - ShapeOwnerSW *so = shape->get_owners().front()->key(); - so->remove_shape(shape); - } - - shape_owner.free(p_rid); - memdelete(shape); - } else if (body_owner.owns(p_rid)) { - BodySW *body = body_owner.get(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 (area_owner.owns(p_rid)) { - AreaSW *area = area_owner.get(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)) { - SpaceSW *space = space_owner.get(p_rid); - - while (space->get_objects().size()) { - CollisionObjectSW *co = (CollisionObjectSW *)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)) { - JointSW *joint = joint_owner.get(p_rid); - - for (int i = 0; i < joint->get_body_count(); i++) { - joint->get_body_ptr()[i]->remove_constraint(joint); - } - joint_owner.free(p_rid); - memdelete(joint); - - } else { - ERR_FAIL_MSG("Invalid RID."); - } -}; - -void PhysicsServerSW::set_active(bool p_active) { - active = p_active; -}; - -void PhysicsServerSW::set_collision_iterations(int p_iterations) { - iterations = p_iterations; -}; - -void PhysicsServerSW::init() { - iterations = 8; // 8? - stepper = memnew(StepSW); -}; - -void PhysicsServerSW::step(real_t p_step) { -#ifndef _3D_DISABLED - - if (!active) { - return; - } - - _update_shapes(); - - island_count = 0; - active_objects = 0; - collision_pairs = 0; - for (RBSet::Element *E = active_spaces.front(); E; E = E->next()) { - stepper->step((SpaceSW *)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 PhysicsServerSW::flush_queries() { -#ifndef _3D_DISABLED - - if (!active) { - return; - } - - flushing_queries = true; - - uint64_t time_beg = OS::get_singleton()->get_ticks_usec(); - - for (RBSet::Element *E = active_spaces.front(); E; E = E->next()) { - SpaceSW *space = (SpaceSW *)E->get(); - space->call_queries(); - } - - flushing_queries = false; - - if (ScriptDebugger::get_singleton() && ScriptDebugger::get_singleton()->is_profiling()) { - uint64_t total_time[SpaceSW::ELAPSED_TIME_MAX]; - static const char *time_name[SpaceSW::ELAPSED_TIME_MAX] = { - "integrate_forces", - "generate_islands", - "setup_constraints", - "solve_constraints", - "integrate_velocities" - }; - - for (int i = 0; i < SpaceSW::ELAPSED_TIME_MAX; i++) { - total_time[i] = 0; - } - - for (RBSet::Element *E = active_spaces.front(); E; E = E->next()) { - for (int i = 0; i < SpaceSW::ELAPSED_TIME_MAX; i++) { - total_time[i] += E->get()->get_elapsed_time(SpaceSW::ElapsedTime(i)); - } - } - - Array values; - values.resize(SpaceSW::ELAPSED_TIME_MAX * 2); - for (int i = 0; i < SpaceSW::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)); - - ScriptDebugger::get_singleton()->add_profiling_frame_data("physics_3d", values); - } -#endif -}; - -void PhysicsServerSW::finish() { - memdelete(stepper); -}; - -int PhysicsServerSW::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 PhysicsServerSW::_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 PhysicsServerSW::_shape_col_cbk(const Vector3 &p_point_A, const Vector3 &p_point_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++; - } -} - -PhysicsServerSW *PhysicsServerSW::singleton = nullptr; -PhysicsServerSW::PhysicsServerSW() { - singleton = this; - - bool use_bvh_or_octree = GLOBAL_GET("physics/3d/pandemonium_physics/use_bvh"); - - if (use_bvh_or_octree) { - BroadPhaseSW::create_func = BroadPhaseBVH::_create; - } else { - BroadPhaseSW::create_func = BroadPhaseOctree::_create; - } - - island_count = 0; - active_objects = 0; - collision_pairs = 0; - - active = true; - flushing_queries = false; -}; - -PhysicsServerSW::~PhysicsServerSW(){ - -}; diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h deleted file mode 100644 index 20cf5ac..0000000 --- a/servers/physics/physics_server_sw.h +++ /dev/null @@ -1,372 +0,0 @@ -#ifndef PHYSICS_SERVER_SW -#define PHYSICS_SERVER_SW -/*************************************************************************/ -/* physics_server_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" -#include "servers/physics_server.h" -#include "shape_sw.h" -#include "space_sw.h" -#include "step_sw.h" - -class PhysicsServerSW : public PhysicsServer { - GDCLASS(PhysicsServerSW, PhysicsServer); - - friend class PhysicsDirectSpaceStateSW; - bool active; - int iterations; - - int island_count; - int active_objects; - int collision_pairs; - - bool flushing_queries; - - StepSW *stepper; - RBSet active_spaces; - - mutable RID_Owner shape_owner; - mutable RID_Owner space_owner; - mutable RID_Owner area_owner; - mutable RID_Owner body_owner; - mutable RID_Owner joint_owner; - - //void _clear_query(QuerySW *p_query); - friend class CollisionObjectSW; - SelfList::List pending_shape_update_list; - void _update_shapes(); - -public: - static PhysicsServerSW *singleton; - - struct CollCbkData { - int max; - int amount; - Vector3 *ptr; - }; - - static void _shape_col_cbk(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata); - - virtual RID shape_create(ShapeType p_shape); - virtual void shape_set_data(RID p_shape, const Variant &p_data); - virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias); - - virtual ShapeType shape_get_type(RID p_shape) const; - virtual Variant shape_get_data(RID p_shape) const; - - virtual void shape_set_margin(RID p_shape, real_t p_margin); - virtual real_t shape_get_margin(RID p_shape) const; - - virtual real_t shape_get_custom_solver_bias(RID p_shape) const; - - /* SPACE API */ - - virtual RID space_create(); - virtual void space_set_active(RID p_space, bool p_active); - virtual bool space_is_active(RID p_space) const; - - virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value); - virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const; - - // this function only works on physics process, errors and returns null otherwise - virtual PhysicsDirectSpaceState *space_get_direct_state(RID p_space); - - virtual void space_set_debug_contacts(RID p_space, int p_max_contacts); - virtual Vector space_get_contacts(RID p_space) const; - virtual int space_get_contact_count(RID p_space) const; - - /* AREA API */ - - virtual RID area_create(); - - virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode); - virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const; - - virtual void area_set_space(RID p_area, RID p_space); - virtual RID area_get_space(RID p_area) const; - - virtual void area_add_shape(RID p_area, RID p_shape, const Transform &p_transform = Transform(), bool p_disabled = false); - virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape); - virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform); - - virtual int area_get_shape_count(RID p_area) const; - virtual RID area_get_shape(RID p_area, int p_shape_idx) const; - virtual Transform area_get_shape_transform(RID p_area, int p_shape_idx) const; - - virtual void area_remove_shape(RID p_area, int p_shape_idx); - virtual void area_clear_shapes(RID p_area); - - virtual void area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled); - - virtual void area_attach_object_instance_id(RID p_area, ObjectID p_id); - virtual ObjectID area_get_object_instance_id(RID p_area) const; - - virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value); - virtual void area_set_transform(RID p_area, const Transform &p_transform); - - virtual Variant area_get_param(RID p_area, AreaParameter p_param) const; - virtual Transform area_get_transform(RID p_area) const; - - virtual void area_set_ray_pickable(RID p_area, bool p_enable); - virtual bool area_is_ray_pickable(RID p_area) const; - - virtual void area_set_collision_mask(RID p_area, uint32_t p_mask); - virtual void area_set_collision_layer(RID p_area, uint32_t p_layer); - - virtual void area_set_monitorable(RID p_area, bool p_monitorable); - - virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method); - virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method); - - /* BODY API */ - - // create a body of a given type - virtual RID body_create(BodyMode p_mode = BODY_MODE_RIGID, bool p_init_sleeping = false); - - virtual void body_set_space(RID p_body, RID p_space); - virtual RID body_get_space(RID p_body) const; - - virtual void body_set_mode(RID p_body, BodyMode p_mode); - virtual BodyMode body_get_mode(RID p_body) const; - - virtual void body_add_shape(RID p_body, RID p_shape, const Transform &p_transform = Transform(), bool p_disabled = false); - virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape); - virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform); - - virtual int body_get_shape_count(RID p_body) const; - virtual RID body_get_shape(RID p_body, int p_shape_idx) const; - virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const; - - virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled); - - virtual void body_remove_shape(RID p_body, int p_shape_idx); - virtual void body_clear_shapes(RID p_body); - - virtual void body_attach_object_instance_id(RID p_body, uint32_t p_id); - virtual uint32_t body_get_object_instance_id(RID p_body) const; - - virtual void body_set_enable_continuous_collision_detection(RID p_body, bool p_enable); - virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const; - - virtual void body_set_collision_layer(RID p_body, uint32_t p_layer); - virtual uint32_t body_get_collision_layer(RID p_body) const; - - virtual void body_set_collision_mask(RID p_body, uint32_t p_mask); - virtual uint32_t body_get_collision_mask(RID p_body) const; - - virtual void body_set_user_flags(RID p_body, uint32_t p_flags); - virtual uint32_t body_get_user_flags(RID p_body) const; - - virtual void body_set_param(RID p_body, BodyParameter p_param, real_t p_value); - virtual real_t body_get_param(RID p_body, BodyParameter p_param) const; - - virtual void body_set_kinematic_safe_margin(RID p_body, real_t p_margin); - virtual real_t body_get_kinematic_safe_margin(RID p_body) const; - - virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant); - virtual Variant body_get_state(RID p_body, BodyState p_state) const; - - virtual void body_set_applied_force(RID p_body, const Vector3 &p_force); - virtual Vector3 body_get_applied_force(RID p_body) const; - - virtual void body_set_applied_torque(RID p_body, const Vector3 &p_torque); - virtual Vector3 body_get_applied_torque(RID p_body) const; - - virtual void body_add_central_force(RID p_body, const Vector3 &p_force); - virtual void body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_pos); - virtual void body_add_torque(RID p_body, const Vector3 &p_torque); - - virtual void body_apply_central_impulse(RID p_body, const Vector3 &p_impulse); - virtual void body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse); - virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse); - virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity); - - virtual void body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock); - virtual bool body_is_axis_locked(RID p_body, BodyAxis p_axis) const; - - virtual void body_add_collision_exception(RID p_body, RID p_body_b); - virtual void body_remove_collision_exception(RID p_body, RID p_body_b); - virtual void body_get_collision_exceptions(RID p_body, List *p_exceptions); - - virtual void body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold); - virtual real_t body_get_contacts_reported_depth_threshold(RID p_body) const; - - virtual void body_set_omit_force_integration(RID p_body, bool p_omit); - virtual bool body_is_omitting_force_integration(RID p_body) const; - - virtual void body_set_max_contacts_reported(RID p_body, int p_contacts); - virtual int body_get_max_contacts_reported(RID p_body) const; - - virtual void body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant()); - - virtual void body_set_ray_pickable(RID p_body, bool p_enable); - virtual bool body_is_ray_pickable(RID p_body) const; - - virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true, const RBSet &p_exclude = RBSet()); - virtual int body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin = 0.001); - - // this function only works on physics process, errors and returns null otherwise - virtual PhysicsDirectBodyState *body_get_direct_state(RID p_body); - - /* SOFT BODY */ - - virtual RID soft_body_create(bool p_init_sleeping = false) { return RID(); } - - virtual void soft_body_update_rendering_server(RID p_body, class SoftBodyRenderingServerHandler *p_rendering_server_handler) {} - - virtual void soft_body_set_space(RID p_body, RID p_space) {} - virtual RID soft_body_get_space(RID p_body) const { return RID(); } - - virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer) {} - virtual uint32_t soft_body_get_collision_layer(RID p_body) const { return 0; } - - virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask) {} - virtual uint32_t soft_body_get_collision_mask(RID p_body) const { return 0; } - - virtual void soft_body_add_collision_exception(RID p_body, RID p_body_b) {} - virtual void soft_body_remove_collision_exception(RID p_body, RID p_body_b) {} - virtual void soft_body_get_collision_exceptions(RID p_body, List *p_exceptions) {} - - virtual void soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {} - virtual Variant soft_body_get_state(RID p_body, BodyState p_state) const { return Variant(); } - - virtual void soft_body_set_transform(RID p_body, const Transform &p_transform) {} - virtual Vector3 soft_body_get_vertex_position(RID p_body, int vertex_index) const { return Vector3(); } - - virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) {} - virtual bool soft_body_is_ray_pickable(RID p_body) const { return false; } - - virtual void soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) {} - virtual int soft_body_get_simulation_precision(RID p_body) { return 0; } - - virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass) {} - virtual real_t soft_body_get_total_mass(RID p_body) { return 0.; } - - virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) {} - virtual real_t soft_body_get_linear_stiffness(RID p_body) { return 0.; } - - virtual void soft_body_set_areaAngular_stiffness(RID p_body, real_t p_stiffness) {} - virtual real_t soft_body_get_areaAngular_stiffness(RID p_body) { return 0.; } - - virtual void soft_body_set_volume_stiffness(RID p_body, real_t p_stiffness) {} - virtual real_t soft_body_get_volume_stiffness(RID p_body) { return 0.; } - - virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) {} - virtual real_t soft_body_get_pressure_coefficient(RID p_body) { return 0.; } - - virtual void soft_body_set_pose_matching_coefficient(RID p_body, real_t p_pose_matching_coefficient) {} - virtual real_t soft_body_get_pose_matching_coefficient(RID p_body) { return 0.; } - - virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) {} - virtual real_t soft_body_get_damping_coefficient(RID p_body) { return 0.; } - - virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) {} - virtual real_t soft_body_get_drag_coefficient(RID p_body) { return 0.; } - - virtual void soft_body_set_mesh(RID p_body, const REF &p_mesh) {} - - virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) {} - virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) { return Vector3(); } - - virtual Vector3 soft_body_get_point_offset(RID p_body, int p_point_index) const { return Vector3(); } - - virtual void soft_body_remove_all_pinned_points(RID p_body) {} - virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) {} - virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) { return false; } - - /* JOINT API */ - - virtual RID joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B); - - virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value); - virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const; - - virtual void pin_joint_set_local_a(RID p_joint, const Vector3 &p_A); - virtual Vector3 pin_joint_get_local_a(RID p_joint) const; - - virtual void pin_joint_set_local_b(RID p_joint, const Vector3 &p_B); - virtual Vector3 pin_joint_get_local_b(RID p_joint) const; - - virtual RID joint_create_hinge(RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B); - virtual RID joint_create_hinge_simple(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); - - virtual void hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value); - virtual real_t hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const; - - virtual void hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value); - virtual bool hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const; - - virtual RID joint_create_slider(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B); //reference frame is A - - virtual void slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value); - virtual real_t slider_joint_get_param(RID p_joint, SliderJointParam p_param) const; - - virtual RID joint_create_cone_twist(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B); //reference frame is A - - virtual void cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value); - virtual real_t cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const; - - virtual RID joint_create_generic_6dof(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B); //reference frame is A - - virtual void generic_6dof_joint_set_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param, real_t p_value); - virtual real_t generic_6dof_joint_get_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param); - - virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag, bool p_enable); - virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag); - - virtual JointType joint_get_type(RID p_joint) const; - - virtual void joint_set_solver_priority(RID p_joint, int p_priority); - virtual int joint_get_solver_priority(RID p_joint) const; - - virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable); - virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const; - - /* MISC */ - - virtual void free(RID p_rid); - - virtual void set_active(bool p_active); - virtual void init(); - virtual void step(real_t p_step); - virtual void flush_queries(); - virtual void finish(); - - virtual void set_collision_iterations(int p_iterations); - - virtual bool is_flushing_queries() const { return flushing_queries; } - - int get_process_info(ProcessInfo p_info); - - PhysicsServerSW(); - ~PhysicsServerSW(); -}; - -#endif diff --git a/servers/physics/shape_sw.cpp b/servers/physics/shape_sw.cpp deleted file mode 100644 index ebfcd28..0000000 --- a/servers/physics/shape_sw.cpp +++ /dev/null @@ -1,2210 +0,0 @@ -/*************************************************************************/ -/* shape_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" - -#include "core/io/image.h" -#include "core/math/convex_hull.h" -#include "core/math/geometry.h" -#include "core/containers/sort_array.h" - -// HeightMapShapeSW 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 ShapeSW::configure(const AABB &p_aabb) { - aabb = p_aabb; - configured = true; - for (RBMap::Element *E = owners.front(); E; E = E->next()) { - ShapeOwnerSW *co = (ShapeOwnerSW *)E->key(); - co->_shape_changed(); - } -} - -Vector3 ShapeSW::get_support(const Vector3 &p_normal) const { - Vector3 res; - int amnt; - FeatureType type; - get_supports(p_normal, 1, &res, amnt, type); - return res; -} - -void ShapeSW::add_owner(ShapeOwnerSW *p_owner) { - RBMap::Element *E = owners.find(p_owner); - if (E) { - E->get()++; - } else { - owners[p_owner] = 1; - } -} - -void ShapeSW::remove_owner(ShapeOwnerSW *p_owner) { - RBMap::Element *E = owners.find(p_owner); - ERR_FAIL_COND(!E); - E->get()--; - if (E->get() == 0) { - owners.erase(E); - } -} - -bool ShapeSW::is_owner(ShapeOwnerSW *p_owner) const { - return owners.has(p_owner); -} - -const RBMap &ShapeSW::get_owners() const { - return owners; -} - -ShapeSW::ShapeSW() { - custom_bias = 0; - configured = false; -} - -ShapeSW::~ShapeSW() { - ERR_FAIL_COND(owners.size()); -} - -Plane PlaneShapeSW::get_plane() const { - return plane; -} - -void PlaneShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - // gibberish, a plane is infinity - r_min = -1e7; - r_max = 1e7; -} - -Vector3 PlaneShapeSW::get_support(const Vector3 &p_normal) const { - return p_normal * 1e15; -} - -bool PlaneShapeSW::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 PlaneShapeSW::intersect_point(const Vector3 &p_point) const { - return plane.distance_to(p_point) < 0; -} - -Vector3 PlaneShapeSW::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 PlaneShapeSW::get_moment_of_inertia(real_t p_mass) const { - return Vector3(); //wtf -} - -void PlaneShapeSW::_setup(const Plane &p_plane) { - plane = p_plane; - configure(AABB(Vector3(-1e4, -1e4, -1e4), Vector3(1e4 * 2, 1e4 * 2, 1e4 * 2))); -} - -void PlaneShapeSW::set_data(const Variant &p_data) { - _setup(p_data); -} - -Variant PlaneShapeSW::get_data() const { - return plane; -} - -PlaneShapeSW::PlaneShapeSW() { -} - -// - -real_t RayShapeSW::get_length() const { - return length; -} - -bool RayShapeSW::get_slips_on_slope() const { - return slips_on_slope; -} - -void RayShapeSW::project_range(const Vector3 &p_normal, const Transform &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 RayShapeSW::get_support(const Vector3 &p_normal) const { - if (p_normal.z > 0) { - return Vector3(0, 0, length); - } else { - return Vector3(0, 0, 0); - } -} - -void RayShapeSW::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 RayShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - return false; //simply not possible -} - -bool RayShapeSW::intersect_point(const Vector3 &p_point) const { - return false; //simply not possible -} - -Vector3 RayShapeSW::get_closest_point_to(const Vector3 &p_point) const { - Vector3 s[2] = { - Vector3(0, 0, 0), - Vector3(0, 0, length) - }; - - return Geometry::get_closest_point_to_segment(p_point, s); -} - -Vector3 RayShapeSW::get_moment_of_inertia(real_t p_mass) const { - return Vector3(); -} - -void RayShapeSW::_setup(real_t p_length, bool p_slips_on_slope) { - length = p_length; - slips_on_slope = p_slips_on_slope; - configure(AABB(Vector3(0, 0, 0), Vector3(0.1, 0.1, length))); -} - -void RayShapeSW::set_data(const Variant &p_data) { - Dictionary d = p_data; - _setup(d["length"], d["slips_on_slope"]); -} - -Variant RayShapeSW::get_data() const { - Dictionary d; - d["length"] = length; - d["slips_on_slope"] = slips_on_slope; - return d; -} - -RayShapeSW::RayShapeSW() { - length = 1; - slips_on_slope = false; -} - -/********** SPHERE *************/ - -real_t SphereShapeSW::get_radius() const { - return radius; -} - -void SphereShapeSW::project_range(const Vector3 &p_normal, const Transform &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 SphereShapeSW::get_support(const Vector3 &p_normal) const { - return p_normal * radius; -} - -void SphereShapeSW::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 SphereShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - return Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal); -} - -bool SphereShapeSW::intersect_point(const Vector3 &p_point) const { - return p_point.length() < radius; -} - -Vector3 SphereShapeSW::get_closest_point_to(const Vector3 &p_point) const { - Vector3 p = p_point; - float l = p.length(); - if (l < radius) { - return p_point; - } - return (p / l) * radius; -} - -Vector3 SphereShapeSW::get_moment_of_inertia(real_t p_mass) const { - real_t s = 0.4 * p_mass * radius * radius; - return Vector3(s, s, s); -} - -void SphereShapeSW::_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 SphereShapeSW::set_data(const Variant &p_data) { - _setup(p_data); -} - -Variant SphereShapeSW::get_data() const { - return radius; -} - -SphereShapeSW::SphereShapeSW() { - radius = 0; -} - -/********** BOX *************/ - -void BoxShapeSW::project_range(const Vector3 &p_normal, const Transform &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 BoxShapeSW::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 BoxShapeSW::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 BoxShapeSW::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 BoxShapeSW::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 BoxShapeSW::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 - float 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 = Geometry::get_closest_point_to_segment(p_point, s); - - float d = p_point.distance_to(closest_edge); - if (d < min_distance) { - min_point = closest_edge; - min_distance = d; - } - } - - return min_point; -} - -Vector3 BoxShapeSW::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 BoxShapeSW::_setup(const Vector3 &p_half_extents) { - half_extents = p_half_extents.abs(); - - configure(AABB(-half_extents, half_extents * 2)); -} - -void BoxShapeSW::set_data(const Variant &p_data) { - _setup(p_data); -} - -Variant BoxShapeSW::get_data() const { - return half_extents; -} - -BoxShapeSW::BoxShapeSW() { -} - -/********** CAPSULE *************/ - -void CapsuleShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - Vector3 n = p_transform.basis.xform_inv(p_normal).normalized(); - real_t h = (n.z > 0) ? height : -height; - - n *= radius; - n.z += h * 0.5; - - r_max = p_normal.dot(p_transform.xform(n)); - r_min = p_normal.dot(p_transform.xform(-n)); -} - -Vector3 CapsuleShapeSW::get_support(const Vector3 &p_normal) const { - Vector3 n = p_normal; - - real_t h = (n.z > 0) ? height : -height; - - n *= radius; - n.z += h * 0.5; - return n; -} - -void CapsuleShapeSW::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.z; - - if (Math::abs(d) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { - // make it flat - n.z = 0.0; - n.normalize(); - n *= radius; - - r_amount = 2; - r_type = FEATURE_EDGE; - r_supports[0] = n; - r_supports[0].z += height * 0.5; - r_supports[1] = n; - r_supports[1].z -= height * 0.5; - - } else { - real_t h = (d > 0) ? height : -height; - - n *= radius; - n.z += h * 0.5; - r_amount = 1; - r_type = FEATURE_POINT; - *r_supports = n; - } -} - -bool CapsuleShapeSW::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 = Geometry::segment_intersects_cylinder(p_begin, p_end, height, 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 = Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(0, 0, height * 0.5), 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 = Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(0, 0, height * -0.5), 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 CapsuleShapeSW::intersect_point(const Vector3 &p_point) const { - if (Math::abs(p_point.z) < height * 0.5) { - return Vector3(p_point.x, p_point.y, 0).length() < radius; - } else { - Vector3 p = p_point; - p.z = Math::abs(p.z) - height * 0.5; - return p.length() < radius; - } -} - -Vector3 CapsuleShapeSW::get_closest_point_to(const Vector3 &p_point) const { - Vector3 s[2] = { - Vector3(0, 0, -height * 0.5), - Vector3(0, 0, height * 0.5), - }; - - Vector3 p = Geometry::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 CapsuleShapeSW::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 CapsuleShapeSW::_setup(real_t p_height, real_t p_radius) { - height = p_height; - radius = p_radius; - configure(AABB(Vector3(-radius, -radius, -height * 0.5 - radius), Vector3(radius * 2, radius * 2, height + radius * 2.0))); -} - -void CapsuleShapeSW::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 CapsuleShapeSW::get_data() const { - Dictionary d; - d["radius"] = radius; - d["height"] = height; - return d; -} - -CapsuleShapeSW::CapsuleShapeSW() { - height = radius = 0; -} - -/********** CYLINDER *************/ - -void CylinderShapeSW::project_range(const Vector3 &p_normal, const Transform &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 CylinderShapeSW::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 CylinderShapeSW::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 CylinderShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - return Geometry::segment_intersects_cylinder(p_begin, p_end, height, radius, &r_result, &r_normal, 1); -} - -bool CylinderShapeSW::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 CylinderShapeSW::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(circle_pos, Vector3(0.0, dir, 0.0)); - 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 = Geometry::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 CylinderShapeSW::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 CylinderShapeSW::_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 CylinderShapeSW::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 CylinderShapeSW::get_data() const { - Dictionary d; - d["radius"] = radius; - d["height"] = height; - return d; -} - -CylinderShapeSW::CylinderShapeSW() { - height = radius = 0; -} - -/********** CONVEX POLYGON *************/ - -void ConvexPolygonShapeSW::project_range(const Vector3 &p_normal, const Transform &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 ConvexPolygonShapeSW::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 ConvexPolygonShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); - int fc = mesh.faces.size(); - - const Geometry::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 ConvexPolygonShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - const Geometry::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 ConvexPolygonShapeSW::intersect_point(const Vector3 &p_point) const { - const Geometry::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 ConvexPolygonShapeSW::get_closest_point_to(const Vector3 &p_point) const { - const Geometry::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(a, n).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; - } - - float min_distance = 1e20; - Vector3 min_point; - - //check edges - const Geometry::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 = Geometry::get_closest_point_to_segment(p_point, s); - float d = closest.distance_to(p_point); - if (d < min_distance) { - min_distance = d; - min_point = closest; - } - } - - return min_point; -} - -Vector3 ConvexPolygonShapeSW::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 ConvexPolygonShapeSW::_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 ConvexPolygonShapeSW::set_data(const Variant &p_data) { - _setup(p_data); -} - -Variant ConvexPolygonShapeSW::get_data() const { - return mesh.vertices; -} - -ConvexPolygonShapeSW::ConvexPolygonShapeSW() { -} - -/********** FACE POLYGON *************/ - -void FaceShapeSW::project_range(const Vector3 &p_normal, const Transform &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 FaceShapeSW::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 FaceShapeSW::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 FaceShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - bool c = Geometry::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) { - r_normal = -r_normal; - } - } - - return c; -} - -bool FaceShapeSW::intersect_point(const Vector3 &p_point) const { - return false; //face is flat -} - -Vector3 FaceShapeSW::get_closest_point_to(const Vector3 &p_point) const { - return Face3(vertex[0], vertex[1], vertex[2]).get_closest_point_to(p_point); -} - -Vector3 FaceShapeSW::get_moment_of_inertia(real_t p_mass) const { - return Vector3(); // Sorry, but i don't think anyone cares, FaceShape! -} - -FaceShapeSW::FaceShapeSW() { - configure(AABB()); -} - -PoolVector ConcavePolygonShapeSW::get_faces() const { - PoolVector 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 ConcavePolygonShapeSW::project_range(const Vector3 &p_normal, const Transform &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; - } - PoolVector::Read r = vertices.read(); - const Vector3 *vptr = r.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 ConcavePolygonShapeSW::get_support(const Vector3 &p_normal) const { - int count = vertices.size(); - if (count == 0) { - return Vector3(); - } - - PoolVector::Read r = vertices.read(); - const Vector3 *vptr = r.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 ConcavePolygonShapeSW::_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) { - Vector3 res; - Vector3 vertices[3] = { - p_params->vertices[p_params->faces[bvh->face_index].indices[0]], - p_params->vertices[p_params->faces[bvh->face_index].indices[1]], - p_params->vertices[p_params->faces[bvh->face_index].indices[2]] - }; - - if (Geometry::segment_intersects_triangle( - p_params->from, - p_params->to, - vertices[0], - vertices[1], - vertices[2], - &res)) { - real_t d = p_params->dir.dot(res) - p_params->dir.dot(p_params->from); - //TODO, seems segmen/triangle intersection is broken :( - if (d > 0 && d < p_params->min_d) { - p_params->min_d = d; - p_params->result = res; - p_params->normal = Plane(vertices[0], vertices[1], vertices[2]).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 ConcavePolygonShapeSW::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 - PoolVector::Read fr = faces.read(); - PoolVector::Read vr = vertices.read(); - PoolVector::Read br = bvh.read(); - - _SegmentCullParams params; - params.from = p_begin; - params.to = p_end; - params.collisions = 0; - params.dir = (p_end - p_begin).normalized(); - - params.faces = fr.ptr(); - params.vertices = vr.ptr(); - params.bvh = br.ptr(); - - params.min_d = 1e20; - // cull - _cull_segment(0, ¶ms); - - if (params.collisions > 0) { - r_result = params.result; - r_normal = params.normal; - return true; - } else { - return false; - } -} - -bool ConcavePolygonShapeSW::intersect_point(const Vector3 &p_point) const { - return false; //face is flat -} - -Vector3 ConcavePolygonShapeSW::get_closest_point_to(const Vector3 &p_point) const { - return Vector3(); -} - -bool ConcavePolygonShapeSW::_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]; - FaceShapeSW *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 ConcavePolygonShapeSW::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 - PoolVector::Read fr = faces.read(); - PoolVector::Read vr = vertices.read(); - PoolVector::Read br = bvh.read(); - - FaceShapeSW face; // use this to send in the callback - - _CullParams params; - params.aabb = local_aabb; - params.face = &face; - params.faces = fr.ptr(); - params.vertices = vr.ptr(); - params.bvh = br.ptr(); - params.callback = p_callback; - params.userdata = p_userdata; - - // cull - _cull(0, ¶ms); -} - -Vector3 ConcavePolygonShapeSW::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 ConcavePolygonShapeSW::_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 ConcavePolygonShapeSW::_setup(PoolVector p_faces) { - 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; - - PoolVector::Read r = p_faces.read(); - const Vector3 *facesr = r.ptr(); - - PoolVector<_VolumeSW_BVH_Element> bvh_array; - bvh_array.resize(src_face_count); - - PoolVector<_VolumeSW_BVH_Element>::Write bvhw = bvh_array.write(); - _VolumeSW_BVH_Element *bvh_arrayw = bvhw.ptr(); - - faces.resize(src_face_count); - PoolVector::Write w = faces.write(); - Face *facesw = w.ptr(); - - vertices.resize(src_face_count * 3); - - PoolVector::Write vw = vertices.write(); - Vector3 *verticesw = vw.ptr(); - - 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.position + bvh_arrayw[i].aabb.size * 0.5; - 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); - } - } - - w.release(); - vw.release(); - - int count = 0; - _VolumeSW_BVH *bvh_tree = _volume_sw_build_bvh(bvh_arrayw, src_face_count, count); - - bvh.resize(count + 1); - - PoolVector::Write bvhw2 = bvh.write(); - BVH *bvh_arrayw2 = bvhw2.ptr(); - - int idx = 0; - _fill_bvh(bvh_tree, bvh_arrayw2, idx); - - configure(_aabb); // this type of shape has no margin -} - -void ConcavePolygonShapeSW::set_data(const Variant &p_data) { - _setup(p_data); -} - -Variant ConcavePolygonShapeSW::get_data() const { - return get_faces(); -} - -ConcavePolygonShapeSW::ConcavePolygonShapeSW() { -} - -/* HEIGHT MAP SHAPE */ - -PoolVector HeightMapShapeSW::get_heights() const { - return heights; -} - -int HeightMapShapeSW::get_width() const { - return width; -} - -int HeightMapShapeSW::get_depth() const { - return depth; -} - -void HeightMapShapeSW::project_range(const Vector3 &p_normal, const Transform &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, 0), r_min, r_max); -} - -Vector3 HeightMapShapeSW::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 HeightMapShapeSW *heightmap = nullptr; - FaceShapeSW *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 HeightMapShapeSW::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 *= HeightMapShapeSW::BOUNDS_CHUNK_SIZE; - exit_pos *= HeightMapShapeSW::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 HeightMapShapeSW::_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; - - FaceShapeSW face; - - _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 HeightMapShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const { - if (heights.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. - FaceShapeSW face; - - _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.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 HeightMapShapeSW::intersect_point(const Vector3 &p_point) const { - return false; -} - -Vector3 HeightMapShapeSW::get_closest_point_to(const Vector3 &p_point) const { - return Vector3(); -} - -void HeightMapShapeSW::_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.z + 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 HeightMapShapeSW::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const { - if (heights.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]); - - FaceShapeSW face; - - 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 HeightMapShapeSW::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 HeightMapShapeSW::_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) { - float 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 HeightMapShapeSW::_setup(const PoolVector &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 HeightMapShapeSW::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"]; - PoolVector heights_buffer; - if (heights_variant.get_type() == Variant::POOL_REAL_ARRAY) { - // 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); - - PoolByteArray im_data = image->get_data(); - heights_buffer.resize(image->get_width() * image->get_height()); - - PoolRealArray::Write w = heights_buffer.write(); - PoolByteArray::Read r = im_data.read(); - float *rp = (float *)r.ptr(); - for (int i = 0; i < heights_buffer.size(); ++i) { - w[i] = rp[i]; - } - } else { - ERR_FAIL_MSG("Expected PoolRealArray or float Image."); - } - - // 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 { - PoolVector::Read r = heights.read(); - int heights_size = heights.size(); - for (int i = 0; i < heights_size; ++i) { - real_t h = r[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 HeightMapShapeSW::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; -} - -HeightMapShapeSW::HeightMapShapeSW() { - width = 0; - depth = 0; -} diff --git a/servers/physics/shape_sw.h b/servers/physics/shape_sw.h deleted file mode 100644 index b8cbf47..0000000 --- a/servers/physics/shape_sw.h +++ /dev/null @@ -1,528 +0,0 @@ -#ifndef SHAPE_SW_H -#define SHAPE_SW_H -/*************************************************************************/ -/* shape_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "core/containers/local_vector.h" -#include "core/math/bsp_tree.h" -#include "core/math/geometry.h" -#include "servers/physics_server.h" -/* - -SHAPE_LINE, ///< plane:"plane" -SHAPE_SEGMENT, ///< real_t:"length" -SHAPE_CIRCLE, ///< real_t:"radius" -SHAPE_RECTANGLE, ///< vec3:"extents" -SHAPE_CONVEX_POLYGON, ///< array of planes:"planes" -SHAPE_CONCAVE_POLYGON, ///< Vector3 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector3 array) -SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error - -*/ - -class ShapeSW; - -class ShapeOwnerSW : public RID_Data { -public: - virtual void _shape_changed() = 0; - virtual void remove_shape(ShapeSW *p_shape) = 0; - - virtual ~ShapeOwnerSW() {} -}; - -class ShapeSW : public RID_Data { - RID self; - AABB aabb; - bool configured; - real_t custom_bias; - - RBMap 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_volume(); } - - _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } - _FORCE_INLINE_ RID get_self() const { return self; } - - virtual PhysicsServer::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 Transform &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(ShapeOwnerSW *p_owner); - void remove_owner(ShapeOwnerSW *p_owner); - bool is_owner(ShapeOwnerSW *p_owner) const; - const RBMap &get_owners() const; - - ShapeSW(); - virtual ~ShapeSW(); -}; - -class ConcaveShapeSW : public ShapeSW { -public: - // Returns true to stop the query. - typedef bool (*QueryCallback)(void *p_userdata, ShapeSW *p_convex); - - virtual bool is_concave() const { return true; } - 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 void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0; - - ConcaveShapeSW() {} -}; - -class PlaneShapeSW : public ShapeSW { - Plane plane; - - void _setup(const Plane &p_plane); - -public: - Plane get_plane() const; - - virtual real_t get_area() const { return Math_INF; } - virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_PLANE; } - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; - 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 { 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; - - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; - - PlaneShapeSW(); -}; - -class RayShapeSW : public ShapeSW { - real_t length; - bool slips_on_slope; - - void _setup(real_t p_length, bool p_slips_on_slope); - -public: - real_t get_length() const; - bool get_slips_on_slope() const; - - virtual real_t get_area() const { return 0.0; } - virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_RAY; } - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; - 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; - - 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; - - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; - - RayShapeSW(); -}; - -class SphereShapeSW : public ShapeSW { - real_t radius; - - void _setup(real_t p_radius); - -public: - real_t get_radius() const; - - virtual real_t get_area() const { return 4.0 / 3.0 * Math_PI * radius * radius * radius; } - - virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_SPHERE; } - - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; - 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; - 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; - - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; - - SphereShapeSW(); -}; - -class BoxShapeSW : public ShapeSW { - 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 { return 8 * half_extents.x * half_extents.y * half_extents.z; } - - virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_BOX; } - - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; - 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; - 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; - - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; - - BoxShapeSW(); -}; - -class CapsuleShapeSW : public ShapeSW { - real_t height; - real_t radius; - - 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 { return height * Math_PI * radius * radius; } - - virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CAPSULE; } - - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; - 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; - 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; - - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; - - CapsuleShapeSW(); -}; - -class CylinderShapeSW : public ShapeSW { - real_t height; - real_t radius; - - 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 { return 4.0 / 3.0 * Math_PI * radius * radius * radius + height * Math_PI * radius * radius; } - - virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CYLINDER; } - - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; - 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; - 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; - - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; - - CylinderShapeSW(); -}; - -struct ConvexPolygonShapeSW : public ShapeSW { - Geometry::MeshData mesh; - - void _setup(const Vector &p_vertices); - -public: - const Geometry::MeshData &get_mesh() const { return mesh; } - - virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONVEX_POLYGON; } - - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; - 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; - 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; - - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; - - ConvexPolygonShapeSW(); -}; - -struct _VolumeSW_BVH; -struct FaceShapeSW; - -struct ConcavePolygonShapeSW : public ConcaveShapeSW { - // always a trimesh - - struct Face { - Vector3 normal; - int indices[3]; - }; - - PoolVector faces; - PoolVector vertices; - - struct BVH { - AABB aabb; - int left; - int right; - - int face_index; - }; - - PoolVector bvh; - - struct _CullParams { - AABB aabb; - QueryCallback callback; - void *userdata; - const Face *faces; - const Vector3 *vertices; - const BVH *bvh; - FaceShapeSW *face; - }; - - struct _SegmentCullParams { - Vector3 from; - Vector3 to; - const Face *faces; - const Vector3 *vertices; - const BVH *bvh; - Vector3 dir; - - Vector3 result; - Vector3 normal; - real_t min_d; - int collisions; - }; - - 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(PoolVector p_faces); - -public: - PoolVector get_faces() const; - - virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; } - - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - - 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 void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const; - - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; - - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; - - ConcavePolygonShapeSW(); -}; - -struct HeightMapShapeSW : public ConcaveShapeSW { - friend struct _HeightmapSegmentCullParams; - - PoolVector heights; - int width; - int depth; - Vector3 local_origin; - - // Accelerator. - struct Range { - float min = 0.0; - float 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 PoolVector &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height); - -public: - PoolVector get_heights() const; - int get_width() const; - int get_depth() const; - - virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_HEIGHTMAP; } - - virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, 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 void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const; - - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; - - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; - - HeightMapShapeSW(); -}; - -//used internally -struct FaceShapeSW : public ShapeSW { - Vector3 normal; //cache - Vector3 vertex[3]; - - virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; } - - const Vector3 &get_vertex(int p_idx) const { return vertex[p_idx]; } - - void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; - 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; - 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; - - Vector3 get_moment_of_inertia(real_t p_mass) const; - - virtual void set_data(const Variant &p_data) {} - virtual Variant get_data() const { return Variant(); } - - FaceShapeSW(); -}; - -struct MotionShapeSW : public ShapeSW { - ShapeSW *shape; - Vector3 motion; - - virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONVEX_POLYGON; } - - void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - Vector3 cast = p_transform.basis.xform(motion); - real_t mina, maxa; - real_t minb, maxb; - Transform 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); - } - - Vector3 get_support(const Vector3 &p_normal) const { - 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 { r_amount = 0; } - bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { return false; } - virtual bool intersect_point(const Vector3 &p_point) const { return false; } - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const { return p_point; } - - 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(); } - - MotionShapeSW() { configure(AABB()); } -}; - -struct _ShapeTestConvexBSPSW { - const BSP_Tree *bsp; - const ShapeSW *shape; - Transform transform; - - _FORCE_INLINE_ void project_range(const Vector3 &p_normal, real_t &r_min, real_t &r_max) const { - shape->project_range(p_normal, transform, r_min, r_max); - } -}; - -#endif // SHAPESW_H diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp deleted file mode 100644 index 9247814..0000000 --- a/servers/physics/space_sw.cpp +++ /dev/null @@ -1,1328 +0,0 @@ -/*************************************************************************/ -/* space_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" - -#include "collision_solver_sw.h" -#include "core/config/project_settings.h" -#include "physics_server_sw.h" - -#define TEST_MOTION_MARGIN_MIN_VALUE 0.0001 -#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05 - -_FORCE_INLINE_ static bool _can_collide_with(CollisionObjectSW *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() == CollisionObjectSW::TYPE_AREA && !p_collide_with_areas) { - return false; - } - - if (p_object->get_type() == CollisionObjectSW::TYPE_BODY && !p_collide_with_bodies) { - return false; - } - - return true; -} - -int PhysicsDirectSpaceStateSW::intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const RBSet &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, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - int cc = 0; - - //Transform 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 CollisionObjectSW *col_obj = space->intersection_query_results[i]; - int shape_idx = space->intersection_query_subindex_results[i]; - - Transform 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 != 0) { - 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 PhysicsDirectSpaceStateSW::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const RBSet &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, SpaceSW::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 CollisionObjectSW *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 CollisionObjectSW *col_obj = space->intersection_query_results[i]; - - int shape_idx = space->intersection_query_subindex_results[i]; - Transform 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 ShapeSW *shape = col_obj->get_shape(shape_idx); - - Vector3 shape_point, shape_normal; - - if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) { - Transform 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 != 0) { - 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 PhysicsDirectSpaceStateSW::intersect_shape(const RID &p_shape, const Transform &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const RBSet &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - if (p_result_max <= 0) { - return 0; - } - - ShapeSW *shape = static_cast(PhysicsServer::get_singleton())->shape_owner.get(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, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - int cc = 0; - - //Transform 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 CollisionObjectSW *col_obj = space->intersection_query_results[i]; - int shape_idx = space->intersection_query_subindex_results[i]; - - if (!CollisionSolverSW::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 != 0) { - 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 PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const RBSet &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, ShapeRestInfo *r_info) { - ShapeSW *shape = static_cast(PhysicsServer::get_singleton())->shape_owner.get(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, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - real_t best_safe = 1; - real_t best_unsafe = 1; - - Transform xform_inv = p_xform.affine_inverse(); - MotionShapeSW 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 CollisionObjectSW *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; - - Transform 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 (CollisionSolverSW::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 (!CollisionSolverSW::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 = !CollisionSolverSW::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() == CollisionObjectSW::TYPE_BODY) { - const BodySW *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 PhysicsDirectSpaceStateSW::collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const RBSet &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - if (p_result_max <= 0) { - return false; - } - - ShapeSW *shape = static_cast(PhysicsServer::get_singleton())->shape_owner.get(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, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - bool collided = false; - r_result_count = 0; - - PhysicsServerSW::CollCbkData cbk; - cbk.max = p_result_max; - cbk.amount = 0; - cbk.ptr = r_results; - CollisionSolverSW::CallbackResult cbkres = PhysicsServerSW::_shape_col_cbk; - - PhysicsServerSW::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 CollisionObjectSW *col_obj = space->intersection_query_results[i]; - int shape_idx = space->intersection_query_subindex_results[i]; - - if (p_exclude.has(col_obj->get_self())) { - continue; - } - - if (CollisionSolverSW::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 _RestCallbackData { - const CollisionObjectSW *object; - const CollisionObjectSW *best_object; - int local_shape; - int best_local_shape; - int shape; - int best_shape; - Vector3 best_contact; - Vector3 best_normal; - real_t best_len; - real_t min_allowed_depth; -}; - -static void _rest_cbk_result(const Vector3 &p_point_A, const Vector3 &p_point_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; - } - if (len <= rd->best_len) { - return; - } - - rd->best_len = len; - rd->best_contact = p_point_B; - rd->best_normal = contact_rel / len; - rd->best_object = rd->object; - rd->best_shape = rd->shape; - rd->best_local_shape = rd->local_shape; -} -bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const RBSet &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - ShapeSW *shape = static_cast(PhysicsServer::get_singleton())->shape_owner.get(p_shape); - ERR_FAIL_COND_V(!shape, 0); - - real_t margin = MAX(p_margin, TEST_MOTION_MARGIN_MIN_VALUE); - - real_t min_contact_depth = margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; - - AABB aabb = p_shape_xform.xform(shape->get_aabb()); - aabb = aabb.grow(margin); - - int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - - _RestCallbackData 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 CollisionObjectSW *col_obj = space->intersection_query_results[i]; - int shape_idx = space->intersection_query_subindex_results[i]; - - if (p_exclude.has(col_obj->get_self())) { - continue; - } - - rcd.object = col_obj; - rcd.shape = shape_idx; - bool sc = CollisionSolverSW::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, 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() == CollisionObjectSW::TYPE_BODY) { - const BodySW *body = static_cast(rcd.best_object); - Vector3 rel_vec = rcd.best_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 PhysicsDirectSpaceStateSW::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const { - CollisionObjectSW *obj = PhysicsServerSW::singleton->area_owner.getornull(p_object); - if (!obj) { - obj = PhysicsServerSW::singleton->body_owner.getornull(p_object); - } - ERR_FAIL_COND_V(!obj, Vector3()); - - ERR_FAIL_COND_V(obj->get_space() != space, Vector3()); - - float 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; - } - - Transform shape_xform = obj->get_transform() * obj->get_shape_transform(i); - ShapeSW *shape = obj->get_shape(i); - - Vector3 point = shape->get_closest_point_to(shape_xform.affine_inverse().xform(p_point)); - point = shape_xform.xform(point); - - float 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; - } -} - -PhysicsDirectSpaceStateSW::PhysicsDirectSpaceStateSW() { - space = nullptr; -} - -//////////////////////////////////////////////////////////////////////////////////////////////////////////// - -int SpaceSW::_cull_aabb_for_body(BodySW *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() == CollisionObjectSW::TYPE_AREA) { - keep = false; - } else if ((static_cast(intersection_query_results[i])->test_collision_mask(p_body)) == 0) { - 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; -} - -int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer::SeparationResult *r_results, int p_result_max, real_t p_margin) { - 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) { - return 0; - } - // Undo the currently transform the physics server is aware of and apply the provided one - body_aabb = p_transform.xform(p_body->get_inv_transform().xform(body_aabb)); - body_aabb = body_aabb.grow(p_margin); - - Transform body_transform = p_transform; - - for (int i = 0; i < p_result_max; i++) { - //reset results - r_results[i].collision_depth = -1.0; - } - - int rays_found = 0; - - { - // raycast AND separate - - const int max_results = 32; - int recover_attempts = 4; - Vector3 sr[max_results * 2]; - PhysicsServerSW::CollCbkData cbk; - cbk.max = max_results; - PhysicsServerSW::CollCbkData *cbkptr = &cbk; - CollisionSolverSW::CallbackResult cbkres = PhysicsServerSW::_shape_col_cbk; - - do { - Vector3 recover_motion; - - 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; - } - - ShapeSW *body_shape = p_body->get_shape(j); - - if (body_shape->get_type() != PhysicsServer::SHAPE_RAY) { - continue; - } - - Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); - - for (int i = 0; i < amount; i++) { - const CollisionObjectSW *col_obj = intersection_query_results[i]; - int shape_idx = intersection_query_subindex_results[i]; - - cbk.amount = 0; - cbk.ptr = sr; - - if (CollisionObjectSW::TYPE_BODY == col_obj->get_type()) { - const BodySW *b = static_cast(col_obj); - if (p_infinite_inertia && PhysicsServer::BODY_MODE_STATIC != b->get_mode() && PhysicsServer::BODY_MODE_KINEMATIC != b->get_mode()) { - continue; - } - } - - ShapeSW *against_shape = col_obj->get_shape(shape_idx); - if (CollisionSolverSW::solve_static(body_shape, body_shape_xform, against_shape, col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_margin)) { - if (cbk.amount > 0) { - collided = true; - } - - int ray_index = -1; //reuse shape - for (int k = 0; k < rays_found; k++) { - if (r_results[k].collision_local_shape == j) { - ray_index = k; - } - } - - if (ray_index == -1 && rays_found < p_result_max) { - ray_index = rays_found; - rays_found++; - } - - if (ray_index != -1) { - PhysicsServer::SeparationResult &result = r_results[ray_index]; - - for (int k = 0; k < cbk.amount; k++) { - Vector3 a = sr[k * 2 + 0]; - Vector3 b = sr[k * 2 + 1]; - - // Compute plane on b towards a. - Vector3 n = (a - b).normalized(); - float d = n.dot(b); - - // Compute depth on recovered motion. - float depth = n.dot(a + recover_motion) - d; - - // Apply recovery without margin. - float separation_depth = depth - p_margin; - if (separation_depth > 0.0) { - // Only recover if there is penetration. - recover_motion -= n * separation_depth; - } - - if (depth > result.collision_depth) { - result.collision_depth = depth; - result.collision_point = b; - result.collision_normal = -n; - result.collision_local_shape = j; - result.collider = col_obj->get_self(); - result.collider_id = col_obj->get_instance_id(); - result.collider_shape = shape_idx; - if (col_obj->get_type() == CollisionObjectSW::TYPE_BODY) { - BodySW *body = (BodySW *)col_obj; - - Vector3 rel_vec = b - (body->get_transform().origin + body->get_center_of_mass()); - result.collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec); - } - } - } - } - } - } - } - - if (!collided || recover_motion == Vector3()) { - break; - } - - body_transform.origin += recover_motion; - body_aabb.position += recover_motion; - - recover_attempts--; - } while (recover_attempts); - } - - r_recover_motion = body_transform.origin - p_transform.origin; - return rays_found; -} - -bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer::MotionResult *r_result, bool p_exclude_raycast_shapes, const RBSet &p_exclude) { - //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 = 0; - r_result->collider_shape = 0; - } - - 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 = PhysicsServer::MotionResult(); - r_result->motion = p_motion; - } - - return false; - } - - real_t margin = MAX(p_margin, TEST_MOTION_MARGIN_MIN_VALUE); - - // Undo the currently transform the physics server is aware of and apply the provided one - body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb)); - body_aabb = body_aabb.grow(margin); - - real_t min_contact_depth = margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; - - float motion_length = p_motion.length(); - Vector3 motion_normal = p_motion / motion_length; - - Transform body_transform = p_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 { - PhysicsServerSW::CollCbkData cbk; - cbk.max = max_results; - cbk.amount = 0; - cbk.ptr = sr; - - PhysicsServerSW::CollCbkData *cbkptr = &cbk; - CollisionSolverSW::CallbackResult cbkres = PhysicsServerSW::_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; - } - - Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); - ShapeSW *body_shape = p_body->get_shape(j); - if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer::SHAPE_RAY) { - continue; - } - - for (int i = 0; i < amount; i++) { - const CollisionObjectSW *col_obj = intersection_query_results[i]; - if (p_exclude.has(col_obj->get_self())) { - continue; - } - int shape_idx = intersection_query_subindex_results[i]; - - if (CollisionObjectSW::TYPE_BODY == col_obj->get_type()) { - const BodySW *b = static_cast(col_obj); - if (p_infinite_inertia && PhysicsServer::BODY_MODE_STATIC != b->get_mode() && PhysicsServer::BODY_MODE_KINEMATIC != b->get_mode()) { - continue; - } - } - - if (CollisionSolverSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, margin)) { - collided = cbk.amount > 0; - } - } - } - - 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(); - float d = n.dot(b); - - // Compute depth on recovered motion. - float 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_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; - } - - Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); - ShapeSW *body_shape = p_body->get_shape(j); - - if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer::SHAPE_RAY) { - continue; - } - - Transform body_shape_xform_inv = body_shape_xform.affine_inverse(); - MotionShapeSW mshape; - mshape.shape = body_shape; - mshape.motion = body_shape_xform_inv.basis.xform(p_motion); - - bool stuck = false; - - real_t best_safe = 1; - real_t best_unsafe = 1; - - for (int i = 0; i < amount; i++) { - const CollisionObjectSW *col_obj = intersection_query_results[i]; - if (p_exclude.has(col_obj->get_self())) { - continue; - } - int shape_idx = intersection_query_subindex_results[i]; - - if (CollisionObjectSW::TYPE_BODY == col_obj->get_type()) { - const BodySW *b = static_cast(col_obj); - if (p_infinite_inertia && PhysicsServer::BODY_MODE_STATIC != b->get_mode() && PhysicsServer::BODY_MODE_KINEMATIC != b->get_mode()) { - continue; - } - } - - //test initial overlap, does it collide if going all the way? - Vector3 point_A, point_B; - Vector3 sep_axis = motion_normal; - - Transform 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 (CollisionSolverSW::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 (!CollisionSolverSW::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_motion * fraction); - - Vector3 lA, lB; - Vector3 sep = motion_normal; //important optimization for this to work fast enough - bool collided = !CollisionSolverSW::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 - Transform ugt = body_transform; - ugt.origin += p_motion * unsafe; - - _RestCallbackData 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); - - body_aabb.position += p_motion * unsafe; - int amount = _cull_aabb_for_body(p_body, body_aabb); - - int from_shape = best_shape != -1 ? best_shape : 0; - int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count(); - - for (int j = from_shape; j < to_shape; j++) { - if (p_body->is_shape_disabled(j)) { - continue; - } - - Transform body_shape_xform = ugt * p_body->get_shape_transform(j); - ShapeSW *body_shape = p_body->get_shape(j); - - if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer::SHAPE_RAY) { - continue; - } - - for (int i = 0; i < amount; i++) { - const CollisionObjectSW *col_obj = intersection_query_results[i]; - if (p_exclude.has(col_obj->get_self())) { - continue; - } - int shape_idx = intersection_query_subindex_results[i]; - - if (CollisionObjectSW::TYPE_BODY == col_obj->get_type()) { - const BodySW *b = static_cast(col_obj); - if (p_infinite_inertia && PhysicsServer::BODY_MODE_STATIC != b->get_mode() && PhysicsServer::BODY_MODE_KINEMATIC != b->get_mode()) { - continue; - } - } - - rcd.object = col_obj; - rcd.shape = shape_idx; - rcd.local_shape = j; - bool sc = CollisionSolverSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, margin); - if (!sc) { - continue; - } - } - } - - if (rcd.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; - //r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape); - - const BodySW *body = static_cast(rcd.best_object); - - Vector3 rel_vec = rcd.best_contact - (body->get_transform().origin + body->get_center_of_mass()); - r_result->collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec); - - r_result->motion = safe * p_motion; - r_result->remainder = p_motion - safe * p_motion; - r_result->motion += (body_transform.get_origin() - p_from.get_origin()); - } - - collided = true; - } - } - - if (!collided && r_result) { - r_result->motion = p_motion; - r_result->remainder = Vector3(); - r_result->motion += (body_transform.get_origin() - p_from.get_origin()); - } - - return collided; -} - -// Assumes a valid collision pair, this should have been checked beforehand in the BVH or octree. -void *SpaceSW::_broadphase_pair(CollisionObjectSW *p_object_A, int p_subindex_A, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_self) { - // An existing pair - nothing to do, pair is still valid. - if (p_pair_data) { - return p_pair_data; - } - - // New pair - CollisionObjectSW::Type type_A = p_object_A->get_type(); - CollisionObjectSW::Type type_B = p_object_B->get_type(); - if (type_A > type_B) { - SWAP(p_object_A, p_object_B); - SWAP(p_subindex_A, p_subindex_B); - SWAP(type_A, type_B); - } - - SpaceSW *self = (SpaceSW *)p_self; - - self->collision_pairs++; - - if (type_A == CollisionObjectSW::TYPE_AREA) { - AreaSW *area_a = static_cast(p_object_A); - if (type_B == CollisionObjectSW::TYPE_AREA) { - AreaSW *area_b = static_cast(p_object_B); - Area2PairSW *area2_pair = memnew(Area2PairSW(area_b, p_subindex_B, area_a, p_subindex_A)); - return area2_pair; - } else { - BodySW *body_b = static_cast(p_object_B); - AreaPairSW *area_pair = memnew(AreaPairSW(body_b, p_subindex_B, area_a, p_subindex_A)); - return area_pair; - } - } else { - BodySW *body_a = static_cast(p_object_A); - BodySW *body_b = static_cast(p_object_B); - BodyPairSW *body_pair = memnew(BodyPairSW(body_a, p_subindex_A, body_b, p_subindex_B)); - return body_pair; - } - - return nullptr; -} - -void SpaceSW::_broadphase_unpair(CollisionObjectSW *p_object_A, int p_subindex_A, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_self) { - if (!p_pair_data) { - return; - } - - SpaceSW *self = (SpaceSW *)p_self; - self->collision_pairs--; - ConstraintSW *c = (ConstraintSW *)p_pair_data; - memdelete(c); -} - -const SelfList::List &SpaceSW::get_active_body_list() const { - return active_list; -} -void SpaceSW::body_add_to_active_list(SelfList *p_body) { - active_list.add(p_body); -} -void SpaceSW::body_remove_from_active_list(SelfList *p_body) { - active_list.remove(p_body); -} - -void SpaceSW::body_add_to_inertia_update_list(SelfList *p_body) { - inertia_update_list.add(p_body); -} - -void SpaceSW::body_remove_from_inertia_update_list(SelfList *p_body) { - inertia_update_list.remove(p_body); -} - -BroadPhaseSW *SpaceSW::get_broadphase() { - return broadphase; -} - -void SpaceSW::add_object(CollisionObjectSW *p_object) { - ERR_FAIL_COND(objects.has(p_object)); - objects.insert(p_object); -} - -void SpaceSW::remove_object(CollisionObjectSW *p_object) { - ERR_FAIL_COND(!objects.has(p_object)); - objects.erase(p_object); -} - -const RBSet &SpaceSW::get_objects() const { - return objects; -} - -void SpaceSW::body_add_to_state_query_list(SelfList *p_body) { - state_query_list.add(p_body); -} -void SpaceSW::body_remove_from_state_query_list(SelfList *p_body) { - state_query_list.remove(p_body); -} - -void SpaceSW::area_add_to_monitor_query_list(SelfList *p_area) { - monitor_query_list.add(p_area); -} -void SpaceSW::area_remove_from_monitor_query_list(SelfList *p_area) { - monitor_query_list.remove(p_area); -} - -void SpaceSW::area_add_to_moved_list(SelfList *p_area) { - area_moved_list.add(p_area); -} - -void SpaceSW::area_remove_from_moved_list(SelfList *p_area) { - area_moved_list.remove(p_area); -} - -const SelfList::List &SpaceSW::get_moved_area_list() const { - return area_moved_list; -} - -void SpaceSW::call_queries() { - while (state_query_list.first()) { - BodySW *b = state_query_list.first()->self(); - state_query_list.remove(state_query_list.first()); - b->call_queries(); - } - - while (monitor_query_list.first()) { - AreaSW *a = monitor_query_list.first()->self(); - monitor_query_list.remove(monitor_query_list.first()); - a->call_queries(); - } -} - -void SpaceSW::setup() { - contact_debug_count = 0; - while (inertia_update_list.first()) { - inertia_update_list.first()->self()->update_inertias(); - inertia_update_list.remove(inertia_update_list.first()); - } -} - -void SpaceSW::update() { - broadphase->update(); -} - -void SpaceSW::set_param(PhysicsServer::SpaceParameter p_param, real_t p_value) { - switch (p_param) { - case PhysicsServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: - contact_recycle_radius = p_value; - break; - case PhysicsServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: - contact_max_separation = p_value; - break; - case PhysicsServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: - contact_max_allowed_penetration = p_value; - break; - case PhysicsServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: - body_linear_velocity_sleep_threshold = p_value; - break; - case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: - body_angular_velocity_sleep_threshold = p_value; - break; - case PhysicsServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: - body_time_to_sleep = p_value; - break; - case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: - body_angular_velocity_damp_ratio = p_value; - break; - case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: - constraint_bias = p_value; - break; - } -} - -real_t SpaceSW::get_param(PhysicsServer::SpaceParameter p_param) const { - switch (p_param) { - case PhysicsServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: - return contact_recycle_radius; - case PhysicsServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: - return contact_max_separation; - case PhysicsServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: - return contact_max_allowed_penetration; - case PhysicsServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: - return body_linear_velocity_sleep_threshold; - case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: - return body_angular_velocity_sleep_threshold; - case PhysicsServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: - return body_time_to_sleep; - case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: - return body_angular_velocity_damp_ratio; - case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: - return constraint_bias; - } - return 0; -} - -void SpaceSW::lock() { - locked = true; -} - -void SpaceSW::unlock() { - locked = false; -} - -bool SpaceSW::is_locked() const { - return locked; -} - -PhysicsDirectSpaceStateSW *SpaceSW::get_direct_state() { - return direct_access; -} - -SpaceSW::SpaceSW() { - collision_pairs = 0; - active_objects = 0; - island_count = 0; - contact_debug_count = 0; - - locked = false; - contact_recycle_radius = 0.01; - contact_max_separation = 0.05; - contact_max_allowed_penetration = 0.01; - - constraint_bias = 0.01; - 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", (8.0 / 180.0 * Math_PI)); - 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::REAL, "physics/3d/time_before_sleep", PROPERTY_HINT_RANGE, "0,5,0.01,or_greater")); - body_angular_velocity_damp_ratio = 10; - - broadphase = BroadPhaseSW::create_func(); - broadphase->set_pair_callback(_broadphase_pair, this); - broadphase->set_unpair_callback(_broadphase_unpair, this); - area = nullptr; - - direct_access = memnew(PhysicsDirectSpaceStateSW); - direct_access->space = this; - - for (int i = 0; i < ELAPSED_TIME_MAX; i++) { - elapsed_time[i] = 0; - } -} - -SpaceSW::~SpaceSW() { - memdelete(broadphase); - memdelete(direct_access); -} diff --git a/servers/physics/space_sw.h b/servers/physics/space_sw.h deleted file mode 100644 index 9c02855..0000000 --- a/servers/physics/space_sw.h +++ /dev/null @@ -1,210 +0,0 @@ -#ifndef SPACE_SW_H -#define SPACE_SW_H -/*************************************************************************/ -/* space_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" -#include "area_sw.h" -#include "body_pair_sw.h" -#include "body_sw.h" -#include "broad_phase_sw.h" -#include "collision_object_sw.h" -#include "core/containers/hash_map.h" -#include "core/config/project_settings.h" -#include "core/typedefs.h" - -class PhysicsDirectSpaceStateSW : public PhysicsDirectSpaceState { - GDCLASS(PhysicsDirectSpaceStateSW, PhysicsDirectSpaceState); - -public: - SpaceSW *space; - - virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const RBSet &p_exclude = RBSet(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false); - virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const RBSet &p_exclude = RBSet(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_ray = false); - virtual int intersect_shape(const RID &p_shape, const Transform &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const RBSet &p_exclude = RBSet(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false); - virtual bool cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const RBSet &p_exclude = RBSet(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, ShapeRestInfo *r_info = nullptr); - virtual bool collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const RBSet &p_exclude = RBSet(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false); - virtual bool rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const RBSet &p_exclude = RBSet(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false); - virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const; - - PhysicsDirectSpaceStateSW(); -}; - -class SpaceSW : public RID_Data { -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]; - - PhysicsDirectSpaceStateSW *direct_access; - RID self; - - BroadPhaseSW *broadphase; - SelfList::List active_list; - SelfList::List inertia_update_list; - SelfList::List state_query_list; - SelfList::List monitor_query_list; - SelfList::List area_moved_list; - - static void *_broadphase_pair(CollisionObjectSW *p_object_A, int p_subindex_A, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_self); - static void _broadphase_unpair(CollisionObjectSW *p_object_A, int p_subindex_A, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_self); - - RBSet objects; - - AreaSW *area; - - real_t contact_recycle_radius; - real_t contact_max_separation; - real_t contact_max_allowed_penetration; - real_t constraint_bias; - - enum { - - INTERSECTION_QUERY_MAX = 2048 - }; - - CollisionObjectSW *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; - - real_t step; - int island_count; - int active_objects; - int collision_pairs; - - RID static_global_body; - - Vector contact_debug; - int contact_debug_count; - - friend class PhysicsDirectSpaceStateSW; - - int _cull_aabb_for_body(BodySW *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; } - - _FORCE_INLINE_ void set_step(const real_t &p_step) { step = p_step; } - _FORCE_INLINE_ real_t get_step() const { return step; } - - void set_default_area(AreaSW *p_area) { area = p_area; } - AreaSW *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_inertia_update_list(SelfList *p_body); - void body_remove_from_inertia_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; - - BroadPhaseSW *get_broadphase(); - - void add_object(CollisionObjectSW *p_object); - void remove_object(CollisionObjectSW *p_object); - const RBSet &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(); - - void set_param(PhysicsServer::SpaceParameter p_param, real_t p_value); - real_t get_param(PhysicsServer::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; } - - PhysicsDirectSpaceStateSW *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.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]; } - - int test_body_ray_separation(BodySW *p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer::SeparationResult *r_results, int p_result_max, real_t p_margin); - bool test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer::MotionResult *r_result, bool p_exclude_raycast_shapes, const RBSet &p_exclude = RBSet()); - - SpaceSW(); - ~SpaceSW(); -}; - -#endif // SPACE__SW_H diff --git a/servers/physics/step_sw.cpp b/servers/physics/step_sw.cpp deleted file mode 100644 index c6dc930..0000000 --- a/servers/physics/step_sw.cpp +++ /dev/null @@ -1,290 +0,0 @@ -/*************************************************************************/ -/* step_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" -#include "joints_sw.h" - -#include "core/os/os.h" - -void StepSW::_populate_island(BodySW *p_body, BodySW **p_island, ConstraintSW **p_constraint_island) { - p_body->set_island_step(_step); - p_body->set_island_next(*p_island); - *p_island = p_body; - - for (RBMap::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) { - ConstraintSW *c = (ConstraintSW *)E->key(); - if (c->get_island_step() == _step) { - continue; //already processed - } - c->set_island_step(_step); - c->set_island_next(*p_constraint_island); - *p_constraint_island = c; - - for (int i = 0; i < c->get_body_count(); i++) { - if (i == E->get()) { - continue; - } - BodySW *b = c->get_body_ptr()[i]; - if (b->get_island_step() == _step || b->get_mode() == PhysicsServer::BODY_MODE_STATIC || b->get_mode() == PhysicsServer::BODY_MODE_KINEMATIC) { - continue; //no go - } - _populate_island(c->get_body_ptr()[i], p_island, p_constraint_island); - } - } -} - -void StepSW::_setup_island(ConstraintSW *p_island, real_t p_delta) { - ConstraintSW *ci = p_island; - while (ci) { - ci->setup(p_delta); - //todo remove from island if process fails - ci = ci->get_island_next(); - } -} - -void StepSW::_solve_island(ConstraintSW *p_island, int p_iterations, real_t p_delta) { - int at_priority = 1; - - while (p_island) { - for (int i = 0; i < p_iterations; i++) { - ConstraintSW *ci = p_island; - while (ci) { - ci->solve(p_delta); - ci = ci->get_island_next(); - } - } - - at_priority++; - - { - ConstraintSW *ci = p_island; - ConstraintSW *prev = nullptr; - while (ci) { - if (ci->get_priority() < at_priority) { - if (prev) { - prev->set_island_next(ci->get_island_next()); //remove - } else { - p_island = ci->get_island_next(); - } - } else { - prev = ci; - } - - ci = ci->get_island_next(); - } - } - } -} - -void StepSW::_check_suspend(BodySW *p_island, real_t p_delta) { - bool can_sleep = true; - - BodySW *b = p_island; - while (b) { - if (b->get_mode() == PhysicsServer::BODY_MODE_STATIC || b->get_mode() == PhysicsServer::BODY_MODE_KINEMATIC) { - b = b->get_island_next(); - continue; //ignore for static - } - - if (!b->sleep_test(p_delta)) { - can_sleep = false; - } - - b = b->get_island_next(); - } - - //put all to sleep or wake up everyoen - - b = p_island; - while (b) { - if (b->get_mode() == PhysicsServer::BODY_MODE_STATIC || b->get_mode() == PhysicsServer::BODY_MODE_KINEMATIC) { - b = b->get_island_next(); - continue; //ignore for static - } - - bool active = b->is_active(); - - if (active == can_sleep) { - b->set_active(!can_sleep); - } - - b = b->get_island_next(); - } -} - -void StepSW::step(SpaceSW *p_space, real_t p_delta, int p_iterations) { - p_space->lock(); // can't access space during this - p_space->set_step(p_delta); - p_space->setup(); //update inertias, etc - - 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); - - // Update the broadphase to register collision pairs. - p_space->update(); - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime); - profile_begtime = profile_endtime; - } - - /* GENERATE CONSTRAINT ISLANDS */ - - BodySW *island_list = nullptr; - ConstraintSW *constraint_island_list = nullptr; - b = body_list->first(); - - int island_count = 0; - - while (b) { - BodySW *body = b->self(); - - if (body->get_island_step() != _step) { - BodySW *island = nullptr; - ConstraintSW *constraint_island = nullptr; - _populate_island(body, &island, &constraint_island); - - island->set_island_list_next(island_list); - island_list = island; - - if (constraint_island) { - constraint_island->set_island_list_next(constraint_island_list); - constraint_island_list = constraint_island; - island_count++; - } - } - b = b->next(); - } - - p_space->set_island_count(island_count); - - const SelfList::List &aml = p_space->get_moved_area_list(); - - while (aml.first()) { - for (const RBSet::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { - ConstraintSW *c = E->get(); - if (c->get_island_step() == _step) { - continue; - } - c->set_island_step(_step); - c->set_island_next(nullptr); - c->set_island_list_next(constraint_island_list); - constraint_island_list = c; - } - p_space->area_remove_from_moved_list((SelfList *)aml.first()); //faster to remove here - } - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); - profile_begtime = profile_endtime; - } - - /* SETUP CONSTRAINT ISLANDS */ - - { - ConstraintSW *ci = constraint_island_list; - while (ci) { - _setup_island(ci, p_delta); - ci = ci->get_island_list_next(); - } - } - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime); - profile_begtime = profile_endtime; - } - - /* SOLVE CONSTRAINT ISLANDS */ - - { - ConstraintSW *ci = constraint_island_list; - while (ci) { - //iterating each island separatedly improves cache efficiency - _solve_island(ci, p_iterations, p_delta); - ci = ci->get_island_list_next(); - } - } - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(SpaceSW::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 */ - - { - BodySW *bi = island_list; - while (bi) { - _check_suspend(bi, p_delta); - bi = bi->get_island_list_next(); - } - } - - { //profile - profile_endtime = OS::get_singleton()->get_ticks_usec(); - p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime); - profile_begtime = profile_endtime; - } - - p_space->unlock(); - _step++; -} - -StepSW::StepSW() { - _step = 1; -} diff --git a/servers/physics/step_sw.h b/servers/physics/step_sw.h deleted file mode 100644 index 96983a4..0000000 --- a/servers/physics/step_sw.h +++ /dev/null @@ -1,48 +0,0 @@ -#ifndef STEP_SW_H -#define STEP_SW_H -/*************************************************************************/ -/* step_sw.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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_sw.h" - -class StepSW { - uint64_t _step; - - void _populate_island(BodySW *p_body, BodySW **p_island, ConstraintSW **p_constraint_island); - void _setup_island(ConstraintSW *p_island, real_t p_delta); - void _solve_island(ConstraintSW *p_island, int p_iterations, real_t p_delta); - void _check_suspend(BodySW *p_island, real_t p_delta); - -public: - void step(SpaceSW *p_space, real_t p_delta, int p_iterations); - StepSW(); -}; - -#endif // STEP__SW_H diff --git a/servers/physics_server.cpp b/servers/physics_server.cpp deleted file mode 100644 index 090ecda..0000000 --- a/servers/physics_server.cpp +++ /dev/null @@ -1,870 +0,0 @@ -/*************************************************************************/ -/* physics_server.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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.h" - -#include "core/object/method_bind_ext.gen.inc" -#include "core/string/print_string.h" -#include "core/config/project_settings.h" - -PhysicsServer *PhysicsServer::singleton = nullptr; - -void PhysicsDirectBodyState::integrate_forces() { - real_t step = get_step(); - Vector3 lv = get_linear_velocity(); - lv += get_total_gravity() * step; - - Vector3 av = get_angular_velocity(); - - float linear_damp = 1.0 - step * get_total_linear_damp(); - - if (linear_damp < 0) { // reached zero in the given time - linear_damp = 0; - } - - float angular_damp = 1.0 - step * get_total_angular_damp(); - - if (angular_damp < 0) { // reached zero in the given time - angular_damp = 0; - } - - lv *= linear_damp; - av *= angular_damp; - - set_linear_velocity(lv); - set_angular_velocity(av); -} - -Object *PhysicsDirectBodyState::get_contact_collider_object(int p_contact_idx) const { - ObjectID objid = get_contact_collider_id(p_contact_idx); - Object *obj = ObjectDB::get_instance(objid); - return obj; -} - -PhysicsServer *PhysicsServer::get_singleton() { - return singleton; -} - -void PhysicsDirectBodyState::_bind_methods() { - ClassDB::bind_method(D_METHOD("get_total_gravity"), &PhysicsDirectBodyState::get_total_gravity); - ClassDB::bind_method(D_METHOD("get_total_linear_damp"), &PhysicsDirectBodyState::get_total_linear_damp); - ClassDB::bind_method(D_METHOD("get_total_angular_damp"), &PhysicsDirectBodyState::get_total_angular_damp); - - ClassDB::bind_method(D_METHOD("get_center_of_mass"), &PhysicsDirectBodyState::get_center_of_mass); - ClassDB::bind_method(D_METHOD("get_principal_inertia_axes"), &PhysicsDirectBodyState::get_principal_inertia_axes); - - ClassDB::bind_method(D_METHOD("get_inverse_mass"), &PhysicsDirectBodyState::get_inverse_mass); - ClassDB::bind_method(D_METHOD("get_inverse_inertia"), &PhysicsDirectBodyState::get_inverse_inertia); - - ClassDB::bind_method(D_METHOD("set_linear_velocity", "velocity"), &PhysicsDirectBodyState::set_linear_velocity); - ClassDB::bind_method(D_METHOD("get_linear_velocity"), &PhysicsDirectBodyState::get_linear_velocity); - - ClassDB::bind_method(D_METHOD("set_angular_velocity", "velocity"), &PhysicsDirectBodyState::set_angular_velocity); - ClassDB::bind_method(D_METHOD("get_angular_velocity"), &PhysicsDirectBodyState::get_angular_velocity); - - ClassDB::bind_method(D_METHOD("set_transform", "transform"), &PhysicsDirectBodyState::set_transform); - ClassDB::bind_method(D_METHOD("get_transform"), &PhysicsDirectBodyState::get_transform); - - ClassDB::bind_method(D_METHOD("get_velocity_at_local_position", "local_position"), &PhysicsDirectBodyState::get_velocity_at_local_position); - - ClassDB::bind_method(D_METHOD("add_central_force", "force"), &PhysicsDirectBodyState::add_central_force); - ClassDB::bind_method(D_METHOD("add_force", "force", "position"), &PhysicsDirectBodyState::add_force); - ClassDB::bind_method(D_METHOD("add_torque", "torque"), &PhysicsDirectBodyState::add_torque); - ClassDB::bind_method(D_METHOD("apply_central_impulse", "j"), &PhysicsDirectBodyState::apply_central_impulse); - ClassDB::bind_method(D_METHOD("apply_impulse", "position", "j"), &PhysicsDirectBodyState::apply_impulse); - ClassDB::bind_method(D_METHOD("apply_torque_impulse", "j"), &PhysicsDirectBodyState::apply_torque_impulse); - - ClassDB::bind_method(D_METHOD("set_sleep_state", "enabled"), &PhysicsDirectBodyState::set_sleep_state); - ClassDB::bind_method(D_METHOD("is_sleeping"), &PhysicsDirectBodyState::is_sleeping); - - ClassDB::bind_method(D_METHOD("get_contact_count"), &PhysicsDirectBodyState::get_contact_count); - - ClassDB::bind_method(D_METHOD("get_contact_local_position", "contact_idx"), &PhysicsDirectBodyState::get_contact_local_position); - ClassDB::bind_method(D_METHOD("get_contact_local_normal", "contact_idx"), &PhysicsDirectBodyState::get_contact_local_normal); - ClassDB::bind_method(D_METHOD("get_contact_impulse", "contact_idx"), &PhysicsDirectBodyState::get_contact_impulse); - ClassDB::bind_method(D_METHOD("get_contact_local_shape", "contact_idx"), &PhysicsDirectBodyState::get_contact_local_shape); - ClassDB::bind_method(D_METHOD("get_contact_collider", "contact_idx"), &PhysicsDirectBodyState::get_contact_collider); - ClassDB::bind_method(D_METHOD("get_contact_collider_position", "contact_idx"), &PhysicsDirectBodyState::get_contact_collider_position); - ClassDB::bind_method(D_METHOD("get_contact_collider_id", "contact_idx"), &PhysicsDirectBodyState::get_contact_collider_id); - ClassDB::bind_method(D_METHOD("get_contact_collider_object", "contact_idx"), &PhysicsDirectBodyState::get_contact_collider_object); - ClassDB::bind_method(D_METHOD("get_contact_collider_shape", "contact_idx"), &PhysicsDirectBodyState::get_contact_collider_shape); - ClassDB::bind_method(D_METHOD("get_contact_collider_velocity_at_position", "contact_idx"), &PhysicsDirectBodyState::get_contact_collider_velocity_at_position); - ClassDB::bind_method(D_METHOD("get_step"), &PhysicsDirectBodyState::get_step); - ClassDB::bind_method(D_METHOD("integrate_forces"), &PhysicsDirectBodyState::integrate_forces); - ClassDB::bind_method(D_METHOD("get_space_state"), &PhysicsDirectBodyState::get_space_state); - - ADD_PROPERTY(PropertyInfo(Variant::REAL, "step"), "", "get_step"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "inverse_mass"), "", "get_inverse_mass"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "total_angular_damp"), "", "get_total_angular_damp"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "total_linear_damp"), "", "get_total_linear_damp"); - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "inverse_inertia"), "", "get_inverse_inertia"); - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "total_gravity"), "", "get_total_gravity"); - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "center_of_mass"), "", "get_center_of_mass"); - ADD_PROPERTY(PropertyInfo(Variant::BASIS, "principal_inertia_axes"), "", "get_principal_inertia_axes"); - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "angular_velocity"), "set_angular_velocity", "get_angular_velocity"); - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "linear_velocity"), "set_linear_velocity", "get_linear_velocity"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "sleeping"), "set_sleep_state", "is_sleeping"); - ADD_PROPERTY(PropertyInfo(Variant::TRANSFORM2D, "transform"), "set_transform", "get_transform"); -} - -PhysicsDirectBodyState::PhysicsDirectBodyState() {} - -/////////////////////////////////////////////////////// - -void PhysicsShapeQueryParameters::set_shape(const RES &p_shape) { - ERR_FAIL_COND(p_shape.is_null()); - shape = p_shape->get_rid(); -} - -void PhysicsShapeQueryParameters::set_shape_rid(const RID &p_shape) { - shape = p_shape; -} - -RID PhysicsShapeQueryParameters::get_shape_rid() const { - return shape; -} - -void PhysicsShapeQueryParameters::set_transform(const Transform &p_transform) { - transform = p_transform; -} -Transform PhysicsShapeQueryParameters::get_transform() const { - return transform; -} - -void PhysicsShapeQueryParameters::set_margin(float p_margin) { - margin = p_margin; -} - -float PhysicsShapeQueryParameters::get_margin() const { - return margin; -} - -void PhysicsShapeQueryParameters::set_collision_mask(uint32_t p_collision_mask) { - collision_mask = p_collision_mask; -} - -uint32_t PhysicsShapeQueryParameters::get_collision_mask() const { - return collision_mask; -} - -void PhysicsShapeQueryParameters::set_exclude(const Vector &p_exclude) { - exclude.clear(); - for (int i = 0; i < p_exclude.size(); i++) { - exclude.insert(p_exclude[i]); - } -} - -Vector PhysicsShapeQueryParameters::get_exclude() const { - Vector ret; - ret.resize(exclude.size()); - int idx = 0; - for (RBSet::Element *E = exclude.front(); E; E = E->next()) { - ret.write[idx++] = E->get(); - } - return ret; -} - -void PhysicsShapeQueryParameters::set_collide_with_bodies(bool p_enable) { - collide_with_bodies = p_enable; -} - -bool PhysicsShapeQueryParameters::is_collide_with_bodies_enabled() const { - return collide_with_bodies; -} - -void PhysicsShapeQueryParameters::set_collide_with_areas(bool p_enable) { - collide_with_areas = p_enable; -} - -bool PhysicsShapeQueryParameters::is_collide_with_areas_enabled() const { - return collide_with_areas; -} - -void PhysicsShapeQueryParameters::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_shape", "shape"), &PhysicsShapeQueryParameters::set_shape); - ClassDB::bind_method(D_METHOD("set_shape_rid", "shape"), &PhysicsShapeQueryParameters::set_shape_rid); - ClassDB::bind_method(D_METHOD("get_shape_rid"), &PhysicsShapeQueryParameters::get_shape_rid); - - ClassDB::bind_method(D_METHOD("set_transform", "transform"), &PhysicsShapeQueryParameters::set_transform); - ClassDB::bind_method(D_METHOD("get_transform"), &PhysicsShapeQueryParameters::get_transform); - - ClassDB::bind_method(D_METHOD("set_margin", "margin"), &PhysicsShapeQueryParameters::set_margin); - ClassDB::bind_method(D_METHOD("get_margin"), &PhysicsShapeQueryParameters::get_margin); - - ClassDB::bind_method(D_METHOD("set_collision_mask", "collision_mask"), &PhysicsShapeQueryParameters::set_collision_mask); - ClassDB::bind_method(D_METHOD("get_collision_mask"), &PhysicsShapeQueryParameters::get_collision_mask); - - ClassDB::bind_method(D_METHOD("set_exclude", "exclude"), &PhysicsShapeQueryParameters::set_exclude); - ClassDB::bind_method(D_METHOD("get_exclude"), &PhysicsShapeQueryParameters::get_exclude); - - ClassDB::bind_method(D_METHOD("set_collide_with_bodies", "enable"), &PhysicsShapeQueryParameters::set_collide_with_bodies); - ClassDB::bind_method(D_METHOD("is_collide_with_bodies_enabled"), &PhysicsShapeQueryParameters::is_collide_with_bodies_enabled); - - ClassDB::bind_method(D_METHOD("set_collide_with_areas", "enable"), &PhysicsShapeQueryParameters::set_collide_with_areas); - ClassDB::bind_method(D_METHOD("is_collide_with_areas_enabled"), &PhysicsShapeQueryParameters::is_collide_with_areas_enabled); - - ADD_PROPERTY(PropertyInfo(Variant::INT, "collision_mask", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_collision_mask", "get_collision_mask"); - ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "exclude", PROPERTY_HINT_NONE, itos(Variant::RID) + ":"), "set_exclude", "get_exclude"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "margin", PROPERTY_HINT_RANGE, "0,100,0.01"), "set_margin", "get_margin"); - //ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "shape", PROPERTY_HINT_RESOURCE_TYPE, "Shape2D"), "set_shape", ""); // FIXME: Lacks a getter - ADD_PROPERTY(PropertyInfo(Variant::RID, "shape_rid"), "set_shape_rid", "get_shape_rid"); - ADD_PROPERTY(PropertyInfo(Variant::TRANSFORM, "transform"), "set_transform", "get_transform"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "collide_with_bodies"), "set_collide_with_bodies", "is_collide_with_bodies_enabled"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "collide_with_areas"), "set_collide_with_areas", "is_collide_with_areas_enabled"); -} - -PhysicsShapeQueryParameters::PhysicsShapeQueryParameters() { - margin = 0; - collision_mask = 0x7FFFFFFF; - collide_with_bodies = true; - collide_with_areas = false; -} - -///////////////////////////////////// - -Dictionary PhysicsDirectSpaceState::_intersect_ray(const Vector3 &p_from, const Vector3 &p_to, const Vector &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - RayResult inters; - RBSet exclude; - for (int i = 0; i < p_exclude.size(); i++) { - exclude.insert(p_exclude[i]); - } - - bool res = intersect_ray(p_from, p_to, inters, exclude, p_collision_mask, p_collide_with_bodies, p_collide_with_areas); - - if (!res) { - return Dictionary(); - } - - Dictionary d; - d["position"] = inters.position; - d["normal"] = inters.normal; - d["collider_id"] = inters.collider_id; - d["collider"] = inters.collider; - d["shape"] = inters.shape; - d["rid"] = inters.rid; - - return d; -} - -Array PhysicsDirectSpaceState::_intersect_point(const Vector3 &p_point, int p_max_results, const Vector &p_exclude, uint32_t p_layers, bool p_collide_with_bodies, bool p_collide_with_areas) { - RBSet exclude; - for (int i = 0; i < p_exclude.size(); i++) { - exclude.insert(p_exclude[i]); - } - - Vector ret; - ret.resize(p_max_results); - - int rc = intersect_point(p_point, ret.ptrw(), ret.size(), exclude, p_layers, p_collide_with_bodies, p_collide_with_areas); - - if (rc == 0) { - return Array(); - } - - Array r; - r.resize(rc); - for (int i = 0; i < rc; i++) { - Dictionary d; - d["rid"] = ret[i].rid; - d["collider_id"] = ret[i].collider_id; - d["collider"] = ret[i].collider; - d["shape"] = ret[i].shape; - r[i] = d; - } - return r; -} - -Array PhysicsDirectSpaceState::_intersect_shape(const Ref &p_shape_query, int p_max_results) { - ERR_FAIL_COND_V(!p_shape_query.is_valid(), Array()); - - Vector sr; - sr.resize(p_max_results); - int rc = intersect_shape(p_shape_query->shape, p_shape_query->transform, p_shape_query->margin, sr.ptrw(), sr.size(), p_shape_query->exclude, p_shape_query->collision_mask, p_shape_query->collide_with_bodies, p_shape_query->collide_with_areas); - Array ret; - ret.resize(rc); - for (int i = 0; i < rc; i++) { - Dictionary d; - d["rid"] = sr[i].rid; - d["collider_id"] = sr[i].collider_id; - d["collider"] = sr[i].collider; - d["shape"] = sr[i].shape; - ret[i] = d; - } - - return ret; -} - -Array PhysicsDirectSpaceState::_cast_motion(const Ref &p_shape_query, const Vector3 &p_motion) { - ERR_FAIL_COND_V(!p_shape_query.is_valid(), Array()); - - float closest_safe = 1.0f, closest_unsafe = 1.0f; - bool res = cast_motion(p_shape_query->shape, p_shape_query->transform, p_motion, p_shape_query->margin, closest_safe, closest_unsafe, p_shape_query->exclude, p_shape_query->collision_mask, p_shape_query->collide_with_bodies, p_shape_query->collide_with_areas); - if (!res) { - return Array(); - } - Array ret; - ret.resize(2); - ret[0] = closest_safe; - ret[1] = closest_unsafe; - return ret; -} -Array PhysicsDirectSpaceState::_collide_shape(const Ref &p_shape_query, int p_max_results) { - ERR_FAIL_COND_V(!p_shape_query.is_valid(), Array()); - - Vector ret; - ret.resize(p_max_results * 2); - int rc = 0; - bool res = collide_shape(p_shape_query->shape, p_shape_query->transform, p_shape_query->margin, ret.ptrw(), p_max_results, rc, p_shape_query->exclude, p_shape_query->collision_mask, p_shape_query->collide_with_bodies, p_shape_query->collide_with_areas); - if (!res) { - return Array(); - } - Array r; - r.resize(rc * 2); - for (int i = 0; i < rc * 2; i++) { - r[i] = ret[i]; - } - return r; -} -Dictionary PhysicsDirectSpaceState::_get_rest_info(const Ref &p_shape_query) { - ERR_FAIL_COND_V(!p_shape_query.is_valid(), Dictionary()); - - ShapeRestInfo sri; - - bool res = rest_info(p_shape_query->shape, p_shape_query->transform, p_shape_query->margin, &sri, p_shape_query->exclude, p_shape_query->collision_mask, p_shape_query->collide_with_bodies, p_shape_query->collide_with_areas); - Dictionary r; - if (!res) { - return r; - } - - r["point"] = sri.point; - r["normal"] = sri.normal; - r["rid"] = sri.rid; - r["collider_id"] = sri.collider_id; - r["shape"] = sri.shape; - r["linear_velocity"] = sri.linear_velocity; - - return r; -} - -PhysicsDirectSpaceState::PhysicsDirectSpaceState() { -} - -void PhysicsDirectSpaceState::_bind_methods() { - ClassDB::bind_method(D_METHOD("intersect_point", "point", "max_results", "exclude", "collision_layer", "collide_with_bodies", "collide_with_areas"), &PhysicsDirectSpaceState::_intersect_point, DEFVAL(32), DEFVAL(Array()), DEFVAL(0x7FFFFFFF), DEFVAL(true), DEFVAL(false)); - ClassDB::bind_method(D_METHOD("intersect_ray", "from", "to", "exclude", "collision_mask", "collide_with_bodies", "collide_with_areas"), &PhysicsDirectSpaceState::_intersect_ray, DEFVAL(Array()), DEFVAL(0x7FFFFFFF), DEFVAL(true), DEFVAL(false)); - ClassDB::bind_method(D_METHOD("intersect_shape", "shape", "max_results"), &PhysicsDirectSpaceState::_intersect_shape, DEFVAL(32)); - ClassDB::bind_method(D_METHOD("cast_motion", "shape", "motion"), &PhysicsDirectSpaceState::_cast_motion); - ClassDB::bind_method(D_METHOD("collide_shape", "shape", "max_results"), &PhysicsDirectSpaceState::_collide_shape, DEFVAL(32)); - ClassDB::bind_method(D_METHOD("get_rest_info", "shape"), &PhysicsDirectSpaceState::_get_rest_info); -} - -/////////////////////////////// - -Vector3 PhysicsTestMotionResult::get_motion() const { - return result.motion; -} - -Vector3 PhysicsTestMotionResult::get_motion_remainder() const { - return result.remainder; -} - -Vector3 PhysicsTestMotionResult::get_collision_point() const { - return result.collision_point; -} - -Vector3 PhysicsTestMotionResult::get_collision_normal() const { - return result.collision_normal; -} - -Vector3 PhysicsTestMotionResult::get_collider_velocity() const { - return result.collider_velocity; -} - -ObjectID PhysicsTestMotionResult::get_collider_id() const { - return result.collider_id; -} - -RID PhysicsTestMotionResult::get_collider_rid() const { - return result.collider; -} - -Object *PhysicsTestMotionResult::get_collider() const { - return ObjectDB::get_instance(result.collider_id); -} - -int PhysicsTestMotionResult::get_collider_shape() const { - return result.collider_shape; -} - -real_t PhysicsTestMotionResult::get_collision_depth() const { - return result.collision_depth; -} - -real_t PhysicsTestMotionResult::get_collision_safe_fraction() const { - return result.collision_safe_fraction; -} - -real_t PhysicsTestMotionResult::get_collision_unsafe_fraction() const { - return result.collision_unsafe_fraction; -} - -void PhysicsTestMotionResult::_bind_methods() { - ClassDB::bind_method(D_METHOD("get_motion"), &PhysicsTestMotionResult::get_motion); - ClassDB::bind_method(D_METHOD("get_motion_remainder"), &PhysicsTestMotionResult::get_motion_remainder); - ClassDB::bind_method(D_METHOD("get_collision_point"), &PhysicsTestMotionResult::get_collision_point); - ClassDB::bind_method(D_METHOD("get_collision_normal"), &PhysicsTestMotionResult::get_collision_normal); - ClassDB::bind_method(D_METHOD("get_collider_velocity"), &PhysicsTestMotionResult::get_collider_velocity); - ClassDB::bind_method(D_METHOD("get_collider_id"), &PhysicsTestMotionResult::get_collider_id); - ClassDB::bind_method(D_METHOD("get_collider_rid"), &PhysicsTestMotionResult::get_collider_rid); - ClassDB::bind_method(D_METHOD("get_collider"), &PhysicsTestMotionResult::get_collider); - ClassDB::bind_method(D_METHOD("get_collider_shape"), &PhysicsTestMotionResult::get_collider_shape); - ClassDB::bind_method(D_METHOD("get_collision_depth"), &PhysicsTestMotionResult::get_collision_depth); - ClassDB::bind_method(D_METHOD("get_collision_safe_fraction"), &PhysicsTestMotionResult::get_collision_safe_fraction); - ClassDB::bind_method(D_METHOD("get_collision_unsafe_fraction"), &PhysicsTestMotionResult::get_collision_unsafe_fraction); - - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "motion"), "", "get_motion"); - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "motion_remainder"), "", "get_motion_remainder"); - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "collision_point"), "", "get_collision_point"); - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "collision_normal"), "", "get_collision_normal"); - ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "collider_velocity"), "", "get_collider_velocity"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "collider_id", PROPERTY_HINT_OBJECT_ID), "", "get_collider_id"); - ADD_PROPERTY(PropertyInfo(Variant::RID, "collider_rid"), "", "get_collider_rid"); - ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "collider"), "", "get_collider"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "collider_shape"), "", "get_collider_shape"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "collision_depth"), "", "get_collision_depth"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "collision_safe_fraction"), "", "get_collision_safe_fraction"); - ADD_PROPERTY(PropertyInfo(Variant::REAL, "collision_unsafe_fraction"), "", "get_collision_unsafe_fraction"); -} - -/////////////////////////////////////// - -bool PhysicsServer::_body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, const Ref &p_result, bool p_exclude_raycast_shapes, const Vector &p_exclude) { - MotionResult *r = nullptr; - if (p_result.is_valid()) { - r = p_result->get_result_ptr(); - } - RBSet exclude; - for (int i = 0; i < p_exclude.size(); i++) { - exclude.insert(p_exclude[i]); - } - return body_test_motion(p_body, p_from, p_motion, p_infinite_inertia, r, p_exclude_raycast_shapes, exclude); -} - -void PhysicsServer::_bind_methods() { -#ifndef _3D_DISABLED - - ClassDB::bind_method(D_METHOD("shape_create", "type"), &PhysicsServer::shape_create); - ClassDB::bind_method(D_METHOD("shape_set_data", "shape", "data"), &PhysicsServer::shape_set_data); - - ClassDB::bind_method(D_METHOD("shape_get_type", "shape"), &PhysicsServer::shape_get_type); - ClassDB::bind_method(D_METHOD("shape_get_data", "shape"), &PhysicsServer::shape_get_data); - - ClassDB::bind_method(D_METHOD("space_create"), &PhysicsServer::space_create); - ClassDB::bind_method(D_METHOD("space_set_active", "space", "active"), &PhysicsServer::space_set_active); - ClassDB::bind_method(D_METHOD("space_is_active", "space"), &PhysicsServer::space_is_active); - ClassDB::bind_method(D_METHOD("space_set_param", "space", "param", "value"), &PhysicsServer::space_set_param); - ClassDB::bind_method(D_METHOD("space_get_param", "space", "param"), &PhysicsServer::space_get_param); - ClassDB::bind_method(D_METHOD("space_get_direct_state", "space"), &PhysicsServer::space_get_direct_state); - - ClassDB::bind_method(D_METHOD("area_create"), &PhysicsServer::area_create); - ClassDB::bind_method(D_METHOD("area_set_space", "area", "space"), &PhysicsServer::area_set_space); - ClassDB::bind_method(D_METHOD("area_get_space", "area"), &PhysicsServer::area_get_space); - - ClassDB::bind_method(D_METHOD("area_set_space_override_mode", "area", "mode"), &PhysicsServer::area_set_space_override_mode); - ClassDB::bind_method(D_METHOD("area_get_space_override_mode", "area"), &PhysicsServer::area_get_space_override_mode); - - ClassDB::bind_method(D_METHOD("area_add_shape", "area", "shape", "transform", "disabled"), &PhysicsServer::area_add_shape, DEFVAL(Transform()), DEFVAL(false)); - ClassDB::bind_method(D_METHOD("area_set_shape", "area", "shape_idx", "shape"), &PhysicsServer::area_set_shape); - ClassDB::bind_method(D_METHOD("area_set_shape_transform", "area", "shape_idx", "transform"), &PhysicsServer::area_set_shape_transform); - ClassDB::bind_method(D_METHOD("area_set_shape_disabled", "area", "shape_idx", "disabled"), &PhysicsServer::area_set_shape_disabled); - - ClassDB::bind_method(D_METHOD("area_get_shape_count", "area"), &PhysicsServer::area_get_shape_count); - ClassDB::bind_method(D_METHOD("area_get_shape", "area", "shape_idx"), &PhysicsServer::area_get_shape); - ClassDB::bind_method(D_METHOD("area_get_shape_transform", "area", "shape_idx"), &PhysicsServer::area_get_shape_transform); - - ClassDB::bind_method(D_METHOD("area_remove_shape", "area", "shape_idx"), &PhysicsServer::area_remove_shape); - ClassDB::bind_method(D_METHOD("area_clear_shapes", "area"), &PhysicsServer::area_clear_shapes); - - ClassDB::bind_method(D_METHOD("area_set_collision_layer", "area", "layer"), &PhysicsServer::area_set_collision_layer); - ClassDB::bind_method(D_METHOD("area_set_collision_mask", "area", "mask"), &PhysicsServer::area_set_collision_mask); - - ClassDB::bind_method(D_METHOD("area_set_param", "area", "param", "value"), &PhysicsServer::area_set_param); - ClassDB::bind_method(D_METHOD("area_set_transform", "area", "transform"), &PhysicsServer::area_set_transform); - - ClassDB::bind_method(D_METHOD("area_get_param", "area", "param"), &PhysicsServer::area_get_param); - ClassDB::bind_method(D_METHOD("area_get_transform", "area"), &PhysicsServer::area_get_transform); - - ClassDB::bind_method(D_METHOD("area_attach_object_instance_id", "area", "id"), &PhysicsServer::area_attach_object_instance_id); - ClassDB::bind_method(D_METHOD("area_get_object_instance_id", "area"), &PhysicsServer::area_get_object_instance_id); - - ClassDB::bind_method(D_METHOD("area_set_monitor_callback", "area", "receiver", "method"), &PhysicsServer::area_set_monitor_callback); - ClassDB::bind_method(D_METHOD("area_set_area_monitor_callback", "area", "receiver", "method"), &PhysicsServer::area_set_area_monitor_callback); - ClassDB::bind_method(D_METHOD("area_set_monitorable", "area", "monitorable"), &PhysicsServer::area_set_monitorable); - - ClassDB::bind_method(D_METHOD("area_set_ray_pickable", "area", "enable"), &PhysicsServer::area_set_ray_pickable); - ClassDB::bind_method(D_METHOD("area_is_ray_pickable", "area"), &PhysicsServer::area_is_ray_pickable); - - ClassDB::bind_method(D_METHOD("body_create", "mode", "init_sleeping"), &PhysicsServer::body_create, DEFVAL(BODY_MODE_RIGID), DEFVAL(false)); - - ClassDB::bind_method(D_METHOD("body_set_space", "body", "space"), &PhysicsServer::body_set_space); - ClassDB::bind_method(D_METHOD("body_get_space", "body"), &PhysicsServer::body_get_space); - - ClassDB::bind_method(D_METHOD("body_set_mode", "body", "mode"), &PhysicsServer::body_set_mode); - ClassDB::bind_method(D_METHOD("body_get_mode", "body"), &PhysicsServer::body_get_mode); - - ClassDB::bind_method(D_METHOD("body_set_collision_layer", "body", "layer"), &PhysicsServer::body_set_collision_layer); - ClassDB::bind_method(D_METHOD("body_get_collision_layer", "body"), &PhysicsServer::body_get_collision_layer); - - ClassDB::bind_method(D_METHOD("body_set_collision_mask", "body", "mask"), &PhysicsServer::body_set_collision_mask); - ClassDB::bind_method(D_METHOD("body_get_collision_mask", "body"), &PhysicsServer::body_get_collision_mask); - - ClassDB::bind_method(D_METHOD("body_add_shape", "body", "shape", "transform", "disabled"), &PhysicsServer::body_add_shape, DEFVAL(Transform()), DEFVAL(false)); - ClassDB::bind_method(D_METHOD("body_set_shape", "body", "shape_idx", "shape"), &PhysicsServer::body_set_shape); - ClassDB::bind_method(D_METHOD("body_set_shape_transform", "body", "shape_idx", "transform"), &PhysicsServer::body_set_shape_transform); - ClassDB::bind_method(D_METHOD("body_set_shape_disabled", "body", "shape_idx", "disabled"), &PhysicsServer::body_set_shape_disabled); - - ClassDB::bind_method(D_METHOD("body_get_shape_count", "body"), &PhysicsServer::body_get_shape_count); - ClassDB::bind_method(D_METHOD("body_get_shape", "body", "shape_idx"), &PhysicsServer::body_get_shape); - ClassDB::bind_method(D_METHOD("body_get_shape_transform", "body", "shape_idx"), &PhysicsServer::body_get_shape_transform); - - ClassDB::bind_method(D_METHOD("body_remove_shape", "body", "shape_idx"), &PhysicsServer::body_remove_shape); - ClassDB::bind_method(D_METHOD("body_clear_shapes", "body"), &PhysicsServer::body_clear_shapes); - - ClassDB::bind_method(D_METHOD("body_attach_object_instance_id", "body", "id"), &PhysicsServer::body_attach_object_instance_id); - ClassDB::bind_method(D_METHOD("body_get_object_instance_id", "body"), &PhysicsServer::body_get_object_instance_id); - - ClassDB::bind_method(D_METHOD("body_set_enable_continuous_collision_detection", "body", "enable"), &PhysicsServer::body_set_enable_continuous_collision_detection); - ClassDB::bind_method(D_METHOD("body_is_continuous_collision_detection_enabled", "body"), &PhysicsServer::body_is_continuous_collision_detection_enabled); - - ClassDB::bind_method(D_METHOD("body_set_param", "body", "param", "value"), &PhysicsServer::body_set_param); - ClassDB::bind_method(D_METHOD("body_get_param", "body", "param"), &PhysicsServer::body_get_param); - - ClassDB::bind_method(D_METHOD("body_set_kinematic_safe_margin", "body", "margin"), &PhysicsServer::body_set_kinematic_safe_margin); - ClassDB::bind_method(D_METHOD("body_get_kinematic_safe_margin", "body"), &PhysicsServer::body_get_kinematic_safe_margin); - - ClassDB::bind_method(D_METHOD("body_set_state", "body", "state", "value"), &PhysicsServer::body_set_state); - ClassDB::bind_method(D_METHOD("body_get_state", "body", "state"), &PhysicsServer::body_get_state); - - ClassDB::bind_method(D_METHOD("body_add_central_force", "body", "force"), &PhysicsServer::body_add_central_force); - ClassDB::bind_method(D_METHOD("body_add_force", "body", "force", "position"), &PhysicsServer::body_add_force); - ClassDB::bind_method(D_METHOD("body_add_torque", "body", "torque"), &PhysicsServer::body_add_torque); - - ClassDB::bind_method(D_METHOD("body_apply_central_impulse", "body", "impulse"), &PhysicsServer::body_apply_central_impulse); - ClassDB::bind_method(D_METHOD("body_apply_impulse", "body", "position", "impulse"), &PhysicsServer::body_apply_impulse); - ClassDB::bind_method(D_METHOD("body_apply_torque_impulse", "body", "impulse"), &PhysicsServer::body_apply_torque_impulse); - ClassDB::bind_method(D_METHOD("body_set_axis_velocity", "body", "axis_velocity"), &PhysicsServer::body_set_axis_velocity); - - ClassDB::bind_method(D_METHOD("body_set_axis_lock", "body", "axis", "lock"), &PhysicsServer::body_set_axis_lock); - ClassDB::bind_method(D_METHOD("body_is_axis_locked", "body", "axis"), &PhysicsServer::body_is_axis_locked); - - ClassDB::bind_method(D_METHOD("body_add_collision_exception", "body", "excepted_body"), &PhysicsServer::body_add_collision_exception); - ClassDB::bind_method(D_METHOD("body_remove_collision_exception", "body", "excepted_body"), &PhysicsServer::body_remove_collision_exception); - - ClassDB::bind_method(D_METHOD("body_set_max_contacts_reported", "body", "amount"), &PhysicsServer::body_set_max_contacts_reported); - ClassDB::bind_method(D_METHOD("body_get_max_contacts_reported", "body"), &PhysicsServer::body_get_max_contacts_reported); - - ClassDB::bind_method(D_METHOD("body_set_omit_force_integration", "body", "enable"), &PhysicsServer::body_set_omit_force_integration); - ClassDB::bind_method(D_METHOD("body_is_omitting_force_integration", "body"), &PhysicsServer::body_is_omitting_force_integration); - - ClassDB::bind_method(D_METHOD("body_set_force_integration_callback", "body", "receiver", "method", "userdata"), &PhysicsServer::body_set_force_integration_callback, DEFVAL(Variant())); - - ClassDB::bind_method(D_METHOD("body_set_ray_pickable", "body", "enable"), &PhysicsServer::body_set_ray_pickable); - ClassDB::bind_method(D_METHOD("body_is_ray_pickable", "body"), &PhysicsServer::body_is_ray_pickable); - - ClassDB::bind_method(D_METHOD("body_test_motion", "body", "from", "motion", "infinite_inertia", "result", "exclude_raycast_shapes", "exclude"), &PhysicsServer::_body_test_motion, DEFVAL(Variant()), DEFVAL(true), DEFVAL(Array())); - - ClassDB::bind_method(D_METHOD("body_get_direct_state", "body"), &PhysicsServer::body_get_direct_state); - - /* JOINT API */ - - BIND_ENUM_CONSTANT(JOINT_PIN); - BIND_ENUM_CONSTANT(JOINT_HINGE); - BIND_ENUM_CONSTANT(JOINT_SLIDER); - BIND_ENUM_CONSTANT(JOINT_CONE_TWIST); - BIND_ENUM_CONSTANT(JOINT_6DOF); - - ClassDB::bind_method(D_METHOD("joint_create_pin", "body_A", "local_A", "body_B", "local_B"), &PhysicsServer::joint_create_pin); - ClassDB::bind_method(D_METHOD("pin_joint_set_param", "joint", "param", "value"), &PhysicsServer::pin_joint_set_param); - ClassDB::bind_method(D_METHOD("pin_joint_get_param", "joint", "param"), &PhysicsServer::pin_joint_get_param); - - ClassDB::bind_method(D_METHOD("pin_joint_set_local_a", "joint", "local_A"), &PhysicsServer::pin_joint_set_local_a); - ClassDB::bind_method(D_METHOD("pin_joint_get_local_a", "joint"), &PhysicsServer::pin_joint_get_local_a); - - ClassDB::bind_method(D_METHOD("pin_joint_set_local_b", "joint", "local_B"), &PhysicsServer::pin_joint_set_local_b); - ClassDB::bind_method(D_METHOD("pin_joint_get_local_b", "joint"), &PhysicsServer::pin_joint_get_local_b); - - BIND_ENUM_CONSTANT(PIN_JOINT_BIAS); - BIND_ENUM_CONSTANT(PIN_JOINT_DAMPING); - BIND_ENUM_CONSTANT(PIN_JOINT_IMPULSE_CLAMP); - - BIND_ENUM_CONSTANT(HINGE_JOINT_BIAS); - BIND_ENUM_CONSTANT(HINGE_JOINT_LIMIT_UPPER); - BIND_ENUM_CONSTANT(HINGE_JOINT_LIMIT_LOWER); - BIND_ENUM_CONSTANT(HINGE_JOINT_LIMIT_BIAS); - BIND_ENUM_CONSTANT(HINGE_JOINT_LIMIT_SOFTNESS); - BIND_ENUM_CONSTANT(HINGE_JOINT_LIMIT_RELAXATION); - BIND_ENUM_CONSTANT(HINGE_JOINT_MOTOR_TARGET_VELOCITY); - BIND_ENUM_CONSTANT(HINGE_JOINT_MOTOR_MAX_IMPULSE); - - BIND_ENUM_CONSTANT(HINGE_JOINT_FLAG_USE_LIMIT); - BIND_ENUM_CONSTANT(HINGE_JOINT_FLAG_ENABLE_MOTOR); - - ClassDB::bind_method(D_METHOD("joint_create_hinge", "body_A", "hinge_A", "body_B", "hinge_B"), &PhysicsServer::joint_create_hinge); - - ClassDB::bind_method(D_METHOD("hinge_joint_set_param", "joint", "param", "value"), &PhysicsServer::hinge_joint_set_param); - ClassDB::bind_method(D_METHOD("hinge_joint_get_param", "joint", "param"), &PhysicsServer::hinge_joint_get_param); - - ClassDB::bind_method(D_METHOD("hinge_joint_set_flag", "joint", "flag", "enabled"), &PhysicsServer::hinge_joint_set_flag); - ClassDB::bind_method(D_METHOD("hinge_joint_get_flag", "joint", "flag"), &PhysicsServer::hinge_joint_get_flag); - - ClassDB::bind_method(D_METHOD("joint_create_slider", "body_A", "local_ref_A", "body_B", "local_ref_B"), &PhysicsServer::joint_create_slider); - - ClassDB::bind_method(D_METHOD("slider_joint_set_param", "joint", "param", "value"), &PhysicsServer::slider_joint_set_param); - ClassDB::bind_method(D_METHOD("slider_joint_get_param", "joint", "param"), &PhysicsServer::slider_joint_get_param); - - BIND_ENUM_CONSTANT(SLIDER_JOINT_LINEAR_LIMIT_UPPER); - BIND_ENUM_CONSTANT(SLIDER_JOINT_LINEAR_LIMIT_LOWER); - BIND_ENUM_CONSTANT(SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS); - BIND_ENUM_CONSTANT(SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION); - BIND_ENUM_CONSTANT(SLIDER_JOINT_LINEAR_LIMIT_DAMPING); - BIND_ENUM_CONSTANT(SLIDER_JOINT_LINEAR_MOTION_SOFTNESS); - BIND_ENUM_CONSTANT(SLIDER_JOINT_LINEAR_MOTION_RESTITUTION); - BIND_ENUM_CONSTANT(SLIDER_JOINT_LINEAR_MOTION_DAMPING); - BIND_ENUM_CONSTANT(SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS); - BIND_ENUM_CONSTANT(SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION); - BIND_ENUM_CONSTANT(SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING); - - BIND_ENUM_CONSTANT(SLIDER_JOINT_ANGULAR_LIMIT_UPPER); - BIND_ENUM_CONSTANT(SLIDER_JOINT_ANGULAR_LIMIT_LOWER); - BIND_ENUM_CONSTANT(SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS); - BIND_ENUM_CONSTANT(SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION); - BIND_ENUM_CONSTANT(SLIDER_JOINT_ANGULAR_LIMIT_DAMPING); - BIND_ENUM_CONSTANT(SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS); - BIND_ENUM_CONSTANT(SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION); - BIND_ENUM_CONSTANT(SLIDER_JOINT_ANGULAR_MOTION_DAMPING); - BIND_ENUM_CONSTANT(SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS); - BIND_ENUM_CONSTANT(SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION); - BIND_ENUM_CONSTANT(SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING); - BIND_ENUM_CONSTANT(SLIDER_JOINT_MAX); - - ClassDB::bind_method(D_METHOD("joint_create_cone_twist", "body_A", "local_ref_A", "body_B", "local_ref_B"), &PhysicsServer::joint_create_cone_twist); - - ClassDB::bind_method(D_METHOD("cone_twist_joint_set_param", "joint", "param", "value"), &PhysicsServer::cone_twist_joint_set_param); - ClassDB::bind_method(D_METHOD("cone_twist_joint_get_param", "joint", "param"), &PhysicsServer::cone_twist_joint_get_param); - - BIND_ENUM_CONSTANT(CONE_TWIST_JOINT_SWING_SPAN); - BIND_ENUM_CONSTANT(CONE_TWIST_JOINT_TWIST_SPAN); - BIND_ENUM_CONSTANT(CONE_TWIST_JOINT_BIAS); - BIND_ENUM_CONSTANT(CONE_TWIST_JOINT_SOFTNESS); - BIND_ENUM_CONSTANT(CONE_TWIST_JOINT_RELAXATION); - - BIND_ENUM_CONSTANT(G6DOF_JOINT_LINEAR_LOWER_LIMIT); - BIND_ENUM_CONSTANT(G6DOF_JOINT_LINEAR_UPPER_LIMIT); - BIND_ENUM_CONSTANT(G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS); - BIND_ENUM_CONSTANT(G6DOF_JOINT_LINEAR_RESTITUTION); - BIND_ENUM_CONSTANT(G6DOF_JOINT_LINEAR_DAMPING); - BIND_ENUM_CONSTANT(G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY); - BIND_ENUM_CONSTANT(G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT); - BIND_ENUM_CONSTANT(G6DOF_JOINT_ANGULAR_LOWER_LIMIT); - BIND_ENUM_CONSTANT(G6DOF_JOINT_ANGULAR_UPPER_LIMIT); - BIND_ENUM_CONSTANT(G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS); - BIND_ENUM_CONSTANT(G6DOF_JOINT_ANGULAR_DAMPING); - BIND_ENUM_CONSTANT(G6DOF_JOINT_ANGULAR_RESTITUTION); - BIND_ENUM_CONSTANT(G6DOF_JOINT_ANGULAR_FORCE_LIMIT); - BIND_ENUM_CONSTANT(G6DOF_JOINT_ANGULAR_ERP); - BIND_ENUM_CONSTANT(G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY); - BIND_ENUM_CONSTANT(G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT); - - BIND_ENUM_CONSTANT(G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT); - BIND_ENUM_CONSTANT(G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT); - BIND_ENUM_CONSTANT(G6DOF_JOINT_FLAG_ENABLE_MOTOR); - BIND_ENUM_CONSTANT(G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR); - - ClassDB::bind_method(D_METHOD("joint_get_type", "joint"), &PhysicsServer::joint_get_type); - - ClassDB::bind_method(D_METHOD("joint_set_solver_priority", "joint", "priority"), &PhysicsServer::joint_set_solver_priority); - ClassDB::bind_method(D_METHOD("joint_get_solver_priority", "joint"), &PhysicsServer::joint_get_solver_priority); - - ClassDB::bind_method(D_METHOD("joint_create_generic_6dof", "body_A", "local_ref_A", "body_B", "local_ref_B"), &PhysicsServer::joint_create_generic_6dof); - - ClassDB::bind_method(D_METHOD("generic_6dof_joint_set_param", "joint", "axis", "param", "value"), &PhysicsServer::generic_6dof_joint_set_param); - ClassDB::bind_method(D_METHOD("generic_6dof_joint_get_param", "joint", "axis", "param"), &PhysicsServer::generic_6dof_joint_get_param); - - ClassDB::bind_method(D_METHOD("generic_6dof_joint_set_flag", "joint", "axis", "flag", "enable"), &PhysicsServer::generic_6dof_joint_set_flag); - ClassDB::bind_method(D_METHOD("generic_6dof_joint_get_flag", "joint", "axis", "flag"), &PhysicsServer::generic_6dof_joint_get_flag); - - ClassDB::bind_method(D_METHOD("free_rid", "rid"), &PhysicsServer::free); - - ClassDB::bind_method(D_METHOD("set_active", "active"), &PhysicsServer::set_active); - - ClassDB::bind_method(D_METHOD("set_collision_iterations", "iterations"), &PhysicsServer::set_collision_iterations); - - ClassDB::bind_method(D_METHOD("get_process_info", "process_info"), &PhysicsServer::get_process_info); - - BIND_ENUM_CONSTANT(SHAPE_PLANE); - BIND_ENUM_CONSTANT(SHAPE_RAY); - BIND_ENUM_CONSTANT(SHAPE_SPHERE); - BIND_ENUM_CONSTANT(SHAPE_BOX); - BIND_ENUM_CONSTANT(SHAPE_CAPSULE); - BIND_ENUM_CONSTANT(SHAPE_CYLINDER); - BIND_ENUM_CONSTANT(SHAPE_CONVEX_POLYGON); - BIND_ENUM_CONSTANT(SHAPE_CONCAVE_POLYGON); - BIND_ENUM_CONSTANT(SHAPE_HEIGHTMAP); - BIND_ENUM_CONSTANT(SHAPE_CUSTOM); - - BIND_ENUM_CONSTANT(AREA_PARAM_GRAVITY); - BIND_ENUM_CONSTANT(AREA_PARAM_GRAVITY_VECTOR); - BIND_ENUM_CONSTANT(AREA_PARAM_GRAVITY_IS_POINT); - BIND_ENUM_CONSTANT(AREA_PARAM_GRAVITY_DISTANCE_SCALE); - BIND_ENUM_CONSTANT(AREA_PARAM_GRAVITY_POINT_ATTENUATION); - BIND_ENUM_CONSTANT(AREA_PARAM_LINEAR_DAMP); - BIND_ENUM_CONSTANT(AREA_PARAM_ANGULAR_DAMP); - BIND_ENUM_CONSTANT(AREA_PARAM_PRIORITY); - - BIND_ENUM_CONSTANT(AREA_SPACE_OVERRIDE_DISABLED); - BIND_ENUM_CONSTANT(AREA_SPACE_OVERRIDE_COMBINE); - BIND_ENUM_CONSTANT(AREA_SPACE_OVERRIDE_COMBINE_REPLACE); - BIND_ENUM_CONSTANT(AREA_SPACE_OVERRIDE_REPLACE); - BIND_ENUM_CONSTANT(AREA_SPACE_OVERRIDE_REPLACE_COMBINE); - - BIND_ENUM_CONSTANT(BODY_MODE_STATIC); - BIND_ENUM_CONSTANT(BODY_MODE_KINEMATIC); - BIND_ENUM_CONSTANT(BODY_MODE_RIGID); - BIND_ENUM_CONSTANT(BODY_MODE_CHARACTER); - - BIND_ENUM_CONSTANT(BODY_PARAM_BOUNCE); - BIND_ENUM_CONSTANT(BODY_PARAM_FRICTION); - BIND_ENUM_CONSTANT(BODY_PARAM_MASS); - BIND_ENUM_CONSTANT(BODY_PARAM_GRAVITY_SCALE); - BIND_ENUM_CONSTANT(BODY_PARAM_LINEAR_DAMP); - BIND_ENUM_CONSTANT(BODY_PARAM_ANGULAR_DAMP); - BIND_ENUM_CONSTANT(BODY_PARAM_MAX); - - BIND_ENUM_CONSTANT(BODY_STATE_TRANSFORM); - BIND_ENUM_CONSTANT(BODY_STATE_LINEAR_VELOCITY); - BIND_ENUM_CONSTANT(BODY_STATE_ANGULAR_VELOCITY); - BIND_ENUM_CONSTANT(BODY_STATE_SLEEPING); - BIND_ENUM_CONSTANT(BODY_STATE_CAN_SLEEP); - - BIND_ENUM_CONSTANT(AREA_BODY_ADDED); - BIND_ENUM_CONSTANT(AREA_BODY_REMOVED); - - BIND_ENUM_CONSTANT(INFO_ACTIVE_OBJECTS); - BIND_ENUM_CONSTANT(INFO_COLLISION_PAIRS); - BIND_ENUM_CONSTANT(INFO_ISLAND_COUNT); - - BIND_ENUM_CONSTANT(SPACE_PARAM_CONTACT_RECYCLE_RADIUS); - BIND_ENUM_CONSTANT(SPACE_PARAM_CONTACT_MAX_SEPARATION); - BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION); - BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD); - BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD); - BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_TIME_TO_SLEEP); - BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO); - BIND_ENUM_CONSTANT(SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS); - - BIND_ENUM_CONSTANT(BODY_AXIS_LINEAR_X); - BIND_ENUM_CONSTANT(BODY_AXIS_LINEAR_Y); - BIND_ENUM_CONSTANT(BODY_AXIS_LINEAR_Z); - BIND_ENUM_CONSTANT(BODY_AXIS_ANGULAR_X); - BIND_ENUM_CONSTANT(BODY_AXIS_ANGULAR_Y); - BIND_ENUM_CONSTANT(BODY_AXIS_ANGULAR_Z); - -#endif -} - -PhysicsServer::PhysicsServer() { - ERR_FAIL_COND(singleton != nullptr); - singleton = this; -} - -PhysicsServer::~PhysicsServer() { - singleton = nullptr; -} - -Vector PhysicsServerManager::physics_servers; -int PhysicsServerManager::default_server_id = -1; -int PhysicsServerManager::default_server_priority = -1; -const String PhysicsServerManager::setting_property_name("physics/3d/physics_engine"); - -void PhysicsServerManager::on_servers_changed() { - String physics_servers2("DEFAULT"); - for (int i = get_servers_count() - 1; 0 <= i; --i) { - physics_servers2 += "," + get_server_name(i); - } - ProjectSettings::get_singleton()->set_custom_property_info(setting_property_name, PropertyInfo(Variant::STRING, setting_property_name, PROPERTY_HINT_ENUM, physics_servers2)); -} - -void PhysicsServerManager::register_server(const String &p_name, CreatePhysicsServerCallback p_creat_callback) { - ERR_FAIL_COND(!p_creat_callback); - ERR_FAIL_COND(find_server_id(p_name) != -1); - physics_servers.push_back(ClassInfo(p_name, p_creat_callback)); - on_servers_changed(); -} - -void PhysicsServerManager::set_default_server(const String &p_name, int p_priority) { - const int id = find_server_id(p_name); - ERR_FAIL_COND(id == -1); // Not found - if (default_server_priority < p_priority) { - default_server_id = id; - default_server_priority = p_priority; - } -} - -int PhysicsServerManager::find_server_id(const String &p_name) { - for (int i = physics_servers.size() - 1; 0 <= i; --i) { - if (p_name == physics_servers[i].name) { - return i; - } - } - return -1; -} - -int PhysicsServerManager::get_servers_count() { - return physics_servers.size(); -} - -String PhysicsServerManager::get_server_name(int p_id) { - ERR_FAIL_INDEX_V(p_id, get_servers_count(), ""); - return physics_servers[p_id].name; -} - -PhysicsServer *PhysicsServerManager::new_default_server() { - ERR_FAIL_COND_V(default_server_id == -1, nullptr); - return physics_servers[default_server_id].create_callback(); -} - -PhysicsServer *PhysicsServerManager::new_server(const String &p_name) { - int id = find_server_id(p_name); - if (id == -1) { - return nullptr; - } else { - return physics_servers[id].create_callback(); - } -} diff --git a/servers/physics_server.h b/servers/physics_server.h deleted file mode 100644 index 746377a..0000000 --- a/servers/physics_server.h +++ /dev/null @@ -1,842 +0,0 @@ -#ifndef PHYSICS_SERVER_H -#define PHYSICS_SERVER_H -/*************************************************************************/ -/* physics_server.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 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 "core/object/object.h" -#include "core/object/resource.h" - -class PhysicsDirectSpaceState; - -class PhysicsDirectBodyState : public Object { - GDCLASS(PhysicsDirectBodyState, Object); - -protected: - static void _bind_methods(); - -public: - virtual Vector3 get_total_gravity() const = 0; - virtual float get_total_angular_damp() const = 0; - virtual float get_total_linear_damp() const = 0; - - virtual Vector3 get_center_of_mass() const = 0; - virtual Basis get_principal_inertia_axes() const = 0; - virtual float get_inverse_mass() const = 0; // get the mass - virtual Vector3 get_inverse_inertia() const = 0; // get density of this body space - virtual Basis get_inverse_inertia_tensor() const = 0; // get density of this body space - - virtual void set_linear_velocity(const Vector3 &p_velocity) = 0; - virtual Vector3 get_linear_velocity() const = 0; - - virtual void set_angular_velocity(const Vector3 &p_velocity) = 0; - virtual Vector3 get_angular_velocity() const = 0; - - virtual void set_transform(const Transform &p_transform) = 0; - virtual Transform get_transform() const = 0; - - virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const = 0; - - virtual void add_central_force(const Vector3 &p_force) = 0; - virtual void add_force(const Vector3 &p_force, const Vector3 &p_pos) = 0; - virtual void add_torque(const Vector3 &p_torque) = 0; - virtual void apply_central_impulse(const Vector3 &p_j) = 0; - virtual void apply_impulse(const Vector3 &p_pos, const Vector3 &p_j) = 0; - virtual void apply_torque_impulse(const Vector3 &p_j) = 0; - - virtual void set_sleep_state(bool p_enable) = 0; - virtual bool is_sleeping() const = 0; - - virtual int get_contact_count() const = 0; - - virtual Vector3 get_contact_local_position(int p_contact_idx) const = 0; - virtual Vector3 get_contact_local_normal(int p_contact_idx) const = 0; - virtual float get_contact_impulse(int p_contact_idx) const = 0; - virtual int get_contact_local_shape(int p_contact_idx) const = 0; - - virtual RID get_contact_collider(int p_contact_idx) const = 0; - virtual Vector3 get_contact_collider_position(int p_contact_idx) const = 0; - virtual ObjectID get_contact_collider_id(int p_contact_idx) const = 0; - virtual Object *get_contact_collider_object(int p_contact_idx) const; - virtual int get_contact_collider_shape(int p_contact_idx) const = 0; - virtual Vector3 get_contact_collider_velocity_at_position(int p_contact_idx) const = 0; - - virtual real_t get_step() const = 0; - virtual void integrate_forces(); - - virtual PhysicsDirectSpaceState *get_space_state() = 0; - - PhysicsDirectBodyState(); -}; - -class PhysicsShapeQueryParameters : public Reference { - GDCLASS(PhysicsShapeQueryParameters, Reference); - friend class PhysicsDirectSpaceState; - - RID shape; - Transform transform; - float margin; - RBSet exclude; - uint32_t collision_mask; - - bool collide_with_bodies; - bool collide_with_areas; - -protected: - static void _bind_methods(); - -public: - void set_shape(const RES &p_shape); - void set_shape_rid(const RID &p_shape); - RID get_shape_rid() const; - - void set_transform(const Transform &p_transform); - Transform get_transform() const; - - void set_margin(float p_margin); - float get_margin() const; - - void set_collision_mask(uint32_t p_collision_mask); - uint32_t get_collision_mask() const; - - void set_exclude(const Vector &p_exclude); - Vector get_exclude() const; - - void set_collide_with_bodies(bool p_enable); - bool is_collide_with_bodies_enabled() const; - - void set_collide_with_areas(bool p_enable); - bool is_collide_with_areas_enabled() const; - - PhysicsShapeQueryParameters(); -}; - -class PhysicsDirectSpaceState : public Object { - GDCLASS(PhysicsDirectSpaceState, Object); - -private: - Dictionary _intersect_ray(const Vector3 &p_from, const Vector3 &p_to, const Vector &p_exclude = Vector(), uint32_t p_collision_mask = 0, bool p_collide_with_bodies = true, bool p_collide_with_areas = false); - Array _intersect_point(const Vector3 &p_point, int p_max_results = 32, const Vector &p_exclude = Vector(), uint32_t p_layers = 0, bool p_collide_with_bodies = true, bool p_collide_with_areas = false); - Array _intersect_shape(const Ref &p_shape_query, int p_max_results = 32); - Array _cast_motion(const Ref &p_shape_query, const Vector3 &p_motion); - Array _collide_shape(const Ref &p_shape_query, int p_max_results = 32); - Dictionary _get_rest_info(const Ref &p_shape_query); - -protected: - static void _bind_methods(); - -public: - struct ShapeResult { - RID rid; - ObjectID collider_id = 0; - Object *collider = nullptr; - int shape = 0; - }; - - virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const RBSet &p_exclude = RBSet(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) = 0; - - struct RayResult { - Vector3 position; - Vector3 normal; - RID rid; - ObjectID collider_id = 0; - Object *collider = nullptr; - int shape = 0; - }; - - virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const RBSet &p_exclude = RBSet(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_ray = false) = 0; - - virtual int intersect_shape(const RID &p_shape, const Transform &p_xform, float p_margin, ShapeResult *r_results, int p_result_max, const RBSet &p_exclude = RBSet(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) = 0; - - struct ShapeRestInfo { - Vector3 point; - Vector3 normal; - RID rid; - ObjectID collider_id = 0; - int shape = 0; - Vector3 linear_velocity; //velocity at contact point - }; - - virtual bool cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, float p_margin, float &p_closest_safe, float &p_closest_unsafe, const RBSet &p_exclude = RBSet(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, ShapeRestInfo *r_info = nullptr) = 0; - - virtual bool collide_shape(RID p_shape, const Transform &p_shape_xform, float p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const RBSet &p_exclude = RBSet(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) = 0; - - virtual bool rest_info(RID p_shape, const Transform &p_shape_xform, float p_margin, ShapeRestInfo *r_info, const RBSet &p_exclude = RBSet(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) = 0; - - virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const = 0; - - PhysicsDirectSpaceState(); -}; - -class PhysicsTestMotionResult; - -class PhysicsServer : public Object { - GDCLASS(PhysicsServer, Object); - - static PhysicsServer *singleton; - - virtual bool _body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, const Ref &p_result = Ref(), bool p_exclude_raycast_shapes = true, const Vector &p_exclude = Vector()); - -protected: - static void _bind_methods(); - -public: - static PhysicsServer *get_singleton(); - - enum ShapeType { - SHAPE_PLANE, ///< plane:"plane" - SHAPE_RAY, ///< float:"length" - SHAPE_SPHERE, ///< float:"radius" - SHAPE_BOX, ///< vec3:"extents" - SHAPE_CAPSULE, ///< dict( float:"radius", float:"height"):capsule - SHAPE_CYLINDER, ///< dict( float:"radius", float:"height"):cylinder - SHAPE_CONVEX_POLYGON, ///< array of planes:"planes" - SHAPE_CONCAVE_POLYGON, ///< vector3 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector3 array) - SHAPE_HEIGHTMAP, ///< dict( int:"width", int:"depth",float:"cell_size", float_array:"heights" - SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error - }; - - virtual RID shape_create(ShapeType p_shape) = 0; - virtual void shape_set_data(RID p_shape, const Variant &p_data) = 0; - virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias) = 0; - - virtual ShapeType shape_get_type(RID p_shape) const = 0; - virtual Variant shape_get_data(RID p_shape) const = 0; - - virtual void shape_set_margin(RID p_shape, real_t p_margin) = 0; - virtual real_t shape_get_margin(RID p_shape) const = 0; - - virtual real_t shape_get_custom_solver_bias(RID p_shape) const = 0; - - /* SPACE API */ - - virtual RID space_create() = 0; - virtual void space_set_active(RID p_space, bool p_active) = 0; - virtual bool space_is_active(RID p_space) const = 0; - - enum SpaceParameter { - - SPACE_PARAM_CONTACT_RECYCLE_RADIUS, - SPACE_PARAM_CONTACT_MAX_SEPARATION, - SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION, - SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD, - SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD, - SPACE_PARAM_BODY_TIME_TO_SLEEP, - SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO, - SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS, - }; - - virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) = 0; - virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const = 0; - - // this function only works on physics process, errors and returns null otherwise - virtual PhysicsDirectSpaceState *space_get_direct_state(RID p_space) = 0; - - virtual void space_set_debug_contacts(RID p_space, int p_max_contacts) = 0; - virtual Vector space_get_contacts(RID p_space) const = 0; - virtual int space_get_contact_count(RID p_space) const = 0; - - //missing space parameters - - /* AREA API */ - - //missing attenuation? missing better override? - - enum AreaParameter { - AREA_PARAM_GRAVITY, - AREA_PARAM_GRAVITY_VECTOR, - AREA_PARAM_GRAVITY_IS_POINT, - AREA_PARAM_GRAVITY_DISTANCE_SCALE, - AREA_PARAM_GRAVITY_POINT_ATTENUATION, - AREA_PARAM_LINEAR_DAMP, - AREA_PARAM_ANGULAR_DAMP, - AREA_PARAM_PRIORITY - }; - - virtual RID area_create() = 0; - - virtual void area_set_space(RID p_area, RID p_space) = 0; - virtual RID area_get_space(RID p_area) const = 0; - - enum AreaSpaceOverrideMode { - AREA_SPACE_OVERRIDE_DISABLED, - AREA_SPACE_OVERRIDE_COMBINE, - AREA_SPACE_OVERRIDE_COMBINE_REPLACE, - AREA_SPACE_OVERRIDE_REPLACE, - AREA_SPACE_OVERRIDE_REPLACE_COMBINE - }; - - virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) = 0; - virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const = 0; - - virtual void area_add_shape(RID p_area, RID p_shape, const Transform &p_transform = Transform(), bool p_disabled = false) = 0; - virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape) = 0; - virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform) = 0; - - virtual int area_get_shape_count(RID p_area) const = 0; - virtual RID area_get_shape(RID p_area, int p_shape_idx) const = 0; - virtual Transform area_get_shape_transform(RID p_area, int p_shape_idx) const = 0; - - virtual void area_remove_shape(RID p_area, int p_shape_idx) = 0; - virtual void area_clear_shapes(RID p_area) = 0; - - virtual void area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) = 0; - - virtual void area_attach_object_instance_id(RID p_area, ObjectID p_id) = 0; - virtual ObjectID area_get_object_instance_id(RID p_area) const = 0; - - virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) = 0; - virtual void area_set_transform(RID p_area, const Transform &p_transform) = 0; - - virtual Variant area_get_param(RID p_parea, AreaParameter p_param) const = 0; - virtual Transform area_get_transform(RID p_area) const = 0; - - virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) = 0; - virtual void area_set_collision_layer(RID p_area, uint32_t p_layer) = 0; - - virtual void area_set_monitorable(RID p_area, bool p_monitorable) = 0; - - virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) = 0; - virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) = 0; - - virtual void area_set_ray_pickable(RID p_area, bool p_enable) = 0; - virtual bool area_is_ray_pickable(RID p_area) const = 0; - - /* BODY API */ - - //missing ccd? - - enum BodyMode { - BODY_MODE_STATIC, - BODY_MODE_KINEMATIC, - BODY_MODE_RIGID, - BODY_MODE_CHARACTER - }; - - virtual RID body_create(BodyMode p_mode = BODY_MODE_RIGID, bool p_init_sleeping = false) = 0; - - virtual void body_set_space(RID p_body, RID p_space) = 0; - virtual RID body_get_space(RID p_body) const = 0; - - virtual void body_set_mode(RID p_body, BodyMode p_mode) = 0; - virtual BodyMode body_get_mode(RID p_body) const = 0; - - virtual void body_add_shape(RID p_body, RID p_shape, const Transform &p_transform = Transform(), bool p_disabled = false) = 0; - virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape) = 0; - virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform) = 0; - - virtual int body_get_shape_count(RID p_body) const = 0; - virtual RID body_get_shape(RID p_body, int p_shape_idx) const = 0; - virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const = 0; - - virtual void body_remove_shape(RID p_body, int p_shape_idx) = 0; - virtual void body_clear_shapes(RID p_body) = 0; - - virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) = 0; - - virtual void body_attach_object_instance_id(RID p_body, uint32_t p_id) = 0; - virtual uint32_t body_get_object_instance_id(RID p_body) const = 0; - - virtual void body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) = 0; - virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const = 0; - - virtual void body_set_collision_layer(RID p_body, uint32_t p_layer) = 0; - virtual uint32_t body_get_collision_layer(RID p_body) const = 0; - - virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) = 0; - virtual uint32_t body_get_collision_mask(RID p_body) const = 0; - - virtual void body_set_user_flags(RID p_body, uint32_t p_flags) = 0; - virtual uint32_t body_get_user_flags(RID p_body) const = 0; - - // common body variables - enum BodyParameter { - BODY_PARAM_BOUNCE, - BODY_PARAM_FRICTION, - BODY_PARAM_MASS, ///< unused for static, always infinite - BODY_PARAM_GRAVITY_SCALE, - BODY_PARAM_LINEAR_DAMP, - BODY_PARAM_ANGULAR_DAMP, - BODY_PARAM_MAX, - }; - - virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value) = 0; - virtual float body_get_param(RID p_body, BodyParameter p_param) const = 0; - - virtual void body_set_kinematic_safe_margin(RID p_body, real_t p_margin) = 0; - virtual real_t body_get_kinematic_safe_margin(RID p_body) const = 0; - - //state - enum BodyState { - BODY_STATE_TRANSFORM, - BODY_STATE_LINEAR_VELOCITY, - BODY_STATE_ANGULAR_VELOCITY, - BODY_STATE_SLEEPING, - BODY_STATE_CAN_SLEEP - }; - - virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) = 0; - virtual Variant body_get_state(RID p_body, BodyState p_state) const = 0; - - //do something about it - virtual void body_set_applied_force(RID p_body, const Vector3 &p_force) = 0; - virtual Vector3 body_get_applied_force(RID p_body) const = 0; - - virtual void body_set_applied_torque(RID p_body, const Vector3 &p_torque) = 0; - virtual Vector3 body_get_applied_torque(RID p_body) const = 0; - - virtual void body_add_central_force(RID p_body, const Vector3 &p_force) = 0; - virtual void body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_pos) = 0; - virtual void body_add_torque(RID p_body, const Vector3 &p_torque) = 0; - - virtual void body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) = 0; - virtual void body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse) = 0; - virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) = 0; - virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) = 0; - - enum BodyAxis { - BODY_AXIS_LINEAR_X = 1 << 0, - BODY_AXIS_LINEAR_Y = 1 << 1, - BODY_AXIS_LINEAR_Z = 1 << 2, - BODY_AXIS_ANGULAR_X = 1 << 3, - BODY_AXIS_ANGULAR_Y = 1 << 4, - BODY_AXIS_ANGULAR_Z = 1 << 5 - }; - - virtual void body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) = 0; - virtual bool body_is_axis_locked(RID p_body, BodyAxis p_axis) const = 0; - - //fix - virtual void body_add_collision_exception(RID p_body, RID p_body_b) = 0; - virtual void body_remove_collision_exception(RID p_body, RID p_body_b) = 0; - virtual void body_get_collision_exceptions(RID p_body, List *p_exceptions) = 0; - - virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) = 0; - virtual int body_get_max_contacts_reported(RID p_body) const = 0; - - //missing remove - virtual void body_set_contacts_reported_depth_threshold(RID p_body, float p_threshold) = 0; - virtual float body_get_contacts_reported_depth_threshold(RID p_body) const = 0; - - virtual void body_set_omit_force_integration(RID p_body, bool p_omit) = 0; - virtual bool body_is_omitting_force_integration(RID p_body) const = 0; - - virtual void body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant()) = 0; - - virtual void body_set_ray_pickable(RID p_body, bool p_enable) = 0; - virtual bool body_is_ray_pickable(RID p_body) const = 0; - - // this function only works on physics process, errors and returns null otherwise - virtual PhysicsDirectBodyState *body_get_direct_state(RID p_body) = 0; - - struct MotionResult { - Vector3 motion; - Vector3 remainder; - - Vector3 collision_point; - Vector3 collision_normal; - Vector3 collider_velocity; - real_t collision_depth = 0.0; - real_t collision_safe_fraction = 0.0; - real_t collision_unsafe_fraction = 0.0; - int collision_local_shape = 0; - ObjectID collider_id = 0; - RID collider; - int collider_shape = 0; - Variant collider_metadata; - }; - - virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true, const RBSet &p_exclude = RBSet()) = 0; - - struct SeparationResult { - float collision_depth; - Vector3 collision_point; - Vector3 collision_normal; - Vector3 collider_velocity; - int collision_local_shape; - ObjectID collider_id; - RID collider; - int collider_shape; - Variant collider_metadata; - }; - - virtual int body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin = 0.001) = 0; - - /* SOFT BODY */ - - virtual RID soft_body_create(bool p_init_sleeping = false) = 0; - - virtual void soft_body_update_rendering_server(RID p_body, class SoftBodyRenderingServerHandler *p_rendering_server_handler) = 0; - - virtual void soft_body_set_space(RID p_body, RID p_space) = 0; - virtual RID soft_body_get_space(RID p_body) const = 0; - - virtual void soft_body_set_mesh(RID p_body, const REF &p_mesh) = 0; - - virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer) = 0; - virtual uint32_t soft_body_get_collision_layer(RID p_body) const = 0; - - virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask) = 0; - virtual uint32_t soft_body_get_collision_mask(RID p_body) const = 0; - - virtual void soft_body_add_collision_exception(RID p_body, RID p_body_b) = 0; - virtual void soft_body_remove_collision_exception(RID p_body, RID p_body_b) = 0; - virtual void soft_body_get_collision_exceptions(RID p_body, List *p_exceptions) = 0; - - virtual void soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) = 0; - virtual Variant soft_body_get_state(RID p_body, BodyState p_state) const = 0; - - virtual void soft_body_set_transform(RID p_body, const Transform &p_transform) = 0; - virtual Vector3 soft_body_get_vertex_position(RID p_body, int vertex_index) const = 0; - - virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) = 0; - virtual bool soft_body_is_ray_pickable(RID p_body) const = 0; - - virtual void soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) = 0; - virtual int soft_body_get_simulation_precision(RID p_body) = 0; - - virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass) = 0; - virtual real_t soft_body_get_total_mass(RID p_body) = 0; - - virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) = 0; - virtual real_t soft_body_get_linear_stiffness(RID p_body) = 0; - - virtual void soft_body_set_areaAngular_stiffness(RID p_body, real_t p_stiffness) = 0; - virtual real_t soft_body_get_areaAngular_stiffness(RID p_body) = 0; - - virtual void soft_body_set_volume_stiffness(RID p_body, real_t p_stiffness) = 0; - virtual real_t soft_body_get_volume_stiffness(RID p_body) = 0; - - virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) = 0; - virtual real_t soft_body_get_pressure_coefficient(RID p_body) = 0; - - virtual void soft_body_set_pose_matching_coefficient(RID p_body, real_t p_pose_matching_coefficient) = 0; - virtual real_t soft_body_get_pose_matching_coefficient(RID p_body) = 0; - - virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) = 0; - virtual real_t soft_body_get_damping_coefficient(RID p_body) = 0; - - virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) = 0; - virtual real_t soft_body_get_drag_coefficient(RID p_body) = 0; - - virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) = 0; - virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) = 0; - - virtual Vector3 soft_body_get_point_offset(RID p_body, int p_point_index) const = 0; - - virtual void soft_body_remove_all_pinned_points(RID p_body) = 0; - virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) = 0; - virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) = 0; - - /* JOINT API */ - - enum JointType { - - JOINT_PIN, - JOINT_HINGE, - JOINT_SLIDER, - JOINT_CONE_TWIST, - JOINT_6DOF - - }; - - virtual JointType joint_get_type(RID p_joint) const = 0; - - virtual void joint_set_solver_priority(RID p_joint, int p_priority) = 0; - virtual int joint_get_solver_priority(RID p_joint) const = 0; - - virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) = 0; - virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const = 0; - - virtual RID joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) = 0; - - enum PinJointParam { - PIN_JOINT_BIAS, - PIN_JOINT_DAMPING, - PIN_JOINT_IMPULSE_CLAMP - }; - - virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, float p_value) = 0; - virtual float pin_joint_get_param(RID p_joint, PinJointParam p_param) const = 0; - - virtual void pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) = 0; - virtual Vector3 pin_joint_get_local_a(RID p_joint) const = 0; - - virtual void pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) = 0; - virtual Vector3 pin_joint_get_local_b(RID p_joint) const = 0; - - enum HingeJointParam { - - HINGE_JOINT_BIAS, - HINGE_JOINT_LIMIT_UPPER, - HINGE_JOINT_LIMIT_LOWER, - HINGE_JOINT_LIMIT_BIAS, - HINGE_JOINT_LIMIT_SOFTNESS, - HINGE_JOINT_LIMIT_RELAXATION, - HINGE_JOINT_MOTOR_TARGET_VELOCITY, - HINGE_JOINT_MOTOR_MAX_IMPULSE, - HINGE_JOINT_MAX - }; - - enum HingeJointFlag { - HINGE_JOINT_FLAG_USE_LIMIT, - HINGE_JOINT_FLAG_ENABLE_MOTOR, - HINGE_JOINT_FLAG_MAX - }; - - virtual RID joint_create_hinge(RID p_body_A, const Transform &p_hinge_A, RID p_body_B, const Transform &p_hinge_B) = 0; - virtual RID joint_create_hinge_simple(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) = 0; - - virtual void hinge_joint_set_param(RID p_joint, HingeJointParam p_param, float p_value) = 0; - virtual float hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const = 0; - - virtual void hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) = 0; - virtual bool hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const = 0; - - enum SliderJointParam { - SLIDER_JOINT_LINEAR_LIMIT_UPPER, - SLIDER_JOINT_LINEAR_LIMIT_LOWER, - SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS, - SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION, - SLIDER_JOINT_LINEAR_LIMIT_DAMPING, - SLIDER_JOINT_LINEAR_MOTION_SOFTNESS, - SLIDER_JOINT_LINEAR_MOTION_RESTITUTION, - SLIDER_JOINT_LINEAR_MOTION_DAMPING, - SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS, - SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION, - SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING, - - SLIDER_JOINT_ANGULAR_LIMIT_UPPER, - SLIDER_JOINT_ANGULAR_LIMIT_LOWER, - SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS, - SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION, - SLIDER_JOINT_ANGULAR_LIMIT_DAMPING, - SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS, - SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION, - SLIDER_JOINT_ANGULAR_MOTION_DAMPING, - SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS, - SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION, - SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING, - SLIDER_JOINT_MAX - - }; - - virtual RID joint_create_slider(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) = 0; //reference frame is A - - virtual void slider_joint_set_param(RID p_joint, SliderJointParam p_param, float p_value) = 0; - virtual float slider_joint_get_param(RID p_joint, SliderJointParam p_param) const = 0; - - enum ConeTwistJointParam { - CONE_TWIST_JOINT_SWING_SPAN, - CONE_TWIST_JOINT_TWIST_SPAN, - CONE_TWIST_JOINT_BIAS, - CONE_TWIST_JOINT_SOFTNESS, - CONE_TWIST_JOINT_RELAXATION, - CONE_TWIST_MAX - }; - - virtual RID joint_create_cone_twist(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) = 0; //reference frame is A - - virtual void cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, float p_value) = 0; - virtual float cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const = 0; - - enum G6DOFJointAxisParam { - G6DOF_JOINT_LINEAR_LOWER_LIMIT, - G6DOF_JOINT_LINEAR_UPPER_LIMIT, - G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS, - G6DOF_JOINT_LINEAR_RESTITUTION, - G6DOF_JOINT_LINEAR_DAMPING, - G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY, - G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT, - G6DOF_JOINT_LINEAR_SPRING_STIFFNESS, - G6DOF_JOINT_LINEAR_SPRING_DAMPING, - G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT, - G6DOF_JOINT_ANGULAR_LOWER_LIMIT, - G6DOF_JOINT_ANGULAR_UPPER_LIMIT, - G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS, - G6DOF_JOINT_ANGULAR_DAMPING, - G6DOF_JOINT_ANGULAR_RESTITUTION, - G6DOF_JOINT_ANGULAR_FORCE_LIMIT, - G6DOF_JOINT_ANGULAR_ERP, - G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY, - G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT, - G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS, - G6DOF_JOINT_ANGULAR_SPRING_DAMPING, - G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT, - G6DOF_JOINT_MAX - }; - - enum G6DOFJointAxisFlag { - - G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT, - G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT, - G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING, - G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING, - G6DOF_JOINT_FLAG_ENABLE_MOTOR, - G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR, - G6DOF_JOINT_FLAG_MAX - }; - - virtual RID joint_create_generic_6dof(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) = 0; //reference frame is A - - virtual void generic_6dof_joint_set_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param, float p_value) = 0; - virtual float generic_6dof_joint_get_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param) = 0; - - virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag, bool p_enable) = 0; - virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag) = 0; - - /* QUERY API */ - - enum AreaBodyStatus { - AREA_BODY_ADDED, - AREA_BODY_REMOVED - }; - - /* MISC */ - - virtual void free(RID p_rid) = 0; - - virtual void set_active(bool p_active) = 0; - virtual void init() = 0; - virtual void step(float p_step) = 0; - virtual void flush_queries() = 0; - virtual void finish() = 0; - - virtual bool is_flushing_queries() const = 0; - - virtual void set_collision_iterations(int p_iterations) = 0; - - enum ProcessInfo { - - INFO_ACTIVE_OBJECTS, - INFO_COLLISION_PAIRS, - INFO_ISLAND_COUNT - }; - - virtual int get_process_info(ProcessInfo p_info) = 0; - - PhysicsServer(); - ~PhysicsServer(); -}; - -class PhysicsTestMotionResult : public Reference { - GDCLASS(PhysicsTestMotionResult, Reference); - - PhysicsServer::MotionResult result; - bool colliding = false; - friend class PhysicsServer; - -protected: - static void _bind_methods(); - -public: - PhysicsServer::MotionResult *get_result_ptr() const { return const_cast(&result); } - - //bool is_colliding() const; - Vector3 get_motion() const; - Vector3 get_motion_remainder() const; - - Vector3 get_collision_point() const; - Vector3 get_collision_normal() const; - Vector3 get_collider_velocity() const; - ObjectID get_collider_id() const; - RID get_collider_rid() const; - Object *get_collider() const; - int get_collider_shape() const; - real_t get_collision_depth() const; - real_t get_collision_safe_fraction() const; - real_t get_collision_unsafe_fraction() const; -}; - -typedef PhysicsServer *(*CreatePhysicsServerCallback)(); - -class PhysicsServerManager { - struct ClassInfo { - String name; - CreatePhysicsServerCallback create_callback; - - ClassInfo() : - name(""), - create_callback(nullptr) {} - - ClassInfo(String p_name, CreatePhysicsServerCallback p_create_callback) : - name(p_name), - create_callback(p_create_callback) {} - - ClassInfo(const ClassInfo &p_ci) : - name(p_ci.name), - create_callback(p_ci.create_callback) {} - - ClassInfo operator=(const ClassInfo &p_ci) { - name = p_ci.name; - create_callback = p_ci.create_callback; - return *this; - } - }; - - static Vector physics_servers; - static int default_server_id; - static int default_server_priority; - -public: - static const String setting_property_name; - -private: - static void on_servers_changed(); - -public: - static void register_server(const String &p_name, CreatePhysicsServerCallback p_creat_callback); - static void set_default_server(const String &p_name, int p_priority = 0); - static int find_server_id(const String &p_name); - static int get_servers_count(); - static String get_server_name(int p_id); - static PhysicsServer *new_default_server(); - static PhysicsServer *new_server(const String &p_name); -}; - -VARIANT_ENUM_CAST(PhysicsServer::ShapeType); -VARIANT_ENUM_CAST(PhysicsServer::SpaceParameter); -VARIANT_ENUM_CAST(PhysicsServer::AreaParameter); -VARIANT_ENUM_CAST(PhysicsServer::AreaSpaceOverrideMode); -VARIANT_ENUM_CAST(PhysicsServer::BodyMode); -VARIANT_ENUM_CAST(PhysicsServer::BodyParameter); -VARIANT_ENUM_CAST(PhysicsServer::BodyState); -VARIANT_ENUM_CAST(PhysicsServer::BodyAxis); -VARIANT_ENUM_CAST(PhysicsServer::PinJointParam); -VARIANT_ENUM_CAST(PhysicsServer::JointType); -VARIANT_ENUM_CAST(PhysicsServer::HingeJointParam); -VARIANT_ENUM_CAST(PhysicsServer::HingeJointFlag); -VARIANT_ENUM_CAST(PhysicsServer::SliderJointParam); -VARIANT_ENUM_CAST(PhysicsServer::ConeTwistJointParam); -VARIANT_ENUM_CAST(PhysicsServer::G6DOFJointAxisParam); -VARIANT_ENUM_CAST(PhysicsServer::G6DOFJointAxisFlag); -VARIANT_ENUM_CAST(PhysicsServer::AreaBodyStatus); -VARIANT_ENUM_CAST(PhysicsServer::ProcessInfo); - -#endif diff --git a/servers/register_server_types.cpp b/servers/register_server_types.cpp index 249f74b..4a0d8f8 100644 --- a/servers/register_server_types.cpp +++ b/servers/register_server_types.cpp @@ -59,13 +59,11 @@ #include "navigation/navigation_path_query_result_3d.h" #include "navigation_2d_server.h" #include "navigation_server.h" -#include "physics/physics_server_sw.h" #include "physics_2d/physics_2d_server_sw.h" #include "physics_2d/physics_2d_server_wrap_mt.h" #include "rendering/shader_types.h" #include "scene/debugger/script_debugger_remote.h" #include "servers/physics_2d_server.h" -#include "servers/physics_server.h" #include "servers/rendering_server.h" static void _debugger_get_resource_usage(List *r_usage) { @@ -89,10 +87,6 @@ static void _debugger_get_resource_usage(List(); } @@ -114,7 +108,6 @@ void register_server_types() { ClassDB::register_class(); - ClassDB::register_virtual_class(); ClassDB::register_virtual_class(); ClassDB::register_virtual_class(); @@ -185,11 +178,6 @@ void register_server_types() { ClassDB::register_class(); ClassDB::register_class(); - ClassDB::register_class(); - ClassDB::register_virtual_class(); - ClassDB::register_virtual_class(); - ClassDB::register_class(); - ScriptDebuggerRemote::resource_usage_func = _debugger_get_resource_usage; // Physics 2D @@ -199,13 +187,6 @@ void register_server_types() { Physics2DServerManager::register_server("PandemoniumPhysics", &_createPandemoniumPhysics2DCallback); Physics2DServerManager::set_default_server("PandemoniumPhysics"); - // Physics 3D - GLOBAL_DEF(PhysicsServerManager::setting_property_name, "DEFAULT"); - ProjectSettings::get_singleton()->set_custom_property_info(PhysicsServerManager::setting_property_name, PropertyInfo(Variant::STRING, PhysicsServerManager::setting_property_name, PROPERTY_HINT_ENUM, "DEFAULT")); - - PhysicsServerManager::register_server("PandemoniumPhysics", &_createPandemoniumPhysicsCallback); - PhysicsServerManager::set_default_server("PandemoniumPhysics"); - // Navigation GLOBAL_DEF(NavigationMeshGeneratorManager::setting_property_name, "DEFAULT"); ProjectSettings::get_singleton()->set_custom_property_info(NavigationMeshGeneratorManager::setting_property_name, PropertyInfo(Variant::STRING, NavigationMeshGeneratorManager::setting_property_name, PROPERTY_HINT_ENUM, "DEFAULT")); @@ -225,7 +206,6 @@ void unregister_server_types() { void register_server_singletons() { Engine::get_singleton()->add_singleton(Engine::Singleton("RenderingServer", RenderingServer::get_singleton())); - Engine::get_singleton()->add_singleton(Engine::Singleton("PhysicsServer", PhysicsServer::get_singleton())); Engine::get_singleton()->add_singleton(Engine::Singleton("Physics2DServer", Physics2DServer::get_singleton())); Engine::get_singleton()->add_singleton(Engine::Singleton("AudioServer", AudioServer::get_singleton())); Engine::get_singleton()->add_singleton(Engine::Singleton("NavigationServer", NavigationServer::get_singleton()));