Relintai/pandemonium_engine_minimal
1
0
Fork 0
mirror of https://github.com/Relintai/pandemonium_engine_minimal.git synced 2025-03-23 19:57:33 +01:00
Code Issues Packages Projects Releases Wiki Activity

Removed PhysicsServer.

Browse Source
This commit is contained in:
Relintai 2023-12-15 09:24:57 +01:00
parent c809e0d588
commit 8adb27eb0f
117 changed files with 20 additions and 33235 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
main/main.cpp
View File

@ -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);

16
main/performance.cpp
View File

@ -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,

3
main/performance.h
View File

@ -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,

188
modules/navigation_geometry_parsers/geometry_parser_3d/staticbody3d_navigation_geometry_parser_3d.cpp
View File

@ -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<StaticBody>(p_node) != nullptr);
}
void StaticBody3DNavigationGeometryParser3D::parse_geometry(Node *p_node, Ref<NavigationMesh> p_navigationmesh, Ref<NavigationMeshSourceGeometryData3D> 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<StaticBody>(p_node) && parsed_geometry_type != NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES) {
StaticBody *static_body = Object::cast_to<StaticBody>(p_node);
if (static_body->get_collision_layer() & navigationmesh_collision_mask) {
List<uint32_t> shape_owners;
static_body->get_shape_owners(&shape_owners);
for (List<uint32_t>::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<Shape> 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<BoxShape>(*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<CapsuleShape>(*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<CylinderShape>(*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<SphereShape>(*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<ConcavePolygonShape>(*s);
if (concave_polygon) {
p_source_geometry->add_faces(concave_polygon->get_faces(), transform);
}
ConvexPolygonShape *convex_polygon = Object::cast_to<ConvexPolygonShape>(*s);
if (convex_polygon) {
Vector<Vector3> 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<HeightMapShape>(*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<real_t> &map_data = heightmap_shape->get_map_data();
Vector2 heightmap_gridsize(heightmap_width - 1, heightmap_depth - 1);
Vector2 start = heightmap_gridsize * -0.5;
PoolVector<Vector3> 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);
}
}
}
}
}
}
}
}

43
modules/navigation_geometry_parsers/geometry_parser_3d/staticbody3d_navigation_geometry_parser_3d.h
View File

@ -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<NavigationMesh> p_navigationmesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry) ;
};
#endif // STATICBODY3D_NAVIGATION_GEOMETRY_PARSER_3D_H

2
modules/navigation_geometry_parsers/register_types.cpp
View File

@ -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
}
}

644
scene/3d/area.cpp
View File

@ -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<Node>(obj);
ERR_FAIL_COND(!node);
RBMap<ObjectID, BodyState>::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<Node>(obj);
ERR_FAIL_COND(!node);
RBMap<ObjectID, BodyState>::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<Node>(obj);
RBMap<ObjectID, BodyState>::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<ObjectID, BodyState> bmcopy = body_map;
body_map.clear();
//disconnect all monitored stuff
for (RBMap<ObjectID, BodyState>::Element *E = bmcopy.front(); E; E = E->next()) {
Object *obj = ObjectDB::get_instance(E->key());
Node *node = Object::cast_to<Node>(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<ObjectID, AreaState> bmcopy = area_map;
area_map.clear();
//disconnect all monitored stuff
for (RBMap<ObjectID, AreaState>::Element *E = bmcopy.front(); E; E = E->next()) {
Object *obj = ObjectDB::get_instance(E->key());
Node *node = Object::cast_to<Node>(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<Node>(obj);
ERR_FAIL_COND(!node);
RBMap<ObjectID, AreaState>::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<Node>(obj);
ERR_FAIL_COND(!node);
RBMap<ObjectID, AreaState>::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<Node>(obj);
RBMap<ObjectID, AreaState>::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<ObjectID, BodyState>::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<ObjectID, AreaState>::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<ObjectID, AreaState>::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<ObjectID, BodyState>::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() {
}

201
scene/3d/area.h
View File

@ -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<ShapePair> shapes;
};
RBMap<ObjectID, BodyState> 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<AreaShapePair> shapes;
};
RBMap<ObjectID, AreaState> 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

277
scene/3d/audio_stream_player_3d.cpp
View File

@ -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<World3D> 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<RID>(), area_mask, false, true);
Area *area = nullptr;
for (int i = 0; i < areas; i++) {
if (!sr[i].collider) {
continue;
}
Area *tarea = Object::cast_to<Area>(sr[i].collider);
if (!tarea) {
continue;
}
if (!tarea->is_overriding_audio_bus() && !tarea->is_using_reverb_bus()) {
continue;
}
area = tarea;
break;
}
List<Camera *> cameras;
world->get_camera_list(&cameras);
for (List<Camera *>::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");
}
}

213
scene/3d/camera.cpp
View File

@ -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<Spatial>(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<Vector3> 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<CollisionObject>(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<CollisionObject>(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);
}

58
scene/3d/camera.h
View File

@ -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<RID> exclude;
Vector<Vector3> 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

593
scene/3d/collision_object.cpp
View File

@ -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<uint32_t, ShapeData>::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<World3D> 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<InputEvent> &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<Shape> &p_shape) {
for (RBMap<uint32_t, ShapeData>::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> 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<uint32_t>::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> 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<uint32_t, ShapeData>::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<uint32_t, ShapeData>::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<uint32_t> *r_owners) {
for (RBMap<uint32_t, ShapeData>::Element *E = shapes.front(); E; E = E->next()) {
r_owners->push_back(E->key());
}
}
Array CollisionObject::_get_shape_owners() {
Array ret;
for (RBMap<uint32_t, ShapeData>::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<Shape> &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<Shape> CollisionObject::shape_owner_get_shape(uint32_t p_owner, int p_shape) const {
ERR_FAIL_COND_V(!shapes.has(p_owner), Ref<Shape>());
ERR_FAIL_INDEX_V(p_shape, shapes[p_owner].shapes.size(), Ref<Shape>());
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<uint32_t, ShapeData>::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<uint32_t, ShapeData>::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);
}

148
scene/3d/collision_object.h
View File

@ -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> shape;
int index;
};
Vector<ShapeBase> shapes;
bool disabled;
ShapeData() {
disabled = false;
owner_id = 0;
}
};
int total_subshapes;
RBMap<uint32_t, ShapeData> shapes;
bool only_update_transform_changes = false; //this is used for sync physics in KinematicBody
bool capture_input_on_drag;
bool ray_pickable;
RBSet<uint32_t> 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<Shape> &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<InputEvent> &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<uint32_t> *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<Shape> &p_shape);
int shape_owner_get_shape_count(uint32_t p_owner) const;
Ref<Shape> 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

221
scene/3d/collision_polygon.cpp
View File

@ -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<Vector<Vector2>> 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<ConvexPolygonShape> convex = memnew(ConvexPolygonShape);
PoolVector<Vector3> cp;
int cs = decomp[i].size();
cp.resize(cs * 2);
{
PoolVector<Vector3>::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<CollisionObject>(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<Point2> &p_polygon) {
polygon = p_polygon;
if (parent) {
_build_polygon();
}
update_configuration_warning();
update_gizmos();
}
Vector<Point2> 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<CollisionObject>(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;
}

83
scene/3d/collision_polygon.h
View File

@ -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<Point2> 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<Point2> &p_polygon);
Vector<Point2> 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

227
scene/3d/collision_shape.cpp
View File

@ -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<MeshInstance>(n);
if (mi) {
Ref<Mesh> m = mi->get_mesh();
if (m.is_valid()) {
Ref<Shape> 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<CollisionObject>(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<CollisionObject>(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<RigidBody>(get_parent()) &&
Object::cast_to<ConcavePolygonShape>(*shape) &&
Object::cast_to<RigidBody>(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<CollisionShape *>(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<Shape> &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<Shape> 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);
}

77
scene/3d/collision_shape.h
View File

@ -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> 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<Shape> &p_shape);
Ref<Shape> 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

601
scene/3d/mesh_instance.cpp
View File

@ -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<Mesh> &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<Mesh> MeshInstance::get_mesh() const {
return mesh;
}
#ifdef MODULE_SKELETON_3D_ENABLED
void MeshInstance::_resolve_skeleton_path() {
Ref<SkinReference> new_skin_reference;
if (!skeleton_path.is_empty()) {
Skeleton *skeleton = Object::cast_to<Skeleton>(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<ArrayMesh> 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<Material> mat = get_active_material(surface_index);
if (mat.is_valid()) {
Ref<SpatialMaterial> 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> 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<Mesh> 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<Skin> &p_skin) {
skin_internal = p_skin;
skin = p_skin;
if (!is_inside_tree()) {
return;
}
_resolve_skeleton_path();
}
Ref<Skin> 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<Face3> MeshInstance::get_faces(uint32_t p_usage_flags) const {
}
Node *MeshInstance::create_trimesh_collision_node() {
if (mesh.is_null()) {
return nullptr;
}
Ref<Shape> 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<StaticBody>(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<CollisionShape>(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<Ref<Shape>> 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<StaticBody>(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<CollisionShape>(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> 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<StaticBody>(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<CollisionShape>(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<Material> &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<Material> MeshInstance::get_surface_material(int p_surface) const {
@ -757,12 +236,6 @@ void MeshInstance::set_material_override(const Ref<Material> &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<Material> &p_material) {
@ -773,37 +246,9 @@ void MeshInstance::set_material_overlay(const Ref<Material> &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
}

57
scene/3d/mesh_instance.h
View File

@ -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> mesh;
#ifdef MODULE_SKELETON_3D_ENABLED
Ref<Skin> skin;
Ref<Skin> skin_internal;
Ref<SkinReference> 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> mesh_instance;
LocalVector<SurfaceData> 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<MeshInstance *> p_list, bool p_use_global_space, bool p_check_compatibility);
@ -129,14 +80,6 @@ public:
void set_mesh(const Ref<Mesh> &p_mesh);
Ref<Mesh> get_mesh() const;
#ifdef MODULE_SKELETON_3D_ENABLED
void set_skin(const Ref<Skin> &p_skin);
Ref<Skin> 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<Material> &p_material);
Ref<Material> get_surface_material(int p_surface) const;

609
scene/3d/navigation_obstacle.cpp
View File

@ -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<Navigation>(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<Navigation>(p_nav);
ERR_FAIL_NULL(nav);
set_navigation(nav);
}
Node *NavigationObstacle::get_navigation_node() const {
return Object::cast_to<Node>(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<Vector3> &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<ArrayMesh>(memnew(ArrayMesh));
}
fake_agent_radius_debug_mesh->clear_surfaces();
Vector<Vector3> face_vertex_array;
Vector<int> 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<SpatialMaterial> 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<ArrayMesh>(memnew(ArrayMesh));
}
static_obstacle_debug_mesh->clear_surfaces();
Vector<Vector2> 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<int> 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<Vector3> face_vertex_array;
Vector<int> 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<Vector3> 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<SpatialMaterial> face_material;
Ref<SpatialMaterial> 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

132
scene/3d/navigation_obstacle.h
View File

@ -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<Vector3> 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<ArrayMesh> fake_agent_radius_debug_mesh;
RID static_obstacle_debug_instance;
Ref<ArrayMesh> 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<Vector3> &p_vertices);
Vector<Vector3> 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

1624
scene/3d/physics_body.cpp
View File

File diff suppressed because it is too large Load Diff

386
scene/3d/physics_body.h
View File

@ -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<PhysicsMaterial> 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<PhysicsMaterial> &p_physics_material_override);
Ref<PhysicsMaterial> 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<PhysicsMaterial> 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<ShapePair> shapes;
};
struct ContactMonitor {
bool locked;
RBMap<ObjectID, BodyState> 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<PhysicsMaterial> &p_physics_material_override);
Ref<PhysicsMaterial> 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<Collision> colliders;
Vector<Ref<KinematicCollision>> slide_colliders;
Ref<KinematicCollision> motion_cache;
Ref<KinematicCollision> _move(const Vector3 &p_motion, bool p_infinite_inertia = true, bool p_exclude_raycast_shapes = true, bool p_test_only = false);
Ref<KinematicCollision> _get_slide_collision(int p_bounce);
Ref<KinematicCollision> _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<RID> &p_exclude = RBSet<RID>());
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

1079
scene/3d/physics_joint.cpp
View File

File diff suppressed because it is too large Load Diff

336
scene/3d/physics_joint.h
View File

@ -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

526
scene/3d/ray_cast.cpp
View File

@ -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<CollisionObject>(get_parent())) {
if (exclude_parent_body) {
exclude.insert(Object::cast_to<CollisionObject>(get_parent())->get_rid());
} else {
exclude.erase(Object::cast_to<CollisionObject>(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<CollisionObject>(get_parent())) {
if (exclude_parent_body) {
exclude.insert(Object::cast_to<CollisionObject>(get_parent())->get_rid());
} else {
exclude.erase(Object::cast_to<CollisionObject>(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<World3D> 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<CollisionObject>(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<CollisionObject>(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<CollisionObject>(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<Vector3> &RayCast::get_debug_shape_vertices() const {
return debug_shape_vertices;
}
const Vector<Vector3> &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<SpatialMaterial> 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<ArrayMesh> 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<SpatialMaterial> 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<SpatialMaterial> material = static_cast<Ref<SpatialMaterial>>(debug_material);
material->set_albedo(color);
}
void RayCast::_update_debug_shape() {
if (!enabled) {
return;
}
if (!debug_shape) {
_create_debug_shape();
}
MeshInstance *mi = static_cast<MeshInstance *>(debug_shape);
Ref<ArrayMesh> 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<MeshInstance *>(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() {
}

124
scene/3d/ray_cast.h
View File

@ -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<RID> exclude;
uint32_t collision_mask;
bool exclude_parent_body;
Node *debug_shape;
Ref<Material> debug_material;
Color debug_shape_custom_color = Color(0.0, 0.0, 0.0);
int debug_shape_thickness = 2;
Vector<Vector3> debug_shape_vertices;
Vector<Vector3> 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<Vector3> &get_debug_shape_vertices() const;
const Vector<Vector3> &get_debug_line_vertices() const;
Ref<SpatialMaterial> 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

638
scene/3d/shape_cast.cpp
View File

@ -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<CollisionObject>(get_parent())) {
if (exclude_parent_body) {
exclude.insert(Object::cast_to<CollisionObject>(get_parent())->get_rid());
} else {
exclude.erase(Object::cast_to<CollisionObject>(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<ConcavePolygonShape>(*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<Resource> p_res) {
if (is_inside_tree()) {
_update_debug_shape();
}
update_gizmos();
}
void ShapeCast::set_shape(const Ref<Shape> &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<Shape> 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<CollisionObject>(get_parent())) {
if (exclude_parent_body) {
exclude.insert(Object::cast_to<CollisionObject>(get_parent())->get_rid());
} else {
exclude.erase(Object::cast_to<CollisionObject>(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<World3D> 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<RID> 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<CollisionObject>(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<CollisionObject>(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<Vector3> &ShapeCast::get_debug_shape_vertices() const {
return debug_shape_vertices;
}
const Vector<Vector3> &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<SpatialMaterial> 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<ArrayMesh> 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<SpatialMaterial> 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<SpatialMaterial> material = static_cast<Ref<SpatialMaterial>>(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<MeshInstance *>(debug_shape);
Ref<ArrayMesh> 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<MeshInstance *>(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);
}
}

148
scene/3d/shape_cast.h
View File

@ -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<Resource> p_res);
Ref<Shape> shape;
RID shape_rid;
Vector3 target_position = Vector3(0, -1, 0);
RBSet<RID> 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<Material> debug_material;
Color debug_shape_custom_color = Color(0.0, 0.0, 0.0);
Vector<Vector3> debug_shape_vertices;
Vector<Vector3> 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<PhysicsDirectSpaceState::ShapeRestInfo> 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<Shape> &p_shape);
Ref<Shape> 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<Vector3> &get_debug_shape_vertices() const;
const Vector<Vector3> &get_debug_line_vertices() const;
Ref<SpatialMaterial> 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

856
scene/3d/soft_body.cpp
View File

@ -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<PinnedPoint>::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<PropertyInfo> *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<PinnedPoint>::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<PinnedPoint>::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<PinnedPoint>::Write w = pinned_points.write();
pin_point(w[p_item].point_index, true, p_value);
_make_cache_dirty();
} else if ("offset" == p_what) {
PoolVector<PinnedPoint>::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<PinnedPoint>::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<PinnedPoint>::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<Ref<Material>> 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<ArrayMesh> 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<SoftBody::PinnedPoint> p_pinned_points_indices) {
pinned_points = p_pinned_points_indices;
PoolVector<PinnedPoint>::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::PinnedPoint> SoftBody::get_pinned_points_indices() {
return pinned_points;
}
Array SoftBody::get_collision_exceptions() {
List<RID> exceptions;
PhysicsServer::get_singleton()->soft_body_get_collision_exceptions(physics_rid, &exceptions);
Array ret;
for (List<RID>::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<PhysicsBody>(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<CollisionObject>(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<CollisionObject>(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<PinnedPoint>::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<PinnedPoint>::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<Spatial>(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<Spatial>(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<Spatial>(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<PinnedPoint>::Read r = pinned_points.read();
PoolVector<PinnedPoint>::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<Spatial>(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<SoftBody::PinnedPoint *>(&pinned_points.read()[id]);
return id;
}
}
int SoftBody::_has_pinned_point(int p_point_index) const {
PoolVector<PinnedPoint>::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;
}

204
scene/3d/soft_body.h
View File

@ -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<uint8_t> buffer;
uint32_t stride;
uint32_t offset_vertices;
uint32_t offset_normal;
PoolVector<uint8_t>::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<PinnedPoint> pinned_points;
bool simulation_started;
bool pinned_points_cache_dirty;
Ref<ArrayMesh> 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<PropertyInfo> *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<PinnedPoint> p_pinned_points_indices);
PoolVector<PinnedPoint> 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

174
scene/3d/spring_arm.cpp
View File

@ -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<Shape> p_shape) {
shape = p_shape;
}
Ref<Shape> 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<Spatial>(get_child(i));
if (child) {
childs_transform.basis = child->get_global_transform().basis;
child->set_global_transform(childs_transform);
}
}
}

73
scene/3d/spring_arm.h
View File

@ -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> shape;
RBSet<RID> 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<Shape> p_shape);
Ref<Shape> 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

965
scene/3d/vehicle_body.cpp
View File

@ -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<VehicleBody>(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<VehicleBody>(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<VehicleBody>(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<PhysicsBody>(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<PhysicsDirectBodyState>(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);
}

211
scene/3d/vehicle_body.h
View File

@ -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<RID> exclude;
Vector<Vector3> m_forwardWS;
Vector<Vector3> m_axle;
Vector<real_t> m_forwardImpulse;
Vector<real_t> 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<VehicleWheel *> 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

17
scene/3d/visibility_notifier.cpp
View File

@ -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<RigidBody>(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<AnimationPlayer>(p_node) || Object::cast_to<AnimationTree>(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<RigidBody>(p_node);
if (rb) {
rb->set_sleeping(!p_enabled);
}
}
if (enabler[ENABLER_PAUSE_ANIMATIONS]) {
AnimationPlayer *ap = Object::cast_to<AnimationPlayer>(p_node);
if (ap) {

3
scene/main/scene_tree.cpp
View File

@ -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()) {

146
scene/main/viewport.cpp
View File

@ -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<InputEvent> &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<InputEventMouseMotion> 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<Vector3> 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<CollisionObject>(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<RID>(), 0xFFFFFFFF, true, true, true);
ObjectID new_collider = 0;
if (col) {
CollisionObject *co = Object::cast_to<CollisionObject>(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<CollisionObject>(ObjectDB::get_instance(physics_object_over));
if (co) {
co->_mouse_exit();
}
}
if (new_collider) {
CollisionObject *co = Object::cast_to<CollisionObject>(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<CollisionObject>(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<Control *>::Element *Viewport::_gui_show_modal(Control *p_control) {

3
scene/main/viewport.h
View File

@ -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<InputEvent> &p_input_event, const Vector3 &p_pos, const Vector3 &p_normal, int p_shape);
bool handle_input_locally;
bool local_input_handled;

64
scene/register_scene_types.cpp
View File

@ -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<CullInstance>();
ClassDB::register_virtual_class<GeometryInstance>();
ClassDB::register_class<Camera>();
ClassDB::register_class<ClippedCamera>();
ClassDB::register_class<Listener>();
ClassDB::register_class<InterpolatedCamera>();
ClassDB::register_class<MeshInstance>();
@ -468,24 +448,7 @@ void register_scene_types() {
OS::get_singleton()->yield(); //may take time to init
ClassDB::register_virtual_class<CollisionObject>();
ClassDB::register_virtual_class<PhysicsBody>();
ClassDB::register_class<StaticBody>();
ClassDB::register_class<RigidBody>();
ClassDB::register_class<KinematicCollision>();
ClassDB::register_class<KinematicBody>();
ClassDB::register_class<SpringArm>();
ClassDB::register_class<SoftBody>();
ClassDB::register_class<VehicleBody>();
ClassDB::register_class<VehicleWheel>();
ClassDB::register_class<Area>();
ClassDB::register_class<ProximityGroup>();
ClassDB::register_class<CollisionShape>();
ClassDB::register_class<CollisionPolygon>();
ClassDB::register_class<RayCast>();
ClassDB::register_class<ShapeCast>();
ClassDB::register_class<MultiMeshInstance>();
ClassDB::register_class<Curve3D>();
@ -496,17 +459,10 @@ void register_scene_types() {
ClassDB::register_class<WorldEnvironment3D>();
ClassDB::register_class<RemoteTransform>();
ClassDB::register_virtual_class<Joint>();
ClassDB::register_class<PinJoint>();
ClassDB::register_class<HingeJoint>();
ClassDB::register_class<SliderJoint>();
ClassDB::register_class<ConeTwistJoint>();
ClassDB::register_class<Generic6DOFJoint>();
ClassDB::register_class<Navigation>();
ClassDB::register_class<NavigationMeshInstance>();
ClassDB::register_class<NavigationAgent>();
ClassDB::register_class<NavigationObstacle>();
ClassDB::register_class<NavigationLink3D>();
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<Shape>();
ClassDB::register_class<RayShape>();
ClassDB::register_class<SphereShape>();
ClassDB::register_class<BoxShape>();
ClassDB::register_class<CapsuleShape>();
ClassDB::register_class<CylinderShape>();
ClassDB::register_class<HeightMapShape>();
ClassDB::register_class<PlaneShape>();
ClassDB::register_class<ConvexPolygonShape>();
ClassDB::register_class<ConcavePolygonShape>();
ClassDB::register_virtual_class<OccluderShape>();
ClassDB::register_class<OccluderShapeSphere>();
ClassDB::register_class<OccluderShapePolygon>();
OS::get_singleton()->yield(); //may take time to init
ClassDB::register_class<SpatialVelocityTracker>();
#endif

21
scene/resources/mesh/importer_mesh.cpp
View File

@ -422,27 +422,6 @@ Vector<Face3> ImporterMesh::get_faces() const {
return faces;
}
Ref<Shape> ImporterMesh::create_trimesh_shape() const {
Vector<Face3> faces = get_faces();
if (faces.size() == 0) {
return Ref<Shape>();
}
PoolVector<Vector3> 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<ConcavePolygonShape> shape = memnew(ConcavePolygonShape);
shape->set_faces(face_points);
return shape;
}
Ref<NavigationMesh> ImporterMesh::create_navigation_mesh() {
Vector<Face3> faces = get_faces();
if (faces.size() == 0) {

3
scene/resources/mesh/importer_mesh.h
View File

@ -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<Material> &p_material);
Vector<Face3> get_faces() const;
Ref<Shape> create_trimesh_shape() const;
Ref<NavigationMesh> create_navigation_mesh();
bool has_mesh() const;

104
scene/resources/mesh/mesh.cpp
View File

@ -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 <stdlib.h>
@ -202,68 +200,6 @@ PoolVector<Face3> Mesh::get_faces() const {
return PoolVector<Face3>();
}
Ref<Shape> Mesh::create_convex_shape(bool p_clean, bool p_simplify) const {
if (p_simplify) {
Vector<Ref<Shape>> decomposed = convex_decompose(1);
if (decomposed.size() == 1) {
return decomposed[0];
} else {
ERR_PRINT("Convex shape simplification failed, falling back to simpler process.");
}
}
PoolVector<Vector3> vertices;
for (int i = 0; i < get_surface_count(); i++) {
Array a = surface_get_arrays(i);
ERR_FAIL_COND_V(a.empty(), Ref<ConvexPolygonShape>());
PoolVector<Vector3> v = a[ARRAY_VERTEX];
vertices.append_array(v);
}
Ref<ConvexPolygonShape> 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<Vector3>::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<Shape> Mesh::create_trimesh_shape() const {
PoolVector<Face3> faces = get_faces();
if (faces.size() == 0) {
return Ref<Shape>();
}
PoolVector<Vector3> 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<ConcavePolygonShape> shape = memnew(ConcavePolygonShape);
shape->set_faces(face_points);
return shape;
}
Ref<Mesh> 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<Ref<Shape>> Mesh::convex_decompose(int p_max_convex_hulls) const {
ERR_FAIL_COND_V(!convex_decomposition_function, Vector<Ref<Shape>>());
Ref<TriangleMesh> tm = generate_triangle_mesh();
ERR_FAIL_COND_V(!tm.is_valid(), Vector<Ref<Shape>>());
const PoolVector<TriangleMesh::Triangle> &triangles = tm->get_triangles();
int triangle_count = triangles.size();
PoolVector<uint32_t> indices;
{
indices.resize(triangle_count * 3);
PoolVector<uint32_t>::Write w = indices.write();
PoolVector<TriangleMesh::Triangle>::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<Vector3> &vertices = tm->get_vertices();
int vertex_count = vertices.size();
Vector<PoolVector<Vector3>> decomposed = convex_decomposition_function((real_t *)vertices.read().ptr(), vertex_count, indices.read().ptr(), triangle_count, p_max_convex_hulls, nullptr);
Vector<Ref<Shape>> ret;
for (int i = 0; i < decomposed.size(); i++) {
Ref<ConvexPolygonShape> 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);

6
scene/resources/mesh/mesh.h
View File

@ -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<Vector3> &r_lines);
void generate_debug_mesh_indices(Vector<Vector3> &r_points);
Ref<Shape> create_trimesh_shape() const;
Ref<Shape> create_convex_shape(bool p_clean = true, bool p_simplify = false) const;
Ref<Mesh> create_outline(float p_margin) const;
virtual AABB get_aabb() const = 0;
@ -149,8 +145,6 @@ public:
static ConvexDecompositionFunc convex_decomposition_function;
Vector<Ref<Shape>> convex_decompose(int p_max_convex_hulls = -1) const;
Mesh();
};

1
scene/resources/physics_material.h
View File

@ -31,7 +31,6 @@
/*************************************************************************/
#include "core/object/resource.h"
#include "servers/physics_server.h"
class PhysicsMaterial : public Resource {
GDCLASS(PhysicsMaterial, Resource);

80
scene/resources/shapes/box_shape.cpp
View File

@ -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<Vector3> BoxShape::get_debug_mesh_lines() {
Vector<Vector3> 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));
}

54
scene/resources/shapes/box_shape.h
View File

@ -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<Vector3> get_debug_mesh_lines();
virtual real_t get_enclosing_radius() const;
BoxShape();
};
#endif // BOX_SHAPE_H

119
scene/resources/shapes/capsule_shape.cpp
View File

@ -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<Vector3> CapsuleShape::get_debug_mesh_lines() {
float radius = get_radius();
float height = get_height();
Vector<Vector3> 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();
}

57
scene/resources/shapes/capsule_shape.h
View File

@ -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<Vector3> get_debug_mesh_lines();
virtual real_t get_enclosing_radius() const;
CapsuleShape();
};
#endif // CAPSULE_SHAPE_H

96
scene/resources/shapes/concave_polygon_shape.cpp
View File

@ -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<Vector3> ConcavePolygonShape::get_debug_mesh_lines() {
RBSet<DrawEdge> edges;
PoolVector<Vector3> data = get_faces();
int datalen = data.size();
ERR_FAIL_COND_V((datalen % 3) != 0, Vector<Vector3>());
PoolVector<Vector3>::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<Vector3> points;
points.resize(edges.size() * 2);
int idx = 0;
for (RBSet<DrawEdge>::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<Vector3> data = get_faces();
PoolVector<Vector3>::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<Vector3> &p_faces) {
PhysicsServer::get_singleton()->shape_set_data(get_shape(), p_faces);
_update_shape();
notify_change_to_owners();
}
PoolVector<Vector3> 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));
}

73
scene/resources/shapes/concave_polygon_shape.h
View File

@ -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<Vector3> &p_faces);
PoolVector<Vector3> get_faces() const;
Vector<Vector3> get_debug_mesh_lines();
virtual real_t get_enclosing_radius() const;
ConcavePolygonShape();
};
#endif // CONCAVE_POLYGON_SHAPE_H

90
scene/resources/shapes/convex_polygon_shape.cpp
View File

@ -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<Vector3> ConvexPolygonShape::get_debug_mesh_lines() {
PoolVector<Vector3> points = get_points();
if (points.size() > 3) {
Vector<Vector3> varr = Variant(points);
Geometry::MeshData md;
Error err = ConvexHullComputer::convex_hull(varr, md);
if (err == OK) {
Vector<Vector3> 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<Vector3>();
}
real_t ConvexPolygonShape::get_enclosing_radius() const {
PoolVector<Vector3> data = get_points();
PoolVector<Vector3>::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<Vector3> &p_points) {
points = p_points;
_update_shape();
notify_change_to_owners();
}
PoolVector<Vector3> 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))) {
}

54
scene/resources/shapes/convex_polygon_shape.h
View File

@ -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<Vector3> points;
protected:
static void _bind_methods();
virtual void _update_shape();
public:
void set_points(const PoolVector<Vector3> &p_points);
PoolVector<Vector3> get_points() const;
virtual Vector<Vector3> get_debug_mesh_lines();
virtual real_t get_enclosing_radius() const;
ConvexPolygonShape();
};
#endif // CONVEX_POLYGON_SHAPE_H

112
scene/resources/shapes/cylinder_shape.cpp
View File

@ -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<Vector3> CylinderShape::get_debug_mesh_lines() {
float radius = get_radius();
float height = get_height();
Vector<Vector3> 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();
}

56
scene/resources/shapes/cylinder_shape.h
View File

@ -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<Vector3> get_debug_mesh_lines();
virtual real_t get_enclosing_radius() const;
CylinderShape();
};
#endif // CYLINDER_SHAPE_H

214
scene/resources/shapes/height_map_shape.cpp
View File

@ -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<Vector3> HeightMapShape::get_debug_mesh_lines() {
Vector<Vector3> 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();
}

62
scene/resources/shapes/height_map_shape.h
View File

@ -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<Vector3> get_debug_mesh_lines();
virtual real_t get_enclosing_radius() const;
HeightMapShape();
};
#endif /* !HEIGHT_MAP_SHAPE_H */

89
scene/resources/shapes/plane_shape.cpp
View File

@ -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<Vector3> PlaneShape::get_debug_mesh_lines() {
Plane p = get_plane();
Vector<Vector3> 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));
}

55
scene/resources/shapes/plane_shape.h
View File

@ -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<Vector3> get_debug_mesh_lines();
virtual real_t get_enclosing_radius() const {
// Should be infinite?
return 0;
};
PlaneShape();
};
#endif // PLANE_SHAPE_H

98
scene/resources/shapes/ray_shape.cpp
View File

@ -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<Vector3> RayShape::get_debug_mesh_lines() {
Vector<Vector3> 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");
}

56
scene/resources/shapes/ray_shape.h
View File

@ -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<Vector3> get_debug_mesh_lines();
virtual real_t get_enclosing_radius() const;
RayShape();
};
#endif // RAY_SHAPE_H

122
scene/resources/shapes/shape.cpp
View File

@ -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<Vector3> &array, const Transform &p_xform) {
Vector<Vector3> toadd = get_debug_mesh_lines();
if (toadd.size()) {
int base = array.size();
array.resize(base + toadd.size());
PoolVector<Vector3>::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<ArrayMesh> Shape::get_debug_mesh() {
if (debug_mesh_cache.is_valid()) {
return debug_mesh_cache;
}
Vector<Vector3> lines = get_debug_mesh_lines();
debug_mesh_cache = Ref<ArrayMesh>(memnew(ArrayMesh));
if (!lines.empty()) {
//make mesh
PoolVector<Vector3> array;
array.resize(lines.size());
{
PoolVector<Vector3>::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<SceneTree>(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);
}

70
scene/resources/shapes/shape.h
View File

@ -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<ArrayMesh> 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<ArrayMesh> get_debug_mesh();
virtual Vector<Vector3> get_debug_mesh_lines() = 0; // { return Vector<Vector3>(); }
/// 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<Vector3> &array, const Transform &p_xform);
real_t get_margin() const;
void set_margin(real_t p_margin);
Shape();
~Shape();
};
#endif // SHAPE_H

87
scene/resources/shapes/sphere_shape.cpp
View File

@ -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<Vector3> SphereShape::get_debug_mesh_lines() {
float r = get_radius();
Vector<Vector3> 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);
}

54
scene/resources/shapes/sphere_shape.h
View File

@ -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<Vector3> get_debug_mesh_lines();
virtual real_t get_enclosing_radius() const;
SphereShape();
};
#endif // SPHERE_SHAPE_H

16
scene/resources/world_3d.cpp
View File

@ -304,10 +304,6 @@ Ref<Environment3D> 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<Camera *> *r_cameras) {
for (RBMap<Camera *, SpatialIndexer::CameraData>::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);

3
scene/resources/world_3d.h
View File

@ -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<Camera *> *r_cameras);
PhysicsDirectSpaceState *get_direct_space_state();
void move_cameras_into(Ref<World3D> target);
World3D();

7
servers/physics/SCsub
View File

@ -1,7 +0,0 @@
#!/usr/bin/env python
Import("env")
env.add_source_files(env.servers_sources, "*.cpp")
SConscript("joints/SCsub")

155
servers/physics/area_pair_sw.cpp
View File

@ -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);
}

69
servers/physics/area_pair_sw.h
View File

@ -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

296
servers/physics/area_sw.cpp
View File

@ -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<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E;) {
if (E->get().state == 0) { // Nothing happened
RBMap<BodyKey, BodyState>::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<BodyKey, BodyState>::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<BodyKey, BodyState>::Element *E = monitored_areas.front(); E;) {
if (E->get().state == 0) { // Nothing happened
RBMap<BodyKey, BodyState>::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<BodyKey, BodyState>::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() {
}

198
servers/physics/area_sw.h
View File

@ -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<AreaSW> monitor_query_list;
SelfList<AreaSW> 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<BodyKey, BodyState> monitored_bodies;
RBMap<BodyKey, BodyState> monitored_areas;
//virtual void shape_changed_notify(ShapeSW *p_shape);
//virtual void shape_deleted_notify(ShapeSW *p_shape);
RBSet<ConstraintSW *> 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<ConstraintSW *> &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

468
servers/physics/body_pair_sw.cpp
View File

@ -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);
}

95
servers/physics/body_pair_sw.h
View File

@ -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

814
servers/physics/body_sw.cpp
View File

@ -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;i<get_shape_count();i++) {
Shape &s=shapes[i];
if (s.bpid>0) {
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<<i)) {
transform.origin[i]=0.0;
}
}*/
transform.origin += total_linear_velocity * p_step;
_set_transform(transform);
_set_inv_transform(get_transform().inverse());
_update_transform_dependant();
/*
if (fi_callback) {
get_space()->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<ConstraintSW *, int>::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();
}

494
servers/physics/body_sw.h
View File

@ -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<BodySW> active_list;
SelfList<BodySW> inertia_update_list;
SelfList<BodySW> direct_state_query_list;
VSet<RID> 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<ConstraintSW *, int> 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<AreaCMP> 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<Contact> 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<RID> &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<ConstraintSW *, int> &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

212
servers/physics/broad_phase_basic.cpp
View File

@ -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<ID, Element>::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<ID, Element>::Element *E = element_map.find(p_id);
ERR_FAIL_COND(!E);
E->get()._static = p_static;
}
void BroadPhaseBasic::remove(ID p_id) {
RBMap<ID, Element>::Element *E = element_map.find(p_id);
ERR_FAIL_COND(!E);
List<PairKey> to_erase;
//unpair must be done immediately on removal to avoid potential invalid pointers
for (RBMap<PairKey, void *>::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<ID, Element>::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<ID, Element>::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<ID, Element>::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<ID, Element>::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<ID, Element>::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<ID, Element>::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<ID, Element>::Element *I = element_map.front(); I; I = I->next()) {
for (RBMap<ID, Element>::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<PairKey, void *>::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;
}

105
servers/physics/broad_phase_basic.h
View File

@ -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<ID, Element> 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<PairKey, void *> 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

141
servers/physics/broad_phase_bvh.cpp
View File

@ -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;
}

100
servers/physics/broad_phase_bvh.h
View File

@ -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 T>
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 T>
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<CollisionObjectSW, 2, true, 128, UserPairTestFunction<CollisionObjectSW>, UserCullTestFunction<CollisionObjectSW>> 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

128
servers/physics/broad_phase_octree.cpp
View File

@ -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;
}

72
servers/physics/broad_phase_octree.h
View File

@ -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<CollisionObjectSW, true> 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

36
servers/physics/broad_phase_sw.cpp
View File

@ -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() {
}

72
servers/physics/broad_phase_sw.h
View File

@ -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

262
servers/physics/collision_object_sw.cpp
View File

@ -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;
}

184
servers/physics/collision_object_sw.h
View File

@ -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<Shape> shapes;
SpaceSW *space;
Transform transform;
Transform inv_transform;
bool _static;
SelfList<CollisionObjectSW> 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

2313
servers/physics/collision_solver_sat.cpp
View File

File diff suppressed because it is too large Load Diff

37
servers/physics/collision_solver_sat.h
View File

@ -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

405
servers/physics/collision_solver_sw.cpp
View File

@ -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<const PlaneShapeSW *>(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<const RayShapeSW *>(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<const ConcaveShapeSW *>(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<const PlaneShapeSW *>(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<const ConcaveShapeSW *>(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..
}
}

52
servers/physics/collision_solver_sw.h
View File

@ -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

83
servers/physics/constraint_sw.h
View File

@ -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

1035
servers/physics/gjk_epa.cpp
View File

File diff suppressed because it is too large Load Diff

39
servers/physics/gjk_epa.h
View File

@ -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

5
servers/physics/joints/SCsub
View File

@ -1,5 +0,0 @@
#!/usr/bin/env python
Import("env")
env.add_source_files(env.servers_sources, "*.cpp")

356
servers/physics/joints/cone_twist_joint_sw.cpp
View File

@ -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;
}

141
servers/physics/joints/cone_twist_joint_sw.h
View File

@ -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

663
servers/physics/joints/generic_6dof_joint_sw.cpp
View File

@ -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;
}

364
servers/physics/joints/generic_6dof_joint_sw.h
View File

@ -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

487
servers/physics/joints/hinge_joint_sw.cpp
View File

@ -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;
}

118
servers/physics/joints/hinge_joint_sw.h
View File

@ -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

Some files were not shown because too many files have changed in this diff Show More