diff options
Diffstat (limited to 'servers')
33 files changed, 2096 insertions, 672 deletions
diff --git a/servers/audio/sample_manager_sw.cpp b/servers/audio/sample_manager_sw.cpp index 5a5aa1a34c..29564fe018 100644 --- a/servers/audio/sample_manager_sw.cpp +++ b/servers/audio/sample_manager_sw.cpp @@ -139,7 +139,7 @@ void SampleManagerMallocSW::sample_set_data(RID p_sample, const DVector<uint8_t> DVector<uint8_t>::Read buffer_r=p_buffer.read(); const uint8_t *src = buffer_r.ptr(); uint8_t *dst = (uint8_t*)s->data; - print_line("set data: "+itos(s->length_bytes)); + //print_line("set data: "+itos(s->length_bytes)); for(int i=0;i<s->length_bytes;i++) { diff --git a/servers/physics/area_sw.h b/servers/physics/area_sw.h index 3e39dc3bb6..569195d2c3 100644 --- a/servers/physics/area_sw.h +++ b/servers/physics/area_sw.h @@ -70,7 +70,7 @@ class AreaSW : public CollisionObjectSW{ return area_shape < p_key.area_shape; } else - return body_shape < p_key.area_shape; + return body_shape < p_key.body_shape; } else return rid < p_key.rid; diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp index c50bfac472..3c838367c2 100644 --- a/servers/physics/body_pair_sw.cpp +++ b/servers/physics/body_pair_sw.cpp @@ -29,7 +29,7 @@ #include "body_pair_sw.h" #include "collision_solver_sw.h" #include "space_sw.h" - +#include "os/os.h" /* #define NO_ACCUMULATE_IMPULSES @@ -174,6 +174,11 @@ void BodyPairSW::validate_contacts() { bool BodyPairSW::setup(float p_step) { + //cannot collide + if (A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC)) { + collided=false; + return false; + } offset_B = B->get_transform().get_origin() - A->get_transform().get_origin(); @@ -197,10 +202,6 @@ bool BodyPairSW::setup(float p_step) { return false; - //cannot collide - if (A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC)) { - return false; - } real_t max_penetration = space->get_contact_max_allowed_penetration(); @@ -217,6 +218,7 @@ bool BodyPairSW::setup(float p_step) { } + real_t inv_dt = 1.0/p_step; for(int i=0;i<contact_count;i++) { diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp index 52edc0faa7..0fd754ba25 100644 --- a/servers/physics/body_sw.cpp +++ b/servers/physics/body_sw.cpp @@ -195,7 +195,7 @@ void BodySW::set_mode(PhysicsServer::BodyMode p_mode) { _set_inv_transform(get_transform().affine_inverse()); _inv_mass=0; _set_static(p_mode==PhysicsServer::BODY_MODE_STATIC); - set_active(p_mode==PhysicsServer::BODY_MODE_KINEMATIC); + //set_active(p_mode==PhysicsServer::BODY_MODE_KINEMATIC); linear_velocity=Vector3(); angular_velocity=Vector3(); } break; @@ -203,14 +203,12 @@ void BodySW::set_mode(PhysicsServer::BodyMode p_mode) { _inv_mass=mass>0?(1.0/mass):0; _set_static(false); - simulated_motion=false; //jic } break; case PhysicsServer::BODY_MODE_CHARACTER: { _inv_mass=mass>0?(1.0/mass):0; _set_static(false); - simulated_motion=false; //jic } break; } @@ -235,13 +233,19 @@ void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant& p_varian case PhysicsServer::BODY_STATE_TRANSFORM: { - if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC) { + if (mode==PhysicsServer::BODY_MODE_KINEMATIC) { + new_transform=p_variant; + //wakeup_neighbours(); + set_active(true); + + } else if (mode==PhysicsServer::BODY_MODE_STATIC) { _set_transform(p_variant); _set_inv_transform(get_transform().affine_inverse()); wakeup_neighbours(); } else { Transform t = p_variant; t.orthonormalize(); + new_transform=get_transform(); //used as old to compute motion _set_transform(t); _set_inv_transform(get_transform().inverse()); @@ -353,7 +357,7 @@ void BodySW::_compute_area_gravity(const AreaSW *p_area) { void BodySW::integrate_forces(real_t p_step) { - if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC) + if (mode==PhysicsServer::BODY_MODE_STATIC) return; AreaSW *current_area = get_space()->get_default_area(); @@ -374,28 +378,56 @@ void BodySW::integrate_forces(real_t p_step) { _compute_area_gravity(current_area); density=current_area->get_density(); - if (!omit_force_integration) { - //overriden by direct state query + Vector3 motion; + bool do_motion=false; - Vector3 force=gravity*mass; - force+=applied_force; - Vector3 torque=applied_torque; + if (mode==PhysicsServer::BODY_MODE_KINEMATIC) { - real_t damp = 1.0 - p_step * density; + //compute motion, angular and etc. velocities from prev transform + linear_velocity = (new_transform.origin - get_transform().origin)/p_step; - if (damp<0) // reached zero in the given time - damp=0; + //compute a FAKE angular velocity, not so easy + Matrix3 rot=new_transform.basis.orthonormalized().transposed() * get_transform().basis.orthonormalized(); + Vector3 axis; + float angle; + + rot.get_axis_and_angle(axis,angle); + axis.normalize(); + angular_velocity=axis.normalized() * (angle/p_step); + + motion = new_transform.origin - get_transform().origin; + do_motion=true; + + } else { + if (!omit_force_integration) { + //overriden by direct state query - real_t angular_damp = 1.0 - p_step * density * get_space()->get_body_angular_velocity_damp_ratio(); + Vector3 force=gravity*mass; + force+=applied_force; + Vector3 torque=applied_torque; - if (angular_damp<0) // reached zero in the given time - angular_damp=0; + real_t damp = 1.0 - p_step * density; - linear_velocity*=damp; - angular_velocity*=angular_damp; + if (damp<0) // reached zero in the given time + damp=0; + + real_t angular_damp = 1.0 - p_step * density * get_space()->get_body_angular_velocity_damp_ratio(); + + if (angular_damp<0) // reached zero in the given time + angular_damp=0; + + linear_velocity*=damp; + angular_velocity*=angular_damp; + + linear_velocity+=_inv_mass * force * p_step; + angular_velocity+=_inv_inertia_tensor.xform(torque)*p_step; + } + + if (continuous_cd) { + motion=linear_velocity*p_step; + do_motion=true; + } - linear_velocity+=_inv_mass * force * p_step; - angular_velocity+=_inv_inertia_tensor.xform(torque)*p_step; } applied_force=Vector3(); @@ -406,8 +438,11 @@ void BodySW::integrate_forces(real_t p_step) { biased_angular_velocity=Vector3(); biased_linear_velocity=Vector3(); - if (continuous_cd) //shapes temporarily extend for raycast - _update_shapes_with_motion(linear_velocity*p_step); + + if (do_motion) {//shapes temporarily extend for raycast + _update_shapes_with_motion(motion); + } + current_area=NULL; // clear the area, so it is set in the next frame contact_count=0; @@ -419,9 +454,16 @@ void BodySW::integrate_velocities(real_t p_step) { if (mode==PhysicsServer::BODY_MODE_STATIC) return; + if (fi_callback) + get_space()->body_add_to_state_query_list(&direct_state_query_list); + if (mode==PhysicsServer::BODY_MODE_KINEMATIC) { - if (fi_callback) - get_space()->body_add_to_state_query_list(&direct_state_query_list); + + _set_transform(new_transform,false); + _set_inv_transform(new_transform.affine_inverse()); ; + if (linear_velocity==Vector3() && angular_velocity==Vector3()) + set_active(false); //stopped moving, deactivate + return; } @@ -475,14 +517,13 @@ void BodySW::integrate_velocities(real_t p_step) { _update_inertia_tensor(); - if (fi_callback) { - - get_space()->body_add_to_state_query_list(&direct_state_query_list); - } + //if (fi_callback) { + // get_space()->body_add_to_state_query_list(&direct_state_query_list); + // } - +/* void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) { Transform inv_xform = p_xform.affine_inverse(); @@ -514,6 +555,7 @@ void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) { } +*/ void BodySW::wakeup_neighbours() { @@ -562,12 +604,7 @@ void BodySW::call_queries() { } - if (simulated_motion) { - // linear_velocity=Vector3(); - // angular_velocity=0; - simulated_motion=false; - } } @@ -634,7 +671,7 @@ BodySW::BodySW() : CollisionObjectSW(TYPE_BODY), active_list(this), inertia_upda _set_static(false); density=0; contact_count=0; - simulated_motion=false; + still_time=0; continuous_cd=false; can_sleep=false; diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h index 8d30069777..f627c41231 100644 --- a/servers/physics/body_sw.h +++ b/servers/physics/body_sw.h @@ -71,11 +71,12 @@ class BodySW : public CollisionObjectSW { VSet<RID> exceptions; bool omit_force_integration; bool active; - bool simulated_motion; + bool continuous_cd; bool can_sleep; void _update_inertia(); virtual void _shapes_changed(); + Transform new_transform; Map<ConstraintSW*,int> constraint_map; @@ -235,7 +236,7 @@ public: void integrate_forces(real_t p_step); void integrate_velocities(real_t p_step); - void simulate_motion(const Transform& p_xform,real_t p_step); + //void simulate_motion(const Transform& p_xform,real_t p_step); void call_queries(); void wakeup_neighbours(); diff --git a/servers/physics/collision_object_sw.cpp b/servers/physics/collision_object_sw.cpp index 156004d15d..f34aa19cae 100644 --- a/servers/physics/collision_object_sw.cpp +++ b/servers/physics/collision_object_sw.cpp @@ -216,4 +216,5 @@ CollisionObjectSW::CollisionObjectSW(Type p_type) { type=p_type; space=NULL; instance_id=0; + layer_mask=1; } diff --git a/servers/physics/collision_object_sw.h b/servers/physics/collision_object_sw.h index 6d60f2f078..558a48f6fd 100644 --- a/servers/physics/collision_object_sw.h +++ b/servers/physics/collision_object_sw.h @@ -47,6 +47,7 @@ private: Type type; RID self; ObjectID instance_id; + uint32_t layer_mask; struct Shape { @@ -71,7 +72,7 @@ protected: void _update_shapes_with_motion(const Vector3& p_motion); void _unregister_shapes(); - _FORCE_INLINE_ void _set_transform(const Transform& p_transform) { transform=p_transform; _update_shapes(); } + _FORCE_INLINE_ void _set_transform(const Transform& p_transform,bool p_update_shapes=true) { transform=p_transform; if (p_update_shapes) _update_shapes(); } _FORCE_INLINE_ void _set_inv_transform(const Transform& p_transform) { inv_transform=p_transform; } void _set_static(bool p_static); @@ -104,6 +105,8 @@ public: _FORCE_INLINE_ SpaceSW* get_space() const { return space; } + _FORCE_INLINE_ void set_layer_mask(uint32_t p_mask) { layer_mask=p_mask; } + _FORCE_INLINE_ uint32_t get_layer_mask() const { return layer_mask; } void remove_shape(ShapeSW *p_shape); void remove_shape(int p_index); diff --git a/servers/physics/collision_solver_sat.cpp b/servers/physics/collision_solver_sat.cpp index 1cd40db772..750874f507 100644 --- a/servers/physics/collision_solver_sat.cpp +++ b/servers/physics/collision_solver_sat.cpp @@ -43,7 +43,9 @@ 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; + // 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 @@ -231,11 +233,14 @@ static void _generate_contacts_face_face(const Vector3 * p_points_A,int p_point_ for (int i=0;i<clipbuf_len;i++) { float d = plane_B.distance_to(clipbuf_src[i]); - if (d>CMP_EPSILON) - continue; + //if (d>CMP_EPSILON) + // continue; Vector3 closest_B=clipbuf_src[i] - plane_B.normal*d; + if (p_callback->normal.dot(clipbuf_src[i]) >= p_callback->normal.dot(closest_B)) + continue; + p_callback->call(clipbuf_src[i],closest_B); added++; @@ -301,7 +306,7 @@ static void _generate_contacts_from_supports(const Vector3 * p_points_A,int p_po -template<class ShapeA, class ShapeB> +template<class ShapeA, class ShapeB, bool withMargin=false> class SeparatorAxisTest { const ShapeA *shape_A; @@ -311,7 +316,8 @@ class SeparatorAxisTest { real_t best_depth; Vector3 best_axis; _CollectorCallback *callback; - + real_t margin_A; + real_t margin_B; Vector3 separator_axis; public: @@ -340,6 +346,13 @@ public: shape_A->project_range(axis,*transform_A,min_A,max_A); shape_B->project_range(axis,*transform_B,min_B,max_B); + if (withMargin) { + min_A-=margin_A; + max_A+=margin_A; + min_B-=margin_B; + max_B+=margin_B; + } + min_B -= ( max_A - min_A ) * 0.5; max_B += ( max_A - min_A ) * 0.5; @@ -394,6 +407,14 @@ public: supports_A[i] = transform_A->xform(supports_A[i]); } + if (withMargin) { + + for(int i=0;i<support_count_A;i++) { + supports_A[i]+=-best_axis*margin_A; + } + + } + Vector3 supports_B[max_supports]; int support_count_B; @@ -401,8 +422,16 @@ public: for(int i=0;i<support_count_B;i++) { supports_B[i] = transform_B->xform(supports_B[i]); } + + if (withMargin) { + + for(int i=0;i<support_count_B;i++) { + supports_B[i]+=best_axis*margin_B; + } + } /* 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]); @@ -423,13 +452,16 @@ public: } - _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) { + _FORCE_INLINE_ SeparatorAxisTest(const ShapeA *p_shape_A,const Transform& p_transform_A, const ShapeB *p_shape_B,const Transform& p_transform_B,_CollectorCallback *p_callback,real_t p_margin_A=0,real_t p_margin_B=0) { best_depth=1e15; shape_A=p_shape_A; shape_B=p_shape_B; transform_A=&p_transform_A; transform_B=&p_transform_B; callback=p_callback; + margin_A=p_margin_A; + margin_B=p_margin_B; + } }; @@ -440,16 +472,17 @@ public: /****** SAT TESTS *******/ -typedef void (*CollisionFunc)(const ShapeSW*,const Transform&,const ShapeSW*,const Transform&,_CollectorCallback *p_callback); +typedef void (*CollisionFunc)(const ShapeSW*,const Transform&,const ShapeSW*,const Transform&,_CollectorCallback *p_callback,float,float); -static void _collision_sphere_sphere(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) { +template<bool withMargin> +static void _collision_sphere_sphere(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) { const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a); const SphereShapeSW *sphere_B = static_cast<const SphereShapeSW*>(p_b); - SeparatorAxisTest<SphereShapeSW,SphereShapeSW> separator(sphere_A,p_transform_a,sphere_B,p_transform_b,p_collector); + SeparatorAxisTest<SphereShapeSW,SphereShapeSW,withMargin> separator(sphere_A,p_transform_a,sphere_B,p_transform_b,p_collector,p_margin_a,p_margin_b); // previous axis @@ -462,13 +495,14 @@ static void _collision_sphere_sphere(const ShapeSW *p_a,const Transform &p_trans separator.generate_contacts(); } -static void _collision_sphere_box(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) { +template<bool withMargin> +static void _collision_sphere_box(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) { const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a); const BoxShapeSW *box_B = static_cast<const BoxShapeSW*>(p_b); - SeparatorAxisTest<SphereShapeSW,BoxShapeSW> separator(sphere_A,p_transform_a,box_B,p_transform_b,p_collector); + SeparatorAxisTest<SphereShapeSW,BoxShapeSW,withMargin> separator(sphere_A,p_transform_a,box_B,p_transform_b,p_collector,p_margin_a,p_margin_b); if (!separator.test_previous_axis()) return; @@ -516,13 +550,13 @@ static void _collision_sphere_box(const ShapeSW *p_a,const Transform &p_transfor } - -static void _collision_sphere_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) { +template<bool withMargin> +static void _collision_sphere_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) { const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a); const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW*>(p_b); - SeparatorAxisTest<SphereShapeSW,CapsuleShapeSW> separator(sphere_A,p_transform_a,capsule_B,p_transform_b,p_collector); + SeparatorAxisTest<SphereShapeSW,CapsuleShapeSW,withMargin> separator(sphere_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_margin_a,p_margin_b); if (!separator.test_previous_axis()) return; @@ -540,7 +574,7 @@ static void _collision_sphere_capsule(const ShapeSW *p_a,const Transform &p_tran Vector3 capsule_ball_2 = p_transform_b.origin - capsule_axis; - if (!separator.test_axis( (capsule_ball_1 - p_transform_a.origin).normalized() ) ) + if (!separator.test_axis( (capsule_ball_2 - p_transform_a.origin).normalized() ) ) return; //capsule edge, sphere @@ -556,13 +590,14 @@ static void _collision_sphere_capsule(const ShapeSW *p_a,const Transform &p_tran separator.generate_contacts(); } -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) { +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,float p_margin_a,float p_margin_b) { const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a); const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b); - SeparatorAxisTest<SphereShapeSW,ConvexPolygonShapeSW> separator(sphere_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector); + SeparatorAxisTest<SphereShapeSW,ConvexPolygonShapeSW,withMargin> separator(sphere_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector,p_margin_a,p_margin_b); if (!separator.test_previous_axis()) @@ -626,14 +661,15 @@ static void _collision_sphere_convex_polygon(const ShapeSW *p_a,const Transform } -static void _collision_sphere_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b, _CollectorCallback *p_collector) { +template<bool withMargin> +static void _collision_sphere_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) { const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a); const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b); - SeparatorAxisTest<SphereShapeSW,FaceShapeSW> separator(sphere_A,p_transform_a,face_B,p_transform_b,p_collector); + SeparatorAxisTest<SphereShapeSW,FaceShapeSW,withMargin> separator(sphere_A,p_transform_a,face_B,p_transform_b,p_collector,p_margin_a,p_margin_b); Vector3 vertex[3]={ @@ -669,16 +705,14 @@ static void _collision_sphere_face(const ShapeSW *p_a,const Transform &p_transfo } - - - -static void _collision_box_box(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) { +template<bool withMargin> +static void _collision_box_box(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) { const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a); const BoxShapeSW *box_B = static_cast<const BoxShapeSW*>(p_b); - SeparatorAxisTest<BoxShapeSW,BoxShapeSW> separator(box_A,p_transform_a,box_B,p_transform_b,p_collector); + SeparatorAxisTest<BoxShapeSW,BoxShapeSW,withMargin> separator(box_A,p_transform_a,box_B,p_transform_b,p_collector,p_margin_a,p_margin_b); if (!separator.test_previous_axis()) return; @@ -723,18 +757,69 @@ static void _collision_box_box(const ShapeSW *p_a,const Transform &p_transform_a } } + if (withMargin) { + //add endpoint test between closest vertices and edges + + // calculate closest point to sphere + + Vector3 ab_vec = p_transform_b.origin - p_transform_a.origin; + + Vector3 cnormal_a=p_transform_a.basis.xform_inv( ab_vec ); + + Vector3 support_a=p_transform_a.xform( Vector3( + + (cnormal_a.x<0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, + (cnormal_a.y<0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, + (cnormal_a.z<0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z + ) ); + + + Vector3 cnormal_b=p_transform_b.basis.xform_inv( -ab_vec ); + + Vector3 support_b=p_transform_b.xform( Vector3( + + (cnormal_b.x<0) ? -box_B->get_half_extents().x : box_B->get_half_extents().x, + (cnormal_b.y<0) ? -box_B->get_half_extents().y : box_B->get_half_extents().y, + (cnormal_b.z<0) ? -box_B->get_half_extents().z : box_B->get_half_extents().z + ) ); + + Vector3 axis_ab = (support_a-support_b); + + if (!separator.test_axis( axis_ab.normalized() )) { + return; + } + + //now try edges, which become cylinders! + + for(int i=0;i<3;i++) { + + //a ->b + Vector3 axis_a = p_transform_a.basis.get_axis(i); + + if (!separator.test_axis( axis_ab.cross(axis_a).cross(axis_a).normalized() )) + return; + + //b ->a + Vector3 axis_b = p_transform_b.basis.get_axis(i); + + if (!separator.test_axis( axis_ab.cross(axis_b).cross(axis_b).normalized() )) + return; + + } + } + separator.generate_contacts(); } - -static void _collision_box_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) { +template<bool withMargin> +static void _collision_box_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) { const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a); const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW*>(p_b); - SeparatorAxisTest<BoxShapeSW,CapsuleShapeSW> separator(box_A,p_transform_a,capsule_B,p_transform_b,p_collector); + SeparatorAxisTest<BoxShapeSW,CapsuleShapeSW,withMargin> separator(box_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_margin_a,p_margin_b); if (!separator.test_previous_axis()) return; @@ -828,15 +913,15 @@ static void _collision_box_capsule(const ShapeSW *p_a,const Transform &p_transfo separator.generate_contacts(); } - -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) { +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,float p_margin_a,float p_margin_b) { const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a); const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b); - SeparatorAxisTest<BoxShapeSW,ConvexPolygonShapeSW> separator(box_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector); + SeparatorAxisTest<BoxShapeSW,ConvexPolygonShapeSW,withMargin> separator(box_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector,p_margin_a,p_margin_b); if (!separator.test_previous_axis()) return; @@ -886,18 +971,84 @@ static void _collision_box_convex_polygon(const ShapeSW *p_a,const Transform &p_ } } + if (withMargin) { + + // calculate closest points between vertices and box edges + for(int v=0;v<vertex_count;v++) { + + + Vector3 vtxb = p_transform_b.xform(vertices[v]); + Vector3 ab_vec = vtxb - p_transform_a.origin; + + Vector3 cnormal_a=p_transform_a.basis.xform_inv( ab_vec ); + + Vector3 support_a=p_transform_a.xform( Vector3( + + (cnormal_a.x<0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, + (cnormal_a.y<0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, + (cnormal_a.z<0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z + ) ); + + + Vector3 axis_ab = support_a-vtxb; + + if (!separator.test_axis( axis_ab.normalized() )) { + return; + } + + //now try edges, which become cylinders! + + for(int i=0;i<3;i++) { + + //a ->b + Vector3 axis_a = p_transform_a.basis.get_axis(i); + + if (!separator.test_axis( axis_ab.cross(axis_a).cross(axis_a).normalized() )) + return; + } + } + + //convex edges and box points + for (int i=0;i<2;i++) { + for (int j=0;j<2;j++) { + for (int k=0;k<2;k++) { + Vector3 he = box_A->get_half_extents(); + he.x*=(i*2-1); + he.y*=(j*2-1); + he.z*=(k*2-1); + Vector3 point=p_transform_a.origin; + for(int l=0;l<3;l++) + point+=p_transform_a.basis.get_axis(l)*he[l]; + + for(int e=0;e<edge_count;e++) { + + Vector3 p1=p_transform_b.xform(vertices[edges[e].a]); + Vector3 p2=p_transform_b.xform(vertices[edges[e].b]); + Vector3 n = (p2-p1); + + + if (!separator.test_axis( (point-p2).cross(n).cross(n).normalized() )) + return; + } + } + } + } + } + separator.generate_contacts(); } -static void _collision_box_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b, _CollectorCallback *p_collector) { + +template<bool withMargin> +static void _collision_box_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) { const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a); const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b); - SeparatorAxisTest<BoxShapeSW,FaceShapeSW> separator(box_A,p_transform_a,face_B,p_transform_b,p_collector); + SeparatorAxisTest<BoxShapeSW,FaceShapeSW,withMargin> separator(box_A,p_transform_a,face_B,p_transform_b,p_collector,p_margin_a,p_margin_b); Vector3 vertex[3]={ p_transform_b.xform( face_B->vertex[0] ), @@ -918,13 +1069,14 @@ static void _collision_box_face(const ShapeSW *p_a,const Transform &p_transform_ } // combined edges + for(int i=0;i<3;i++) { Vector3 e=vertex[i]-vertex[(i+1)%3]; - for (int i=0;i<3;i++) { + for (int j=0;j<3;j++) { - Vector3 axis = p_transform_a.basis.get_axis(i); + Vector3 axis = p_transform_a.basis.get_axis(j); if (!separator.test_axis( e.cross(axis).normalized() )) return; @@ -932,16 +1084,82 @@ static void _collision_box_face(const ShapeSW *p_a,const Transform &p_transform_ } + if (withMargin) { + + // calculate closest points between vertices and box edges + for(int v=0;v<3;v++) { + + + Vector3 ab_vec = vertex[v] - p_transform_a.origin; + + Vector3 cnormal_a=p_transform_a.basis.xform_inv( ab_vec ); + + Vector3 support_a=p_transform_a.xform( Vector3( + + (cnormal_a.x<0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, + (cnormal_a.y<0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, + (cnormal_a.z<0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z + ) ); + + + Vector3 axis_ab = support_a-vertex[v]; + + if (!separator.test_axis( axis_ab.normalized() )) { + return; + } + + //now try edges, which become cylinders! + + for(int i=0;i<3;i++) { + + //a ->b + Vector3 axis_a = p_transform_a.basis.get_axis(i); + + if (!separator.test_axis( axis_ab.cross(axis_a).cross(axis_a).normalized() )) + return; + } + } + + //convex edges and box points, there has to be a way to speed up this (get closest point?) + for (int i=0;i<2;i++) { + for (int j=0;j<2;j++) { + for (int k=0;k<2;k++) { + Vector3 he = box_A->get_half_extents(); + he.x*=(i*2-1); + he.y*=(j*2-1); + he.z*=(k*2-1); + Vector3 point=p_transform_a.origin; + for(int l=0;l<3;l++) + point+=p_transform_a.basis.get_axis(l)*he[l]; + + for(int e=0;e<3;e++) { + + Vector3 p1=vertex[e]; + Vector3 p2=vertex[(e+1)%3]; + + Vector3 n = (p2-p1); + + if (!separator.test_axis( (point-p2).cross(n).cross(n).normalized() )) + return; + } + } + } + } + + } + separator.generate_contacts(); } -static void _collision_capsule_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) { + +template<bool withMargin> +static void _collision_capsule_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) { const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW*>(p_a); const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW*>(p_b); - SeparatorAxisTest<CapsuleShapeSW,CapsuleShapeSW> separator(capsule_A,p_transform_a,capsule_B,p_transform_b,p_collector); + SeparatorAxisTest<CapsuleShapeSW,CapsuleShapeSW,withMargin> separator(capsule_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_margin_a,p_margin_b); if (!separator.test_previous_axis()) return; @@ -993,13 +1211,14 @@ static void _collision_capsule_capsule(const ShapeSW *p_a,const Transform &p_tra } -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) { +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,float p_margin_a,float p_margin_b) { const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW*>(p_a); const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b); - SeparatorAxisTest<CapsuleShapeSW,ConvexPolygonShapeSW> separator(capsule_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector); + SeparatorAxisTest<CapsuleShapeSW,ConvexPolygonShapeSW,withMargin> separator(capsule_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector,p_margin_a,p_margin_b); if (!separator.test_previous_axis()) return; @@ -1063,12 +1282,14 @@ static void _collision_capsule_convex_polygon(const ShapeSW *p_a,const Transform } -static void _collision_capsule_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b, _CollectorCallback *p_collector) { + +template<bool withMargin> +static void _collision_capsule_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) { const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW*>(p_a); const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b); - SeparatorAxisTest<CapsuleShapeSW,FaceShapeSW> separator(capsule_A,p_transform_a,face_B,p_transform_b,p_collector); + SeparatorAxisTest<CapsuleShapeSW,FaceShapeSW,withMargin> separator(capsule_A,p_transform_a,face_B,p_transform_b,p_collector,p_margin_a,p_margin_b); @@ -1125,13 +1346,14 @@ static void _collision_capsule_face(const ShapeSW *p_a,const Transform &p_transf } -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) { +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,float p_margin_a,float p_margin_b) { const ConvexPolygonShapeSW *convex_polygon_A = static_cast<const ConvexPolygonShapeSW*>(p_a); const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b); - SeparatorAxisTest<ConvexPolygonShapeSW,ConvexPolygonShapeSW> separator(convex_polygon_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector); + SeparatorAxisTest<ConvexPolygonShapeSW,ConvexPolygonShapeSW,withMargin> separator(convex_polygon_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector,p_margin_a,p_margin_b); if (!separator.test_previous_axis()) return; @@ -1192,17 +1414,70 @@ static void _collision_convex_polygon_convex_polygon(const ShapeSW *p_a,const Tr } } + if (withMargin) { + + //vertex-vertex + for(int i=0;i<vertex_count_A;i++) { + + Vector3 va = p_transform_a.xform(vertices_A[i]); + + for(int j=0;j<vertex_count_B;j++) { + + if (!separator.test_axis( (va-p_transform_b.xform(vertices_B[j])).normalized() )) + return; + + } + } + //edge-vertex( hsell) + + for (int i=0;i<edge_count_A;i++) { + + Vector3 e1=p_transform_a.basis.xform( vertices_A[ edges_A[i].a] ); + Vector3 e2=p_transform_a.basis.xform( vertices_A[ edges_A[i].b] ); + Vector3 n = (e2-e1); + + for(int j=0;j<vertex_count_B;j++) { + + Vector3 e3=p_transform_b.xform(vertices_B[j]); + + + if (!separator.test_axis( (e1-e3).cross(n).cross(n).normalized() )) + return; + } + } + + for (int i=0;i<edge_count_B;i++) { + + Vector3 e1=p_transform_b.basis.xform( vertices_B[ edges_B[i].a] ); + Vector3 e2=p_transform_b.basis.xform( vertices_B[ edges_B[i].b] ); + Vector3 n = (e2-e1); + + for(int j=0;j<vertex_count_A;j++) { + + Vector3 e3=p_transform_a.xform(vertices_A[j]); + + + if (!separator.test_axis( (e1-e3).cross(n).cross(n).normalized() )) + return; + } + } + + + } + separator.generate_contacts(); } -static void _collision_convex_polygon_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b, _CollectorCallback *p_collector) { + +template<bool withMargin> +static void _collision_convex_polygon_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) { const ConvexPolygonShapeSW *convex_polygon_A = static_cast<const ConvexPolygonShapeSW*>(p_a); const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b); - SeparatorAxisTest<ConvexPolygonShapeSW,FaceShapeSW> separator(convex_polygon_A,p_transform_a,face_B,p_transform_b,p_collector); + SeparatorAxisTest<ConvexPolygonShapeSW,FaceShapeSW,withMargin> separator(convex_polygon_A,p_transform_a,face_B,p_transform_b,p_collector,p_margin_a,p_margin_b); const Geometry::MeshData &mesh = convex_polygon_A->get_mesh(); @@ -1252,12 +1527,62 @@ static void _collision_convex_polygon_face(const ShapeSW *p_a,const Transform &p } } + + if (withMargin) { + + //vertex-vertex + for(int i=0;i<vertex_count;i++) { + + Vector3 va = p_transform_a.xform(vertices[i]); + + for(int j=0;j<3;j++) { + + if (!separator.test_axis( (va-vertex[j]).normalized() )) + return; + + } + } + //edge-vertex( hsell) + + for (int i=0;i<edge_count;i++) { + + Vector3 e1=p_transform_a.basis.xform( vertices[ edges[i].a] ); + Vector3 e2=p_transform_a.basis.xform( vertices[ edges[i].b] ); + Vector3 n = (e2-e1); + + for(int j=0;j<3;j++) { + + Vector3 e3=vertex[j]; + + + if (!separator.test_axis( (e1-e3).cross(n).cross(n).normalized() )) + return; + } + } + + for (int i=0;i<3;i++) { + + Vector3 e1=vertex[i]; + Vector3 e2=vertex[(i+1)%3]; + Vector3 n = (e2-e1); + + for(int j=0;j<vertex_count;j++) { + + Vector3 e3=p_transform_a.xform(vertices[j]); + + + if (!separator.test_axis( (e1-e3).cross(n).cross(n).normalized() )) + return; + } + } + } + separator.generate_contacts(); } -bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata,bool p_swap,Vector3* r_prev_axis) { +bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata,bool p_swap,Vector3* r_prev_axis,float p_margin_a,float p_margin_b) { PhysicsServer::ShapeType type_A=p_shape_A->get_type(); @@ -1273,26 +1598,54 @@ bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_tran static const CollisionFunc collision_table[5][5]={ - {_collision_sphere_sphere, - _collision_sphere_box, - _collision_sphere_capsule, - _collision_sphere_convex_polygon, - _collision_sphere_face}, + {_collision_sphere_sphere<false>, + _collision_sphere_box<false>, + _collision_sphere_capsule<false>, + _collision_sphere_convex_polygon<false>, + _collision_sphere_face<false>}, + {0, + _collision_box_box<false>, + _collision_box_capsule<false>, + _collision_box_convex_polygon<false>, + _collision_box_face<false>}, + {0, + 0, + _collision_capsule_capsule<false>, + _collision_capsule_convex_polygon<false>, + _collision_capsule_face<false>}, + {0, + 0, + 0, + _collision_convex_polygon_convex_polygon<false>, + _collision_convex_polygon_face<false>}, {0, - _collision_box_box, - _collision_box_capsule, - _collision_box_convex_polygon, - _collision_box_face}, + 0, + 0, + 0, + 0}, + }; + + static const CollisionFunc collision_table_margin[5][5]={ + {_collision_sphere_sphere<true>, + _collision_sphere_box<true>, + _collision_sphere_capsule<true>, + _collision_sphere_convex_polygon<true>, + _collision_sphere_face<true>}, + {0, + _collision_box_box<true>, + _collision_box_capsule<true>, + _collision_box_convex_polygon<true>, + _collision_box_face<true>}, {0, 0, - _collision_capsule_capsule, - _collision_capsule_convex_polygon, - _collision_capsule_face}, + _collision_capsule_capsule<true>, + _collision_capsule_convex_polygon<true>, + _collision_capsule_face<true>}, {0, 0, 0, - _collision_convex_polygon_convex_polygon, - _collision_convex_polygon_face}, + _collision_convex_polygon_convex_polygon<true>, + _collision_convex_polygon_face<true>}, {0, 0, 0, @@ -1311,20 +1664,30 @@ bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_tran const ShapeSW *B=p_shape_B; const Transform *transform_A=&p_transform_A; const Transform *transform_B=&p_transform_B; + float margin_A=p_margin_a; + float margin_B=p_margin_b; if (type_A > type_B) { SWAP(A,B); SWAP(transform_A,transform_B); SWAP(type_A,type_B); + SWAP(margin_A,margin_B); callback.swap = !callback.swap; } - CollisionFunc collision_func = collision_table[type_A-2][type_B-2]; + CollisionFunc collision_func; + if (margin_A!=0.0 || margin_B!=0.0) { + collision_func = collision_table_margin[type_A-2][type_B-2]; + + } else { + collision_func = collision_table[type_A-2][type_B-2]; + + } ERR_FAIL_COND_V(!collision_func,false); - collision_func(A,*transform_A,B,*transform_B,&callback); + collision_func(A,*transform_A,B,*transform_B,&callback,margin_A,margin_B); return callback.collided; diff --git a/servers/physics/collision_solver_sat.h b/servers/physics/collision_solver_sat.h index 5023a17810..eeba53f160 100644 --- a/servers/physics/collision_solver_sat.h +++ b/servers/physics/collision_solver_sat.h @@ -32,6 +32,6 @@ #include "collision_solver_sw.h" -bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector3* r_prev_axis=NULL); +bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector3* r_prev_axis=NULL,float p_margin_a=0,float p_margin_b=0); #endif // COLLISION_SOLVER_SAT_H diff --git a/servers/physics/collision_solver_sw.cpp b/servers/physics/collision_solver_sw.cpp index da28a4934f..673f2d4385 100644 --- a/servers/physics/collision_solver_sw.cpp +++ b/servers/physics/collision_solver_sw.cpp @@ -114,6 +114,10 @@ struct _ConcaveCollisionInfo { bool collided; int aabb_tests; int collisions; + bool tested; + float margin_A; + float margin_B; + Vector3 close_A,close_B; }; @@ -123,7 +127,7 @@ void CollisionSolverSW::concave_callback(void *p_userdata, ShapeSW *p_convex) { _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo*)(p_userdata); cinfo.aabb_tests++; - bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex,*cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result ); + bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex,*cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result,NULL,cinfo.margin_A,cinfo.margin_B); if (!collided) return; @@ -132,7 +136,7 @@ void CollisionSolverSW::concave_callback(void *p_userdata, ShapeSW *p_convex) { } -bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) { +bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,float p_margin_A,float p_margin_B) { const ConcaveShapeSW *concave_B=static_cast<const ConcaveShapeSW*>(p_shape_B); @@ -146,6 +150,8 @@ bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A,const Transform& cinfo.swap_result=p_swap_result; cinfo.collided=false; cinfo.collisions=0; + cinfo.margin_A=p_margin_A; + cinfo.margin_B=p_margin_B; cinfo.aabb_tests=0; @@ -163,21 +169,23 @@ bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A,const Transform& float smin,smax; p_shape_A->project_range(axis,rel_transform,smin,smax); + smin-=p_margin_A; + smax+=p_margin_A; smin*=axis_scale; smax*=axis_scale; + local_aabb.pos[i]=smin; local_aabb.size[i]=smax-smin; } concave_B->cull(local_aabb,concave_callback,&cinfo); - return cinfo.collided; } -bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,Vector3 *r_sep_axis) { +bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,Vector3 *r_sep_axis,float p_margin_A,float p_margin_B) { PhysicsServer::ShapeType type_A=p_shape_A->get_type(); @@ -225,17 +233,126 @@ bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A,const Transform& p return false; if (!swap) - return solve_concave(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false); + return solve_concave(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false,p_margin_A,p_margin_B); else - return solve_concave(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true); + return solve_concave(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true,p_margin_A,p_margin_B); + + + + } else { + + return collision_solver(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback,p_userdata,false,r_sep_axis,p_margin_A,p_margin_B); + } + + + return false; +} + + +void CollisionSolverSW::concave_distance_callback(void *p_userdata, ShapeSW *p_convex) { + + + _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo*)(p_userdata); + cinfo.aabb_tests++; + if (cinfo.collided) + return; + + Vector3 close_A,close_B; + cinfo.collided = !gjk_epa_calculate_distance(cinfo.shape_A,*cinfo.transform_A,p_convex,*cinfo.transform_B,close_A,close_B); + + if (cinfo.collided) + return; + if (!cinfo.tested || close_A.distance_squared_to(close_B) < cinfo.close_A.distance_squared_to(cinfo.close_B)) { + + cinfo.close_A=close_A; + cinfo.close_B=close_B; + cinfo.tested=true; + } + + cinfo.collisions++; + +} + + +bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,Vector3& r_point_A,Vector3& r_point_B,const AABB& p_concave_hint,Vector3 *r_sep_axis) { + + if (p_shape_A->is_concave()) + return false; + + if (p_shape_B->get_type()==PhysicsServer::SHAPE_PLANE) { + + return false; //unsupported + } else if (p_shape_B->is_concave()) { + if (p_shape_A->is_concave()) + return false; + + + const ConcaveShapeSW *concave_B=static_cast<const ConcaveShapeSW*>(p_shape_B); + + _ConcaveCollisionInfo cinfo; + cinfo.transform_A=&p_transform_A; + cinfo.shape_A=p_shape_A; + cinfo.transform_B=&p_transform_B; + cinfo.result_callback=NULL; + cinfo.userdata=NULL; + cinfo.swap_result=false; + cinfo.collided=false; + cinfo.collisions=0; + cinfo.aabb_tests=0; + cinfo.tested=false; + Transform rel_transform = p_transform_A; + rel_transform.origin-=p_transform_B.origin; + + //quickly compute a local AABB + + bool use_cc_hint=p_concave_hint!=AABB(); + AABB cc_hint_aabb; + if (use_cc_hint) { + cc_hint_aabb=p_concave_hint; + cc_hint_aabb.pos-=p_transform_B.origin; + } + + AABB local_aabb; + for(int i=0;i<3;i++) { + + Vector3 axis( p_transform_B.basis.get_axis(i) ); + float axis_scale = 1.0/axis.length(); + axis*=axis_scale; + + float smin,smax; + + if (use_cc_hint) { + cc_hint_aabb.project_range_in_plane(Plane(axis,0),smin,smax); + } else { + p_shape_A->project_range(axis,rel_transform,smin,smax); + } + + smin*=axis_scale; + smax*=axis_scale; + + local_aabb.pos[i]=smin; + local_aabb.size[i]=smax-smin; + } + + + 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; + + } + + return !cinfo.collided; } else { - return collision_solver(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback,p_userdata,false,r_sep_axis); + return gjk_epa_calculate_distance(p_shape_A,p_transform_A,p_shape_B,p_transform_B,r_point_A,r_point_B); //should pass sepaxis.. } return false; } + diff --git a/servers/physics/collision_solver_sw.h b/servers/physics/collision_solver_sw.h index e135ab92e0..430f057c7c 100644 --- a/servers/physics/collision_solver_sw.h +++ b/servers/physics/collision_solver_sw.h @@ -40,12 +40,14 @@ private: static void concave_callback(void *p_userdata, ShapeSW *p_convex); static bool solve_static_plane(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result); static bool solve_ray(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result); - static bool solve_concave(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result); + static bool solve_concave(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,float p_margin_A=0,float p_margin_B=0); + static void concave_distance_callback(void *p_userdata, ShapeSW *p_convex); public: - static bool solve_static(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,Vector3 *r_sep_axis=NULL); + static bool solve_static(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,Vector3 *r_sep_axis=NULL,float p_margin_A=0,float p_margin_B=0); + static bool solve_distance(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,Vector3& r_point_A,Vector3& r_point_B,const AABB& p_concave_hint,Vector3 *r_sep_axis=NULL); }; diff --git a/servers/physics/gjk_epa.cpp b/servers/physics/gjk_epa.cpp index 37edc0d414..9b5b3d4f67 100644 --- a/servers/physics/gjk_epa.cpp +++ b/servers/physics/gjk_epa.cpp @@ -1,31 +1,14 @@ -/*************************************************************************/ -/* gjk_epa.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* http://www.godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ +/*************************************************/ +/* gjk_epa.cpp */ +/*************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/*************************************************/ +/* Source code within this file is: */ +/* (c) 2007-2010 Juan Linietsky, Ariel Manzur */ +/* All Rights Reserved. */ +/*************************************************/ + #include "gjk_epa.h" /*************** Bullet's GJK-EPA2 IMPLEMENTATION *******************/ @@ -798,8 +781,8 @@ bool Distance( const ShapeSW* shape0, w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; } - results.witnesses[0] = wtrs0.xform(w0); - results.witnesses[1] = wtrs0.xform(w1); + results.witnesses[0] = w0; + results.witnesses[1] = w1; results.normal = w0-w1; results.distance = results.normal.length(); results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1; @@ -881,6 +864,24 @@ bool Penetration( const ShapeSW* shape0, + + +bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, Vector3& r_result_A, Vector3& r_result_B) { + + + GjkEpa2::sResults res; + + if (GjkEpa2::Distance(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_transform_B.origin-p_transform_A.origin,res)) { + + r_result_A=res.witnesses[0]; + r_result_B=res.witnesses[1]; + return true; + } + + return false; + +} + bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap ) { GjkEpa2::sResults res; diff --git a/servers/physics/gjk_epa.h b/servers/physics/gjk_epa.h index 0303478f17..08b0a65b15 100644 --- a/servers/physics/gjk_epa.h +++ b/servers/physics/gjk_epa.h @@ -1,31 +1,14 @@ -/*************************************************************************/ -/* gjk_epa.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* http://www.godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ +/*************************************************/ +/* gjk_epa.h */ +/*************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/*************************************************/ +/* Source code within this file is: */ +/* (c) 2007-2010 Juan Linietsky, Ariel Manzur */ +/* All Rights Reserved. */ +/*************************************************/ + #ifndef GJK_EPA_H #define GJK_EPA_H @@ -36,5 +19,6 @@ #include "collision_solver_sw.h" bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false); +bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, Vector3& r_result_A, Vector3& r_result_B); #endif diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp index aff60b5881..a30383a88d 100644 --- a/servers/physics/physics_server_sw.cpp +++ b/servers/physics/physics_server_sw.cpp @@ -568,6 +568,25 @@ bool PhysicsServerSW::body_is_continuous_collision_detection_enabled(RID p_body) return body->is_continuous_collision_detection_enabled(); } +void PhysicsServerSW::body_set_layer_mask(RID p_body, uint32_t p_mask) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->set_layer_mask(p_mask); + +} + +uint32_t PhysicsServerSW::body_get_layer_mask(RID p_body, uint32_t p_mask) const{ + + const BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,0); + + return body->get_layer_mask(); + +} + + void PhysicsServerSW::body_attach_object_instance_ID(RID p_body,uint32_t p_ID) { BodySW *body = body_owner.get(p_body); @@ -618,13 +637,6 @@ float PhysicsServerSW::body_get_param(RID p_body, BodyParameter p_param) const { }; -void PhysicsServerSW::body_static_simulate_motion(RID p_body,const Transform& p_new_transform) { - - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND(!body); - body->simulate_motion(p_new_transform,last_step); - -}; void PhysicsServerSW::body_set_state(RID p_body, BodyState p_state, const Variant& p_variant) { @@ -1021,11 +1033,18 @@ void PhysicsServerSW::step(float p_step) { last_step=p_step; PhysicsDirectBodyStateSW::singleton->step=p_step; + island_count=0; + active_objects=0; + collision_pairs=0; for( Set<const SpaceSW*>::Element *E=active_spaces.front();E;E=E->next()) { stepper->step((SpaceSW*)E->get(),p_step,iterations); + island_count+=E->get()->get_island_count(); + active_objects+=E->get()->get_active_objects(); + collision_pairs+=E->get()->get_collision_pairs(); } -}; + +} void PhysicsServerSW::sync() { @@ -1054,9 +1073,72 @@ void PhysicsServerSW::finish() { }; +int PhysicsServerSW::get_process_info(ProcessInfo p_info) { + + switch(p_info) { + + case INFO_ACTIVE_OBJECTS: { + + return active_objects; + } break; + case INFO_COLLISION_PAIRS: { + return collision_pairs; + } break; + case INFO_ISLAND_COUNT: { + + return island_count; + } break; + + } + + return 0; +} + + +void PhysicsServerSW::_shape_col_cbk(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata) { + + + CollCbkData *cbk=(CollCbkData *)p_userdata; + + if (cbk->max==0) + return; + + if (cbk->amount == cbk->max) { + //find least deep + float min_depth=1e20; + int min_depth_idx=0; + for(int i=0;i<cbk->amount;i++) { + + float d = cbk->ptr[i*2+0].distance_squared_to(cbk->ptr[i*2+1]); + if (d<min_depth) { + min_depth=d; + min_depth_idx=i; + } + + } + + float d = p_point_A.distance_squared_to(p_point_B); + if (d<min_depth) + return; + cbk->ptr[min_depth_idx*2+0]=p_point_A; + cbk->ptr[min_depth_idx*2+1]=p_point_B; + + + } else { + + cbk->ptr[cbk->amount*2+0]=p_point_A; + cbk->ptr[cbk->amount*2+1]=p_point_B; + cbk->amount++; + } +} + + PhysicsServerSW::PhysicsServerSW() { BroadPhaseSW::create_func=BroadPhaseOctree::_create; + island_count=0; + active_objects=0; + collision_pairs=0; active=true; diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h index 0822d76936..185867e817 100644 --- a/servers/physics/physics_server_sw.h +++ b/servers/physics/physics_server_sw.h @@ -47,6 +47,10 @@ friend class PhysicsDirectSpaceStateSW; bool doing_sync; real_t last_step; + int island_count; + int active_objects; + int collision_pairs; + StepSW *stepper; Set<const SpaceSW*> active_spaces; @@ -61,6 +65,15 @@ friend class PhysicsDirectSpaceStateSW; // void _clear_query(QuerySW *p_query); public: + struct CollCbkData { + + int max; + int amount; + Vector3 *ptr; + }; + + static void _shape_col_cbk(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata); + virtual RID shape_create(ShapeType p_shape); virtual void shape_set_data(RID p_shape, const Variant& p_data); virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias); @@ -146,15 +159,15 @@ public: virtual void body_set_enable_continuous_collision_detection(RID p_body,bool p_enable); virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const; + virtual void body_set_layer_mask(RID p_body, uint32_t p_mask); + virtual uint32_t body_get_layer_mask(RID p_body, uint32_t p_mask) const; + virtual void body_set_user_flags(RID p_body, uint32_t p_flags); virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const; virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value); virtual float body_get_param(RID p_body, BodyParameter p_param) const; - //advanced simulation - virtual void body_static_simulate_motion(RID p_body,const Transform& p_new_transform); - virtual void body_set_state(RID p_body, BodyState p_state, const Variant& p_variant); virtual Variant body_get_state(RID p_body, BodyState p_state) const; @@ -209,6 +222,8 @@ public: virtual void flush_queries(); virtual void finish(); + int get_process_info(ProcessInfo p_info); + PhysicsServerSW(); ~PhysicsServerSW(); diff --git a/servers/physics/shape_sw.cpp b/servers/physics/shape_sw.cpp index 1312b7da95..bd4be05bb9 100644 --- a/servers/physics/shape_sw.cpp +++ b/servers/physics/shape_sw.cpp @@ -928,8 +928,8 @@ void FaceShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supp for (int i=0;i<3;i++) { int nx=(i+1)%3; - //if (i!=vert_support_idx && nx!=vert_support_idx) - // continue; + if (i!=vert_support_idx && nx!=vert_support_idx) + continue; // check if edge is valid as a support float dot=(vertex[i]-vertex[nx]).normalized().dot(n); @@ -951,8 +951,12 @@ bool FaceShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end, bool c=Geometry::segment_intersects_triangle(p_begin,p_end,vertex[0],vertex[1],vertex[2],&r_result); - if (c) + if (c) { r_normal=Plane(vertex[0],vertex[1],vertex[2]).normal; + if (r_normal.dot(p_end-p_begin)>0) { + r_normal=-r_normal; + } + } return c; } @@ -1070,13 +1074,15 @@ void ConcavePolygonShapeSW::_cull_segment(int p_idx,_SegmentCullParams *p_params &res)) { - float d=p_params->normal.dot(res) - p_params->normal.dot(p_params->from); + float d=p_params->dir.dot(res) - p_params->dir.dot(p_params->from); //TODO, seems segmen/triangle intersection is broken :( if (d>0 && d<p_params->min_d) { p_params->min_d=d; p_params->result=res; p_params->normal=Plane(vertices[0],vertices[1],vertices[2]).normal; + if (p_params->normal.dot(p_params->dir)>0) + p_params->normal=-p_params->normal; p_params->collisions++; } @@ -1107,7 +1113,7 @@ bool ConcavePolygonShapeSW::intersect_segment(const Vector3& p_begin,const Vecto params.from=p_begin; params.to=p_end; params.collisions=0; - params.normal=(p_end-p_begin).normalized(); + params.dir=(p_end-p_begin).normalized(); params.faces=fr.ptr(); params.vertices=vr.ptr(); diff --git a/servers/physics/shape_sw.h b/servers/physics/shape_sw.h index 890d6d8741..cdb21556b8 100644 --- a/servers/physics/shape_sw.h +++ b/servers/physics/shape_sw.h @@ -26,405 +26,446 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#ifndef SHAPE_SW_H
-#define SHAPE_SW_H
-
-#include "servers/physics_server.h"
-#include "bsp_tree.h"
-#include "geometry.h"
-/*
-
-SHAPE_LINE, ///< plane:"plane"
-SHAPE_SEGMENT, ///< float:"length"
-SHAPE_CIRCLE, ///< float:"radius"
-SHAPE_RECTANGLE, ///< vec3:"extents"
-SHAPE_CONVEX_POLYGON, ///< array of planes:"planes"
-SHAPE_CONCAVE_POLYGON, ///< Vector3 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector3 array)
-SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error
-
-*/
-
-class ShapeSW;
-
-class ShapeOwnerSW {
-public:
-
- virtual void _shape_changed()=0;
- virtual void remove_shape(ShapeSW *p_shape)=0;
-
- virtual ~ShapeOwnerSW() {}
-};
-
-
-class ShapeSW {
-
- RID self;
- AABB aabb;
- bool configured;
- real_t custom_bias;
-
- Map<ShapeOwnerSW*,int> owners;
-protected:
-
- void configure(const AABB& p_aabb);
-public:
-
- enum {
- MAX_SUPPORTS=8
- };
-
- _FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
- _FORCE_INLINE_ RID get_self() const {return self; }
-
- virtual PhysicsServer::ShapeType get_type() const=0;
-
- _FORCE_INLINE_ AABB get_aabb() const { return aabb; }
- _FORCE_INLINE_ bool is_configured() const { return configured; }
-
- virtual bool is_concave() const { return false; }
-
- virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const=0;
- virtual Vector3 get_support(const Vector3& p_normal) const;
- virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const=0;
-
- virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_point, Vector3 &r_normal) const=0;
- virtual Vector3 get_moment_of_inertia(float p_mass) const=0;
-
- virtual void set_data(const Variant& p_data)=0;
- virtual Variant get_data() const=0;
-
- _FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias=p_bias; }
- _FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; }
-
- void add_owner(ShapeOwnerSW *p_owner);
- void remove_owner(ShapeOwnerSW *p_owner);
- bool is_owner(ShapeOwnerSW *p_owner) const;
- const Map<ShapeOwnerSW*,int>& get_owners() const;
-
- ShapeSW();
- virtual ~ShapeSW();
-};
-
-
-class ConcaveShapeSW : public ShapeSW {
-
-public:
-
- virtual bool is_concave() const { return true; }
- typedef void (*Callback)(void* p_userdata,ShapeSW *p_convex);
- virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { r_amount=0; }
-
- virtual void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const=0;
-
- ConcaveShapeSW() {}
-};
-
-class PlaneShapeSW : public ShapeSW {
-
- Plane plane;
-
- void _setup(const Plane& p_plane);
-public:
-
- Plane get_plane() const;
-
- virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_PLANE; }
- virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
- virtual Vector3 get_support(const Vector3& p_normal) const;
- virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { r_amount=0; }
-
- virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
-
- virtual Vector3 get_moment_of_inertia(float p_mass) const;
-
- virtual void set_data(const Variant& p_data);
- virtual Variant get_data() const;
-
- PlaneShapeSW();
-};
-
-class RayShapeSW : public ShapeSW {
-
- float length;
-
- void _setup(float p_length);
-public:
-
- float get_length() const;
-
- virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_RAY; }
- virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
- virtual Vector3 get_support(const Vector3& p_normal) const;
- virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
-
- virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
-
- virtual Vector3 get_moment_of_inertia(float p_mass) const;
-
- virtual void set_data(const Variant& p_data);
- virtual Variant get_data() const;
-
- RayShapeSW();
-};
-
-class SphereShapeSW : public ShapeSW {
-
- real_t radius;
-
- void _setup(real_t p_radius);
-public:
-
- real_t get_radius() const;
-
- virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_SPHERE; }
-
- virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
- virtual Vector3 get_support(const Vector3& p_normal) const;
- virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
- virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
-
- virtual Vector3 get_moment_of_inertia(float p_mass) const;
-
- virtual void set_data(const Variant& p_data);
- virtual Variant get_data() const;
-
- SphereShapeSW();
-};
-
-class BoxShapeSW : public ShapeSW {
-
- Vector3 half_extents;
- void _setup(const Vector3& p_half_extents);
-public:
-
- _FORCE_INLINE_ Vector3 get_half_extents() const { return half_extents; }
-
- virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_BOX; }
-
- virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
- virtual Vector3 get_support(const Vector3& p_normal) const;
- virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
- virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
-
- virtual Vector3 get_moment_of_inertia(float p_mass) const;
-
- virtual void set_data(const Variant& p_data);
- virtual Variant get_data() const;
-
- BoxShapeSW();
-};
-
-class CapsuleShapeSW : public ShapeSW {
-
- real_t height;
- real_t radius;
-
-
- void _setup(real_t p_height,real_t p_radius);
-public:
-
- _FORCE_INLINE_ real_t get_height() const { return height; }
- _FORCE_INLINE_ real_t get_radius() const { return radius; }
-
- virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CAPSULE; }
-
- virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
- virtual Vector3 get_support(const Vector3& p_normal) const;
- virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
- virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
-
- virtual Vector3 get_moment_of_inertia(float p_mass) const;
-
- virtual void set_data(const Variant& p_data);
- virtual Variant get_data() const;
-
- CapsuleShapeSW();
-};
-
-struct ConvexPolygonShapeSW : public ShapeSW {
-
- Geometry::MeshData mesh;
-
- void _setup(const Vector<Vector3>& p_vertices);
-public:
-
- const Geometry::MeshData& get_mesh() const { return mesh; }
-
- virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONVEX_POLYGON; }
-
- virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
- virtual Vector3 get_support(const Vector3& p_normal) const;
- virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
- virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
-
- virtual Vector3 get_moment_of_inertia(float p_mass) const;
-
- virtual void set_data(const Variant& p_data);
- virtual Variant get_data() const;
-
- ConvexPolygonShapeSW();
-
-};
-
-
-struct _VolumeSW_BVH;
-struct FaceShapeSW;
-
-struct ConcavePolygonShapeSW : public ConcaveShapeSW {
- // always a trimesh
-
- struct Face {
-
- Vector3 normal;
- int indices[3];
- };
-
- DVector<Face> faces;
- DVector<Vector3> vertices;
-
- struct BVH {
-
- AABB aabb;
- int left;
- int right;
-
- int face_index;
- };
-
- DVector<BVH> bvh;
-
- struct _CullParams {
-
- AABB aabb;
- Callback callback;
- void *userdata;
- const Face *faces;
- const Vector3 *vertices;
- const BVH *bvh;
- FaceShapeSW *face;
- };
-
- struct _SegmentCullParams {
-
- Vector3 from;
- Vector3 to;
- const Face *faces;
- const Vector3 *vertices;
- const BVH *bvh;
-
- Vector3 result;
- Vector3 normal;
- real_t min_d;
- int collisions;
-
- };
-
- void _cull_segment(int p_idx,_SegmentCullParams *p_params) const;
- void _cull(int p_idx,_CullParams *p_params) const;
-
- void _fill_bvh(_VolumeSW_BVH* p_bvh_tree,BVH* p_bvh_array,int& p_idx);
-
-
- void _setup(DVector<Vector3> p_faces);
-public:
-
- DVector<Vector3> get_faces() const;
-
- virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; }
-
- virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
- virtual Vector3 get_support(const Vector3& p_normal) const;
-
- virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
-
- virtual void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const;
-
- virtual Vector3 get_moment_of_inertia(float p_mass) const;
-
- virtual void set_data(const Variant& p_data);
- virtual Variant get_data() const;
-
- ConcavePolygonShapeSW();
-
-};
-
-
-struct HeightMapShapeSW : public ConcaveShapeSW {
-
- DVector<real_t> heights;
- int width;
- int depth;
- float cell_size;
-
-// void _cull_segment(int p_idx,_SegmentCullParams *p_params) const;
-// void _cull(int p_idx,_CullParams *p_params) const;
-
- void _setup(DVector<float> p_heights,int p_width,int p_depth,float p_cell_size);
-public:
-
- DVector<real_t> get_heights() const;
- int get_width() const;
- int get_depth() const;
- float get_cell_size() const;
-
- virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_HEIGHTMAP; }
-
- virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
- virtual Vector3 get_support(const Vector3& p_normal) const;
- virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
-
- virtual void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const;
-
- virtual Vector3 get_moment_of_inertia(float p_mass) const;
-
- virtual void set_data(const Variant& p_data);
- virtual Variant get_data() const;
-
- HeightMapShapeSW();
-
-};
-
-//used internally
-struct FaceShapeSW : public ShapeSW {
-
- Vector3 normal; //cache
- Vector3 vertex[3];
-
- virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; }
-
- const Vector3& get_vertex(int p_idx) const { return vertex[p_idx]; }
-
- void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
- Vector3 get_support(const Vector3& p_normal) const;
- virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
- bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
-
- Vector3 get_moment_of_inertia(float p_mass) const;
-
- virtual void set_data(const Variant& p_data) {}
- virtual Variant get_data() const { return Variant(); }
-
- FaceShapeSW();
-};
-
-
-
-
-
-struct _ShapeTestConvexBSPSW {
-
- const BSP_Tree *bsp;
- const ShapeSW *shape;
- Transform transform;
-
- _FORCE_INLINE_ void project_range(const Vector3& p_normal, real_t& r_min, real_t& r_max) const {
-
- shape->project_range(p_normal,transform,r_min,r_max);
- }
-
-};
-
-
-
-
-#endif // SHAPESW_H
+#ifndef SHAPE_SW_H +#define SHAPE_SW_H + +#include "servers/physics_server.h" +#include "bsp_tree.h" +#include "geometry.h" +/* + +SHAPE_LINE, ///< plane:"plane" +SHAPE_SEGMENT, ///< float:"length" +SHAPE_CIRCLE, ///< float:"radius" +SHAPE_RECTANGLE, ///< vec3:"extents" +SHAPE_CONVEX_POLYGON, ///< array of planes:"planes" +SHAPE_CONCAVE_POLYGON, ///< Vector3 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector3 array) +SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error + +*/ + +class ShapeSW; + +class ShapeOwnerSW { +public: + + virtual void _shape_changed()=0; + virtual void remove_shape(ShapeSW *p_shape)=0; + + virtual ~ShapeOwnerSW() {} +}; + + +class ShapeSW { + + RID self; + AABB aabb; + bool configured; + real_t custom_bias; + + Map<ShapeOwnerSW*,int> owners; +protected: + + void configure(const AABB& p_aabb); +public: + + enum { + MAX_SUPPORTS=8 + }; + + _FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; } + _FORCE_INLINE_ RID get_self() const {return self; } + + virtual PhysicsServer::ShapeType get_type() const=0; + + _FORCE_INLINE_ AABB get_aabb() const { return aabb; } + _FORCE_INLINE_ bool is_configured() const { return configured; } + + virtual bool is_concave() const { return false; } + + virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const=0; + virtual Vector3 get_support(const Vector3& p_normal) const; + virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const=0; + + virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_point, Vector3 &r_normal) const=0; + virtual Vector3 get_moment_of_inertia(float p_mass) const=0; + + virtual void set_data(const Variant& p_data)=0; + virtual Variant get_data() const=0; + + _FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias=p_bias; } + _FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; } + + void add_owner(ShapeOwnerSW *p_owner); + void remove_owner(ShapeOwnerSW *p_owner); + bool is_owner(ShapeOwnerSW *p_owner) const; + const Map<ShapeOwnerSW*,int>& get_owners() const; + + ShapeSW(); + virtual ~ShapeSW(); +}; + + +class ConcaveShapeSW : public ShapeSW { + +public: + + virtual bool is_concave() const { return true; } + typedef void (*Callback)(void* p_userdata,ShapeSW *p_convex); + virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { r_amount=0; } + + virtual void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const=0; + + ConcaveShapeSW() {} +}; + +class PlaneShapeSW : public ShapeSW { + + Plane plane; + + void _setup(const Plane& p_plane); +public: + + Plane get_plane() const; + + virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_PLANE; } + virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const; + virtual Vector3 get_support(const Vector3& p_normal) const; + virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { r_amount=0; } + + virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const; + + virtual Vector3 get_moment_of_inertia(float p_mass) const; + + virtual void set_data(const Variant& p_data); + virtual Variant get_data() const; + + PlaneShapeSW(); +}; + +class RayShapeSW : public ShapeSW { + + float length; + + void _setup(float p_length); +public: + + float get_length() const; + + virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_RAY; } + virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const; + virtual Vector3 get_support(const Vector3& p_normal) const; + virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const; + + virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const; + + virtual Vector3 get_moment_of_inertia(float p_mass) const; + + virtual void set_data(const Variant& p_data); + virtual Variant get_data() const; + + RayShapeSW(); +}; + +class SphereShapeSW : public ShapeSW { + + real_t radius; + + void _setup(real_t p_radius); +public: + + real_t get_radius() const; + + virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_SPHERE; } + + virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const; + virtual Vector3 get_support(const Vector3& p_normal) const; + virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const; + virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const; + + virtual Vector3 get_moment_of_inertia(float p_mass) const; + + virtual void set_data(const Variant& p_data); + virtual Variant get_data() const; + + SphereShapeSW(); +}; + +class BoxShapeSW : public ShapeSW { + + Vector3 half_extents; + void _setup(const Vector3& p_half_extents); +public: + + _FORCE_INLINE_ Vector3 get_half_extents() const { return half_extents; } + + virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_BOX; } + + virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const; + virtual Vector3 get_support(const Vector3& p_normal) const; + virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const; + virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const; + + virtual Vector3 get_moment_of_inertia(float p_mass) const; + + virtual void set_data(const Variant& p_data); + virtual Variant get_data() const; + + BoxShapeSW(); +}; + +class CapsuleShapeSW : public ShapeSW { + + real_t height; + real_t radius; + + + void _setup(real_t p_height,real_t p_radius); +public: + + _FORCE_INLINE_ real_t get_height() const { return height; } + _FORCE_INLINE_ real_t get_radius() const { return radius; } + + virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CAPSULE; } + + virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const; + virtual Vector3 get_support(const Vector3& p_normal) const; + virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const; + virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const; + + virtual Vector3 get_moment_of_inertia(float p_mass) const; + + virtual void set_data(const Variant& p_data); + virtual Variant get_data() const; + + CapsuleShapeSW(); +}; + +struct ConvexPolygonShapeSW : public ShapeSW { + + Geometry::MeshData mesh; + + void _setup(const Vector<Vector3>& p_vertices); +public: + + const Geometry::MeshData& get_mesh() const { return mesh; } + + virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONVEX_POLYGON; } + + virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const; + virtual Vector3 get_support(const Vector3& p_normal) const; + virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const; + virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const; + + virtual Vector3 get_moment_of_inertia(float p_mass) const; + + virtual void set_data(const Variant& p_data); + virtual Variant get_data() const; + + ConvexPolygonShapeSW(); + +}; + + +struct _VolumeSW_BVH; +struct FaceShapeSW; + +struct ConcavePolygonShapeSW : public ConcaveShapeSW { + // always a trimesh + + struct Face { + + Vector3 normal; + int indices[3]; + }; + + DVector<Face> faces; + DVector<Vector3> vertices; + + struct BVH { + + AABB aabb; + int left; + int right; + + int face_index; + }; + + DVector<BVH> bvh; + + struct _CullParams { + + AABB aabb; + Callback callback; + void *userdata; + const Face *faces; + const Vector3 *vertices; + const BVH *bvh; + FaceShapeSW *face; + }; + + struct _SegmentCullParams { + + Vector3 from; + Vector3 to; + const Face *faces; + const Vector3 *vertices; + const BVH *bvh; + Vector3 dir; + + Vector3 result; + Vector3 normal; + real_t min_d; + int collisions; + + }; + + void _cull_segment(int p_idx,_SegmentCullParams *p_params) const; + void _cull(int p_idx,_CullParams *p_params) const; + + void _fill_bvh(_VolumeSW_BVH* p_bvh_tree,BVH* p_bvh_array,int& p_idx); + + + void _setup(DVector<Vector3> p_faces); +public: + + DVector<Vector3> get_faces() const; + + virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; } + + virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const; + virtual Vector3 get_support(const Vector3& p_normal) const; + + virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const; + + virtual void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const; + + virtual Vector3 get_moment_of_inertia(float p_mass) const; + + virtual void set_data(const Variant& p_data); + virtual Variant get_data() const; + + ConcavePolygonShapeSW(); + +}; + + +struct HeightMapShapeSW : public ConcaveShapeSW { + + DVector<real_t> heights; + int width; + int depth; + float cell_size; + +// void _cull_segment(int p_idx,_SegmentCullParams *p_params) const; +// void _cull(int p_idx,_CullParams *p_params) const; + + void _setup(DVector<float> p_heights,int p_width,int p_depth,float p_cell_size); +public: + + DVector<real_t> get_heights() const; + int get_width() const; + int get_depth() const; + float get_cell_size() const; + + virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_HEIGHTMAP; } + + virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const; + virtual Vector3 get_support(const Vector3& p_normal) const; + virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const; + + virtual void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const; + + virtual Vector3 get_moment_of_inertia(float p_mass) const; + + virtual void set_data(const Variant& p_data); + virtual Variant get_data() const; + + HeightMapShapeSW(); + +}; + +//used internally +struct FaceShapeSW : public ShapeSW { + + Vector3 normal; //cache + Vector3 vertex[3]; + + virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; } + + const Vector3& get_vertex(int p_idx) const { return vertex[p_idx]; } + + void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const; + Vector3 get_support(const Vector3& p_normal) const; + virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const; + bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const; + + Vector3 get_moment_of_inertia(float p_mass) const; + + virtual void set_data(const Variant& p_data) {} + virtual Variant get_data() const { return Variant(); } + + FaceShapeSW(); +}; + + +struct MotionShapeSW : public ShapeSW { + + ShapeSW *shape; + Vector3 motion; + + virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONVEX_POLYGON; } + + + void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { + + Vector3 cast = p_transform.basis.xform(motion); + real_t mina,maxa; + real_t minb,maxb; + Transform ofsb = p_transform; + ofsb.origin+=cast; + shape->project_range(p_normal,p_transform,mina,maxa); + shape->project_range(p_normal,ofsb,minb,maxb); + r_min=MIN(mina,minb); + r_max=MAX(maxa,maxb); + } + + Vector3 get_support(const Vector3& p_normal) const { + + Vector3 support = shape->get_support(p_normal); + if (p_normal.dot(motion)>0) { + support+=motion; + } + return support; + } + virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {} + bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { return false; } + + Vector3 get_moment_of_inertia(float p_mass) const { return Vector3(); } + + virtual void set_data(const Variant& p_data) {} + virtual Variant get_data() const { return Variant(); } + + MotionShapeSW() { configure(AABB()); } +}; + + + + +struct _ShapeTestConvexBSPSW { + + const BSP_Tree *bsp; + const ShapeSW *shape; + Transform transform; + + _FORCE_INLINE_ void project_range(const Vector3& p_normal, real_t& r_min, real_t& r_max) const { + + shape->project_range(p_normal,transform,r_min,r_max); + } + +}; + + + + +#endif // SHAPESW_H diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp index ad86a62280..da023e1144 100644 --- a/servers/physics/space_sw.cpp +++ b/servers/physics/space_sw.cpp @@ -32,7 +32,22 @@ #include "physics_server_sw.h"
-bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3& p_from, const Vector3& p_to,RayResult &r_result,const Set<RID>& p_exclude,uint32_t p_user_mask) {
+_FORCE_INLINE_ static bool _match_object_type_query(CollisionObjectSW *p_object, uint32_t p_layer_mask, uint32_t p_type_mask) {
+
+ if ((p_object->get_layer_mask()&p_layer_mask)==0)
+ return false;
+
+ if (p_object->get_type()==CollisionObjectSW::TYPE_AREA && !(p_type_mask&PhysicsDirectSpaceState::TYPE_MASK_AREA))
+ return false;
+
+ BodySW *body = static_cast<BodySW*>(p_object);
+
+ return (1<<body->get_mode())&p_type_mask;
+
+}
+
+
+bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3& p_from, const Vector3& p_to,RayResult &r_result,const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) {
ERR_FAIL_COND_V(space->locked,false);
@@ -58,8 +73,8 @@ bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3& p_from, const Vecto for(int i=0;i<amount;i++) {
- if (space->intersection_query_results[i]->get_type()==CollisionObjectSW::TYPE_AREA)
- continue; //ignore area
+ if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+ continue;
if (p_exclude.has( space->intersection_query_results[i]->get_self()))
continue;
@@ -114,7 +129,7 @@ bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3& p_from, const Vecto }
-int PhysicsDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Transform& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_user_mask) {
+int PhysicsDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Transform& p_xform,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) {
if (p_result_max<=0)
return 0;
@@ -136,8 +151,9 @@ int PhysicsDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Transfo if (cc>=p_result_max)
break;
- if (space->intersection_query_results[i]->get_type()==CollisionObjectSW::TYPE_AREA)
- continue; //ignore area
+ if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+ continue;
+
if (p_exclude.has( space->intersection_query_results[i]->get_self()))
continue;
@@ -146,7 +162,7 @@ int PhysicsDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Transfo const CollisionObjectSW *col_obj=space->intersection_query_results[i];
int shape_idx=space->intersection_query_subindex_results[i];
- if (!CollisionSolverSW::solve_static(shape,p_xform,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), NULL,NULL,NULL))
+ if (!CollisionSolverSW::solve_static(shape,p_xform,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), NULL,NULL,NULL,p_margin,0))
continue;
r_results[cc].collider_id=col_obj->get_instance_id();
@@ -163,6 +179,283 @@ int PhysicsDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Transfo }
+
+bool PhysicsDirectSpaceStateSW::cast_motion(const RID& p_shape, const Transform& p_xform,const Vector3& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask,ShapeRestInfo *r_info) {
+
+
+
+ ShapeSW *shape = static_cast<PhysicsServerSW*>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);
+ ERR_FAIL_COND_V(!shape,false);
+
+ AABB aabb = p_xform.xform(shape->get_aabb());
+ aabb=aabb.merge(AABB(aabb.pos+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);
+
+ float best_safe=1;
+ float best_unsafe=1;
+
+ Transform xform_inv = p_xform.affine_inverse();
+ MotionShapeSW mshape;
+ mshape.shape=shape;
+ mshape.motion=xform_inv.basis.xform(p_motion);
+
+ bool best_first=true;
+
+ Vector3 closest_A,closest_B;
+
+ for(int i=0;i<amount;i++) {
+
+
+ if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+ continue;
+
+ if (p_exclude.has( space->intersection_query_results[i]->get_self()))
+ continue; //ignore excluded
+
+
+ const CollisionObjectSW *col_obj=space->intersection_query_results[i];
+ int shape_idx=space->intersection_query_subindex_results[i];
+
+ Vector3 point_A,point_B;
+ Vector3 sep_axis=p_motion.normalized();
+
+ 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;
+ }
+
+
+ //test initial overlap
+#if 0
+ if (CollisionSolverSW::solve_static(shape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,NULL,NULL,&sep_axis)) {
+ print_line("failed initial cast (collision at begining)");
+ return false;
+ }
+#else
+ 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;
+ }
+#endif
+
+
+ //just do kinematic solving
+ float low=0;
+ float hi=1;
+ Vector3 mnormal=p_motion.normalized();
+
+ for(int i=0;i<8;i++) { //steps should be customizable..
+
+ Transform xfa = p_xform;
+ float ofs = (low+hi)*0.5;
+
+ Vector3 sep=mnormal; //important optimization for this to work fast enough
+
+ mshape.motion=xform_inv.basis.xform(p_motion*ofs);
+
+ Vector3 lA,lB;
+
+ bool collided = !CollisionSolverSW::solve_distance(&mshape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,lA,lB,aabb,&sep);
+
+ if (collided) {
+
+ //print_line(itos(i)+": "+rtos(ofs));
+ hi=ofs;
+ } else {
+
+ point_A=lA;
+ point_B=lB;
+ low=ofs;
+ }
+ }
+
+ if (low<best_safe) {
+ best_first=true; //force reset
+ best_safe=low;
+ best_unsafe=hi;
+ }
+
+ if (r_info && (best_first || (point_A.distance_squared_to(point_B) < closest_A.distance_squared_to(closest_B) && low<=best_safe))) {
+ closest_A=point_A;
+ closest_B=point_B;
+ r_info->collider_id=col_obj->get_instance_id();
+ r_info->rid=col_obj->get_self();
+ r_info->shape=shape_idx;
+ r_info->point=closest_B;
+ r_info->normal=(closest_A-closest_B).normalized();
+ best_first=false;
+ if (col_obj->get_type()==CollisionObjectSW::TYPE_BODY) {
+ const BodySW *body=static_cast<const BodySW*>(col_obj);
+ r_info->linear_velocity= body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - closest_B);
+ }
+
+ }
+
+
+ }
+
+ p_closest_safe=best_safe;
+ p_closest_unsafe=best_unsafe;
+
+ return true;
+}
+
+bool PhysicsDirectSpaceStateSW::collide_shape(RID p_shape, const Transform& p_shape_xform,float p_margin,Vector3 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask){
+
+ if (p_result_max<=0)
+ return 0;
+
+ ShapeSW *shape = static_cast<PhysicsServerSW*>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);
+ ERR_FAIL_COND_V(!shape,0);
+
+ AABB aabb = p_shape_xform.xform(shape->get_aabb());
+ aabb=aabb.grow(p_margin);
+
+ int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+
+ bool collided=false;
+ int cc=0;
+ r_result_count=0;
+
+ PhysicsServerSW::CollCbkData cbk;
+ cbk.max=p_result_max;
+ cbk.amount=0;
+ cbk.ptr=r_results;
+ CollisionSolverSW::CallbackResult cbkres=NULL;
+
+ PhysicsServerSW::CollCbkData *cbkptr=NULL;
+ if (p_result_max>0) {
+ cbkptr=&cbk;
+ cbkres=PhysicsServerSW::_shape_col_cbk;
+ }
+
+
+ for(int i=0;i<amount;i++) {
+
+ if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+ continue;
+
+ const CollisionObjectSW *col_obj=space->intersection_query_results[i];
+ int shape_idx=space->intersection_query_subindex_results[i];
+
+ if (p_exclude.has( col_obj->get_self() )) {
+ continue;
+ }
+
+ //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;
+ }
+
+ }
+
+ r_result_count=cbk.amount;
+
+ return collided;
+
+}
+
+
+struct _RestCallbackData {
+
+ const CollisionObjectSW *object;
+ const CollisionObjectSW *best_object;
+ int shape;
+ int best_shape;
+ Vector3 best_contact;
+ Vector3 best_normal;
+ float best_len;
+};
+
+static void _rest_cbk_result(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata) {
+
+
+ _RestCallbackData *rd=(_RestCallbackData*)p_userdata;
+
+ Vector3 contact_rel = p_point_B - p_point_A;
+ float len = contact_rel.length();
+ if (len <= rd->best_len)
+ return;
+
+ rd->best_len=len;
+ rd->best_contact=p_point_B;
+ rd->best_normal=contact_rel/len;
+ rd->best_object=rd->object;
+ rd->best_shape=rd->shape;
+
+}
+bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform& p_shape_xform,float p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) {
+
+
+ ShapeSW *shape = static_cast<PhysicsServerSW*>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);
+ ERR_FAIL_COND_V(!shape,0);
+
+ AABB aabb = p_shape_xform.xform(shape->get_aabb());
+ aabb=aabb.grow(p_margin);
+
+ int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+
+ _RestCallbackData rcd;
+ rcd.best_len=0;
+ rcd.best_object=NULL;
+ rcd.best_shape=0;
+
+ for(int i=0;i<amount;i++) {
+
+
+ if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+ continue;
+
+ const CollisionObjectSW *col_obj=space->intersection_query_results[i];
+ int shape_idx=space->intersection_query_subindex_results[i];
+
+ if (p_exclude.has( col_obj->get_self() ))
+ continue;
+
+ rcd.object=col_obj;
+ rcd.shape=shape_idx;
+ bool sc = CollisionSolverSW::solve_static(shape,p_shape_xform,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),_rest_cbk_result,&rcd,NULL,p_margin);
+ if (!sc)
+ continue;
+
+
+ }
+
+ if (rcd.best_len==0)
+ return false;
+
+ r_info->collider_id=rcd.best_object->get_instance_id();
+ r_info->shape=rcd.best_shape;
+ r_info->normal=rcd.best_normal;
+ r_info->point=rcd.best_contact;
+ r_info->rid=rcd.best_object->get_self();
+ if (rcd.best_object->get_type()==CollisionObjectSW::TYPE_BODY) {
+
+ const BodySW *body = static_cast<const BodySW*>(rcd.best_object);
+ Vector3 rel_vec = r_info->point-body->get_transform().get_origin();
+ r_info->linear_velocity = body->get_linear_velocity() +
+ (body->get_angular_velocity()).cross(body->get_transform().origin-rcd.best_contact);// * mPos);
+
+
+ } else {
+ r_info->linear_velocity=Vector3();
+ }
+
+ return true;
+}
+
+
PhysicsDirectSpaceStateSW::PhysicsDirectSpaceStateSW() {
@@ -194,6 +487,8 @@ void* SpaceSW::_broadphase_pair(CollisionObjectSW *A,int p_subindex_A,CollisionO SpaceSW *self = (SpaceSW*)p_self;
+ self->collision_pairs++;
+
if (type_A==CollisionObjectSW::TYPE_AREA) {
@@ -221,6 +516,7 @@ void SpaceSW::_broadphase_unpair(CollisionObjectSW *A,int p_subindex_A,Collision SpaceSW *self = (SpaceSW*)p_self;
+ self->collision_pairs--;
ConstraintSW *c = (ConstraintSW*)p_data;
memdelete(c);
}
@@ -398,6 +694,9 @@ PhysicsDirectSpaceStateSW *SpaceSW::get_direct_state() { SpaceSW::SpaceSW() {
+ collision_pairs=0;
+ active_objects=0;
+ island_count=0;
locked=false;
contact_recycle_radius=0.01;
diff --git a/servers/physics/space_sw.h b/servers/physics/space_sw.h index cec1053400..a97647dcfc 100644 --- a/servers/physics/space_sw.h +++ b/servers/physics/space_sw.h @@ -46,8 +46,11 @@ public: SpaceSW *space;
- bool intersect_ray(const Vector3& p_from, const Vector3& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
- int intersect_shape(const RID& p_shape, const Transform& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
+ 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_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
+ virtual int intersect_shape(const RID& p_shape, const Transform& p_xform,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
+ virtual bool cast_motion(const RID& p_shape, const Transform& p_xform,const Vector3& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION,ShapeRestInfo *r_info=NULL);
+ virtual bool collide_shape(RID p_shape, const Transform& p_shape_xform,float p_margin,Vector3 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
+ virtual bool rest_info(RID p_shape, const Transform& p_shape_xform,float p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
PhysicsDirectSpaceStateSW();
};
@@ -94,6 +97,10 @@ class SpaceSW { bool locked;
+ int island_count;
+ int active_objects;
+ int collision_pairs;
+
friend class PhysicsDirectSpaceStateSW;
public:
@@ -147,6 +154,14 @@ public: void set_param(PhysicsServer::SpaceParameter p_param, real_t p_value);
real_t get_param(PhysicsServer::SpaceParameter p_param) const;
+ void set_island_count(int p_island_count) { island_count=p_island_count; }
+ int get_island_count() const { return island_count; }
+
+ void set_active_objects(int p_active_objects) { active_objects=p_active_objects; }
+ int get_active_objects() const { return active_objects; }
+
+ int get_collision_pairs() const { return collision_pairs; }
+
PhysicsDirectSpaceStateSW *get_direct_state();
SpaceSW();
diff --git a/servers/physics/step_sw.cpp b/servers/physics/step_sw.cpp index b7815d2250..6d95804875 100644 --- a/servers/physics/step_sw.cpp +++ b/servers/physics/step_sw.cpp @@ -49,7 +49,7 @@ void StepSW::_populate_island(BodySW* p_body,BodySW** p_island,ConstraintSW **p_ if (i==E->get()) continue; BodySW *b = c->get_body_ptr()[i]; - if (b->get_island_step()==_step || b->get_mode()==PhysicsServer::BODY_MODE_STATIC) + if (b->get_island_step()==_step || b->get_mode()==PhysicsServer::BODY_MODE_STATIC || b->get_mode()==PhysicsServer::BODY_MODE_KINEMATIC) continue; //no go _populate_island(c->get_body_ptr()[i],p_island,p_constraint_island); } @@ -86,8 +86,10 @@ void StepSW::_check_suspend(BodySW *p_island,float p_delta) { BodySW *b = p_island; while(b) { - if (b->get_mode()==PhysicsServer::BODY_MODE_STATIC) + if (b->get_mode()==PhysicsServer::BODY_MODE_STATIC || b->get_mode()==PhysicsServer::BODY_MODE_KINEMATIC) { + b=b->get_island_next(); continue; //ignore for static + } if (!b->sleep_test(p_delta)) can_sleep=false; @@ -100,8 +102,10 @@ void StepSW::_check_suspend(BodySW *p_island,float p_delta) { b = p_island; while(b) { - if (b->get_mode()==PhysicsServer::BODY_MODE_STATIC) + if (b->get_mode()==PhysicsServer::BODY_MODE_STATIC || b->get_mode()==PhysicsServer::BODY_MODE_KINEMATIC) { + b=b->get_island_next(); continue; //ignore for static + } bool active = b->is_active(); @@ -131,6 +135,7 @@ void StepSW::step(SpaceSW* p_space,float p_delta,int p_iterations) { active_count++; } + p_space->set_active_objects(active_count); /* GENERATE CONSTRAINT ISLANDS */ @@ -164,6 +169,8 @@ void StepSW::step(SpaceSW* p_space,float p_delta,int p_iterations) { b=b->next(); } + p_space->set_island_count(island_count); + const SelfList<AreaSW>::List &aml = p_space->get_moved_area_list(); while(aml.first()) { @@ -207,9 +214,9 @@ void StepSW::step(SpaceSW* p_space,float p_delta,int p_iterations) { b = body_list->first(); while(b) { - + const SelfList<BodySW>*n=b->next(); b->self()->integrate_velocities(p_delta); - b=b->next(); + b=n; } /* SLEEP / WAKE UP ISLANDS */ diff --git a/servers/physics_2d/area_2d_sw.h b/servers/physics_2d/area_2d_sw.h index 51e6ccd166..0eda1050fa 100644 --- a/servers/physics_2d/area_2d_sw.h +++ b/servers/physics_2d/area_2d_sw.h @@ -71,7 +71,7 @@ class Area2DSW : public CollisionObject2DSW{ return area_shape < p_key.area_shape; } else - return body_shape < p_key.area_shape; + return body_shape < p_key.body_shape; } else return rid < p_key.rid; diff --git a/servers/physics_2d/collision_solver_2d_sat.cpp b/servers/physics_2d/collision_solver_2d_sat.cpp index 7d85183645..f73ed5732e 100644 --- a/servers/physics_2d/collision_solver_2d_sat.cpp +++ b/servers/physics_2d/collision_solver_2d_sat.cpp @@ -593,8 +593,8 @@ static void _collision_segment_segment(const Shape2DSW* p_a,const Matrix32& p_tr //this collision is kind of pointless - if (!separator.test_previous_axis()) - return; + //if (!separator.test_previous_axis()) + // return; if (!separator.test_cast()) return; diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp index 2171a9c2c4..09fa3f9b6a 100644 --- a/servers/physics_2d/physics_2d_server_sw.cpp +++ b/servers/physics_2d/physics_2d_server_sw.cpp @@ -1086,9 +1086,15 @@ void Physics2DServerSW::step(float p_step) { last_step=p_step; Physics2DDirectBodyStateSW::singleton->step=p_step; + island_count=0; + active_objects=0; + collision_pairs=0; for( Set<const Space2DSW*>::Element *E=active_spaces.front();E;E=E->next()) { stepper->step((Space2DSW*)E->get(),p_step,iterations); + island_count+=E->get()->get_island_count(); + active_objects+=E->get()->get_active_objects(); + collision_pairs+=E->get()->get_collision_pairs(); } }; @@ -1118,6 +1124,27 @@ void Physics2DServerSW::finish() { memdelete(direct_state); }; +int Physics2DServerSW::get_process_info(ProcessInfo p_info) { + + switch(p_info) { + + case INFO_ACTIVE_OBJECTS: { + + return active_objects; + } break; + case INFO_COLLISION_PAIRS: { + return collision_pairs; + } break; + case INFO_ISLAND_COUNT: { + + return island_count; + } break; + + } + + return 0; +} + Physics2DServerSW::Physics2DServerSW() { @@ -1125,8 +1152,13 @@ Physics2DServerSW::Physics2DServerSW() { // BroadPhase2DSW::create_func=BroadPhase2DBasic::_create; active=true; + island_count=0; + active_objects=0; + collision_pairs=0; + }; + Physics2DServerSW::~Physics2DServerSW() { }; diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h index 09ca029127..7ffffe669f 100644 --- a/servers/physics_2d/physics_2d_server_sw.h +++ b/servers/physics_2d/physics_2d_server_sw.h @@ -47,6 +47,11 @@ friend class Physics2DDirectSpaceStateSW; bool doing_sync; real_t last_step; + int island_count; + int active_objects; + int collision_pairs; + + Step2DSW *stepper; Set<const Space2DSW*> active_spaces; @@ -223,6 +228,8 @@ public: virtual void flush_queries(); virtual void finish(); + int get_process_info(ProcessInfo p_info); + Physics2DServerSW(); ~Physics2DServerSW(); diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h index d3fcf1fab2..8500a6194f 100644 --- a/servers/physics_2d/shape_2d_sw.h +++ b/servers/physics_2d/shape_2d_sw.h @@ -582,5 +582,6 @@ public: }; +#undef DEFAULT_PROJECT_RANGE_CAST #endif // SHAPE_2D_2DSW_H diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index 5fbf828c38..21a99cd4b2 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -323,7 +323,7 @@ bool Physics2DDirectSpaceStateSW::collide_shape(RID p_shape, const Matrix32& p_s } -struct _RestCallbackData { +struct _RestCallbackData2D { const CollisionObject2DSW *object; const CollisionObject2DSW *best_object; @@ -337,7 +337,7 @@ struct _RestCallbackData { static void _rest_cbk_result(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata) { - _RestCallbackData *rd=(_RestCallbackData*)p_userdata; + _RestCallbackData2D *rd=(_RestCallbackData2D*)p_userdata; Vector2 contact_rel = p_point_B - p_point_A; float len = contact_rel.length(); @@ -365,7 +365,7 @@ bool Physics2DDirectSpaceStateSW::rest_info(RID p_shape, const Matrix32& p_shape int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results); - _RestCallbackData rcd; + _RestCallbackData2D rcd; rcd.best_len=0; rcd.best_object=NULL; rcd.best_shape=0; @@ -443,6 +443,7 @@ void* Space2DSW::_broadphase_pair(CollisionObject2DSW *A,int p_subindex_A,Collis } Space2DSW *self = (Space2DSW*)p_self; + self->collision_pairs++; if (type_A==CollisionObject2DSW::TYPE_AREA) { @@ -468,8 +469,8 @@ void* Space2DSW::_broadphase_pair(CollisionObject2DSW *A,int p_subindex_A,Collis void Space2DSW::_broadphase_unpair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_self) { - Space2DSW *self = (Space2DSW*)p_self; + self->collision_pairs--; Constraint2DSW *c = (Constraint2DSW*)p_data; memdelete(c); } @@ -646,6 +647,10 @@ Physics2DDirectSpaceStateSW *Space2DSW::get_direct_state() { Space2DSW::Space2DSW() { + collision_pairs=0; + active_objects=0; + island_count=0; + locked=false; contact_recycle_radius=0.01; contact_max_separation=0.05; diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h index bd41097fba..c638a0c45b 100644 --- a/servers/physics_2d/space_2d_sw.h +++ b/servers/physics_2d/space_2d_sw.h @@ -97,6 +97,10 @@ class Space2DSW { bool locked; + int island_count; + int active_objects; + int collision_pairs; + friend class Physics2DDirectSpaceStateSW; public: @@ -153,6 +157,14 @@ public: void set_param(Physics2DServer::SpaceParameter p_param, real_t p_value); real_t get_param(Physics2DServer::SpaceParameter p_param) const; + void set_island_count(int p_island_count) { island_count=p_island_count; } + int get_island_count() const { return island_count; } + + void set_active_objects(int p_active_objects) { active_objects=p_active_objects; } + int get_active_objects() const { return active_objects; } + + int get_collision_pairs() const { return collision_pairs; } + Physics2DDirectSpaceStateSW *get_direct_state(); Space2DSW(); diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp index 29f4a58287..e75f9300ce 100644 --- a/servers/physics_2d/step_2d_sw.cpp +++ b/servers/physics_2d/step_2d_sw.cpp @@ -137,6 +137,8 @@ void Step2DSW::step(Space2DSW* p_space,float p_delta,int p_iterations) { active_count++; } + p_space->set_active_objects(active_count); + /* GENERATE CONSTRAINT ISLANDS */ Body2DSW *island_list=NULL; @@ -168,6 +170,8 @@ void Step2DSW::step(Space2DSW* p_space,float p_delta,int p_iterations) { b=b->next(); } + p_space->set_island_count(island_count); + const SelfList<Area2DSW>::List &aml = p_space->get_moved_area_list(); diff --git a/servers/physics_2d_server.cpp b/servers/physics_2d_server.cpp index 9cbd7414bd..da8ac5f9c8 100644 --- a/servers/physics_2d_server.cpp +++ b/servers/physics_2d_server.cpp @@ -110,17 +110,132 @@ Physics2DDirectBodyState::Physics2DDirectBodyState() {} -Variant Physics2DDirectSpaceState::_intersect_ray(const Vector2& p_from, const Vector2& p_to,const Vector<RID>& p_exclude,uint32_t p_user_mask) { +void Physics2DShapeQueryParameters::set_shape(const RES &p_shape) { + + ERR_FAIL_COND(p_shape.is_null()); + shape=p_shape->get_rid(); +} + +void Physics2DShapeQueryParameters::set_shape_rid(const RID& p_shape) { + + shape=p_shape; +} + +RID Physics2DShapeQueryParameters::get_shape_rid() const { + + return shape; +} + +void Physics2DShapeQueryParameters::set_transform(const Matrix32& p_transform){ + + transform=p_transform; +} +Matrix32 Physics2DShapeQueryParameters::get_transform() const{ + + return transform; +} + +void Physics2DShapeQueryParameters::set_motion(const Vector2& p_motion){ + + motion=p_motion; +} +Vector2 Physics2DShapeQueryParameters::get_motion() const{ + + return motion; +} + +void Physics2DShapeQueryParameters::set_margin(float p_margin){ + + margin=p_margin; +} +float Physics2DShapeQueryParameters::get_margin() const{ + + return margin; +} + +void Physics2DShapeQueryParameters::set_layer_mask(int p_layer_mask){ + + layer_mask=p_layer_mask; +} +int Physics2DShapeQueryParameters::get_layer_mask() const{ + + return layer_mask; +} + + +void Physics2DShapeQueryParameters::set_object_type_mask(int p_object_type_mask){ + + object_type_mask=p_object_type_mask; +} +int Physics2DShapeQueryParameters::get_object_type_mask() const{ + + return object_type_mask; +} +void Physics2DShapeQueryParameters::set_exclude(const Vector<RID>& p_exclude) { + + exclude.clear();; + for(int i=0;i<p_exclude.size();i++) + exclude.insert(p_exclude[i]); + +} + +Vector<RID> Physics2DShapeQueryParameters::get_exclude() const{ + + Vector<RID> ret; + ret.resize(exclude.size()); + int idx=0; + for(Set<RID>::Element *E=exclude.front();E;E=E->next()) { + ret[idx]=E->get(); + } + return ret; +} + +void Physics2DShapeQueryParameters::_bind_methods() { + + ObjectTypeDB::bind_method(_MD("set_shape","shape:Shape2D"),&Physics2DShapeQueryParameters::set_shape); + ObjectTypeDB::bind_method(_MD("set_shape_rid","shape"),&Physics2DShapeQueryParameters::set_shape_rid); + ObjectTypeDB::bind_method(_MD("get_shape_rid"),&Physics2DShapeQueryParameters::get_shape_rid); + + ObjectTypeDB::bind_method(_MD("set_transform","transform"),&Physics2DShapeQueryParameters::set_transform); + ObjectTypeDB::bind_method(_MD("get_transform"),&Physics2DShapeQueryParameters::get_transform); + + ObjectTypeDB::bind_method(_MD("set_motion","motion"),&Physics2DShapeQueryParameters::set_motion); + ObjectTypeDB::bind_method(_MD("get_motion"),&Physics2DShapeQueryParameters::get_motion); + + ObjectTypeDB::bind_method(_MD("set_margin","margin"),&Physics2DShapeQueryParameters::set_margin); + ObjectTypeDB::bind_method(_MD("get_margin"),&Physics2DShapeQueryParameters::get_margin); + + ObjectTypeDB::bind_method(_MD("set_layer_mask","layer_mask"),&Physics2DShapeQueryParameters::set_layer_mask); + ObjectTypeDB::bind_method(_MD("get_layer_mask"),&Physics2DShapeQueryParameters::get_layer_mask); + + ObjectTypeDB::bind_method(_MD("set_object_type_mask","object_type_mask"),&Physics2DShapeQueryParameters::set_object_type_mask); + ObjectTypeDB::bind_method(_MD("get_object_type_mask"),&Physics2DShapeQueryParameters::get_object_type_mask); + + ObjectTypeDB::bind_method(_MD("set_exclude","exclude"),&Physics2DShapeQueryParameters::set_exclude); + ObjectTypeDB::bind_method(_MD("get_exclude"),&Physics2DShapeQueryParameters::get_exclude); + + +} + +Physics2DShapeQueryParameters::Physics2DShapeQueryParameters() { + + margin=0; + layer_mask=0x7FFFFFFF; + object_type_mask=Physics2DDirectSpaceState::TYPE_MASK_COLLISION; +} + + +Dictionary Physics2DDirectSpaceState::_intersect_ray(const Vector2& p_from, const Vector2& p_to,const Vector<RID>& p_exclude,uint32_t p_layers,uint32_t p_object_type_mask) { RayResult inters; Set<RID> exclude; for(int i=0;i<p_exclude.size();i++) exclude.insert(p_exclude[i]); - bool res = intersect_ray(p_from,p_to,inters,exclude,p_user_mask); + bool res = intersect_ray(p_from,p_to,inters,exclude,p_layers,p_object_type_mask); if (!res) - return Variant(); + return Dictionary(true); Dictionary d(true); d["position"]=inters.position; @@ -133,59 +248,71 @@ Variant Physics2DDirectSpaceState::_intersect_ray(const Vector2& p_from, const V return d; } -Variant Physics2DDirectSpaceState::_intersect_shape(const RID& p_shape, const Matrix32& p_xform,int p_result_max,const Vector<RID>& p_exclude,uint32_t p_user_mask) { - - ERR_FAIL_INDEX_V(p_result_max,4096,Variant()); - if (p_result_max<=0) - return Variant(); - - Set<RID> exclude; - for(int i=0;i<p_exclude.size();i++) - exclude.insert(p_exclude[i]); - - ShapeResult *res=(ShapeResult*)alloca(p_result_max*sizeof(ShapeResult)); - - int rc = intersect_shape(p_shape,p_xform,Vector2(),0,res,p_result_max,exclude,p_user_mask); +Array Physics2DDirectSpaceState::_intersect_shape(const Ref<Physics2DShapeQueryParameters> &psq, int p_max_results) { - if (rc==0) - return Variant(); + Vector<ShapeResult> sr; + sr.resize(p_max_results); + int rc = intersect_shape(psq->shape,psq->transform,psq->motion,psq->margin,sr.ptr(),sr.size(),psq->exclude,psq->layer_mask,psq->object_type_mask); + Array ret; + ret.resize(rc); + for(int i=0;i<rc;i++) { - Ref<Physics2DShapeQueryResult> result = memnew( Physics2DShapeQueryResult ); - result->result.resize(rc); - for(int i=0;i<rc;i++) - result->result[i]=res[i]; - - return result; + Dictionary d; + d["rid"]=sr[i].rid; + d["collider_id"]=sr[i].collider_id; + d["collider"]=sr[i].collider; + d["shape"]=sr[i].shape; + ret[i]=d; + } + return ret; } +Array Physics2DDirectSpaceState::_cast_motion(const Ref<Physics2DShapeQueryParameters> &psq){ -Variant Physics2DDirectSpaceState::_cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,const Vector<RID>& p_exclude,uint32_t p_user_mask) { - + float closest_safe,closest_unsafe; + bool res = cast_motion(psq->shape,psq->transform,psq->motion,psq->margin,closest_safe,closest_unsafe,psq->exclude,psq->layer_mask,psq->object_type_mask); + if (!res) + return Array(); + Array ret(true); + ret.resize(2); + ret[0]=closest_safe; + ret[0]=closest_unsafe; + return ret; -#if 0 - Set<RID> exclude; - for(int i=0;i<p_exclude.size();i++) - exclude.insert(p_exclude[i]); +} +Array Physics2DDirectSpaceState::_collide_shape(const Ref<Physics2DShapeQueryParameters> &psq, int p_max_results){ + Vector<Vector2> ret; + ret.resize(p_max_results*2); + int rc=0; + bool res = collide_shape(psq->shape,psq->transform,psq->motion,psq->margin,ret.ptr(),p_max_results,rc,psq->exclude,psq->layer_mask,psq->object_type_mask); + if (!res) + return Array(); + Array r; + r.resize(rc*2); + for(int i=0;i<rc*2;i++) + r[i]=ret[i]; + return r; - bool result = cast_motion(p_shape,p_xform,p_motion,0,mcc,exclude,p_user_mask); +} +Dictionary Physics2DDirectSpaceState::_get_rest_info(const Ref<Physics2DShapeQueryParameters> &psq){ - if (!result) - return Variant(); + ShapeRestInfo sri; - Dictionary d(true); - d["point"]=mcc.point; - d["normal"]=mcc.normal; - d["rid"]=mcc.rid; - d["collider_id"]=mcc.collider_id; - d["collider"]=mcc.collider; - d["shape"]=mcc.shape; + bool res = rest_info(psq->shape,psq->transform,psq->motion,psq->margin,&sri,psq->exclude,psq->layer_mask,psq->object_type_mask); + Dictionary r(true); + if (!res) + return r; - return d; -#endif - return Variant(); + r["point"]=sri.point; + r["normal"]=sri.normal; + r["rid"]=sri.rid; + r["collider_id"]=sri.collider_id; + r["shape"]=sri.shape; + r["linear_velocity"]=sri.linear_velocity; + return r; } @@ -200,9 +327,19 @@ Physics2DDirectSpaceState::Physics2DDirectSpaceState() { void Physics2DDirectSpaceState::_bind_methods() { - ObjectTypeDB::bind_method(_MD("intersect_ray:Dictionary","from","to","exclude","umask"),&Physics2DDirectSpaceState::_intersect_ray,DEFVAL(Array()),DEFVAL(0)); - ObjectTypeDB::bind_method(_MD("intersect_shape:Physics2DShapeQueryResult","shape","xform","result_max","exclude","umask"),&Physics2DDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0)); - ObjectTypeDB::bind_method(_MD("cast_motion","shape","xform","motion","exclude","umask"),&Physics2DDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0)); + ObjectTypeDB::bind_method(_MD("intersect_ray:Dictionary","from","to","exclude","layer_mask","type_mask"),&Physics2DDirectSpaceState::_intersect_ray,DEFVAL(Array()),DEFVAL(0x7FFFFFFF),DEFVAL(TYPE_MASK_COLLISION)); + ObjectTypeDB::bind_method(_MD("intersect_shape","shape:Physics2DShapeQueryParameters","max_results"),&Physics2DDirectSpaceState::_intersect_shape,DEFVAL(32)); + ObjectTypeDB::bind_method(_MD("cast_motion","shape:Physics2DShapeQueryParameters"),&Physics2DDirectSpaceState::_cast_motion); + ObjectTypeDB::bind_method(_MD("collide_shape","shape:Physics2DShapeQueryParameters","max_results"),&Physics2DDirectSpaceState::_collide_shape,DEFVAL(32)); + ObjectTypeDB::bind_method(_MD("get_rest_info","shape:Physics2DShapeQueryParameters"),&Physics2DDirectSpaceState::_get_rest_info); + //ObjectTypeDB::bind_method(_MD("cast_motion","shape","xform","motion","exclude","umask"),&Physics2DDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0)); + + BIND_CONSTANT( TYPE_MASK_STATIC_BODY ); + BIND_CONSTANT( TYPE_MASK_KINEMATIC_BODY ); + BIND_CONSTANT( TYPE_MASK_RIGID_BODY ); + BIND_CONSTANT( TYPE_MASK_CHARACTER_BODY ); + BIND_CONSTANT( TYPE_MASK_AREA ); + BIND_CONSTANT( TYPE_MASK_COLLISION ); } @@ -375,6 +512,8 @@ void Physics2DServer::_bind_methods() { ObjectTypeDB::bind_method(_MD("set_active","active"),&Physics2DServer::set_active); + ObjectTypeDB::bind_method(_MD("get_process_info"),&Physics2DServer::get_process_info); + // ObjectTypeDB::bind_method(_MD("init"),&Physics2DServer::init); // ObjectTypeDB::bind_method(_MD("step"),&Physics2DServer::step); // ObjectTypeDB::bind_method(_MD("sync"),&Physics2DServer::sync); @@ -434,6 +573,10 @@ void Physics2DServer::_bind_methods() { BIND_CONSTANT( AREA_BODY_ADDED ); BIND_CONSTANT( AREA_BODY_REMOVED ); + BIND_CONSTANT( INFO_ACTIVE_OBJECTS ); + BIND_CONSTANT( INFO_COLLISION_PAIRS ); + BIND_CONSTANT( INFO_ISLAND_COUNT ); + } diff --git a/servers/physics_2d_server.h b/servers/physics_2d_server.h index def1e69992..17a21e46a3 100644 --- a/servers/physics_2d_server.h +++ b/servers/physics_2d_server.h @@ -31,6 +31,7 @@ #include "object.h" #include "reference.h" +#include "resource.h" class Physics2DDirectSpaceState; @@ -84,14 +85,60 @@ public: class Physics2DShapeQueryResult; +//used for script +class Physics2DShapeQueryParameters : public Reference { + + OBJ_TYPE(Physics2DShapeQueryParameters, Reference); +friend class Physics2DDirectSpaceState; + RID shape; + Matrix32 transform; + Vector2 motion; + float margin; + Set<RID> exclude; + uint32_t layer_mask; + uint32_t object_type_mask; +protected: + static void _bind_methods(); +public: + + + void set_shape(const RES& p_shape); + void set_shape_rid(const RID& p_shape); + RID get_shape_rid() const; + + void set_transform(const Matrix32& p_transform); + Matrix32 get_transform() const; + + void set_motion(const Vector2& p_motion); + Vector2 get_motion() const; + + void set_margin(float p_margin); + float get_margin() const; + + void set_layer_mask(int p_layer_mask); + int get_layer_mask() const; + + void set_object_type_mask(int p_object_type_mask); + int get_object_type_mask() const; + + void set_exclude(const Vector<RID>& p_exclude); + Vector<RID> get_exclude() const; + + Physics2DShapeQueryParameters(); + +}; + + class Physics2DDirectSpaceState : public Object { OBJ_TYPE( Physics2DDirectSpaceState, Object ); - Variant _intersect_ray(const Vector2& p_from, const Vector2& p_to,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_layers=0); - Variant _intersect_shape(const RID& p_shape, const Matrix32& p_xform,int p_result_max=64,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_layers=0); - Variant _cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_layers=0); + Dictionary _intersect_ray(const Vector2& p_from, const Vector2& p_to,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_layers=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION); + Array _intersect_shape(const Ref<Physics2DShapeQueryParameters> &p_shape_query,int p_max_results=32); + Array _cast_motion(const Ref<Physics2DShapeQueryParameters> &p_shape_query); + Array _collide_shape(const Ref<Physics2DShapeQueryParameters> &p_shape_query,int p_max_results=32); + Dictionary _get_rest_info(const Ref<Physics2DShapeQueryParameters> &p_shape_query); protected: static void _bind_methods(); @@ -131,8 +178,6 @@ public: virtual int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0; - - virtual bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0; virtual bool collide_shape(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,Vector2 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0; @@ -448,6 +493,15 @@ public: virtual void flush_queries()=0; virtual void finish()=0; + enum ProcessInfo { + + INFO_ACTIVE_OBJECTS, + INFO_COLLISION_PAIRS, + INFO_ISLAND_COUNT + }; + + virtual int get_process_info(ProcessInfo p_info)=0; + Physics2DServer(); ~Physics2DServer(); }; @@ -465,5 +519,6 @@ VARIANT_ENUM_CAST( Physics2DServer::JointType ); VARIANT_ENUM_CAST( Physics2DServer::DampedStringParam ); //VARIANT_ENUM_CAST( Physics2DServer::ObjectType ); VARIANT_ENUM_CAST( Physics2DServer::AreaBodyStatus ); +VARIANT_ENUM_CAST( Physics2DServer::ProcessInfo ); #endif diff --git a/servers/physics_server.cpp b/servers/physics_server.cpp index 070bc5e062..e2dd3e14eb 100644 --- a/servers/physics_server.cpp +++ b/servers/physics_server.cpp @@ -113,6 +113,112 @@ PhysicsDirectBodyState::PhysicsDirectBodyState() {} +void PhysicsShapeQueryParameters::set_shape(const RES &p_shape) { + + ERR_FAIL_COND(p_shape.is_null()); + shape=p_shape->get_rid(); +} + +void PhysicsShapeQueryParameters::set_shape_rid(const RID& p_shape) { + + shape=p_shape; +} + +RID PhysicsShapeQueryParameters::get_shape_rid() const { + + return shape; +} + +void PhysicsShapeQueryParameters::set_transform(const Matrix32& p_transform){ + + transform=p_transform; +} +Matrix32 PhysicsShapeQueryParameters::get_transform() const{ + + return transform; +} + +void PhysicsShapeQueryParameters::set_margin(float p_margin){ + + margin=p_margin; +} + +float PhysicsShapeQueryParameters::get_margin() const{ + + return margin; +} + +void PhysicsShapeQueryParameters::set_layer_mask(int p_layer_mask){ + + layer_mask=p_layer_mask; +} +int PhysicsShapeQueryParameters::get_layer_mask() const{ + + return layer_mask; +} + + +void PhysicsShapeQueryParameters::set_object_type_mask(int p_object_type_mask){ + + object_type_mask=p_object_type_mask; +} +int PhysicsShapeQueryParameters::get_object_type_mask() const{ + + return object_type_mask; +} +void PhysicsShapeQueryParameters::set_exclude(const Vector<RID>& p_exclude) { + + exclude.clear();; + for(int i=0;i<p_exclude.size();i++) + exclude.insert(p_exclude[i]); + +} + +Vector<RID> PhysicsShapeQueryParameters::get_exclude() const{ + + Vector<RID> ret; + ret.resize(exclude.size()); + int idx=0; + for(Set<RID>::Element *E=exclude.front();E;E=E->next()) { + ret[idx]=E->get(); + } + return ret; +} + +void PhysicsShapeQueryParameters::_bind_methods() { + + ObjectTypeDB::bind_method(_MD("set_shape","shape:Shape"),&PhysicsShapeQueryParameters::set_shape); + ObjectTypeDB::bind_method(_MD("set_shape_rid","shape"),&PhysicsShapeQueryParameters::set_shape_rid); + ObjectTypeDB::bind_method(_MD("get_shape_rid"),&PhysicsShapeQueryParameters::get_shape_rid); + + ObjectTypeDB::bind_method(_MD("set_transform","transform"),&PhysicsShapeQueryParameters::set_transform); + ObjectTypeDB::bind_method(_MD("get_transform"),&PhysicsShapeQueryParameters::get_transform); + + ObjectTypeDB::bind_method(_MD("set_margin","margin"),&PhysicsShapeQueryParameters::set_margin); + ObjectTypeDB::bind_method(_MD("get_margin"),&PhysicsShapeQueryParameters::get_margin); + + ObjectTypeDB::bind_method(_MD("set_layer_mask","layer_mask"),&PhysicsShapeQueryParameters::set_layer_mask); + ObjectTypeDB::bind_method(_MD("get_layer_mask"),&PhysicsShapeQueryParameters::get_layer_mask); + + ObjectTypeDB::bind_method(_MD("set_object_type_mask","object_type_mask"),&PhysicsShapeQueryParameters::set_object_type_mask); + ObjectTypeDB::bind_method(_MD("get_object_type_mask"),&PhysicsShapeQueryParameters::get_object_type_mask); + + ObjectTypeDB::bind_method(_MD("set_exclude","exclude"),&PhysicsShapeQueryParameters::set_exclude); + ObjectTypeDB::bind_method(_MD("get_exclude"),&PhysicsShapeQueryParameters::get_exclude); + + +} + +PhysicsShapeQueryParameters::PhysicsShapeQueryParameters() { + + margin=0; + layer_mask=0x7FFFFFFF; + object_type_mask=PhysicsDirectSpaceState::TYPE_MASK_COLLISION; +} + + + +///////////////////////////////////// Variant PhysicsDirectSpaceState::_intersect_ray(const Vector3& p_from, const Vector3& p_to,const Vector<RID>& p_exclude,uint32_t p_user_mask) { RayResult inters; @@ -150,7 +256,7 @@ Variant PhysicsDirectSpaceState::_intersect_shape(const RID& p_shape, const Tran ShapeResult *res=(ShapeResult*)alloca(p_result_max*sizeof(ShapeResult)); - int rc = intersect_shape(p_shape,p_xform,res,p_result_max,exclude,p_user_mask); + int rc = intersect_shape(p_shape,p_xform,0,res,p_result_max,exclude); if (rc==0) return Variant(); @@ -308,8 +414,6 @@ void PhysicsServer::_bind_methods() { ObjectTypeDB::bind_method(_MD("body_set_param","body","param","value"),&PhysicsServer::body_set_param); ObjectTypeDB::bind_method(_MD("body_get_param","body","param"),&PhysicsServer::body_get_param); - ObjectTypeDB::bind_method(_MD("body_static_simulate_motion","body","new_xform"),&PhysicsServer::body_static_simulate_motion); - ObjectTypeDB::bind_method(_MD("body_set_state","body","state","value"),&PhysicsServer::body_set_state); ObjectTypeDB::bind_method(_MD("body_get_state","body","state"),&PhysicsServer::body_get_state); @@ -355,6 +459,8 @@ void PhysicsServer::_bind_methods() { //ObjectTypeDB::bind_method(_MD("flush_queries"),&PhysicsServer::flush_queries); + ObjectTypeDB::bind_method(_MD("get_process_info"),&PhysicsServer::get_process_info); + BIND_CONSTANT( SHAPE_PLANE ); BIND_CONSTANT( SHAPE_RAY ); BIND_CONSTANT( SHAPE_SPHERE ); @@ -407,6 +513,11 @@ void PhysicsServer::_bind_methods() { BIND_CONSTANT( AREA_BODY_ADDED ); BIND_CONSTANT( AREA_BODY_REMOVED ); + BIND_CONSTANT( INFO_ACTIVE_OBJECTS ); + BIND_CONSTANT( INFO_COLLISION_PAIRS ); + BIND_CONSTANT( INFO_ISLAND_COUNT ); + + } diff --git a/servers/physics_server.h b/servers/physics_server.h index 955caffe5b..5709974cc0 100644 --- a/servers/physics_server.h +++ b/servers/physics_server.h @@ -30,7 +30,7 @@ #define PHYSICS_SERVER_H #include "object.h" -#include "reference.h" +#include "resource.h" class PhysicsDirectSpaceState; @@ -87,6 +87,45 @@ public: class PhysicsShapeQueryResult; +class PhysicsShapeQueryParameters : public Reference { + + OBJ_TYPE(PhysicsShapeQueryParameters, Reference); +friend class PhysicsDirectSpaceState; + RID shape; + Matrix32 transform; + float margin; + Set<RID> exclude; + uint32_t layer_mask; + uint32_t object_type_mask; +protected: + static void _bind_methods(); +public: + + + void set_shape(const RES& p_shape); + void set_shape_rid(const RID& p_shape); + RID get_shape_rid() const; + + void set_transform(const Matrix32& p_transform); + Matrix32 get_transform() const; + + void set_margin(float p_margin); + float get_margin() const; + + void set_layer_mask(int p_layer_mask); + int get_layer_mask() const; + + void set_object_type_mask(int p_object_type_mask); + int get_object_type_mask() const; + + void set_exclude(const Vector<RID>& p_exclude); + Vector<RID> get_exclude() const; + + PhysicsShapeQueryParameters(); + +}; + + class PhysicsDirectSpaceState : public Object { @@ -101,6 +140,15 @@ protected: public: + enum ObjectTypeMask { + TYPE_MASK_STATIC_BODY=1<<0, + TYPE_MASK_KINEMATIC_BODY=1<<1, + TYPE_MASK_RIGID_BODY=1<<2, + TYPE_MASK_CHARACTER_BODY=1<<3, + TYPE_MASK_AREA=1<<4, + TYPE_MASK_COLLISION=TYPE_MASK_STATIC_BODY|TYPE_MASK_CHARACTER_BODY|TYPE_MASK_KINEMATIC_BODY|TYPE_MASK_RIGID_BODY + }; + struct RayResult { Vector3 position; @@ -111,7 +159,7 @@ public: int shape; }; - 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_user_mask=0)=0; + 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_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0; struct ShapeResult { @@ -122,7 +170,25 @@ public: }; - virtual int intersect_shape(const RID& p_shape, const Transform& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0)=0; + virtual int intersect_shape(const RID& p_shape, const Transform& p_xform,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0; + + struct ShapeRestInfo { + + Vector3 point; + Vector3 normal; + RID rid; + ObjectID collider_id; + int shape; + Vector3 linear_velocity; //velocity at contact point + + }; + + virtual bool cast_motion(const RID& p_shape, const Transform& p_xform,const Vector3& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION,ShapeRestInfo *r_info=NULL)=0; + + virtual bool collide_shape(RID p_shape, const Transform& p_shape_xform,float p_margin,Vector3 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0; + + virtual bool rest_info(RID p_shape, const Transform& p_shape_xform,float p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0; + PhysicsDirectSpaceState(); }; @@ -303,6 +369,9 @@ public: virtual void body_set_enable_continuous_collision_detection(RID p_body,bool p_enable)=0; virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const=0; + virtual void body_set_layer_mask(RID p_body, uint32_t p_mask)=0; + virtual uint32_t body_get_layer_mask(RID p_body, uint32_t p_mask) const=0; + virtual void body_set_user_flags(RID p_body, uint32_t p_flags)=0; virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const=0; @@ -317,8 +386,6 @@ public: virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value)=0; virtual float body_get_param(RID p_body, BodyParameter p_param) const=0; - //advanced simulation - virtual void body_static_simulate_motion(RID p_body,const Transform& p_new_transform)=0; //state enum BodyState { @@ -420,6 +487,15 @@ public: virtual void flush_queries()=0; virtual void finish()=0; + enum ProcessInfo { + + INFO_ACTIVE_OBJECTS, + INFO_COLLISION_PAIRS, + INFO_ISLAND_COUNT + }; + + virtual int get_process_info(ProcessInfo p_info)=0; + PhysicsServer(); ~PhysicsServer(); }; @@ -437,5 +513,6 @@ VARIANT_ENUM_CAST( PhysicsServer::BodyAxisLock ); //VARIANT_ENUM_CAST( PhysicsServer::DampedStringParam ); //VARIANT_ENUM_CAST( PhysicsServer::ObjectType ); VARIANT_ENUM_CAST( PhysicsServer::AreaBodyStatus ); +VARIANT_ENUM_CAST( PhysicsServer::ProcessInfo ); #endif diff --git a/servers/register_server_types.cpp b/servers/register_server_types.cpp index 638156d813..f8d38d15c0 100644 --- a/servers/register_server_types.cpp +++ b/servers/register_server_types.cpp @@ -55,6 +55,7 @@ void register_server_types() { ObjectTypeDB::register_virtual_type<Physics2DDirectBodyState>(); ObjectTypeDB::register_virtual_type<Physics2DDirectSpaceState>(); ObjectTypeDB::register_virtual_type<Physics2DShapeQueryResult>(); + ObjectTypeDB::register_type<Physics2DShapeQueryParameters>(); ObjectTypeDB::register_virtual_type<PhysicsDirectBodyState>(); ObjectTypeDB::register_virtual_type<PhysicsDirectSpaceState>(); |