diff options
Diffstat (limited to 'servers/physics')
| -rw-r--r-- | servers/physics/body_pair_sw.cpp | 34 | ||||
| -rw-r--r-- | servers/physics/body_sw.cpp | 16 | ||||
| -rw-r--r-- | servers/physics/body_sw.h | 8 | ||||
| -rw-r--r-- | servers/physics/collision_solver_sat.cpp | 84 | ||||
| -rw-r--r-- | servers/physics/collision_solver_sw.cpp | 4 | ||||
| -rw-r--r-- | servers/physics/joints/hinge_joint_sw.cpp | 6 | ||||
| -rw-r--r-- | servers/physics/physics_server_sw.cpp | 38 | ||||
| -rw-r--r-- | servers/physics/physics_server_sw.h | 11 | ||||
| -rw-r--r-- | servers/physics/space_sw.cpp | 199 | ||||
| -rw-r--r-- | servers/physics/space_sw.h | 15 | ||||
| -rw-r--r-- | servers/physics/step_sw.cpp | 1 |
11 files changed, 268 insertions, 148 deletions
diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp index 5a41b621eb..0ce38e4486 100644 --- a/servers/physics/body_pair_sw.cpp +++ b/servers/physics/body_pair_sw.cpp @@ -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_sw.cpp b/servers/physics/body_sw.cpp index 59f987fc17..cc9681193c 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; diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h index 5df270f679..9d7b147fd6 100644 --- a/servers/physics/body_sw.h +++ b/servers/physics/body_sw.h @@ -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/collision_solver_sat.cpp b/servers/physics/collision_solver_sat.cpp index 8f2b147460..294b1df241 100644 --- a/servers/physics/collision_solver_sat.cpp +++ b/servers/physics/collision_solver_sat.cpp @@ -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 @@ -410,26 +404,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 +426,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 +538,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 +835,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 +1117,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 +1243,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 +1489,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..2f2f6d2908 100644 --- a/servers/physics/collision_solver_sw.cpp +++ b/servers/physics/collision_solver_sw.cpp @@ -176,7 +176,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 +363,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/hinge_joint_sw.cpp b/servers/physics/joints/hinge_joint_sw.cpp index d660eba879..368a349632 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) diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp index a06942cb2a..472283833e 100644 --- a/servers/physics/physics_server_sw.cpp +++ b/servers/physics/physics_server_sw.cpp @@ -124,6 +124,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 +299,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); @@ -701,20 +709,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 +949,19 @@ 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); + ERR_FAIL_COND_V(!body->get_space(), false); + ERR_FAIL_COND_V(body->get_space()->is_locked(), false); + + _update_shapes(); + + return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, body->get_kinematic_margin(), r_result, p_exclude_raycast_shapes); +} + +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); @@ -964,7 +970,7 @@ 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_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) { diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h index 57037fb325..4131c5e248 100644 --- a/servers/physics/physics_server_sw.h +++ b/servers/physics/physics_server_sw.h @@ -85,6 +85,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 +192,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 +234,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); diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp index b604e5cdf6..b2ab7bec16 100644 --- a/servers/physics/space_sw.cpp +++ b/servers/physics/space_sw.cpp @@ -34,12 +34,22 @@ #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); @@ -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,144 @@ 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; + + for (int i = 0; i < p_body->get_shape_count(); i++) { + + if (i == 0) + body_aabb = p_body->get_shape_aabb(i); + else + body_aabb = body_aabb.merge(p_body->get_shape_aabb(i)); + } + + // 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 @@ -597,6 +733,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 +795,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 +821,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 +849,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 { diff --git a/servers/physics/space_sw.h b/servers/physics/space_sw.h index 4d864e9a51..e7231df532 100644 --- a/servers/physics/space_sw.h +++ b/servers/physics/space_sw.h @@ -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..4128e1ec1a 100644 --- a/servers/physics/step_sw.cpp +++ b/servers/physics/step_sw.cpp @@ -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 */ { |