diff options
Diffstat (limited to 'servers/physics')
36 files changed, 5142 insertions, 621 deletions
diff --git a/servers/physics/SCsub b/servers/physics/SCsub index 16fe3a59ac..3b84c5ef18 100644 --- a/servers/physics/SCsub +++ b/servers/physics/SCsub @@ -4,4 +4,6 @@ env.add_source_files(env.servers_sources,"*.cpp") Export('env') +SConscript("joints/SCsub") + diff --git a/servers/physics/area_sw.cpp b/servers/physics/area_sw.cpp index 33962d993a..8192a98400 100644 --- a/servers/physics/area_sw.cpp +++ b/servers/physics/area_sw.cpp @@ -43,6 +43,7 @@ void AreaSW::set_transform(const Transform& p_transform) { get_space()->area_add_to_moved_list(&moved_list); _set_transform(p_transform); + _set_inv_transform(p_transform.affine_inverse()); } void AreaSW::set_space(SpaceSW *p_space) { @@ -181,6 +182,7 @@ AreaSW::AreaSW() : CollisionObjectSW(TYPE_AREA), monitor_query_list(this), move point_attenuation=1; density=0.1; priority=0; + ray_pickable=false; } diff --git a/servers/physics/area_sw.h b/servers/physics/area_sw.h index 3e39dc3bb6..a864055d17 100644 --- a/servers/physics/area_sw.h +++ b/servers/physics/area_sw.h @@ -48,6 +48,7 @@ class AreaSW : public CollisionObjectSW{ float point_attenuation; float density; int priority; + bool ray_pickable; ObjectID monitor_callback_id; StringName monitor_callback_method; @@ -70,7 +71,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; @@ -138,6 +139,11 @@ public: _FORCE_INLINE_ void remove_constraint( ConstraintSW* p_constraint) { constraints.erase(p_constraint); } _FORCE_INLINE_ const Set<ConstraintSW*>& get_constraints() const { return constraints; } + _FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable=p_enable; } + _FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; } + + + void set_transform(const Transform& p_transform); void set_space(SpaceSW *p_space); diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp index c50bfac472..d112afa8e2 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++) { @@ -296,6 +298,17 @@ bool BodyPairSW::setup(float p_step) { A->apply_bias_impulse( c.rA, -jb_vec ); B->apply_bias_impulse( c.rB, jb_vec ); + c.bounce = MAX(A->get_bounce(),B->get_bounce()); + if (c.bounce) { + + Vector3 crA = A->get_angular_velocity().cross( c.rA ); + Vector3 crB = B->get_angular_velocity().cross( c.rB ); + Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; + //normal impule + c.bounce = c.bounce * dv.dot(c.normal); + } + + } return true; @@ -348,8 +361,7 @@ void BodyPairSW::solve(float p_step) { if (Math::abs(vn)>MIN_VELOCITY) { - real_t bounce=0; - real_t jn = (-bounce -vn)*c.mass_normal; + real_t jn = -(c.bounce + vn)*c.mass_normal; real_t jnOld = c.acc_normal_impulse; c.acc_normal_impulse = MAX(jnOld + jn, 0.0f); diff --git a/servers/physics/body_pair_sw.h b/servers/physics/body_pair_sw.h index ad66227b36..937c295c63 100644 --- a/servers/physics/body_pair_sw.h +++ b/servers/physics/body_pair_sw.h @@ -61,6 +61,7 @@ class BodyPairSW : public ConstraintSW { real_t acc_bias_impulse; // accumulated normal impulse for position bias (Pnb) real_t mass_normal; real_t bias; + real_t bounce; real_t depth; bool active; 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..6317186d5f 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,29 @@ 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); + _FORCE_INLINE_ Vector3 get_velocity_in_local_point(const Vector3& rel_pos) const { + + return linear_velocity + angular_velocity.cross(rel_pos); + } + + _FORCE_INLINE_ real_t compute_impulse_denominator(const Vector3& p_pos, const Vector3& p_normal) const { + + Vector3 r0 = p_pos - get_transform().origin; + + Vector3 c0 = (r0).cross(p_normal); + + Vector3 vec = (_inv_inertia_tensor.xform_inv(c0)).cross(r0); + + return _inv_mass + p_normal.dot(vec); + + } + + _FORCE_INLINE_ real_t compute_angular_impulse_denominator(const Vector3& p_axis) const { + + return p_axis.dot( _inv_inertia_tensor.xform_inv(p_axis) ); + } + + //void simulate_motion(const Transform& p_xform,real_t p_step); void call_queries(); void wakeup_neighbours(); 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/joints/SCsub b/servers/physics/joints/SCsub new file mode 100644 index 0000000000..97d6edea21 --- /dev/null +++ b/servers/physics/joints/SCsub @@ -0,0 +1,8 @@ +Import('env') + +env.add_source_files(env.servers_sources,"*.cpp") + +Export('env') + + + diff --git a/servers/physics/joints/cone_twist_joint_sw.cpp b/servers/physics/joints/cone_twist_joint_sw.cpp new file mode 100644 index 0000000000..d97d8c599f --- /dev/null +++ b/servers/physics/joints/cone_twist_joint_sw.cpp @@ -0,0 +1,340 @@ +#include "cone_twist_joint_sw.h" + +static void plane_space(const Vector3& n, Vector3& p, Vector3& q) { + + if (Math::abs(n.z) > 0.707106781186547524400844362) { + // choose p in y-z plane + real_t a = n[1]*n[1] + n[2]*n[2]; + real_t k = 1.0/Math::sqrt(a); + p=Vector3(0,-n[2]*k,n[1]*k); + // set q = n x p + q=Vector3(a*k,-n[0]*p[2],n[0]*p[1]); + } + else { + // choose p in x-y plane + real_t a = n.x*n.x + n.y*n.y; + real_t k = 1.0/Math::sqrt(a); + p=Vector3(-n.y*k,n.x*k,0); + // set q = n x p + q=Vector3(-n.z*p.y,n.z*p.x,a*k); + } +} + + +static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) +{ + real_t coeff_1 = Math_PI / 4.0f; + real_t coeff_2 = 3.0f * coeff_1; + real_t abs_y = Math::abs(y); + real_t angle; + if (x >= 0.0f) { + real_t r = (x - abs_y) / (x + abs_y); + angle = coeff_1 - coeff_1 * r; + } else { + real_t r = (x + abs_y) / (abs_y - x); + angle = coeff_2 - coeff_1 * r; + } + return (y < 0.0f) ? -angle : angle; +} + +ConeTwistJointSW::ConeTwistJointSW(BodySW* rbA,BodySW* rbB,const Transform& rbAFrame, const Transform& rbBFrame) : JointSW(_arr,2) { + + A=rbA; + B=rbB; + + + m_rbAFrame=rbAFrame; + m_rbBFrame=rbBFrame; + + m_swingSpan1 = Math_PI/4.0; + m_swingSpan2 = Math_PI/4.0; + m_twistSpan = Math_PI*2; + m_biasFactor = 0.3f; + m_relaxationFactor = 1.0f; + + m_solveTwistLimit = false; + m_solveSwingLimit = false; + + A->add_constraint(this,0); + B->add_constraint(this,1); + + m_appliedImpulse=0; +} + + +bool ConeTwistJointSW::setup(float p_step) { + m_appliedImpulse = real_t(0.); + + //set bias, sign, clear accumulator + m_swingCorrection = real_t(0.); + m_twistLimitSign = real_t(0.); + m_solveTwistLimit = false; + m_solveSwingLimit = false; + m_accTwistLimitImpulse = real_t(0.); + m_accSwingLimitImpulse = real_t(0.); + + if (!m_angularOnly) + { + Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); + Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); + Vector3 relPos = pivotBInW - pivotAInW; + + Vector3 normal[3]; + if (relPos.length_squared() > CMP_EPSILON) + { + normal[0] = relPos.normalized(); + } + else + { + normal[0]=Vector3(real_t(1.0),0,0); + } + + plane_space(normal[0], normal[1], normal[2]); + + for (int i=0;i<3;i++) + { + memnew_placement(&m_jac[i], JacobianEntrySW( + A->get_transform().basis.transposed(), + B->get_transform().basis.transposed(), + pivotAInW - A->get_transform().origin, + pivotBInW - B->get_transform().origin, + normal[i], + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass())); + } + } + + Vector3 b1Axis1,b1Axis2,b1Axis3; + Vector3 b2Axis1,b2Axis2; + + b1Axis1 = A->get_transform().basis.xform( this->m_rbAFrame.basis.get_axis(0) ); + b2Axis1 = B->get_transform().basis.xform( this->m_rbBFrame.basis.get_axis(0) ); + + real_t swing1=real_t(0.),swing2 = real_t(0.); + + real_t swx=real_t(0.),swy = real_t(0.); + real_t thresh = real_t(10.); + real_t fact; + + // Get Frame into world space + if (m_swingSpan1 >= real_t(0.05f)) + { + b1Axis2 = A->get_transform().basis.xform( this->m_rbAFrame.basis.get_axis(1) ); +// swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) ); + swx = b2Axis1.dot(b1Axis1); + swy = b2Axis1.dot(b1Axis2); + swing1 = atan2fast(swy, swx); + fact = (swy*swy + swx*swx) * thresh * thresh; + fact = fact / (fact + real_t(1.0)); + swing1 *= fact; + + } + + if (m_swingSpan2 >= real_t(0.05f)) + { + b1Axis3 = A->get_transform().basis.xform( this->m_rbAFrame.basis.get_axis(2) ); +// swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) ); + swx = b2Axis1.dot(b1Axis1); + swy = b2Axis1.dot(b1Axis3); + swing2 = atan2fast(swy, swx); + fact = (swy*swy + swx*swx) * thresh * thresh; + fact = fact / (fact + real_t(1.0)); + swing2 *= fact; + } + + real_t RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1); + real_t RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2); + real_t EllipseAngle = Math::abs(swing1*swing1)* RMaxAngle1Sq + Math::abs(swing2*swing2) * RMaxAngle2Sq; + + if (EllipseAngle > 1.0f) + { + m_swingCorrection = EllipseAngle-1.0f; + m_solveSwingLimit = true; + + // Calculate necessary axis & factors + m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3)); + m_swingAxis.normalize(); + + real_t swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; + m_swingAxis *= swingAxisSign; + + m_kSwing = real_t(1.) / (A->compute_angular_impulse_denominator(m_swingAxis) + + B->compute_angular_impulse_denominator(m_swingAxis)); + + } + + // Twist limits + if (m_twistSpan >= real_t(0.)) + { + Vector3 b2Axis2 = B->get_transform().basis.xform( this->m_rbBFrame.basis.get_axis(1) ); + Quat rotationArc = Quat(b2Axis1,b1Axis1); + Vector3 TwistRef = rotationArc.xform(b2Axis2); + real_t twist = atan2fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) ); + + real_t lockedFreeFactor = (m_twistSpan > real_t(0.05f)) ? m_limitSoftness : real_t(0.); + if (twist <= -m_twistSpan*lockedFreeFactor) + { + m_twistCorrection = -(twist + m_twistSpan); + m_solveTwistLimit = true; + + m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; + m_twistAxis.normalize(); + m_twistAxis *= -1.0f; + + m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + + B->compute_angular_impulse_denominator(m_twistAxis)); + + } else + if (twist > m_twistSpan*lockedFreeFactor) + { + m_twistCorrection = (twist - m_twistSpan); + m_solveTwistLimit = true; + + m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; + m_twistAxis.normalize(); + + m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + + B->compute_angular_impulse_denominator(m_twistAxis)); + + } + } + + return true; +} + +void ConeTwistJointSW::solve(real_t timeStep) +{ + + Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); + Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); + + real_t tau = real_t(0.3); + + //linear part + if (!m_angularOnly) + { + Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; + Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; + + Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1); + Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2); + Vector3 vel = vel1 - vel2; + + for (int i=0;i<3;i++) + { + const Vector3& normal = m_jac[i].m_linearJointAxis; + real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal(); + + real_t rel_vel; + rel_vel = normal.dot(vel); + //positional error (zeroth order error) + real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + real_t impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; + m_appliedImpulse += impulse; + Vector3 impulse_vector = normal * impulse; + A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector); + B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector); + } + } + + { + ///solve angular part + const Vector3& angVelA = A->get_angular_velocity(); + const Vector3& angVelB = B->get_angular_velocity(); + + // solve swing limit + if (m_solveSwingLimit) + { + real_t amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(real_t(1.)/timeStep)*m_biasFactor); + real_t impulseMag = amplitude * m_kSwing; + + // Clamp the accumulated impulse + real_t temp = m_accSwingLimitImpulse; + m_accSwingLimitImpulse = MAX(m_accSwingLimitImpulse + impulseMag, real_t(0.0) ); + impulseMag = m_accSwingLimitImpulse - temp; + + Vector3 impulse = m_swingAxis * impulseMag; + + A->apply_torque_impulse(impulse); + B->apply_torque_impulse(-impulse); + + } + + // solve twist limit + if (m_solveTwistLimit) + { + real_t amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(real_t(1.)/timeStep)*m_biasFactor ); + real_t impulseMag = amplitude * m_kTwist; + + // Clamp the accumulated impulse + real_t temp = m_accTwistLimitImpulse; + m_accTwistLimitImpulse = MAX(m_accTwistLimitImpulse + impulseMag, real_t(0.0) ); + impulseMag = m_accTwistLimitImpulse - temp; + + Vector3 impulse = m_twistAxis * impulseMag; + + A->apply_torque_impulse(impulse); + B->apply_torque_impulse(-impulse); + + } + + } + +} + +void ConeTwistJointSW::set_param(PhysicsServer::ConeTwistJointParam p_param, float p_value) { + + switch(p_param) { + case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN: { + + m_swingSpan1=p_value; + m_swingSpan2=p_value; + } break; + case PhysicsServer::CONE_TWIST_JOINT_TWIST_SPAN: { + + m_twistSpan=p_value; + } break; + case PhysicsServer::CONE_TWIST_JOINT_BIAS: { + + m_biasFactor=p_value; + } break; + case PhysicsServer::CONE_TWIST_JOINT_SOFTNESS: { + + m_limitSoftness=p_value; + } break; + case PhysicsServer::CONE_TWIST_JOINT_RELAXATION: { + + m_relaxationFactor=p_value; + } break; + } +} + +float ConeTwistJointSW::get_param(PhysicsServer::ConeTwistJointParam p_param) const{ + + switch(p_param) { + case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN: { + + return m_swingSpan1; + } break; + case PhysicsServer::CONE_TWIST_JOINT_TWIST_SPAN: { + + return m_twistSpan; + } break; + case PhysicsServer::CONE_TWIST_JOINT_BIAS: { + + return m_biasFactor; + } break; + case PhysicsServer::CONE_TWIST_JOINT_SOFTNESS: { + + return m_limitSoftness; + } break; + case PhysicsServer::CONE_TWIST_JOINT_RELAXATION: { + + return m_relaxationFactor; + } break; + } + + return 0; +} diff --git a/servers/physics/joints/cone_twist_joint_sw.h b/servers/physics/joints/cone_twist_joint_sw.h new file mode 100644 index 0000000000..63502d2036 --- /dev/null +++ b/servers/physics/joints/cone_twist_joint_sw.h @@ -0,0 +1,125 @@ +#ifndef CONE_TWIST_JOINT_SW_H +#define CONE_TWIST_JOINT_SW_H + +#include "servers/physics/joints_sw.h" +#include "servers/physics/joints/jacobian_entry_sw.h" + + +/* +Bullet Continuous Collision Detection and Physics Library +ConeTwistJointSW is Copyright (c) 2007 Starbreeze Studios + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. + +Written by: Marcus Hennix +*/ + + + +///ConeTwistJointSW can be used to simulate ragdoll joints (upper arm, leg etc) +class ConeTwistJointSW : public JointSW +{ +#ifdef IN_PARALLELL_SOLVER +public: +#endif + + union { + struct { + BodySW *A; + BodySW *B; + }; + + BodySW *_arr[2]; + }; + + JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints + + + real_t m_appliedImpulse; + Transform m_rbAFrame; + Transform m_rbBFrame; + + real_t m_limitSoftness; + real_t m_biasFactor; + real_t m_relaxationFactor; + + real_t m_swingSpan1; + real_t m_swingSpan2; + real_t m_twistSpan; + + Vector3 m_swingAxis; + Vector3 m_twistAxis; + + real_t m_kSwing; + real_t m_kTwist; + + real_t m_twistLimitSign; + real_t m_swingCorrection; + real_t m_twistCorrection; + + real_t m_accSwingLimitImpulse; + real_t m_accTwistLimitImpulse; + + bool m_angularOnly; + bool m_solveTwistLimit; + bool m_solveSwingLimit; + + +public: + + virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_CONE_TWIST; } + + virtual bool setup(float p_step); + virtual void solve(float p_step); + + ConeTwistJointSW(BodySW* rbA,BodySW* rbB,const Transform& rbAFrame, const Transform& rbBFrame); + + + void setAngularOnly(bool angularOnly) + { + m_angularOnly = angularOnly; + } + + void setLimit(real_t _swingSpan1,real_t _swingSpan2,real_t _twistSpan, real_t _softness = 0.8f, real_t _biasFactor = 0.3f, real_t _relaxationFactor = 1.0f) + { + m_swingSpan1 = _swingSpan1; + m_swingSpan2 = _swingSpan2; + m_twistSpan = _twistSpan; + + m_limitSoftness = _softness; + m_biasFactor = _biasFactor; + m_relaxationFactor = _relaxationFactor; + } + + inline int getSolveTwistLimit() + { + return m_solveTwistLimit; + } + + inline int getSolveSwingLimit() + { + return m_solveTwistLimit; + } + + inline real_t getTwistLimitSign() + { + return m_twistLimitSign; + } + + void set_param(PhysicsServer::ConeTwistJointParam p_param, float p_value); + float get_param(PhysicsServer::ConeTwistJointParam p_param) const; + + +}; + + + +#endif // CONE_TWIST_JOINT_SW_H diff --git a/servers/physics/joints/generic_6dof_joint_sw.cpp b/servers/physics/joints/generic_6dof_joint_sw.cpp new file mode 100644 index 0000000000..3d569df2c9 --- /dev/null +++ b/servers/physics/joints/generic_6dof_joint_sw.cpp @@ -0,0 +1,691 @@ +#include "generic_6dof_joint_sw.h" + + + +#define GENERIC_D6_DISABLE_WARMSTARTING 1 + +real_t btGetMatrixElem(const Matrix3& mat, int index); +real_t btGetMatrixElem(const Matrix3& mat, int index) +{ + int i = index%3; + int j = index/3; + return mat[i][j]; +} + +///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html +bool matrixToEulerXYZ(const Matrix3& mat,Vector3& xyz); +bool matrixToEulerXYZ(const Matrix3& mat,Vector3& xyz) +{ +// // rot = cy*cz -cy*sz sy +// // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx +// // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy +// + + if (btGetMatrixElem(mat,2) < real_t(1.0)) + { + if (btGetMatrixElem(mat,2) > real_t(-1.0)) + { + xyz[0] = Math::atan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8)); + xyz[1] = Math::asin(btGetMatrixElem(mat,2)); + xyz[2] = Math::atan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); + return true; + } + else + { + // WARNING. Not unique. XA - ZA = -atan2(r10,r11) + xyz[0] = -Math::atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + xyz[1] = -Math_PI*0.5; + xyz[2] = real_t(0.0); + return false; + } + } + else + { + // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) + xyz[0] = Math::atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + xyz[1] = Math_PI*0.5; + xyz[2] = 0.0; + + } + + + return false; +} + + + +//////////////////////////// G6DOFRotationalLimitMotorSW //////////////////////////////////// + + +int G6DOFRotationalLimitMotorSW::testLimitValue(real_t test_value) +{ + if(m_loLimit>m_hiLimit) + { + m_currentLimit = 0;//Free from violation + return 0; + } + + if (test_value < m_loLimit) + { + m_currentLimit = 1;//low limit violation + m_currentLimitError = test_value - m_loLimit; + return 1; + } + else if (test_value> m_hiLimit) + { + m_currentLimit = 2;//High limit violation + m_currentLimitError = test_value - m_hiLimit; + return 2; + }; + + m_currentLimit = 0;//Free from violation + return 0; + +} + + +real_t G6DOFRotationalLimitMotorSW::solveAngularLimits( + real_t timeStep,Vector3& axis,real_t jacDiagABInv, + BodySW * body0, BodySW * body1) +{ + if (needApplyTorques()==false) return 0.0f; + + real_t target_velocity = m_targetVelocity; + real_t maxMotorForce = m_maxMotorForce; + + //current error correction + if (m_currentLimit!=0) + { + target_velocity = -m_ERP*m_currentLimitError/(timeStep); + maxMotorForce = m_maxLimitForce; + } + + maxMotorForce *= timeStep; + + // current velocity difference + Vector3 vel_diff = body0->get_angular_velocity(); + if (body1) + { + vel_diff -= body1->get_angular_velocity(); + } + + + + real_t rel_vel = axis.dot(vel_diff); + + // correction velocity + real_t motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel); + + + if ( motor_relvel < CMP_EPSILON && motor_relvel > -CMP_EPSILON ) + { + return 0.0f;//no need for applying force + } + + + // correction impulse + real_t unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv; + + // clip correction impulse + real_t clippedMotorImpulse; + + ///@todo: should clip against accumulated impulse + if (unclippedMotorImpulse>0.0f) + { + clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse; + } + else + { + clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse; + } + + + // sort with accumulated impulses + real_t lo = real_t(-1e30); + real_t hi = real_t(1e30); + + real_t oldaccumImpulse = m_accumulatedImpulse; + real_t sum = oldaccumImpulse + clippedMotorImpulse; + m_accumulatedImpulse = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum; + + clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; + + + + Vector3 motorImp = clippedMotorImpulse * axis; + + + body0->apply_torque_impulse(motorImp); + if (body1) body1->apply_torque_impulse(-motorImp); + + return clippedMotorImpulse; + + +} + +//////////////////////////// End G6DOFRotationalLimitMotorSW //////////////////////////////////// + +//////////////////////////// G6DOFTranslationalLimitMotorSW //////////////////////////////////// +real_t G6DOFTranslationalLimitMotorSW::solveLinearAxis( + real_t timeStep, + real_t jacDiagABInv, + BodySW* body1,const Vector3 &pointInA, + BodySW* body2,const Vector3 &pointInB, + int limit_index, + const Vector3 & axis_normal_on_a, + const Vector3 & anchorPos) +{ + +///find relative velocity +// Vector3 rel_pos1 = pointInA - body1->get_transform().origin; +// Vector3 rel_pos2 = pointInB - body2->get_transform().origin; + Vector3 rel_pos1 = anchorPos - body1->get_transform().origin; + Vector3 rel_pos2 = anchorPos - body2->get_transform().origin; + + Vector3 vel1 = body1->get_velocity_in_local_point(rel_pos1); + Vector3 vel2 = body2->get_velocity_in_local_point(rel_pos2); + Vector3 vel = vel1 - vel2; + + real_t rel_vel = axis_normal_on_a.dot(vel); + + + +/// apply displacement correction + +//positional error (zeroth order error) + real_t depth = -(pointInA - pointInB).dot(axis_normal_on_a); + real_t lo = real_t(-1e30); + real_t hi = real_t(1e30); + + real_t minLimit = m_lowerLimit[limit_index]; + real_t maxLimit = m_upperLimit[limit_index]; + + //handle the limits + if (minLimit < maxLimit) + { + { + if (depth > maxLimit) + { + depth -= maxLimit; + lo = real_t(0.); + + } + else + { + if (depth < minLimit) + { + depth -= minLimit; + hi = real_t(0.); + } + else + { + return 0.0f; + } + } + } + } + + real_t normalImpulse= m_limitSoftness[limit_index]*(m_restitution[limit_index]*depth/timeStep - m_damping[limit_index]*rel_vel) * jacDiagABInv; + + + + + real_t oldNormalImpulse = m_accumulatedImpulse[limit_index]; + real_t sum = oldNormalImpulse + normalImpulse; + m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum; + normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; + + Vector3 impulse_vector = axis_normal_on_a * normalImpulse; + body1->apply_impulse( rel_pos1, impulse_vector); + body2->apply_impulse( rel_pos2, -impulse_vector); + return normalImpulse; +} + +//////////////////////////// G6DOFTranslationalLimitMotorSW //////////////////////////////////// + + +Generic6DOFJointSW::Generic6DOFJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB, bool useLinearReferenceFrameA) + : JointSW(_arr,2) + , m_frameInA(frameInA) + , m_frameInB(frameInB), + m_useLinearReferenceFrameA(useLinearReferenceFrameA) +{ + A=rbA; + B=rbB; + A->add_constraint(this,0); + B->add_constraint(this,1); +} + + + + + +void Generic6DOFJointSW::calculateAngleInfo() +{ + Matrix3 relative_frame = m_calculatedTransformA.basis.inverse()*m_calculatedTransformB.basis; + + matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff); + + + + // in euler angle mode we do not actually constrain the angular velocity + // along the axes axis[0] and axis[2] (although we do use axis[1]) : + // + // to get constrain w2-w1 along ...not + // ------ --------------------- ------ + // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] + // d(angle[1])/dt = 0 ax[1] + // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] + // + // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. + // to prove the result for angle[0], write the expression for angle[0] from + // GetInfo1 then take the derivative. to prove this for angle[2] it is + // easier to take the euler rate expression for d(angle[2])/dt with respect + // to the components of w and set that to 0. + + Vector3 axis0 = m_calculatedTransformB.basis.get_axis(0); + Vector3 axis2 = m_calculatedTransformA.basis.get_axis(2); + + m_calculatedAxis[1] = axis2.cross(axis0); + m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); + m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); + + +// if(m_debugDrawer) +// { +// +// char buff[300]; +// sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ", +// m_calculatedAxisAngleDiff[0], +// m_calculatedAxisAngleDiff[1], +// m_calculatedAxisAngleDiff[2]); +// m_debugDrawer->reportErrorWarning(buff); +// } + +} + +void Generic6DOFJointSW::calculateTransforms() +{ + m_calculatedTransformA = A->get_transform() * m_frameInA; + m_calculatedTransformB = B->get_transform() * m_frameInB; + + calculateAngleInfo(); +} + + +void Generic6DOFJointSW::buildLinearJacobian( + JacobianEntrySW & jacLinear,const Vector3 & normalWorld, + const Vector3 & pivotAInW,const Vector3 & pivotBInW) +{ + memnew_placement(&jacLinear, JacobianEntrySW( + A->get_transform().basis.transposed(), + B->get_transform().basis.transposed(), + pivotAInW - A->get_transform().origin, + pivotBInW - B->get_transform().origin, + normalWorld, + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass())); + +} + +void Generic6DOFJointSW::buildAngularJacobian( + JacobianEntrySW & jacAngular,const Vector3 & jointAxisW) +{ + memnew_placement(&jacAngular, JacobianEntrySW(jointAxisW, + A->get_transform().basis.transposed(), + B->get_transform().basis.transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); + +} + +bool Generic6DOFJointSW::testAngularLimitMotor(int axis_index) +{ + real_t angle = m_calculatedAxisAngleDiff[axis_index]; + + //test limits + m_angularLimits[axis_index].testLimitValue(angle); + return m_angularLimits[axis_index].needApplyTorques(); +} + +bool Generic6DOFJointSW::setup(float p_step) { + + // Clear accumulated impulses for the next simulation step + m_linearLimits.m_accumulatedImpulse=Vector3(real_t(0.), real_t(0.), real_t(0.)); + int i; + for(i = 0; i < 3; i++) + { + m_angularLimits[i].m_accumulatedImpulse = real_t(0.); + } + //calculates transform + calculateTransforms(); + +// const Vector3& pivotAInW = m_calculatedTransformA.origin; +// const Vector3& pivotBInW = m_calculatedTransformB.origin; + calcAnchorPos(); + Vector3 pivotAInW = m_AnchorPos; + Vector3 pivotBInW = m_AnchorPos; + +// not used here +// Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; +// Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; + + Vector3 normalWorld; + //linear part + for (i=0;i<3;i++) + { + if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) + { + if (m_useLinearReferenceFrameA) + normalWorld = m_calculatedTransformA.basis.get_axis(i); + else + normalWorld = m_calculatedTransformB.basis.get_axis(i); + + buildLinearJacobian( + m_jacLinear[i],normalWorld , + pivotAInW,pivotBInW); + + } + } + + // angular part + for (i=0;i<3;i++) + { + //calculates error angle + if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i)) + { + normalWorld = this->getAxis(i); + // Create angular atom + buildAngularJacobian(m_jacAng[i],normalWorld); + } + } + + return true; +} + + +void Generic6DOFJointSW::solve(real_t timeStep) +{ + m_timeStep = timeStep; + + //calculateTransforms(); + + int i; + + // linear + + Vector3 pointInA = m_calculatedTransformA.origin; + Vector3 pointInB = m_calculatedTransformB.origin; + + real_t jacDiagABInv; + Vector3 linear_axis; + for (i=0;i<3;i++) + { + if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) + { + jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal(); + + if (m_useLinearReferenceFrameA) + linear_axis = m_calculatedTransformA.basis.get_axis(i); + else + linear_axis = m_calculatedTransformB.basis.get_axis(i); + + m_linearLimits.solveLinearAxis( + m_timeStep, + jacDiagABInv, + A,pointInA, + B,pointInB, + i,linear_axis, m_AnchorPos); + + } + } + + // angular + Vector3 angular_axis; + real_t angularJacDiagABInv; + for (i=0;i<3;i++) + { + if (m_angularLimits[i].m_enableLimit && m_angularLimits[i].needApplyTorques()) + { + + // get axis + angular_axis = getAxis(i); + + angularJacDiagABInv = real_t(1.) / m_jacAng[i].getDiagonal(); + + m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, A,B); + } + } +} + +void Generic6DOFJointSW::updateRHS(real_t timeStep) +{ + (void)timeStep; + +} + +Vector3 Generic6DOFJointSW::getAxis(int axis_index) const +{ + return m_calculatedAxis[axis_index]; +} + +real_t Generic6DOFJointSW::getAngle(int axis_index) const +{ + return m_calculatedAxisAngleDiff[axis_index]; +} + +void Generic6DOFJointSW::calcAnchorPos(void) +{ + real_t imA = A->get_inv_mass(); + real_t imB = B->get_inv_mass(); + real_t weight; + if(imB == real_t(0.0)) + { + weight = real_t(1.0); + } + else + { + weight = imA / (imA + imB); + } + const Vector3& pA = m_calculatedTransformA.origin; + const Vector3& pB = m_calculatedTransformB.origin; + m_AnchorPos = pA * weight + pB * (real_t(1.0) - weight); + return; +} // Generic6DOFJointSW::calcAnchorPos() + + +void Generic6DOFJointSW::set_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param, float p_value) { + + ERR_FAIL_INDEX(p_axis,3); + switch(p_param) { + case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { + + m_linearLimits.m_lowerLimit[p_axis]=p_value; + } break; + case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT: { + + m_linearLimits.m_upperLimit[p_axis]=p_value; + + } break; + case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: { + + m_linearLimits.m_limitSoftness[p_axis]=p_value; + + } break; + case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION: { + + m_linearLimits.m_restitution[p_axis]=p_value; + + } break; + case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING: { + + m_linearLimits.m_damping[p_axis]=p_value; + + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: { + + m_angularLimits[p_axis].m_loLimit=p_value; + + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: { + + m_angularLimits[p_axis].m_hiLimit=p_value; + + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: { + + m_angularLimits[p_axis].m_limitSoftness; + + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING: { + + m_angularLimits[p_axis].m_damping=p_value; + + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION: { + + m_angularLimits[p_axis].m_bounce=p_value; + + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: { + + m_angularLimits[p_axis].m_maxLimitForce=p_value; + + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP: { + + m_angularLimits[p_axis].m_ERP=p_value; + + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: { + + m_angularLimits[p_axis].m_targetVelocity=p_value; + + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: { + + m_angularLimits[p_axis].m_maxLimitForce=p_value; + + } break; + } +} + +float Generic6DOFJointSW::get_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param) const{ + ERR_FAIL_INDEX_V(p_axis,3,0); + switch(p_param) { + case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { + + return m_linearLimits.m_lowerLimit[p_axis]; + } break; + case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT: { + + return m_linearLimits.m_upperLimit[p_axis]; + + } break; + case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: { + + return m_linearLimits.m_limitSoftness[p_axis]; + + } break; + case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION: { + + return m_linearLimits.m_restitution[p_axis]; + + } break; + case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING: { + + return m_linearLimits.m_damping[p_axis]; + + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: { + + return m_angularLimits[p_axis].m_loLimit; + + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: { + + return m_angularLimits[p_axis].m_hiLimit; + + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: { + + return m_angularLimits[p_axis].m_limitSoftness; + + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING: { + + return m_angularLimits[p_axis].m_damping; + + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION: { + + return m_angularLimits[p_axis].m_bounce; + + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: { + + return m_angularLimits[p_axis].m_maxLimitForce; + + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP: { + + return m_angularLimits[p_axis].m_ERP; + + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: { + + return m_angularLimits[p_axis].m_targetVelocity; + + } break; + case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: { + + return m_angularLimits[p_axis].m_maxLimitForce; + + } break; + } + return 0; +} + +void Generic6DOFJointSW::set_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value){ + + ERR_FAIL_INDEX(p_axis,3); + + switch(p_flag) { + case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: { + + m_linearLimits.enable_limit[p_axis]=p_value; + } break; + case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: { + + m_angularLimits[p_axis].m_enableLimit=p_value; + } break; + case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR: { + + m_angularLimits[p_axis].m_enableMotor=p_value; + } break; + } + + +} +bool Generic6DOFJointSW::get_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag) const{ + + ERR_FAIL_INDEX_V(p_axis,3,0); + switch(p_flag) { + case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: { + + return m_linearLimits.enable_limit[p_axis]; + } break; + case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: { + + return m_angularLimits[p_axis].m_enableLimit; + } break; + case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR: { + + return m_angularLimits[p_axis].m_enableMotor; + } break; + } + + return 0; +} diff --git a/servers/physics/joints/generic_6dof_joint_sw.h b/servers/physics/joints/generic_6dof_joint_sw.h new file mode 100644 index 0000000000..7f762e51a2 --- /dev/null +++ b/servers/physics/joints/generic_6dof_joint_sw.h @@ -0,0 +1,428 @@ +#ifndef GENERIC_6DOF_JOINT_SW_H +#define GENERIC_6DOF_JOINT_SW_H + +#include "servers/physics/joints_sw.h" +#include "servers/physics/joints/jacobian_entry_sw.h" + + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +/* +2007-09-09 +Generic6DOFJointSW Refactored by Francisco Le?n +email: projectileman@yahoo.com +http://gimpact.sf.net +*/ + + +//! Rotation Limit structure for generic joints +class G6DOFRotationalLimitMotorSW { +public: + //! limit_parameters + //!@{ + real_t m_loLimit;//!< joint limit + real_t m_hiLimit;//!< joint limit + real_t m_targetVelocity;//!< target motor velocity + real_t m_maxMotorForce;//!< max force on motor + real_t m_maxLimitForce;//!< max force on limit + real_t m_damping;//!< Damping. + real_t m_limitSoftness;//! Relaxation factor + real_t m_ERP;//!< Error tolerance factor when joint is at limit + real_t m_bounce;//!< restitution factor + bool m_enableMotor; + bool m_enableLimit; + + //!@} + + //! temp_variables + //!@{ + real_t m_currentLimitError;//! How much is violated this limit + int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit + real_t m_accumulatedImpulse; + //!@} + + G6DOFRotationalLimitMotorSW() + { + m_accumulatedImpulse = 0.f; + m_targetVelocity = 0; + m_maxMotorForce = 0.1f; + m_maxLimitForce = 300.0f; + m_loLimit = -1e30; + m_hiLimit = 1e30; + m_ERP = 0.5f; + m_bounce = 0.0f; + m_damping = 1.0f; + m_limitSoftness = 0.5f; + m_currentLimit = 0; + m_currentLimitError = 0; + m_enableMotor = false; + m_enableLimit=false; + } + + G6DOFRotationalLimitMotorSW(const G6DOFRotationalLimitMotorSW & limot) + { + m_targetVelocity = limot.m_targetVelocity; + m_maxMotorForce = limot.m_maxMotorForce; + m_limitSoftness = limot.m_limitSoftness; + m_loLimit = limot.m_loLimit; + m_hiLimit = limot.m_hiLimit; + m_ERP = limot.m_ERP; + m_bounce = limot.m_bounce; + m_currentLimit = limot.m_currentLimit; + m_currentLimitError = limot.m_currentLimitError; + m_enableMotor = limot.m_enableMotor; + } + + + + //! Is limited + bool isLimited() + { + if(m_loLimit>=m_hiLimit) return false; + return true; + } + + //! Need apply correction + bool needApplyTorques() + { + if(m_currentLimit == 0 && m_enableMotor == false) return false; + return true; + } + + //! calculates error + /*! + calculates m_currentLimit and m_currentLimitError. + */ + int testLimitValue(real_t test_value); + + //! apply the correction impulses for two bodies + real_t solveAngularLimits(real_t timeStep,Vector3& axis, real_t jacDiagABInv,BodySW * body0, BodySW * body1); + + +}; + + + +class G6DOFTranslationalLimitMotorSW +{ +public: + Vector3 m_lowerLimit;//!< the constraint lower limits + Vector3 m_upperLimit;//!< the constraint upper limits + Vector3 m_accumulatedImpulse; + //! Linear_Limit_parameters + //!@{ + Vector3 m_limitSoftness;//!< Softness for linear limit + Vector3 m_damping;//!< Damping for linear limit + Vector3 m_restitution;//! Bounce parameter for linear limit + //!@} + bool enable_limit[3]; + + G6DOFTranslationalLimitMotorSW() + { + m_lowerLimit=Vector3(0.f,0.f,0.f); + m_upperLimit=Vector3(0.f,0.f,0.f); + m_accumulatedImpulse=Vector3(0.f,0.f,0.f); + + m_limitSoftness = Vector3(1,1,1)*0.7f; + m_damping = Vector3(1,1,1)*real_t(1.0f); + m_restitution = Vector3(1,1,1)*real_t(0.5f); + + enable_limit[0]=true; + enable_limit[1]=true; + enable_limit[2]=true; + } + + G6DOFTranslationalLimitMotorSW(const G6DOFTranslationalLimitMotorSW & other ) + { + m_lowerLimit = other.m_lowerLimit; + m_upperLimit = other.m_upperLimit; + m_accumulatedImpulse = other.m_accumulatedImpulse; + + m_limitSoftness = other.m_limitSoftness ; + m_damping = other.m_damping; + m_restitution = other.m_restitution; + } + + //! Test limit + /*! + - free means upper < lower, + - locked means upper == lower + - limited means upper > lower + - limitIndex: first 3 are linear, next 3 are angular + */ + inline bool isLimited(int limitIndex) + { + return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); + } + + + real_t solveLinearAxis( + real_t timeStep, + real_t jacDiagABInv, + BodySW* body1,const Vector3 &pointInA, + BodySW* body2,const Vector3 &pointInB, + int limit_index, + const Vector3 & axis_normal_on_a, + const Vector3 & anchorPos); + + +}; + + +class Generic6DOFJointSW : public JointSW +{ +protected: + + + union { + struct { + BodySW *A; + BodySW *B; + }; + + BodySW *_arr[2]; + }; + + //! relative_frames + //!@{ + Transform m_frameInA;//!< the constraint space w.r.t body A + Transform m_frameInB;//!< the constraint space w.r.t body B + //!@} + + //! Jacobians + //!@{ + JacobianEntrySW m_jacLinear[3];//!< 3 orthogonal linear constraints + JacobianEntrySW m_jacAng[3];//!< 3 orthogonal angular constraints + //!@} + + //! Linear_Limit_parameters + //!@{ + G6DOFTranslationalLimitMotorSW m_linearLimits; + //!@} + + + //! hinge_parameters + //!@{ + G6DOFRotationalLimitMotorSW m_angularLimits[3]; + //!@} + + +protected: + //! temporal variables + //!@{ + real_t m_timeStep; + Transform m_calculatedTransformA; + Transform m_calculatedTransformB; + Vector3 m_calculatedAxisAngleDiff; + Vector3 m_calculatedAxis[3]; + + Vector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes + + bool m_useLinearReferenceFrameA; + + //!@} + + Generic6DOFJointSW& operator=(Generic6DOFJointSW& other) + { + ERR_PRINT("pito"); + (void) other; + return *this; + } + + + + void buildLinearJacobian( + JacobianEntrySW & jacLinear,const Vector3 & normalWorld, + const Vector3 & pivotAInW,const Vector3 & pivotBInW); + + void buildAngularJacobian(JacobianEntrySW & jacAngular,const Vector3 & jointAxisW); + + + //! calcs the euler angles between the two bodies. + void calculateAngleInfo(); + + + +public: + Generic6DOFJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB ,bool useLinearReferenceFrameA); + + virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_6DOF; } + + virtual bool setup(float p_step); + virtual void solve(float p_step); + + + //! Calcs global transform of the offsets + /*! + Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies. + \sa Generic6DOFJointSW.getCalculatedTransformA , Generic6DOFJointSW.getCalculatedTransformB, Generic6DOFJointSW.calculateAngleInfo + */ + void calculateTransforms(); + + //! Gets the global transform of the offset for body A + /*! + \sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo. + */ + const Transform & getCalculatedTransformA() const + { + return m_calculatedTransformA; + } + + //! Gets the global transform of the offset for body B + /*! + \sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo. + */ + const Transform & getCalculatedTransformB() const + { + return m_calculatedTransformB; + } + + const Transform & getFrameOffsetA() const + { + return m_frameInA; + } + + const Transform & getFrameOffsetB() const + { + return m_frameInB; + } + + + Transform & getFrameOffsetA() + { + return m_frameInA; + } + + Transform & getFrameOffsetB() + { + return m_frameInB; + } + + + //! performs Jacobian calculation, and also calculates angle differences and axis + + + void updateRHS(real_t timeStep); + + //! Get the rotation axis in global coordinates + /*! + \pre Generic6DOFJointSW.buildJacobian must be called previously. + */ + Vector3 getAxis(int axis_index) const; + + //! Get the relative Euler angle + /*! + \pre Generic6DOFJointSW.buildJacobian must be called previously. + */ + real_t getAngle(int axis_index) const; + + //! Test angular limit. + /*! + Calculates angular correction and returns true if limit needs to be corrected. + \pre Generic6DOFJointSW.buildJacobian must be called previously. + */ + bool testAngularLimitMotor(int axis_index); + + void setLinearLowerLimit(const Vector3& linearLower) + { + m_linearLimits.m_lowerLimit = linearLower; + } + + void setLinearUpperLimit(const Vector3& linearUpper) + { + m_linearLimits.m_upperLimit = linearUpper; + } + + void setAngularLowerLimit(const Vector3& angularLower) + { + m_angularLimits[0].m_loLimit = angularLower.x; + m_angularLimits[1].m_loLimit = angularLower.y; + m_angularLimits[2].m_loLimit = angularLower.z; + } + + void setAngularUpperLimit(const Vector3& angularUpper) + { + m_angularLimits[0].m_hiLimit = angularUpper.x; + m_angularLimits[1].m_hiLimit = angularUpper.y; + m_angularLimits[2].m_hiLimit = angularUpper.z; + } + + //! Retrieves the angular limit informacion + G6DOFRotationalLimitMotorSW * getRotationalLimitMotor(int index) + { + return &m_angularLimits[index]; + } + + //! Retrieves the limit informacion + G6DOFTranslationalLimitMotorSW * getTranslationalLimitMotor() + { + return &m_linearLimits; + } + + //first 3 are linear, next 3 are angular + void setLimit(int axis, real_t lo, real_t hi) + { + if(axis<3) + { + m_linearLimits.m_lowerLimit[axis] = lo; + m_linearLimits.m_upperLimit[axis] = hi; + } + else + { + m_angularLimits[axis-3].m_loLimit = lo; + m_angularLimits[axis-3].m_hiLimit = hi; + } + } + + //! Test limit + /*! + - free means upper < lower, + - locked means upper == lower + - limited means upper > lower + - limitIndex: first 3 are linear, next 3 are angular + */ + bool isLimited(int limitIndex) + { + if(limitIndex<3) + { + return m_linearLimits.isLimited(limitIndex); + + } + return m_angularLimits[limitIndex-3].isLimited(); + } + + const BodySW* getRigidBodyA() const + { + return A; + } + const BodySW* getRigidBodyB() const + { + return B; + } + + virtual void calcAnchorPos(void); // overridable + + void set_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param, float p_value); + float get_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param) const; + + void set_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value); + bool get_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag) const; + +}; + + +#endif // GENERIC_6DOF_JOINT_SW_H diff --git a/servers/physics/joints/hinge_joint_sw.cpp b/servers/physics/joints/hinge_joint_sw.cpp new file mode 100644 index 0000000000..feaf00290d --- /dev/null +++ b/servers/physics/joints/hinge_joint_sw.cpp @@ -0,0 +1,438 @@ +#include "hinge_joint_sw.h" + +static void plane_space(const Vector3& n, Vector3& p, Vector3& q) { + + if (Math::abs(n.z) > 0.707106781186547524400844362) { + // choose p in y-z plane + real_t a = n[1]*n[1] + n[2]*n[2]; + real_t k = 1.0/Math::sqrt(a); + p=Vector3(0,-n[2]*k,n[1]*k); + // set q = n x p + q=Vector3(a*k,-n[0]*p[2],n[0]*p[1]); + } + else { + // choose p in x-y plane + real_t a = n.x*n.x + n.y*n.y; + real_t k = 1.0/Math::sqrt(a); + p=Vector3(-n.y*k,n.x*k,0); + // set q = n x p + q=Vector3(-n.z*p.y,n.z*p.x,a*k); + } +} + +HingeJointSW::HingeJointSW(BodySW* rbA,BodySW* rbB, const Transform& frameA, const Transform& frameB) : JointSW(_arr,2) { + + A=rbA; + B=rbB; + + m_rbAFrame=frameA; + m_rbBFrame=frameB; + // flip axis + m_rbBFrame.basis[0][2] *= real_t(-1.); + m_rbBFrame.basis[1][2] *= real_t(-1.); + m_rbBFrame.basis[2][2] *= real_t(-1.); + + + //start with free + m_lowerLimit = Math_PI; + m_upperLimit = -Math_PI; + + + m_useLimit = false; + m_biasFactor = 0.3f; + m_relaxationFactor = 1.0f; + m_limitSoftness = 0.9f; + m_solveLimit = false; + + tau=0.3; + + m_angularOnly=false; + m_enableAngularMotor=false; + + A->add_constraint(this,0); + B->add_constraint(this,1); + +} + +HingeJointSW::HingeJointSW(BodySW* rbA,BodySW* rbB, const Vector3& pivotInA,const Vector3& pivotInB, + const Vector3& axisInA,const Vector3& axisInB) : JointSW(_arr,2) { + + A=rbA; + B=rbB; + + m_rbAFrame.origin = pivotInA; + + // since no frame is given, assume this to be zero angle and just pick rb transform axis + Vector3 rbAxisA1 = rbA->get_transform().basis.get_axis(0); + + Vector3 rbAxisA2; + real_t projection = axisInA.dot(rbAxisA1); + if (projection >= 1.0f - CMP_EPSILON) { + rbAxisA1 = -rbA->get_transform().basis.get_axis(2); + rbAxisA2 = rbA->get_transform().basis.get_axis(1); + } else if (projection <= -1.0f + CMP_EPSILON) { + rbAxisA1 = rbA->get_transform().basis.get_axis(2); + rbAxisA2 = rbA->get_transform().basis.get_axis(1); + } else { + rbAxisA2 = axisInA.cross(rbAxisA1); + rbAxisA1 = rbAxisA2.cross(axisInA); + } + + m_rbAFrame.basis=Matrix3( rbAxisA1.x,rbAxisA2.x,axisInA.x, + rbAxisA1.y,rbAxisA2.y,axisInA.y, + rbAxisA1.z,rbAxisA2.z,axisInA.z ); + + Quat rotationArc = Quat(axisInA,axisInB); + Vector3 rbAxisB1 = rotationArc.xform(rbAxisA1); + Vector3 rbAxisB2 = axisInB.cross(rbAxisB1); + + m_rbBFrame.origin = pivotInB; + m_rbBFrame.basis=Matrix3( rbAxisB1.x,rbAxisB2.x,-axisInB.x, + rbAxisB1.y,rbAxisB2.y,-axisInB.y, + rbAxisB1.z,rbAxisB2.z,-axisInB.z ); + + //start with free + m_lowerLimit = Math_PI; + m_upperLimit = -Math_PI; + + + m_useLimit = false; + m_biasFactor = 0.3f; + m_relaxationFactor = 1.0f; + m_limitSoftness = 0.9f; + m_solveLimit = false; + + tau=0.3; + + m_angularOnly=false; + m_enableAngularMotor=false; + + A->add_constraint(this,0); + B->add_constraint(this,1); + +} + + + +bool HingeJointSW::setup(float p_step) { + + m_appliedImpulse = real_t(0.); + + if (!m_angularOnly) + { + Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); + Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); + Vector3 relPos = pivotBInW - pivotAInW; + + Vector3 normal[3]; + if (relPos.length_squared() > CMP_EPSILON) + { + normal[0] = relPos.normalized(); + } + else + { + normal[0]=Vector3(real_t(1.0),0,0); + } + + plane_space(normal[0], normal[1], normal[2]); + + for (int i=0;i<3;i++) + { + memnew_placement(&m_jac[i], JacobianEntrySW( + A->get_transform().basis.transposed(), + B->get_transform().basis.transposed(), + pivotAInW - A->get_transform().origin, + pivotBInW - B->get_transform().origin, + normal[i], + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass()) ); + } + } + + //calculate two perpendicular jointAxis, orthogonal to hingeAxis + //these two jointAxis require equal angular velocities for both bodies + + //this is unused for now, it's a todo + Vector3 jointAxis0local; + Vector3 jointAxis1local; + + plane_space(m_rbAFrame.basis.get_axis(2),jointAxis0local,jointAxis1local); + + A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(2) ); + Vector3 jointAxis0 = A->get_transform().basis.xform( jointAxis0local ); + Vector3 jointAxis1 = A->get_transform().basis.xform( jointAxis1local ); + Vector3 hingeAxisWorld = A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(2) ); + + memnew_placement(&m_jacAng[0], JacobianEntrySW(jointAxis0, + A->get_transform().basis.transposed(), + B->get_transform().basis.transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); + + memnew_placement(&m_jacAng[1], JacobianEntrySW(jointAxis1, + A->get_transform().basis.transposed(), + B->get_transform().basis.transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); + + memnew_placement(&m_jacAng[2], JacobianEntrySW(hingeAxisWorld, + A->get_transform().basis.transposed(), + B->get_transform().basis.transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); + + + // Compute limit information + real_t hingeAngle = get_hinge_angle(); + +// print_line("angle: "+rtos(hingeAngle)); + //set bias, sign, clear accumulator + m_correction = real_t(0.); + m_limitSign = real_t(0.); + m_solveLimit = false; + m_accLimitImpulse = real_t(0.); + + + + /*if (m_useLimit) { + print_line("low: "+rtos(m_lowerLimit)); + print_line("hi: "+rtos(m_upperLimit)); + }*/ + +// if (m_lowerLimit < m_upperLimit) + if (m_useLimit && m_lowerLimit <= m_upperLimit) + { +// if (hingeAngle <= m_lowerLimit*m_limitSoftness) + if (hingeAngle <= m_lowerLimit) + { + m_correction = (m_lowerLimit - hingeAngle); + m_limitSign = 1.0f; + m_solveLimit = true; + } +// else if (hingeAngle >= m_upperLimit*m_limitSoftness) + else if (hingeAngle >= m_upperLimit) + { + m_correction = m_upperLimit - hingeAngle; + m_limitSign = -1.0f; + m_solveLimit = true; + } + } + + //Compute K = J*W*J' for hinge axis + Vector3 axisA = A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(2) ); + m_kHinge = 1.0f / (A->compute_angular_impulse_denominator(axisA) + + B->compute_angular_impulse_denominator(axisA)); + + return true; +} + +void HingeJointSW::solve(float p_step) { + + Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); + Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); + + //real_t tau = real_t(0.3); + + //linear part + if (!m_angularOnly) + { + Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; + Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; + + Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1); + Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2); + Vector3 vel = vel1 - vel2; + + for (int i=0;i<3;i++) + { + const Vector3& normal = m_jac[i].m_linearJointAxis; + real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal(); + + real_t rel_vel; + rel_vel = normal.dot(vel); + //positional error (zeroth order error) + real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + real_t impulse = depth*tau/p_step * jacDiagABInv - rel_vel * jacDiagABInv; + m_appliedImpulse += impulse; + Vector3 impulse_vector = normal * impulse; + A->apply_impulse(pivotAInW - A->get_transform().origin,impulse_vector); + B->apply_impulse(pivotBInW - B->get_transform().origin,-impulse_vector); + } + } + + + { + ///solve angular part + + // get axes in world space + Vector3 axisA = A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(2) ); + Vector3 axisB = B->get_transform().basis.xform( m_rbBFrame.basis.get_axis(2) ); + + const Vector3& angVelA = A->get_angular_velocity(); + const Vector3& angVelB = B->get_angular_velocity(); + + Vector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); + Vector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); + + Vector3 angAorthog = angVelA - angVelAroundHingeAxisA; + Vector3 angBorthog = angVelB - angVelAroundHingeAxisB; + Vector3 velrelOrthog = angAorthog-angBorthog; + { + //solve orthogonal angular velocity correction + real_t relaxation = real_t(1.); + real_t len = velrelOrthog.length(); + if (len > real_t(0.00001)) + { + Vector3 normal = velrelOrthog.normalized(); + real_t denom = A->compute_angular_impulse_denominator(normal) + + B->compute_angular_impulse_denominator(normal); + // scale for mass and relaxation + velrelOrthog *= (real_t(1.)/denom) * m_relaxationFactor; + } + + //solve angular positional correction + Vector3 angularError = -axisA.cross(axisB) *(real_t(1.)/p_step); + real_t len2 = angularError.length(); + if (len2>real_t(0.00001)) + { + Vector3 normal2 = angularError.normalized(); + real_t denom2 = A->compute_angular_impulse_denominator(normal2) + + B->compute_angular_impulse_denominator(normal2); + angularError *= (real_t(1.)/denom2) * relaxation; + } + + A->apply_torque_impulse(-velrelOrthog+angularError); + B->apply_torque_impulse(velrelOrthog-angularError); + + // solve limit + if (m_solveLimit) + { + real_t amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (real_t(1.)/p_step)*m_biasFactor ) * m_limitSign; + + real_t impulseMag = amplitude * m_kHinge; + + // Clamp the accumulated impulse + real_t temp = m_accLimitImpulse; + m_accLimitImpulse = MAX(m_accLimitImpulse + impulseMag, real_t(0) ); + impulseMag = m_accLimitImpulse - temp; + + + Vector3 impulse = axisA * impulseMag * m_limitSign; + A->apply_torque_impulse(impulse); + B->apply_torque_impulse(-impulse); + } + } + + //apply motor + if (m_enableAngularMotor) + { + //todo: add limits too + Vector3 angularLimit(0,0,0); + + Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; + real_t projRelVel = velrel.dot(axisA); + + real_t desiredMotorVel = m_motorTargetVelocity; + real_t motor_relvel = desiredMotorVel - projRelVel; + + real_t unclippedMotorImpulse = m_kHinge * motor_relvel;; + //todo: should clip against accumulated impulse + real_t clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; + clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; + Vector3 motorImp = clippedMotorImpulse * axisA; + + A->apply_torque_impulse(motorImp+angularLimit); + B->apply_torque_impulse(-motorImp-angularLimit); + + } + } + +} +/* +void HingeJointSW::updateRHS(real_t timeStep) +{ + (void)timeStep; + +} +*/ + +static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) +{ + real_t coeff_1 = Math_PI / 4.0f; + real_t coeff_2 = 3.0f * coeff_1; + real_t abs_y = Math::abs(y); + real_t angle; + if (x >= 0.0f) { + real_t r = (x - abs_y) / (x + abs_y); + angle = coeff_1 - coeff_1 * r; + } else { + real_t r = (x + abs_y) / (abs_y - x); + angle = coeff_2 - coeff_1 * r; + } + return (y < 0.0f) ? -angle : angle; +} + + +real_t HingeJointSW::get_hinge_angle() { + const Vector3 refAxis0 = A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(0) ); + const Vector3 refAxis1 = A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(1) ); + const Vector3 swingAxis = B->get_transform().basis.xform( m_rbBFrame.basis.get_axis(1) ); + + return atan2fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1) ); +} + + +void HingeJointSW::set_param(PhysicsServer::HingeJointParam p_param, float p_value) { + + switch (p_param) { + + case PhysicsServer::HINGE_JOINT_BIAS: tau=p_value; break; + case PhysicsServer::HINGE_JOINT_LIMIT_UPPER: m_upperLimit=p_value; break; + case PhysicsServer::HINGE_JOINT_LIMIT_LOWER: m_lowerLimit=p_value; break; + case PhysicsServer::HINGE_JOINT_LIMIT_BIAS: m_biasFactor=p_value; break; + case PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS: m_limitSoftness=p_value; break; + case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION: m_relaxationFactor=p_value; break; + case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY: m_motorTargetVelocity=p_value; break; + case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE: m_maxMotorImpulse=p_value; break; + + } +} + +float HingeJointSW::get_param(PhysicsServer::HingeJointParam p_param) const{ + + switch (p_param) { + + case PhysicsServer::HINGE_JOINT_BIAS: return tau; + case PhysicsServer::HINGE_JOINT_LIMIT_UPPER: return m_upperLimit; + case PhysicsServer::HINGE_JOINT_LIMIT_LOWER: return m_lowerLimit; + case PhysicsServer::HINGE_JOINT_LIMIT_BIAS: return m_biasFactor; + case PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS: return m_limitSoftness; + case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION: return m_relaxationFactor; + case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY: return m_motorTargetVelocity; + case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE: return m_maxMotorImpulse; + + } + + return 0; +} + +void HingeJointSW::set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value){ + + print_line(p_flag+": "+itos(p_value)); + switch (p_flag) { + case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT: m_useLimit=p_value; break; + case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR: m_enableAngularMotor=p_value; break; + } + +} +bool HingeJointSW::get_flag(PhysicsServer::HingeJointFlag p_flag) const{ + + switch (p_flag) { + case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT: return m_useLimit; + case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR:return m_enableAngularMotor; + } + + return false; +} diff --git a/servers/physics/joints/hinge_joint_sw.h b/servers/physics/joints/hinge_joint_sw.h new file mode 100644 index 0000000000..4f6cdaf799 --- /dev/null +++ b/servers/physics/joints/hinge_joint_sw.h @@ -0,0 +1,89 @@ +#ifndef HINGE_JOINT_SW_H +#define HINGE_JOINT_SW_H + +#include "servers/physics/joints_sw.h" +#include "servers/physics/joints/jacobian_entry_sw.h" + + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +class HingeJointSW : public JointSW { + + union { + struct { + BodySW *A; + BodySW *B; + }; + + BodySW *_arr[2]; + }; + + JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints + JacobianEntrySW m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor + + Transform m_rbAFrame; // constraint axii. Assumes z is hinge axis. + Transform m_rbBFrame; + + real_t m_motorTargetVelocity; + real_t m_maxMotorImpulse; + + real_t m_limitSoftness; + real_t m_biasFactor; + real_t m_relaxationFactor; + + real_t m_lowerLimit; + real_t m_upperLimit; + + real_t m_kHinge; + + real_t m_limitSign; + real_t m_correction; + + real_t m_accLimitImpulse; + + real_t tau; + + bool m_useLimit; + bool m_angularOnly; + bool m_enableAngularMotor; + bool m_solveLimit; + + real_t m_appliedImpulse; + + +public: + + virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_HINGE; } + + virtual bool setup(float p_step); + virtual void solve(float p_step); + + real_t get_hinge_angle(); + + void set_param(PhysicsServer::HingeJointParam p_param, float p_value); + float get_param(PhysicsServer::HingeJointParam p_param) const; + + void set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value); + bool get_flag(PhysicsServer::HingeJointFlag p_flag) const; + + HingeJointSW(BodySW* rbA,BodySW* rbB, const Transform& frameA, const Transform& frameB); + HingeJointSW(BodySW* rbA,BodySW* rbB, const Vector3& pivotInA,const Vector3& pivotInB, const Vector3& axisInA,const Vector3& axisInB); + +}; + +#endif // HINGE_JOINT_SW_H diff --git a/servers/physics/joints/jacobian_entry_sw.cpp b/servers/physics/joints/jacobian_entry_sw.cpp new file mode 100644 index 0000000000..faa3cf15c4 --- /dev/null +++ b/servers/physics/joints/jacobian_entry_sw.cpp @@ -0,0 +1,2 @@ +#include "jacobian_entry_sw.h" + diff --git a/servers/physics/joints/jacobian_entry_sw.h b/servers/physics/joints/jacobian_entry_sw.h new file mode 100644 index 0000000000..16fa034215 --- /dev/null +++ b/servers/physics/joints/jacobian_entry_sw.h @@ -0,0 +1,146 @@ +#ifndef JACOBIAN_ENTRY_SW_H +#define JACOBIAN_ENTRY_SW_H + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "transform.h" + +class JacobianEntrySW { +public: + JacobianEntrySW() {}; + //constraint between two different rigidbodies + JacobianEntrySW( + const Matrix3& world2A, + const Matrix3& world2B, + const Vector3& rel_pos1,const Vector3& rel_pos2, + const Vector3& jointAxis, + const Vector3& inertiaInvA, + const real_t massInvA, + const Vector3& inertiaInvB, + const real_t massInvB) + :m_linearJointAxis(jointAxis) + { + m_aJ = world2A.xform(rel_pos1.cross(m_linearJointAxis)); + m_bJ = world2B.xform(rel_pos2.cross(-m_linearJointAxis)); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); + + ERR_FAIL_COND(m_Adiag <= real_t(0.0)); + } + + //angular constraint between two different rigidbodies + JacobianEntrySW(const Vector3& jointAxis, + const Matrix3& world2A, + const Matrix3& world2B, + const Vector3& inertiaInvA, + const Vector3& inertiaInvB) + :m_linearJointAxis(Vector3(real_t(0.),real_t(0.),real_t(0.))) + { + m_aJ= world2A.xform(jointAxis); + m_bJ = world2B.xform(-jointAxis); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); + + ERR_FAIL_COND(m_Adiag <= real_t(0.0)); + } + + //angular constraint between two different rigidbodies + JacobianEntrySW(const Vector3& axisInA, + const Vector3& axisInB, + const Vector3& inertiaInvA, + const Vector3& inertiaInvB) + : m_linearJointAxis(Vector3(real_t(0.),real_t(0.),real_t(0.))) + , m_aJ(axisInA) + , m_bJ(-axisInB) + { + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); + + ERR_FAIL_COND(m_Adiag <= real_t(0.0)); + } + + //constraint on one rigidbody + JacobianEntrySW( + const Matrix3& world2A, + const Vector3& rel_pos1,const Vector3& rel_pos2, + const Vector3& jointAxis, + const Vector3& inertiaInvA, + const real_t massInvA) + :m_linearJointAxis(jointAxis) + { + m_aJ= world2A.xform(rel_pos1.cross(jointAxis)); + m_bJ = world2A.xform(rel_pos2.cross(-jointAxis)); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = Vector3(real_t(0.),real_t(0.),real_t(0.)); + m_Adiag = massInvA + m_0MinvJt.dot(m_aJ); + + ERR_FAIL_COND(m_Adiag <= real_t(0.0)); + } + + real_t getDiagonal() const { return m_Adiag; } + + // for two constraints on the same rigidbody (for example vehicle friction) + real_t getNonDiagonal(const JacobianEntrySW& jacB, const real_t massInvA) const + { + const JacobianEntrySW& jacA = *this; + real_t lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis); + real_t ang = jacA.m_0MinvJt.dot(jacB.m_aJ); + return lin + ang; + } + + + + // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) + real_t getNonDiagonal(const JacobianEntrySW& jacB,const real_t massInvA,const real_t massInvB) const + { + const JacobianEntrySW& jacA = *this; + Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis; + Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ; + Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ; + Vector3 lin0 = massInvA * lin ; + Vector3 lin1 = massInvB * lin; + Vector3 sum = ang0+ang1+lin0+lin1; + return sum[0]+sum[1]+sum[2]; + } + + real_t getRelativeVelocity(const Vector3& linvelA,const Vector3& angvelA,const Vector3& linvelB,const Vector3& angvelB) + { + Vector3 linrel = linvelA - linvelB; + Vector3 angvela = angvelA * m_aJ; + Vector3 angvelb = angvelB * m_bJ; + linrel *= m_linearJointAxis; + angvela += angvelb; + angvela += linrel; + real_t rel_vel2 = angvela[0]+angvela[1]+angvela[2]; + return rel_vel2 + CMP_EPSILON; + } +//private: + + Vector3 m_linearJointAxis; + Vector3 m_aJ; + Vector3 m_bJ; + Vector3 m_0MinvJt; + Vector3 m_1MinvJt; + //Optimization: can be stored in the w/last component of one of the vectors + real_t m_Adiag; + +}; + + +#endif // JACOBIAN_ENTRY_SW_H diff --git a/servers/physics/joints/pin_joint_sw.cpp b/servers/physics/joints/pin_joint_sw.cpp new file mode 100644 index 0000000000..229863fb7b --- /dev/null +++ b/servers/physics/joints/pin_joint_sw.cpp @@ -0,0 +1,127 @@ +#include "pin_joint_sw.h" + +bool PinJointSW::setup(float p_step) { + + m_appliedImpulse = real_t(0.); + + Vector3 normal(0,0,0); + + for (int i=0;i<3;i++) + { + normal[i] = 1; + memnew_placement(&m_jac[i],JacobianEntrySW( + A->get_transform().basis.transposed(), + B->get_transform().basis.transposed(), + A->get_transform().xform(m_pivotInA) - A->get_transform().origin, + B->get_transform().xform(m_pivotInB) - B->get_transform().origin, + normal, + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass())); + normal[i] = 0; + } + + return true; +} + +void PinJointSW::solve(float p_step){ + + Vector3 pivotAInW = A->get_transform().xform(m_pivotInA); + Vector3 pivotBInW = B->get_transform().xform(m_pivotInB); + + + Vector3 normal(0,0,0); + + +// Vector3 angvelA = A->get_transform().origin.getBasis().transpose() * A->getAngularVelocity(); +// Vector3 angvelB = B->get_transform().origin.getBasis().transpose() * B->getAngularVelocity(); + + for (int i=0;i<3;i++) + { + normal[i] = 1; + real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal(); + + Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; + Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; + //this jacobian entry could be re-used for all iterations + + Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1); + Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2); + Vector3 vel = vel1 - vel2; + + real_t rel_vel; + rel_vel = normal.dot(vel); + + /* + //velocity error (first order error) + real_t rel_vel = m_jac[i].getRelativeVelocity(A->getLinearVelocity(),angvelA, + B->getLinearVelocity(),angvelB); + */ + + //positional error (zeroth order error) + real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + + real_t impulse = depth*m_tau/p_step * jacDiagABInv - m_damping * rel_vel * jacDiagABInv; + + real_t impulseClamp = m_impulseClamp; + if (impulseClamp > 0) + { + if (impulse < -impulseClamp) + impulse = -impulseClamp; + if (impulse > impulseClamp) + impulse = impulseClamp; + } + + m_appliedImpulse+=impulse; + Vector3 impulse_vector = normal * impulse; + A->apply_impulse(pivotAInW - A->get_transform().origin,impulse_vector); + B->apply_impulse(pivotBInW - B->get_transform().origin,-impulse_vector); + + normal[i] = 0; + } +} + +void PinJointSW::set_param(PhysicsServer::PinJointParam p_param,float p_value) { + + switch(p_param) { + case PhysicsServer::PIN_JOINT_BIAS: m_tau=p_value; break; + case PhysicsServer::PIN_JOINT_DAMPING: m_damping=p_value; break; + case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP: m_impulseClamp=p_value; break; + } +} + +float PinJointSW::get_param(PhysicsServer::PinJointParam p_param) const{ + + switch(p_param) { + case PhysicsServer::PIN_JOINT_BIAS: return m_tau; + case PhysicsServer::PIN_JOINT_DAMPING: return m_damping; + case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP: return m_impulseClamp; + } + + return 0; +} + +PinJointSW::PinJointSW(BodySW* p_body_a,const Vector3& p_pos_a,BodySW* p_body_b,const Vector3& p_pos_b) : JointSW(_arr,2) { + + A=p_body_a; + B=p_body_b; + m_pivotInA=p_pos_a; + m_pivotInB=p_pos_b; + + m_tau=0.3; + m_damping=1; + m_impulseClamp=0; + m_appliedImpulse=0; + + A->add_constraint(this,0); + B->add_constraint(this,1); + + +} + +PinJointSW::~PinJointSW() { + + + +} diff --git a/servers/physics/joints/pin_joint_sw.h b/servers/physics/joints/pin_joint_sw.h new file mode 100644 index 0000000000..dae6e7d5f2 --- /dev/null +++ b/servers/physics/joints/pin_joint_sw.h @@ -0,0 +1,67 @@ +#ifndef PIN_JOINT_SW_H +#define PIN_JOINT_SW_H + +#include "servers/physics/joints_sw.h" +#include "servers/physics/joints/jacobian_entry_sw.h" + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +class PinJointSW : public JointSW { + + union { + struct { + BodySW *A; + BodySW *B; + }; + + BodySW *_arr[2]; + }; + + + real_t m_tau; //bias + real_t m_damping; + real_t m_impulseClamp; + real_t m_appliedImpulse; + + JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints + + Vector3 m_pivotInA; + Vector3 m_pivotInB; + +public: + + virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_PIN; } + + virtual bool setup(float p_step); + virtual void solve(float p_step); + + void set_param(PhysicsServer::PinJointParam p_param,float p_value); + float get_param(PhysicsServer::PinJointParam p_param) const; + + void set_pos_A(const Vector3& p_pos) { m_pivotInA=p_pos; } + void set_pos_B(const Vector3& p_pos) { m_pivotInB=p_pos; } + + Vector3 get_pos_A() { return m_pivotInB; } + Vector3 get_pos_B() { return m_pivotInA; } + + PinJointSW(BodySW* p_body_a,const Vector3& p_pos_a,BodySW* p_body_b,const Vector3& p_pos_b); + ~PinJointSW(); +}; + + + +#endif // PIN_JOINT_SW_H diff --git a/servers/physics/joints/slider_joint_sw.cpp b/servers/physics/joints/slider_joint_sw.cpp new file mode 100644 index 0000000000..faa6875378 --- /dev/null +++ b/servers/physics/joints/slider_joint_sw.cpp @@ -0,0 +1,439 @@ +#include "slider_joint_sw.h" + +//----------------------------------------------------------------------------- + +static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) +{ + real_t coeff_1 = Math_PI / 4.0f; + real_t coeff_2 = 3.0f * coeff_1; + real_t abs_y = Math::abs(y); + real_t angle; + if (x >= 0.0f) { + real_t r = (x - abs_y) / (x + abs_y); + angle = coeff_1 - coeff_1 * r; + } else { + real_t r = (x + abs_y) / (abs_y - x); + angle = coeff_2 - coeff_1 * r; + } + return (y < 0.0f) ? -angle : angle; +} + + +void SliderJointSW::initParams() +{ + m_lowerLinLimit = real_t(1.0); + m_upperLinLimit = real_t(-1.0); + m_lowerAngLimit = real_t(0.); + m_upperAngLimit = real_t(0.); + m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingDirLin = real_t(0.); + m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingDirAng = real_t(0.); + m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING; + m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING; + m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING; + m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING; + + m_poweredLinMotor = false; + m_targetLinMotorVelocity = real_t(0.); + m_maxLinMotorForce = real_t(0.); + m_accumulatedLinMotorImpulse = real_t(0.0); + + m_poweredAngMotor = false; + m_targetAngMotorVelocity = real_t(0.); + m_maxAngMotorForce = real_t(0.); + m_accumulatedAngMotorImpulse = real_t(0.0); + +} // SliderJointSW::initParams() + +//----------------------------------------------------------------------------- + + +//----------------------------------------------------------------------------- + +SliderJointSW::SliderJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB) + : JointSW(_arr,2) + , m_frameInA(frameInA) + , m_frameInB(frameInB) +{ + + A=rbA; + B=rbB; + + A->add_constraint(this,0); + B->add_constraint(this,1); + + initParams(); +} // SliderJointSW::SliderJointSW() + +//----------------------------------------------------------------------------- + +bool SliderJointSW::setup(float p_step) +{ + + //calculate transforms + m_calculatedTransformA = A->get_transform() * m_frameInA; + m_calculatedTransformB = B->get_transform() * m_frameInB; + m_realPivotAInW = m_calculatedTransformA.origin; + m_realPivotBInW = m_calculatedTransformB.origin; + m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X + m_delta = m_realPivotBInW - m_realPivotAInW; + m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; + m_relPosA = m_projPivotInW - A->get_transform().origin; + m_relPosB = m_realPivotBInW - B->get_transform().origin; + Vector3 normalWorld; + int i; + //linear part + for(i = 0; i < 3; i++) + { + normalWorld = m_calculatedTransformA.basis.get_axis(i); + memnew_placement(&m_jacLin[i], JacobianEntrySW( + A->get_transform().basis.transposed(), + B->get_transform().basis.transposed(), + m_relPosA, + m_relPosB, + normalWorld, + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass() + )); + m_jacLinDiagABInv[i] = real_t(1.) / m_jacLin[i].getDiagonal(); + m_depth[i] = m_delta.dot(normalWorld); + } + testLinLimits(); + // angular part + for(i = 0; i < 3; i++) + { + normalWorld = m_calculatedTransformA.basis.get_axis(i); + memnew_placement(&m_jacAng[i], JacobianEntrySW( + normalWorld, + A->get_transform().basis.transposed(), + B->get_transform().basis.transposed(), + A->get_inv_inertia(), + B->get_inv_inertia() + )); + } + testAngLimits(); + Vector3 axisA = m_calculatedTransformA.basis.get_axis(0); + m_kAngle = real_t(1.0 )/ (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA)); + // clear accumulator for motors + m_accumulatedLinMotorImpulse = real_t(0.0); + m_accumulatedAngMotorImpulse = real_t(0.0); + + return true; +} // SliderJointSW::buildJacobianInt() + +//----------------------------------------------------------------------------- + +void SliderJointSW::solve(real_t p_step) { + + int i; + // linear + Vector3 velA = A->get_velocity_in_local_point(m_relPosA); + Vector3 velB = B->get_velocity_in_local_point(m_relPosB); + Vector3 vel = velA - velB; + for(i = 0; i < 3; i++) + { + const Vector3& normal = m_jacLin[i].m_linearJointAxis; + real_t rel_vel = normal.dot(vel); + // calculate positional error + real_t depth = m_depth[i]; + // get parameters + real_t softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin); + real_t restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin); + real_t damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin); + // calcutate and apply impulse + real_t normalImpulse = softness * (restitution * depth / p_step - damping * rel_vel) * m_jacLinDiagABInv[i]; + Vector3 impulse_vector = normal * normalImpulse; + A->apply_impulse( m_relPosA, impulse_vector); + B->apply_impulse(m_relPosB,-impulse_vector); + if(m_poweredLinMotor && (!i)) + { // apply linear motor + if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce) + { + real_t desiredMotorVel = m_targetLinMotorVelocity; + real_t motor_relvel = desiredMotorVel + rel_vel; + normalImpulse = -motor_relvel * m_jacLinDiagABInv[i]; + // clamp accumulated impulse + real_t new_acc = m_accumulatedLinMotorImpulse + Math::abs(normalImpulse); + if(new_acc > m_maxLinMotorForce) + { + new_acc = m_maxLinMotorForce; + } + real_t del = new_acc - m_accumulatedLinMotorImpulse; + if(normalImpulse < real_t(0.0)) + { + normalImpulse = -del; + } + else + { + normalImpulse = del; + } + m_accumulatedLinMotorImpulse = new_acc; + // apply clamped impulse + impulse_vector = normal * normalImpulse; + A->apply_impulse( m_relPosA, impulse_vector); + B->apply_impulse( m_relPosB,-impulse_vector); + } + } + } + // angular + // get axes in world space + Vector3 axisA = m_calculatedTransformA.basis.get_axis(0); + Vector3 axisB = m_calculatedTransformB.basis.get_axis(0); + + const Vector3& angVelA = A->get_angular_velocity(); + const Vector3& angVelB = B->get_angular_velocity(); + + Vector3 angVelAroundAxisA = axisA * axisA.dot(angVelA); + Vector3 angVelAroundAxisB = axisB * axisB.dot(angVelB); + + Vector3 angAorthog = angVelA - angVelAroundAxisA; + Vector3 angBorthog = angVelB - angVelAroundAxisB; + Vector3 velrelOrthog = angAorthog-angBorthog; + //solve orthogonal angular velocity correction + real_t len = velrelOrthog.length(); + if (len > real_t(0.00001)) + { + Vector3 normal = velrelOrthog.normalized(); + real_t denom = A->compute_angular_impulse_denominator(normal) + B->compute_angular_impulse_denominator(normal); + velrelOrthog *= (real_t(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; + } + //solve angular positional correction + Vector3 angularError = axisA.cross(axisB) *(real_t(1.)/p_step); + real_t len2 = angularError.length(); + if (len2>real_t(0.00001)) + { + Vector3 normal2 = angularError.normalized(); + real_t denom2 = A->compute_angular_impulse_denominator(normal2) + B->compute_angular_impulse_denominator(normal2); + angularError *= (real_t(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; + } + // apply impulse + A->apply_torque_impulse(-velrelOrthog+angularError); + B->apply_torque_impulse(velrelOrthog-angularError); + real_t impulseMag; + //solve angular limits + if(m_solveAngLim) + { + impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / p_step; + impulseMag *= m_kAngle * m_softnessLimAng; + } + else + { + impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / p_step; + impulseMag *= m_kAngle * m_softnessDirAng; + } + Vector3 impulse = axisA * impulseMag; + A->apply_torque_impulse(impulse); + B->apply_torque_impulse(-impulse); + //apply angular motor + if(m_poweredAngMotor) + { + if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce) + { + Vector3 velrel = angVelAroundAxisA - angVelAroundAxisB; + real_t projRelVel = velrel.dot(axisA); + + real_t desiredMotorVel = m_targetAngMotorVelocity; + real_t motor_relvel = desiredMotorVel - projRelVel; + + real_t angImpulse = m_kAngle * motor_relvel; + // clamp accumulated impulse + real_t new_acc = m_accumulatedAngMotorImpulse + Math::abs(angImpulse); + if(new_acc > m_maxAngMotorForce) + { + new_acc = m_maxAngMotorForce; + } + real_t del = new_acc - m_accumulatedAngMotorImpulse; + if(angImpulse < real_t(0.0)) + { + angImpulse = -del; + } + else + { + angImpulse = del; + } + m_accumulatedAngMotorImpulse = new_acc; + // apply clamped impulse + Vector3 motorImp = angImpulse * axisA; + A->apply_torque_impulse(motorImp); + B->apply_torque_impulse(-motorImp); + } + } +} // SliderJointSW::solveConstraint() + +//----------------------------------------------------------------------------- + +//----------------------------------------------------------------------------- + +void SliderJointSW::calculateTransforms(void){ + m_calculatedTransformA = A->get_transform() * m_frameInA ; + m_calculatedTransformB = B->get_transform() * m_frameInB; + m_realPivotAInW = m_calculatedTransformA.origin; + m_realPivotBInW = m_calculatedTransformB.origin; + m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X + m_delta = m_realPivotBInW - m_realPivotAInW; + m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; + Vector3 normalWorld; + int i; + //linear part + for(i = 0; i < 3; i++) + { + normalWorld = m_calculatedTransformA.basis.get_axis(i); + m_depth[i] = m_delta.dot(normalWorld); + } +} // SliderJointSW::calculateTransforms() + +//----------------------------------------------------------------------------- + +void SliderJointSW::testLinLimits(void) +{ + m_solveLinLim = false; + m_linPos = m_depth[0]; + if(m_lowerLinLimit <= m_upperLinLimit) + { + if(m_depth[0] > m_upperLinLimit) + { + m_depth[0] -= m_upperLinLimit; + m_solveLinLim = true; + } + else if(m_depth[0] < m_lowerLinLimit) + { + m_depth[0] -= m_lowerLinLimit; + m_solveLinLim = true; + } + else + { + m_depth[0] = real_t(0.); + } + } + else + { + m_depth[0] = real_t(0.); + } +} // SliderJointSW::testLinLimits() + +//----------------------------------------------------------------------------- + + +void SliderJointSW::testAngLimits(void) +{ + m_angDepth = real_t(0.); + m_solveAngLim = false; + if(m_lowerAngLimit <= m_upperAngLimit) + { + const Vector3 axisA0 = m_calculatedTransformA.basis.get_axis(1); + const Vector3 axisA1 = m_calculatedTransformA.basis.get_axis(2); + const Vector3 axisB0 = m_calculatedTransformB.basis.get_axis(1); + real_t rot = atan2fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); + if(rot < m_lowerAngLimit) + { + m_angDepth = rot - m_lowerAngLimit; + m_solveAngLim = true; + } + else if(rot > m_upperAngLimit) + { + m_angDepth = rot - m_upperAngLimit; + m_solveAngLim = true; + } + } +} // SliderJointSW::testAngLimits() + + +//----------------------------------------------------------------------------- + + + +Vector3 SliderJointSW::getAncorInA(void) +{ + Vector3 ancorInA; + ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * real_t(0.5) * m_sliderAxis; + ancorInA = A->get_transform().inverse().xform( ancorInA ); + return ancorInA; +} // SliderJointSW::getAncorInA() + +//----------------------------------------------------------------------------- + +Vector3 SliderJointSW::getAncorInB(void) +{ + Vector3 ancorInB; + ancorInB = m_frameInB.origin; + return ancorInB; +} // SliderJointSW::getAncorInB(); + +void SliderJointSW::set_param(PhysicsServer::SliderJointParam p_param, float p_value) { + + switch(p_param) { + case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: m_upperLinLimit=p_value; break; + case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: m_lowerLinLimit=p_value; break; + case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: m_softnessLimLin=p_value; break; + case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: m_restitutionLimLin=p_value; break; + case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: m_dampingLimLin=p_value; break; + case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: m_softnessDirLin=p_value; break; + case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: m_restitutionDirLin=p_value; break; + case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING: m_dampingDirLin=p_value; break; + case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: m_softnessOrthoLin=p_value; break; + case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: m_restitutionOrthoLin=p_value; break; + case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: m_dampingOrthoLin=p_value; break; + + case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: m_upperAngLimit=p_value; break; + case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: m_lowerAngLimit=p_value; break; + case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: m_softnessLimAng=p_value; break; + case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: m_restitutionLimAng=p_value; break; + case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: m_dampingLimAng=p_value; break; + case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: m_softnessDirAng=p_value; break; + case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: m_restitutionDirAng=p_value; break; + case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: m_dampingDirAng=p_value; break; + case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: m_softnessOrthoAng=p_value; break; + case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: m_restitutionOrthoAng=p_value; break; + case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: m_dampingOrthoAng=p_value; break; + + } + +} + +float SliderJointSW::get_param(PhysicsServer::SliderJointParam p_param) const { + + switch(p_param) { + case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: return m_upperLinLimit; + case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: return m_lowerLinLimit; + case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: return m_softnessLimLin; + case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: return m_restitutionLimLin; + case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: return m_dampingLimLin; + case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: return m_softnessDirLin; + case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: return m_restitutionDirLin; + case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING: return m_dampingDirLin; + case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: return m_softnessOrthoLin; + case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: return m_restitutionOrthoLin; + case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: return m_dampingOrthoLin; + + case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: return m_upperAngLimit; + case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: return m_lowerAngLimit; + case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: return m_softnessLimAng; + case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: return m_restitutionLimAng; + case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: return m_dampingLimAng; + case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: return m_softnessDirAng; + case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: return m_restitutionDirAng; + case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: return m_dampingDirAng; + case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: return m_softnessOrthoAng; + case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: return m_restitutionOrthoAng; + case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: return m_dampingOrthoAng; + + } + + return 0; + +} + + diff --git a/servers/physics/joints/slider_joint_sw.h b/servers/physics/joints/slider_joint_sw.h new file mode 100644 index 0000000000..517bb5e6bc --- /dev/null +++ b/servers/physics/joints/slider_joint_sw.h @@ -0,0 +1,218 @@ +#ifndef SLIDER_JOINT_SW_H +#define SLIDER_JOINT_SW_H + +#include "servers/physics/joints_sw.h" +#include "servers/physics/joints/jacobian_entry_sw.h" + + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +Added by Roman Ponomarev (rponom@gmail.com) +April 04, 2008 + +*/ + +#define SLIDER_CONSTRAINT_DEF_SOFTNESS (real_t(1.0)) +#define SLIDER_CONSTRAINT_DEF_DAMPING (real_t(1.0)) +#define SLIDER_CONSTRAINT_DEF_RESTITUTION (real_t(0.7)) + +//----------------------------------------------------------------------------- + +class SliderJointSW : public JointSW { +protected: + + union { + struct { + BodySW *A; + BodySW *B; + }; + + BodySW *_arr[2]; + }; + + Transform m_frameInA; + Transform m_frameInB; + + // linear limits + real_t m_lowerLinLimit; + real_t m_upperLinLimit; + // angular limits + real_t m_lowerAngLimit; + real_t m_upperAngLimit; + // softness, restitution and damping for different cases + // DirLin - moving inside linear limits + // LimLin - hitting linear limit + // DirAng - moving inside angular limits + // LimAng - hitting angular limit + // OrthoLin, OrthoAng - against constraint axis + real_t m_softnessDirLin; + real_t m_restitutionDirLin; + real_t m_dampingDirLin; + real_t m_softnessDirAng; + real_t m_restitutionDirAng; + real_t m_dampingDirAng; + real_t m_softnessLimLin; + real_t m_restitutionLimLin; + real_t m_dampingLimLin; + real_t m_softnessLimAng; + real_t m_restitutionLimAng; + real_t m_dampingLimAng; + real_t m_softnessOrthoLin; + real_t m_restitutionOrthoLin; + real_t m_dampingOrthoLin; + real_t m_softnessOrthoAng; + real_t m_restitutionOrthoAng; + real_t m_dampingOrthoAng; + + // for interlal use + bool m_solveLinLim; + bool m_solveAngLim; + + JacobianEntrySW m_jacLin[3]; + real_t m_jacLinDiagABInv[3]; + + JacobianEntrySW m_jacAng[3]; + + real_t m_timeStep; + Transform m_calculatedTransformA; + Transform m_calculatedTransformB; + + Vector3 m_sliderAxis; + Vector3 m_realPivotAInW; + Vector3 m_realPivotBInW; + Vector3 m_projPivotInW; + Vector3 m_delta; + Vector3 m_depth; + Vector3 m_relPosA; + Vector3 m_relPosB; + + real_t m_linPos; + + real_t m_angDepth; + real_t m_kAngle; + + bool m_poweredLinMotor; + real_t m_targetLinMotorVelocity; + real_t m_maxLinMotorForce; + real_t m_accumulatedLinMotorImpulse; + + bool m_poweredAngMotor; + real_t m_targetAngMotorVelocity; + real_t m_maxAngMotorForce; + real_t m_accumulatedAngMotorImpulse; + + //------------------------ + void initParams(); +public: + // constructors + SliderJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB); + //SliderJointSW(); + // overrides + + // access + const BodySW* getRigidBodyA() const { return A; } + const BodySW* getRigidBodyB() const { return B; } + const Transform & getCalculatedTransformA() const { return m_calculatedTransformA; } + const Transform & getCalculatedTransformB() const { return m_calculatedTransformB; } + const Transform & getFrameOffsetA() const { return m_frameInA; } + const Transform & getFrameOffsetB() const { return m_frameInB; } + Transform & getFrameOffsetA() { return m_frameInA; } + Transform & getFrameOffsetB() { return m_frameInB; } + real_t getLowerLinLimit() { return m_lowerLinLimit; } + void setLowerLinLimit(real_t lowerLimit) { m_lowerLinLimit = lowerLimit; } + real_t getUpperLinLimit() { return m_upperLinLimit; } + void setUpperLinLimit(real_t upperLimit) { m_upperLinLimit = upperLimit; } + real_t getLowerAngLimit() { return m_lowerAngLimit; } + void setLowerAngLimit(real_t lowerLimit) { m_lowerAngLimit = lowerLimit; } + real_t getUpperAngLimit() { return m_upperAngLimit; } + void setUpperAngLimit(real_t upperLimit) { m_upperAngLimit = upperLimit; } + + real_t getSoftnessDirLin() { return m_softnessDirLin; } + real_t getRestitutionDirLin() { return m_restitutionDirLin; } + real_t getDampingDirLin() { return m_dampingDirLin ; } + real_t getSoftnessDirAng() { return m_softnessDirAng; } + real_t getRestitutionDirAng() { return m_restitutionDirAng; } + real_t getDampingDirAng() { return m_dampingDirAng; } + real_t getSoftnessLimLin() { return m_softnessLimLin; } + real_t getRestitutionLimLin() { return m_restitutionLimLin; } + real_t getDampingLimLin() { return m_dampingLimLin; } + real_t getSoftnessLimAng() { return m_softnessLimAng; } + real_t getRestitutionLimAng() { return m_restitutionLimAng; } + real_t getDampingLimAng() { return m_dampingLimAng; } + real_t getSoftnessOrthoLin() { return m_softnessOrthoLin; } + real_t getRestitutionOrthoLin() { return m_restitutionOrthoLin; } + real_t getDampingOrthoLin() { return m_dampingOrthoLin; } + real_t getSoftnessOrthoAng() { return m_softnessOrthoAng; } + real_t getRestitutionOrthoAng() { return m_restitutionOrthoAng; } + real_t getDampingOrthoAng() { return m_dampingOrthoAng; } + void setSoftnessDirLin(real_t softnessDirLin) { m_softnessDirLin = softnessDirLin; } + void setRestitutionDirLin(real_t restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; } + void setDampingDirLin(real_t dampingDirLin) { m_dampingDirLin = dampingDirLin; } + void setSoftnessDirAng(real_t softnessDirAng) { m_softnessDirAng = softnessDirAng; } + void setRestitutionDirAng(real_t restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; } + void setDampingDirAng(real_t dampingDirAng) { m_dampingDirAng = dampingDirAng; } + void setSoftnessLimLin(real_t softnessLimLin) { m_softnessLimLin = softnessLimLin; } + void setRestitutionLimLin(real_t restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; } + void setDampingLimLin(real_t dampingLimLin) { m_dampingLimLin = dampingLimLin; } + void setSoftnessLimAng(real_t softnessLimAng) { m_softnessLimAng = softnessLimAng; } + void setRestitutionLimAng(real_t restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; } + void setDampingLimAng(real_t dampingLimAng) { m_dampingLimAng = dampingLimAng; } + void setSoftnessOrthoLin(real_t softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; } + void setRestitutionOrthoLin(real_t restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; } + void setDampingOrthoLin(real_t dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; } + void setSoftnessOrthoAng(real_t softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; } + void setRestitutionOrthoAng(real_t restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; } + void setDampingOrthoAng(real_t dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; } + void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; } + bool getPoweredLinMotor() { return m_poweredLinMotor; } + void setTargetLinMotorVelocity(real_t targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; } + real_t getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; } + void setMaxLinMotorForce(real_t maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; } + real_t getMaxLinMotorForce() { return m_maxLinMotorForce; } + void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; } + bool getPoweredAngMotor() { return m_poweredAngMotor; } + void setTargetAngMotorVelocity(real_t targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; } + real_t getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; } + void setMaxAngMotorForce(real_t maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; } + real_t getMaxAngMotorForce() { return m_maxAngMotorForce; } + real_t getLinearPos() { return m_linPos; } + + // access for ODE solver + bool getSolveLinLimit() { return m_solveLinLim; } + real_t getLinDepth() { return m_depth[0]; } + bool getSolveAngLimit() { return m_solveAngLim; } + real_t getAngDepth() { return m_angDepth; } + // shared code used by ODE solver + void calculateTransforms(void); + void testLinLimits(void); + void testAngLimits(void); + // access for PE Solver + Vector3 getAncorInA(void); + Vector3 getAncorInB(void); + + void set_param(PhysicsServer::SliderJointParam p_param, float p_value); + float get_param(PhysicsServer::SliderJointParam p_param) const; + + bool setup(float p_step); + void solve(float p_step); + + virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_SLIDER; } + +}; + + +#endif // SLIDER_JOINT_SW_H diff --git a/servers/physics/joints_sw.h b/servers/physics/joints_sw.h index c10568fb52..30227f156b 100644 --- a/servers/physics/joints_sw.h +++ b/servers/physics/joints_sw.h @@ -36,22 +36,12 @@ class JointSW : public ConstraintSW { - real_t max_force; - real_t bias; - real_t max_bias; -public: - - _FORCE_INLINE_ void set_max_force(real_t p_force) { max_force=p_force; } - _FORCE_INLINE_ real_t get_max_force() const { return max_force; } - _FORCE_INLINE_ void set_bias(real_t p_bias) { bias=p_bias; } - _FORCE_INLINE_ real_t get_bias() const { return bias; } - - _FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias=p_bias; } - _FORCE_INLINE_ real_t get_max_bias() const { return max_bias; } +public: -// virtual PhysicsServer::JointType get_type() const=0; - JointSW(BodySW **p_body_ptr=NULL,int p_body_count=0) : ConstraintSW(p_body_ptr,p_body_count) { bias=0; max_force=max_bias=3.40282e+38; }; + virtual PhysicsServer::JointType get_type() const=0; + _FORCE_INLINE_ JointSW(BodySW **p_body_ptr=NULL,int p_body_count=0) : ConstraintSW(p_body_ptr,p_body_count) { + } }; diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp index aff60b5881..043d2149fe 100644 --- a/servers/physics/physics_server_sw.cpp +++ b/servers/physics/physics_server_sw.cpp @@ -29,6 +29,12 @@ #include "physics_server_sw.h" #include "broad_phase_basic.h" #include "broad_phase_octree.h" +#include "joints/pin_joint_sw.h" +#include "joints/hinge_joint_sw.h" +#include "joints/slider_joint_sw.h" +#include "joints/cone_twist_joint_sw.h" +#include "joints/generic_6dof_joint_sw.h" + RID PhysicsServerSW::shape_create(ShapeType p_shape) { @@ -137,6 +143,10 @@ RID PhysicsServerSW::space_create() { space->set_default_area(area); area->set_space(space); area->set_priority(-1); + RID sgb = body_create(); + body_set_space(sgb,id); + body_set_mode(sgb,BODY_MODE_STATIC); + space->set_static_global_body(sgb); return id; }; @@ -394,6 +404,25 @@ void PhysicsServerSW::area_set_monitor_callback(RID p_area,Object *p_receiver,co } +void PhysicsServerSW::area_set_ray_pickable(RID p_area,bool p_enable) { + + AreaSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + + area->set_ray_pickable(p_enable); + +} + +bool PhysicsServerSW::area_is_ray_pickable(RID p_area) const{ + + AreaSW *area = area_owner.get(p_area); + ERR_FAIL_COND_V(!area,false); + + return area->is_ray_pickable(); + +} + + /* BODY API */ @@ -568,6 +597,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 +666,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) { @@ -798,6 +839,308 @@ void PhysicsServerSW::body_set_force_integration_callback(RID p_body,Object *p_r /* JOINT API */ +RID PhysicsServerSW::joint_create_pin(RID p_body_A,const Vector3& p_local_A,RID p_body_B,const Vector3& p_local_B) { + + BodySW *body_A = body_owner.get(p_body_A); + ERR_FAIL_COND_V(!body_A,RID()); + + if (!p_body_B.is_valid()) { + ERR_FAIL_COND_V(!body_A->get_space(),RID()); + p_body_B=body_A->get_space()->get_static_global_body(); + } + + BodySW *body_B = body_owner.get(p_body_B); + ERR_FAIL_COND_V(!body_B,RID()); + + ERR_FAIL_COND_V(body_A==body_B,RID()); + + JointSW *joint = memnew( PinJointSW(body_A,p_local_A,body_B,p_local_B) ); + RID rid = joint_owner.make_rid(joint); + joint->set_self(rid); + return rid; +} + +void PhysicsServerSW::pin_joint_set_param(RID p_joint,PinJointParam p_param, float p_value){ + + JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type()!=JOINT_PIN); + PinJointSW *pin_joint = static_cast<PinJointSW*>(joint); + pin_joint->set_param(p_param,p_value); + +} +float PhysicsServerSW::pin_joint_get_param(RID p_joint,PinJointParam p_param) const{ + + JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND_V(!joint,0); + ERR_FAIL_COND_V(joint->get_type()!=JOINT_PIN,0); + PinJointSW *pin_joint = static_cast<PinJointSW*>(joint); + return pin_joint->get_param(p_param); + +} + +void PhysicsServerSW::pin_joint_set_local_A(RID p_joint, const Vector3& p_A){ + + JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type()!=JOINT_PIN); + PinJointSW *pin_joint = static_cast<PinJointSW*>(joint); + pin_joint->set_pos_A(p_A); + +} +Vector3 PhysicsServerSW::pin_joint_get_local_A(RID p_joint) const{ + + JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND_V(!joint,Vector3()); + ERR_FAIL_COND_V(joint->get_type()!=JOINT_PIN,Vector3()); + PinJointSW *pin_joint = static_cast<PinJointSW*>(joint); + return pin_joint->get_pos_A(); + +} + +void PhysicsServerSW::pin_joint_set_local_B(RID p_joint, const Vector3& p_B){ + + JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type()!=JOINT_PIN); + PinJointSW *pin_joint = static_cast<PinJointSW*>(joint); + pin_joint->set_pos_B(p_B); + +} +Vector3 PhysicsServerSW::pin_joint_get_local_B(RID p_joint) const{ + + JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND_V(!joint,Vector3()); + ERR_FAIL_COND_V(joint->get_type()!=JOINT_PIN,Vector3()); + PinJointSW *pin_joint = static_cast<PinJointSW*>(joint); + return pin_joint->get_pos_B(); + +} + + +RID PhysicsServerSW::joint_create_hinge(RID p_body_A,const Transform& p_frame_A,RID p_body_B,const Transform& p_frame_B) { + + BodySW *body_A = body_owner.get(p_body_A); + ERR_FAIL_COND_V(!body_A,RID()); + + if (!p_body_B.is_valid()) { + ERR_FAIL_COND_V(!body_A->get_space(),RID()); + p_body_B=body_A->get_space()->get_static_global_body(); + } + + BodySW *body_B = body_owner.get(p_body_B); + ERR_FAIL_COND_V(!body_B,RID()); + + ERR_FAIL_COND_V(body_A==body_B,RID()); + + JointSW *joint = memnew( HingeJointSW(body_A,body_B,p_frame_A,p_frame_B) ); + RID rid = joint_owner.make_rid(joint); + joint->set_self(rid); + return rid; + +} + + +RID PhysicsServerSW::joint_create_hinge_simple(RID p_body_A,const Vector3& p_pivot_A,const Vector3& p_axis_A,RID p_body_B,const Vector3& p_pivot_B,const Vector3& p_axis_B) { + + BodySW *body_A = body_owner.get(p_body_A); + ERR_FAIL_COND_V(!body_A,RID()); + + if (!p_body_B.is_valid()) { + ERR_FAIL_COND_V(!body_A->get_space(),RID()); + p_body_B=body_A->get_space()->get_static_global_body(); + } + + BodySW *body_B = body_owner.get(p_body_B); + ERR_FAIL_COND_V(!body_B,RID()); + + ERR_FAIL_COND_V(body_A==body_B,RID()); + + JointSW *joint = memnew( HingeJointSW(body_A,body_B,p_pivot_A,p_pivot_B,p_axis_A,p_axis_B) ); + RID rid = joint_owner.make_rid(joint); + joint->set_self(rid); + return rid; + +} + +void PhysicsServerSW::hinge_joint_set_param(RID p_joint,HingeJointParam p_param, float p_value){ + + JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type()!=JOINT_HINGE); + HingeJointSW *hinge_joint = static_cast<HingeJointSW*>(joint); + hinge_joint->set_param(p_param,p_value); + +} +float PhysicsServerSW::hinge_joint_get_param(RID p_joint,HingeJointParam p_param) const{ + + JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND_V(!joint,0); + ERR_FAIL_COND_V(joint->get_type()!=JOINT_HINGE,0); + HingeJointSW *hinge_joint = static_cast<HingeJointSW*>(joint); + return hinge_joint->get_param(p_param); + +} + +void PhysicsServerSW::hinge_joint_set_flag(RID p_joint,HingeJointFlag p_flag, bool p_value){ + + JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type()!=JOINT_HINGE); + HingeJointSW *hinge_joint = static_cast<HingeJointSW*>(joint); + hinge_joint->set_flag(p_flag,p_value); + +} +bool PhysicsServerSW::hinge_joint_get_flag(RID p_joint,HingeJointFlag p_flag) const{ + + JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND_V(!joint,false); + ERR_FAIL_COND_V(joint->get_type()!=JOINT_HINGE,false); + HingeJointSW *hinge_joint = static_cast<HingeJointSW*>(joint); + return hinge_joint->get_flag(p_flag); +} + +PhysicsServerSW::JointType PhysicsServerSW::joint_get_type(RID p_joint) const { + + JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND_V(!joint,JOINT_PIN); + return joint->get_type(); +} + +RID PhysicsServerSW::joint_create_slider(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B) { + + BodySW *body_A = body_owner.get(p_body_A); + ERR_FAIL_COND_V(!body_A,RID()); + + if (!p_body_B.is_valid()) { + ERR_FAIL_COND_V(!body_A->get_space(),RID()); + p_body_B=body_A->get_space()->get_static_global_body(); + } + + BodySW *body_B = body_owner.get(p_body_B); + ERR_FAIL_COND_V(!body_B,RID()); + + ERR_FAIL_COND_V(body_A==body_B,RID()); + + JointSW *joint = memnew( SliderJointSW(body_A,body_B,p_local_frame_A,p_local_frame_B) ); + RID rid = joint_owner.make_rid(joint); + joint->set_self(rid); + return rid; +} + +void PhysicsServerSW::slider_joint_set_param(RID p_joint,SliderJointParam p_param, float p_value){ + + JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type()!=JOINT_SLIDER); + SliderJointSW *slider_joint = static_cast<SliderJointSW*>(joint); + slider_joint->set_param(p_param,p_value); +} +float PhysicsServerSW::slider_joint_get_param(RID p_joint,SliderJointParam p_param) const{ + + JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND_V(!joint,0); + ERR_FAIL_COND_V(joint->get_type()!=JOINT_CONE_TWIST,0); + SliderJointSW *slider_joint = static_cast<SliderJointSW*>(joint); + return slider_joint->get_param(p_param); +} + + +RID PhysicsServerSW::joint_create_cone_twist(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B) { + + BodySW *body_A = body_owner.get(p_body_A); + ERR_FAIL_COND_V(!body_A,RID()); + + if (!p_body_B.is_valid()) { + ERR_FAIL_COND_V(!body_A->get_space(),RID()); + p_body_B=body_A->get_space()->get_static_global_body(); + } + + BodySW *body_B = body_owner.get(p_body_B); + ERR_FAIL_COND_V(!body_B,RID()); + + ERR_FAIL_COND_V(body_A==body_B,RID()); + + JointSW *joint = memnew( ConeTwistJointSW(body_A,body_B,p_local_frame_A,p_local_frame_B) ); + RID rid = joint_owner.make_rid(joint); + joint->set_self(rid); + return rid; +} + +void PhysicsServerSW::cone_twist_joint_set_param(RID p_joint,ConeTwistJointParam p_param, float p_value) { + + JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type()!=JOINT_CONE_TWIST); + ConeTwistJointSW *cone_twist_joint = static_cast<ConeTwistJointSW*>(joint); + cone_twist_joint->set_param(p_param,p_value); +} +float PhysicsServerSW::cone_twist_joint_get_param(RID p_joint,ConeTwistJointParam p_param) const { + + JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND_V(!joint,0); + ERR_FAIL_COND_V(joint->get_type()!=JOINT_CONE_TWIST,0); + ConeTwistJointSW *cone_twist_joint = static_cast<ConeTwistJointSW*>(joint); + return cone_twist_joint->get_param(p_param); +} + + +RID PhysicsServerSW::joint_create_generic_6dof(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B) { + + BodySW *body_A = body_owner.get(p_body_A); + ERR_FAIL_COND_V(!body_A,RID()); + + if (!p_body_B.is_valid()) { + ERR_FAIL_COND_V(!body_A->get_space(),RID()); + p_body_B=body_A->get_space()->get_static_global_body(); + } + + BodySW *body_B = body_owner.get(p_body_B); + ERR_FAIL_COND_V(!body_B,RID()); + + ERR_FAIL_COND_V(body_A==body_B,RID()); + + JointSW *joint = memnew( Generic6DOFJointSW(body_A,body_B,p_local_frame_A,p_local_frame_B,true) ); + RID rid = joint_owner.make_rid(joint); + joint->set_self(rid); + return rid; +} + +void PhysicsServerSW::generic_6dof_joint_set_param(RID p_joint,Vector3::Axis p_axis,G6DOFJointAxisParam p_param, float p_value){ + + JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type()!=JOINT_6DOF); + Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW*>(joint); + generic_6dof_joint->set_param(p_axis,p_param,p_value); +} +float PhysicsServerSW::generic_6dof_joint_get_param(RID p_joint,Vector3::Axis p_axis,G6DOFJointAxisParam p_param){ + + JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND_V(!joint,0); + ERR_FAIL_COND_V(joint->get_type()!=JOINT_6DOF,0); + Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW*>(joint); + return generic_6dof_joint->get_param(p_axis,p_param); +} + +void PhysicsServerSW::generic_6dof_joint_set_flag(RID p_joint,Vector3::Axis p_axis,G6DOFJointAxisFlag p_flag, bool p_enable){ + + JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND(!joint); + ERR_FAIL_COND(joint->get_type()!=JOINT_6DOF); + Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW*>(joint); + generic_6dof_joint->set_flag(p_axis,p_flag,p_enable); +} +bool PhysicsServerSW::generic_6dof_joint_get_flag(RID p_joint,Vector3::Axis p_axis,G6DOFJointAxisFlag p_flag){ + + JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND_V(!joint,false); + ERR_FAIL_COND_V(joint->get_type()!=JOINT_6DOF,false); + Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW*>(joint); + return generic_6dof_joint->get_flag(p_axis,p_flag); +} + + #if 0 void PhysicsServerSW::joint_set_param(RID p_joint, JointParam p_param, real_t p_value) { @@ -976,12 +1319,18 @@ void PhysicsServerSW::free(RID p_rid) { active_spaces.erase(space); free(space->get_default_area()->get_self()); + free(space->get_static_global_body()); + space_owner.free(p_rid); memdelete(space); } else if (joint_owner.owns(p_rid)) { JointSW *joint = joint_owner.get(p_rid); + for(int i=0;i<joint->get_body_count();i++) { + + joint->get_body_ptr()[i]->remove_constraint(joint); + } joint_owner.free(p_rid); memdelete(joint); @@ -1021,11 +1370,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 +1410,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..76e5aecf55 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); @@ -112,6 +125,9 @@ public: virtual Variant area_get_param(RID p_parea,AreaParameter p_param) const; virtual Transform area_get_transform(RID p_area) const; + virtual void area_set_ray_pickable(RID p_area,bool p_enable); + virtual bool area_is_ray_pickable(RID p_area) const; + virtual void area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method); @@ -146,15 +162,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; @@ -186,6 +202,48 @@ public: virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant()); /* JOINT API */ + + virtual RID joint_create_pin(RID p_body_A,const Vector3& p_local_A,RID p_body_B,const Vector3& p_local_B); + + virtual void pin_joint_set_param(RID p_joint,PinJointParam p_param, float p_value); + virtual float pin_joint_get_param(RID p_joint,PinJointParam p_param) const; + + virtual void pin_joint_set_local_A(RID p_joint, const Vector3& p_A); + virtual Vector3 pin_joint_get_local_A(RID p_joint) const; + + virtual void pin_joint_set_local_B(RID p_joint, const Vector3& p_B); + virtual Vector3 pin_joint_get_local_B(RID p_joint) const; + + virtual RID joint_create_hinge(RID p_body_A,const Transform& p_frame_A,RID p_body_B,const Transform& p_frame_B); + virtual RID joint_create_hinge_simple(RID p_body_A,const Vector3& p_pivot_A,const Vector3& p_axis_A,RID p_body_B,const Vector3& p_pivot_B,const Vector3& p_axis_B); + + virtual void hinge_joint_set_param(RID p_joint,HingeJointParam p_param, float p_value); + virtual float hinge_joint_get_param(RID p_joint,HingeJointParam p_param) const; + + virtual void hinge_joint_set_flag(RID p_joint,HingeJointFlag p_flag, bool p_value); + virtual bool hinge_joint_get_flag(RID p_joint,HingeJointFlag p_flag) const; + + + virtual RID joint_create_slider(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B); //reference frame is A + + virtual void slider_joint_set_param(RID p_joint,SliderJointParam p_param, float p_value); + virtual float slider_joint_get_param(RID p_joint,SliderJointParam p_param) const; + + virtual RID joint_create_cone_twist(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B); //reference frame is A + + virtual void cone_twist_joint_set_param(RID p_joint,ConeTwistJointParam p_param, float p_value); + virtual float cone_twist_joint_get_param(RID p_joint,ConeTwistJointParam p_param) const; + + virtual RID joint_create_generic_6dof(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B); //reference frame is A + + virtual void generic_6dof_joint_set_param(RID p_joint,Vector3::Axis,G6DOFJointAxisParam p_param, float p_value); + virtual float generic_6dof_joint_get_param(RID p_joint,Vector3::Axis,G6DOFJointAxisParam p_param); + + virtual void generic_6dof_joint_set_flag(RID p_joint,Vector3::Axis,G6DOFJointAxisFlag p_flag, bool p_enable); + virtual bool generic_6dof_joint_get_flag(RID p_joint,Vector3::Axis,G6DOFJointAxisFlag p_flag); + + virtual JointType joint_get_type(RID p_joint) const; + #if 0 virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value); virtual real_t joint_get_param(RID p_joint,JointParam p_param) const; @@ -209,6 +267,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..ca3eea364a 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,11 @@ 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 (space->intersection_query_results[i]->get_type()==CollisionObjectSW::TYPE_AREA && !(static_cast<AreaSW*>(space->intersection_query_results[i])->is_ray_pickable()))
+ continue;
if (p_exclude.has( space->intersection_query_results[i]->get_self()))
continue;
@@ -76,6 +94,7 @@ bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3& p_from, const Vecto Vector3 shape_point,shape_normal;
+
if (shape->intersect_segment(local_from,local_to,shape_point,shape_normal)) {
Transform xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
@@ -114,7 +133,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 +155,10 @@ 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;
+
+ //area cant be picked by ray (default)
if (p_exclude.has( space->intersection_query_results[i]->get_self()))
continue;
@@ -146,7 +167,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 +184,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 +492,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 +521,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 +699,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..4bd9bc6f51 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,12 @@ class SpaceSW { bool locked;
+ int island_count;
+ int active_objects;
+ int collision_pairs;
+
+ RID static_global_body;
+
friend class PhysicsDirectSpaceStateSW;
public:
@@ -147,8 +156,21 @@ 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();
+
+ void set_static_global_body(RID p_body) { static_global_body=p_body; }
+ RID get_static_global_body() { return static_global_body; }
+
+
SpaceSW();
~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 */ |