diff options
Diffstat (limited to 'servers/physics')
28 files changed, 428 insertions, 192 deletions
diff --git a/servers/physics/SCsub b/servers/physics/SCsub index c0ee2cc739..c5cc889112 100644 --- a/servers/physics/SCsub +++ b/servers/physics/SCsub @@ -4,6 +4,4 @@ Import('env') env.add_source_files(env.servers_sources, "*.cpp") -Export('env') - SConscript("joints/SCsub") diff --git a/servers/physics/area_pair_sw.cpp b/servers/physics/area_pair_sw.cpp index d2fef0ab77..5e295edcd1 100644 --- a/servers/physics/area_pair_sw.cpp +++ b/servers/physics/area_pair_sw.cpp @@ -147,10 +147,10 @@ Area2PairSW::~Area2PairSW() { if (colliding) { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) + if (area_b->has_area_monitor_callback()) area_b->remove_area_from_query(area_a, shape_a, shape_b); - if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) + if (area_a->has_area_monitor_callback()) area_a->remove_area_from_query(area_b, shape_b, shape_a); } diff --git a/servers/physics/area_sw.h b/servers/physics/area_sw.h index ae19b0e04e..63a4db5d02 100644 --- a/servers/physics/area_sw.h +++ b/servers/physics/area_sw.h @@ -32,7 +32,7 @@ #define AREA_SW_H #include "collision_object_sw.h" -#include "self_list.h" +#include "core/self_list.h" #include "servers/physics_server.h" //#include "servers/physics/query_sw.h" diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp index 5a41b621eb..357fc05355 100644 --- a/servers/physics/body_pair_sw.cpp +++ b/servers/physics/body_pair_sw.cpp @@ -31,7 +31,7 @@ #include "body_pair_sw.h" #include "collision_solver_sw.h" -#include "os/os.h" +#include "core/os/os.h" #include "space_sw.h" /* @@ -212,41 +212,11 @@ bool BodyPairSW::_test_ccd(real_t p_step, BodySW *p_A, int p_shape_A, const Tran } real_t combine_bounce(BodySW *A, BodySW *B) { - const PhysicsServer::CombineMode cm = A->get_bounce_combine_mode(); - - switch (cm) { - case PhysicsServer::COMBINE_MODE_INHERIT: - if (B->get_bounce_combine_mode() != PhysicsServer::COMBINE_MODE_INHERIT) - return combine_bounce(B, A); - // else use MAX [This is used when the two bodies doesn't use physical material] - case PhysicsServer::COMBINE_MODE_MAX: - return MAX(A->get_bounce(), B->get_bounce()); - case PhysicsServer::COMBINE_MODE_MIN: - return MIN(A->get_bounce(), B->get_bounce()); - case PhysicsServer::COMBINE_MODE_MULTIPLY: - return A->get_bounce() * B->get_bounce(); - default: // Is always PhysicsServer::COMBINE_MODE_AVERAGE: - return (A->get_bounce() + B->get_bounce()) / 2; - } + return CLAMP(A->get_bounce() + B->get_bounce(), 0, 1); } real_t combine_friction(BodySW *A, BodySW *B) { - const PhysicsServer::CombineMode cm = A->get_friction_combine_mode(); - - switch (cm) { - case PhysicsServer::COMBINE_MODE_INHERIT: - if (B->get_friction_combine_mode() != PhysicsServer::COMBINE_MODE_INHERIT) - return combine_friction(B, A); - // else use Multiply [This is used when the two bodies doesn't use physical material] - case PhysicsServer::COMBINE_MODE_MULTIPLY: - return A->get_friction() * B->get_friction(); - case PhysicsServer::COMBINE_MODE_MAX: - return MAX(A->get_friction(), B->get_friction()); - case PhysicsServer::COMBINE_MODE_MIN: - return MIN(A->get_friction(), B->get_friction()); - default: // Is always PhysicsServer::COMBINE_MODE_AVERAGE: - return (A->get_friction() + B->get_friction()) / 2; - } + return ABS(MIN(A->get_friction(), B->get_friction())); } bool BodyPairSW::setup(real_t p_step) { diff --git a/servers/physics/body_pair_sw.h b/servers/physics/body_pair_sw.h index fd85d77718..17ff9d6a88 100644 --- a/servers/physics/body_pair_sw.h +++ b/servers/physics/body_pair_sw.h @@ -76,7 +76,6 @@ class BodyPairSW : public ConstraintSW { Contact contacts[MAX_CONTACTS]; int contact_count; bool collided; - int cc; static void _contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata); diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp index 59f987fc17..36511f78ce 100644 --- a/servers/physics/body_sw.cpp +++ b/servers/physics/body_sw.cpp @@ -423,22 +423,6 @@ void BodySW::_compute_area_gravity_and_dampenings(const AreaSW *p_area) { area_angular_damp += p_area->get_angular_damp(); } -void BodySW::set_combine_mode(PhysicsServer::BodyParameter p_param, PhysicsServer::CombineMode p_mode) { - if (p_param == PhysicsServer::BODY_PARAM_BOUNCE) { - bounce_combine_mode = p_mode; - } else { - friction_combine_mode = p_mode; - } -} - -PhysicsServer::CombineMode BodySW::get_combine_mode(PhysicsServer::BodyParameter p_param) const { - if (p_param == PhysicsServer::BODY_PARAM_BOUNCE) { - return bounce_combine_mode; - } else { - return friction_combine_mode; - } -} - void BodySW::set_axis_lock(PhysicsServer::BodyAxis p_axis, bool lock) { if (lock) { locked_axis |= p_axis; @@ -771,10 +755,10 @@ void BodySW::set_kinematic_margin(real_t p_margin) { BodySW::BodySW() : CollisionObjectSW(TYPE_BODY), + locked_axis(0), active_list(this), inertia_update_list(this), - direct_state_query_list(this), - locked_axis(0) { + direct_state_query_list(this) { mode = PhysicsServer::BODY_MODE_RIGID; active = true; diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h index 5df270f679..0f7797254e 100644 --- a/servers/physics/body_sw.h +++ b/servers/physics/body_sw.h @@ -33,7 +33,7 @@ #include "area_sw.h" #include "collision_object_sw.h" -#include "vset.h" +#include "core/vset.h" class ConstraintSW; @@ -49,8 +49,6 @@ class BodySW : public CollisionObjectSW { real_t mass; real_t bounce; real_t friction; - PhysicsServer::CombineMode bounce_combine_mode; - PhysicsServer::CombineMode friction_combine_mode; real_t linear_damp; real_t angular_damp; @@ -304,12 +302,6 @@ public: _FORCE_INLINE_ Vector3 get_gravity() const { return gravity; } _FORCE_INLINE_ real_t get_bounce() const { return bounce; } - void set_combine_mode(PhysicsServer::BodyParameter p_param, PhysicsServer::CombineMode p_mode); - PhysicsServer::CombineMode get_combine_mode(PhysicsServer::BodyParameter p_param) const; - - _FORCE_INLINE_ PhysicsServer::CombineMode get_bounce_combine_mode() const { return bounce_combine_mode; } - _FORCE_INLINE_ PhysicsServer::CombineMode get_friction_combine_mode() const { return friction_combine_mode; } - void set_axis_lock(PhysicsServer::BodyAxis p_axis, bool lock); bool is_axis_locked(PhysicsServer::BodyAxis p_axis) const; diff --git a/servers/physics/broad_phase_basic.cpp b/servers/physics/broad_phase_basic.cpp index 52483a8b14..d55951b39a 100644 --- a/servers/physics/broad_phase_basic.cpp +++ b/servers/physics/broad_phase_basic.cpp @@ -29,8 +29,8 @@ /*************************************************************************/ #include "broad_phase_basic.h" -#include "list.h" -#include "print_string.h" +#include "core/list.h" +#include "core/print_string.h" BroadPhaseSW::ID BroadPhaseBasic::create(CollisionObjectSW *p_object, int p_subindex) { diff --git a/servers/physics/broad_phase_basic.h b/servers/physics/broad_phase_basic.h index 47fcdb3060..500b8544a1 100644 --- a/servers/physics/broad_phase_basic.h +++ b/servers/physics/broad_phase_basic.h @@ -32,7 +32,7 @@ #define BROAD_PHASE_BASIC_H #include "broad_phase_sw.h" -#include "map.h" +#include "core/map.h" class BroadPhaseBasic : public BroadPhaseSW { diff --git a/servers/physics/broad_phase_octree.h b/servers/physics/broad_phase_octree.h index e7028eba98..c4b8ecb299 100644 --- a/servers/physics/broad_phase_octree.h +++ b/servers/physics/broad_phase_octree.h @@ -32,7 +32,7 @@ #define BROAD_PHASE_OCTREE_H #include "broad_phase_sw.h" -#include "octree.h" +#include "core/math/octree.h" class BroadPhaseOctree : public BroadPhaseSW { diff --git a/servers/physics/broad_phase_sw.h b/servers/physics/broad_phase_sw.h index 7559942cd4..2db1c1dd06 100644 --- a/servers/physics/broad_phase_sw.h +++ b/servers/physics/broad_phase_sw.h @@ -31,8 +31,8 @@ #ifndef BROAD_PHASE_SW_H #define BROAD_PHASE_SW_H -#include "aabb.h" -#include "math_funcs.h" +#include "core/math/aabb.h" +#include "core/math/math_funcs.h" class CollisionObjectSW; diff --git a/servers/physics/collision_object_sw.h b/servers/physics/collision_object_sw.h index b6430b38dc..993799ee10 100644 --- a/servers/physics/collision_object_sw.h +++ b/servers/physics/collision_object_sw.h @@ -32,7 +32,7 @@ #define COLLISION_OBJECT_SW_H #include "broad_phase_sw.h" -#include "self_list.h" +#include "core/self_list.h" #include "servers/physics_server.h" #include "shape_sw.h" diff --git a/servers/physics/collision_solver_sat.cpp b/servers/physics/collision_solver_sat.cpp index 8f2b147460..f17f6f7014 100644 --- a/servers/physics/collision_solver_sat.cpp +++ b/servers/physics/collision_solver_sat.cpp @@ -29,7 +29,7 @@ /*************************************************************************/ #include "collision_solver_sat.h" -#include "geometry.h" +#include "core/math/geometry.h" #define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.02 @@ -44,12 +44,6 @@ struct _CollectorCallback { _FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B) { - /* - if (normal.dot(p_point_A) >= normal.dot(p_point_B)) - return; - print_line("** A: "+p_point_A+" B: "+p_point_B+" D: "+rtos(p_point_A.distance_to(p_point_B))); - */ - if (swap) callback(p_point_B, p_point_A, userdata); else @@ -104,7 +98,6 @@ static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_ Vector3 c = rel_A.cross(rel_B).cross(rel_B); - //if ( Math::abs(rel_A.dot(c) )<_EDGE_IS_VALID_SUPPORT_TRESHOLD ) { if (Math::abs(rel_A.dot(c)) < CMP_EPSILON) { // should handle somehow.. @@ -410,26 +403,13 @@ public: supports_B[i] += best_axis * margin_B; } } - /* - print_line("best depth: "+rtos(best_depth)); - print_line("best axis: "+(best_axis)); - for(int i=0;i<support_count_A;i++) { - - print_line("A-"+itos(i)+": "+supports_A[i]); - } - for(int i=0;i<support_count_B;i++) { - print_line("B-"+itos(i)+": "+supports_B[i]); - } -*/ callback->normal = best_axis; if (callback->prev_axis) *callback->prev_axis = best_axis; _generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback); callback->collided = true; - //CollisionSolverSW::CallbackResult cbk=NULL; - //cbk(Vector3(),Vector3(),NULL); } _FORCE_INLINE_ SeparatorAxisTest(const ShapeA *p_shape_A, const Transform &p_transform_A, const ShapeB *p_shape_B, const Transform &p_transform_B, _CollectorCallback *p_callback, real_t p_margin_A = 0, real_t p_margin_B = 0) { @@ -445,9 +425,6 @@ public: }; /****** SAT TESTS *******/ -/****** SAT TESTS *******/ -/****** SAT TESTS *******/ -/****** SAT TESTS *******/ typedef void (*CollisionFunc)(const ShapeSW *, const Transform &, const ShapeSW *, const Transform &, _CollectorCallback *p_callback, real_t, real_t); @@ -560,6 +537,12 @@ static void _collision_sphere_capsule(const ShapeSW *p_a, const Transform &p_tra } template <bool withMargin> +static void _collision_sphere_cylinder(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + return; +} + +template <bool withMargin> static void _collision_sphere_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW *>(p_a); @@ -851,6 +834,12 @@ static void _collision_box_capsule(const ShapeSW *p_a, const Transform &p_transf } template <bool withMargin> +static void _collision_box_cylinder(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + return; +} + +template <bool withMargin> static void _collision_box_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const BoxShapeSW *box_A = static_cast<const BoxShapeSW *>(p_a); @@ -1127,6 +1116,12 @@ static void _collision_capsule_capsule(const ShapeSW *p_a, const Transform &p_tr } template <bool withMargin> +static void _collision_capsule_cylinder(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + return; +} + +template <bool withMargin> static void _collision_capsule_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW *>(p_a); @@ -1247,6 +1242,24 @@ static void _collision_capsule_face(const ShapeSW *p_a, const Transform &p_trans } template <bool withMargin> +static void _collision_cylinder_cylinder(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + return; +} + +template <bool withMargin> +static void _collision_cylinder_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + return; +} + +template <bool withMargin> +static void _collision_cylinder_face(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + + return; +} + +template <bool withMargin> static void _collision_convex_polygon_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const ConvexPolygonShapeSW *convex_polygon_A = static_cast<const ConvexPolygonShapeSW *>(p_a); @@ -1475,59 +1488,81 @@ bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_tran ERR_FAIL_COND_V(type_B == PhysicsServer::SHAPE_RAY, false); ERR_FAIL_COND_V(p_shape_B->is_concave(), false); - static const CollisionFunc collision_table[5][5] = { + static const CollisionFunc collision_table[6][6] = { { _collision_sphere_sphere<false>, _collision_sphere_box<false>, _collision_sphere_capsule<false>, + _collision_sphere_cylinder<false>, _collision_sphere_convex_polygon<false>, _collision_sphere_face<false> }, { 0, _collision_box_box<false>, _collision_box_capsule<false>, + _collision_box_cylinder<false>, _collision_box_convex_polygon<false>, _collision_box_face<false> }, { 0, 0, _collision_capsule_capsule<false>, + _collision_capsule_cylinder<false>, _collision_capsule_convex_polygon<false>, _collision_capsule_face<false> }, { 0, 0, 0, + _collision_cylinder_cylinder<false>, + _collision_cylinder_convex_polygon<false>, + _collision_cylinder_face<false> }, + { 0, + 0, + 0, + 0, _collision_convex_polygon_convex_polygon<false>, _collision_convex_polygon_face<false> }, { 0, 0, 0, 0, + 0, 0 }, }; - static const CollisionFunc collision_table_margin[5][5] = { + static const CollisionFunc collision_table_margin[6][6] = { { _collision_sphere_sphere<true>, _collision_sphere_box<true>, _collision_sphere_capsule<true>, + _collision_sphere_cylinder<true>, _collision_sphere_convex_polygon<true>, _collision_sphere_face<true> }, { 0, _collision_box_box<true>, _collision_box_capsule<true>, + _collision_box_cylinder<true>, _collision_box_convex_polygon<true>, _collision_box_face<true> }, { 0, 0, _collision_capsule_capsule<true>, + _collision_capsule_cylinder<true>, _collision_capsule_convex_polygon<true>, _collision_capsule_face<true> }, { 0, 0, 0, + _collision_cylinder_cylinder<true>, + _collision_cylinder_convex_polygon<true>, + _collision_cylinder_face<true> }, + { 0, + 0, + 0, + 0, _collision_convex_polygon_convex_polygon<true>, _collision_convex_polygon_face<true> }, { 0, 0, 0, 0, + 0, 0 }, }; diff --git a/servers/physics/collision_solver_sw.cpp b/servers/physics/collision_solver_sw.cpp index 0037b9a862..86ef719f6f 100644 --- a/servers/physics/collision_solver_sw.cpp +++ b/servers/physics/collision_solver_sw.cpp @@ -31,7 +31,6 @@ #include "collision_solver_sw.h" #include "collision_solver_sat.h" -#include "collision_solver_sat.h" #include "gjk_epa.h" #define collision_solver sat_calculate_penetration @@ -176,7 +175,6 @@ bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A, const Transform } concave_B->cull(local_aabb, concave_callback, &cinfo); - //print_line("COL AABB TESTS: "+itos(cinfo.aabb_tests)); return cinfo.collided; } @@ -364,13 +362,10 @@ bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A, const Transform concave_B->cull(local_aabb, concave_distance_callback, &cinfo); if (!cinfo.collided) { - //print_line(itos(cinfo.tested)); r_point_A = cinfo.close_A; r_point_B = cinfo.close_B; } - //print_line("DIST AABB TESTS: "+itos(cinfo.aabb_tests)); - return !cinfo.collided; } else { diff --git a/servers/physics/joints/SCsub b/servers/physics/joints/SCsub index ccc76e823f..d730144861 100644 --- a/servers/physics/joints/SCsub +++ b/servers/physics/joints/SCsub @@ -3,5 +3,3 @@ Import('env') env.add_source_files(env.servers_sources, "*.cpp") - -Export('env') diff --git a/servers/physics/joints/cone_twist_joint_sw.cpp b/servers/physics/joints/cone_twist_joint_sw.cpp index c06f27cc57..37fcde4b76 100644 --- a/servers/physics/joints/cone_twist_joint_sw.cpp +++ b/servers/physics/joints/cone_twist_joint_sw.cpp @@ -332,6 +332,7 @@ void ConeTwistJointSW::set_param(PhysicsServer::ConeTwistJointParam p_param, rea m_relaxationFactor = p_value; } break; + case PhysicsServer::CONE_TWIST_MAX: break; // Can't happen, but silences warning } } @@ -358,6 +359,7 @@ real_t ConeTwistJointSW::get_param(PhysicsServer::ConeTwistJointParam p_param) c return m_relaxationFactor; } break; + case PhysicsServer::CONE_TWIST_MAX: break; // Can't happen, but silences warning } return 0; diff --git a/servers/physics/joints/generic_6dof_joint_sw.cpp b/servers/physics/joints/generic_6dof_joint_sw.cpp index c95e5cef32..3a965ff800 100644 --- a/servers/physics/joints/generic_6dof_joint_sw.cpp +++ b/servers/physics/joints/generic_6dof_joint_sw.cpp @@ -83,7 +83,7 @@ int G6DOFRotationalLimitMotorSW::testLimitValue(real_t test_value) { real_t G6DOFRotationalLimitMotorSW::solveAngularLimits( real_t timeStep, Vector3 &axis, real_t jacDiagABInv, BodySW *body0, BodySW *body1) { - if (needApplyTorques() == false) return 0.0f; + if (!needApplyTorques()) return 0.0f; real_t target_velocity = m_targetVelocity; real_t maxMotorForce = m_maxMotorForce; @@ -497,6 +497,31 @@ void Generic6DOFJointSW::set_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJoi m_angularLimits[p_axis].m_maxLimitForce = p_value; } break; + case PhysicsServer::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: { + // Not implemented in GodotPhysics backend + } break; + case PhysicsServer::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: { + // Not implemented in GodotPhysics backend + } break; + case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: { + // Not implemented in GodotPhysics backend + } break; + case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_DAMPING: { + // Not implemented in GodotPhysics backend + } break; + case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: { + // Not implemented in GodotPhysics backend + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: { + // Not implemented in GodotPhysics backend + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: { + // Not implemented in GodotPhysics backend + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: { + // Not implemented in GodotPhysics backend + } break; + case PhysicsServer::G6DOF_JOINT_MAX: break; // Can't happen, but silences warning } } @@ -572,6 +597,31 @@ real_t Generic6DOFJointSW::get_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJ return m_angularLimits[p_axis].m_maxMotorForce; } break; + case PhysicsServer::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: { + // Not implemented in GodotPhysics backend + } break; + case PhysicsServer::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: { + // Not implemented in GodotPhysics backend + } break; + case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: { + // Not implemented in GodotPhysics backend + } break; + case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_DAMPING: { + // Not implemented in GodotPhysics backend + } break; + case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: { + // Not implemented in GodotPhysics backend + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: { + // Not implemented in GodotPhysics backend + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: { + // Not implemented in GodotPhysics backend + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: { + // Not implemented in GodotPhysics backend + } break; + case PhysicsServer::G6DOF_JOINT_MAX: break; // Can't happen, but silences warning } return 0; } @@ -593,6 +643,16 @@ void Generic6DOFJointSW::set_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJoin m_angularLimits[p_axis].m_enableMotor = p_value; } break; + case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: { + // Not implemented in GodotPhysics backend + } break; + case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: { + // Not implemented in GodotPhysics backend + } break; + case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: { + // Not implemented in GodotPhysics backend + } break; + case PhysicsServer::G6DOF_JOINT_FLAG_MAX: break; // Can't happen, but silences warning } } bool Generic6DOFJointSW::get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag) const { @@ -611,6 +671,16 @@ bool Generic6DOFJointSW::get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJoin return m_angularLimits[p_axis].m_enableMotor; } break; + case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: { + // Not implemented in GodotPhysics backend + } break; + case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: { + // Not implemented in GodotPhysics backend + } break; + case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: { + // Not implemented in GodotPhysics backend + } break; + case PhysicsServer::G6DOF_JOINT_FLAG_MAX: break; // Can't happen, but silences warning } return 0; diff --git a/servers/physics/joints/generic_6dof_joint_sw.h b/servers/physics/joints/generic_6dof_joint_sw.h index b350546c5d..035525c9e6 100644 --- a/servers/physics/joints/generic_6dof_joint_sw.h +++ b/servers/physics/joints/generic_6dof_joint_sw.h @@ -118,14 +118,12 @@ public: //! Is limited bool isLimited() { - if (m_loLimit >= m_hiLimit) return false; - return true; + return (m_loLimit < m_hiLimit); } //! Need apply correction bool needApplyTorques() { - if (m_currentLimit == 0 && m_enableMotor == false) return false; - return true; + return (m_enableMotor || m_currentLimit != 0); } //! calculates error diff --git a/servers/physics/joints/hinge_joint_sw.cpp b/servers/physics/joints/hinge_joint_sw.cpp index d660eba879..50de0e871e 100644 --- a/servers/physics/joints/hinge_joint_sw.cpp +++ b/servers/physics/joints/hinge_joint_sw.cpp @@ -224,18 +224,12 @@ bool HingeJointSW::setup(real_t p_step) { // Compute limit information real_t hingeAngle = get_hinge_angle(); - //print_line("angle: "+rtos(hingeAngle)); //set bias, sign, clear accumulator m_correction = real_t(0.); m_limitSign = real_t(0.); m_solveLimit = false; m_accLimitImpulse = real_t(0.); - /*if (m_useLimit) { - print_line("low: "+rtos(m_lowerLimit)); - print_line("hi: "+rtos(m_upperLimit)); - }*/ - //if (m_lowerLimit < m_upperLimit) if (m_useLimit && m_lowerLimit <= m_upperLimit) { //if (hingeAngle <= m_lowerLimit*m_limitSoftness) @@ -415,6 +409,7 @@ void HingeJointSW::set_param(PhysicsServer::HingeJointParam p_param, real_t p_va case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION: m_relaxationFactor = p_value; break; case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY: m_motorTargetVelocity = p_value; break; case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE: m_maxMotorImpulse = p_value; break; + case PhysicsServer::HINGE_JOINT_MAX: break; // Can't happen, but silences warning } } @@ -430,6 +425,7 @@ real_t HingeJointSW::get_param(PhysicsServer::HingeJointParam p_param) const { case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION: return m_relaxationFactor; case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY: return m_motorTargetVelocity; case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE: return m_maxMotorImpulse; + case PhysicsServer::HINGE_JOINT_MAX: break; // Can't happen, but silences warning } return 0; @@ -440,6 +436,7 @@ void HingeJointSW::set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value) switch (p_flag) { case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT: m_useLimit = p_value; break; case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR: m_enableAngularMotor = p_value; break; + case PhysicsServer::HINGE_JOINT_FLAG_MAX: break; // Can't happen, but silences warning } } bool HingeJointSW::get_flag(PhysicsServer::HingeJointFlag p_flag) const { @@ -447,6 +444,7 @@ bool HingeJointSW::get_flag(PhysicsServer::HingeJointFlag p_flag) const { switch (p_flag) { case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT: return m_useLimit; case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR: return m_enableAngularMotor; + case PhysicsServer::HINGE_JOINT_FLAG_MAX: break; // Can't happen, but silences warning } return false; diff --git a/servers/physics/joints/jacobian_entry_sw.h b/servers/physics/joints/jacobian_entry_sw.h index 42c90c9ae9..4bc1255a9a 100644 --- a/servers/physics/joints/jacobian_entry_sw.h +++ b/servers/physics/joints/jacobian_entry_sw.h @@ -50,7 +50,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#include "transform.h" +#include "core/math/transform.h" class JacobianEntrySW { public: diff --git a/servers/physics/joints/slider_joint_sw.cpp b/servers/physics/joints/slider_joint_sw.cpp index c0e9660b22..30700d45f1 100644 --- a/servers/physics/joints/slider_joint_sw.cpp +++ b/servers/physics/joints/slider_joint_sw.cpp @@ -404,6 +404,8 @@ void SliderJointSW::set_param(PhysicsServer::SliderJointParam p_param, real_t p_ case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: m_softnessOrthoAng = p_value; break; case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: m_restitutionOrthoAng = p_value; break; case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: m_dampingOrthoAng = p_value; break; + + case PhysicsServer::SLIDER_JOINT_MAX: break; // Can't happen, but silences warning } } @@ -433,6 +435,8 @@ real_t SliderJointSW::get_param(PhysicsServer::SliderJointParam p_param) const { case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: return m_softnessOrthoAng; case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: return m_restitutionOrthoAng; case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: return m_dampingOrthoAng; + + case PhysicsServer::SLIDER_JOINT_MAX: break; // Can't happen, but silences warning } return 0; diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp index a06942cb2a..fddb531a4f 100644 --- a/servers/physics/physics_server_sw.cpp +++ b/servers/physics/physics_server_sw.cpp @@ -32,13 +32,19 @@ #include "broad_phase_basic.h" #include "broad_phase_octree.h" +#include "core/os/os.h" +#include "core/script_language.h" #include "joints/cone_twist_joint_sw.h" #include "joints/generic_6dof_joint_sw.h" #include "joints/hinge_joint_sw.h" #include "joints/pin_joint_sw.h" #include "joints/slider_joint_sw.h" -#include "os/os.h" -#include "script_language.h" + +#define FLUSH_QUERY_CHECK \ + if (flushing_queries) { \ + ERR_EXPLAIN("Can't change this state while flushing queries. Use call_deferred()/set_deferred() to change monitoring state instead"); \ + ERR_FAIL(); \ + } RID PhysicsServerSW::shape_create(ShapeType p_shape) { @@ -124,6 +130,13 @@ Variant PhysicsServerSW::shape_get_data(RID p_shape) const { return shape->get_data(); }; +void PhysicsServerSW::shape_set_margin(RID p_shape, real_t p_margin) { +} + +real_t PhysicsServerSW::shape_get_margin(RID p_shape) const { + return 0.0; +} + real_t PhysicsServerSW::shape_get_custom_solver_bias(RID p_shape) const { const ShapeSW *shape = shape_owner.get(p_shape); @@ -292,6 +305,7 @@ void PhysicsServerSW::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) { area->set_shape(p_shape_idx, shape); } + void PhysicsServerSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform) { AreaSW *area = area_owner.get(p_area); @@ -344,6 +358,8 @@ void PhysicsServerSW::area_clear_shapes(RID p_area) { void PhysicsServerSW::area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) { + FLUSH_QUERY_CHECK + AreaSW *area = area_owner.get(p_area); ERR_FAIL_COND(!area); ERR_FAIL_INDEX(p_shape_idx, area->get_shape_count()); @@ -427,6 +443,8 @@ void PhysicsServerSW::area_set_collision_mask(RID p_area, uint32_t p_mask) { void PhysicsServerSW::area_set_monitorable(RID p_area, bool p_monitorable) { + FLUSH_QUERY_CHECK + AreaSW *area = area_owner.get(p_area); ERR_FAIL_COND(!area); @@ -574,6 +592,8 @@ RID PhysicsServerSW::body_get_shape(RID p_body, int p_shape_idx) const { void PhysicsServerSW::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) { + FLUSH_QUERY_CHECK + BodySW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count()); @@ -701,20 +721,6 @@ real_t PhysicsServerSW::body_get_param(RID p_body, BodyParameter p_param) const return body->get_param(p_param); }; -void PhysicsServerSW::body_set_combine_mode(RID p_body, BodyParameter p_param, CombineMode p_mode) { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - - body->set_combine_mode(p_param, p_mode); -} - -PhysicsServer::CombineMode PhysicsServerSW::body_get_combine_mode(RID p_body, BodyParameter p_param) const { - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body, COMBINE_MODE_INHERIT); - - return body->get_combine_mode(p_param); -} - void PhysicsServerSW::body_set_kinematic_safe_margin(RID p_body, real_t p_margin) { BodySW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); @@ -955,7 +961,7 @@ bool PhysicsServerSW::body_is_ray_pickable(RID p_body) const { return body->is_ray_pickable(); } -bool PhysicsServerSW::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result) { +bool PhysicsServerSW::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result, bool p_exclude_raycast_shapes) { BodySW *body = body_owner.get(p_body); ERR_FAIL_COND_V(!body, false); @@ -964,7 +970,19 @@ bool PhysicsServerSW::body_test_motion(RID p_body, const Transform &p_from, cons _update_shapes(); - return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, body->get_kinematic_margin(), r_result); + return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, body->get_kinematic_margin(), r_result, p_exclude_raycast_shapes); +} + +int PhysicsServerSW::body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body, false); + ERR_FAIL_COND_V(!body->get_space(), false); + ERR_FAIL_COND_V(body->get_space()->is_locked(), false); + + _update_shapes(); + + return body->get_space()->test_body_ray_separation(body, p_transform, p_infinite_inertia, r_recover_motion, r_results, p_result_max, p_margin); } PhysicsDirectBodyState *PhysicsServerSW::body_get_direct_state(RID p_body) { @@ -1453,6 +1471,8 @@ void PhysicsServerSW::flush_queries() { doing_sync = true; + flushing_queries = true; + uint64_t time_beg = OS::get_singleton()->get_ticks_usec(); for (Set<const SpaceSW *>::Element *E = active_spaces.front(); E; E = E->next()) { @@ -1461,6 +1481,8 @@ void PhysicsServerSW::flush_queries() { space->call_queries(); } + flushing_queries = false; + if (ScriptDebugger::get_singleton() && ScriptDebugger::get_singleton()->is_profiling()) { uint64_t total_time[SpaceSW::ELAPSED_TIME_MAX]; @@ -1574,6 +1596,7 @@ PhysicsServerSW::PhysicsServerSW() { collision_pairs = 0; active = true; + flushing_queries = false; }; PhysicsServerSW::~PhysicsServerSW(){ diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h index 57037fb325..c361d00fcc 100644 --- a/servers/physics/physics_server_sw.h +++ b/servers/physics/physics_server_sw.h @@ -51,6 +51,8 @@ class PhysicsServerSW : public PhysicsServer { int active_objects; int collision_pairs; + bool flushing_queries; + StepSW *stepper; Set<const SpaceSW *> active_spaces; @@ -85,6 +87,10 @@ public: virtual ShapeType shape_get_type(RID p_shape) const; virtual Variant shape_get_data(RID p_shape) const; + + virtual void shape_set_margin(RID p_shape, real_t p_margin); + virtual real_t shape_get_margin(RID p_shape) const; + virtual real_t shape_get_custom_solver_bias(RID p_shape) const; /* SPACE API */ @@ -188,10 +194,6 @@ public: virtual void body_set_param(RID p_body, BodyParameter p_param, real_t p_value); virtual real_t body_get_param(RID p_body, BodyParameter p_param) const; - /// p_param accept only Bounce and Friction - virtual void body_set_combine_mode(RID p_body, BodyParameter p_param, CombineMode p_mode); - virtual CombineMode body_get_combine_mode(RID p_body, BodyParameter p_param) const; - virtual void body_set_kinematic_safe_margin(RID p_body, real_t p_margin); virtual real_t body_get_kinematic_safe_margin(RID p_body) const; @@ -234,7 +236,8 @@ public: virtual void body_set_ray_pickable(RID p_body, bool p_enable); virtual bool body_is_ray_pickable(RID p_body) const; - virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = NULL); + virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = NULL, bool p_exclude_raycast_shapes = true); + virtual int body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin = 0.001); // this function only works on physics process, errors and returns null otherwise virtual PhysicsDirectBodyState *body_get_direct_state(RID p_body); @@ -345,6 +348,9 @@ public: virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag, bool p_enable); virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag); + virtual void generic_6dof_joint_set_precision(RID p_joint, int precision) {} + virtual int generic_6dof_joint_get_precision(RID p_joint) { return 0; } + virtual JointType joint_get_type(RID p_joint) const; virtual void joint_set_solver_priority(RID p_joint, int p_priority); @@ -364,6 +370,8 @@ public: virtual void flush_queries(); virtual void finish(); + virtual bool is_flushing_queries() const { return flushing_queries; } + int get_process_info(ProcessInfo p_info); PhysicsServerSW(); diff --git a/servers/physics/shape_sw.cpp b/servers/physics/shape_sw.cpp index 9d2e5e846d..e7fc821c2c 100644 --- a/servers/physics/shape_sw.cpp +++ b/servers/physics/shape_sw.cpp @@ -30,9 +30,9 @@ #include "shape_sw.h" -#include "geometry.h" -#include "quick_hull.h" -#include "sort.h" +#include "core/math/geometry.h" +#include "core/math/quick_hull.h" +#include "core/sort.h" #define _POINT_SNAP 0.001953125 #define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.0002 @@ -1047,7 +1047,7 @@ void FaceShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_su /** FIND SUPPORT VERTEX **/ int vert_support_idx = -1; - real_t support_max; + real_t support_max = 0; for (int i = 0; i < 3; i++) { diff --git a/servers/physics/shape_sw.h b/servers/physics/shape_sw.h index 7be818b23c..073d19e317 100644 --- a/servers/physics/shape_sw.h +++ b/servers/physics/shape_sw.h @@ -31,8 +31,8 @@ #ifndef SHAPE_SW_H #define SHAPE_SW_H -#include "bsp_tree.h" -#include "geometry.h" +#include "core/math/bsp_tree.h" +#include "core/math/geometry.h" #include "servers/physics_server.h" /* diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp index b604e5cdf6..3b5344f020 100644 --- a/servers/physics/space_sw.cpp +++ b/servers/physics/space_sw.cpp @@ -31,15 +31,25 @@ #include "space_sw.h" #include "collision_solver_sw.h" +#include "core/project_settings.h" #include "physics_server_sw.h" -#include "project_settings.h" -_FORCE_INLINE_ static bool _can_collide_with(CollisionObjectSW *p_object, uint32_t p_collision_mask) { +_FORCE_INLINE_ static bool _can_collide_with(CollisionObjectSW *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - return p_object->get_collision_layer() & p_collision_mask; + if (!(p_object->get_collision_layer() & p_collision_mask)) { + return false; + } + + if (p_object->get_type() == CollisionObjectSW::TYPE_AREA && !p_collide_with_areas) + return false; + + if (p_object->get_type() == CollisionObjectSW::TYPE_BODY && !p_collide_with_bodies) + return false; + + return true; } -int PhysicsDirectSpaceStateSW::intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask) { +int PhysicsDirectSpaceStateSW::intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { ERR_FAIL_COND_V(space->locked, false); int amount = space->broadphase->cull_point(p_point, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); @@ -52,7 +62,7 @@ int PhysicsDirectSpaceStateSW::intersect_point(const Vector3 &p_point, ShapeResu if (cc >= p_result_max) break; - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask)) + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) continue; //area can't be picked by ray (default) @@ -83,7 +93,7 @@ int PhysicsDirectSpaceStateSW::intersect_point(const Vector3 &p_point, ShapeResu return cc; } -bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_pick_ray) { +bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_ray) { ERR_FAIL_COND_V(space->locked, false); @@ -95,7 +105,7 @@ bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3 &p_from, const Vecto int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); - //todo, create another array tha references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision + //todo, create another array that references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision bool collided = false; Vector3 res_point, res_normal; @@ -105,7 +115,7 @@ bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3 &p_from, const Vecto for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask)) + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) continue; if (p_pick_ray && !(static_cast<CollisionObjectSW *>(space->intersection_query_results[i])->is_ray_pickable())) @@ -161,7 +171,7 @@ bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3 &p_from, const Vecto return true; } -int PhysicsDirectSpaceStateSW::intersect_shape(const RID &p_shape, const Transform &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask) { +int PhysicsDirectSpaceStateSW::intersect_shape(const RID &p_shape, const Transform &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { if (p_result_max <= 0) return 0; @@ -182,7 +192,7 @@ int PhysicsDirectSpaceStateSW::intersect_shape(const RID &p_shape, const Transfo if (cc >= p_result_max) break; - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask)) + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) continue; //area can't be picked by ray (default) @@ -212,7 +222,7 @@ int PhysicsDirectSpaceStateSW::intersect_shape(const RID &p_shape, const Transfo return cc; } -bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, ShapeRestInfo *r_info) { +bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, ShapeRestInfo *r_info) { ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.get(p_shape); ERR_FAIL_COND_V(!shape, false); @@ -221,11 +231,6 @@ bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform aabb = aabb.merge(AABB(aabb.position + p_motion, aabb.size)); //motion aabb = aabb.grow(p_margin); - /* - if (p_motion!=Vector3()) - print_line(p_motion); - */ - int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); real_t best_safe = 1; @@ -242,7 +247,7 @@ bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask)) + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) continue; if (p_exclude.has(space->intersection_query_results[i]->get_self())) @@ -257,7 +262,6 @@ bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform Transform col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); //test initial overlap, does it collide if going all the way? if (CollisionSolverSW::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { - //print_line("failed motion cast (no collision)"); continue; } @@ -265,7 +269,6 @@ bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform sep_axis = p_motion.normalized(); if (!CollisionSolverSW::solve_distance(shape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { - //print_line("failed motion cast (no collision)"); return false; } @@ -288,7 +291,6 @@ bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform if (collided) { - //print_line(itos(i)+": "+rtos(ofs)); hi = ofs; } else { @@ -326,7 +328,7 @@ bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform return true; } -bool PhysicsDirectSpaceStateSW::collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_mask) { +bool PhysicsDirectSpaceStateSW::collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { if (p_result_max <= 0) return 0; @@ -356,7 +358,7 @@ bool PhysicsDirectSpaceStateSW::collide_shape(RID p_shape, const Transform &p_sh for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask)) + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) continue; const CollisionObjectSW *col_obj = space->intersection_query_results[i]; @@ -366,9 +368,6 @@ bool PhysicsDirectSpaceStateSW::collide_shape(RID p_shape, const Transform &p_sh continue; } - //print_line("AGAINST: "+itos(col_obj->get_self().get_id())+":"+itos(shape_idx)); - //print_line("THE ABBB: "+(col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).xform(col_obj->get_shape(shape_idx)->get_aabb())); - if (CollisionSolverSW::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, NULL, p_margin)) { collided = true; } @@ -405,7 +404,7 @@ static void _rest_cbk_result(const Vector3 &p_point_A, const Vector3 &p_point_B, rd->best_object = rd->object; rd->best_shape = rd->shape; } -bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask) { +bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform &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) { ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.get(p_shape); ERR_FAIL_COND_V(!shape, 0); @@ -422,7 +421,7 @@ bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform &p_shape_ for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask)) + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) continue; const CollisionObjectSW *col_obj = space->intersection_query_results[i]; @@ -541,7 +540,154 @@ int SpaceSW::_cull_aabb_for_body(BodySW *p_body, const AABB &p_aabb) { return amount; } -bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer::MotionResult *r_result) { +int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer::SeparationResult *r_results, int p_result_max, real_t p_margin) { + + AABB body_aabb; + + bool shapes_found = false; + + for (int i = 0; i < p_body->get_shape_count(); i++) { + + if (p_body->is_shape_set_as_disabled(i)) + continue; + + if (!shapes_found) { + body_aabb = p_body->get_shape_aabb(i); + shapes_found = true; + } else { + body_aabb = body_aabb.merge(p_body->get_shape_aabb(i)); + } + } + + if (!shapes_found) { + return 0; + } + // Undo the currently transform the physics server is aware of and apply the provided one + body_aabb = p_transform.xform(p_body->get_inv_transform().xform(body_aabb)); + body_aabb = body_aabb.grow(p_margin); + + Transform body_transform = p_transform; + + for (int i = 0; i < p_result_max; i++) { + //reset results + r_results[i].collision_depth = 0; + } + + int rays_found = 0; + + { + // raycast AND separate + + const int max_results = 32; + int recover_attempts = 4; + Vector3 sr[max_results * 2]; + PhysicsServerSW::CollCbkData cbk; + cbk.max = max_results; + PhysicsServerSW::CollCbkData *cbkptr = &cbk; + CollisionSolverSW::CallbackResult cbkres = PhysicsServerSW::_shape_col_cbk; + + do { + + Vector3 recover_motion; + + bool collided = false; + + int amount = _cull_aabb_for_body(p_body, body_aabb); + int ray_index = 0; + + for (int j = 0; j < p_body->get_shape_count(); j++) { + if (p_body->is_shape_set_as_disabled(j)) + continue; + + ShapeSW *body_shape = p_body->get_shape(j); + + if (body_shape->get_type() != PhysicsServer::SHAPE_RAY) + continue; + + Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); + + for (int i = 0; i < amount; i++) { + + const CollisionObjectSW *col_obj = intersection_query_results[i]; + int shape_idx = intersection_query_subindex_results[i]; + + cbk.amount = 0; + cbk.ptr = sr; + + if (CollisionObjectSW::TYPE_BODY == col_obj->get_type()) { + const BodySW *b = static_cast<const BodySW *>(col_obj); + if (p_infinite_inertia && PhysicsServer::BODY_MODE_STATIC != b->get_mode() && PhysicsServer::BODY_MODE_KINEMATIC != b->get_mode()) { + continue; + } + } + + ShapeSW *against_shape = col_obj->get_shape(shape_idx); + if (CollisionSolverSW::solve_static(body_shape, body_shape_xform, against_shape, col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, NULL, p_margin)) { + if (cbk.amount > 0) { + collided = true; + } + + if (ray_index < p_result_max) { + PhysicsServer::SeparationResult &result = r_results[ray_index]; + + for (int k = 0; k < cbk.amount; k++) { + Vector3 a = sr[k * 2 + 0]; + Vector3 b = sr[k * 2 + 1]; + + recover_motion += (b - a) * 0.4; + + float depth = a.distance_to(b); + if (depth > result.collision_depth) { + + result.collision_depth = depth; + result.collision_point = b; + result.collision_normal = (b - a).normalized(); + result.collision_local_shape = shape_idx; + result.collider = col_obj->get_self(); + result.collider_id = col_obj->get_instance_id(); + //result.collider_metadata = col_obj->get_shape_metadata(shape_idx); + if (col_obj->get_type() == CollisionObjectSW::TYPE_BODY) { + BodySW *body = (BodySW *)col_obj; + + Vector3 rel_vec = b - body->get_transform().get_origin(); + //result.collider_velocity = Vector3(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); + result.collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - rel_vec); // * mPos); + } + } + } + } + } + } + + ray_index++; + } + + rays_found = MAX(ray_index, rays_found); + + if (!collided || recover_motion == Vector3()) { + break; + } + + body_transform.origin += recover_motion; + body_aabb.position += recover_motion; + + recover_attempts--; + } while (recover_attempts); + } + + //optimize results (remove non colliding) + for (int i = 0; i < rays_found; i++) { + if (r_results[i].collision_depth == 0) { + rays_found--; + SWAP(r_results[i], r_results[rays_found]); + } + } + + r_recover_motion = body_transform.origin - p_transform.origin; + return rays_found; +} + +bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer::MotionResult *r_result, bool p_exclude_raycast_shapes) { //give me back regular physics engine logic //this is madness @@ -555,13 +701,23 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve r_result->collider_shape = 0; } AABB body_aabb; + bool shapes_found = false; for (int i = 0; i < p_body->get_shape_count(); i++) { - if (i == 0) + if (p_body->is_shape_set_as_disabled(i)) + continue; + + if (!shapes_found) { body_aabb = p_body->get_shape_aabb(i); - else + shapes_found = true; + } else { body_aabb = body_aabb.merge(p_body->get_shape_aabb(i)); + } + } + + if (!shapes_found) { + return false; } // Undo the currently transform the physics server is aware of and apply the provided one @@ -597,6 +753,10 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); ShapeSW *body_shape = p_body->get_shape(j); + if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer::SHAPE_RAY) { + continue; + } + for (int i = 0; i < amount; i++) { const CollisionObjectSW *col_obj = intersection_query_results[i]; @@ -655,6 +815,10 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); ShapeSW *body_shape = p_body->get_shape(j); + if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer::SHAPE_RAY) { + continue; + } + Transform body_shape_xform_inv = body_shape_xform.affine_inverse(); MotionShapeSW mshape; mshape.shape = body_shape; @@ -677,13 +841,11 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve Transform col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); //test initial overlap, does it collide if going all the way? if (CollisionSolverSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { - //print_line("failed motion cast (no collision)"); continue; } sep_axis = p_motion.normalized(); if (!CollisionSolverSW::solve_distance(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { - //print_line("failed motion cast (no collision)"); stuck = true; break; } @@ -707,7 +869,6 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve if (collided) { - //print_line(itos(i)+": "+rtos(ofs)); hi = ofs; } else { @@ -1042,6 +1203,7 @@ SpaceSW::SpaceSW() { body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_linear", 0.1); body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_angular", (8.0 / 180.0 * Math_PI)); body_time_to_sleep = GLOBAL_DEF("physics/3d/time_before_sleep", 0.5); + ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/time_before_sleep", PropertyInfo(Variant::REAL, "physics/3d/time_before_sleep", PROPERTY_HINT_RANGE, "0,5,0.01,or_greater")); body_angular_velocity_damp_ratio = 10; broadphase = BroadPhaseSW::create_func(); diff --git a/servers/physics/space_sw.h b/servers/physics/space_sw.h index 4d864e9a51..d550b374e3 100644 --- a/servers/physics/space_sw.h +++ b/servers/physics/space_sw.h @@ -37,9 +37,9 @@ #include "body_sw.h" #include "broad_phase_sw.h" #include "collision_object_sw.h" -#include "hash_map.h" -#include "project_settings.h" -#include "typedefs.h" +#include "core/hash_map.h" +#include "core/project_settings.h" +#include "core/typedefs.h" class PhysicsDirectSpaceStateSW : public PhysicsDirectSpaceState { @@ -48,12 +48,12 @@ class PhysicsDirectSpaceStateSW : public PhysicsDirectSpaceState { public: SpaceSW *space; - virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF); - virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_pick_ray = false); - virtual int intersect_shape(const RID &p_shape, const Transform &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF); - virtual bool cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, ShapeRestInfo *r_info = NULL); - virtual bool collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF); - virtual bool rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF); + virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false); + virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_ray = false); + virtual int intersect_shape(const RID &p_shape, const Transform &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false); + virtual bool cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, ShapeRestInfo *r_info = NULL); + virtual bool collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false); + virtual bool rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false); virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const; PhysicsDirectSpaceStateSW(); @@ -197,7 +197,8 @@ 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(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer::MotionResult *r_result); + int test_body_ray_separation(BodySW *p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer::SeparationResult *r_results, int p_result_max, real_t p_margin); + bool test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer::MotionResult *r_result, bool p_exclude_raycast_shapes); SpaceSW(); ~SpaceSW(); diff --git a/servers/physics/step_sw.cpp b/servers/physics/step_sw.cpp index ad08cb6353..5238f24b20 100644 --- a/servers/physics/step_sw.cpp +++ b/servers/physics/step_sw.cpp @@ -31,7 +31,7 @@ #include "step_sw.h" #include "joints_sw.h" -#include "os/os.h" +#include "core/os/os.h" void StepSW::_populate_island(BodySW *p_body, BodySW **p_island, ConstraintSW **p_constraint_island) { @@ -228,7 +228,6 @@ void StepSW::step(SpaceSW *p_space, real_t p_delta, int p_iterations) { profile_begtime = profile_endtime; } - //print_line("island count: "+itos(island_count)+" active count: "+itos(active_count)); /* SETUP CONSTRAINT ISLANDS */ { |