diff options
Diffstat (limited to 'servers/physics_3d')
37 files changed, 666 insertions, 754 deletions
diff --git a/servers/physics_3d/area_3d_sw.cpp b/servers/physics_3d/area_3d_sw.cpp index c9e8bcb8ca..630ab7e229 100644 --- a/servers/physics_3d/area_3d_sw.cpp +++ b/servers/physics_3d/area_3d_sw.cpp @@ -329,17 +329,7 @@ Area3DSW::Area3DSW() : monitor_query_list(this), moved_list(this) { _set_static(true); //areas are never active - space_override_mode = PhysicsServer3D::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); - monitorable = false; } Area3DSW::~Area3DSW() { diff --git a/servers/physics_3d/area_3d_sw.h b/servers/physics_3d/area_3d_sw.h index d5f1e60119..af5c23949c 100644 --- a/servers/physics_3d/area_3d_sw.h +++ b/servers/physics_3d/area_3d_sw.h @@ -41,20 +41,20 @@ class SoftBody3DSW; class Constraint3DSW; class Area3DSW : public CollisionObject3DSW { - PhysicsServer3D::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; + PhysicsServer3D::AreaSpaceOverrideMode space_override_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; + real_t gravity = 9.80665; + Vector3 gravity_vector = Vector3(0, -1, 0); + bool gravity_is_point = false; + real_t gravity_distance_scale = 0.0; + real_t point_attenuation = 1.0; + real_t linear_damp = 0.1; + real_t angular_damp = 0.1; real_t wind_force_magnitude = 0.0; real_t wind_attenuation_factor = 0.0; Vector3 wind_source; Vector3 wind_direction; - int priority; - bool monitorable; + int priority = 0; + bool monitorable = false; ObjectID monitor_callback_id; StringName monitor_callback_method; @@ -68,8 +68,8 @@ class Area3DSW : public CollisionObject3DSW { struct BodyKey { RID rid; ObjectID instance_id; - uint32_t body_shape; - uint32_t area_shape; + uint32_t body_shape = 0; + uint32_t area_shape = 0; _FORCE_INLINE_ bool operator<(const BodyKey &p_key) const { if (rid == p_key.rid) { @@ -90,10 +90,9 @@ class Area3DSW : public CollisionObject3DSW { }; struct BodyState { - int state; + int state = 0; _FORCE_INLINE_ void inc() { state++; } _FORCE_INLINE_ void dec() { state--; } - _FORCE_INLINE_ BodyState() { state = 0; } }; Map<BodyKey, BodyState> monitored_soft_bodies; @@ -232,8 +231,8 @@ void Area3DSW::remove_area_from_query(Area3DSW *p_area, uint32_t p_area_shape, u } struct AreaCMP { - Area3DSW *area; - int refCount; + Area3DSW *area = nullptr; + int refCount = 0; _FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); } _FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); } _FORCE_INLINE_ AreaCMP() {} diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp index 41745545d8..069374d122 100644 --- a/servers/physics_3d/body_3d_sw.cpp +++ b/servers/physics_3d/body_3d_sw.cpp @@ -110,7 +110,7 @@ void Body3DSW::update_mass_properties() { real_t mass = area * this->mass / total_area; - Basis shape_inertia_tensor = shape->get_moment_of_inertia(mass).to_diagonal_matrix(); + Basis shape_inertia_tensor = Basis::from_scale(shape->get_moment_of_inertia(mass)); Transform3D shape_transform = get_shape_transform(i); Basis shape_basis = shape_transform.basis.orthonormalized(); @@ -123,7 +123,7 @@ void Body3DSW::update_mass_properties() { // 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)); + inertia_tensor = Basis(); } // Handle partial custom inertia. @@ -154,7 +154,7 @@ void Body3DSW::update_mass_properties() { _inv_inertia = Vector3(); _inv_mass = 0; } break; - case PhysicsServer3D::BODY_MODE_DYNAMIC_LOCKED: { + case PhysicsServer3D::BODY_MODE_DYNAMIC_LINEAR: { _inv_inertia_tensor.set_zero(); _inv_mass = 1.0 / mass; @@ -215,7 +215,7 @@ void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, const Variant & } else { calculate_inertia = false; if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC) { - principal_inertia_axes_local.set_diagonal(Vector3(1.0, 1.0, 1.0)); + principal_inertia_axes_local = Basis(); _inv_inertia = inertia.inverse(); _update_transform_dependant(); } @@ -301,7 +301,7 @@ void Body3DSW::set_mode(PhysicsServer3D::BodyMode p_mode) { case PhysicsServer3D::BODY_MODE_DYNAMIC: { _inv_mass = mass > 0 ? (1.0 / mass) : 0; if (!calculate_inertia) { - principal_inertia_axes_local.set_diagonal(Vector3(1.0, 1.0, 1.0)); + principal_inertia_axes_local = Basis(); _inv_inertia = inertia.inverse(); _update_transform_dependant(); } @@ -310,7 +310,7 @@ void Body3DSW::set_mode(PhysicsServer3D::BodyMode p_mode) { set_active(true); } break; - case PhysicsServer3D::BODY_MODE_DYNAMIC_LOCKED: { + case PhysicsServer3D::BODY_MODE_DYNAMIC_LINEAR: { _inv_mass = mass > 0 ? (1.0 / mass) : 0; _inv_inertia = Vector3(); angular_velocity = Vector3(); @@ -688,13 +688,13 @@ void BodySW::simulate_motion(const Transform3D& p_xform,real_t p_step) { */ void Body3DSW::wakeup_neighbours() { - for (Map<Constraint3DSW *, int>::Element *E = constraint_map.front(); E; E = E->next()) { - const Constraint3DSW *c = E->key(); + for (const KeyValue<Constraint3DSW *, int> &E : constraint_map) { + const Constraint3DSW *c = E.key; Body3DSW **n = c->get_body_ptr(); int bc = c->get_body_count(); for (int i = 0; i < bc; i++) { - if (i == E->get()) { + if (i == E.value) { continue; } Body3DSW *b = n[i]; diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h index 8b74c7e5b9..fc47040389 100644 --- a/servers/physics_3d/body_3d_sw.h +++ b/servers/physics_3d/body_3d_sw.h @@ -108,10 +108,10 @@ class Body3DSW : public CollisionObject3DSW { struct Contact { Vector3 local_pos; Vector3 local_normal; - real_t depth; - int local_shape; + real_t depth = 0.0; + int local_shape = 0; Vector3 collider_pos; - int collider_shape; + int collider_shape = 0; ObjectID collider_instance_id; RID collider; Vector3 collider_velocity_at_pos; diff --git a/servers/physics_3d/body_direct_state_3d_sw.cpp b/servers/physics_3d/body_direct_state_3d_sw.cpp index d197dd288d..d61a6ac8e4 100644 --- a/servers/physics_3d/body_direct_state_3d_sw.cpp +++ b/servers/physics_3d/body_direct_state_3d_sw.cpp @@ -66,6 +66,7 @@ Basis PhysicsDirectBodyState3DSW::get_inverse_inertia_tensor() const { } void PhysicsDirectBodyState3DSW::set_linear_velocity(const Vector3 &p_velocity) { + body->wakeup(); body->set_linear_velocity(p_velocity); } @@ -74,6 +75,7 @@ Vector3 PhysicsDirectBodyState3DSW::get_linear_velocity() const { } void PhysicsDirectBodyState3DSW::set_angular_velocity(const Vector3 &p_velocity) { + body->wakeup(); body->set_angular_velocity(p_velocity); } @@ -94,26 +96,32 @@ Vector3 PhysicsDirectBodyState3DSW::get_velocity_at_local_position(const Vector3 } void PhysicsDirectBodyState3DSW::add_central_force(const Vector3 &p_force) { + body->wakeup(); body->add_central_force(p_force); } void PhysicsDirectBodyState3DSW::add_force(const Vector3 &p_force, const Vector3 &p_position) { + body->wakeup(); body->add_force(p_force, p_position); } void PhysicsDirectBodyState3DSW::add_torque(const Vector3 &p_torque) { + body->wakeup(); body->add_torque(p_torque); } void PhysicsDirectBodyState3DSW::apply_central_impulse(const Vector3 &p_impulse) { + body->wakeup(); body->apply_central_impulse(p_impulse); } void PhysicsDirectBodyState3DSW::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) { + body->wakeup(); body->apply_impulse(p_impulse, p_position); } void PhysicsDirectBodyState3DSW::apply_torque_impulse(const Vector3 &p_impulse) { + body->wakeup(); body->apply_torque_impulse(p_impulse); } diff --git a/servers/physics_3d/body_pair_3d_sw.h b/servers/physics_3d/body_pair_3d_sw.h index 19d6a46880..01afb07e13 100644 --- a/servers/physics_3d/body_pair_3d_sw.h +++ b/servers/physics_3d/body_pair_3d_sw.h @@ -41,18 +41,18 @@ protected: struct Contact { Vector3 position; Vector3 normal; - int index_A, index_B; + int index_A = 0, index_B = 0; Vector3 local_A, local_B; - real_t acc_normal_impulse; // accumulated normal impulse (Pn) + real_t acc_normal_impulse = 0.0; // 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; + real_t acc_bias_impulse = 0.0; // accumulated normal impulse for position bias (Pnb) + real_t acc_bias_impulse_center_of_mass = 0.0; // accumulated normal impulse for position bias applied to com + real_t mass_normal = 0.0; + real_t bias = 0.0; + real_t bounce = 0.0; + + real_t depth = 0.0; + bool active = false; Vector3 rA, rB; // Offset in world orientation with respect to center of mass }; diff --git a/servers/physics_3d/broad_phase_3d_bvh.cpp b/servers/physics_3d/broad_phase_3d_bvh.cpp index f9f64f786d..d89e0e1f6d 100644 --- a/servers/physics_3d/broad_phase_3d_bvh.cpp +++ b/servers/physics_3d/broad_phase_3d_bvh.cpp @@ -114,7 +114,4 @@ BroadPhase3DSW *BroadPhase3DBVH::_create() { BroadPhase3DBVH::BroadPhase3DBVH() { bvh.set_pair_callback(_pair_callback, this); bvh.set_unpair_callback(_unpair_callback, this); - pair_callback = nullptr; - pair_userdata = nullptr; - unpair_userdata = nullptr; } diff --git a/servers/physics_3d/broad_phase_3d_bvh.h b/servers/physics_3d/broad_phase_3d_bvh.h index 30b8b7f2aa..03131c9db2 100644 --- a/servers/physics_3d/broad_phase_3d_bvh.h +++ b/servers/physics_3d/broad_phase_3d_bvh.h @@ -40,10 +40,10 @@ class BroadPhase3DBVH : public BroadPhase3DSW { static void *_pair_callback(void *, uint32_t, CollisionObject3DSW *, int, uint32_t, CollisionObject3DSW *, int); static void _unpair_callback(void *, uint32_t, CollisionObject3DSW *, int, uint32_t, CollisionObject3DSW *, int, void *); - PairCallback pair_callback; - void *pair_userdata; - UnpairCallback unpair_callback; - void *unpair_userdata; + PairCallback pair_callback = nullptr; + void *pair_userdata = nullptr; + UnpairCallback unpair_callback = nullptr; + void *unpair_userdata = nullptr; public: // 0 is an invalid ID diff --git a/servers/physics_3d/collision_object_3d_sw.cpp b/servers/physics_3d/collision_object_3d_sw.cpp index 24c7d7b85c..098f627d11 100644 --- a/servers/physics_3d/collision_object_3d_sw.cpp +++ b/servers/physics_3d/collision_object_3d_sw.cpp @@ -236,11 +236,5 @@ void CollisionObject3DSW::_shape_changed() { CollisionObject3DSW::CollisionObject3DSW(Type p_type) : pending_shape_update_list(this) { - _static = true; type = p_type; - space = nullptr; - - collision_layer = 1; - collision_mask = 1; - ray_pickable = true; } diff --git a/servers/physics_3d/collision_object_3d_sw.h b/servers/physics_3d/collision_object_3d_sw.h index 6ffab54645..3aa48946b7 100644 --- a/servers/physics_3d/collision_object_3d_sw.h +++ b/servers/physics_3d/collision_object_3d_sw.h @@ -56,26 +56,24 @@ private: Type type; RID self; ObjectID instance_id; - uint32_t collision_layer; - uint32_t collision_mask; + uint32_t collision_layer = 1; + uint32_t collision_mask = 1; struct Shape { Transform3D xform; Transform3D xform_inv; BroadPhase3DSW::ID bpid; AABB aabb_cache; //for rayqueries - real_t area_cache; - Shape3DSW *shape; - bool disabled; - - Shape() { disabled = false; } + real_t area_cache = 0.0; + Shape3DSW *shape = nullptr; + bool disabled = false; }; Vector<Shape> shapes; - Space3DSW *space; + Space3DSW *space = nullptr; Transform3D transform; Transform3D inv_transform; - bool _static; + bool _static = true; SelfList<CollisionObject3DSW> pending_shape_update_list; @@ -102,7 +100,7 @@ protected: virtual void _shapes_changed() = 0; void _set_space(Space3DSW *p_space); - bool ray_pickable; + bool ray_pickable = true; CollisionObject3DSW(Type p_type); diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp index de81348b4e..76738bb746 100644 --- a/servers/physics_3d/collision_solver_3d_sat.cpp +++ b/servers/physics_3d/collision_solver_3d_sat.cpp @@ -66,11 +66,11 @@ struct _CollectorCallback { CollisionSolver3DSW::CallbackResult callback; - void *userdata; - bool swap; - bool collided; + void *userdata = nullptr; + bool swap = false; + bool collided = false; Vector3 normal; - Vector3 *prev_axis; + Vector3 *prev_axis = nullptr; _FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B) { if (swap) { @@ -606,15 +606,15 @@ static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_po template <class ShapeA, class ShapeB, bool withMargin = false> class SeparatorAxisTest { - const ShapeA *shape_A; - const ShapeB *shape_B; - const Transform3D *transform_A; - const Transform3D *transform_B; - real_t best_depth; + const ShapeA *shape_A = nullptr; + const ShapeB *shape_B = nullptr; + const Transform3D *transform_A = nullptr; + const Transform3D *transform_B = nullptr; + real_t best_depth = 1e15; Vector3 best_axis; - _CollectorCallback *callback; - real_t margin_A; - real_t margin_B; + _CollectorCallback *callback = nullptr; + real_t margin_A = 0.0; + real_t margin_B = 0.0; Vector3 separator_axis; public: @@ -749,7 +749,6 @@ public: } _FORCE_INLINE_ SeparatorAxisTest(const ShapeA *p_shape_A, const Transform3D &p_transform_A, const ShapeB *p_shape_B, const Transform3D &p_transform_B, _CollectorCallback *p_callback, real_t p_margin_A = 0, real_t p_margin_B = 0) { - best_depth = 1e15; shape_A = p_shape_A; shape_B = p_shape_B; transform_A = &p_transform_A; @@ -2272,13 +2271,13 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector3 *r_prev_axis, real_t p_margin_a, real_t p_margin_b) { PhysicsServer3D::ShapeType type_A = p_shape_A->get_type(); - ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_PLANE, false); + ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_WORLD_BOUNDARY, false); ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_SEPARATION_RAY, false); ERR_FAIL_COND_V(p_shape_A->is_concave(), false); PhysicsServer3D::ShapeType type_B = p_shape_B->get_type(); - ERR_FAIL_COND_V(type_B == PhysicsServer3D::SHAPE_PLANE, false); + ERR_FAIL_COND_V(type_B == PhysicsServer3D::SHAPE_WORLD_BOUNDARY, false); ERR_FAIL_COND_V(type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY, false); ERR_FAIL_COND_V(p_shape_B->is_concave(), false); diff --git a/servers/physics_3d/collision_solver_3d_sw.cpp b/servers/physics_3d/collision_solver_3d_sw.cpp index 4a4a8164d3..dcc363638e 100644 --- a/servers/physics_3d/collision_solver_3d_sw.cpp +++ b/servers/physics_3d/collision_solver_3d_sw.cpp @@ -37,12 +37,12 @@ #define collision_solver sat_calculate_penetration //#define collision_solver gjk_epa_calculate_penetration -bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { - const PlaneShape3DSW *plane = static_cast<const PlaneShape3DSW *>(p_shape_A); - if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_PLANE) { +bool CollisionSolver3DSW::solve_static_world_boundary(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { + const WorldBoundaryShape3DSW *world_boundary = static_cast<const WorldBoundaryShape3DSW *>(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { return false; } - Plane p = p_transform_A.xform(plane->get_plane()); + Plane p = p_transform_A.xform(world_boundary->get_plane()); static const int max_supports = 16; Vector3 supports[max_supports]; @@ -365,8 +365,8 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo swap = true; } - if (type_A == PhysicsServer3D::SHAPE_PLANE) { - if (type_B == PhysicsServer3D::SHAPE_PLANE) { + if (type_A == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { + if (type_B == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { return false; } if (type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY) { @@ -377,9 +377,9 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo } if (swap) { - return solve_static_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); + return solve_static_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); } else { - return solve_static_plane(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); + return solve_static_world_boundary(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); } } else if (type_A == PhysicsServer3D::SHAPE_SEPARATION_RAY) { @@ -443,12 +443,12 @@ bool CollisionSolver3DSW::concave_distance_callback(void *p_userdata, Shape3DSW return false; } -bool CollisionSolver3DSW::solve_distance_plane(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) { - const PlaneShape3DSW *plane = static_cast<const PlaneShape3DSW *>(p_shape_A); - if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_PLANE) { +bool CollisionSolver3DSW::solve_distance_world_boundary(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) { + const WorldBoundaryShape3DSW *world_boundary = static_cast<const WorldBoundaryShape3DSW *>(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { return false; } - Plane p = p_transform_A.xform(plane->get_plane()); + Plane p = p_transform_A.xform(world_boundary->get_plane()); static const int max_supports = 16; Vector3 supports[max_supports]; @@ -500,9 +500,9 @@ bool CollisionSolver3DSW::solve_distance(const Shape3DSW *p_shape_A, const Trans return false; } - if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_PLANE) { + if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { Vector3 a, b; - bool col = solve_distance_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, a, b); + bool col = solve_distance_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, a, b); r_point_A = b; r_point_B = a; return !col; diff --git a/servers/physics_3d/collision_solver_3d_sw.h b/servers/physics_3d/collision_solver_3d_sw.h index c13614ab3e..0a9ea7c0eb 100644 --- a/servers/physics_3d/collision_solver_3d_sw.h +++ b/servers/physics_3d/collision_solver_3d_sw.h @@ -42,12 +42,12 @@ private: static void soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); static bool soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex); static bool concave_callback(void *p_userdata, Shape3DSW *p_convex); - static bool solve_static_plane(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool solve_static_world_boundary(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); static bool solve_separation_ray(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0); static bool solve_soft_body(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); static bool solve_concave(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A = 0, real_t p_margin_B = 0); static bool concave_distance_callback(void *p_userdata, Shape3DSW *p_convex); - static bool solve_distance_plane(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B); + static bool solve_distance_world_boundary(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B); public: static bool solve_static(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp index f2f712193a..a1dbdd0a70 100644 --- a/servers/physics_3d/gjk_epa.cpp +++ b/servers/physics_3d/gjk_epa.cpp @@ -37,7 +37,7 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans https://bulletphysics.org +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the @@ -96,7 +96,7 @@ struct sResults { Vector3 witnesses[2]; Vector3 normal; - real_t distance; + real_t distance = 0.0; }; // Shorthands diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp index 7315e9c709..bb9cc1bf67 100644 --- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp @@ -92,20 +92,8 @@ ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Trans m_rbAFrame = rbAFrame; m_rbBFrame = rbBFrame; - m_swingSpan1 = Math_TAU / 8.0; - m_swingSpan2 = Math_TAU / 8.0; - m_twistSpan = Math_TAU; - 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 ConeTwistJoint3DSW::setup(real_t p_timestep) { diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h b/servers/physics_3d/joints/cone_twist_joint_3d_sw.h index 608847352c..bf7e593820 100644 --- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h +++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.h @@ -67,39 +67,39 @@ public: Body3DSW *B; }; - Body3DSW *_arr[2]; + Body3DSW *_arr[2] = { nullptr, nullptr }; }; - JacobianEntry3DSW m_jac[3]; //3 orthogonal linear constraints + JacobianEntry3DSW m_jac[3] = {}; //3 orthogonal linear constraints - real_t m_appliedImpulse; + real_t m_appliedImpulse = 0.0; Transform3D m_rbAFrame; Transform3D m_rbBFrame; - real_t m_limitSoftness; - real_t m_biasFactor; - real_t m_relaxationFactor; + real_t m_limitSoftness = 0.0; + real_t m_biasFactor = 0.3; + real_t m_relaxationFactor = 1.0; - real_t m_swingSpan1; - real_t m_swingSpan2; - real_t m_twistSpan; + real_t m_swingSpan1 = Math_TAU / 8.0; + real_t m_swingSpan2 = 0.0; + real_t m_twistSpan = 0.0; Vector3 m_swingAxis; Vector3 m_twistAxis; - real_t m_kSwing; - real_t m_kTwist; + real_t m_kSwing = 0.0; + real_t m_kTwist = 0.0; - real_t m_twistLimitSign; - real_t m_swingCorrection; - real_t m_twistCorrection; + real_t m_twistLimitSign = 0.0; + real_t m_swingCorrection = 0.0; + real_t m_twistCorrection = 0.0; - real_t m_accSwingLimitImpulse; - real_t m_accTwistLimitImpulse; + real_t m_accSwingLimitImpulse = 0.0; + real_t m_accTwistLimitImpulse = 0.0; - bool m_angularOnly; - bool m_solveTwistLimit; - bool m_solveSwingLimit; + bool m_angularOnly = false; + bool m_solveTwistLimit = false; + bool m_solveSwingLimit = false; public: virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; } diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp index d2b64ce6e3..56aba24b42 100644 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp @@ -34,7 +34,7 @@ Adapted to Godot from the Bullet library. /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org +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. diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h index c2a0443aff..6492e40393 100644 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h @@ -40,7 +40,7 @@ Adapted to Godot from the Bullet library. /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org +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. @@ -65,43 +65,28 @@ class G6DOFRotationalLimitMotor3DSW { 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; + real_t m_loLimit = -1e30; //!< joint limit + real_t m_hiLimit = 1e30; //!< joint limit + real_t m_targetVelocity = 0.0; //!< target motor velocity + real_t m_maxMotorForce = 0.1; //!< max force on motor + real_t m_maxLimitForce = 300.0; //!< max force on limit + real_t m_damping = 1.0; //!< Damping. + real_t m_limitSoftness = 0.5; //! Relaxation factor + real_t m_ERP = 0.5; //!< Error tolerance factor when joint is at limit + real_t m_bounce = 0.0; //!< restitution factor + bool m_enableMotor = false; + bool m_enableLimit = false; //!@} //! temp_variables //!@{ - real_t m_currentLimitError; //!< How much is violated this limit - int m_currentLimit; //!< 0=free, 1=at lo limit, 2=at hi limit - real_t m_accumulatedImpulse; + real_t m_currentLimitError = 0.0; //!< How much is violated this limit + int m_currentLimit = 0; //!< 0=free, 1=at lo limit, 2=at hi limit + real_t m_accumulatedImpulse = 0.0; //!@} - G6DOFRotationalLimitMotor3DSW() { - 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; - } + G6DOFRotationalLimitMotor3DSW() {} //! Is limited bool isLimited() { @@ -125,30 +110,16 @@ public: class G6DOFTranslationalLimitMotor3DSW { public: - Vector3 m_lowerLimit; //!< the constraint lower limits - Vector3 m_upperLimit; //!< the constraint upper limits - Vector3 m_accumulatedImpulse; + Vector3 m_lowerLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint lower limits + Vector3 m_upperLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint upper limits + Vector3 m_accumulatedImpulse = Vector3(0.0, 0.0, 0.0); //! Linear_Limit_parameters //!@{ - Vector3 m_limitSoftness; //!< Softness for linear limit - Vector3 m_damping; //!< Damping for linear limit - Vector3 m_restitution; //! Bounce parameter for linear limit + Vector3 m_limitSoftness = Vector3(0.7, 0.7, 0.7); //!< Softness for linear limit + Vector3 m_damping = Vector3(1.0, 1.0, 1.0); //!< Damping for linear limit + Vector3 m_restitution = Vector3(0.5, 0.5, 0.5); //! Bounce parameter for linear limit //!@} - bool enable_limit[3]; - - G6DOFTranslationalLimitMotor3DSW() { - 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; - } + bool enable_limit[3] = { true, true, true }; //! Test limit /*! @@ -180,7 +151,7 @@ protected: Body3DSW *B; }; - Body3DSW *_arr[2]; + Body3DSW *_arr[2] = { nullptr, nullptr }; }; //! relative_frames @@ -208,7 +179,7 @@ protected: protected: //! temporal variables //!@{ - real_t m_timeStep; + real_t m_timeStep = 0.0; Transform3D m_calculatedTransformA; Transform3D m_calculatedTransformB; Vector3 m_calculatedAxisAngleDiff; @@ -216,7 +187,7 @@ protected: Vector3 m_AnchorPos; // point between pivots of bodies A and B to solve linear axes - bool m_useLinearReferenceFrameA; + bool m_useLinearReferenceFrameA = false; //!@} diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp index e2bf2845fe..a45fcf7eb5 100644 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp @@ -34,7 +34,7 @@ Adapted to Godot from the Bullet library. /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org +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. @@ -79,21 +79,6 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D & 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); } @@ -135,21 +120,6 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivo 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); } diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.h b/servers/physics_3d/joints/hinge_joint_3d_sw.h index 572c35266f..a4ceff9ffe 100644 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.h +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.h @@ -40,7 +40,7 @@ Adapted to Godot from the Bullet library. /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org +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. @@ -60,7 +60,7 @@ class HingeJoint3DSW : public Joint3DSW { Body3DSW *B; }; - Body3DSW *_arr[2]; + Body3DSW *_arr[2] = {}; }; JacobianEntry3DSW m_jac[3]; //3 orthogonal linear constraints @@ -69,31 +69,31 @@ class HingeJoint3DSW : public Joint3DSW { Transform3D m_rbAFrame; // constraint axii. Assumes z is hinge axis. Transform3D m_rbBFrame; - real_t m_motorTargetVelocity; - real_t m_maxMotorImpulse; + real_t m_motorTargetVelocity = 0.0; + real_t m_maxMotorImpulse = 0.0; - real_t m_limitSoftness; - real_t m_biasFactor; - real_t m_relaxationFactor; + real_t m_limitSoftness = 0.9; + real_t m_biasFactor = 0.3; + real_t m_relaxationFactor = 1.0; - real_t m_lowerLimit; - real_t m_upperLimit; + real_t m_lowerLimit = Math_PI; + real_t m_upperLimit = -Math_PI; - real_t m_kHinge; + real_t m_kHinge = 0.0; - real_t m_limitSign; - real_t m_correction; + real_t m_limitSign = 0.0; + real_t m_correction = 0.0; - real_t m_accLimitImpulse; + real_t m_accLimitImpulse = 0.0; - real_t tau; + real_t tau = 0.3; - bool m_useLimit; - bool m_angularOnly; - bool m_enableAngularMotor; - bool m_solveLimit; + bool m_useLimit = false; + bool m_angularOnly = false; + bool m_enableAngularMotor = false; + bool m_solveLimit = false; - real_t m_appliedImpulse; + real_t m_appliedImpulse = 0.0; public: virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_HINGE; } diff --git a/servers/physics_3d/joints/jacobian_entry_3d_sw.h b/servers/physics_3d/joints/jacobian_entry_3d_sw.h index 30c80db23f..7294ff78e3 100644 --- a/servers/physics_3d/joints/jacobian_entry_3d_sw.h +++ b/servers/physics_3d/joints/jacobian_entry_3d_sw.h @@ -37,7 +37,7 @@ Adapted to Godot from the Bullet library. /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org +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. @@ -163,7 +163,7 @@ public: 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 m_Adiag = 1.0; }; #endif // JACOBIAN_ENTRY_SW_H diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.cpp b/servers/physics_3d/joints/pin_joint_3d_sw.cpp index 7a713c1161..f41151ec0e 100644 --- a/servers/physics_3d/joints/pin_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/pin_joint_3d_sw.cpp @@ -34,7 +34,7 @@ Adapted to Godot from the Bullet library. /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org +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. @@ -171,11 +171,6 @@ PinJoint3DSW::PinJoint3DSW(Body3DSW *p_body_a, const Vector3 &p_pos_a, Body3DSW m_pivotInA = p_pos_a; m_pivotInB = p_pos_b; - m_tau = 0.3; - m_damping = 1; - m_impulseClamp = 0; - m_appliedImpulse = 0; - A->add_constraint(this, 0); B->add_constraint(this, 1); } diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.h b/servers/physics_3d/joints/pin_joint_3d_sw.h index 09deefc5c4..79af48f2a5 100644 --- a/servers/physics_3d/joints/pin_joint_3d_sw.h +++ b/servers/physics_3d/joints/pin_joint_3d_sw.h @@ -40,7 +40,7 @@ Adapted to Godot from the Bullet library. /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org +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. @@ -60,15 +60,15 @@ class PinJoint3DSW : public Joint3DSW { Body3DSW *B; }; - Body3DSW *_arr[2]; + Body3DSW *_arr[2] = {}; }; - real_t m_tau; //bias - real_t m_damping; - real_t m_impulseClamp; - real_t m_appliedImpulse; + real_t m_tau = 0.3; //bias + real_t m_damping = 1.0; + real_t m_impulseClamp = 0.0; + real_t m_appliedImpulse = 0.0; - JacobianEntry3DSW m_jac[3]; //3 orthogonal linear constraints + JacobianEntry3DSW m_jac[3] = {}; //3 orthogonal linear constraints Vector3 m_pivotInA; Vector3 m_pivotInB; diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.cpp b/servers/physics_3d/joints/slider_joint_3d_sw.cpp index 9f01196c30..e10ed436d5 100644 --- a/servers/physics_3d/joints/slider_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/slider_joint_3d_sw.cpp @@ -34,7 +34,7 @@ Adapted to Godot from the Bullet library. /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org +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. @@ -72,41 +72,6 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { return (y < 0.0f) ? -angle : angle; } -void SliderJoint3DSW::initParams() { - m_lowerLinLimit = real_t(1.0); - m_upperLinLimit = real_t(-1.0); - m_lowerAngLimit = real_t(0.); - m_upperAngLimit = real_t(0.); - m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingDirLin = real_t(0.); - m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingDirAng = real_t(0.); - m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING; - m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING; - m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING; - m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING; - - m_poweredLinMotor = false; - m_targetLinMotorVelocity = real_t(0.); - m_maxLinMotorForce = real_t(0.); - m_accumulatedLinMotorImpulse = real_t(0.0); - - m_poweredAngMotor = false; - m_targetAngMotorVelocity = real_t(0.); - m_maxAngMotorForce = real_t(0.); - m_accumulatedAngMotorImpulse = real_t(0.0); -} // SliderJointSW::initParams() - //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- @@ -120,8 +85,6 @@ SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D A->add_constraint(this, 0); B->add_constraint(this, 1); - - initParams(); } // SliderJointSW::SliderJointSW() //----------------------------------------------------------------------------- diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.h b/servers/physics_3d/joints/slider_joint_3d_sw.h index f09476f570..d32ad9469e 100644 --- a/servers/physics_3d/joints/slider_joint_3d_sw.h +++ b/servers/physics_3d/joints/slider_joint_3d_sw.h @@ -40,7 +40,7 @@ Adapted to Godot from the Bullet library. /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org +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. @@ -73,53 +73,53 @@ protected: Body3DSW *B; }; - Body3DSW *_arr[2]; + Body3DSW *_arr[2] = { nullptr, nullptr }; }; Transform3D m_frameInA; Transform3D m_frameInB; // linear limits - real_t m_lowerLinLimit; - real_t m_upperLinLimit; + real_t m_lowerLinLimit = 1.0; + real_t m_upperLinLimit = -1.0; // angular limits - real_t m_lowerAngLimit; - real_t m_upperAngLimit; + real_t m_lowerAngLimit = 0.0; + real_t m_upperAngLimit = 0.0; // softness, restitution and damping for different cases // DirLin - moving inside linear limits // LimLin - hitting linear limit // DirAng - moving inside angular limits // LimAng - hitting angular limit // OrthoLin, OrthoAng - against constraint axis - real_t m_softnessDirLin; - real_t m_restitutionDirLin; - real_t m_dampingDirLin; - real_t m_softnessDirAng; - real_t m_restitutionDirAng; - real_t m_dampingDirAng; - real_t m_softnessLimLin; - real_t m_restitutionLimLin; - real_t m_dampingLimLin; - real_t m_softnessLimAng; - real_t m_restitutionLimAng; - real_t m_dampingLimAng; - real_t m_softnessOrthoLin; - real_t m_restitutionOrthoLin; - real_t m_dampingOrthoLin; - real_t m_softnessOrthoAng; - real_t m_restitutionOrthoAng; - real_t m_dampingOrthoAng; + real_t m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingDirLin = 0.0; + real_t m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingDirAng = 0.0; + real_t m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING; + real_t m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING; + real_t m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING; + real_t m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING; // for interlal use - bool m_solveLinLim; - bool m_solveAngLim; + bool m_solveLinLim = false; + bool m_solveAngLim = false; - JacobianEntry3DSW m_jacLin[3]; - real_t m_jacLinDiagABInv[3]; + JacobianEntry3DSW m_jacLin[3] = {}; + real_t m_jacLinDiagABInv[3] = {}; - JacobianEntry3DSW m_jacAng[3]; + JacobianEntry3DSW m_jacAng[3] = {}; - real_t m_timeStep; + real_t m_timeStep = 0.0; Transform3D m_calculatedTransformA; Transform3D m_calculatedTransformB; @@ -132,23 +132,20 @@ protected: Vector3 m_relPosA; Vector3 m_relPosB; - real_t m_linPos; + real_t m_linPos = 0.0; - real_t m_angDepth; - real_t m_kAngle; + real_t m_angDepth = 0.0; + real_t m_kAngle = 0.0; - bool m_poweredLinMotor; - real_t m_targetLinMotorVelocity; - real_t m_maxLinMotorForce; - real_t m_accumulatedLinMotorImpulse; + bool m_poweredLinMotor = false; + real_t m_targetLinMotorVelocity = 0.0; + real_t m_maxLinMotorForce = 0.0; + real_t m_accumulatedLinMotorImpulse = 0.0; - bool m_poweredAngMotor; - real_t m_targetAngMotorVelocity; - real_t m_maxAngMotorForce; - real_t m_accumulatedAngMotorImpulse; - - //------------------------ - void initParams(); + bool m_poweredAngMotor = false; + real_t m_targetAngMotorVelocity = 0.0; + real_t m_maxAngMotorForce = 0.0; + real_t m_accumulatedAngMotorImpulse = 0.0; public: // constructors diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp index 8de54c5c48..b16e199a03 100644 --- a/servers/physics_3d/physics_server_3d_sw.cpp +++ b/servers/physics_3d/physics_server_3d_sw.cpp @@ -43,8 +43,8 @@ #define FLUSH_QUERY_CHECK(m_object) \ ERR_FAIL_COND_MSG(m_object->get_space() && flushing_queries, "Can't change this state while flushing queries. Use call_deferred() or set_deferred() to change monitoring state instead."); -RID PhysicsServer3DSW::plane_shape_create() { - Shape3DSW *shape = memnew(PlaneShape3DSW); +RID PhysicsServer3DSW::world_boundary_shape_create() { + Shape3DSW *shape = memnew(WorldBoundaryShape3DSW); RID rid = shape_owner.make_rid(shape); shape->set_self(rid); return rid; @@ -102,25 +102,25 @@ RID PhysicsServer3DSW::custom_shape_create() { } void PhysicsServer3DSW::shape_set_data(RID p_shape, const Variant &p_data) { - Shape3DSW *shape = shape_owner.getornull(p_shape); + Shape3DSW *shape = shape_owner.get_or_null(p_shape); ERR_FAIL_COND(!shape); shape->set_data(p_data); }; void PhysicsServer3DSW::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) { - Shape3DSW *shape = shape_owner.getornull(p_shape); + Shape3DSW *shape = shape_owner.get_or_null(p_shape); ERR_FAIL_COND(!shape); shape->set_custom_bias(p_bias); } PhysicsServer3D::ShapeType PhysicsServer3DSW::shape_get_type(RID p_shape) const { - const Shape3DSW *shape = shape_owner.getornull(p_shape); + const Shape3DSW *shape = shape_owner.get_or_null(p_shape); ERR_FAIL_COND_V(!shape, SHAPE_CUSTOM); return shape->get_type(); }; Variant PhysicsServer3DSW::shape_get_data(RID p_shape) const { - const Shape3DSW *shape = shape_owner.getornull(p_shape); + const Shape3DSW *shape = shape_owner.get_or_null(p_shape); ERR_FAIL_COND_V(!shape, Variant()); ERR_FAIL_COND_V(!shape->is_configured(), Variant()); return shape->get_data(); @@ -134,7 +134,7 @@ real_t PhysicsServer3DSW::shape_get_margin(RID p_shape) const { } real_t PhysicsServer3DSW::shape_get_custom_solver_bias(RID p_shape) const { - const Shape3DSW *shape = shape_owner.getornull(p_shape); + const Shape3DSW *shape = shape_owner.get_or_null(p_shape); ERR_FAIL_COND_V(!shape, 0); return shape->get_custom_bias(); } @@ -144,7 +144,7 @@ RID PhysicsServer3DSW::space_create() { RID id = space_owner.make_rid(space); space->set_self(id); RID area_id = area_create(); - Area3DSW *area = area_owner.getornull(area_id); + Area3DSW *area = area_owner.get_or_null(area_id); ERR_FAIL_COND_V(!area, RID()); space->set_default_area(area); area->set_space(space); @@ -158,7 +158,7 @@ RID PhysicsServer3DSW::space_create() { }; void PhysicsServer3DSW::space_set_active(RID p_space, bool p_active) { - Space3DSW *space = space_owner.getornull(p_space); + Space3DSW *space = space_owner.get_or_null(p_space); ERR_FAIL_COND(!space); if (p_active) { active_spaces.insert(space); @@ -168,27 +168,27 @@ void PhysicsServer3DSW::space_set_active(RID p_space, bool p_active) { } bool PhysicsServer3DSW::space_is_active(RID p_space) const { - const Space3DSW *space = space_owner.getornull(p_space); + const Space3DSW *space = space_owner.get_or_null(p_space); ERR_FAIL_COND_V(!space, false); return active_spaces.has(space); } void PhysicsServer3DSW::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) { - Space3DSW *space = space_owner.getornull(p_space); + Space3DSW *space = space_owner.get_or_null(p_space); ERR_FAIL_COND(!space); space->set_param(p_param, p_value); } real_t PhysicsServer3DSW::space_get_param(RID p_space, SpaceParameter p_param) const { - const Space3DSW *space = space_owner.getornull(p_space); + const Space3DSW *space = space_owner.get_or_null(p_space); ERR_FAIL_COND_V(!space, 0); return space->get_param(p_param); } PhysicsDirectSpaceState3D *PhysicsServer3DSW::space_get_direct_state(RID p_space) { - Space3DSW *space = space_owner.getornull(p_space); + Space3DSW *space = space_owner.get_or_null(p_space); ERR_FAIL_COND_V(!space, nullptr); ERR_FAIL_COND_V_MSG((using_threads && !doing_sync) || space->is_locked(), nullptr, "Space state is inaccessible right now, wait for iteration or physics process notification."); @@ -196,19 +196,19 @@ PhysicsDirectSpaceState3D *PhysicsServer3DSW::space_get_direct_state(RID p_space } void PhysicsServer3DSW::space_set_debug_contacts(RID p_space, int p_max_contacts) { - Space3DSW *space = space_owner.getornull(p_space); + Space3DSW *space = space_owner.get_or_null(p_space); ERR_FAIL_COND(!space); space->set_debug_contacts(p_max_contacts); } Vector<Vector3> PhysicsServer3DSW::space_get_contacts(RID p_space) const { - Space3DSW *space = space_owner.getornull(p_space); + Space3DSW *space = space_owner.get_or_null(p_space); ERR_FAIL_COND_V(!space, Vector<Vector3>()); return space->get_debug_contacts(); } int PhysicsServer3DSW::space_get_contact_count(RID p_space) const { - Space3DSW *space = space_owner.getornull(p_space); + Space3DSW *space = space_owner.get_or_null(p_space); ERR_FAIL_COND_V(!space, 0); return space->get_debug_contact_count(); } @@ -221,12 +221,12 @@ RID PhysicsServer3DSW::area_create() { }; void PhysicsServer3DSW::area_set_space(RID p_area, RID p_space) { - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); Space3DSW *space = nullptr; if (p_space.is_valid()) { - space = space_owner.getornull(p_space); + space = space_owner.get_or_null(p_space); ERR_FAIL_COND(!space); } @@ -239,7 +239,7 @@ void PhysicsServer3DSW::area_set_space(RID p_area, RID p_space) { }; RID PhysicsServer3DSW::area_get_space(RID p_area) const { - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND_V(!area, RID()); Space3DSW *space = area->get_space(); @@ -250,34 +250,34 @@ RID PhysicsServer3DSW::area_get_space(RID p_area) const { }; void PhysicsServer3DSW::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) { - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); area->set_space_override_mode(p_mode); } PhysicsServer3D::AreaSpaceOverrideMode PhysicsServer3DSW::area_get_space_override_mode(RID p_area) const { - const Area3DSW *area = area_owner.getornull(p_area); + const Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND_V(!area, AREA_SPACE_OVERRIDE_DISABLED); return area->get_space_override_mode(); } void PhysicsServer3DSW::area_add_shape(RID p_area, RID p_shape, const Transform3D &p_transform, bool p_disabled) { - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); - Shape3DSW *shape = shape_owner.getornull(p_shape); + Shape3DSW *shape = shape_owner.get_or_null(p_shape); ERR_FAIL_COND(!shape); area->add_shape(shape, p_transform, p_disabled); } void PhysicsServer3DSW::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) { - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); - Shape3DSW *shape = shape_owner.getornull(p_shape); + Shape3DSW *shape = shape_owner.get_or_null(p_shape); ERR_FAIL_COND(!shape); ERR_FAIL_COND(!shape->is_configured()); @@ -285,21 +285,21 @@ void PhysicsServer3DSW::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) } void PhysicsServer3DSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform3D &p_transform) { - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); area->set_shape_transform(p_shape_idx, p_transform); } int PhysicsServer3DSW::area_get_shape_count(RID p_area) const { - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND_V(!area, -1); return area->get_shape_count(); } RID PhysicsServer3DSW::area_get_shape(RID p_area, int p_shape_idx) const { - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND_V(!area, RID()); Shape3DSW *shape = area->get_shape(p_shape_idx); @@ -309,21 +309,21 @@ RID PhysicsServer3DSW::area_get_shape(RID p_area, int p_shape_idx) const { } Transform3D PhysicsServer3DSW::area_get_shape_transform(RID p_area, int p_shape_idx) const { - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND_V(!area, Transform3D()); return area->get_shape_transform(p_shape_idx); } void PhysicsServer3DSW::area_remove_shape(RID p_area, int p_shape_idx) { - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); area->remove_shape(p_shape_idx); } void PhysicsServer3DSW::area_clear_shapes(RID p_area) { - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); while (area->get_shape_count()) { @@ -332,7 +332,7 @@ void PhysicsServer3DSW::area_clear_shapes(RID p_area) { } void PhysicsServer3DSW::area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) { - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); ERR_FAIL_INDEX(p_shape_idx, area->get_shape_count()); FLUSH_QUERY_CHECK(area); @@ -341,74 +341,74 @@ void PhysicsServer3DSW::area_set_shape_disabled(RID p_area, int p_shape_idx, boo void PhysicsServer3DSW::area_attach_object_instance_id(RID p_area, ObjectID p_id) { if (space_owner.owns(p_area)) { - Space3DSW *space = space_owner.getornull(p_area); + Space3DSW *space = space_owner.get_or_null(p_area); p_area = space->get_default_area()->get_self(); } - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); area->set_instance_id(p_id); } ObjectID PhysicsServer3DSW::area_get_object_instance_id(RID p_area) const { if (space_owner.owns(p_area)) { - Space3DSW *space = space_owner.getornull(p_area); + Space3DSW *space = space_owner.get_or_null(p_area); p_area = space->get_default_area()->get_self(); } - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND_V(!area, ObjectID()); return area->get_instance_id(); } void PhysicsServer3DSW::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) { if (space_owner.owns(p_area)) { - Space3DSW *space = space_owner.getornull(p_area); + Space3DSW *space = space_owner.get_or_null(p_area); p_area = space->get_default_area()->get_self(); } - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); area->set_param(p_param, p_value); }; void PhysicsServer3DSW::area_set_transform(RID p_area, const Transform3D &p_transform) { - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); area->set_transform(p_transform); }; Variant PhysicsServer3DSW::area_get_param(RID p_area, AreaParameter p_param) const { if (space_owner.owns(p_area)) { - Space3DSW *space = space_owner.getornull(p_area); + Space3DSW *space = space_owner.get_or_null(p_area); p_area = space->get_default_area()->get_self(); } - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND_V(!area, Variant()); return area->get_param(p_param); }; Transform3D PhysicsServer3DSW::area_get_transform(RID p_area) const { - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND_V(!area, Transform3D()); return area->get_transform(); }; void PhysicsServer3DSW::area_set_collision_layer(RID p_area, uint32_t p_layer) { - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); area->set_collision_layer(p_layer); } void PhysicsServer3DSW::area_set_collision_mask(RID p_area, uint32_t p_mask) { - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); area->set_collision_mask(p_mask); } void PhysicsServer3DSW::area_set_monitorable(RID p_area, bool p_monitorable) { - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); FLUSH_QUERY_CHECK(area); @@ -416,21 +416,21 @@ void PhysicsServer3DSW::area_set_monitorable(RID p_area, bool p_monitorable) { } void PhysicsServer3DSW::area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); area->set_monitor_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method); } void PhysicsServer3DSW::area_set_ray_pickable(RID p_area, bool p_enable) { - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); area->set_ray_pickable(p_enable); } void PhysicsServer3DSW::area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { - Area3DSW *area = area_owner.getornull(p_area); + Area3DSW *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); area->set_area_monitor_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method); @@ -446,12 +446,12 @@ RID PhysicsServer3DSW::body_create() { }; void PhysicsServer3DSW::body_set_space(RID p_body, RID p_space) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); Space3DSW *space = nullptr; if (p_space.is_valid()) { - space = space_owner.getornull(p_space); + space = space_owner.get_or_null(p_space); ERR_FAIL_COND(!space); } @@ -464,7 +464,7 @@ void PhysicsServer3DSW::body_set_space(RID p_body, RID p_space) { }; RID PhysicsServer3DSW::body_get_space(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, RID()); Space3DSW *space = body->get_space(); @@ -475,55 +475,55 @@ RID PhysicsServer3DSW::body_get_space(RID p_body) const { }; void PhysicsServer3DSW::body_set_mode(RID p_body, BodyMode p_mode) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->set_mode(p_mode); }; PhysicsServer3D::BodyMode PhysicsServer3DSW::body_get_mode(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, BODY_MODE_STATIC); return body->get_mode(); }; void PhysicsServer3DSW::body_add_shape(RID p_body, RID p_shape, const Transform3D &p_transform, bool p_disabled) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); - Shape3DSW *shape = shape_owner.getornull(p_shape); + Shape3DSW *shape = shape_owner.get_or_null(p_shape); ERR_FAIL_COND(!shape); body->add_shape(shape, p_transform, p_disabled); } void PhysicsServer3DSW::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); - Shape3DSW *shape = shape_owner.getornull(p_shape); + Shape3DSW *shape = shape_owner.get_or_null(p_shape); ERR_FAIL_COND(!shape); ERR_FAIL_COND(!shape->is_configured()); body->set_shape(p_shape_idx, shape); } void PhysicsServer3DSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform3D &p_transform) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->set_shape_transform(p_shape_idx, p_transform); } int PhysicsServer3DSW::body_get_shape_count(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, -1); return body->get_shape_count(); } RID PhysicsServer3DSW::body_get_shape(RID p_body, int p_shape_idx) const { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, RID()); Shape3DSW *shape = body->get_shape(p_shape_idx); @@ -533,7 +533,7 @@ RID PhysicsServer3DSW::body_get_shape(RID p_body, int p_shape_idx) const { } void PhysicsServer3DSW::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count()); FLUSH_QUERY_CHECK(body); @@ -542,21 +542,21 @@ void PhysicsServer3DSW::body_set_shape_disabled(RID p_body, int p_shape_idx, boo } Transform3D PhysicsServer3DSW::body_get_shape_transform(RID p_body, int p_shape_idx) const { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, Transform3D()); return body->get_shape_transform(p_shape_idx); } void PhysicsServer3DSW::body_remove_shape(RID p_body, int p_shape_idx) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->remove_shape(p_shape_idx); } void PhysicsServer3DSW::body_clear_shapes(RID p_body) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); while (body->get_shape_count()) { @@ -565,21 +565,21 @@ void PhysicsServer3DSW::body_clear_shapes(RID p_body) { } void PhysicsServer3DSW::body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->set_continuous_collision_detection(p_enable); } bool PhysicsServer3DSW::body_is_continuous_collision_detection_enabled(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, false); return body->is_continuous_collision_detection_enabled(); } void PhysicsServer3DSW::body_set_collision_layer(RID p_body, uint32_t p_layer) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->set_collision_layer(p_layer); @@ -587,14 +587,14 @@ void PhysicsServer3DSW::body_set_collision_layer(RID p_body, uint32_t p_layer) { } uint32_t PhysicsServer3DSW::body_get_collision_layer(RID p_body) const { - const Body3DSW *body = body_owner.getornull(p_body); + const Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, 0); return body->get_collision_layer(); } void PhysicsServer3DSW::body_set_collision_mask(RID p_body, uint32_t p_mask) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->set_collision_mask(p_mask); @@ -602,20 +602,20 @@ void PhysicsServer3DSW::body_set_collision_mask(RID p_body, uint32_t p_mask) { } uint32_t PhysicsServer3DSW::body_get_collision_mask(RID p_body) const { - const Body3DSW *body = body_owner.getornull(p_body); + const Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, 0); return body->get_collision_mask(); } void PhysicsServer3DSW::body_attach_object_instance_id(RID p_body, ObjectID p_id) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); if (body) { body->set_instance_id(p_id); return; } - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); if (soft_body) { soft_body->set_instance_id(p_id); return; @@ -625,61 +625,61 @@ void PhysicsServer3DSW::body_attach_object_instance_id(RID p_body, ObjectID p_id }; ObjectID PhysicsServer3DSW::body_get_object_instance_id(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, ObjectID()); return body->get_instance_id(); }; void PhysicsServer3DSW::body_set_user_flags(RID p_body, uint32_t p_flags) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); }; uint32_t PhysicsServer3DSW::body_get_user_flags(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, 0); return 0; }; void PhysicsServer3DSW::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->set_param(p_param, p_value); }; Variant PhysicsServer3DSW::body_get_param(RID p_body, BodyParameter p_param) const { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, 0); return body->get_param(p_param); }; void PhysicsServer3DSW::body_reset_mass_properties(RID p_body) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); return body->reset_mass_properties(); } void PhysicsServer3DSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->set_state(p_state, p_variant); }; Variant PhysicsServer3DSW::body_get_state(RID p_body, BodyState p_state) const { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, Variant()); return body->get_state(p_state); }; void PhysicsServer3DSW::body_set_applied_force(RID p_body, const Vector3 &p_force) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->set_applied_force(p_force); @@ -687,13 +687,13 @@ void PhysicsServer3DSW::body_set_applied_force(RID p_body, const Vector3 &p_forc }; Vector3 PhysicsServer3DSW::body_get_applied_force(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, Vector3()); return body->get_applied_force(); }; void PhysicsServer3DSW::body_set_applied_torque(RID p_body, const Vector3 &p_torque) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->set_applied_torque(p_torque); @@ -701,14 +701,14 @@ void PhysicsServer3DSW::body_set_applied_torque(RID p_body, const Vector3 &p_tor }; Vector3 PhysicsServer3DSW::body_get_applied_torque(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, Vector3()); return body->get_applied_torque(); }; void PhysicsServer3DSW::body_add_central_force(RID p_body, const Vector3 &p_force) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->add_central_force(p_force); @@ -716,7 +716,7 @@ void PhysicsServer3DSW::body_add_central_force(RID p_body, const Vector3 &p_forc } void PhysicsServer3DSW::body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->add_force(p_force, p_position); @@ -724,7 +724,7 @@ void PhysicsServer3DSW::body_add_force(RID p_body, const Vector3 &p_force, const }; void PhysicsServer3DSW::body_add_torque(RID p_body, const Vector3 &p_torque) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->add_torque(p_torque); @@ -732,7 +732,7 @@ void PhysicsServer3DSW::body_add_torque(RID p_body, const Vector3 &p_torque) { }; void PhysicsServer3DSW::body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); _update_shapes(); @@ -742,7 +742,7 @@ void PhysicsServer3DSW::body_apply_central_impulse(RID p_body, const Vector3 &p_ } void PhysicsServer3DSW::body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); _update_shapes(); @@ -752,7 +752,7 @@ void PhysicsServer3DSW::body_apply_impulse(RID p_body, const Vector3 &p_impulse, }; void PhysicsServer3DSW::body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); _update_shapes(); @@ -762,7 +762,7 @@ void PhysicsServer3DSW::body_apply_torque_impulse(RID p_body, const Vector3 &p_i }; void PhysicsServer3DSW::body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); _update_shapes(); @@ -776,7 +776,7 @@ void PhysicsServer3DSW::body_set_axis_velocity(RID p_body, const Vector3 &p_axis }; void PhysicsServer3DSW::body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->set_axis_lock(p_axis, p_lock); @@ -784,13 +784,13 @@ void PhysicsServer3DSW::body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_l } bool PhysicsServer3DSW::body_is_axis_locked(RID p_body, BodyAxis p_axis) const { - const Body3DSW *body = body_owner.getornull(p_body); + const Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, 0); return body->is_axis_locked(p_axis); } void PhysicsServer3DSW::body_add_collision_exception(RID p_body, RID p_body_b) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->add_exception(p_body_b); @@ -798,7 +798,7 @@ void PhysicsServer3DSW::body_add_collision_exception(RID p_body, RID p_body_b) { }; void PhysicsServer3DSW::body_remove_collision_exception(RID p_body, RID p_body_b) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->remove_exception(p_body_b); @@ -806,7 +806,7 @@ void PhysicsServer3DSW::body_remove_collision_exception(RID p_body, RID p_body_b }; void PhysicsServer3DSW::body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); for (int i = 0; i < body->get_exceptions().size(); i++) { @@ -815,74 +815,74 @@ void PhysicsServer3DSW::body_get_collision_exceptions(RID p_body, List<RID> *p_e }; void PhysicsServer3DSW::body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); }; real_t PhysicsServer3DSW::body_get_contacts_reported_depth_threshold(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, 0); return 0; }; void PhysicsServer3DSW::body_set_omit_force_integration(RID p_body, bool p_omit) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->set_omit_force_integration(p_omit); }; bool PhysicsServer3DSW::body_is_omitting_force_integration(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, false); return body->get_omit_force_integration(); }; void PhysicsServer3DSW::body_set_max_contacts_reported(RID p_body, int p_contacts) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->set_max_contacts_reported(p_contacts); } int PhysicsServer3DSW::body_get_max_contacts_reported(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, -1); return body->get_max_contacts_reported(); } void PhysicsServer3DSW::body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->set_state_sync_callback(p_instance, p_callback); } void PhysicsServer3DSW::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->set_force_integration_callback(p_callable, p_udata); } void PhysicsServer3DSW::body_set_ray_pickable(RID p_body, bool p_enable) { - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->set_ray_pickable(p_enable); } -bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin, MotionResult *r_result, bool p_collide_separation_ray, const Set<RID> &p_exclude) { - Body3DSW *body = body_owner.getornull(p_body); +bool PhysicsServer3DSW::body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result) { + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, false); ERR_FAIL_COND_V(!body->get_space(), false); ERR_FAIL_COND_V(body->get_space()->is_locked(), false); _update_shapes(); - return body->get_space()->test_body_motion(body, p_from, p_motion, p_margin, r_result, p_collide_separation_ray, p_exclude); + return body->get_space()->test_body_motion(body, p_parameters, r_result); } PhysicsDirectBodyState3D *PhysicsServer3DSW::body_get_direct_state(RID p_body) { ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); - Body3DSW *body = body_owner.getornull(p_body); + Body3DSW *body = body_owner.get_or_null(p_body); ERR_FAIL_NULL_V(body, nullptr); ERR_FAIL_NULL_V(body->get_space(), nullptr); @@ -901,19 +901,19 @@ RID PhysicsServer3DSW::soft_body_create() { } void PhysicsServer3DSW::soft_body_update_rendering_server(RID p_body, RenderingServerHandler *p_rendering_server_handler) { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND(!soft_body); soft_body->update_rendering_server(p_rendering_server_handler); } void PhysicsServer3DSW::soft_body_set_space(RID p_body, RID p_space) { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND(!soft_body); Space3DSW *space = nullptr; if (p_space.is_valid()) { - space = space_owner.getornull(p_space); + space = space_owner.get_or_null(p_space); ERR_FAIL_COND(!space); } @@ -925,7 +925,7 @@ void PhysicsServer3DSW::soft_body_set_space(RID p_body, RID p_space) { } RID PhysicsServer3DSW::soft_body_get_space(RID p_body) const { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!soft_body, RID()); Space3DSW *space = soft_body->get_space(); @@ -936,49 +936,49 @@ RID PhysicsServer3DSW::soft_body_get_space(RID p_body) const { } void PhysicsServer3DSW::soft_body_set_collision_layer(RID p_body, uint32_t p_layer) { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND(!soft_body); soft_body->set_collision_layer(p_layer); } uint32_t PhysicsServer3DSW::soft_body_get_collision_layer(RID p_body) const { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!soft_body, 0); return soft_body->get_collision_layer(); } void PhysicsServer3DSW::soft_body_set_collision_mask(RID p_body, uint32_t p_mask) { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND(!soft_body); soft_body->set_collision_mask(p_mask); } uint32_t PhysicsServer3DSW::soft_body_get_collision_mask(RID p_body) const { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!soft_body, 0); return soft_body->get_collision_mask(); } void PhysicsServer3DSW::soft_body_add_collision_exception(RID p_body, RID p_body_b) { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND(!soft_body); soft_body->add_exception(p_body_b); } void PhysicsServer3DSW::soft_body_remove_collision_exception(RID p_body, RID p_body_b) { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND(!soft_body); soft_body->remove_exception(p_body_b); } void PhysicsServer3DSW::soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND(!soft_body); for (int i = 0; i < soft_body->get_exceptions().size(); i++) { @@ -987,154 +987,154 @@ void PhysicsServer3DSW::soft_body_get_collision_exceptions(RID p_body, List<RID> } void PhysicsServer3DSW::soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND(!soft_body); soft_body->set_state(p_state, p_variant); } Variant PhysicsServer3DSW::soft_body_get_state(RID p_body, BodyState p_state) const { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!soft_body, Variant()); return soft_body->get_state(p_state); } void PhysicsServer3DSW::soft_body_set_transform(RID p_body, const Transform3D &p_transform) { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND(!soft_body); soft_body->set_state(BODY_STATE_TRANSFORM, p_transform); } void PhysicsServer3DSW::soft_body_set_ray_pickable(RID p_body, bool p_enable) { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND(!soft_body); soft_body->set_ray_pickable(p_enable); } void PhysicsServer3DSW::soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND(!soft_body); soft_body->set_iteration_count(p_simulation_precision); } int PhysicsServer3DSW::soft_body_get_simulation_precision(RID p_body) const { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!soft_body, 0.f); return soft_body->get_iteration_count(); } void PhysicsServer3DSW::soft_body_set_total_mass(RID p_body, real_t p_total_mass) { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND(!soft_body); soft_body->set_total_mass(p_total_mass); } real_t PhysicsServer3DSW::soft_body_get_total_mass(RID p_body) const { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!soft_body, 0.f); return soft_body->get_total_mass(); } void PhysicsServer3DSW::soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND(!soft_body); soft_body->set_linear_stiffness(p_stiffness); } real_t PhysicsServer3DSW::soft_body_get_linear_stiffness(RID p_body) const { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!soft_body, 0.f); return soft_body->get_linear_stiffness(); } void PhysicsServer3DSW::soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND(!soft_body); soft_body->set_pressure_coefficient(p_pressure_coefficient); } real_t PhysicsServer3DSW::soft_body_get_pressure_coefficient(RID p_body) const { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!soft_body, 0.f); return soft_body->get_pressure_coefficient(); } void PhysicsServer3DSW::soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND(!soft_body); soft_body->set_damping_coefficient(p_damping_coefficient); } real_t PhysicsServer3DSW::soft_body_get_damping_coefficient(RID p_body) const { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!soft_body, 0.f); return soft_body->get_damping_coefficient(); } void PhysicsServer3DSW::soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND(!soft_body); soft_body->set_drag_coefficient(p_drag_coefficient); } real_t PhysicsServer3DSW::soft_body_get_drag_coefficient(RID p_body) const { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!soft_body, 0.f); return soft_body->get_drag_coefficient(); } -void PhysicsServer3DSW::soft_body_set_mesh(RID p_body, const REF &p_mesh) { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); +void PhysicsServer3DSW::soft_body_set_mesh(RID p_body, RID p_mesh) { + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND(!soft_body); soft_body->set_mesh(p_mesh); } AABB PhysicsServer3DSW::soft_body_get_bounds(RID p_body) const { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!soft_body, AABB()); return soft_body->get_bounds(); } void PhysicsServer3DSW::soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND(!soft_body); soft_body->set_vertex_position(p_point_index, p_global_position); } Vector3 PhysicsServer3DSW::soft_body_get_point_global_position(RID p_body, int p_point_index) const { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!soft_body, Vector3()); return soft_body->get_vertex_position(p_point_index); } void PhysicsServer3DSW::soft_body_remove_all_pinned_points(RID p_body) { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND(!soft_body); soft_body->unpin_all_vertices(); } void PhysicsServer3DSW::soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND(!soft_body); if (p_pin) { @@ -1145,7 +1145,7 @@ void PhysicsServer3DSW::soft_body_pin_point(RID p_body, int p_point_index, bool } bool PhysicsServer3DSW::soft_body_is_point_pinned(RID p_body, int p_point_index) const { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!soft_body, false); return soft_body->is_vertex_pinned(p_point_index); @@ -1161,7 +1161,7 @@ RID PhysicsServer3DSW::joint_create() { } void PhysicsServer3DSW::joint_clear(RID p_joint) { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); if (joint->get_type() != JOINT_TYPE_MAX) { Joint3DSW *empty_joint = memnew(Joint3DSW); empty_joint->copy_settings_from(joint); @@ -1172,7 +1172,7 @@ void PhysicsServer3DSW::joint_clear(RID p_joint) { } void PhysicsServer3DSW::joint_make_pin(RID p_joint, RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) { - Body3DSW *body_A = body_owner.getornull(p_body_A); + Body3DSW *body_A = body_owner.get_or_null(p_body_A); ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { @@ -1180,12 +1180,12 @@ void PhysicsServer3DSW::joint_make_pin(RID p_joint, RID p_body_A, const Vector3 p_body_B = body_A->get_space()->get_static_global_body(); } - Body3DSW *body_B = body_owner.getornull(p_body_B); + Body3DSW *body_B = body_owner.get_or_null(p_body_B); ERR_FAIL_COND(!body_B); ERR_FAIL_COND(body_A == body_B); - Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + Joint3DSW *prev_joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(PinJoint3DSW(body_A, p_local_A, body_B, p_local_B)); @@ -1196,7 +1196,7 @@ void PhysicsServer3DSW::joint_make_pin(RID p_joint, RID p_body_A, const Vector3 } void PhysicsServer3DSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND(!joint); ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); @@ -1204,7 +1204,7 @@ void PhysicsServer3DSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, } real_t PhysicsServer3DSW::pin_joint_get_param(RID p_joint, PinJointParam p_param) const { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND_V(!joint, 0); ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, 0); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); @@ -1212,7 +1212,7 @@ real_t PhysicsServer3DSW::pin_joint_get_param(RID p_joint, PinJointParam p_param } void PhysicsServer3DSW::pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND(!joint); ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); @@ -1220,7 +1220,7 @@ void PhysicsServer3DSW::pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) { } Vector3 PhysicsServer3DSW::pin_joint_get_local_a(RID p_joint) const { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND_V(!joint, Vector3()); ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, Vector3()); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); @@ -1228,7 +1228,7 @@ Vector3 PhysicsServer3DSW::pin_joint_get_local_a(RID p_joint) const { } void PhysicsServer3DSW::pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND(!joint); ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); @@ -1236,7 +1236,7 @@ void PhysicsServer3DSW::pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) { } Vector3 PhysicsServer3DSW::pin_joint_get_local_b(RID p_joint) const { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND_V(!joint, Vector3()); ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, Vector3()); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); @@ -1244,7 +1244,7 @@ Vector3 PhysicsServer3DSW::pin_joint_get_local_b(RID p_joint) const { } void PhysicsServer3DSW::joint_make_hinge(RID p_joint, RID p_body_A, const Transform3D &p_frame_A, RID p_body_B, const Transform3D &p_frame_B) { - Body3DSW *body_A = body_owner.getornull(p_body_A); + Body3DSW *body_A = body_owner.get_or_null(p_body_A); ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { @@ -1252,12 +1252,12 @@ void PhysicsServer3DSW::joint_make_hinge(RID p_joint, RID p_body_A, const Transf p_body_B = body_A->get_space()->get_static_global_body(); } - Body3DSW *body_B = body_owner.getornull(p_body_B); + Body3DSW *body_B = body_owner.get_or_null(p_body_B); ERR_FAIL_COND(!body_B); ERR_FAIL_COND(body_A == body_B); - Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + Joint3DSW *prev_joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(HingeJoint3DSW(body_A, body_B, p_frame_A, p_frame_B)); @@ -1268,7 +1268,7 @@ void PhysicsServer3DSW::joint_make_hinge(RID p_joint, RID p_body_A, const Transf } void PhysicsServer3DSW::joint_make_hinge_simple(RID p_joint, RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) { - Body3DSW *body_A = body_owner.getornull(p_body_A); + Body3DSW *body_A = body_owner.get_or_null(p_body_A); ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { @@ -1276,12 +1276,12 @@ void PhysicsServer3DSW::joint_make_hinge_simple(RID p_joint, RID p_body_A, const p_body_B = body_A->get_space()->get_static_global_body(); } - Body3DSW *body_B = body_owner.getornull(p_body_B); + Body3DSW *body_B = body_owner.get_or_null(p_body_B); ERR_FAIL_COND(!body_B); ERR_FAIL_COND(body_A == body_B); - Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + Joint3DSW *prev_joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(HingeJoint3DSW(body_A, body_B, p_pivot_A, p_pivot_B, p_axis_A, p_axis_B)); @@ -1292,7 +1292,7 @@ void PhysicsServer3DSW::joint_make_hinge_simple(RID p_joint, RID p_body_A, const } void PhysicsServer3DSW::hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND(!joint); ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE); HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint); @@ -1300,7 +1300,7 @@ void PhysicsServer3DSW::hinge_joint_set_param(RID p_joint, HingeJointParam p_par } real_t PhysicsServer3DSW::hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND_V(!joint, 0); ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, 0); HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint); @@ -1308,7 +1308,7 @@ real_t PhysicsServer3DSW::hinge_joint_get_param(RID p_joint, HingeJointParam p_p } void PhysicsServer3DSW::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND(!joint); ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE); HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint); @@ -1316,7 +1316,7 @@ void PhysicsServer3DSW::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, } bool PhysicsServer3DSW::hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND_V(!joint, false); ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, false); HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint); @@ -1324,19 +1324,19 @@ bool PhysicsServer3DSW::hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) } void PhysicsServer3DSW::joint_set_solver_priority(RID p_joint, int p_priority) { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND(!joint); joint->set_priority(p_priority); } int PhysicsServer3DSW::joint_get_solver_priority(RID p_joint) const { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND_V(!joint, 0); return joint->get_priority(); } void PhysicsServer3DSW::joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND(!joint); joint->disable_collisions_between_bodies(p_disable); @@ -1356,20 +1356,20 @@ void PhysicsServer3DSW::joint_disable_collisions_between_bodies(RID p_joint, con } bool PhysicsServer3DSW::joint_is_disabled_collisions_between_bodies(RID p_joint) const { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND_V(!joint, true); return joint->is_disabled_collisions_between_bodies(); } PhysicsServer3DSW::JointType PhysicsServer3DSW::joint_get_type(RID p_joint) const { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND_V(!joint, JOINT_TYPE_PIN); return joint->get_type(); } void PhysicsServer3DSW::joint_make_slider(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) { - Body3DSW *body_A = body_owner.getornull(p_body_A); + Body3DSW *body_A = body_owner.get_or_null(p_body_A); ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { @@ -1377,12 +1377,12 @@ void PhysicsServer3DSW::joint_make_slider(RID p_joint, RID p_body_A, const Trans p_body_B = body_A->get_space()->get_static_global_body(); } - Body3DSW *body_B = body_owner.getornull(p_body_B); + Body3DSW *body_B = body_owner.get_or_null(p_body_B); ERR_FAIL_COND(!body_B); ERR_FAIL_COND(body_A == body_B); - Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + Joint3DSW *prev_joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(SliderJoint3DSW(body_A, body_B, p_local_frame_A, p_local_frame_B)); @@ -1393,7 +1393,7 @@ void PhysicsServer3DSW::joint_make_slider(RID p_joint, RID p_body_A, const Trans } void PhysicsServer3DSW::slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND(!joint); ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_SLIDER); SliderJoint3DSW *slider_joint = static_cast<SliderJoint3DSW *>(joint); @@ -1401,7 +1401,7 @@ void PhysicsServer3DSW::slider_joint_set_param(RID p_joint, SliderJointParam p_p } real_t PhysicsServer3DSW::slider_joint_get_param(RID p_joint, SliderJointParam p_param) const { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND_V(!joint, 0); ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0); SliderJoint3DSW *slider_joint = static_cast<SliderJoint3DSW *>(joint); @@ -1409,7 +1409,7 @@ real_t PhysicsServer3DSW::slider_joint_get_param(RID p_joint, SliderJointParam p } void PhysicsServer3DSW::joint_make_cone_twist(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) { - Body3DSW *body_A = body_owner.getornull(p_body_A); + Body3DSW *body_A = body_owner.get_or_null(p_body_A); ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { @@ -1417,12 +1417,12 @@ void PhysicsServer3DSW::joint_make_cone_twist(RID p_joint, RID p_body_A, const T p_body_B = body_A->get_space()->get_static_global_body(); } - Body3DSW *body_B = body_owner.getornull(p_body_B); + Body3DSW *body_B = body_owner.get_or_null(p_body_B); ERR_FAIL_COND(!body_B); ERR_FAIL_COND(body_A == body_B); - Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + Joint3DSW *prev_joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(ConeTwistJoint3DSW(body_A, body_B, p_local_frame_A, p_local_frame_B)); @@ -1433,7 +1433,7 @@ void PhysicsServer3DSW::joint_make_cone_twist(RID p_joint, RID p_body_A, const T } void PhysicsServer3DSW::cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND(!joint); ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_CONE_TWIST); ConeTwistJoint3DSW *cone_twist_joint = static_cast<ConeTwistJoint3DSW *>(joint); @@ -1441,7 +1441,7 @@ void PhysicsServer3DSW::cone_twist_joint_set_param(RID p_joint, ConeTwistJointPa } real_t PhysicsServer3DSW::cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND_V(!joint, 0); ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0); ConeTwistJoint3DSW *cone_twist_joint = static_cast<ConeTwistJoint3DSW *>(joint); @@ -1449,7 +1449,7 @@ real_t PhysicsServer3DSW::cone_twist_joint_get_param(RID p_joint, ConeTwistJoint } void PhysicsServer3DSW::joint_make_generic_6dof(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) { - Body3DSW *body_A = body_owner.getornull(p_body_A); + Body3DSW *body_A = body_owner.get_or_null(p_body_A); ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { @@ -1457,12 +1457,12 @@ void PhysicsServer3DSW::joint_make_generic_6dof(RID p_joint, RID p_body_A, const p_body_B = body_A->get_space()->get_static_global_body(); } - Body3DSW *body_B = body_owner.getornull(p_body_B); + Body3DSW *body_B = body_owner.get_or_null(p_body_B); ERR_FAIL_COND(!body_B); ERR_FAIL_COND(body_A == body_B); - Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + Joint3DSW *prev_joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(Generic6DOFJoint3DSW(body_A, body_B, p_local_frame_A, p_local_frame_B, true)); @@ -1473,7 +1473,7 @@ void PhysicsServer3DSW::joint_make_generic_6dof(RID p_joint, RID p_body_A, const } void PhysicsServer3DSW::generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param, real_t p_value) { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND(!joint); ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF); Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint); @@ -1481,7 +1481,7 @@ void PhysicsServer3DSW::generic_6dof_joint_set_param(RID p_joint, Vector3::Axis } real_t PhysicsServer3DSW::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param) const { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND_V(!joint, 0); ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, 0); Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint); @@ -1489,7 +1489,7 @@ real_t PhysicsServer3DSW::generic_6dof_joint_get_param(RID p_joint, Vector3::Axi } void PhysicsServer3DSW::generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag, bool p_enable) { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND(!joint); ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF); Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint); @@ -1497,7 +1497,7 @@ void PhysicsServer3DSW::generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p } bool PhysicsServer3DSW::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag) const { - Joint3DSW *joint = joint_owner.getornull(p_joint); + Joint3DSW *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND_V(!joint, false); ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, false); Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint); @@ -1508,7 +1508,7 @@ void PhysicsServer3DSW::free(RID p_rid) { _update_shapes(); //just in case if (shape_owner.owns(p_rid)) { - Shape3DSW *shape = shape_owner.getornull(p_rid); + Shape3DSW *shape = shape_owner.get_or_null(p_rid); while (shape->get_owners().size()) { ShapeOwner3DSW *so = shape->get_owners().front()->key(); @@ -1518,7 +1518,7 @@ void PhysicsServer3DSW::free(RID p_rid) { shape_owner.free(p_rid); memdelete(shape); } else if (body_owner.owns(p_rid)) { - Body3DSW *body = body_owner.getornull(p_rid); + Body3DSW *body = body_owner.get_or_null(p_rid); /* if (body->get_state_query()) @@ -1537,14 +1537,14 @@ void PhysicsServer3DSW::free(RID p_rid) { body_owner.free(p_rid); memdelete(body); } else if (soft_body_owner.owns(p_rid)) { - SoftBody3DSW *soft_body = soft_body_owner.getornull(p_rid); + SoftBody3DSW *soft_body = soft_body_owner.get_or_null(p_rid); soft_body->set_space(nullptr); soft_body_owner.free(p_rid); memdelete(soft_body); } else if (area_owner.owns(p_rid)) { - Area3DSW *area = area_owner.getornull(p_rid); + Area3DSW *area = area_owner.get_or_null(p_rid); /* if (area->get_monitor_query()) @@ -1560,7 +1560,7 @@ void PhysicsServer3DSW::free(RID p_rid) { area_owner.free(p_rid); memdelete(area); } else if (space_owner.owns(p_rid)) { - Space3DSW *space = space_owner.getornull(p_rid); + Space3DSW *space = space_owner.get_or_null(p_rid); while (space->get_objects().size()) { CollisionObject3DSW *co = (CollisionObject3DSW *)space->get_objects().front()->get(); @@ -1574,7 +1574,7 @@ void PhysicsServer3DSW::free(RID p_rid) { space_owner.free(p_rid); memdelete(space); } else if (joint_owner.owns(p_rid)) { - Joint3DSW *joint = joint_owner.getornull(p_rid); + Joint3DSW *joint = joint_owner.get_or_null(p_rid); joint_owner.free(p_rid); memdelete(joint); @@ -1744,11 +1744,5 @@ PhysicsServer3DSW::PhysicsServer3DSW(bool p_using_threads) { singletonsw = this; BroadPhase3DSW::create_func = BroadPhase3DBVH::_create; - island_count = 0; - active_objects = 0; - collision_pairs = 0; using_threads = p_using_threads; - active = true; - flushing_queries = false; - doing_sync = false; }; diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/physics_server_3d_sw.h index 071ad0a694..54a787198d 100644 --- a/servers/physics_3d/physics_server_3d_sw.h +++ b/servers/physics_3d/physics_server_3d_sw.h @@ -42,18 +42,18 @@ class PhysicsServer3DSW : public PhysicsServer3D { GDCLASS(PhysicsServer3DSW, PhysicsServer3D); friend class PhysicsDirectSpaceState3DSW; - bool active; - int iterations; + bool active = true; + int iterations = 0; - int island_count; - int active_objects; - int collision_pairs; + int island_count = 0; + int active_objects = 0; + int collision_pairs = 0; - bool using_threads; - bool doing_sync; - bool flushing_queries; + bool using_threads = false; + bool doing_sync = false; + bool flushing_queries = false; - Step3DSW *stepper; + Step3DSW *stepper = nullptr; Set<const Space3DSW *> active_spaces; mutable RID_PtrOwner<Shape3DSW, true> shape_owner; @@ -79,7 +79,7 @@ public: static void _shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); - virtual RID plane_shape_create() override; + virtual RID world_boundary_shape_create() override; virtual RID separation_ray_shape_create() override; virtual RID sphere_shape_create() override; virtual RID box_shape_create() override; @@ -242,7 +242,7 @@ public: virtual void body_set_ray_pickable(RID p_body, bool p_enable) override; - virtual bool body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_collide_separation_ray = false, const Set<RID> &p_exclude = Set<RID>()) override; + virtual bool body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result = nullptr) override; // this function only works on physics process, errors and returns null otherwise virtual PhysicsDirectBodyState3D *body_get_direct_state(RID p_body) override; @@ -291,7 +291,7 @@ public: virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) override; virtual real_t soft_body_get_drag_coefficient(RID p_body) const override; - virtual void soft_body_set_mesh(RID p_body, const REF &p_mesh) override; + virtual void soft_body_set_mesh(RID p_body, RID p_mesh) override; virtual AABB soft_body_get_bounds(RID p_body) const override; diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.cpp b/servers/physics_3d/physics_server_3d_wrap_mt.cpp index 0a89c1a9c9..c424100bba 100644 --- a/servers/physics_3d/physics_server_3d_wrap_mt.cpp +++ b/servers/physics_3d/physics_server_3d_wrap_mt.cpp @@ -119,8 +119,6 @@ PhysicsServer3DWrapMT::PhysicsServer3DWrapMT(PhysicsServer3D *p_contained, bool command_queue(p_create_thread) { physics_3d_server = p_contained; create_thread = p_create_thread; - step_pending = 0; - step_thread_up = false; pool_max_size = GLOBAL_GET("memory/limits/multithreaded_server/rid_pool_prealloc"); @@ -131,7 +129,6 @@ PhysicsServer3DWrapMT::PhysicsServer3DWrapMT(PhysicsServer3D *p_contained, bool } main_thread = Thread::get_caller_id(); - first_frame = true; } PhysicsServer3DWrapMT::~PhysicsServer3DWrapMT() { diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.h b/servers/physics_3d/physics_server_3d_wrap_mt.h index 58986969d4..17d02addda 100644 --- a/servers/physics_3d/physics_server_3d_wrap_mt.h +++ b/servers/physics_3d/physics_server_3d_wrap_mt.h @@ -58,7 +58,7 @@ class PhysicsServer3DWrapMT : public PhysicsServer3D { bool create_thread = false; Semaphore step_sem; - int step_pending; + int step_pending = 0; void thread_step(real_t p_delta); void thread_flush(); @@ -78,7 +78,7 @@ public: #include "servers/server_wrap_mt_common.h" //FUNC1RID(shape,ShapeType); todo fix - FUNCRID(plane_shape) + FUNCRID(world_boundary_shape) FUNCRID(separation_ray_shape) FUNCRID(sphere_shape) FUNCRID(box_shape) @@ -253,9 +253,9 @@ public: FUNC2(body_set_ray_pickable, RID, bool); - bool body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_collide_separation_ray = false, const Set<RID> &p_exclude = Set<RID>()) override { + bool body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result = nullptr) override { ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); - return physics_3d_server->body_test_motion(p_body, p_from, p_motion, p_margin, r_result, p_collide_separation_ray, p_exclude); + return physics_3d_server->body_test_motion(p_body, p_parameters, r_result); } // this function only works on physics process, errors and returns null otherwise @@ -308,7 +308,7 @@ public: FUNC2(soft_body_set_drag_coefficient, RID, real_t); FUNC1RC(real_t, soft_body_get_drag_coefficient, RID); - FUNC2(soft_body_set_mesh, RID, const REF &); + FUNC2(soft_body_set_mesh, RID, RID); FUNC1RC(AABB, soft_body_get_bounds, RID); diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp index 945d0120be..0fb6d582c8 100644 --- a/servers/physics_3d/shape_3d_sw.cpp +++ b/servers/physics_3d/shape_3d_sw.cpp @@ -39,7 +39,7 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans https://bulletphysics.org +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -61,8 +61,8 @@ subject to the following restrictions: void Shape3DSW::configure(const AABB &p_aabb) { aabb = p_aabb; configured = true; - for (Map<ShapeOwner3DSW *, int>::Element *E = owners.front(); E; E = E->next()) { - ShapeOwner3DSW *co = (ShapeOwner3DSW *)E->key(); + for (const KeyValue<ShapeOwner3DSW *, int> &E : owners) { + ShapeOwner3DSW *co = (ShapeOwner3DSW *)E.key; co->_shape_changed(); } } @@ -101,30 +101,25 @@ const Map<ShapeOwner3DSW *, int> &Shape3DSW::get_owners() const { return owners; } -Shape3DSW::Shape3DSW() { - custom_bias = 0; - configured = false; -} - Shape3DSW::~Shape3DSW() { ERR_FAIL_COND(owners.size()); } -Plane PlaneShape3DSW::get_plane() const { +Plane WorldBoundaryShape3DSW::get_plane() const { return plane; } -void PlaneShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { +void WorldBoundaryShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { // gibberish, a plane is infinity r_min = -1e7; r_max = 1e7; } -Vector3 PlaneShape3DSW::get_support(const Vector3 &p_normal) const { +Vector3 WorldBoundaryShape3DSW::get_support(const Vector3 &p_normal) const { return p_normal * 1e15; } -bool PlaneShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { +bool WorldBoundaryShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { bool inters = plane.intersects_segment(p_begin, p_end, &r_result); if (inters) { r_normal = plane.normal; @@ -132,11 +127,11 @@ bool PlaneShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_ return inters; } -bool PlaneShape3DSW::intersect_point(const Vector3 &p_point) const { +bool WorldBoundaryShape3DSW::intersect_point(const Vector3 &p_point) const { return plane.distance_to(p_point) < 0; } -Vector3 PlaneShape3DSW::get_closest_point_to(const Vector3 &p_point) const { +Vector3 WorldBoundaryShape3DSW::get_closest_point_to(const Vector3 &p_point) const { if (plane.is_point_over(p_point)) { return plane.project(p_point); } else { @@ -144,24 +139,24 @@ Vector3 PlaneShape3DSW::get_closest_point_to(const Vector3 &p_point) const { } } -Vector3 PlaneShape3DSW::get_moment_of_inertia(real_t p_mass) const { - return Vector3(); //wtf +Vector3 WorldBoundaryShape3DSW::get_moment_of_inertia(real_t p_mass) const { + return Vector3(); // not applicable. } -void PlaneShape3DSW::_setup(const Plane &p_plane) { +void WorldBoundaryShape3DSW::_setup(const Plane &p_plane) { plane = p_plane; configure(AABB(Vector3(-1e4, -1e4, -1e4), Vector3(1e4 * 2, 1e4 * 2, 1e4 * 2))); } -void PlaneShape3DSW::set_data(const Variant &p_data) { +void WorldBoundaryShape3DSW::set_data(const Variant &p_data) { _setup(p_data); } -Variant PlaneShape3DSW::get_data() const { +Variant WorldBoundaryShape3DSW::get_data() const { return plane; } -PlaneShape3DSW::PlaneShape3DSW() { +WorldBoundaryShape3DSW::WorldBoundaryShape3DSW() { } // @@ -244,10 +239,7 @@ Variant SeparationRayShape3DSW::get_data() const { return d; } -SeparationRayShape3DSW::SeparationRayShape3DSW() { - length = 1; - slide_on_slope = false; -} +SeparationRayShape3DSW::SeparationRayShape3DSW() {} /********** SPHERE *************/ @@ -311,9 +303,7 @@ Variant SphereShape3DSW::get_data() const { return radius; } -SphereShape3DSW::SphereShape3DSW() { - radius = 0; -} +SphereShape3DSW::SphereShape3DSW() {} /********** BOX *************/ @@ -502,8 +492,7 @@ Variant BoxShape3DSW::get_data() const { return half_extents; } -BoxShape3DSW::BoxShape3DSW() { -} +BoxShape3DSW::BoxShape3DSW() {} /********** CAPSULE *************/ @@ -668,9 +657,7 @@ Variant CapsuleShape3DSW::get_data() const { return d; } -CapsuleShape3DSW::CapsuleShape3DSW() { - height = radius = 0; -} +CapsuleShape3DSW::CapsuleShape3DSW() {} /********** CYLINDER *************/ @@ -848,9 +835,7 @@ Variant CylinderShape3DSW::get_data() const { return d; } -CylinderShape3DSW::CylinderShape3DSW() { - height = radius = 0; -} +CylinderShape3DSW::CylinderShape3DSW() {} /********** CONVEX POLYGON *************/ @@ -1591,7 +1576,7 @@ void ConcavePolygonShape3DSW::_setup(const Vector<Vector3> &p_faces, bool p_back Face3 face(facesr[i * 3 + 0], facesr[i * 3 + 1], facesr[i * 3 + 2]); bvh_arrayw[i].aabb = face.get_aabb(); - bvh_arrayw[i].center = bvh_arrayw[i].aabb.position + bvh_arrayw[i].aabb.size * 0.5; + bvh_arrayw[i].center = bvh_arrayw[i].aabb.get_center(); bvh_arrayw[i].face_index = i; facesw[i].indices[0] = i * 3 + 0; facesw[i].indices[1] = i * 3 + 1; diff --git a/servers/physics_3d/shape_3d_sw.h b/servers/physics_3d/shape_3d_sw.h index 73eddc469c..061d66a085 100644 --- a/servers/physics_3d/shape_3d_sw.h +++ b/servers/physics_3d/shape_3d_sw.h @@ -48,8 +48,8 @@ public: class Shape3DSW { RID self; AABB aabb; - bool configured; - real_t custom_bias; + bool configured = false; + real_t custom_bias = 0.0; Map<ShapeOwner3DSW *, int> owners; @@ -95,7 +95,7 @@ public: bool is_owner(ShapeOwner3DSW *p_owner) const; const Map<ShapeOwner3DSW *, int> &get_owners() const; - Shape3DSW(); + Shape3DSW() {} virtual ~Shape3DSW(); }; @@ -112,7 +112,7 @@ public: ConcaveShape3DSW() {} }; -class PlaneShape3DSW : public Shape3DSW { +class WorldBoundaryShape3DSW : public Shape3DSW { Plane plane; void _setup(const Plane &p_plane); @@ -121,7 +121,7 @@ public: Plane get_plane() const; virtual real_t get_area() const override { return INFINITY; } - virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_PLANE; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_WORLD_BOUNDARY; } virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; virtual Vector3 get_support(const Vector3 &p_normal) const override; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; } @@ -134,12 +134,12 @@ public: virtual void set_data(const Variant &p_data) override; virtual Variant get_data() const override; - PlaneShape3DSW(); + WorldBoundaryShape3DSW(); }; class SeparationRayShape3DSW : public Shape3DSW { - real_t length; - bool slide_on_slope; + real_t length = 1.0; + bool slide_on_slope = false; void _setup(real_t p_length, bool p_slide_on_slope); @@ -166,7 +166,7 @@ public: }; class SphereShape3DSW : public Shape3DSW { - real_t radius; + real_t radius = 0.0; void _setup(real_t p_radius); @@ -218,8 +218,8 @@ public: }; class CapsuleShape3DSW : public Shape3DSW { - real_t height; - real_t radius; + real_t height = 0.0; + real_t radius = 0.0; void _setup(real_t p_height, real_t p_radius); @@ -247,8 +247,8 @@ public: }; class CylinderShape3DSW : public Shape3DSW { - real_t height; - real_t radius; + real_t height = 0.0; + real_t radius = 0.0; void _setup(real_t p_height, real_t p_radius); @@ -308,7 +308,7 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW { struct Face { Vector3 normal; - int indices[3]; + int indices[3] = {}; }; Vector<Face> faces; @@ -316,10 +316,10 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW { struct BVH { AABB aabb; - int left; - int right; + int left = 0; + int right = 0; - int face_index; + int face_index = 0; }; Vector<BVH> bvh; @@ -469,7 +469,7 @@ struct FaceShape3DSW : public Shape3DSW { }; struct MotionShape3DSW : public Shape3DSW { - Shape3DSW *shape; + Shape3DSW *shape = nullptr; Vector3 motion; virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; } diff --git a/servers/physics_3d/soft_body_3d_sw.cpp b/servers/physics_3d/soft_body_3d_sw.cpp index d7e13867bf..c9166810fe 100644 --- a/servers/physics_3d/soft_body_3d_sw.cpp +++ b/servers/physics_3d/soft_body_3d_sw.cpp @@ -33,12 +33,13 @@ #include "core/math/geometry_3d.h" #include "core/templates/map.h" +#include "servers/rendering_server.h" // Based on Bullet soft body. /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org +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. @@ -127,7 +128,7 @@ void SoftBody3DSW::set_space(Space3DSW *p_space) { } } -void SoftBody3DSW::set_mesh(const Ref<Mesh> &p_mesh) { +void SoftBody3DSW::set_mesh(RID p_mesh) { destroy(); soft_mesh = p_mesh; @@ -136,13 +137,11 @@ void SoftBody3DSW::set_mesh(const Ref<Mesh> &p_mesh) { return; } - Array arrays = soft_mesh->surface_get_arrays(0); - ERR_FAIL_COND(!(soft_mesh->surface_get_format(0) & RS::ARRAY_FORMAT_INDEX)); + Array arrays = RenderingServer::get_singleton()->mesh_surface_get_arrays(soft_mesh, 0); - bool success = create_from_trimesh(arrays[RS::ARRAY_INDEX], arrays[RS::ARRAY_VERTEX]); + bool success = create_from_trimesh(arrays[RenderingServer::ARRAY_INDEX], arrays[RenderingServer::ARRAY_VERTEX]); if (!success) { destroy(); - soft_mesh = Ref<Mesh>(); } } @@ -249,8 +248,10 @@ void SoftBody3DSW::update_area() { // Node area. LocalVector<int> counts; - counts.resize(nodes.size()); - memset(counts.ptr(), 0, counts.size() * sizeof(int)); + if (nodes.size() > 0) { + counts.resize(nodes.size()); + memset(counts.ptr(), 0, counts.size() * sizeof(int)); + } for (i = 0, ni = nodes.size(); i < ni; ++i) { nodes[i].area = 0.0; @@ -317,11 +318,13 @@ void SoftBody3DSW::apply_nodes_transform(const Transform3D &p_transform) { } Vector3 SoftBody3DSW::get_vertex_position(int p_index) const { + ERR_FAIL_COND_V(p_index < 0, Vector3()); + if (soft_mesh.is_null()) { return Vector3(); } - ERR_FAIL_INDEX_V(p_index, (int)map_visual_to_physics.size(), Vector3()); + ERR_FAIL_COND_V(p_index >= (int)map_visual_to_physics.size(), Vector3()); uint32_t node_index = map_visual_to_physics[p_index]; ERR_FAIL_COND_V(node_index >= nodes.size(), Vector3()); @@ -329,11 +332,13 @@ Vector3 SoftBody3DSW::get_vertex_position(int p_index) const { } void SoftBody3DSW::set_vertex_position(int p_index, const Vector3 &p_position) { + ERR_FAIL_COND(p_index < 0); + if (soft_mesh.is_null()) { return; } - ERR_FAIL_INDEX(p_index, (int)map_visual_to_physics.size()); + ERR_FAIL_COND(p_index >= (int)map_visual_to_physics.size()); uint32_t node_index = map_visual_to_physics[p_index]; ERR_FAIL_COND(node_index >= nodes.size()); @@ -343,6 +348,8 @@ void SoftBody3DSW::set_vertex_position(int p_index, const Vector3 &p_position) { } void SoftBody3DSW::pin_vertex(int p_index) { + ERR_FAIL_COND(p_index < 0); + if (is_vertex_pinned(p_index)) { return; } @@ -350,7 +357,7 @@ void SoftBody3DSW::pin_vertex(int p_index) { pinned_vertices.push_back(p_index); if (!soft_mesh.is_null()) { - ERR_FAIL_INDEX(p_index, (int)map_visual_to_physics.size()); + ERR_FAIL_COND(p_index >= (int)map_visual_to_physics.size()); uint32_t node_index = map_visual_to_physics[p_index]; ERR_FAIL_COND(node_index >= nodes.size()); @@ -360,13 +367,15 @@ void SoftBody3DSW::pin_vertex(int p_index) { } void SoftBody3DSW::unpin_vertex(int p_index) { + ERR_FAIL_COND(p_index < 0); + uint32_t pinned_count = pinned_vertices.size(); for (uint32_t i = 0; i < pinned_count; ++i) { if (p_index == pinned_vertices[i]) { pinned_vertices.remove(i); if (!soft_mesh.is_null()) { - ERR_FAIL_INDEX(p_index, (int)map_visual_to_physics.size()); + ERR_FAIL_COND(p_index >= (int)map_visual_to_physics.size()); uint32_t node_index = map_visual_to_physics[p_index]; ERR_FAIL_COND(node_index >= nodes.size()); @@ -386,10 +395,10 @@ void SoftBody3DSW::unpin_all_vertices() { real_t inv_node_mass = nodes.size() * inv_total_mass; uint32_t pinned_count = pinned_vertices.size(); for (uint32_t i = 0; i < pinned_count; ++i) { - uint32_t vertex_index = pinned_vertices[i]; + int pinned_vertex = pinned_vertices[i]; - ERR_CONTINUE(vertex_index >= map_visual_to_physics.size()); - uint32_t node_index = map_visual_to_physics[vertex_index]; + ERR_CONTINUE(pinned_vertex >= (int)map_visual_to_physics.size()); + uint32_t node_index = map_visual_to_physics[pinned_vertex]; ERR_CONTINUE(node_index >= nodes.size()); Node &node = nodes[node_index]; @@ -401,6 +410,8 @@ void SoftBody3DSW::unpin_all_vertices() { } bool SoftBody3DSW::is_vertex_pinned(int p_index) const { + ERR_FAIL_COND_V(p_index < 0, false); + uint32_t pinned_count = pinned_vertices.size(); for (uint32_t i = 0; i < pinned_count; ++i) { if (p_index == pinned_vertices[i]) { @@ -465,6 +476,9 @@ Vector3 SoftBody3DSW::get_face_normal(uint32_t p_face_index) const { } bool SoftBody3DSW::create_from_trimesh(const Vector<int> &p_indices, const Vector<Vector3> &p_vertices) { + ERR_FAIL_COND_V(p_indices.is_empty(), false); + ERR_FAIL_COND_V(p_vertices.is_empty(), false); + uint32_t node_count = 0; LocalVector<Vector3> vertices; const int visual_vertex_count(p_vertices.size()); @@ -1225,6 +1239,8 @@ void SoftBody3DSW::deinitialize_shape() { } void SoftBody3DSW::destroy() { + soft_mesh = RID(); + map_visual_to_physics.clear(); node_tree.clear(); diff --git a/servers/physics_3d/soft_body_3d_sw.h b/servers/physics_3d/soft_body_3d_sw.h index 58fd234fde..7d4b83d0ee 100644 --- a/servers/physics_3d/soft_body_3d_sw.h +++ b/servers/physics_3d/soft_body_3d_sw.h @@ -40,12 +40,11 @@ #include "core/templates/local_vector.h" #include "core/templates/set.h" #include "core/templates/vset.h" -#include "scene/resources/mesh.h" class Constraint3DSW; class SoftBody3DSW : public CollisionObject3DSW { - Ref<Mesh> soft_mesh; + RID soft_mesh; struct Node { Vector3 s; // Source position @@ -159,7 +158,7 @@ public: virtual void set_space(Space3DSW *p_space); - void set_mesh(const Ref<Mesh> &p_mesh); + void set_mesh(RID p_mesh); void update_rendering_server(RenderingServerHandler *p_rendering_server_handler); diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp index 3b08184e00..c88747c017 100644 --- a/servers/physics_3d/space_3d_sw.cpp +++ b/servers/physics_3d/space_3d_sw.cpp @@ -34,6 +34,8 @@ #include "core/config/project_settings.h" #include "physics_server_3d_sw.h" +#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05 + _FORCE_INLINE_ static bool _can_collide_with(CollisionObject3DSW *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { if (!(p_object->get_collision_layer() & p_collision_mask)) { return false; @@ -185,7 +187,7 @@ int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Trans return 0; } - Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape); + Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.get_or_null(p_shape); ERR_FAIL_COND_V(!shape, 0); AABB aabb = p_xform.xform(shape->get_aabb()); @@ -236,7 +238,7 @@ int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Trans } bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transform3D &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, ShapeRestInfo *r_info) { - Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape); + Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.get_or_null(p_shape); ERR_FAIL_COND_V(!shape, false); AABB aabb = p_xform.xform(shape->get_aabb()); @@ -359,7 +361,7 @@ bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform3D & return false; } - Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape); + Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.get_or_null(p_shape); ERR_FAIL_COND_V(!shape, 0); AABB aabb = p_shape_xform.xform(shape->get_aabb()); @@ -401,17 +403,27 @@ bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform3D & return collided; } +struct _RestResultData { + const CollisionObject3DSW *object = nullptr; + int local_shape = 0; + int shape = 0; + Vector3 contact; + Vector3 normal; + real_t len = 0.0; +}; + struct _RestCallbackData { - const CollisionObject3DSW *object; - const CollisionObject3DSW *best_object; - int local_shape; - int best_local_shape; - int shape; - int best_shape; - Vector3 best_contact; - Vector3 best_normal; - real_t best_len; - real_t min_allowed_depth; + const CollisionObject3DSW *object = nullptr; + int local_shape = 0; + int shape = 0; + + real_t min_allowed_depth = 0.0; + + _RestResultData best_result; + + int max_results = 0; + int result_count = 0; + _RestResultData *other_results = nullptr; }; static void _rest_cbk_result(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { @@ -422,32 +434,71 @@ static void _rest_cbk_result(const Vector3 &p_point_A, int p_index_A, const Vect if (len < rd->min_allowed_depth) { return; } - if (len <= rd->best_len) { + + bool is_best_result = (len > rd->best_result.len); + + if (rd->other_results && rd->result_count > 0) { + // Consider as new result by default. + int prev_result_count = rd->result_count++; + + int result_index = 0; + real_t tested_len = is_best_result ? rd->best_result.len : len; + for (; result_index < prev_result_count - 1; ++result_index) { + if (tested_len > rd->other_results[result_index].len) { + // Re-using a previous result. + rd->result_count--; + break; + } + } + + if (result_index < rd->max_results - 1) { + _RestResultData &result = rd->other_results[result_index]; + + if (is_best_result) { + // Keep the previous best result as separate result. + result = rd->best_result; + } else { + // Keep this result as separate result. + result.len = len; + result.contact = p_point_B; + result.normal = contact_rel / len; + result.object = rd->object; + result.shape = rd->shape; + result.local_shape = rd->local_shape; + } + } else { + // Discarding this result. + rd->result_count--; + } + } else if (is_best_result) { + rd->result_count = 1; + } + + if (!is_best_result) { return; } - rd->best_len = len; - rd->best_contact = p_point_B; - rd->best_normal = contact_rel / len; - rd->best_object = rd->object; - rd->best_shape = rd->shape; - rd->best_local_shape = rd->local_shape; + rd->best_result.len = len; + rd->best_result.contact = p_point_B; + rd->best_result.normal = contact_rel / len; + rd->best_result.object = rd->object; + rd->best_result.shape = rd->shape; + rd->best_result.local_shape = rd->local_shape; } bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape); + Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.get_or_null(p_shape); ERR_FAIL_COND_V(!shape, 0); + real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; + AABB aabb = p_shape_xform.xform(shape->get_aabb()); aabb = aabb.grow(p_margin); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); _RestCallbackData rcd; - rcd.best_len = 0; - rcd.best_object = nullptr; - rcd.best_shape = 0; - rcd.min_allowed_depth = space->test_motion_min_contact_depth; + rcd.min_allowed_depth = min_contact_depth; for (int i = 0; i < amount; i++) { if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { @@ -470,18 +521,18 @@ bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform3D &p_sh } } - if (rcd.best_len == 0 || !rcd.best_object) { + if (rcd.best_result.len == 0 || !rcd.best_result.object) { return false; } - r_info->collider_id = rcd.best_object->get_instance_id(); - r_info->shape = rcd.best_shape; - r_info->normal = rcd.best_normal; - r_info->point = rcd.best_contact; - r_info->rid = rcd.best_object->get_self(); - if (rcd.best_object->get_type() == CollisionObject3DSW::TYPE_BODY) { - const Body3DSW *body = static_cast<const Body3DSW *>(rcd.best_object); - Vector3 rel_vec = rcd.best_contact - (body->get_transform().origin + body->get_center_of_mass()); + r_info->collider_id = rcd.best_result.object->get_instance_id(); + r_info->shape = rcd.best_result.shape; + r_info->normal = rcd.best_result.normal; + r_info->point = rcd.best_result.contact; + r_info->rid = rcd.best_result.object->get_self(); + if (rcd.best_result.object->get_type() == CollisionObject3DSW::TYPE_BODY) { + const Body3DSW *body = static_cast<const Body3DSW *>(rcd.best_result.object); + Vector3 rel_vec = rcd.best_result.contact - (body->get_transform().origin + body->get_center_of_mass()); r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec); } else { @@ -492,9 +543,9 @@ bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform3D &p_sh } Vector3 PhysicsDirectSpaceState3DSW::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const { - CollisionObject3DSW *obj = PhysicsServer3DSW::singletonsw->area_owner.getornull(p_object); + CollisionObject3DSW *obj = PhysicsServer3DSW::singletonsw->area_owner.get_or_null(p_object); if (!obj) { - obj = PhysicsServer3DSW::singletonsw->body_owner.getornull(p_object); + obj = PhysicsServer3DSW::singletonsw->body_owner.get_or_null(p_object); } ERR_FAIL_COND_V(!obj, Vector3()); @@ -569,7 +620,7 @@ int Space3DSW::_cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb) { return amount; } -bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer3D::MotionResult *r_result, bool p_collide_separation_ray, const Set<RID> &p_exclude) { +bool Space3DSW::test_body_motion(Body3DSW *p_body, const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult *r_result) { //give me back regular physics engine logic //this is madness //and most people using this function will think @@ -577,10 +628,12 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co //this took about a week to get right.. //but is it right? who knows at this point.. + ERR_FAIL_INDEX_V(p_parameters.max_collisions, PhysicsServer3D::MotionResult::MAX_COLLISIONS, false); + if (r_result) { - r_result->collider_id = ObjectID(); - r_result->collider_shape = 0; + *r_result = PhysicsServer3D::MotionResult(); } + AABB body_aabb; bool shapes_found = false; @@ -599,21 +652,22 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co if (!shapes_found) { if (r_result) { - *r_result = PhysicsServer3D::MotionResult(); - r_result->travel = p_motion; + r_result->travel = p_parameters.motion; } return false; } // Undo the currently transform the physics server is aware of and apply the provided one - body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb)); - body_aabb = body_aabb.grow(p_margin); + body_aabb = p_parameters.from.xform(p_body->get_inv_transform().xform(body_aabb)); + body_aabb = body_aabb.grow(p_parameters.margin); + + real_t min_contact_depth = p_parameters.margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; - real_t motion_length = p_motion.length(); - Vector3 motion_normal = p_motion / motion_length; + real_t motion_length = p_parameters.motion.length(); + Vector3 motion_normal = p_parameters.motion / motion_length; - Transform3D body_transform = p_from; + Transform3D body_transform = p_parameters.from; bool recovered = false; @@ -647,13 +701,16 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co for (int i = 0; i < amount; i++) { const CollisionObject3DSW *col_obj = intersection_query_results[i]; - if (p_exclude.has(col_obj->get_self())) { + if (p_parameters.exclude_bodies.has(col_obj->get_self())) { + continue; + } + if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { continue; } int shape_idx = intersection_query_subindex_results[i]; - if (CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_margin)) { + if (CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_parameters.margin)) { collided = cbk.amount > 0; } } @@ -663,8 +720,9 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co break; } - Vector3 recover_motion; + recovered = true; + Vector3 recover_motion; for (int i = 0; i < cbk.amount; i++) { Vector3 a = sr[i * 2 + 0]; Vector3 b = sr[i * 2 + 1]; @@ -675,9 +733,9 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co // Compute depth on recovered motion. real_t depth = n.dot(a + recover_motion) - d; - if (depth > 0.0) { + if (depth > min_contact_depth + CMP_EPSILON) { // Only recover if there is penetration. - recover_motion -= n * depth * 0.4; + recover_motion -= n * (depth - min_contact_depth) * 0.4; } } @@ -686,8 +744,6 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co break; } - recovered = true; - body_transform.origin += recover_motion; body_aabb.position += recover_motion; @@ -704,7 +760,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co // STEP 2 ATTEMPT MOTION AABB motion_aabb = body_aabb; - motion_aabb.position += p_motion; + motion_aabb.position += p_parameters.motion; motion_aabb = motion_aabb.merge(body_aabb); int amount = _cull_aabb_for_body(p_body, motion_aabb); @@ -718,7 +774,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co // Colliding separation rays allows to properly snap to the ground, // otherwise it's not needed in regular motion. - if (!p_collide_separation_ray && (body_shape->get_type() == PhysicsServer3D::SHAPE_SEPARATION_RAY)) { + if (!p_parameters.collide_separation_ray && (body_shape->get_type() == PhysicsServer3D::SHAPE_SEPARATION_RAY)) { // When slide on slope is on, separation ray shape acts like a regular shape. if (!static_cast<SeparationRayShape3DSW *>(body_shape)->get_slide_on_slope()) { continue; @@ -730,7 +786,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co Transform3D body_shape_xform_inv = body_shape_xform.affine_inverse(); MotionShape3DSW mshape; mshape.shape = body_shape; - mshape.motion = body_shape_xform_inv.basis.xform(p_motion); + mshape.motion = body_shape_xform_inv.basis.xform(p_parameters.motion); bool stuck = false; @@ -739,7 +795,10 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co for (int i = 0; i < amount; i++) { const CollisionObject3DSW *col_obj = intersection_query_results[i]; - if (p_exclude.has(col_obj->get_self())) { + if (p_parameters.exclude_bodies.has(col_obj->get_self())) { + continue; + } + if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { continue; } @@ -768,7 +827,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co for (int k = 0; k < 8; k++) { //steps should be customizable.. real_t fraction = low + (hi - low) * fraction_coeff; - mshape.motion = body_shape_xform_inv.basis.xform(p_motion * fraction); + mshape.motion = body_shape_xform_inv.basis.xform(p_parameters.motion * fraction); Vector3 lA, lB; Vector3 sep = motion_normal; //important optimization for this to work fast enough @@ -830,15 +889,18 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co //it collided, let's get the rest info in unsafe advance Transform3D ugt = body_transform; - ugt.origin += p_motion * unsafe; + ugt.origin += p_parameters.motion * unsafe; + + _RestResultData results[PhysicsServer3D::MotionResult::MAX_COLLISIONS]; _RestCallbackData rcd; - rcd.best_len = 0; - rcd.best_object = nullptr; - rcd.best_shape = 0; + if (p_parameters.max_collisions > 1) { + rcd.max_results = p_parameters.max_collisions; + rcd.other_results = results; + } // Allowed depth can't be lower than motion length, in order to handle contacts at low speed. - rcd.min_allowed_depth = MIN(motion_length, test_motion_min_contact_depth); + rcd.min_allowed_depth = MIN(motion_length, min_contact_depth); int from_shape = best_shape != -1 ? best_shape : 0; int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count(); @@ -851,47 +913,59 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co Transform3D body_shape_xform = ugt * p_body->get_shape_transform(j); Shape3DSW *body_shape = p_body->get_shape(j); - body_aabb.position += p_motion * unsafe; + body_aabb.position += p_parameters.motion * unsafe; int amount = _cull_aabb_for_body(p_body, body_aabb); for (int i = 0; i < amount; i++) { const CollisionObject3DSW *col_obj = intersection_query_results[i]; - if (p_exclude.has(col_obj->get_self())) { + if (p_parameters.exclude_bodies.has(col_obj->get_self())) { + continue; + } + if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { continue; } + int shape_idx = intersection_query_subindex_results[i]; rcd.object = col_obj; rcd.shape = shape_idx; - bool sc = CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin); + bool sc = CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_parameters.margin); if (!sc) { continue; } } } - if (rcd.best_len != 0) { + if (rcd.result_count > 0) { if (r_result) { - r_result->collider = rcd.best_object->get_self(); - r_result->collider_id = rcd.best_object->get_instance_id(); - r_result->collider_shape = rcd.best_shape; - r_result->collision_local_shape = rcd.best_local_shape; - r_result->collision_normal = rcd.best_normal; - r_result->collision_point = rcd.best_contact; - r_result->collision_depth = rcd.best_len; - r_result->collision_safe_fraction = safe; - r_result->collision_unsafe_fraction = unsafe; - //r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape); + for (int collision_index = 0; collision_index < rcd.result_count; ++collision_index) { + const _RestResultData &result = (collision_index > 0) ? rcd.other_results[collision_index - 1] : rcd.best_result; + + PhysicsServer3D::MotionCollision &collision = r_result->collisions[collision_index]; - const Body3DSW *body = static_cast<const Body3DSW *>(rcd.best_object); + collision.collider = result.object->get_self(); + collision.collider_id = result.object->get_instance_id(); + collision.collider_shape = result.shape; + collision.local_shape = result.local_shape; + collision.normal = result.normal; + collision.position = result.contact; + collision.depth = result.len; - Vector3 rel_vec = rcd.best_contact - (body->get_transform().origin + body->get_center_of_mass()); - r_result->collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec); + const Body3DSW *body = static_cast<const Body3DSW *>(result.object); - r_result->travel = safe * p_motion; - r_result->remainder = p_motion - safe * p_motion; - r_result->travel += (body_transform.get_origin() - p_from.get_origin()); + Vector3 rel_vec = result.contact - (body->get_transform().origin + body->get_center_of_mass()); + collision.collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec); + } + + r_result->travel = safe * p_parameters.motion; + r_result->remainder = p_parameters.motion - safe * p_parameters.motion; + r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin()); + + r_result->collision_safe_fraction = safe; + r_result->collision_unsafe_fraction = unsafe; + + r_result->collision_count = rcd.result_count; } collided = true; @@ -899,9 +973,12 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co } if (!collided && r_result) { - r_result->travel = p_motion; + r_result->travel = p_parameters.motion; r_result->remainder = Vector3(); - r_result->travel += (body_transform.get_origin() - p_from.get_origin()); + r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin()); + + r_result->collision_safe_fraction = 1.0; + r_result->collision_unsafe_fraction = 1.0; } return collided; @@ -1095,9 +1172,6 @@ void Space3DSW::set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_valu case PhysicsServer3D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias = p_value; break; - case PhysicsServer3D::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH: - test_motion_min_contact_depth = p_value; - break; } } @@ -1119,8 +1193,6 @@ real_t Space3DSW::get_param(PhysicsServer3D::SpaceParameter p_param) const { return body_angular_velocity_damp_ratio; case PhysicsServer3D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: return constraint_bias; - case PhysicsServer3D::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH: - return test_motion_min_contact_depth; } return 0; } @@ -1142,18 +1214,6 @@ PhysicsDirectSpaceState3DSW *Space3DSW::get_direct_state() { } Space3DSW::Space3DSW() { - collision_pairs = 0; - active_objects = 0; - island_count = 0; - contact_debug_count = 0; - - locked = false; - contact_recycle_radius = 0.01; - contact_max_separation = 0.05; - contact_max_allowed_penetration = 0.01; - test_motion_min_contact_depth = 0.00001; - - constraint_bias = 0.01; body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_linear", 0.1); body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_angular", Math::deg2rad(8.0)); body_time_to_sleep = GLOBAL_DEF("physics/3d/time_before_sleep", 0.5); @@ -1163,14 +1223,9 @@ Space3DSW::Space3DSW() { broadphase = BroadPhase3DSW::create_func(); broadphase->set_pair_callback(_broadphase_pair, this); broadphase->set_unpair_callback(_broadphase_unpair, this); - area = nullptr; direct_access = memnew(PhysicsDirectSpaceState3DSW); direct_access->space = this; - - for (int i = 0; i < ELAPSED_TIME_MAX; i++) { - elapsed_time[i] = 0; - } } Space3DSW::~Space3DSW() { diff --git a/servers/physics_3d/space_3d_sw.h b/servers/physics_3d/space_3d_sw.h index 98c335cb80..69cc3c4bbd 100644 --- a/servers/physics_3d/space_3d_sw.h +++ b/servers/physics_3d/space_3d_sw.h @@ -72,7 +72,7 @@ public: }; private: - uint64_t elapsed_time[ELAPSED_TIME_MAX]; + uint64_t elapsed_time[ELAPSED_TIME_MAX] = {}; PhysicsDirectSpaceState3DSW *direct_access; RID self; @@ -90,13 +90,12 @@ private: Set<CollisionObject3DSW *> objects; - Area3DSW *area; + Area3DSW *area = nullptr; - real_t contact_recycle_radius; - real_t contact_max_separation; - real_t contact_max_allowed_penetration; - real_t constraint_bias; - real_t test_motion_min_contact_depth; + real_t contact_recycle_radius = 0.01; + real_t contact_max_separation = 0.05; + real_t contact_max_allowed_penetration = 0.01; + real_t constraint_bias = 0.01; enum { INTERSECTION_QUERY_MAX = 2048 @@ -110,18 +109,18 @@ private: real_t body_time_to_sleep; real_t body_angular_velocity_damp_ratio; - bool locked; + bool locked = false; real_t last_step = 0.001; - int island_count; - int active_objects; - int collision_pairs; + int island_count = 0; + int active_objects = 0; + int collision_pairs = 0; RID static_global_body; Vector<Vector3> contact_debug; - int contact_debug_count; + int contact_debug_count = 0; friend class PhysicsDirectSpaceState3DSW; @@ -208,7 +207,7 @@ public: void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; } uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; } - bool test_body_motion(Body3DSW *p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer3D::MotionResult *r_result, bool p_collide_separation_ray = false, const Set<RID> &p_exclude = Set<RID>()); + bool test_body_motion(Body3DSW *p_body, const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult *r_result); Space3DSW(); ~Space3DSW(); diff --git a/servers/physics_3d/step_3d_sw.cpp b/servers/physics_3d/step_3d_sw.cpp index d0604b0aa0..6572d58c91 100644 --- a/servers/physics_3d/step_3d_sw.cpp +++ b/servers/physics_3d/step_3d_sw.cpp @@ -47,8 +47,8 @@ void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_bod p_body_island.push_back(p_body); } - for (Map<Constraint3DSW *, int>::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) { - Constraint3DSW *constraint = (Constraint3DSW *)E->key(); + for (const KeyValue<Constraint3DSW *, int> &E : p_body->get_constraint_map()) { + Constraint3DSW *constraint = (Constraint3DSW *)E.key; if (constraint->get_island_step() == _step) { continue; // Already processed. } @@ -59,7 +59,7 @@ void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_bod // Find connected rigid bodies. for (int i = 0; i < constraint->get_body_count(); i++) { - if (i == E->get()) { + if (i == E.value) { continue; } Body3DSW *other_body = constraint->get_body_ptr()[i]; @@ -407,8 +407,6 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { } Step3DSW::Step3DSW() { - _step = 1; - body_islands.reserve(BODY_ISLAND_COUNT_RESERVE); constraint_islands.reserve(ISLAND_COUNT_RESERVE); all_constraints.reserve(CONSTRAINT_COUNT_RESERVE); diff --git a/servers/physics_3d/step_3d_sw.h b/servers/physics_3d/step_3d_sw.h index 9c60004b24..f2f879104a 100644 --- a/servers/physics_3d/step_3d_sw.h +++ b/servers/physics_3d/step_3d_sw.h @@ -37,7 +37,7 @@ #include "core/templates/thread_work_pool.h" class Step3DSW { - uint64_t _step; + uint64_t _step = 1; int iterations = 0; real_t delta = 0.0; |