diff options
Diffstat (limited to 'scene/3d')
28 files changed, 4733 insertions, 147 deletions
diff --git a/scene/3d/area.cpp b/scene/3d/area.cpp index 964a086cf6..f5895453cc 100644 --- a/scene/3d/area.cpp +++ b/scene/3d/area.cpp @@ -255,6 +255,17 @@ bool Area::is_monitoring_enabled() const { } +void Area::set_ray_pickable(bool p_ray_pickable) { + + ray_pickable=p_ray_pickable; + PhysicsServer::get_singleton()->area_set_ray_pickable(get_rid(),p_ray_pickable); +} + +bool Area::is_ray_pickable() const { + + return ray_pickable; +} + void Area::_bind_methods() { ObjectTypeDB::bind_method(_MD("_body_enter_scene","id"),&Area::_body_enter_scene); @@ -278,6 +289,9 @@ void Area::_bind_methods() { ObjectTypeDB::bind_method(_MD("set_priority","priority"),&Area::set_priority); ObjectTypeDB::bind_method(_MD("get_priority"),&Area::get_priority); + ObjectTypeDB::bind_method(_MD("set_ray_pickable","ray_pickable"),&Area::set_ray_pickable); + ObjectTypeDB::bind_method(_MD("is_ray_pickable"),&Area::is_ray_pickable); + ObjectTypeDB::bind_method(_MD("set_enable_monitoring","enable"),&Area::set_enable_monitoring); ObjectTypeDB::bind_method(_MD("is_monitoring_enabled"),&Area::is_monitoring_enabled); @@ -296,6 +310,7 @@ void Area::_bind_methods() { ADD_PROPERTY( PropertyInfo(Variant::REAL,"density",PROPERTY_HINT_RANGE,"0,1024,0.001"),_SCS("set_density"),_SCS("get_density")); ADD_PROPERTY( PropertyInfo(Variant::INT,"priority",PROPERTY_HINT_RANGE,"0,128,1"),_SCS("set_priority"),_SCS("get_priority")); ADD_PROPERTY( PropertyInfo(Variant::BOOL,"monitoring"),_SCS("set_enable_monitoring"),_SCS("is_monitoring_enabled")); + ADD_PROPERTY( PropertyInfo(Variant::BOOL,"ray_pickable"),_SCS("set_ray_pickable"),_SCS("is_ray_pickable")); } @@ -308,6 +323,8 @@ Area::Area() : CollisionObject(PhysicsServer::get_singleton()->area_create(),tru density=0.1; priority=0; monitoring=false; + ray_pickable=false; + set_enable_monitoring(true); } diff --git a/scene/3d/area.h b/scene/3d/area.h index 79e98f9dab..92b5d39f59 100644 --- a/scene/3d/area.h +++ b/scene/3d/area.h @@ -52,6 +52,7 @@ private: real_t density; int priority; bool monitoring; + bool ray_pickable; void _body_inout(int p_status,const RID& p_body, int p_instance, int p_body_shape,int p_area_shape); @@ -108,6 +109,9 @@ public: void set_priority(real_t p_priority); real_t get_priority() const; + void set_ray_pickable(bool p_ray_pickable); + bool is_ray_pickable() const; + void set_enable_monitoring(bool p_enable); bool is_monitoring_enabled() const; diff --git a/scene/3d/camera.cpp b/scene/3d/camera.cpp index 56fbf358bc..4245bfa2c9 100644 --- a/scene/3d/camera.cpp +++ b/scene/3d/camera.cpp @@ -74,16 +74,18 @@ bool Camera::_set(const StringName& p_name, const Variant& p_value) { mode=PROJECTION_ORTHOGONAL; changed_all=true; - } else if (p_name=="fov") + } else if (p_name=="fov" || p_name=="fovy" || p_name=="fovx") fov=p_value; - else if (p_name=="size") + else if (p_name=="size" || p_name=="sizex" || p_name=="sizey") size=p_value; else if (p_name=="near") near=p_value; else if (p_name=="far") far=p_value; + else if (p_name=="keep_aspect") + set_keep_aspect_mode(KeepAspect(int(p_value))); else if (p_name=="vaspect") - set_use_vertical_aspect(p_value); + set_keep_aspect_mode(p_value?KEEP_WIDTH:KEEP_HEIGHT); else if (p_name=="current") { if (p_value.operator bool()) { make_current(); @@ -107,16 +109,16 @@ bool Camera::_get(const StringName& p_name,Variant &r_ret) const { if (p_name=="projection") { r_ret= mode; - } else if (p_name=="fov") + } else if (p_name=="fov" || p_name=="fovy" || p_name=="fovx") r_ret= fov; - else if (p_name=="size") + else if (p_name=="size" || p_name=="sizex" || p_name=="sizey") r_ret= size; else if (p_name=="near") r_ret= near; else if (p_name=="far") r_ret= far; - else if (p_name=="vaspect") - r_ret= vaspect; + else if (p_name=="keep_aspect") + r_ret= int(keep_aspect); else if (p_name=="current") { if (is_inside_scene() && get_scene()->is_editor_hint()) { @@ -142,19 +144,29 @@ void Camera::_get_property_list( List<PropertyInfo> *p_list) const { case PROJECTION_PERSPECTIVE: { - p_list->push_back( PropertyInfo( Variant::REAL, "fov" , PROPERTY_HINT_RANGE, "1,89,0.1") ); + p_list->push_back( PropertyInfo( Variant::REAL, "fov" , PROPERTY_HINT_RANGE, "1,89,0.1",PROPERTY_USAGE_NOEDITOR) ); + if (keep_aspect==KEEP_WIDTH) + p_list->push_back( PropertyInfo( Variant::REAL, "fovx" , PROPERTY_HINT_RANGE, "1,89,0.1",PROPERTY_USAGE_EDITOR) ); + else + p_list->push_back( PropertyInfo( Variant::REAL, "fovy" , PROPERTY_HINT_RANGE, "1,89,0.1",PROPERTY_USAGE_EDITOR) ); + } break; case PROJECTION_ORTHOGONAL: { - p_list->push_back( PropertyInfo( Variant::REAL, "size" , PROPERTY_HINT_RANGE, "1,16384,0.01" ) ); + p_list->push_back( PropertyInfo( Variant::REAL, "size" , PROPERTY_HINT_RANGE, "1,16384,0.01",PROPERTY_USAGE_NOEDITOR ) ); + if (keep_aspect==KEEP_WIDTH) + p_list->push_back( PropertyInfo( Variant::REAL, "sizex" , PROPERTY_HINT_RANGE, "0.1,16384,0.01",PROPERTY_USAGE_EDITOR) ); + else + p_list->push_back( PropertyInfo( Variant::REAL, "sizey" , PROPERTY_HINT_RANGE, "0.1,16384,0.01",PROPERTY_USAGE_EDITOR) ); + } break; } p_list->push_back( PropertyInfo( Variant::REAL, "near" , PROPERTY_HINT_EXP_RANGE, "0.01,4096.0,0.01") ); p_list->push_back( PropertyInfo( Variant::REAL, "far" , PROPERTY_HINT_EXP_RANGE, "0.01,4096.0,0.01") ); - p_list->push_back( PropertyInfo( Variant::BOOL, "vaspect") ); + p_list->push_back( PropertyInfo( Variant::INT, "keep_aspect",PROPERTY_HINT_ENUM,"Keep Width,Keep Height") ); p_list->push_back( PropertyInfo( Variant::BOOL, "current" ) ); p_list->push_back( PropertyInfo( Variant::INT, "visible_layers",PROPERTY_HINT_ALL_FLAGS ) ); p_list->push_back( PropertyInfo( Variant::OBJECT, "environment",PROPERTY_HINT_RESOURCE_TYPE,"Environment" ) ); @@ -441,7 +453,7 @@ Vector3 Camera::project_local_ray_normal(const Point2& p_pos) const { ray=Vector3(0,0,-1); } else { CameraMatrix cm; - cm.set_perspective(fov,viewport_size.get_aspect(),near,far,vaspect); + cm.set_perspective(fov,viewport_size.get_aspect(),near,far,keep_aspect==KEEP_WIDTH); float screen_w,screen_h; cm.get_viewport_size(screen_w,screen_h); ray=Vector3( ((p_pos.x/viewport_size.width)*2.0-1.0)*screen_w, ((1.0-(p_pos.y/viewport_size.height))*2.0-1.0)*screen_h,-near).normalized(); @@ -472,7 +484,7 @@ Vector3 Camera::project_ray_origin(const Point2& p_pos) const { Vector2 pos = p_pos / viewport_size; float vsize,hsize; - if (vaspect) { + if (keep_aspect==KEEP_WIDTH) { vsize = size/viewport_size.get_aspect(); hsize = size; } else { @@ -503,9 +515,9 @@ Point2 Camera::unproject_position(const Vector3& p_pos) const { if (mode==PROJECTION_ORTHOGONAL) - cm.set_orthogonal(size,viewport_size.get_aspect(),near,far,vaspect); + cm.set_orthogonal(size,viewport_size.get_aspect(),near,far,keep_aspect==KEEP_WIDTH); else - cm.set_perspective(fov,viewport_size.get_aspect(),near,far,vaspect); + cm.set_perspective(fov,viewport_size.get_aspect(),near,far,keep_aspect==KEEP_WIDTH); Plane p(get_camera_transform().xform_inv(p_pos),1.0); @@ -533,9 +545,9 @@ Vector3 Camera::project_position(const Point2& p_point) const { CameraMatrix cm; if (mode==PROJECTION_ORTHOGONAL) - cm.set_orthogonal(size,viewport_size.get_aspect(),near,far,vaspect); + cm.set_orthogonal(size,viewport_size.get_aspect(),near,far,keep_aspect==KEEP_WIDTH); else - cm.set_perspective(fov,viewport_size.get_aspect(),near,far,vaspect); + cm.set_perspective(fov,viewport_size.get_aspect(),near,far,keep_aspect==KEEP_WIDTH); Size2 vp_size; cm.get_viewport_size(vp_size.x,vp_size.y); @@ -583,6 +595,20 @@ Ref<Environment> Camera::get_environment() const { } +void Camera::set_keep_aspect_mode(KeepAspect p_aspect) { + + keep_aspect=p_aspect; + VisualServer::get_singleton()->camera_set_use_vertical_aspect(camera,p_aspect==KEEP_WIDTH); + + _change_notify(); +} + +Camera::KeepAspect Camera::get_keep_aspect_mode() const{ + + return keep_aspect; +} + + void Camera::_bind_methods() { @@ -608,12 +634,16 @@ void Camera::_bind_methods() { ObjectTypeDB::bind_method( _MD("look_at_from_pos","pos","target","up"),&Camera::look_at_from_pos ); ObjectTypeDB::bind_method(_MD("set_environment","env:Environment"),&Camera::set_environment); ObjectTypeDB::bind_method(_MD("get_environment:Environment"),&Camera::get_environment); - ObjectTypeDB::bind_method(_MD("set_use_vertical_aspect","enable"),&Camera::set_use_vertical_aspect); - ObjectTypeDB::bind_method(_MD("is_using_vertical_aspect"),&Camera::is_using_vertical_aspect); + ObjectTypeDB::bind_method(_MD("set_keep_aspect_mode","mode"),&Camera::set_keep_aspect_mode); + ObjectTypeDB::bind_method(_MD("get_keep_aspect_mode"),&Camera::get_keep_aspect_mode); //ObjectTypeDB::bind_method( _MD("_camera_make_current"),&Camera::_camera_make_current ); BIND_CONSTANT( PROJECTION_PERSPECTIVE ); BIND_CONSTANT( PROJECTION_ORTHOGONAL ); + + BIND_CONSTANT( KEEP_WIDTH ); + BIND_CONSTANT( KEEP_HEIGHT ); + } float Camera::get_fov() const { @@ -661,26 +691,15 @@ Vector<Plane> Camera::get_frustum() const { Size2 viewport_size = viewport_ptr->get_visible_rect().size; CameraMatrix cm; if (mode==PROJECTION_PERSPECTIVE) - cm.set_perspective(fov,viewport_size.get_aspect(),near,far,vaspect); + cm.set_perspective(fov,viewport_size.get_aspect(),near,far,keep_aspect==KEEP_WIDTH); else - cm.set_orthogonal(size,viewport_size.get_aspect(),near,far,vaspect); + cm.set_orthogonal(size,viewport_size.get_aspect(),near,far,keep_aspect==KEEP_WIDTH); return cm.get_projection_planes(get_global_transform()); } -void Camera::set_use_vertical_aspect(bool p_enable) { - - vaspect=p_enable; - VisualServer::get_singleton()->camera_set_use_vertical_aspect(camera,p_enable); -} - - -bool Camera::is_using_vertical_aspect() const{ - - return vaspect; -} void Camera::look_at(const Vector3& p_target, const Vector3& p_up_normal) { @@ -712,7 +731,7 @@ Camera::Camera() { force_change=false; mode=PROJECTION_PERSPECTIVE; set_perspective(60.0,0.1,100.0); - vaspect=false; + keep_aspect=KEEP_HEIGHT; layers=0xFFFFFFFF; //active=false; } diff --git a/scene/3d/camera.h b/scene/3d/camera.h index a8599497ac..014c7cb520 100644 --- a/scene/3d/camera.h +++ b/scene/3d/camera.h @@ -46,6 +46,11 @@ public: PROJECTION_ORTHOGONAL }; + enum KeepAspect { + KEEP_WIDTH, + KEEP_HEIGHT + }; + private: bool force_change; @@ -56,7 +61,7 @@ private: float fov; float size; float near,far; - bool vaspect; + KeepAspect keep_aspect; RID camera; RID scenario_id; @@ -126,8 +131,8 @@ public: void set_environment(const Ref<Environment>& p_environment); Ref<Environment> get_environment() const; - void set_use_vertical_aspect(bool p_enable); - bool is_using_vertical_aspect() const; + void set_keep_aspect_mode(KeepAspect p_aspect); + KeepAspect get_keep_aspect_mode() const; void look_at(const Vector3& p_target, const Vector3& p_up_normal); void look_at_from_pos(const Vector3& p_pos,const Vector3& p_target, const Vector3& p_up_normal); @@ -140,5 +145,6 @@ public: VARIANT_ENUM_CAST( Camera::Projection ); +VARIANT_ENUM_CAST( Camera::KeepAspect ); #endif diff --git a/scene/3d/collision_object.cpp b/scene/3d/collision_object.cpp index 7ad10d3222..06564f5c49 100644 --- a/scene/3d/collision_object.cpp +++ b/scene/3d/collision_object.cpp @@ -28,7 +28,7 @@ /*************************************************************************/ #include "collision_object.h" #include "servers/physics_server.h" - +#include "scene/scene_string_names.h" void CollisionObject::_update_shapes_from_children() { shapes.resize(0); @@ -165,6 +165,34 @@ void CollisionObject::_get_property_list( List<PropertyInfo> *p_list) const { } } + +void CollisionObject::_input_event(const InputEvent& p_input_event,const Vector3& p_pos, const Vector3& p_normal, int p_shape) { + + if (get_script_instance()) { + get_script_instance()->call(SceneStringNames::get_singleton()->_input_event,p_input_event,p_pos,p_normal,p_shape); + } + emit_signal(SceneStringNames::get_singleton()->input_event,p_input_event,p_pos,p_normal,p_shape); +} + +void CollisionObject::_mouse_enter() { + + if (get_script_instance()) { + get_script_instance()->call(SceneStringNames::get_singleton()->_mouse_enter); + } + emit_signal(SceneStringNames::get_singleton()->mouse_enter); +} + + +void CollisionObject::_mouse_exit() { + + if (get_script_instance()) { + get_script_instance()->call(SceneStringNames::get_singleton()->_mouse_exit); + } + emit_signal(SceneStringNames::get_singleton()->mouse_exit); + +} + + void CollisionObject::_bind_methods() { ObjectTypeDB::bind_method(_MD("add_shape","shape:Shape","transform"),&CollisionObject::add_shape,DEFVAL(Transform())); @@ -178,8 +206,16 @@ void CollisionObject::_bind_methods() { ObjectTypeDB::bind_method(_MD("get_shape_transform","shape_idx"),&CollisionObject::get_shape_transform); ObjectTypeDB::bind_method(_MD("remove_shape","shape_idx"),&CollisionObject::remove_shape); ObjectTypeDB::bind_method(_MD("clear_shapes"),&CollisionObject::clear_shapes); + ObjectTypeDB::bind_method(_MD("set_capture_input_on_drag","enable"),&CollisionObject::set_capture_input_on_drag); + ObjectTypeDB::bind_method(_MD("get_capture_input_on_drag"),&CollisionObject::get_capture_input_on_drag); ObjectTypeDB::bind_method(_MD("get_rid"),&CollisionObject::get_rid); + BIND_VMETHOD( MethodInfo("_input_event",PropertyInfo(Variant::INPUT_EVENT,"event"),PropertyInfo(Variant::VECTOR3,"click_pos"),PropertyInfo(Variant::VECTOR3,"click_normal"),PropertyInfo(Variant::INT,"shape_idx"))); + ADD_SIGNAL( MethodInfo("input_event",PropertyInfo(Variant::INPUT_EVENT,"event"),PropertyInfo(Variant::VECTOR3,"click_pos"),PropertyInfo(Variant::VECTOR3,"click_normal"),PropertyInfo(Variant::INT,"shape_idx"))); + ADD_SIGNAL( MethodInfo("mouse_enter")); + ADD_SIGNAL( MethodInfo("mouse_exit")); + + ADD_PROPERTY(PropertyInfo(Variant::BOOL,"input/capture_on_drag"),_SCS("set_capture_input_on_drag"),_SCS("get_capture_input_on_drag")); } @@ -269,10 +305,23 @@ CollisionObject::CollisionObject(RID p_rid, bool p_area) { } +void CollisionObject::set_capture_input_on_drag(bool p_capture) { + + capture_input_on_drag=p_capture; + +} + +bool CollisionObject::get_capture_input_on_drag() const { + + return capture_input_on_drag; +} + CollisionObject::CollisionObject() { + capture_input_on_drag=false; + //owner= //set_transform_notify(true); diff --git a/scene/3d/collision_object.h b/scene/3d/collision_object.h index 54dc6508ab..afd73aa9cc 100644 --- a/scene/3d/collision_object.h +++ b/scene/3d/collision_object.h @@ -50,6 +50,7 @@ class CollisionObject : public Spatial { }; + bool capture_input_on_drag; Vector<ShapeData> shapes; @@ -67,6 +68,11 @@ protected: bool _get(const StringName& p_name,Variant &r_ret) const; void _get_property_list( List<PropertyInfo> *p_list) const; static void _bind_methods(); +friend class Viewport; + virtual void _input_event(const InputEvent& p_input_event,const Vector3& p_pos, const Vector3& p_normal, int p_shape); + virtual void _mouse_enter(); + virtual void _mouse_exit(); + public: @@ -81,6 +87,9 @@ public: void set_shape_as_trigger(int p_shape_idx, bool p_trigger); bool is_shape_set_as_trigger(int p_shape_idx) const; + void set_capture_input_on_drag(bool p_capture); + bool get_capture_input_on_drag() const; + _FORCE_INLINE_ RID get_rid() const { return rid; } CollisionObject(); diff --git a/scene/3d/collision_polygon.cpp b/scene/3d/collision_polygon.cpp new file mode 100644 index 0000000000..5a613f360a --- /dev/null +++ b/scene/3d/collision_polygon.cpp @@ -0,0 +1,206 @@ +#include "collision_polygon.h" + +#include "collision_object.h" +#include "scene/resources/concave_polygon_shape.h" +#include "scene/resources/convex_polygon_shape.h" + +void CollisionPolygon::_add_to_collision_object(Object *p_obj) { + + + CollisionObject *co = p_obj->cast_to<CollisionObject>(); + ERR_FAIL_COND(!co); + + if (polygon.size()==0) + return; + + bool solids=build_mode==BUILD_SOLIDS; + + Vector< Vector<Vector2> > decomp = Geometry::decompose_polygon(polygon); + if (decomp.size()==0) + return; + + if (true || solids) { + + //here comes the sun, lalalala + //decompose concave into multiple convex polygons and add them + for(int i=0;i<decomp.size();i++) { + Ref<ConvexPolygonShape> convex = memnew( ConvexPolygonShape ); + DVector<Vector3> cp; + int cs = decomp[i].size(); + cp.resize(cs*2); + { + DVector<Vector3>::Write w = cp.write(); + int idx=0; + for(int j=0;j<cs;j++) { + + Vector2 d = decomp[i][j]; + w[idx++]=Vector3(d.x,d.y,depth*0.5); + w[idx++]=Vector3(d.x,d.y,-depth*0.5); + } + } + + convex->set_points(cp); + co->add_shape(convex,get_transform()); + + } + + } else { +#if 0 + Ref<ConcavePolygonShape> concave = memnew( ConcavePolygonShape ); + + DVector<Vector2> segments; + segments.resize(polygon.size()*2); + DVector<Vector2>::Write w=segments.write(); + + for(int i=0;i<polygon.size();i++) { + w[(i<<1)+0]=polygon[i]; + w[(i<<1)+1]=polygon[(i+1)%polygon.size()]; + } + + w=DVector<Vector2>::Write(); + concave->set_segments(segments); + + co->add_shape(concave,get_transform()); +#endif + } + + + //co->add_shape(shape,get_transform()); + +} + +void CollisionPolygon::_update_parent() { + + Node *parent = get_parent(); + if (!parent) + return; + CollisionObject *co = parent->cast_to<CollisionObject>(); + if (!co) + return; + co->_update_shapes_from_children(); +} + +void CollisionPolygon::_notification(int p_what) { + + + switch(p_what) { + case NOTIFICATION_TRANSFORM_CHANGED: { + + if (!is_inside_scene()) + break; + _update_parent(); + + } break; +#if 0 + case NOTIFICATION_DRAW: { + for(int i=0;i<polygon.size();i++) { + + Vector2 p = polygon[i]; + Vector2 n = polygon[(i+1)%polygon.size()]; + draw_line(p,n,Color(0,0.6,0.7,0.5),3); + } + + Vector< Vector<Vector2> > decomp = Geometry::decompose_polygon(polygon); +#define DEBUG_DECOMPOSE +#ifdef DEBUG_DECOMPOSE + Color c(0.4,0.9,0.1); + for(int i=0;i<decomp.size();i++) { + + c.set_hsv( Math::fmod(c.get_h() + 0.738,1),c.get_s(),c.get_v(),0.5); + draw_colored_polygon(decomp[i],c); + } +#endif + + } break; +#endif + } +} + +void CollisionPolygon::set_polygon(const Vector<Point2>& p_polygon) { + + polygon=p_polygon; + + for(int i=0;i<polygon.size();i++) { + + Vector3 p1(polygon[i].x,polygon[i].y,depth*0.5); + + if (i==0) + aabb=AABB(p1,Vector3()); + else + aabb.expand_to(p1); + + Vector3 p2(polygon[i].x,polygon[i].y,-depth*0.5); + aabb.expand_to(p2); + + + } + if (aabb==AABB()) { + + aabb=AABB(Vector3(-1,-1,-1),Vector3(2,2,2)); + } else { + aabb.pos-=aabb.size*0.3; + aabb.size+=aabb.size*0.6; + } + _update_parent(); + update_gizmo(); +} + +Vector<Point2> CollisionPolygon::get_polygon() const { + + return polygon; +} + +void CollisionPolygon::set_build_mode(BuildMode p_mode) { + + ERR_FAIL_INDEX(p_mode,2); + build_mode=p_mode; + _update_parent(); +} + +CollisionPolygon::BuildMode CollisionPolygon::get_build_mode() const{ + + return build_mode; +} + +AABB CollisionPolygon::get_item_rect() const { + + return aabb; +} + +void CollisionPolygon::set_depth(float p_depth) { + + depth=p_depth; + _update_parent(); + update_gizmo(); +} + +float CollisionPolygon::get_depth() const { + + return depth; +} + + +void CollisionPolygon::_bind_methods() { + + ObjectTypeDB::bind_method(_MD("_add_to_collision_object"),&CollisionPolygon::_add_to_collision_object); + ObjectTypeDB::bind_method(_MD("set_polygon","polygon"),&CollisionPolygon::set_polygon); + ObjectTypeDB::bind_method(_MD("get_polygon"),&CollisionPolygon::get_polygon); + + ObjectTypeDB::bind_method(_MD("set_depth","depth"),&CollisionPolygon::set_depth); + ObjectTypeDB::bind_method(_MD("get_depth"),&CollisionPolygon::get_depth); + + ObjectTypeDB::bind_method(_MD("set_build_mode"),&CollisionPolygon::set_build_mode); + ObjectTypeDB::bind_method(_MD("get_build_mode"),&CollisionPolygon::get_build_mode); + + ADD_PROPERTY( PropertyInfo(Variant::INT,"build_mode",PROPERTY_HINT_ENUM,"Solids,Triangles"),_SCS("set_build_mode"),_SCS("get_build_mode")); + ADD_PROPERTY( PropertyInfo(Variant::VECTOR2_ARRAY,"polygon"),_SCS("set_polygon"),_SCS("get_polygon")); + ADD_PROPERTY( PropertyInfo(Variant::REAL,"depth"),_SCS("set_depth"),_SCS("get_depth")); +} + +CollisionPolygon::CollisionPolygon() { + + aabb=AABB(Vector3(-1,-1,-1),Vector3(2,2,2)); + build_mode=BUILD_SOLIDS; + depth=1.0; + +} diff --git a/scene/3d/collision_polygon.h b/scene/3d/collision_polygon.h new file mode 100644 index 0000000000..efb3666778 --- /dev/null +++ b/scene/3d/collision_polygon.h @@ -0,0 +1,50 @@ +#ifndef COLLISION_POLYGON_H +#define COLLISION_POLYGON_H + +#include "scene/3d/spatial.h" +#include "scene/resources/shape.h" + + + +class CollisionPolygon : public Spatial { + + OBJ_TYPE(CollisionPolygon,Spatial); +public: + + enum BuildMode { + BUILD_SOLIDS, + BUILD_TRIANGLES, + }; + +protected: + + + float depth; + AABB aabb; + BuildMode build_mode; + Vector<Point2> polygon; + + void _add_to_collision_object(Object *p_obj); + void _update_parent(); + +protected: + + void _notification(int p_what); + static void _bind_methods(); +public: + + void set_build_mode(BuildMode p_mode); + BuildMode get_build_mode() const; + + void set_depth(float p_depth); + float get_depth() const; + + void set_polygon(const Vector<Point2>& p_polygon); + Vector<Point2> get_polygon() const; + + virtual AABB get_item_rect() const; + CollisionPolygon(); +}; + +VARIANT_ENUM_CAST( CollisionPolygon::BuildMode ); +#endif // COLLISION_POLYGON_H diff --git a/scene/3d/light.cpp b/scene/3d/light.cpp index 25f3b3d3a5..1efc74e672 100644 --- a/scene/3d/light.cpp +++ b/scene/3d/light.cpp @@ -460,7 +460,7 @@ void Light::_bind_methods() { ADD_PROPERTY( PropertyInfo( Variant::BOOL, "params/enabled"), _SCS("set_enabled"), _SCS("is_enabled")); - ADD_PROPERTY( PropertyInfo( Variant::INT, "params/bake_mode",PROPERTY_HINT_ENUM,"Disabled,Indirect,Full"), _SCS("set_bake_mode"), _SCS("get_bake_mode")); + ADD_PROPERTY( PropertyInfo( Variant::INT, "params/bake_mode",PROPERTY_HINT_ENUM,"Disabled,Indirect,Indirect+Shadows,Full"), _SCS("set_bake_mode"), _SCS("get_bake_mode")); ADD_PROPERTYI( PropertyInfo( Variant::REAL, "params/energy", PROPERTY_HINT_EXP_RANGE, "0,64,0.01"), _SCS("set_parameter"), _SCS("get_parameter"), PARAM_ENERGY ); /* if (type == VisualServer::LIGHT_OMNI || type == VisualServer::LIGHT_SPOT) { @@ -477,7 +477,7 @@ void Light::_bind_methods() { ADD_PROPERTYI( PropertyInfo( Variant::COLOR, "colors/diffuse"), _SCS("set_color"), _SCS("get_color"),COLOR_DIFFUSE); ADD_PROPERTYI( PropertyInfo( Variant::COLOR, "colors/specular"), _SCS("set_color"), _SCS("get_color"),COLOR_SPECULAR); ADD_PROPERTY( PropertyInfo( Variant::BOOL, "shadow/shadow"), _SCS("set_project_shadows"), _SCS("has_project_shadows")); - ADD_PROPERTYI( PropertyInfo( Variant::REAL, "shadow/darkening", PROPERTY_HINT_RANGE, "0,64,0.01"), _SCS("set_parameter"), _SCS("get_parameter"), PARAM_SHADOW_DARKENING ); + ADD_PROPERTYI( PropertyInfo( Variant::REAL, "shadow/darkening", PROPERTY_HINT_RANGE, "0,1,0.01"), _SCS("set_parameter"), _SCS("get_parameter"), PARAM_SHADOW_DARKENING ); ADD_PROPERTYI( PropertyInfo( Variant::REAL, "shadow/z_offset", PROPERTY_HINT_RANGE, "0,128,0.001"), _SCS("set_parameter"), _SCS("get_parameter"), PARAM_SHADOW_Z_OFFSET); ADD_PROPERTYI( PropertyInfo( Variant::REAL, "shadow/z_slope_scale", PROPERTY_HINT_RANGE, "0,128,0.001"), _SCS("set_parameter"), _SCS("get_parameter"), PARAM_SHADOW_Z_SLOPE_SCALE); ADD_PROPERTYI( PropertyInfo( Variant::REAL, "shadow/esm_multiplier", PROPERTY_HINT_RANGE, "1.0,512.0,0.1"), _SCS("set_parameter"), _SCS("get_parameter"), PARAM_SHADOW_ESM_MULTIPLIER); @@ -498,6 +498,12 @@ void Light::_bind_methods() { BIND_CONSTANT( COLOR_DIFFUSE ); BIND_CONSTANT( COLOR_SPECULAR ); + BIND_CONSTANT( BAKE_MODE_DISABLED ); + BIND_CONSTANT( BAKE_MODE_INDIRECT ); + BIND_CONSTANT( BAKE_MODE_INDIRECT_AND_SHADOWS ); + BIND_CONSTANT( BAKE_MODE_FULL ); + + } diff --git a/scene/3d/light.h b/scene/3d/light.h index 6b1ea2d455..9fdd7295dc 100644 --- a/scene/3d/light.h +++ b/scene/3d/light.h @@ -69,6 +69,7 @@ public: BAKE_MODE_DISABLED, BAKE_MODE_INDIRECT, + BAKE_MODE_INDIRECT_AND_SHADOWS, BAKE_MODE_FULL }; diff --git a/scene/3d/navigation.cpp b/scene/3d/navigation.cpp new file mode 100644 index 0000000000..d2abdad079 --- /dev/null +++ b/scene/3d/navigation.cpp @@ -0,0 +1,603 @@ +#include "navigation.h" + +void Navigation::_navmesh_link(int p_id) { + + ERR_FAIL_COND(!navmesh_map.has(p_id)); + NavMesh &nm=navmesh_map[p_id]; + ERR_FAIL_COND(nm.linked); + + print_line("LINK"); + + DVector<Vector3> vertices=nm.navmesh->get_vertices(); + int len = vertices.size(); + if (len==0) + return; + + DVector<Vector3>::Read r=vertices.read(); + + for(int i=0;i<nm.navmesh->get_polygon_count();i++) { + + //build + + List<Polygon>::Element *P=nm.polygons.push_back(Polygon()); + Polygon &p=P->get(); + + Vector<int> poly = nm.navmesh->get_polygon(i); + int plen=poly.size(); + const int *indices=poly.ptr(); + bool valid=true; + p.edges.resize(plen); + + Vector3 center; + + for(int j=0;j<plen;j++) { + + int idx = indices[j]; + if (idx<0 || idx>=len) { + valid=false; + break; + } + + Polygon::Edge e; + Vector3 ep=nm.xform.xform(r[idx]); + center+=ep; + e.point=_get_point(ep); + p.edges[j]=e; + } + + if (!valid) { + nm.polygons.pop_back(); + ERR_CONTINUE(!valid); + continue; + } + + p.center=center/plen; + + //connect + + for(int j=0;j<plen;j++) { + + int next = (j+1)%plen; + EdgeKey ek(p.edges[j].point,p.edges[next].point); + + Map<EdgeKey,Connection>::Element *C=connections.find(ek); + if (!C) { + + Connection c; + c.A=&p; + c.A_edge=j; + c.B=NULL; + c.B_edge=-1; + connections[ek]=c; + } else { + + if (C->get().B!=NULL) { + print_line(String()+_get_vertex(ek.a)+" -> "+_get_vertex(ek.b)); + } + ERR_CONTINUE(C->get().B!=NULL); //wut + + C->get().B=&p; + C->get().B_edge=j; + C->get().A->edges[C->get().A_edge].C=&p; + C->get().A->edges[C->get().A_edge].C_edge=j;; + p.edges[j].C=C->get().A; + p.edges[j].C_edge=C->get().A_edge; + //connection successful. + } + } + } + + nm.linked=true; + +} + + +void Navigation::_navmesh_unlink(int p_id) { + + ERR_FAIL_COND(!navmesh_map.has(p_id)); + NavMesh &nm=navmesh_map[p_id]; + ERR_FAIL_COND(!nm.linked); + + print_line("UNLINK"); + + for (List<Polygon>::Element *E=nm.polygons.front();E;E=E->next()) { + + + Polygon &p=E->get(); + + int ec = p.edges.size(); + Polygon::Edge *edges=p.edges.ptr(); + + for(int i=0;i<ec;i++) { + int next = (i+1)%ec; + + EdgeKey ek(edges[i].point,edges[next].point); + Map<EdgeKey,Connection>::Element *C=connections.find(ek); + ERR_CONTINUE(!C); + if (C->get().B) { + //disconnect + + C->get().B->edges[C->get().B_edge].C=NULL; + C->get().B->edges[C->get().B_edge].C_edge=-1; + C->get().A->edges[C->get().A_edge].C=NULL; + C->get().A->edges[C->get().A_edge].C_edge=-1; + + if (C->get().A==&E->get()) { + + C->get().A=C->get().B; + C->get().A_edge=C->get().B_edge; + } + C->get().B=NULL; + C->get().B_edge=-1; + + } else { + connections.erase(C); + //erase + } + } + } + + nm.polygons.clear(); + + nm.linked=false; + + +} + + +int Navigation::navmesh_create(const Ref<NavigationMesh>& p_mesh,const Transform& p_xform) { + + int id = last_id++; + NavMesh nm; + nm.linked=false; + nm.navmesh=p_mesh; + nm.xform=p_xform; + navmesh_map[id]=nm; + + _navmesh_link(id); + + return id; +} + +void Navigation::navmesh_set_transform(int p_id, const Transform& p_xform){ + + ERR_FAIL_COND(!navmesh_map.has(p_id)); + NavMesh &nm=navmesh_map[p_id]; + if (nm.xform==p_xform) + return; //bleh + _navmesh_unlink(p_id); + nm.xform=p_xform; + _navmesh_link(p_id); + + + +} +void Navigation::navmesh_remove(int p_id){ + + ERR_FAIL_COND(!navmesh_map.has(p_id)); + _navmesh_unlink(p_id); + navmesh_map.erase(p_id); + +} + +Vector<Vector3> Navigation::get_simple_path(const Vector3& p_start, const Vector3& p_end, bool p_optimize) { + + + Polygon *begin_poly=NULL; + Polygon *end_poly=NULL; + Vector3 begin_point; + Vector3 end_point; + float begin_d=1e20; + float end_d=1e20; + + + for (Map<int,NavMesh>::Element*E=navmesh_map.front();E;E=E->next()) { + + if (!E->get().linked) + continue; + for(List<Polygon>::Element *F=E->get().polygons.front();F;F=F->next()) { + + Polygon &p=F->get(); + for(int i=2;i<p.edges.size();i++) { + + Face3 f(_get_vertex(p.edges[0].point),_get_vertex(p.edges[i-1].point),_get_vertex(p.edges[i].point)); + Vector3 spoint = f.get_closest_point_to(p_start); + float dpoint = spoint.distance_to(p_start); + if (dpoint<begin_d) { + begin_d=dpoint; + begin_poly=&p; + begin_point=spoint; + } + + spoint = f.get_closest_point_to(p_end); + dpoint = spoint.distance_to(p_end); + if (dpoint<end_d) { + end_d=dpoint; + end_poly=&p; + end_point=spoint; + } + } + + p.prev_edge=-1; + } + } + + if (!begin_poly || !end_poly) { + + //print_line("No Path Path"); + return Vector<Vector3>(); //no path + } + + if (begin_poly==end_poly) { + + Vector<Vector3> path; + path.resize(2); + path[0]=begin_point; + path[1]=end_point; + //print_line("Direct Path"); + return path; + } + + + bool found_route=false; + + List<Polygon*> open_list; + + for(int i=0;i<begin_poly->edges.size();i++) { + + if (begin_poly->edges[i].C) { + + begin_poly->edges[i].C->prev_edge=begin_poly->edges[i].C_edge; + begin_poly->edges[i].C->distance=begin_poly->center.distance_to(begin_poly->edges[i].C->center); + open_list.push_back(begin_poly->edges[i].C); + + if (begin_poly->edges[i].C==end_poly) { + found_route=true; + } + } + } + + + while(!found_route) { + + if (open_list.size()==0) { + // print_line("NOU OPEN LIST"); + break; + } + //check open list + + List<Polygon*>::Element *least_cost_poly=NULL; + float least_cost=1e30; + + //this could be faster (cache previous results) + for (List<Polygon*>::Element *E=open_list.front();E;E=E->next()) { + + Polygon *p=E->get(); + + + float cost=p->distance; + cost+=p->center.distance_to(end_point); + + if (cost<least_cost) { + + least_cost_poly=E; + least_cost=cost; + } + } + + + Polygon *p=least_cost_poly->get(); + //open the neighbours for search + + for(int i=0;i<p->edges.size();i++) { + + + Polygon::Edge &e=p->edges[i]; + + if (!e.C) + continue; + + float distance = p->center.distance_to(e.C->center) + p->distance; + + if (e.C->prev_edge!=-1) { + //oh this was visited already, can we win the cost? + + if (e.C->distance>distance) { + + e.C->prev_edge=e.C_edge; + e.C->distance=distance; + } + } else { + //add to open neighbours + + e.C->prev_edge=e.C_edge; + e.C->distance=distance; + open_list.push_back(e.C); + + if (e.C==end_poly) { + //oh my reached end! stop algorithm + found_route=true; + break; + + } + + } + } + + if (found_route) + break; + + open_list.erase(least_cost_poly); + } + + if (found_route) { + + Vector<Vector3> path; + + if (p_optimize) { + //string pulling + + Polygon *apex_poly=end_poly; + Vector3 apex_point=end_point; + Vector3 portal_left=apex_point; + Vector3 portal_right=apex_point; + Polygon *left_poly=end_poly; + Polygon *right_poly=end_poly; + Polygon *p=end_poly; + path.push_back(end_point); + + while(p) { + + Vector3 left; + Vector3 right; + +#define CLOCK_TANGENT(m_a,m_b,m_c) ( ((m_a)-(m_c)).cross((m_a)-(m_b)) ) + + if (p==begin_poly) { + left=begin_point; + right=begin_point; + } else { + int prev = p->prev_edge; + int prev_n = (p->prev_edge+1)%p->edges.size(); + left = _get_vertex(p->edges[prev].point); + right = _get_vertex(p->edges[prev_n].point); + + if (CLOCK_TANGENT(apex_point,left,(left+right)*0.5).dot(up) < 0){ + SWAP(left,right); + } + } + + bool skip=false; + + + if (CLOCK_TANGENT(apex_point,portal_left,left).dot(up) >= 0){ + //process + if (portal_left==apex_point || CLOCK_TANGENT(apex_point,left,portal_right).dot(up) > 0) { + left_poly=p; + portal_left=left; + } else { + + apex_point=portal_right; + p=right_poly; + left_poly=p; + portal_left=apex_point; + portal_right=apex_point; + path.push_back(apex_point); + skip=true; + } + } + + if (!skip && CLOCK_TANGENT(apex_point,portal_right,right).dot(up) <= 0){ + //process + if (portal_right==apex_point || CLOCK_TANGENT(apex_point,right,portal_left).dot(up) < 0) { + right_poly=p; + portal_right=right; + } else { + + apex_point=portal_left; + p=left_poly; + right_poly=p; + portal_right=apex_point; + portal_left=apex_point; + path.push_back(apex_point); + } + } + + if (p!=begin_poly) + p=p->edges[p->prev_edge].C; + else + p=NULL; + + } + + if (path[path.size()-1]!=begin_point) + path.push_back(begin_point); + + path.invert(); + + + + + } else { + //midpoints + Polygon *p=end_poly; + + path.push_back(end_point); + while(true) { + int prev = p->prev_edge; + int prev_n = (p->prev_edge+1)%p->edges.size(); + Vector3 point = (_get_vertex(p->edges[prev].point) + _get_vertex(p->edges[prev_n].point))*0.5; + path.push_back(point); + p = p->edges[prev].C; + if (p==begin_poly) + break; + } + + path.push_back(begin_point); + + + path.invert();; + } + + return path; + } + + + return Vector<Vector3>(); + +} + +Vector3 Navigation::get_closest_point_to_segment(const Vector3& p_from,const Vector3& p_to) { + + + bool use_collision=false; + Vector3 closest_point; + float closest_point_d=1e20; + + for (Map<int,NavMesh>::Element*E=navmesh_map.front();E;E=E->next()) { + + if (!E->get().linked) + continue; + for(List<Polygon>::Element *F=E->get().polygons.front();F;F=F->next()) { + + Polygon &p=F->get(); + for(int i=2;i<p.edges.size();i++) { + + Face3 f(_get_vertex(p.edges[0].point),_get_vertex(p.edges[i-1].point),_get_vertex(p.edges[i].point)); + Vector3 inters; + if (f.intersects_segment(p_from,p_to,&inters)) { + + if (!use_collision) { + closest_point=inters; + use_collision=true; + closest_point_d=p_from.distance_to(inters); + } else if (closest_point_d > inters.distance_to(p_from)){ + + closest_point=inters; + closest_point_d=p_from.distance_to(inters); + } + } + } + + if (!use_collision) { + + for(int i=0;i<p.edges.size();i++) { + + Vector3 a,b; + + Geometry::get_closest_points_between_segments(p_from,p_to,_get_vertex(p.edges[i].point),_get_vertex(p.edges[(i+1)%p.edges.size()].point),a,b); + + float d = a.distance_to(b); + if (d<closest_point_d) { + + closest_point_d=d; + closest_point=b; + } + + } + } + } + } + + return closest_point; +} + +Vector3 Navigation::get_closest_point(const Vector3& p_point) { + + Vector3 closest_point; + float closest_point_d=1e20; + + for (Map<int,NavMesh>::Element*E=navmesh_map.front();E;E=E->next()) { + + if (!E->get().linked) + continue; + for(List<Polygon>::Element *F=E->get().polygons.front();F;F=F->next()) { + + Polygon &p=F->get(); + for(int i=2;i<p.edges.size();i++) { + + Face3 f(_get_vertex(p.edges[0].point),_get_vertex(p.edges[i-1].point),_get_vertex(p.edges[i].point)); + Vector3 inters = f.get_closest_point_to(p_point); + float d = inters.distance_to(p_point); + if (d<closest_point_d) { + closest_point=inters; + closest_point_d=d; + } + } + } + } + + return closest_point; + +} + +Vector3 Navigation::get_closest_point_normal(const Vector3& p_point){ + + Vector3 closest_point; + Vector3 closest_normal; + float closest_point_d=1e20; + + for (Map<int,NavMesh>::Element*E=navmesh_map.front();E;E=E->next()) { + + if (!E->get().linked) + continue; + for(List<Polygon>::Element *F=E->get().polygons.front();F;F=F->next()) { + + Polygon &p=F->get(); + for(int i=2;i<p.edges.size();i++) { + + Face3 f(_get_vertex(p.edges[0].point),_get_vertex(p.edges[i-1].point),_get_vertex(p.edges[i].point)); + Vector3 inters = f.get_closest_point_to(p_point); + float d = inters.distance_to(p_point); + if (d<closest_point_d) { + closest_point=inters; + closest_point_d=d; + closest_normal=f.get_plane().normal; + } + } + } + } + + return closest_normal; + +} + + +void Navigation::set_up_vector(const Vector3& p_up) { + + + up=p_up; +} + +Vector3 Navigation::get_up_vector() const{ + + return up; +} + + +void Navigation::_bind_methods() { + + ObjectTypeDB::bind_method(_MD("navmesh_create","mesh:NavigationMesh","xform"),&Navigation::navmesh_create); + ObjectTypeDB::bind_method(_MD("navmesh_set_transform","id","xform"),&Navigation::navmesh_set_transform); + ObjectTypeDB::bind_method(_MD("navmesh_remove","id"),&Navigation::navmesh_remove); + + ObjectTypeDB::bind_method(_MD("get_simple_path","start","end","optimize"),&Navigation::get_simple_path,DEFVAL(true)); + ObjectTypeDB::bind_method(_MD("get_closest_point_to_segment","start","end"),&Navigation::get_closest_point_to_segment); + ObjectTypeDB::bind_method(_MD("get_closest_point","to_point"),&Navigation::get_closest_point); + ObjectTypeDB::bind_method(_MD("get_closest_point_normal","to_point"),&Navigation::get_closest_point_normal); + + ObjectTypeDB::bind_method(_MD("set_up_vector","up"),&Navigation::set_up_vector); + ObjectTypeDB::bind_method(_MD("get_up_vector"),&Navigation::get_up_vector); + + ADD_PROPERTY( PropertyInfo(Variant::VECTOR3,"up_vector"),_SCS("set_up_vector"),_SCS("get_up_vector")); +} + +Navigation::Navigation() { + + ERR_FAIL_COND( sizeof(Point)!=8 ); + cell_size=0.01; //one centimeter + last_id=1; + up=Vector3(0,1,0); +} + + diff --git a/scene/3d/navigation.h b/scene/3d/navigation.h new file mode 100644 index 0000000000..e8a97a6591 --- /dev/null +++ b/scene/3d/navigation.h @@ -0,0 +1,139 @@ +#ifndef NAVIGATION_H +#define NAVIGATION_H + +#include "scene/3d/spatial.h" +#include "scene/3d/navigation_mesh.h" + +class Navigation : public Spatial { + + OBJ_TYPE( Navigation, Spatial); + + + union Point { + + struct { + int64_t x:21; + int64_t y:22; + int64_t z:21; + }; + + uint64_t key; + bool operator<(const Point& p_key) const { return key < p_key.key; } + }; + + + struct EdgeKey { + + Point a; + Point b; + + bool operator<(const EdgeKey& p_key) const { + return (a.key==p_key.a.key)?(b.key<p_key.b.key):(a.key<p_key.a.key); + }; + + EdgeKey(const Point& p_a=Point(),const Point& p_b=Point()) { + a=p_a; + b=p_b; + if (a.key > b.key) { + SWAP(a,b); + } + } + }; + + + + struct Polygon { + + struct Edge { + Point point; + Polygon *C; //connection + int C_edge; + Edge() { C=NULL; C_edge=-1; } + }; + + Vector<Edge> edges; + + Vector3 center; + + float distance; + int prev_edge; + }; + + + struct Connection { + + Polygon *A; + int A_edge; + Polygon *B; + int B_edge; + Connection() { A=NULL; B=NULL; A_edge=-1; B_edge=-1;} + }; + + Map<EdgeKey,Connection> connections; + + + struct NavMesh { + + Transform xform; + bool linked; + Ref<NavigationMesh> navmesh; + List<Polygon> polygons; + + }; + + + + _FORCE_INLINE_ Point _get_point(const Vector3& p_pos) const { + + int x = int(Math::floor(p_pos.x/cell_size)); + int y = int(Math::floor(p_pos.y/cell_size)); + int z = int(Math::floor(p_pos.z/cell_size)); + + Point p; + p.key=0; + p.x=x; + p.y=y; + p.z=z; + return p; + + } + + _FORCE_INLINE_ Vector3 _get_vertex(const Point& p_point) const { + + return Vector3(p_point.x,p_point.y,p_point.z)*cell_size; + } + + + + void _navmesh_link(int p_id); + void _navmesh_unlink(int p_id); + + float cell_size; + Map<int,NavMesh> navmesh_map; + int last_id; + + Vector3 up; + +protected: + + static void _bind_methods(); + +public: + + void set_up_vector(const Vector3& p_up); + Vector3 get_up_vector() const; + + //API should be as dynamic as possible + int navmesh_create(const Ref<NavigationMesh>& p_mesh,const Transform& p_xform); + void navmesh_set_transform(int p_id, const Transform& p_xform); + void navmesh_remove(int p_id); + + Vector<Vector3> get_simple_path(const Vector3& p_start, const Vector3& p_end,bool p_optimize=true); + Vector3 get_closest_point_to_segment(const Vector3& p_from,const Vector3& p_to); + Vector3 get_closest_point(const Vector3& p_point); + Vector3 get_closest_point_normal(const Vector3& p_point); + + Navigation(); +}; + +#endif // NAVIGATION_H diff --git a/scene/3d/navigation_agent.cpp b/scene/3d/navigation_agent.cpp new file mode 100644 index 0000000000..9b304e45ec --- /dev/null +++ b/scene/3d/navigation_agent.cpp @@ -0,0 +1,5 @@ +#include "navigation_agent.h" + +NavigationAgent::NavigationAgent() +{ +} diff --git a/scene/3d/navigation_agent.h b/scene/3d/navigation_agent.h new file mode 100644 index 0000000000..baceb693a5 --- /dev/null +++ b/scene/3d/navigation_agent.h @@ -0,0 +1,10 @@ +#ifndef NAVIGATION_AGENT_H +#define NAVIGATION_AGENT_H + +class NavigationAgent +{ +public: + NavigationAgent(); +}; + +#endif // NAVIGATION_AGENT_H diff --git a/scene/3d/navigation_mesh.cpp b/scene/3d/navigation_mesh.cpp new file mode 100644 index 0000000000..cf2e22a573 --- /dev/null +++ b/scene/3d/navigation_mesh.cpp @@ -0,0 +1,237 @@ +#include "navigation_mesh.h" +#include "navigation.h" + + +void NavigationMesh::create_from_mesh(const Ref<Mesh>& p_mesh) { + + + vertices=DVector<Vector3>(); + clear_polygons(); + + for(int i=0;i<p_mesh->get_surface_count();i++) { + + if (p_mesh->surface_get_primitive_type(i)!=Mesh::PRIMITIVE_TRIANGLES) + continue; + Array arr = p_mesh->surface_get_arrays(i); + DVector<Vector3> varr = arr[Mesh::ARRAY_VERTEX]; + DVector<int> iarr = arr[Mesh::ARRAY_INDEX]; + if (varr.size()==0 || iarr.size()==0) + continue; + + int from = vertices.size(); + vertices.append_array(varr); + int rlen = iarr.size(); + DVector<int>::Read r = iarr.read(); + + for(int j=0;j<rlen;j+=3) { + Vector<int> vi; + vi.resize(3); + vi[0]=r[j+0]+from; + vi[1]=r[j+1]+from; + vi[2]=r[j+2]+from; + + add_polygon(vi); + } + } +} + +void NavigationMesh::set_vertices(const DVector<Vector3>& p_vertices) { + + vertices=p_vertices; +} + +DVector<Vector3> NavigationMesh::get_vertices() const{ + + return vertices; +} + + +void NavigationMesh::_set_polygons(const Array& p_array) { + + polygons.resize(p_array.size()); + for(int i=0;i<p_array.size();i++) { + polygons[i].indices=p_array[i]; + } +} + +Array NavigationMesh::_get_polygons() const { + + Array ret; + ret.resize(polygons.size()); + for(int i=0;i<ret.size();i++) { + ret[i]=polygons[i].indices; + } + + return ret; +} + + +void NavigationMesh::add_polygon(const Vector<int>& p_polygon){ + + Polygon polygon; + polygon.indices=p_polygon; + polygons.push_back(polygon); + +} +int NavigationMesh::get_polygon_count() const{ + + return polygons.size(); +} +Vector<int> NavigationMesh::get_polygon(int p_idx){ + + ERR_FAIL_INDEX_V(p_idx,polygons.size(),Vector<int>()); + return polygons[p_idx].indices; +} +void NavigationMesh::clear_polygons(){ + + polygons.clear(); +} + +void NavigationMesh::_bind_methods() { + + ObjectTypeDB::bind_method(_MD("set_vertices","vertices"),&NavigationMesh::set_vertices); + ObjectTypeDB::bind_method(_MD("get_vertices"),&NavigationMesh::get_vertices); + + ObjectTypeDB::bind_method(_MD("add_polygon","polygon"),&NavigationMesh::add_polygon); + ObjectTypeDB::bind_method(_MD("get_polygon_count"),&NavigationMesh::get_polygon_count); + ObjectTypeDB::bind_method(_MD("get_polygon","idx"),&NavigationMesh::get_polygon); + ObjectTypeDB::bind_method(_MD("clear_polygons"),&NavigationMesh::clear_polygons); + + ObjectTypeDB::bind_method(_MD("_set_polygons","polygons"),&NavigationMesh::_set_polygons); + ObjectTypeDB::bind_method(_MD("_get_polygons"),&NavigationMesh::_get_polygons); + + ADD_PROPERTY(PropertyInfo(Variant::VECTOR3_ARRAY,"vertices",PROPERTY_HINT_NONE,"",PROPERTY_USAGE_NOEDITOR),_SCS("set_vertices"),_SCS("get_vertices")); + ADD_PROPERTY(PropertyInfo(Variant::ARRAY,"polygons",PROPERTY_HINT_NONE,"",PROPERTY_USAGE_NOEDITOR),_SCS("_set_polygons"),_SCS("_get_polygons")); +} + +NavigationMesh::NavigationMesh() { + +} + +void NavigationMeshInstance::set_enabled(bool p_enabled) { + + if (enabled==p_enabled) + return; + enabled=p_enabled; + + if (!is_inside_scene()) + return; + + if (!enabled) { + + if (nav_id!=-1) { + navigation->navmesh_remove(nav_id); + nav_id=-1; + } + } else { + + if (navigation) { + + if (navmesh.is_valid()) { + + nav_id = navigation->navmesh_create(navmesh,get_relative_transform(navigation)); + } + } + + } + + update_gizmo(); +} + +bool NavigationMeshInstance::is_enabled() const { + + + return enabled; +} + + +///////////////////////////// + + +void NavigationMeshInstance::_notification(int p_what) { + + + switch(p_what) { + case NOTIFICATION_ENTER_SCENE: { + + Spatial *c=this; + while(c) { + + navigation=c->cast_to<Navigation>(); + if (navigation) { + + if (enabled && navmesh.is_valid()) { + + nav_id = navigation->navmesh_create(navmesh,get_relative_transform(navigation)); + } + break; + } + + c=c->get_parent_spatial(); + } + + } break; + case NOTIFICATION_TRANSFORM_CHANGED: { + + if (navigation && nav_id!=-1) { + navigation->navmesh_set_transform(nav_id,get_relative_transform(navigation)); + } + + } break; + case NOTIFICATION_EXIT_SCENE: { + + if (navigation) { + + if (nav_id!=-1) { + navigation->navmesh_remove(nav_id); + nav_id=-1; + } + } + navigation=NULL; + } break; + } +} + + +void NavigationMeshInstance::set_navigation_mesh(const Ref<NavigationMesh>& p_navmesh) { + + if (p_navmesh==navmesh) + return; + + if (navigation && nav_id!=-1) { + navigation->navmesh_remove(nav_id); + nav_id=-1; + } + navmesh=p_navmesh; + + if (navigation && navmesh.is_valid() && enabled) { + nav_id = navigation->navmesh_create(navmesh,get_relative_transform(navigation)); + } + update_gizmo(); + +} + +Ref<NavigationMesh> NavigationMeshInstance::get_navigation_mesh() const{ + + return navmesh; +} + +void NavigationMeshInstance::_bind_methods() { + + ObjectTypeDB::bind_method(_MD("set_navigation_mesh","navmesh"),&NavigationMeshInstance::set_navigation_mesh); + ObjectTypeDB::bind_method(_MD("get_navigation_mesh"),&NavigationMeshInstance::get_navigation_mesh); + + ObjectTypeDB::bind_method(_MD("set_enabled","enabled"),&NavigationMeshInstance::set_enabled); + ObjectTypeDB::bind_method(_MD("is_enabled"),&NavigationMeshInstance::is_enabled); + + ADD_PROPERTY( PropertyInfo(Variant::OBJECT,"navmesh",PROPERTY_HINT_RESOURCE_TYPE,"NavigationMesh"),_SCS("set_navigation_mesh"),_SCS("get_navigation_mesh")); + ADD_PROPERTY( PropertyInfo(Variant::BOOL,"enabled"),_SCS("set_enabled"),_SCS("is_enabled")); +} + +NavigationMeshInstance::NavigationMeshInstance() { + + navigation=NULL; + nav_id=-1; + enabled=true; + +} diff --git a/scene/3d/navigation_mesh.h b/scene/3d/navigation_mesh.h new file mode 100644 index 0000000000..fccf405f9d --- /dev/null +++ b/scene/3d/navigation_mesh.h @@ -0,0 +1,68 @@ +#ifndef NAVIGATION_MESH_H +#define NAVIGATION_MESH_H + +#include "scene/3d/spatial.h" +#include "scene/resources/mesh.h" + + +class NavigationMesh : public Resource { + + OBJ_TYPE( NavigationMesh, Resource ); + + DVector<Vector3> vertices; + struct Polygon { + Vector<int> indices; + }; + Vector<Polygon> polygons; + +protected: + + static void _bind_methods(); + + void _set_polygons(const Array& p_array); + Array _get_polygons() const; +public: + + void create_from_mesh(const Ref<Mesh>& p_mesh); + + void set_vertices(const DVector<Vector3>& p_vertices); + DVector<Vector3> get_vertices() const; + + void add_polygon(const Vector<int>& p_polygon); + int get_polygon_count() const; + Vector<int> get_polygon(int p_idx); + void clear_polygons(); + + NavigationMesh(); +}; + + +class Navigation; + +class NavigationMeshInstance : public Spatial { + + OBJ_TYPE(NavigationMeshInstance,Spatial); + + bool enabled; + int nav_id; + Navigation *navigation; + Ref<NavigationMesh> navmesh; +protected: + + void _notification(int p_what); + static void _bind_methods(); +public: + + + + void set_enabled(bool p_enabled); + bool is_enabled() const; + + void set_navigation_mesh(const Ref<NavigationMesh>& p_navmesh); + Ref<NavigationMesh> get_navigation_mesh() const; + + NavigationMeshInstance(); +}; + + +#endif // NAVIGATION_MESH_H diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp index 2a1a5972a9..f5e3ad66ee 100644 --- a/scene/3d/physics_body.cpp +++ b/scene/3d/physics_body.cpp @@ -43,160 +43,125 @@ void PhysicsBody::_notification(int p_what) { */ } -PhysicsBody::PhysicsBody(PhysicsServer::BodyMode p_mode) : CollisionObject( PhysicsServer::get_singleton()->body_create(p_mode), false) { - - +Vector3 PhysicsBody::get_linear_velocity() const { + return Vector3(); } +Vector3 PhysicsBody::get_angular_velocity() const { -void StaticBody::set_constant_linear_velocity(const Vector3& p_vel) { - - constant_linear_velocity=p_vel; - PhysicsServer::get_singleton()->body_set_state(get_rid(),PhysicsServer::BODY_STATE_LINEAR_VELOCITY,constant_linear_velocity); - + return Vector3(); } -void StaticBody::set_constant_angular_velocity(const Vector3& p_vel) { +float PhysicsBody::get_inverse_mass() const { - constant_angular_velocity=p_vel; - PhysicsServer::get_singleton()->body_set_state(get_rid(),PhysicsServer::BODY_STATE_ANGULAR_VELOCITY,constant_angular_velocity); + return 0; } -Vector3 StaticBody::get_constant_linear_velocity() const { - return constant_linear_velocity; -} -Vector3 StaticBody::get_constant_angular_velocity() const { +void PhysicsBody::set_layer_mask(uint32_t p_mask) { - return constant_angular_velocity; + layer_mask=p_mask; + PhysicsServer::get_singleton()->body_set_layer_mask(get_rid(),p_mask); } +uint32_t PhysicsBody::get_layer_mask() const { -void StaticBody::_state_notify(Object *p_object) { - - if (!pre_xform) - return; - - PhysicsDirectBodyState *p2d = (PhysicsDirectBodyState*)p_object; - setting=true; - - Transform new_xform = p2d->get_transform(); - *pre_xform=new_xform; - set_ignore_transform_notification(true); - set_global_transform(new_xform); - set_ignore_transform_notification(false); - - setting=false; - + return layer_mask; +} +void PhysicsBody::_bind_methods() { + ObjectTypeDB::bind_method(_MD("set_layer_mask","mask"),&PhysicsBody::set_layer_mask); + ObjectTypeDB::bind_method(_MD("get_layer_mask"),&PhysicsBody::get_layer_mask); + ADD_PROPERTY(PropertyInfo(Variant::INT,"layers",PROPERTY_HINT_ALL_FLAGS),_SCS("set_layer_mask"),_SCS("get_layer_mask")); } -void StaticBody::_update_xform() { - if (!pre_xform || !pending) - return; +PhysicsBody::PhysicsBody(PhysicsServer::BodyMode p_mode) : CollisionObject( PhysicsServer::get_singleton()->body_create(p_mode), false) { - setting=true; + layer_mask=1; +} - Transform new_xform = get_global_transform(); //obtain the new one - //set_block_transform_notify(true); - set_ignore_transform_notification(true); - PhysicsServer::get_singleton()->body_set_state(get_rid(),PhysicsServer::BODY_STATE_TRANSFORM,*pre_xform); //then simulate motion! - set_global_transform(*pre_xform); //but restore state to previous one in both visual and physics - set_ignore_transform_notification(false); +void StaticBody::set_friction(real_t p_friction){ - PhysicsServer::get_singleton()->body_static_simulate_motion(get_rid(),new_xform); //then simulate motion! + ERR_FAIL_COND(p_friction<0 || p_friction>1); - setting=false; - pending=false; + friction=p_friction; + PhysicsServer::get_singleton()->body_set_param(get_rid(),PhysicsServer::BODY_PARAM_FRICTION,friction); } +real_t StaticBody::get_friction() const{ -void StaticBody::_notification(int p_what) { + return friction; +} - switch(p_what) { +void StaticBody::set_bounce(real_t p_bounce){ - case NOTIFICATION_ENTER_SCENE: { + ERR_FAIL_COND(p_bounce<0 || p_bounce>1); - if (pre_xform) - *pre_xform = get_global_transform(); - pending=false; - } break; - case NOTIFICATION_TRANSFORM_CHANGED: { + bounce=p_bounce; + PhysicsServer::get_singleton()->body_set_param(get_rid(),PhysicsServer::BODY_PARAM_BOUNCE,bounce); - if (simulating_motion && !pending && is_inside_scene() && !setting && !get_scene()->is_editor_hint()) { +} +real_t StaticBody::get_bounce() const{ - call_deferred(SceneStringNames::get_singleton()->_update_xform); - pending=true; - } + return bounce; +} - } break; - } +void StaticBody::set_constant_linear_velocity(const Vector3& p_vel) { + constant_linear_velocity=p_vel; + PhysicsServer::get_singleton()->body_set_state(get_rid(),PhysicsServer::BODY_STATE_LINEAR_VELOCITY,constant_linear_velocity); } -void StaticBody::set_simulate_motion(bool p_enable) { +void StaticBody::set_constant_angular_velocity(const Vector3& p_vel) { - if (p_enable==simulating_motion) - return; - simulating_motion=p_enable; + constant_angular_velocity=p_vel; + PhysicsServer::get_singleton()->body_set_state(get_rid(),PhysicsServer::BODY_STATE_ANGULAR_VELOCITY,constant_angular_velocity); +} - if (p_enable) { - pre_xform = memnew( Transform ); - if (is_inside_scene()) - *pre_xform=get_transform(); -// query = PhysicsServer::get_singleton()->query_create(this,"_state_notify",Variant()); - // PhysicsServer::get_singleton()->query_body_direct_state(query,get_rid()); - PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(),this,"_state_notify"); +Vector3 StaticBody::get_constant_linear_velocity() const { - } else { - memdelete( pre_xform ); - pre_xform=NULL; - PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(),NULL,StringName()); - pending=false; - } + return constant_linear_velocity; } +Vector3 StaticBody::get_constant_angular_velocity() const { -bool StaticBody::is_simulating_motion() const { - - return simulating_motion; + return constant_angular_velocity; } + + void StaticBody::_bind_methods() { - ObjectTypeDB::bind_method(_MD("set_simulate_motion","enabled"),&StaticBody::set_simulate_motion); - ObjectTypeDB::bind_method(_MD("is_simulating_motion"),&StaticBody::is_simulating_motion); - ObjectTypeDB::bind_method(_MD("_update_xform"),&StaticBody::_update_xform); - ObjectTypeDB::bind_method(_MD("_state_notify"),&StaticBody::_state_notify); ObjectTypeDB::bind_method(_MD("set_constant_linear_velocity","vel"),&StaticBody::set_constant_linear_velocity); ObjectTypeDB::bind_method(_MD("set_constant_angular_velocity","vel"),&StaticBody::set_constant_angular_velocity); ObjectTypeDB::bind_method(_MD("get_constant_linear_velocity"),&StaticBody::get_constant_linear_velocity); ObjectTypeDB::bind_method(_MD("get_constant_angular_velocity"),&StaticBody::get_constant_angular_velocity); - ADD_PROPERTY(PropertyInfo(Variant::BOOL,"simulate_motion"),_SCS("set_simulate_motion"),_SCS("is_simulating_motion")); + ObjectTypeDB::bind_method(_MD("set_friction","friction"),&StaticBody::set_friction); + ObjectTypeDB::bind_method(_MD("get_friction"),&StaticBody::get_friction); + + ObjectTypeDB::bind_method(_MD("set_bounce","bounce"),&StaticBody::set_bounce); + ObjectTypeDB::bind_method(_MD("get_bounce"),&StaticBody::get_bounce); + + ADD_PROPERTY( PropertyInfo(Variant::REAL,"friction",PROPERTY_HINT_RANGE,"0,1,0.01"),_SCS("set_friction"),_SCS("get_friction")); + ADD_PROPERTY( PropertyInfo(Variant::REAL,"bounce",PROPERTY_HINT_RANGE,"0,1,0.01"),_SCS("set_bounce"),_SCS("get_bounce")); + ADD_PROPERTY(PropertyInfo(Variant::VECTOR3,"constant_linear_velocity"),_SCS("set_constant_linear_velocity"),_SCS("get_constant_linear_velocity")); ADD_PROPERTY(PropertyInfo(Variant::VECTOR3,"constant_angular_velocity"),_SCS("set_constant_angular_velocity"),_SCS("get_constant_angular_velocity")); } StaticBody::StaticBody() : PhysicsBody(PhysicsServer::BODY_MODE_STATIC) { - simulating_motion=false; - pre_xform=NULL; - setting=false; - pending=false; - //constant_angular_velocity=0; - + bounce=0; + friction=1; } StaticBody::~StaticBody() { - if (pre_xform) - memdelete(pre_xform); - //if (query.is_valid()) - // PhysicsServer::get_singleton()->free(query); + } @@ -754,4 +719,418 @@ RigidBody::~RigidBody() { } +////////////////////////////////////////////////////// +////////////////////////// + + +Variant KinematicBody::_get_collider() const { + + ObjectID oid=get_collider(); + if (oid==0) + return Variant(); + Object *obj = ObjectDB::get_instance(oid); + if (!obj) + return Variant(); + + Reference *ref = obj->cast_to<Reference>(); + if (ref) { + return Ref<Reference>(ref); + } + + return obj; +} + + +bool KinematicBody::_ignores_mode(PhysicsServer::BodyMode p_mode) const { + + switch(p_mode) { + case PhysicsServer::BODY_MODE_STATIC: return !collide_static; + case PhysicsServer::BODY_MODE_KINEMATIC: return !collide_kinematic; + case PhysicsServer::BODY_MODE_RIGID: return !collide_rigid; + case PhysicsServer::BODY_MODE_CHARACTER: return !collide_character; + } + + return true; +} + +Vector3 KinematicBody::move(const Vector3& p_motion) { + + //give me back regular physics engine logic + //this is madness + //and most people using this function will think + //what it does is simpler than using physics + //this took about a week to get right.. + //but is it right? who knows at this point.. + + + colliding=false; + ERR_FAIL_COND_V(!is_inside_scene(),Vector3()); + PhysicsDirectSpaceState *dss = PhysicsServer::get_singleton()->space_get_direct_state(get_world()->get_space()); + ERR_FAIL_COND_V(!dss,Vector3()); + const int max_shapes=32; + Vector3 sr[max_shapes*2]; + int res_shapes; + + Set<RID> exclude; + exclude.insert(get_rid()); + + + //recover first + int recover_attempts=4; + + bool collided=false; + uint32_t mask=0; + if (collide_static) + mask|=PhysicsDirectSpaceState::TYPE_MASK_STATIC_BODY; + if (collide_kinematic) + mask|=PhysicsDirectSpaceState::TYPE_MASK_KINEMATIC_BODY; + if (collide_rigid) + mask|=PhysicsDirectSpaceState::TYPE_MASK_RIGID_BODY; + if (collide_character) + mask|=PhysicsDirectSpaceState::TYPE_MASK_CHARACTER_BODY; + +// print_line("motion: "+p_motion+" margin: "+rtos(margin)); + + //print_line("margin: "+rtos(margin)); + + float m = margin; + //m=0.001; + + do { + + //motion recover + for(int i=0;i<get_shape_count();i++) { + + + if (dss->collide_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),m,sr,max_shapes,res_shapes,exclude,get_layer_mask(),mask)) { + collided=true; + } + + } + + + + if (!collided) + break; + + //print_line("have to recover"); + Vector3 recover_motion; + bool all_outside=true; + for(int j=0;j<8;j++) { + for(int i=0;i<res_shapes;i++) { + + Vector3 a = sr[i*2+0]; + Vector3 b = sr[i*2+1]; + //print_line(String()+a+" -> "+b); +#if 0 + float d = a.distance_to(b); + + //if (d<margin) + /// continue; + /// + /// + recover_motion+=(b-a)*0.2; +#else + float dist = a.distance_to(b); + if (dist>CMP_EPSILON) { + Vector3 norm = (b-a).normalized(); + if (dist>margin*0.5) + all_outside=false; + float adv = norm.dot(recover_motion); + //print_line(itos(i)+" dist: "+rtos(dist)+" adv: "+rtos(adv)); + recover_motion+=norm*MAX(dist-adv,0)*0.4; + } +#endif + + } + } + + + if (recover_motion==Vector3()) { + collided=false; + break; + } + + //print_line("**** RECOVER: "+recover_motion); + + Transform gt = get_global_transform(); + gt.origin+=recover_motion; + set_global_transform(gt); + + recover_attempts--; + + if (all_outside) + break; + + } while (recover_attempts); + + + //move second + float safe = 1.0; + float unsafe = 1.0; + int best_shape=-1; + + PhysicsDirectSpaceState::ShapeRestInfo rest; + + //print_line("pos: "+get_global_transform().origin); + //print_line("motion: "+p_motion); + + + for(int i=0;i<get_shape_count();i++) { + + + + float lsafe,lunsafe; + PhysicsDirectSpaceState::ShapeRestInfo lrest; + bool valid = dss->cast_motion(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i), p_motion,0, lsafe,lunsafe,exclude,get_layer_mask(),mask,&lrest); + //print_line("shape: "+itos(i)+" travel:"+rtos(ltravel)); + if (!valid) { + safe=0; + unsafe=0; + best_shape=i; //sadly it's the best + //print_line("initial stuck"); + + break; + } + if (lsafe==1.0) { + //print_line("initial free"); + continue; + } + if (lsafe < safe) { + + //print_line("initial at "+rtos(lsafe)); + safe=lsafe; + safe=MAX(0,lsafe-0.01); + unsafe=lunsafe; + best_shape=i; + rest=lrest; + } + } + + + //print_line("best shape: "+itos(best_shape)+" motion "+p_motion); + + if (safe>=1) { + //not collided + colliding=false; + } else { + + colliding=true; + + if (true || (safe==0 && unsafe==0)) { //use it always because it's more precise than GJK + //no advance, use rest info from collision + Transform ugt = get_global_transform(); + ugt.origin+=p_motion*unsafe; + + PhysicsDirectSpaceState::ShapeRestInfo rest_info; + bool c2 = dss->rest_info(get_shape(best_shape)->get_rid(), ugt*get_shape_transform(best_shape), m,&rest,exclude,get_layer_mask(),mask); + if (!c2) { + //should not happen, but floating point precision is so weird.. + colliding=false; + } + + // print_line("Rest Travel: "+rest.normal); + + } + + if (colliding) { + + collision=rest.point; + normal=rest.normal; + collider=rest.collider_id; + collider_vel=rest.linear_velocity; + } + } + + Vector3 motion=p_motion*safe; + //if (colliding) + // motion+=normal*0.001; + Transform gt = get_global_transform(); + gt.origin+=motion; + set_global_transform(gt); + + return p_motion-motion; + +} + +Vector3 KinematicBody::move_to(const Vector3& p_position) { + + return move(p_position-get_global_transform().origin); +} + +bool KinematicBody::can_move_to(const Vector3& p_position, bool p_discrete) { + + ERR_FAIL_COND_V(!is_inside_scene(),false); + PhysicsDirectSpaceState *dss = PhysicsServer::get_singleton()->space_get_direct_state(get_world()->get_space()); + ERR_FAIL_COND_V(!dss,false); + + uint32_t mask=0; + if (collide_static) + mask|=PhysicsDirectSpaceState::TYPE_MASK_STATIC_BODY; + if (collide_kinematic) + mask|=PhysicsDirectSpaceState::TYPE_MASK_KINEMATIC_BODY; + if (collide_rigid) + mask|=PhysicsDirectSpaceState::TYPE_MASK_RIGID_BODY; + if (collide_character) + mask|=PhysicsDirectSpaceState::TYPE_MASK_CHARACTER_BODY; + + Vector3 motion = p_position-get_global_transform().origin; + Transform xform=get_global_transform(); + + if (true || p_discrete) { + + xform.origin+=motion; + motion=Vector3(); + } + + Set<RID> exclude; + exclude.insert(get_rid()); + + //fill exclude list.. + for(int i=0;i<get_shape_count();i++) { + + + bool col = dss->intersect_shape(get_shape(i)->get_rid(), xform * get_shape_transform(i),0,NULL,0,exclude,get_layer_mask(),mask); + if (col) + return false; + } + + return true; +} + +bool KinematicBody::is_colliding() const { + + ERR_FAIL_COND_V(!is_inside_scene(),false); + + return colliding; +} +Vector3 KinematicBody::get_collision_pos() const { + + ERR_FAIL_COND_V(!colliding,Vector3()); + return collision; + +} +Vector3 KinematicBody::get_collision_normal() const { + + ERR_FAIL_COND_V(!colliding,Vector3()); + return normal; + +} + +Vector3 KinematicBody::get_collider_velocity() const { + + return collider_vel; +} + +ObjectID KinematicBody::get_collider() const { + + ERR_FAIL_COND_V(!colliding,0); + return collider; +} + +void KinematicBody::set_collide_with_static_bodies(bool p_enable) { + + collide_static=p_enable; +} +bool KinematicBody::can_collide_with_static_bodies() const { + + return collide_static; +} + +void KinematicBody::set_collide_with_rigid_bodies(bool p_enable) { + + collide_rigid=p_enable; + +} +bool KinematicBody::can_collide_with_rigid_bodies() const { + + + return collide_rigid; +} + +void KinematicBody::set_collide_with_kinematic_bodies(bool p_enable) { + + collide_kinematic=p_enable; + +} +bool KinematicBody::can_collide_with_kinematic_bodies() const { + + return collide_kinematic; +} + +void KinematicBody::set_collide_with_character_bodies(bool p_enable) { + + collide_character=p_enable; +} +bool KinematicBody::can_collide_with_character_bodies() const { + + return collide_character; +} + +void KinematicBody::set_collision_margin(float p_margin) { + + margin=p_margin; +} + +float KinematicBody::get_collision_margin() const{ + + return margin; +} + +void KinematicBody::_bind_methods() { + + + ObjectTypeDB::bind_method(_MD("move","rel_vec"),&KinematicBody::move); + ObjectTypeDB::bind_method(_MD("move_to","position"),&KinematicBody::move_to); + + ObjectTypeDB::bind_method(_MD("can_move_to","position"),&KinematicBody::can_move_to); + + ObjectTypeDB::bind_method(_MD("is_colliding"),&KinematicBody::is_colliding); + + ObjectTypeDB::bind_method(_MD("get_collision_pos"),&KinematicBody::get_collision_pos); + ObjectTypeDB::bind_method(_MD("get_collision_normal"),&KinematicBody::get_collision_normal); + ObjectTypeDB::bind_method(_MD("get_collider_velocity"),&KinematicBody::get_collider_velocity); + ObjectTypeDB::bind_method(_MD("get_collider:Object"),&KinematicBody::_get_collider); + + + ObjectTypeDB::bind_method(_MD("set_collide_with_static_bodies","enable"),&KinematicBody::set_collide_with_static_bodies); + ObjectTypeDB::bind_method(_MD("can_collide_with_static_bodies"),&KinematicBody::can_collide_with_static_bodies); + + ObjectTypeDB::bind_method(_MD("set_collide_with_kinematic_bodies","enable"),&KinematicBody::set_collide_with_kinematic_bodies); + ObjectTypeDB::bind_method(_MD("can_collide_with_kinematic_bodies"),&KinematicBody::can_collide_with_kinematic_bodies); + + ObjectTypeDB::bind_method(_MD("set_collide_with_rigid_bodies","enable"),&KinematicBody::set_collide_with_rigid_bodies); + ObjectTypeDB::bind_method(_MD("can_collide_with_rigid_bodies"),&KinematicBody::can_collide_with_rigid_bodies); + + ObjectTypeDB::bind_method(_MD("set_collide_with_character_bodies","enable"),&KinematicBody::set_collide_with_character_bodies); + ObjectTypeDB::bind_method(_MD("can_collide_with_character_bodies"),&KinematicBody::can_collide_with_character_bodies); + + ObjectTypeDB::bind_method(_MD("set_collision_margin","pixels"),&KinematicBody::set_collision_margin); + ObjectTypeDB::bind_method(_MD("get_collision_margin","pixels"),&KinematicBody::get_collision_margin); + + ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/static"),_SCS("set_collide_with_static_bodies"),_SCS("can_collide_with_static_bodies")); + ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/kinematic"),_SCS("set_collide_with_kinematic_bodies"),_SCS("can_collide_with_kinematic_bodies")); + ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/rigid"),_SCS("set_collide_with_rigid_bodies"),_SCS("can_collide_with_rigid_bodies")); + ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/character"),_SCS("set_collide_with_character_bodies"),_SCS("can_collide_with_character_bodies")); + ADD_PROPERTY( PropertyInfo(Variant::REAL,"collision/margin",PROPERTY_HINT_RANGE,"0.001,256,0.001"),_SCS("set_collision_margin"),_SCS("get_collision_margin")); + + +} + +KinematicBody::KinematicBody() : PhysicsBody(PhysicsServer::BODY_MODE_KINEMATIC){ + + collide_static=true; + collide_rigid=true; + collide_kinematic=true; + collide_character=true; + + colliding=false; + collider=0; + margin=0.001; +} +KinematicBody::~KinematicBody() { + + +} + diff --git a/scene/3d/physics_body.h b/scene/3d/physics_body.h index 6695ee719a..0b7a389449 100644 --- a/scene/3d/physics_body.h +++ b/scene/3d/physics_body.h @@ -38,12 +38,22 @@ class PhysicsBody : public CollisionObject { OBJ_TYPE(PhysicsBody,CollisionObject); + uint32_t layer_mask; protected: + static void _bind_methods(); void _notification(int p_what); PhysicsBody(PhysicsServer::BodyMode p_mode); public: + virtual Vector3 get_linear_velocity() const; + virtual Vector3 get_angular_velocity() const; + virtual float get_inverse_mass() const; + + void set_layer_mask(uint32_t p_mask); + uint32_t get_layer_mask() const; + + PhysicsBody(); }; @@ -52,25 +62,26 @@ class StaticBody : public PhysicsBody { OBJ_TYPE(StaticBody,PhysicsBody); - Transform *pre_xform; - //RID query; - bool setting; - bool pending; - bool simulating_motion; Vector3 constant_linear_velocity; Vector3 constant_angular_velocity; - void _update_xform(); - void _state_notify(Object *p_object); + + real_t bounce; + real_t friction; + protected: - void _notification(int p_what); static void _bind_methods(); public: - void set_simulate_motion(bool p_enable); - bool is_simulating_motion() const; + + void set_friction(real_t p_friction); + real_t get_friction() const; + + void set_bounce(real_t p_bounce); + real_t get_bounce() const; + void set_constant_linear_velocity(const Vector3& p_vel); void set_constant_angular_velocity(const Vector3& p_vel); @@ -183,6 +194,8 @@ public: void set_mass(real_t p_mass); real_t get_mass() const; + virtual float get_inverse_mass() const { return 1.0/mass; } + void set_weight(real_t p_weight); real_t get_weight() const; @@ -231,4 +244,70 @@ public: VARIANT_ENUM_CAST(RigidBody::Mode); VARIANT_ENUM_CAST(RigidBody::AxisLock); + + + + + +class KinematicBody : public PhysicsBody { + + OBJ_TYPE(KinematicBody,PhysicsBody); + + float margin; + bool collide_static; + bool collide_rigid; + bool collide_kinematic; + bool collide_character; + + bool colliding; + Vector3 collision; + Vector3 normal; + Vector3 collider_vel; + ObjectID collider; + + + Variant _get_collider() const; + + _FORCE_INLINE_ bool _ignores_mode(PhysicsServer::BodyMode) const; +protected: + + static void _bind_methods(); +public: + + enum { + SLIDE_FLAG_FLOOR, + SLIDE_FLAG_WALL, + SLIDE_FLAG_ROOF + }; + + Vector3 move(const Vector3& p_motion); + Vector3 move_to(const Vector3& p_position); + + bool can_move_to(const Vector3& p_position,bool p_discrete=false); + bool is_colliding() const; + Vector3 get_collision_pos() const; + Vector3 get_collision_normal() const; + Vector3 get_collider_velocity() const; + ObjectID get_collider() const; + + void set_collide_with_static_bodies(bool p_enable); + bool can_collide_with_static_bodies() const; + + void set_collide_with_rigid_bodies(bool p_enable); + bool can_collide_with_rigid_bodies() const; + + void set_collide_with_kinematic_bodies(bool p_enable); + bool can_collide_with_kinematic_bodies() const; + + void set_collide_with_character_bodies(bool p_enable); + bool can_collide_with_character_bodies() const; + + void set_collision_margin(float p_margin); + float get_collision_margin() const; + + KinematicBody(); + ~KinematicBody(); + +}; + #endif // PHYSICS_BODY__H diff --git a/scene/3d/physics_joint.cpp b/scene/3d/physics_joint.cpp index 961198c8d4..341b02314d 100644 --- a/scene/3d/physics_joint.cpp +++ b/scene/3d/physics_joint.cpp @@ -27,7 +27,1017 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ #include "physics_joint.h" - + + + +void Joint::_update_joint(bool p_only_free) { + + + if (joint.is_valid()) { + if (ba.is_valid() && bb.is_valid()) + PhysicsServer::get_singleton()->body_remove_collision_exception(ba,bb); + PhysicsServer::get_singleton()->free(joint); + joint=RID(); + ba=RID(); + bb=RID(); + } + + if (p_only_free || !is_inside_scene()) + return; + + Node *node_a = has_node( get_node_a() ) ? get_node( get_node_a() ) : (Node*)NULL; + Node *node_b = has_node( get_node_b() ) ? get_node( get_node_b() ) : (Node*)NULL; + + if (!node_a && !node_b) + return; + + PhysicsBody *body_a=node_a ? node_a->cast_to<PhysicsBody>() : (PhysicsBody*)NULL; + PhysicsBody *body_b=node_b ? node_b->cast_to<PhysicsBody>() : (PhysicsBody*)NULL; + + if (!body_a && !body_b) + return; + + if (!body_a) { + SWAP(body_a,body_b); + } else if (body_b) { + //add a collision exception between both + PhysicsServer::get_singleton()->body_add_collision_exception(body_a->get_rid(),body_b->get_rid()); + } + + joint = _configure_joint(body_a,body_b); + + if (body_b && joint.is_valid()) { + + ba=body_a->get_rid(); + bb=body_b->get_rid(); + PhysicsServer::get_singleton()->body_add_collision_exception(body_a->get_rid(),body_b->get_rid()); + + } + +} + + +void Joint::set_node_a(const NodePath& p_node_a) { + + + if (a==p_node_a) + return; + + a=p_node_a; + _update_joint(); +} + +NodePath Joint::get_node_a() const{ + + return a; +} + +void Joint::set_node_b(const NodePath& p_node_b){ + + if (b==p_node_b) + return; + b=p_node_b; + _update_joint(); + +} +NodePath Joint::get_node_b() const{ + + + return b; +} + + +void Joint::_notification(int p_what) { + + switch(p_what) { + + case NOTIFICATION_READY: { + _update_joint(); + } break; + case NOTIFICATION_EXIT_SCENE: { + if (joint.is_valid()) { + _update_joint(true); + + + PhysicsServer::get_singleton()->free(joint); + joint=RID(); + } + } break; + + } + +} + + +void Joint::_bind_methods() { + + + ObjectTypeDB::bind_method( _MD("set_node_a","node"), &Joint::set_node_a ); + ObjectTypeDB::bind_method( _MD("get_node_a"), &Joint::get_node_a ); + + ObjectTypeDB::bind_method( _MD("set_node_b","node"), &Joint::set_node_b ); + ObjectTypeDB::bind_method( _MD("get_node_b"), &Joint::get_node_b ); + + + ADD_PROPERTY( PropertyInfo( Variant::NODE_PATH, "nodes/node_a"), _SCS("set_node_a"),_SCS("get_node_a") ); + ADD_PROPERTY( PropertyInfo( Variant::NODE_PATH, "nodes/node_b"), _SCS("set_node_b"),_SCS("get_node_b") ); + +} + + + +Joint::Joint() { + + +} + + +/////////////////////////////////// + +void PinJoint::_bind_methods() { + + ObjectTypeDB::bind_method(_MD("set_param","param","value"),&PinJoint::set_param); + ObjectTypeDB::bind_method(_MD("get_param","param"),&PinJoint::get_param); + + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"params/bias",PROPERTY_HINT_RANGE,"0.01,0.99,0.01"),_SCS("set_param"),_SCS("get_param"), PARAM_BIAS ); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"params/damping",PROPERTY_HINT_RANGE,"0.01,8.0,0.01"),_SCS("set_param"),_SCS("get_param"), PARAM_DAMPING ); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"params/impulse_clamp",PROPERTY_HINT_RANGE,"0.0,64.0,0.01"),_SCS("set_param"),_SCS("get_param"), PARAM_IMPULSE_CLAMP ); + + BIND_CONSTANT( PARAM_BIAS ); + BIND_CONSTANT( PARAM_DAMPING ); + BIND_CONSTANT( PARAM_IMPULSE_CLAMP ); +} + +void PinJoint::set_param(Param p_param,float p_value){ + + ERR_FAIL_INDEX(p_param,3); + params[p_param]=p_value; + if (get_joint().is_valid()) + PhysicsServer::get_singleton()->pin_joint_set_param(get_joint(),PhysicsServer::PinJointParam(p_param),p_value); +} +float PinJoint::get_param(Param p_param) const{ + + ERR_FAIL_INDEX_V(p_param,3,0); + return params[p_param]; +} + + +RID PinJoint::_configure_joint(PhysicsBody *body_a,PhysicsBody *body_b) { + + + Vector3 pinpos = get_global_transform().origin; + Vector3 local_a = body_a->get_global_transform().affine_inverse().xform(pinpos); + Vector3 local_b; + + if (body_b) + local_b = body_b->get_global_transform().affine_inverse().xform(pinpos); + else + local_b=pinpos; + + RID j = PhysicsServer::get_singleton()->joint_create_pin(body_a->get_rid(),local_a,body_b?body_b->get_rid():RID(),local_b); + for(int i=0;i<3;i++) { + PhysicsServer::get_singleton()->pin_joint_set_param(j,PhysicsServer::PinJointParam(i),params[i]); + } + return j; +} + + +PinJoint::PinJoint() { + + params[PARAM_BIAS]=0.3; + params[PARAM_DAMPING]=1; + params[PARAM_IMPULSE_CLAMP]=0; + +} + + + + +///////////////////////////////////////////////// + + + +/////////////////////////////////// + + +void HingeJoint::_set_upper_limit(float p_limit) { + + set_param(PARAM_LIMIT_UPPER,Math::deg2rad(p_limit)); +} + +float HingeJoint::_get_upper_limit() const { + + return Math::rad2deg(get_param(PARAM_LIMIT_UPPER)); +} + +void HingeJoint::_set_lower_limit(float p_limit) { + + set_param(PARAM_LIMIT_LOWER,Math::deg2rad(p_limit)); + +} + +float HingeJoint::_get_lower_limit() const { + + return Math::rad2deg(get_param(PARAM_LIMIT_LOWER)); + +} + + +void HingeJoint::_bind_methods() { + + ObjectTypeDB::bind_method(_MD("set_param","param","value"),&HingeJoint::set_param); + ObjectTypeDB::bind_method(_MD("get_param","param"),&HingeJoint::get_param); + + ObjectTypeDB::bind_method(_MD("set_flag","flag","enabled"),&HingeJoint::set_flag); + ObjectTypeDB::bind_method(_MD("get_flag","flag"),&HingeJoint::get_flag); + + ObjectTypeDB::bind_method(_MD("_set_upper_limit","upper_limit"),&HingeJoint::_set_upper_limit); + ObjectTypeDB::bind_method(_MD("_get_upper_limit"),&HingeJoint::_get_upper_limit); + + ObjectTypeDB::bind_method(_MD("_set_lower_limit","lower_limit"),&HingeJoint::_set_lower_limit); + ObjectTypeDB::bind_method(_MD("_get_lower_limit"),&HingeJoint::_get_lower_limit); + + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"params/bias",PROPERTY_HINT_RANGE,"0.01,0.99,0.01"),_SCS("set_param"),_SCS("get_param"), PARAM_BIAS ); + + ADD_PROPERTYI( PropertyInfo(Variant::BOOL,"angular_limit/enable"),_SCS("set_flag"),_SCS("get_flag"), FLAG_USE_LIMIT ); + ADD_PROPERTY( PropertyInfo(Variant::REAL,"angular_limit/upper",PROPERTY_HINT_RANGE,"-180,180,0.1"),_SCS("_set_upper_limit"),_SCS("_get_upper_limit") ); + ADD_PROPERTY( PropertyInfo(Variant::REAL,"angular_limit/lower",PROPERTY_HINT_RANGE,"-180,180,0.1"),_SCS("_set_lower_limit"),_SCS("_get_lower_limit") ); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"angular_limit/bias",PROPERTY_HINT_RANGE,"0.01,0.99,0.01"),_SCS("set_param"),_SCS("get_param"), PARAM_LIMIT_BIAS ); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"angular_limit/softness",PROPERTY_HINT_RANGE,"0.01,16,0.01"),_SCS("set_param"),_SCS("get_param"), PARAM_LIMIT_SOFTNESS ); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"angular_limit/relaxation",PROPERTY_HINT_RANGE,"0.01,16,0.01"),_SCS("set_param"),_SCS("get_param"), PARAM_LIMIT_RELAXATION ); + + ADD_PROPERTYI( PropertyInfo(Variant::BOOL,"motor/enable"),_SCS("set_flag"),_SCS("get_flag"), FLAG_ENABLE_MOTOR ); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"motor/target_velocity",PROPERTY_HINT_RANGE,"0.01,4096,0.01"),_SCS("set_param"),_SCS("get_param"), PARAM_MOTOR_TARGET_VELOCITY ); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"motor/max_impulse",PROPERTY_HINT_RANGE,"0.01,1024,0.01"),_SCS("set_param"),_SCS("get_param"), PARAM_MOTOR_MAX_IMPULSE); + + + BIND_CONSTANT( PARAM_BIAS ); + BIND_CONSTANT( PARAM_LIMIT_UPPER ); + BIND_CONSTANT( PARAM_LIMIT_LOWER ); + BIND_CONSTANT( PARAM_LIMIT_BIAS ); + BIND_CONSTANT( PARAM_LIMIT_SOFTNESS ); + BIND_CONSTANT( PARAM_LIMIT_RELAXATION ); + BIND_CONSTANT( PARAM_MOTOR_TARGET_VELOCITY ); + BIND_CONSTANT( PARAM_MOTOR_MAX_IMPULSE ); + BIND_CONSTANT( PARAM_MAX ); + + BIND_CONSTANT( FLAG_USE_LIMIT ); + BIND_CONSTANT( FLAG_ENABLE_MOTOR ); + BIND_CONSTANT( FLAG_MAX ); + +} + +void HingeJoint::set_param(Param p_param,float p_value){ + + ERR_FAIL_INDEX(p_param,PARAM_MAX); + params[p_param]=p_value; + if (get_joint().is_valid()) + PhysicsServer::get_singleton()->hinge_joint_set_param(get_joint(),PhysicsServer::HingeJointParam(p_param),p_value); + + update_gizmo(); + +} +float HingeJoint::get_param(Param p_param) const{ + + ERR_FAIL_INDEX_V(p_param,PARAM_MAX,0); + return params[p_param]; +} + + +void HingeJoint::set_flag(Flag p_flag,bool p_value){ + + ERR_FAIL_INDEX(p_flag,FLAG_MAX); + flags[p_flag]=p_value; + if (get_joint().is_valid()) + PhysicsServer::get_singleton()->hinge_joint_set_flag(get_joint(),PhysicsServer::HingeJointFlag(p_flag),p_value); + + update_gizmo(); +} +bool HingeJoint::get_flag(Flag p_flag) const{ + + ERR_FAIL_INDEX_V(p_flag,FLAG_MAX,false); + return flags[p_flag]; +} + +RID HingeJoint::_configure_joint(PhysicsBody *body_a,PhysicsBody *body_b) { + + + Transform gt = get_global_transform(); + Vector3 hingepos = gt.origin; + Vector3 hingedir = gt.basis.get_axis(2); + + Transform ainv = body_a->get_global_transform().affine_inverse(); + + Transform local_a = ainv * gt; + local_a.orthonormalize(); + Transform local_b = gt; + + if (body_b) { + Transform binv = body_b->get_global_transform().affine_inverse(); + local_b = binv * gt; + } + + local_b.orthonormalize(); + + RID j = PhysicsServer::get_singleton()->joint_create_hinge(body_a->get_rid(),local_a,body_b?body_b->get_rid():RID(),local_b); + for(int i=0;i<PARAM_MAX;i++) { + PhysicsServer::get_singleton()->hinge_joint_set_param(j,PhysicsServer::HingeJointParam(i),params[i]); + } + for(int i=0;i<FLAG_MAX;i++) { + set_flag(Flag(i),flags[i]); + PhysicsServer::get_singleton()->hinge_joint_set_flag(j,PhysicsServer::HingeJointFlag(i),flags[i]); + } + return j; +} + + +HingeJoint::HingeJoint() { + + params[PARAM_BIAS]=0.3; + params[PARAM_LIMIT_UPPER]=Math_PI*0.5; + params[PARAM_LIMIT_LOWER]=-Math_PI*0.5; + params[PARAM_LIMIT_BIAS]=0.3; + params[PARAM_LIMIT_SOFTNESS]=0.9; + params[PARAM_LIMIT_RELAXATION]=1.0; + params[PARAM_MOTOR_TARGET_VELOCITY]=1; + params[PARAM_MOTOR_MAX_IMPULSE]=1; + + + flags[FLAG_USE_LIMIT]=false; + flags[FLAG_ENABLE_MOTOR]=false; + +} + + + + +///////////////////////////////////////////////// + + +////////////////////////////////// + + + +void SliderJoint::_set_upper_limit_angular(float p_limit_angular) { + + set_param(PARAM_ANGULAR_LIMIT_UPPER,Math::deg2rad(p_limit_angular)); +} + +float SliderJoint::_get_upper_limit_angular() const { + + return Math::rad2deg(get_param(PARAM_ANGULAR_LIMIT_UPPER)); +} + +void SliderJoint::_set_lower_limit_angular(float p_limit_angular) { + + set_param(PARAM_ANGULAR_LIMIT_LOWER,Math::deg2rad(p_limit_angular)); + +} + +float SliderJoint::_get_lower_limit_angular() const { + + return Math::rad2deg(get_param(PARAM_ANGULAR_LIMIT_LOWER)); + +} + + +void SliderJoint::_bind_methods() { + + ObjectTypeDB::bind_method(_MD("set_param","param","value"),&SliderJoint::set_param); + ObjectTypeDB::bind_method(_MD("get_param","param"),&SliderJoint::get_param); + + + ObjectTypeDB::bind_method(_MD("_set_upper_limit_angular","upper_limit_angular"),&SliderJoint::_set_upper_limit_angular); + ObjectTypeDB::bind_method(_MD("_get_upper_limit_angular"),&SliderJoint::_get_upper_limit_angular); + + ObjectTypeDB::bind_method(_MD("_set_lower_limit_angular","lower_limit_angular"),&SliderJoint::_set_lower_limit_angular); + ObjectTypeDB::bind_method(_MD("_get_lower_limit_angular"),&SliderJoint::_get_lower_limit_angular); + + + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"linear_limit/upper_distance",PROPERTY_HINT_RANGE,"-1024,1024,0.01"),_SCS("set_param"),_SCS("get_param"), PARAM_LINEAR_LIMIT_UPPER); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"linear_limit/lower_distance",PROPERTY_HINT_RANGE,"-1024,1024,0.01"),_SCS("set_param"),_SCS("get_param"), PARAM_LINEAR_LIMIT_LOWER); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"linear_limit/softness",PROPERTY_HINT_RANGE,"0.01,16.0,0.01") ,_SCS("set_param"),_SCS("get_param"), PARAM_LINEAR_LIMIT_SOFTNESS); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"linear_limit/restitution",PROPERTY_HINT_RANGE,"0.01,16.0,0.01") ,_SCS("set_param"),_SCS("get_param"), PARAM_LINEAR_LIMIT_RESTITUTION); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"linear_limit/damping",PROPERTY_HINT_RANGE,"0,16.0,0.01") ,_SCS("set_param"),_SCS("get_param"), PARAM_LINEAR_LIMIT_DAMPING); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"linear_motion/softness",PROPERTY_HINT_RANGE,"0.01,16.0,0.01") ,_SCS("set_param"),_SCS("get_param"), PARAM_LINEAR_MOTION_SOFTNESS); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"linear_motion/restitution",PROPERTY_HINT_RANGE,"0.01,16.0,0.01") ,_SCS("set_param"),_SCS("get_param"), PARAM_LINEAR_MOTION_RESTITUTION); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"linear_motion/damping",PROPERTY_HINT_RANGE,"0,16.0,0.01") ,_SCS("set_param"),_SCS("get_param"), PARAM_LINEAR_MOTION_DAMPING); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"linear_ortho/softness",PROPERTY_HINT_RANGE,"0.01,16.0,0.01") ,_SCS("set_param"),_SCS("get_param"), PARAM_LINEAR_ORTHOGONAL_SOFTNESS); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"linear_ortho/restitution",PROPERTY_HINT_RANGE,"0.01,16.0,0.01") ,_SCS("set_param"),_SCS("get_param"), PARAM_LINEAR_ORTHOGONAL_RESTITUTION); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"linear_ortho/damping",PROPERTY_HINT_RANGE,"0,16.0,0.01") ,_SCS("set_param"),_SCS("get_param"), PARAM_LINEAR_ORTHOGONAL_DAMPING); + + ADD_PROPERTY( PropertyInfo(Variant::REAL,"angular_limit/upper_angle",PROPERTY_HINT_RANGE,"-180,180,0.1"),_SCS("_set_upper_limit_angular"),_SCS("_get_upper_limit_angular") ); + ADD_PROPERTY( PropertyInfo(Variant::REAL,"angular_limit/lower_angle",PROPERTY_HINT_RANGE,"-180,180,0.1"),_SCS("_set_lower_limit_angular"),_SCS("_get_lower_limit_angular") ); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"angular_limit/softness",PROPERTY_HINT_RANGE,"0.01,16.0,0.01") ,_SCS("set_param"),_SCS("get_param"), PARAM_ANGULAR_LIMIT_SOFTNESS); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"angular_limit/restitution",PROPERTY_HINT_RANGE,"0.01,16.0,0.01") ,_SCS("set_param"),_SCS("get_param"), PARAM_ANGULAR_LIMIT_RESTITUTION); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"angular_limit/damping",PROPERTY_HINT_RANGE,"0,16.0,0.01") ,_SCS("set_param"),_SCS("get_param"), PARAM_ANGULAR_LIMIT_DAMPING); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"angular_motion/softness",PROPERTY_HINT_RANGE,"0.01,16.0,0.01") ,_SCS("set_param"),_SCS("get_param"), PARAM_ANGULAR_MOTION_SOFTNESS); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"angular_motion/restitution",PROPERTY_HINT_RANGE,"0.01,16.0,0.01") ,_SCS("set_param"),_SCS("get_param"), PARAM_ANGULAR_MOTION_RESTITUTION); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"angular_motion/damping",PROPERTY_HINT_RANGE,"0,16.0,0.01") ,_SCS("set_param"),_SCS("get_param"), PARAM_ANGULAR_MOTION_DAMPING); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"angular_ortho/softness",PROPERTY_HINT_RANGE,"0.01,16.0,0.01") ,_SCS("set_param"),_SCS("get_param"), PARAM_ANGULAR_ORTHOGONAL_SOFTNESS); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"angular_ortho/restitution",PROPERTY_HINT_RANGE,"0.01,16.0,0.01") ,_SCS("set_param"),_SCS("get_param"), PARAM_ANGULAR_ORTHOGONAL_RESTITUTION); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"angular_ortho/damping",PROPERTY_HINT_RANGE,"0,16.0,0.01") ,_SCS("set_param"),_SCS("get_param"), PARAM_ANGULAR_ORTHOGONAL_DAMPING); + + + BIND_CONSTANT( PARAM_LINEAR_LIMIT_UPPER); + BIND_CONSTANT( PARAM_LINEAR_LIMIT_LOWER); + BIND_CONSTANT( PARAM_LINEAR_LIMIT_SOFTNESS); + BIND_CONSTANT( PARAM_LINEAR_LIMIT_RESTITUTION); + BIND_CONSTANT( PARAM_LINEAR_LIMIT_DAMPING); + BIND_CONSTANT( PARAM_LINEAR_MOTION_SOFTNESS); + BIND_CONSTANT( PARAM_LINEAR_MOTION_RESTITUTION); + BIND_CONSTANT( PARAM_LINEAR_MOTION_DAMPING); + BIND_CONSTANT( PARAM_LINEAR_ORTHOGONAL_SOFTNESS); + BIND_CONSTANT( PARAM_LINEAR_ORTHOGONAL_RESTITUTION); + BIND_CONSTANT( PARAM_LINEAR_ORTHOGONAL_DAMPING); + + BIND_CONSTANT( PARAM_ANGULAR_LIMIT_UPPER); + BIND_CONSTANT( PARAM_ANGULAR_LIMIT_LOWER); + BIND_CONSTANT( PARAM_ANGULAR_LIMIT_SOFTNESS); + BIND_CONSTANT( PARAM_ANGULAR_LIMIT_RESTITUTION); + BIND_CONSTANT( PARAM_ANGULAR_LIMIT_DAMPING); + BIND_CONSTANT( PARAM_ANGULAR_MOTION_SOFTNESS); + BIND_CONSTANT( PARAM_ANGULAR_MOTION_RESTITUTION); + BIND_CONSTANT( PARAM_ANGULAR_MOTION_DAMPING); + BIND_CONSTANT( PARAM_ANGULAR_ORTHOGONAL_SOFTNESS); + BIND_CONSTANT( PARAM_ANGULAR_ORTHOGONAL_RESTITUTION); + BIND_CONSTANT( PARAM_ANGULAR_ORTHOGONAL_DAMPING); + + BIND_CONSTANT( PARAM_MAX); +} + +void SliderJoint::set_param(Param p_param,float p_value){ + + ERR_FAIL_INDEX(p_param,PARAM_MAX); + params[p_param]=p_value; + if (get_joint().is_valid()) + PhysicsServer::get_singleton()->slider_joint_set_param(get_joint(),PhysicsServer::SliderJointParam(p_param),p_value); + update_gizmo(); + +} +float SliderJoint::get_param(Param p_param) const{ + + ERR_FAIL_INDEX_V(p_param,PARAM_MAX,0); + return params[p_param]; +} + + +RID SliderJoint::_configure_joint(PhysicsBody *body_a,PhysicsBody *body_b) { + + + Transform gt = get_global_transform(); + Vector3 sliderpos = gt.origin; + Vector3 sliderdir = gt.basis.get_axis(2); + + Transform ainv = body_a->get_global_transform().affine_inverse(); + + Transform local_a = ainv * gt; + local_a.orthonormalize(); + Transform local_b = gt; + + if (body_b) { + Transform binv = body_b->get_global_transform().affine_inverse(); + local_b = binv * gt; + } + + local_b.orthonormalize(); + + RID j = PhysicsServer::get_singleton()->joint_create_slider(body_a->get_rid(),local_a,body_b?body_b->get_rid():RID(),local_b); + for(int i=0;i<PARAM_MAX;i++) { + PhysicsServer::get_singleton()->slider_joint_set_param(j,PhysicsServer::SliderJointParam(i),params[i]); + } + + return j; +} + + +SliderJoint::SliderJoint() { + + + + params[ PARAM_LINEAR_LIMIT_UPPER ]=1.0; + params[ PARAM_LINEAR_LIMIT_LOWER ]=-1.0; + params[ PARAM_LINEAR_LIMIT_SOFTNESS ]=1.0; + params[ PARAM_LINEAR_LIMIT_RESTITUTION]=0.7; + params[ PARAM_LINEAR_LIMIT_DAMPING]=1.0; + params[ PARAM_LINEAR_MOTION_SOFTNESS ]=1.0; + params[ PARAM_LINEAR_MOTION_RESTITUTION]=0.7; + params[ PARAM_LINEAR_MOTION_DAMPING]=0;//1.0; + params[ PARAM_LINEAR_ORTHOGONAL_SOFTNESS ]=1.0; + params[ PARAM_LINEAR_ORTHOGONAL_RESTITUTION]=0.7; + params[ PARAM_LINEAR_ORTHOGONAL_DAMPING]=1.0; + + params[ PARAM_ANGULAR_LIMIT_UPPER ]=0 ; + params[ PARAM_ANGULAR_LIMIT_LOWER ]=0 ; + params[ PARAM_ANGULAR_LIMIT_SOFTNESS ]=1.0; + params[ PARAM_ANGULAR_LIMIT_RESTITUTION]=0.7; + params[ PARAM_ANGULAR_LIMIT_DAMPING]=0;//1.0; + params[ PARAM_ANGULAR_MOTION_SOFTNESS ]=1.0; + params[ PARAM_ANGULAR_MOTION_RESTITUTION]=0.7; + params[ PARAM_ANGULAR_MOTION_DAMPING]=1.0; + params[ PARAM_ANGULAR_ORTHOGONAL_SOFTNESS ]=1.0; + params[ PARAM_ANGULAR_ORTHOGONAL_RESTITUTION]=0.7; + params[ PARAM_ANGULAR_ORTHOGONAL_DAMPING]=1.0; +} + + + +////////////////////////////////// + + + +void ConeTwistJoint::_set_swing_span(float p_limit_angular) { + + set_param(PARAM_SWING_SPAN,Math::deg2rad(p_limit_angular)); +} + +float ConeTwistJoint::_get_swing_span() const { + + return Math::rad2deg(get_param(PARAM_SWING_SPAN)); +} + +void ConeTwistJoint::_set_twist_span(float p_limit_angular) { + + set_param(PARAM_TWIST_SPAN,Math::deg2rad(p_limit_angular)); + +} + +float ConeTwistJoint::_get_twist_span() const { + + return Math::rad2deg(get_param(PARAM_TWIST_SPAN)); + +} + + +void ConeTwistJoint::_bind_methods() { + + ObjectTypeDB::bind_method(_MD("set_param","param","value"),&ConeTwistJoint::set_param); + ObjectTypeDB::bind_method(_MD("get_param","param"),&ConeTwistJoint::get_param); + + + ObjectTypeDB::bind_method(_MD("_set_swing_span","swing_span"),&ConeTwistJoint::_set_swing_span); + ObjectTypeDB::bind_method(_MD("_get_swing_span"),&ConeTwistJoint::_get_swing_span); + + ObjectTypeDB::bind_method(_MD("_set_twist_span","twist_span"),&ConeTwistJoint::_set_twist_span); + ObjectTypeDB::bind_method(_MD("_get_twist_span"),&ConeTwistJoint::_get_twist_span); + + + ADD_PROPERTY( PropertyInfo(Variant::REAL,"swing_span",PROPERTY_HINT_RANGE,"-180,180,0.1"),_SCS("_set_swing_span"),_SCS("_get_swing_span") ); + ADD_PROPERTY( PropertyInfo(Variant::REAL,"twist_span",PROPERTY_HINT_RANGE,"-40000,40000,0.1"),_SCS("_set_twist_span"),_SCS("_get_twist_span") ); + + + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"bias",PROPERTY_HINT_RANGE,"0.01,16.0,0.01") ,_SCS("set_param"),_SCS("get_param"), PARAM_BIAS ); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"softness",PROPERTY_HINT_RANGE,"0.01,16.0,0.01") ,_SCS("set_param"),_SCS("get_param"), PARAM_SOFTNESS); + ADD_PROPERTYI( PropertyInfo(Variant::REAL,"relaxation",PROPERTY_HINT_RANGE,"0.01,16.0,0.01") ,_SCS("set_param"),_SCS("get_param"), PARAM_RELAXATION); + + BIND_CONSTANT( PARAM_SWING_SPAN ); + BIND_CONSTANT( PARAM_TWIST_SPAN ); + BIND_CONSTANT( PARAM_BIAS ); + BIND_CONSTANT( PARAM_SOFTNESS ); + BIND_CONSTANT( PARAM_RELAXATION ); + BIND_CONSTANT( PARAM_MAX ); +} + +void ConeTwistJoint::set_param(Param p_param,float p_value){ + + ERR_FAIL_INDEX(p_param,PARAM_MAX); + params[p_param]=p_value; + if (get_joint().is_valid()) + PhysicsServer::get_singleton()->cone_twist_joint_set_param(get_joint(),PhysicsServer::ConeTwistJointParam(p_param),p_value); + + update_gizmo(); +} +float ConeTwistJoint::get_param(Param p_param) const{ + + ERR_FAIL_INDEX_V(p_param,PARAM_MAX,0); + return params[p_param]; +} + + +RID ConeTwistJoint::_configure_joint(PhysicsBody *body_a,PhysicsBody *body_b) { + + + Transform gt = get_global_transform(); + //Vector3 cone_twistpos = gt.origin; + //Vector3 cone_twistdir = gt.basis.get_axis(2); + + Transform ainv = body_a->get_global_transform().affine_inverse(); + + Transform local_a = ainv * gt; + local_a.orthonormalize(); + Transform local_b = gt; + + if (body_b) { + Transform binv = body_b->get_global_transform().affine_inverse(); + local_b = binv * gt; + } + + local_b.orthonormalize(); + + RID j = PhysicsServer::get_singleton()->joint_create_cone_twist(body_a->get_rid(),local_a,body_b?body_b->get_rid():RID(),local_b); + for(int i=0;i<PARAM_MAX;i++) { + PhysicsServer::get_singleton()->cone_twist_joint_set_param(j,PhysicsServer::ConeTwistJointParam(i),params[i]); + } + + return j; +} + + +ConeTwistJoint::ConeTwistJoint() { + + + params[ PARAM_SWING_SPAN ]=Math_PI*0.25; + params[ PARAM_TWIST_SPAN ]=Math_PI; + params[ PARAM_BIAS ]=0.3; + params[ PARAM_SOFTNESS ]=0.8; + params[ PARAM_RELAXATION ]=1.0; + +} + +///////////////////////////////////////////////////////////////////// + + +void Generic6DOFJoint::_set_angular_hi_limit_x(float p_limit_angular) { + + set_param_x(PARAM_ANGULAR_UPPER_LIMIT,Math::deg2rad(p_limit_angular)); +} + +float Generic6DOFJoint::_get_angular_hi_limit_x() const{ + + return Math::rad2deg(get_param_x(PARAM_ANGULAR_UPPER_LIMIT)); + +} + +void Generic6DOFJoint::_set_angular_lo_limit_x(float p_limit_angular) { + + set_param_x(PARAM_ANGULAR_LOWER_LIMIT,Math::deg2rad(p_limit_angular)); +} + +float Generic6DOFJoint::_get_angular_lo_limit_x() const{ + + return Math::rad2deg(get_param_x(PARAM_ANGULAR_LOWER_LIMIT)); + +} + + +void Generic6DOFJoint::_set_angular_hi_limit_y(float p_limit_angular) { + + set_param_y(PARAM_ANGULAR_UPPER_LIMIT,Math::deg2rad(p_limit_angular)); +} + +float Generic6DOFJoint::_get_angular_hi_limit_y() const{ + + return Math::rad2deg(get_param_y(PARAM_ANGULAR_UPPER_LIMIT)); + +} + +void Generic6DOFJoint::_set_angular_lo_limit_y(float p_limit_angular) { + + set_param_y(PARAM_ANGULAR_LOWER_LIMIT,Math::deg2rad(p_limit_angular)); +} + +float Generic6DOFJoint::_get_angular_lo_limit_y() const{ + + return Math::rad2deg(get_param_y(PARAM_ANGULAR_LOWER_LIMIT)); + +} + + +void Generic6DOFJoint::_set_angular_hi_limit_z(float p_limit_angular) { + + set_param_z(PARAM_ANGULAR_UPPER_LIMIT,Math::deg2rad(p_limit_angular)); +} + +float Generic6DOFJoint::_get_angular_hi_limit_z() const{ + + return Math::rad2deg(get_param_z(PARAM_ANGULAR_UPPER_LIMIT)); + +} + +void Generic6DOFJoint::_set_angular_lo_limit_z(float p_limit_angular) { + + set_param_z(PARAM_ANGULAR_LOWER_LIMIT,Math::deg2rad(p_limit_angular)); +} + +float Generic6DOFJoint::_get_angular_lo_limit_z() const{ + + return Math::rad2deg(get_param_z(PARAM_ANGULAR_LOWER_LIMIT)); + +} + + + +void Generic6DOFJoint::_bind_methods(){ + + + ObjectTypeDB::bind_method(_MD("_set_angular_hi_limit_x","angle"),&Generic6DOFJoint::_set_angular_hi_limit_x); + ObjectTypeDB::bind_method(_MD("_get_angular_hi_limit_x"),&Generic6DOFJoint::_get_angular_hi_limit_x); + + ObjectTypeDB::bind_method(_MD("_set_angular_lo_limit_x","angle"),&Generic6DOFJoint::_set_angular_lo_limit_x); + ObjectTypeDB::bind_method(_MD("_get_angular_lo_limit_x"),&Generic6DOFJoint::_get_angular_lo_limit_x); + + ObjectTypeDB::bind_method(_MD("_set_angular_hi_limit_y","angle"),&Generic6DOFJoint::_set_angular_hi_limit_y); + ObjectTypeDB::bind_method(_MD("_get_angular_hi_limit_y"),&Generic6DOFJoint::_get_angular_hi_limit_y); + + ObjectTypeDB::bind_method(_MD("_set_angular_lo_limit_y","angle"),&Generic6DOFJoint::_set_angular_lo_limit_y); + ObjectTypeDB::bind_method(_MD("_get_angular_lo_limit_y"),&Generic6DOFJoint::_get_angular_lo_limit_y); + + ObjectTypeDB::bind_method(_MD("_set_angular_hi_limit_z","angle"),&Generic6DOFJoint::_set_angular_hi_limit_z); + ObjectTypeDB::bind_method(_MD("_get_angular_hi_limit_z"),&Generic6DOFJoint::_get_angular_hi_limit_z); + + ObjectTypeDB::bind_method(_MD("_set_angular_lo_limit_z","angle"),&Generic6DOFJoint::_set_angular_lo_limit_z); + ObjectTypeDB::bind_method(_MD("_get_angular_lo_limit_z"),&Generic6DOFJoint::_get_angular_lo_limit_z); + + ObjectTypeDB::bind_method(_MD("set_param_x","param","value"),&Generic6DOFJoint::set_param_x); + ObjectTypeDB::bind_method(_MD("get_param_x","param"),&Generic6DOFJoint::get_param_x); + + ObjectTypeDB::bind_method(_MD("set_param_y","param","value"),&Generic6DOFJoint::set_param_y); + ObjectTypeDB::bind_method(_MD("get_param_y","param"),&Generic6DOFJoint::get_param_y); + + ObjectTypeDB::bind_method(_MD("set_param_z","param","value"),&Generic6DOFJoint::set_param_z); + ObjectTypeDB::bind_method(_MD("get_param_z","param"),&Generic6DOFJoint::get_param_z); + + ObjectTypeDB::bind_method(_MD("set_flag_x","flag","value"),&Generic6DOFJoint::set_flag_x); + ObjectTypeDB::bind_method(_MD("get_flag_x","flag"),&Generic6DOFJoint::get_flag_x); + + ObjectTypeDB::bind_method(_MD("set_flag_y","flag","value"),&Generic6DOFJoint::set_flag_y); + ObjectTypeDB::bind_method(_MD("get_flag_y","flag"),&Generic6DOFJoint::get_flag_y); + + ObjectTypeDB::bind_method(_MD("set_flag_z","flag","value"),&Generic6DOFJoint::set_flag_z); + ObjectTypeDB::bind_method(_MD("get_flag_z","flag"),&Generic6DOFJoint::get_flag_z); + + + ADD_PROPERTYI(PropertyInfo(Variant::BOOL,"linear_limit_x/enabled"),_SCS("set_flag_x"),_SCS("get_flag_x"),FLAG_ENABLE_LINEAR_LIMIT); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"linear_limit_x/upper_distance"),_SCS("set_param_x"),_SCS("get_param_x"),PARAM_LINEAR_UPPER_LIMIT); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"linear_limit_x/lower_distance"),_SCS("set_param_x"),_SCS("get_param_x"),PARAM_LINEAR_LOWER_LIMIT); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"linear_limit_x/softness",PROPERTY_HINT_RANGE,"0.01,16,0.01"),_SCS("set_param_x"),_SCS("get_param_x"),PARAM_LINEAR_LIMIT_SOFTNESS); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"linear_limit_x/restitution",PROPERTY_HINT_RANGE,"0.01,16,0.01"),_SCS("set_param_x"),_SCS("get_param_x"),PARAM_LINEAR_RESTITUTION); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"linear_limit_x/damping",PROPERTY_HINT_RANGE,"0.01,16,0.01"),_SCS("set_param_x"),_SCS("get_param_x"),PARAM_LINEAR_DAMPING); + ADD_PROPERTYI(PropertyInfo(Variant::BOOL,"angular_limit_x/enabled"),_SCS("set_flag_x"),_SCS("get_flag_x"),FLAG_ENABLE_ANGULAR_LIMIT); + ADD_PROPERTY(PropertyInfo(Variant::REAL,"angular_limit_x/upper_angle",PROPERTY_HINT_RANGE,"-180,180,0.01"),_SCS("_set_angular_hi_limit_x"),_SCS("_get_angular_hi_limit_x")); + ADD_PROPERTY(PropertyInfo(Variant::REAL,"angular_limit_x/lower_angle",PROPERTY_HINT_RANGE,"-180,180,0.01"),_SCS("_set_angular_lo_limit_x"),_SCS("_get_angular_lo_limit_x")); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"angular_limit_x/softness",PROPERTY_HINT_RANGE,"0.01,16,0.01"),_SCS("set_param_x"),_SCS("get_param_x"),PARAM_ANGULAR_LIMIT_SOFTNESS); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"angular_limit_x/restitution",PROPERTY_HINT_RANGE,"0.01,16,0.01"),_SCS("set_param_x"),_SCS("get_param_x"),PARAM_ANGULAR_RESTITUTION); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"angular_limit_x/damping",PROPERTY_HINT_RANGE,"0.01,16,0.01"),_SCS("set_param_x"),_SCS("get_param_x"),PARAM_ANGULAR_DAMPING); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"angular_limit_x/force_limit"),_SCS("set_param_x"),_SCS("get_param_x"),PARAM_ANGULAR_FORCE_LIMIT); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"angular_limit_x/erp"),_SCS("set_param_x"),_SCS("get_param_x"),PARAM_ANGULAR_ERP); + ADD_PROPERTYI(PropertyInfo(Variant::BOOL,"angular_motor_x/enabled"),_SCS("set_flag_x"),_SCS("get_flag_x"),FLAG_ENABLE_MOTOR); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"angular_motor_x/target_velocity"),_SCS("set_param_x"),_SCS("get_param_x"),PARAM_ANGULAR_MOTOR_TARGET_VELOCITY); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"angular_motor_x/force_limit"),_SCS("set_param_x"),_SCS("get_param_x"),PARAM_ANGULAR_MOTOR_FORCE_LIMIT); + + ADD_PROPERTYI(PropertyInfo(Variant::BOOL,"linear_limit_y/enabled"),_SCS("set_flag_y"),_SCS("get_flag_y"),FLAG_ENABLE_LINEAR_LIMIT); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"linear_limit_y/upper_distance"),_SCS("set_param_y"),_SCS("get_param_y"),PARAM_LINEAR_UPPER_LIMIT); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"linear_limit_y/lower_distance"),_SCS("set_param_y"),_SCS("get_param_y"),PARAM_LINEAR_LOWER_LIMIT); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"linear_limit_y/softness",PROPERTY_HINT_RANGE,"0.01,16,0.01"),_SCS("set_param_y"),_SCS("get_param_y"),PARAM_LINEAR_LIMIT_SOFTNESS); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"linear_limit_y/restitution",PROPERTY_HINT_RANGE,"0.01,16,0.01"),_SCS("set_param_y"),_SCS("get_param_y"),PARAM_LINEAR_RESTITUTION); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"linear_limit_y/damping",PROPERTY_HINT_RANGE,"0.01,16,0.01"),_SCS("set_param_y"),_SCS("get_param_y"),PARAM_LINEAR_DAMPING); + ADD_PROPERTYI(PropertyInfo(Variant::BOOL,"angular_limit_y/enabled"),_SCS("set_flag_y"),_SCS("get_flag_y"),FLAG_ENABLE_ANGULAR_LIMIT); + ADD_PROPERTY(PropertyInfo(Variant::REAL,"angular_limit_y/upper_angle",PROPERTY_HINT_RANGE,"-180,180,0.01"),_SCS("_set_angular_hi_limit_y"),_SCS("_get_angular_hi_limit_y")); + ADD_PROPERTY(PropertyInfo(Variant::REAL,"angular_limit_y/lower_angle",PROPERTY_HINT_RANGE,"-180,180,0.01"),_SCS("_set_angular_lo_limit_y"),_SCS("_get_angular_lo_limit_y")); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"angular_limit_y/softness",PROPERTY_HINT_RANGE,"0.01,16,0.01"),_SCS("set_param_y"),_SCS("get_param_y"),PARAM_ANGULAR_LIMIT_SOFTNESS); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"angular_limit_y/restitution",PROPERTY_HINT_RANGE,"0.01,16,0.01"),_SCS("set_param_y"),_SCS("get_param_y"),PARAM_ANGULAR_RESTITUTION); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"angular_limit_y/damping",PROPERTY_HINT_RANGE,"0.01,16,0.01"),_SCS("set_param_y"),_SCS("get_param_y"),PARAM_ANGULAR_DAMPING); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"angular_limit_y/force_limit"),_SCS("set_param_y"),_SCS("get_param_y"),PARAM_ANGULAR_FORCE_LIMIT); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"angular_limit_y/erp"),_SCS("set_param_y"),_SCS("get_param_y"),PARAM_ANGULAR_ERP); + ADD_PROPERTYI(PropertyInfo(Variant::BOOL,"angular_motor_y/enabled"),_SCS("set_flag_y"),_SCS("get_flag_y"),FLAG_ENABLE_MOTOR); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"angular_motor_y/target_velocity"),_SCS("set_param_y"),_SCS("get_param_y"),PARAM_ANGULAR_MOTOR_TARGET_VELOCITY); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"angular_motor_y/force_limit"),_SCS("set_param_y"),_SCS("get_param_y"),PARAM_ANGULAR_MOTOR_FORCE_LIMIT); + + ADD_PROPERTYI(PropertyInfo(Variant::BOOL,"linear_limit_z/enabled"),_SCS("set_flag_z"),_SCS("get_flag_z"),FLAG_ENABLE_LINEAR_LIMIT); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"linear_limit_z/upper_distance"),_SCS("set_param_z"),_SCS("get_param_z"),PARAM_LINEAR_UPPER_LIMIT); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"linear_limit_z/lower_distance"),_SCS("set_param_z"),_SCS("get_param_z"),PARAM_LINEAR_LOWER_LIMIT); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"linear_limit_z/softness",PROPERTY_HINT_RANGE,"0.01,16,0.01"),_SCS("set_param_z"),_SCS("get_param_z"),PARAM_LINEAR_LIMIT_SOFTNESS); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"linear_limit_z/restitution",PROPERTY_HINT_RANGE,"0.01,16,0.01"),_SCS("set_param_z"),_SCS("get_param_z"),PARAM_LINEAR_RESTITUTION); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"linear_limit_z/damping",PROPERTY_HINT_RANGE,"0.01,16,0.01"),_SCS("set_param_z"),_SCS("get_param_z"),PARAM_LINEAR_DAMPING); + ADD_PROPERTYI(PropertyInfo(Variant::BOOL,"angular_limit_z/enabled"),_SCS("set_flag_z"),_SCS("get_flag_z"),FLAG_ENABLE_ANGULAR_LIMIT); + ADD_PROPERTY(PropertyInfo(Variant::REAL,"angular_limit_z/upper_angle",PROPERTY_HINT_RANGE,"-180,180,0.01"),_SCS("_set_angular_hi_limit_z"),_SCS("_get_angular_hi_limit_z")); + ADD_PROPERTY(PropertyInfo(Variant::REAL,"angular_limit_z/lower_angle",PROPERTY_HINT_RANGE,"-180,180,0.01"),_SCS("_set_angular_lo_limit_z"),_SCS("_get_angular_lo_limit_z")); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"angular_limit_z/softness",PROPERTY_HINT_RANGE,"0.01,16,0.01"),_SCS("set_param_z"),_SCS("get_param_z"),PARAM_ANGULAR_LIMIT_SOFTNESS); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"angular_limit_z/restitution",PROPERTY_HINT_RANGE,"0.01,16,0.01"),_SCS("set_param_z"),_SCS("get_param_z"),PARAM_ANGULAR_RESTITUTION); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"angular_limit_z/damping",PROPERTY_HINT_RANGE,"0.01,16,0.01"),_SCS("set_param_z"),_SCS("get_param_z"),PARAM_ANGULAR_DAMPING); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"angular_limit_z/force_limit"),_SCS("set_param_z"),_SCS("get_param_z"),PARAM_ANGULAR_FORCE_LIMIT); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"angular_limit_z/erp"),_SCS("set_param_z"),_SCS("get_param_z"),PARAM_ANGULAR_ERP); + ADD_PROPERTYI(PropertyInfo(Variant::BOOL,"angular_motor_z/enabled"),_SCS("set_flag_z"),_SCS("get_flag_z"),FLAG_ENABLE_MOTOR); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"angular_motor_z/target_velocity"),_SCS("set_param_z"),_SCS("get_param_z"),PARAM_ANGULAR_MOTOR_TARGET_VELOCITY); + ADD_PROPERTYI(PropertyInfo(Variant::REAL,"angular_motor_z/force_limit"),_SCS("set_param_z"),_SCS("get_param_z"),PARAM_ANGULAR_MOTOR_FORCE_LIMIT); + + + BIND_CONSTANT( PARAM_LINEAR_LOWER_LIMIT); + BIND_CONSTANT( PARAM_LINEAR_UPPER_LIMIT); + BIND_CONSTANT( PARAM_LINEAR_LIMIT_SOFTNESS); + BIND_CONSTANT( PARAM_LINEAR_RESTITUTION); + BIND_CONSTANT( PARAM_LINEAR_DAMPING); + BIND_CONSTANT( PARAM_ANGULAR_LOWER_LIMIT); + BIND_CONSTANT( PARAM_ANGULAR_UPPER_LIMIT); + BIND_CONSTANT( PARAM_ANGULAR_LIMIT_SOFTNESS); + BIND_CONSTANT( PARAM_ANGULAR_DAMPING); + BIND_CONSTANT( PARAM_ANGULAR_RESTITUTION); + BIND_CONSTANT( PARAM_ANGULAR_FORCE_LIMIT); + BIND_CONSTANT( PARAM_ANGULAR_ERP); + BIND_CONSTANT( PARAM_ANGULAR_MOTOR_TARGET_VELOCITY); + BIND_CONSTANT( PARAM_ANGULAR_MOTOR_FORCE_LIMIT); + BIND_CONSTANT( PARAM_MAX); + + BIND_CONSTANT( FLAG_ENABLE_LINEAR_LIMIT); + BIND_CONSTANT( FLAG_ENABLE_ANGULAR_LIMIT); + BIND_CONSTANT( FLAG_ENABLE_MOTOR); + BIND_CONSTANT( FLAG_MAX ); +} + + +void Generic6DOFJoint::set_param_x(Param p_param,float p_value){ + + ERR_FAIL_INDEX(p_param,PARAM_MAX); + params_x[p_param]=p_value; + if (get_joint().is_valid()) + PhysicsServer::get_singleton()->generic_6dof_joint_set_param(get_joint(),Vector3::AXIS_X,PhysicsServer::G6DOFJointAxisParam(p_param),p_value); + + update_gizmo(); +} +float Generic6DOFJoint::get_param_x(Param p_param) const{ + + ERR_FAIL_INDEX_V(p_param,PARAM_MAX,0); + return params_x[p_param]; +} + +void Generic6DOFJoint::set_param_y(Param p_param,float p_value){ + + ERR_FAIL_INDEX(p_param,PARAM_MAX); + params_y[p_param]=p_value; + if (get_joint().is_valid()) + PhysicsServer::get_singleton()->generic_6dof_joint_set_param(get_joint(),Vector3::AXIS_Y,PhysicsServer::G6DOFJointAxisParam(p_param),p_value); + update_gizmo(); + +} +float Generic6DOFJoint::get_param_y(Param p_param) const{ + + ERR_FAIL_INDEX_V(p_param,PARAM_MAX,0); + return params_y[p_param]; +} + + +void Generic6DOFJoint::set_param_z(Param p_param,float p_value){ + + ERR_FAIL_INDEX(p_param,PARAM_MAX); + params_z[p_param]=p_value; + if (get_joint().is_valid()) + PhysicsServer::get_singleton()->generic_6dof_joint_set_param(get_joint(),Vector3::AXIS_Z,PhysicsServer::G6DOFJointAxisParam(p_param),p_value); + update_gizmo(); +} +float Generic6DOFJoint::get_param_z(Param p_param) const{ + + ERR_FAIL_INDEX_V(p_param,PARAM_MAX,0); + return params_z[p_param]; +} + + +void Generic6DOFJoint::set_flag_x(Flag p_flag,bool p_enabled){ + + ERR_FAIL_INDEX(p_flag,FLAG_MAX); + flags_x[p_flag]=p_enabled; + if (get_joint().is_valid()) + PhysicsServer::get_singleton()->generic_6dof_joint_set_flag(get_joint(),Vector3::AXIS_X,PhysicsServer::G6DOFJointAxisFlag(p_flag),p_enabled); + update_gizmo(); + +} +bool Generic6DOFJoint::get_flag_x(Flag p_flag) const{ + + ERR_FAIL_INDEX_V(p_flag,FLAG_MAX,false); + return flags_x[p_flag]; + +} + +void Generic6DOFJoint::set_flag_y(Flag p_flag,bool p_enabled){ + + ERR_FAIL_INDEX(p_flag,FLAG_MAX); + flags_y[p_flag]=p_enabled; + if (get_joint().is_valid()) + PhysicsServer::get_singleton()->generic_6dof_joint_set_flag(get_joint(),Vector3::AXIS_Y,PhysicsServer::G6DOFJointAxisFlag(p_flag),p_enabled); + update_gizmo(); +} +bool Generic6DOFJoint::get_flag_y(Flag p_flag) const{ + + ERR_FAIL_INDEX_V(p_flag,FLAG_MAX,false); + return flags_y[p_flag]; + +} + +void Generic6DOFJoint::set_flag_z(Flag p_flag,bool p_enabled){ + + ERR_FAIL_INDEX(p_flag,FLAG_MAX); + flags_z[p_flag]=p_enabled; + if (get_joint().is_valid()) + PhysicsServer::get_singleton()->generic_6dof_joint_set_flag(get_joint(),Vector3::AXIS_Z,PhysicsServer::G6DOFJointAxisFlag(p_flag),p_enabled); + update_gizmo(); +} +bool Generic6DOFJoint::get_flag_z(Flag p_flag) const{ + + ERR_FAIL_INDEX_V(p_flag,FLAG_MAX,false); + return flags_z[p_flag]; + +} + +RID Generic6DOFJoint::_configure_joint(PhysicsBody *body_a,PhysicsBody *body_b) { + + + Transform gt = get_global_transform(); + //Vector3 cone_twistpos = gt.origin; + //Vector3 cone_twistdir = gt.basis.get_axis(2); + + Transform ainv = body_a->get_global_transform().affine_inverse(); + + Transform local_a = ainv * gt; + local_a.orthonormalize(); + Transform local_b = gt; + + if (body_b) { + Transform binv = body_b->get_global_transform().affine_inverse(); + local_b = binv * gt; + } + + local_b.orthonormalize(); + + RID j = PhysicsServer::get_singleton()->joint_create_generic_6dof(body_a->get_rid(),local_a,body_b?body_b->get_rid():RID(),local_b); + for(int i=0;i<PARAM_MAX;i++) { + PhysicsServer::get_singleton()->generic_6dof_joint_set_param(j,Vector3::AXIS_X,PhysicsServer::G6DOFJointAxisParam(i),params_x[i]); + PhysicsServer::get_singleton()->generic_6dof_joint_set_param(j,Vector3::AXIS_Y,PhysicsServer::G6DOFJointAxisParam(i),params_y[i]); + PhysicsServer::get_singleton()->generic_6dof_joint_set_param(j,Vector3::AXIS_Z,PhysicsServer::G6DOFJointAxisParam(i),params_z[i]); + } + for(int i=0;i<FLAG_MAX;i++) { + PhysicsServer::get_singleton()->generic_6dof_joint_set_flag(j,Vector3::AXIS_X,PhysicsServer::G6DOFJointAxisFlag(i),flags_x[i]); + PhysicsServer::get_singleton()->generic_6dof_joint_set_flag(j,Vector3::AXIS_Y,PhysicsServer::G6DOFJointAxisFlag(i),flags_y[i]); + PhysicsServer::get_singleton()->generic_6dof_joint_set_flag(j,Vector3::AXIS_Z,PhysicsServer::G6DOFJointAxisFlag(i),flags_z[i]); + } + + return j; +} + + +Generic6DOFJoint::Generic6DOFJoint() { + + set_param_x( PARAM_LINEAR_LOWER_LIMIT,0); + set_param_x( PARAM_LINEAR_UPPER_LIMIT,0); + set_param_x( PARAM_LINEAR_LIMIT_SOFTNESS,0.7); + set_param_x( PARAM_LINEAR_RESTITUTION,0.5); + set_param_x( PARAM_LINEAR_DAMPING,1.0); + set_param_x( PARAM_ANGULAR_LOWER_LIMIT,0); + set_param_x( PARAM_ANGULAR_UPPER_LIMIT,0); + set_param_x( PARAM_ANGULAR_LIMIT_SOFTNESS,0.5f); + set_param_x( PARAM_ANGULAR_DAMPING,1.0f); + set_param_x( PARAM_ANGULAR_RESTITUTION,0); + set_param_x( PARAM_ANGULAR_FORCE_LIMIT,0); + set_param_x( PARAM_ANGULAR_ERP,0.5); + set_param_x( PARAM_ANGULAR_MOTOR_TARGET_VELOCITY,0); + set_param_x( PARAM_ANGULAR_MOTOR_FORCE_LIMIT,300); + + set_flag_x( FLAG_ENABLE_ANGULAR_LIMIT,true); + set_flag_x( FLAG_ENABLE_LINEAR_LIMIT,true); + set_flag_x( FLAG_ENABLE_MOTOR,false); + + set_param_y( PARAM_LINEAR_LOWER_LIMIT,0); + set_param_y( PARAM_LINEAR_UPPER_LIMIT,0); + set_param_y( PARAM_LINEAR_LIMIT_SOFTNESS,0.7); + set_param_y( PARAM_LINEAR_RESTITUTION,0.5); + set_param_y( PARAM_LINEAR_DAMPING,1.0); + set_param_y( PARAM_ANGULAR_LOWER_LIMIT,0); + set_param_y( PARAM_ANGULAR_UPPER_LIMIT,0); + set_param_y( PARAM_ANGULAR_LIMIT_SOFTNESS,0.5f); + set_param_y( PARAM_ANGULAR_DAMPING,1.0f); + set_param_y( PARAM_ANGULAR_RESTITUTION,0); + set_param_y( PARAM_ANGULAR_FORCE_LIMIT,0); + set_param_y( PARAM_ANGULAR_ERP,0.5); + set_param_y( PARAM_ANGULAR_MOTOR_TARGET_VELOCITY,0); + set_param_y( PARAM_ANGULAR_MOTOR_FORCE_LIMIT,300); + + set_flag_y( FLAG_ENABLE_ANGULAR_LIMIT,true); + set_flag_y( FLAG_ENABLE_LINEAR_LIMIT,true); + set_flag_y( FLAG_ENABLE_MOTOR,false); + + + set_param_z( PARAM_LINEAR_LOWER_LIMIT,0); + set_param_z( PARAM_LINEAR_UPPER_LIMIT,0); + set_param_z( PARAM_LINEAR_LIMIT_SOFTNESS,0.7); + set_param_z( PARAM_LINEAR_RESTITUTION,0.5); + set_param_z( PARAM_LINEAR_DAMPING,1.0); + set_param_z( PARAM_ANGULAR_LOWER_LIMIT,0); + set_param_z( PARAM_ANGULAR_UPPER_LIMIT,0); + set_param_z( PARAM_ANGULAR_LIMIT_SOFTNESS,0.5f); + set_param_z( PARAM_ANGULAR_DAMPING,1.0f); + set_param_z( PARAM_ANGULAR_RESTITUTION,0); + set_param_z( PARAM_ANGULAR_FORCE_LIMIT,0); + set_param_z( PARAM_ANGULAR_ERP,0.5); + set_param_z( PARAM_ANGULAR_MOTOR_TARGET_VELOCITY,0); + set_param_z( PARAM_ANGULAR_MOTOR_FORCE_LIMIT,300); + + set_flag_z( FLAG_ENABLE_ANGULAR_LIMIT,true); + set_flag_z( FLAG_ENABLE_LINEAR_LIMIT,true); + set_flag_z( FLAG_ENABLE_MOTOR,false); + +} + + + + #if 0 void PhysicsJoint::_set(const String& p_name, const Variant& p_value) { diff --git a/scene/3d/physics_joint.h b/scene/3d/physics_joint.h index 4a0c609e69..6daa06da2b 100644 --- a/scene/3d/physics_joint.h +++ b/scene/3d/physics_joint.h @@ -32,6 +32,313 @@ #include "scene/3d/spatial.h" #include "scene/3d/physics_body.h" + +class Joint : public Spatial { + + OBJ_TYPE(Joint,Spatial); + + RID ba,bb; + + RID joint; + + NodePath a; + NodePath b; + + +protected: + + void _update_joint(bool p_only_free=false); + + void _notification(int p_what); + + virtual RID _configure_joint(PhysicsBody *body_a,PhysicsBody *body_b)=0; + + static void _bind_methods(); +public: + + void set_node_a(const NodePath& p_node_a); + NodePath get_node_a() const; + + void set_node_b(const NodePath& p_node_b); + NodePath get_node_b() const; + + RID get_joint() const { return joint; } + Joint(); + +}; + +/////////////////////////////////////////// + + +class PinJoint : public Joint { + + OBJ_TYPE(PinJoint,Joint); +public: + + enum Param { + PARAM_BIAS=PhysicsServer::PIN_JOINT_BIAS, + PARAM_DAMPING=PhysicsServer::PIN_JOINT_DAMPING, + PARAM_IMPULSE_CLAMP=PhysicsServer::PIN_JOINT_IMPULSE_CLAMP + }; + +protected: + + float params[3]; + virtual RID _configure_joint(PhysicsBody *body_a, PhysicsBody *body_b); + static void _bind_methods(); +public: + + void set_param(Param p_param,float p_value); + float get_param(Param p_param) const; + + PinJoint(); +}; + +VARIANT_ENUM_CAST(PinJoint::Param); + + +class HingeJoint : public Joint { + + OBJ_TYPE(HingeJoint,Joint); +public: + + enum Param { + PARAM_BIAS=PhysicsServer::HINGE_JOINT_BIAS, + PARAM_LIMIT_UPPER=PhysicsServer::HINGE_JOINT_LIMIT_UPPER, + PARAM_LIMIT_LOWER=PhysicsServer::HINGE_JOINT_LIMIT_LOWER, + PARAM_LIMIT_BIAS=PhysicsServer::HINGE_JOINT_LIMIT_BIAS, + PARAM_LIMIT_SOFTNESS=PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS, + PARAM_LIMIT_RELAXATION=PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION, + PARAM_MOTOR_TARGET_VELOCITY=PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY, + PARAM_MOTOR_MAX_IMPULSE=PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE, + PARAM_MAX=PhysicsServer::HINGE_JOINT_MAX + }; + + enum Flag { + FLAG_USE_LIMIT=PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT, + FLAG_ENABLE_MOTOR=PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR, + FLAG_MAX=PhysicsServer::HINGE_JOINT_FLAG_MAX + }; + + + +protected: + + float params[PARAM_MAX]; + bool flags[FLAG_MAX]; + virtual RID _configure_joint(PhysicsBody *body_a, PhysicsBody *body_b); + static void _bind_methods(); + + void _set_upper_limit(float p_limit); + float _get_upper_limit() const; + + void _set_lower_limit(float p_limit); + float _get_lower_limit() const; + +public: + + void set_param(Param p_param,float p_value); + float get_param(Param p_param) const; + + void set_flag(Flag p_flag,bool p_value); + bool get_flag(Flag p_flag) const; + + HingeJoint(); +}; + +VARIANT_ENUM_CAST(HingeJoint::Param); +VARIANT_ENUM_CAST(HingeJoint::Flag); + + +class SliderJoint : public Joint { + + OBJ_TYPE(SliderJoint,Joint); +public: + + enum Param { + PARAM_LINEAR_LIMIT_UPPER=PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER, + PARAM_LINEAR_LIMIT_LOWER=PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER, + PARAM_LINEAR_LIMIT_SOFTNESS=PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS, + PARAM_LINEAR_LIMIT_RESTITUTION=PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION, + PARAM_LINEAR_LIMIT_DAMPING=PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING, + PARAM_LINEAR_MOTION_SOFTNESS=PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS, + PARAM_LINEAR_MOTION_RESTITUTION=PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION, + PARAM_LINEAR_MOTION_DAMPING=PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING, + PARAM_LINEAR_ORTHOGONAL_SOFTNESS=PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS, + PARAM_LINEAR_ORTHOGONAL_RESTITUTION=PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION, + PARAM_LINEAR_ORTHOGONAL_DAMPING=PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING, + + PARAM_ANGULAR_LIMIT_UPPER=PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER, + PARAM_ANGULAR_LIMIT_LOWER=PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER, + PARAM_ANGULAR_LIMIT_SOFTNESS=PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS, + PARAM_ANGULAR_LIMIT_RESTITUTION=PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION, + PARAM_ANGULAR_LIMIT_DAMPING=PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING, + PARAM_ANGULAR_MOTION_SOFTNESS=PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS, + PARAM_ANGULAR_MOTION_RESTITUTION=PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION, + PARAM_ANGULAR_MOTION_DAMPING=PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING, + PARAM_ANGULAR_ORTHOGONAL_SOFTNESS=PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS, + PARAM_ANGULAR_ORTHOGONAL_RESTITUTION=PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION, + PARAM_ANGULAR_ORTHOGONAL_DAMPING=PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING, + PARAM_MAX=PhysicsServer::SLIDER_JOINT_MAX + + }; + +protected: + + + + void _set_upper_limit_angular(float p_limit_angular); + float _get_upper_limit_angular() const; + + void _set_lower_limit_angular(float p_limit_angular); + float _get_lower_limit_angular() const; + + float params[PARAM_MAX]; + virtual RID _configure_joint(PhysicsBody *body_a, PhysicsBody *body_b); + static void _bind_methods(); +public: + + void set_param(Param p_param,float p_value); + float get_param(Param p_param) const; + + SliderJoint(); +}; + + +VARIANT_ENUM_CAST(SliderJoint::Param); + + + + +class ConeTwistJoint : public Joint { + + OBJ_TYPE(ConeTwistJoint,Joint); +public: + + enum Param { + + PARAM_SWING_SPAN, + PARAM_TWIST_SPAN, + PARAM_BIAS, + PARAM_SOFTNESS, + PARAM_RELAXATION, + PARAM_MAX + }; + +protected: + + + void _set_swing_span(float p_limit_angular); + float _get_swing_span() const; + + void _set_twist_span(float p_limit_angular); + float _get_twist_span() const; + + float params[PARAM_MAX]; + virtual RID _configure_joint(PhysicsBody *body_a, PhysicsBody *body_b); + static void _bind_methods(); +public: + + void set_param(Param p_param,float p_value); + float get_param(Param p_param) const; + + ConeTwistJoint(); +}; + + +VARIANT_ENUM_CAST(ConeTwistJoint::Param); + + +class Generic6DOFJoint : public Joint { + + OBJ_TYPE(Generic6DOFJoint,Joint); +public: + + enum Param { + + PARAM_LINEAR_LOWER_LIMIT=PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT, + PARAM_LINEAR_UPPER_LIMIT=PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT, + PARAM_LINEAR_LIMIT_SOFTNESS=PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS, + PARAM_LINEAR_RESTITUTION=PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION, + PARAM_LINEAR_DAMPING=PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING, + PARAM_ANGULAR_LOWER_LIMIT=PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT, + PARAM_ANGULAR_UPPER_LIMIT=PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT, + PARAM_ANGULAR_LIMIT_SOFTNESS=PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS, + PARAM_ANGULAR_DAMPING=PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING, + PARAM_ANGULAR_RESTITUTION=PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION, + PARAM_ANGULAR_FORCE_LIMIT=PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT, + PARAM_ANGULAR_ERP=PhysicsServer::G6DOF_JOINT_ANGULAR_ERP, + PARAM_ANGULAR_MOTOR_TARGET_VELOCITY=PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY, + PARAM_ANGULAR_MOTOR_FORCE_LIMIT=PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT, + PARAM_MAX=PhysicsServer::G6DOF_JOINT_MAX, + }; + + enum Flag { + FLAG_ENABLE_LINEAR_LIMIT=PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT, + FLAG_ENABLE_ANGULAR_LIMIT=PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT, + FLAG_ENABLE_MOTOR=PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR, + FLAG_MAX=PhysicsServer::G6DOF_JOINT_FLAG_MAX + }; + + +protected: + + + void _set_angular_hi_limit_x(float p_limit_angular); + float _get_angular_hi_limit_x() const; + + void _set_angular_hi_limit_y(float p_limit_angular); + float _get_angular_hi_limit_y() const; + + void _set_angular_hi_limit_z(float p_limit_angular); + float _get_angular_hi_limit_z() const; + + void _set_angular_lo_limit_x(float p_limit_angular); + float _get_angular_lo_limit_x() const; + + void _set_angular_lo_limit_y(float p_limit_angular); + float _get_angular_lo_limit_y() const; + + void _set_angular_lo_limit_z(float p_limit_angular); + float _get_angular_lo_limit_z() const; + + float params_x[PARAM_MAX]; + bool flags_x[FLAG_MAX]; + float params_y[PARAM_MAX]; + bool flags_y[FLAG_MAX]; + float params_z[PARAM_MAX]; + bool flags_z[FLAG_MAX]; + + virtual RID _configure_joint(PhysicsBody *body_a, PhysicsBody *body_b); + static void _bind_methods(); +public: + + void set_param_x(Param p_param,float p_value); + float get_param_x(Param p_param) const; + + void set_param_y(Param p_param,float p_value); + float get_param_y(Param p_param) const; + + void set_param_z(Param p_param,float p_value); + float get_param_z(Param p_param) const; + + void set_flag_x(Flag p_flag,bool p_enabled); + bool get_flag_x(Flag p_flag) const; + + void set_flag_y(Flag p_flag,bool p_enabled); + bool get_flag_y(Flag p_flag) const; + + void set_flag_z(Flag p_flag,bool p_enabled); + bool get_flag_z(Flag p_flag) const; + + Generic6DOFJoint(); +}; + + +VARIANT_ENUM_CAST(Generic6DOFJoint::Param); +VARIANT_ENUM_CAST(Generic6DOFJoint::Flag); + + #if 0 class PhysicsJoint : public Spatial { diff --git a/scene/3d/skeleton.cpp b/scene/3d/skeleton.cpp index 323bfa4dc4..858ee4e4ad 100644 --- a/scene/3d/skeleton.cpp +++ b/scene/3d/skeleton.cpp @@ -237,6 +237,14 @@ Transform Skeleton::get_bone_transform(int p_bone) const { return bones[p_bone].pose_global * bones[p_bone].rest_global_inverse; } +Transform Skeleton::get_bone_global_pose(int p_bone) const { + + ERR_FAIL_INDEX_V(p_bone,bones.size(),Transform()); + if (dirty) + const_cast<Skeleton*>(this)->notification(NOTIFICATION_UPDATE_SKELETON); + return bones[p_bone].pose_global; +} + RID Skeleton::get_skeleton() const { return skeleton; @@ -511,6 +519,8 @@ void Skeleton::_bind_methods() { ObjectTypeDB::bind_method(_MD("get_bone_pose","bone_idx"),&Skeleton::get_bone_pose); ObjectTypeDB::bind_method(_MD("set_bone_pose","bone_idx","pose"),&Skeleton::set_bone_pose); + ObjectTypeDB::bind_method(_MD("get_bone_global_pose","bone_idx"),&Skeleton::get_bone_global_pose); + ObjectTypeDB::bind_method(_MD("get_bone_custom_pose","bone_idx"),&Skeleton::get_bone_custom_pose); ObjectTypeDB::bind_method(_MD("set_bone_custom_pose","bone_idx","custom_pose"),&Skeleton::set_bone_custom_pose); diff --git a/scene/3d/skeleton.h b/scene/3d/skeleton.h index c95734fbf1..3e0ab0afd7 100644 --- a/scene/3d/skeleton.h +++ b/scene/3d/skeleton.h @@ -116,6 +116,7 @@ public: void set_bone_rest(int p_bone, const Transform& p_rest); Transform get_bone_rest(int p_bone) const; Transform get_bone_transform(int p_bone) const; + Transform get_bone_global_pose(int p_bone) const; void set_bone_enabled(int p_bone, bool p_enabled); bool is_bone_enabled(int p_bone) const; diff --git a/scene/3d/spatial.cpp b/scene/3d/spatial.cpp index c52503870f..13094300d0 100644 --- a/scene/3d/spatial.cpp +++ b/scene/3d/spatial.cpp @@ -506,6 +506,86 @@ Transform Spatial::get_import_transform() const { #endif +void Spatial::_propagate_visibility_changed() { + + notification(NOTIFICATION_VISIBILITY_CHANGED); + emit_signal(SceneStringNames::get_singleton()->visibility_changed); + _change_notify("visibility/visible"); + + for (List<Spatial*>::Element*E=data.children.front();E;E=E->next()) { + + Spatial *c=E->get(); + if (!c || !c->data.visible) + continue; + c->_propagate_visibility_changed(); + } +} + + +void Spatial::show() { + + if (data.visible) + return; + + data.visible=true; + + if (!is_inside_scene()) + return; + + if (!data.parent || is_visible()) { + + _propagate_visibility_changed(); + } +} + +void Spatial::hide(){ + + if (!data.visible) + return; + + bool was_visible = is_visible(); + data.visible=false; + + if (!data.parent || was_visible) { + + _propagate_visibility_changed(); + } + +} +bool Spatial::is_visible() const{ + + const Spatial *s=this; + + while(s) { + if (!s->data.visible) { + return false; + } + s=s->data.parent; + } + + return true; +} + + +bool Spatial::is_hidden() const{ + + return !data.visible; +} + +void Spatial::_set_visible_(bool p_visible) { + + if (p_visible) + show(); + else + hide(); +} + +bool Spatial::_is_visible_() const { + + return !is_hidden(); +} + + void Spatial::_bind_methods() { ObjectTypeDB::bind_method(_MD("set_transform","local"), &Spatial::set_transform); @@ -537,9 +617,18 @@ void Spatial::_bind_methods() { ObjectTypeDB::bind_method(_MD("set_gizmo","gizmo:SpatialGizmo"), &Spatial::set_gizmo); ObjectTypeDB::bind_method(_MD("get_gizmo:SpatialGizmo"), &Spatial::get_gizmo); + ObjectTypeDB::bind_method(_MD("show"), &Spatial::show); + ObjectTypeDB::bind_method(_MD("hide"), &Spatial::hide); + ObjectTypeDB::bind_method(_MD("is_visible"), &Spatial::is_visible); + ObjectTypeDB::bind_method(_MD("is_hidden"), &Spatial::is_hidden); + + ObjectTypeDB::bind_method(_MD("_set_visible_"), &Spatial::_set_visible_); + ObjectTypeDB::bind_method(_MD("_is_visible_"), &Spatial::_is_visible_); + BIND_CONSTANT( NOTIFICATION_TRANSFORM_CHANGED ); BIND_CONSTANT( NOTIFICATION_ENTER_WORLD ); BIND_CONSTANT( NOTIFICATION_EXIT_WORLD ); + BIND_CONSTANT( NOTIFICATION_VISIBILITY_CHANGED ); //ADD_PROPERTY( PropertyInfo(Variant::TRANSFORM,"transform/global",PROPERTY_HINT_NONE, "", PROPERTY_USAGE_EDITOR ), _SCS("set_global_transform"), _SCS("get_global_transform") ); ADD_PROPERTYNZ( PropertyInfo(Variant::TRANSFORM,"transform/local",PROPERTY_HINT_NONE,""), _SCS("set_transform"), _SCS("get_transform") ); @@ -547,8 +636,11 @@ void Spatial::_bind_methods() { ADD_PROPERTY( PropertyInfo(Variant::VECTOR3,"transform/rotation",PROPERTY_HINT_NONE,"",PROPERTY_USAGE_EDITOR), _SCS("_set_rotation_deg"), _SCS("_get_rotation_deg") ); ADD_PROPERTY( PropertyInfo(Variant::VECTOR3,"transform/rotation_rad",PROPERTY_HINT_NONE,"",0), _SCS("set_rotation"), _SCS("get_rotation") ); ADD_PROPERTY( PropertyInfo(Variant::VECTOR3,"transform/scale",PROPERTY_HINT_NONE,"",PROPERTY_USAGE_EDITOR), _SCS("set_scale"), _SCS("get_scale") ); + ADD_PROPERTY( PropertyInfo(Variant::BOOL,"visibility/visible"), _SCS("_set_visible_"), _SCS("_is_visible_") ); //ADD_PROPERTY( PropertyInfo(Variant::TRANSFORM,"transform/local"), _SCS("set_transform"), _SCS("get_transform") ); + ADD_SIGNAL( MethodInfo("visibility_changed" ) ); + } @@ -564,6 +656,7 @@ Spatial::Spatial() : xform_change(this) data.scale=Vector3(1,1,1); data.viewport=NULL; data.inside_world=false; + data.visible=true; #ifdef TOOLS_ENABLED data.gizmo_disabled=false; data.gizmo_dirty=false; diff --git a/scene/3d/spatial.h b/scene/3d/spatial.h index e1119be515..f2ec26eb58 100644 --- a/scene/3d/spatial.h +++ b/scene/3d/spatial.h @@ -91,6 +91,8 @@ class Spatial : public Node { bool ignore_notification; + bool visible; + #ifdef TOOLS_ENABLED Ref<SpatialGizmo> gizmo; bool gizmo_disabled; @@ -109,6 +111,8 @@ class Spatial : public Node { void _set_rotation_deg(const Vector3& p_deg); Vector3 _get_rotation_deg() const; + void _propagate_visibility_changed(); + protected: @@ -118,7 +122,9 @@ protected: void _notification(int p_what); static void _bind_methods(); - + + void _set_visible_(bool p_visible); + bool _is_visible_() const; public: enum { @@ -126,6 +132,7 @@ public: NOTIFICATION_TRANSFORM_CHANGED=SceneMainLoop::NOTIFICATION_TRANSFORM_CHANGED, NOTIFICATION_ENTER_WORLD=41, NOTIFICATION_EXIT_WORLD=42, + NOTIFICATION_VISIBILITY_CHANGED=43, }; Spatial *get_parent_spatial() const; @@ -159,6 +166,11 @@ public: Transform get_relative_transform(const Node *p_parent) const; + void show(); + void hide(); + bool is_visible() const; + bool is_hidden() const; + #ifdef TOOLS_ENABLED void set_import_transform(const Transform& p_transform) ; Transform get_import_transform() const; diff --git a/scene/3d/vehicle_body.cpp b/scene/3d/vehicle_body.cpp new file mode 100644 index 0000000000..07abd1dcd2 --- /dev/null +++ b/scene/3d/vehicle_body.cpp @@ -0,0 +1,1044 @@ +#include "vehicle_body.h" + +#define ROLLING_INFLUENCE_FIX + +class btVehicleJacobianEntry +{ +public: + + 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; + + real_t getDiagonal() const { return m_Adiag; } + + btVehicleJacobianEntry() {}; + //constraint between two different rigidbodies + btVehicleJacobianEntry( + 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); + + //btAssert(m_Adiag > real_t(0.0)); + } + + 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; + } + + +}; + +void VehicleWheel::_notification(int p_what) { + + + if (p_what==NOTIFICATION_ENTER_SCENE) { + + if (!get_parent()) + return; + VehicleBody *cb = get_parent()->cast_to<VehicleBody>(); + if (!cb) + return; + body=cb; + local_xform=get_transform(); + cb->wheels.push_back(this); + + m_chassisConnectionPointCS = get_transform().origin; + m_wheelDirectionCS = -get_transform().basis.get_axis(Vector3::AXIS_Y).normalized(); + m_wheelAxleCS = get_transform().basis.get_axis(Vector3::AXIS_X).normalized(); + + } + if (p_what==NOTIFICATION_EXIT_SCENE) { + + if (!get_parent()) + return; + VehicleBody *cb = get_parent()->cast_to<VehicleBody>(); + if (!cb) + return; + cb->wheels.erase(this); + body=NULL; + } + +} + + +void VehicleWheel::_update(PhysicsDirectBodyState *s) { + + + + if (m_raycastInfo.m_isInContact) + + { + real_t project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS ); + Vector3 chassis_velocity_at_contactPoint; + Vector3 relpos = m_raycastInfo.m_contactPointWS - s->get_transform().origin; + + chassis_velocity_at_contactPoint = s->get_linear_velocity() + + (s->get_angular_velocity()).cross(relpos);// * mPos); + + real_t projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); + if ( project >= real_t(-0.1)) + { + m_suspensionRelativeVelocity = real_t(0.0); + m_clippedInvContactDotSuspension = real_t(1.0) / real_t(0.1); + } + else + { + real_t inv = real_t(-1.) / project; + m_suspensionRelativeVelocity = projVel * inv; + m_clippedInvContactDotSuspension = inv; + } + + } + + else // Not in contact : position wheel in a nice (rest length) position + { + m_raycastInfo.m_suspensionLength = m_suspensionRestLength; + m_suspensionRelativeVelocity = real_t(0.0); + m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; + m_clippedInvContactDotSuspension = real_t(1.0); + } +} + +void VehicleWheel::set_radius(float p_radius) { + + m_wheelRadius=p_radius; + update_gizmo(); +} + +float VehicleWheel::get_radius() const{ + + return m_wheelRadius; +} + +void VehicleWheel::set_suspension_rest_length(float p_length){ + + m_suspensionRestLength=p_length; + update_gizmo(); +} +float VehicleWheel::get_suspension_rest_length() const{ + + return m_suspensionRestLength; +} + +void VehicleWheel::set_suspension_travel(float p_length){ + + m_maxSuspensionTravelCm=p_length/0.01; +} +float VehicleWheel::get_suspension_travel() const{ + + return m_maxSuspensionTravelCm*0.01; +} + +void VehicleWheel::set_suspension_stiffness(float p_value){ + + m_suspensionStiffness=p_value; +} +float VehicleWheel::get_suspension_stiffness() const{ + + return m_suspensionStiffness; +} + +void VehicleWheel::set_suspension_max_force(float p_value){ + + m_maxSuspensionForce=p_value; +} +float VehicleWheel::get_suspension_max_force() const{ + + return m_maxSuspensionForce; +} + +void VehicleWheel::set_damping_compression(float p_value){ + + m_wheelsDampingCompression=p_value; +} +float VehicleWheel::get_damping_compression() const{ + + return m_wheelsDampingRelaxation; +} + +void VehicleWheel::set_damping_relaxation(float p_value){ + + m_wheelsDampingRelaxation=p_value; +} +float VehicleWheel::get_damping_relaxation() const{ + + return m_wheelsDampingRelaxation; +} + +void VehicleWheel::set_friction_slip(float p_value) { + + m_frictionSlip=p_value; +} +float VehicleWheel::get_friction_slip() const{ + + return m_frictionSlip; +} + + +void VehicleWheel::_bind_methods() { + + + ObjectTypeDB::bind_method(_MD("set_radius","length"),&VehicleWheel::set_radius); + ObjectTypeDB::bind_method(_MD("get_radius"),&VehicleWheel::get_radius); + + ObjectTypeDB::bind_method(_MD("set_suspension_rest_length","length"),&VehicleWheel::set_suspension_rest_length); + ObjectTypeDB::bind_method(_MD("get_suspension_rest_length"),&VehicleWheel::get_suspension_rest_length); + + ObjectTypeDB::bind_method(_MD("set_suspension_travel","length"),&VehicleWheel::set_suspension_travel); + ObjectTypeDB::bind_method(_MD("get_suspension_travel"),&VehicleWheel::get_suspension_travel); + + ObjectTypeDB::bind_method(_MD("set_suspension_stiffness","length"),&VehicleWheel::set_suspension_stiffness); + ObjectTypeDB::bind_method(_MD("get_suspension_stiffness"),&VehicleWheel::get_suspension_stiffness); + + ObjectTypeDB::bind_method(_MD("set_suspension_max_force","length"),&VehicleWheel::set_suspension_max_force); + ObjectTypeDB::bind_method(_MD("get_suspension_max_force"),&VehicleWheel::get_suspension_max_force); + + + ObjectTypeDB::bind_method(_MD("set_damping_compression","length"),&VehicleWheel::set_damping_compression); + ObjectTypeDB::bind_method(_MD("get_damping_compression"),&VehicleWheel::get_damping_compression); + + ObjectTypeDB::bind_method(_MD("set_damping_relaxation","length"),&VehicleWheel::set_damping_relaxation); + ObjectTypeDB::bind_method(_MD("get_damping_relaxation"),&VehicleWheel::get_damping_relaxation); + + ObjectTypeDB::bind_method(_MD("set_use_as_traction","enable"),&VehicleWheel::set_use_as_traction); + ObjectTypeDB::bind_method(_MD("is_used_as_traction"),&VehicleWheel::is_used_as_traction); + + ObjectTypeDB::bind_method(_MD("set_use_as_steering","enable"),&VehicleWheel::set_use_as_steering); + ObjectTypeDB::bind_method(_MD("is_used_as_steering"),&VehicleWheel::is_used_as_steering); + + ObjectTypeDB::bind_method(_MD("set_friction_slip","length"),&VehicleWheel::set_friction_slip); + ObjectTypeDB::bind_method(_MD("get_friction_slip"),&VehicleWheel::get_friction_slip); + + + ADD_PROPERTY(PropertyInfo(Variant::BOOL,"type/traction"),_SCS("set_use_as_traction"),_SCS("is_used_as_traction")); + ADD_PROPERTY(PropertyInfo(Variant::BOOL,"type/steering"),_SCS("set_use_as_steering"),_SCS("is_used_as_steering")); + ADD_PROPERTY(PropertyInfo(Variant::REAL,"wheel/radius"),_SCS("set_radius"),_SCS("get_radius")); + ADD_PROPERTY(PropertyInfo(Variant::REAL,"wheel/rest_length"),_SCS("set_suspension_rest_length"),_SCS("get_suspension_rest_length")); + ADD_PROPERTY(PropertyInfo(Variant::REAL,"wheel/friction_slip"),_SCS("set_friction_slip"),_SCS("get_friction_slip")); + ADD_PROPERTY(PropertyInfo(Variant::REAL,"suspension/travel"),_SCS("set_suspension_travel"),_SCS("get_suspension_travel")); + ADD_PROPERTY(PropertyInfo(Variant::REAL,"suspension/stiffness"),_SCS("set_suspension_stiffness"),_SCS("get_suspension_stiffness")); + ADD_PROPERTY(PropertyInfo(Variant::REAL,"suspension/max_force"),_SCS("set_suspension_max_force"),_SCS("get_suspension_max_force")); + ADD_PROPERTY(PropertyInfo(Variant::REAL,"damping/compression"),_SCS("set_damping_compression"),_SCS("get_damping_compression")); + ADD_PROPERTY(PropertyInfo(Variant::REAL,"damping/relaxation"),_SCS("set_damping_relaxation"),_SCS("get_damping_relaxation")); + +} + + +void VehicleWheel::set_use_as_traction(bool p_enable) { + + engine_traction=p_enable; +} + +bool VehicleWheel::is_used_as_traction() const{ + + return engine_traction; +} + + +void VehicleWheel::set_use_as_steering(bool p_enabled){ + + steers=p_enabled; +} + +bool VehicleWheel::is_used_as_steering() const{ + + return steers; +} + + +VehicleWheel::VehicleWheel() { + + + steers=false; + engine_traction=false; + + m_steering = real_t(0.); + //m_engineForce = real_t(0.); + m_rotation = real_t(0.); + m_deltaRotation = real_t(0.); + m_brake = real_t(0.); + m_rollInfluence = real_t(0.1); + + m_suspensionRestLength = 0.15; + m_wheelRadius = 0.5;//0.28; + m_suspensionStiffness = 5.88; + m_wheelsDampingCompression = 0.83; + m_wheelsDampingRelaxation = 0.88; + m_frictionSlip = 10.5; + m_bIsFrontWheel = false; + m_maxSuspensionTravelCm = 500; + m_maxSuspensionForce = 6000; + + m_suspensionRelativeVelocity=0; + m_clippedInvContactDotSuspension=1.0; + m_raycastInfo.m_isInContact=false; + + body=NULL; +} + + +void VehicleBody::_update_wheel_transform(VehicleWheel& wheel ,PhysicsDirectBodyState *s) { + + wheel.m_raycastInfo.m_isInContact = false; + + Transform chassisTrans = s->get_transform(); + //if (interpolatedTransform && (getRigidBody()->getMotionState())) + //{ + // getRigidBody()->getMotionState()->getWorldTransform(chassisTrans); + //} + + wheel.m_raycastInfo.m_hardPointWS = chassisTrans.xform( wheel.m_chassisConnectionPointCS ); + //wheel.m_raycastInfo.m_hardPointWS+=s->get_linear_velocity()*s->get_step(); + wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.get_basis().xform( wheel.m_wheelDirectionCS).normalized(); + wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.get_basis().xform( wheel.m_wheelAxleCS ).normalized(); +} + +void VehicleBody::_update_wheel(int p_idx,PhysicsDirectBodyState *s) { + + VehicleWheel& wheel = *wheels[p_idx]; + _update_wheel_transform(wheel,s); + + Vector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS; + const Vector3& right = wheel.m_raycastInfo.m_wheelAxleWS; + Vector3 fwd = up.cross(right); + fwd = fwd.normalized(); +// up = right.cross(fwd); +// up.normalize(); + + //rotate around steering over de wheelAxleWS + real_t steering = wheel.steers?m_steeringValue:0.0; + //print_line(itos(p_idx)+": "+rtos(steering)); + + Matrix3 steeringMat(up,steering); + + Matrix3 rotatingMat(right,-wheel.m_rotation); + +// if (p_idx==1) +// print_line("steeringMat " +steeringMat); + + Matrix3 basis2( + right[0],up[0],fwd[0], + right[1],up[1],fwd[1], + right[2],up[2],fwd[2] + ); + + wheel.m_worldTransform.set_basis(steeringMat * rotatingMat * basis2); + //wheel.m_worldTransform.set_basis(basis2 * (steeringMat * rotatingMat)); + wheel.m_worldTransform.set_origin( + wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength + ); + +} + + +real_t VehicleBody::_ray_cast(int p_idx,PhysicsDirectBodyState *s) { + + + VehicleWheel& wheel = *wheels[p_idx]; + + _update_wheel_transform(wheel,s); + + + real_t depth = -1; + + real_t raylen = wheel.m_suspensionRestLength+wheel.m_wheelRadius; + + Vector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen); + Vector3 source = wheel.m_raycastInfo.m_hardPointWS; + wheel.m_raycastInfo.m_contactPointWS = source + rayvector; + const Vector3& target = wheel.m_raycastInfo.m_contactPointWS; + source-=wheel.m_wheelRadius * wheel.m_raycastInfo.m_wheelDirectionWS; + + real_t param = real_t(0.); + + + PhysicsDirectSpaceState::RayResult rr; + + + PhysicsDirectSpaceState *ss=s->get_space_state(); + + bool col = ss->intersect_ray(source,target,rr,exclude); + + + wheel.m_raycastInfo.m_groundObject = 0; + + if (col) + { + //print_line("WHEEL "+itos(p_idx)+" FROM "+source+" TO: "+target); + //print_line("WHEEL "+itos(p_idx)+" COLLIDE? "+itos(col)); + param = source.distance_to(rr.position)/source.distance_to(target); + depth = raylen * param; + wheel.m_raycastInfo.m_contactNormalWS = rr.normal; + + wheel.m_raycastInfo.m_isInContact = true; + if (rr.collider) + wheel.m_raycastInfo.m_groundObject=rr.collider->cast_to<PhysicsBody>(); + + + real_t hitDistance = param*raylen; + wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelRadius; + //clamp on max suspension travel + + real_t minSuspensionLength = wheel.m_suspensionRestLength - wheel.m_maxSuspensionTravelCm*real_t(0.01); + real_t maxSuspensionLength = wheel.m_suspensionRestLength+ wheel.m_maxSuspensionTravelCm*real_t(0.01); + if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength) + { + wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength; + } + if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength) + { + wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength; + } + + wheel.m_raycastInfo.m_contactPointWS = rr.position; + + real_t denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS ); + + Vector3 chassis_velocity_at_contactPoint; + //Vector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition(); + + //chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos); + + chassis_velocity_at_contactPoint = s->get_linear_velocity() + + (s->get_angular_velocity()).cross(wheel.m_raycastInfo.m_contactPointWS-s->get_transform().origin);// * mPos); + + + real_t projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); + + if ( denominator >= real_t(-0.1)) + { + wheel.m_suspensionRelativeVelocity = real_t(0.0); + wheel.m_clippedInvContactDotSuspension = real_t(1.0) / real_t(0.1); + } + else + { + real_t inv = real_t(-1.) / denominator; + wheel.m_suspensionRelativeVelocity = projVel * inv; + wheel.m_clippedInvContactDotSuspension = inv; + } + + } else + { + wheel.m_raycastInfo.m_isInContact = false; + //put wheel info as in rest position + wheel.m_raycastInfo.m_suspensionLength = wheel.m_suspensionRestLength; + wheel.m_suspensionRelativeVelocity = real_t(0.0); + wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS; + wheel.m_clippedInvContactDotSuspension = real_t(1.0); + } + + return depth; +} + + +void VehicleBody::_update_suspension(PhysicsDirectBodyState *s) +{ + + real_t deltaTime = s->get_step(); + real_t chassisMass = mass; + + for (int w_it=0; w_it<wheels.size(); w_it++) + { + VehicleWheel& wheel_info = *wheels[w_it]; + + + if ( wheel_info.m_raycastInfo.m_isInContact ) + { + real_t force; + // Spring + { + real_t susp_length = wheel_info.m_suspensionRestLength; + real_t current_length = wheel_info.m_raycastInfo.m_suspensionLength; + + real_t length_diff = (susp_length - current_length); + + force = wheel_info.m_suspensionStiffness + * length_diff * wheel_info.m_clippedInvContactDotSuspension; + } + + // Damper + { + real_t projected_rel_vel = wheel_info.m_suspensionRelativeVelocity; + { + real_t susp_damping; + if ( projected_rel_vel < real_t(0.0) ) + { + susp_damping = wheel_info.m_wheelsDampingCompression; + } + else + { + susp_damping = wheel_info.m_wheelsDampingRelaxation; + } + force -= susp_damping * projected_rel_vel; + } + } + + // RESULT + wheel_info.m_wheelsSuspensionForce = force * chassisMass; + if (wheel_info.m_wheelsSuspensionForce < real_t(0.)) + { + wheel_info.m_wheelsSuspensionForce = real_t(0.); + } + } + else + { + wheel_info.m_wheelsSuspensionForce = real_t(0.0); + } + } + +} + + +//bilateral constraint between two dynamic objects +void VehicleBody::_resolve_single_bilateral(PhysicsDirectBodyState *s, const Vector3& pos1, + PhysicsBody* body2, const Vector3& pos2, const Vector3& normal,real_t& impulse) +{ + + real_t normalLenSqr = normal.length_squared(); + //ERR_FAIL_COND( normalLenSqr < real_t(1.1)); + + if (normalLenSqr > real_t(1.1)) + { + impulse = real_t(0.); + return; + } + + Vector3 rel_pos1 = pos1 - s->get_transform().origin; + Vector3 rel_pos2; + if (body2) + rel_pos2 = pos2 - body2->get_global_transform().origin; + //this jacobian entry could be re-used for all iterations + + Vector3 vel1 = s->get_linear_velocity() + (s->get_angular_velocity()).cross(rel_pos1);// * mPos); + Vector3 vel2; + + if (body2) + vel2=body2->get_linear_velocity() + body2->get_angular_velocity().cross(rel_pos2); + + Vector3 vel = vel1 - vel2; + + Matrix3 b2trans; + float b2invmass=0; + Vector3 b2lv; + Vector3 b2av; + Vector3 b2invinertia; //todo + + if (body2) { + b2trans = body2->get_global_transform().basis.transposed(); + b2invmass = body2->get_inverse_mass(); + b2lv = body2->get_linear_velocity(); + b2av = body2->get_angular_velocity(); + } + + + + btVehicleJacobianEntry jac(s->get_transform().basis.transposed(), + b2trans, + rel_pos1, + rel_pos2, + normal, + s->get_inverse_inertia(), + 1.0/mass, + b2invinertia, + b2invmass); + + real_t jacDiagAB = jac.getDiagonal(); + real_t jacDiagABInv = real_t(1.) / jacDiagAB; + + real_t rel_vel = jac.getRelativeVelocity( + s->get_linear_velocity(), + s->get_transform().basis.transposed().xform(s->get_angular_velocity()), + b2lv, + b2trans.xform(b2av)); + real_t a; + a=jacDiagABInv; + + + rel_vel = normal.dot(vel); + + //todo: move this into proper structure + real_t contactDamping = real_t(0.4); +#define ONLY_USE_LINEAR_MASS +#ifdef ONLY_USE_LINEAR_MASS + real_t massTerm = real_t(1.) / ((1.0/mass) + b2invmass); + impulse = - contactDamping * rel_vel * massTerm; +#else + real_t velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; + impulse = velocityImpulse; +#endif + +} + + + +VehicleBody::btVehicleWheelContactPoint::btVehicleWheelContactPoint(PhysicsDirectBodyState *s,PhysicsBody* body1,const Vector3& frictionPosWorld,const Vector3& frictionDirectionWorld, real_t maxImpulse) + :m_s(s), + m_body1(body1), + m_frictionPositionWorld(frictionPosWorld), + m_frictionDirectionWorld(frictionDirectionWorld), + m_maxImpulse(maxImpulse) +{ + float denom0=0; + float denom1=0; + + { + Vector3 r0 = frictionPosWorld - s->get_transform().origin; + Vector3 c0 = (r0).cross(frictionDirectionWorld); + Vector3 vec = s->get_inverse_inertia_tensor().xform_inv(c0).cross(r0); + denom0= s->get_inverse_mass() + frictionDirectionWorld.dot(vec); + } + + if (body1) { + + Vector3 r0 = frictionPosWorld - body1->get_global_transform().origin; + Vector3 c0 = (r0).cross(frictionDirectionWorld); + Vector3 vec = s->get_inverse_inertia_tensor().xform_inv(c0).cross(r0); + //denom1= body1->get_inverse_mass() + frictionDirectionWorld.dot(vec); + denom1=0; + + } + + + real_t relaxation = 1.f; + m_jacDiagABInv = relaxation/(denom0+denom1); +} + + +real_t VehicleBody::_calc_rolling_friction(btVehicleWheelContactPoint& contactPoint) { + + real_t j1=0.f; + + const Vector3& contactPosWorld = contactPoint.m_frictionPositionWorld; + + Vector3 rel_pos1 = contactPosWorld - contactPoint.m_s->get_transform().origin; + Vector3 rel_pos2; + if (contactPoint.m_body1) + rel_pos2 = contactPosWorld - contactPoint.m_body1->get_global_transform().origin; + + real_t maxImpulse = contactPoint.m_maxImpulse; + + Vector3 vel1 = contactPoint.m_s->get_linear_velocity() + (contactPoint.m_s->get_angular_velocity()).cross(rel_pos1);// * mPos); + + Vector3 vel2; + if (contactPoint.m_body1) { + vel2=contactPoint.m_body1->get_linear_velocity() + contactPoint.m_body1->get_angular_velocity().cross(rel_pos2); + + } + + Vector3 vel = vel1 - vel2; + + real_t vrel = contactPoint.m_frictionDirectionWorld.dot(vel); + + // calculate j that moves us to zero relative velocity + j1 = -vrel * contactPoint.m_jacDiagABInv; + + return CLAMP(j1,-maxImpulse,maxImpulse); +} + + +static const real_t sideFrictionStiffness2 = real_t(1.0); +void VehicleBody::_update_friction(PhysicsDirectBodyState *s) { + + //calculate the impulse, so that the wheels don't move sidewards + int numWheel = wheels.size(); + if (!numWheel) + return; + + m_forwardWS.resize(numWheel); + m_axle.resize(numWheel); + m_forwardImpulse.resize(numWheel); + m_sideImpulse.resize(numWheel); + + int numWheelsOnGround = 0; + + + //collapse all those loops into one! + for (int i=0;i<wheels.size();i++) + { + VehicleWheel& wheelInfo = *wheels[i]; + if (wheelInfo.m_raycastInfo.m_isInContact) + numWheelsOnGround++; + m_sideImpulse[i] = real_t(0.); + m_forwardImpulse[i] = real_t(0.); + + } + + { + + for (int i=0;i<wheels.size();i++) + { + + VehicleWheel& wheelInfo = *wheels[i]; + + + if (wheelInfo.m_raycastInfo.m_isInContact) + { + + //const btTransform& wheelTrans = getWheelTransformWS( i ); + + Matrix3 wheelBasis0 = wheelInfo.m_worldTransform.basis;//get_global_transform().basis; + + m_axle[i] = wheelBasis0.get_axis(Vector3::AXIS_X); + //m_axle[i] = wheelInfo.m_raycastInfo.m_wheelAxleWS; + + const Vector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS; + real_t proj = m_axle[i].dot(surfNormalWS); + m_axle[i] -= surfNormalWS * proj; + m_axle[i] = m_axle[i].normalized(); + + m_forwardWS[i] = surfNormalWS.cross(m_axle[i]); + m_forwardWS[i].normalize(); + + + _resolve_single_bilateral(s, wheelInfo.m_raycastInfo.m_contactPointWS, + wheelInfo.m_raycastInfo.m_groundObject, wheelInfo.m_raycastInfo.m_contactPointWS, + m_axle[i],m_sideImpulse[i]); + + m_sideImpulse[i] *= sideFrictionStiffness2; + + + } + } + } + + real_t sideFactor = real_t(1.); + real_t fwdFactor = 0.5; + + bool sliding = false; + { + for (int wheel =0;wheel <wheels.size();wheel++) + { + VehicleWheel& wheelInfo = *wheels[wheel]; + + + //class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject; + + real_t rollingFriction = 0.f; + + if (wheelInfo.m_raycastInfo.m_isInContact) + { + if (engine_force != 0.f) + { + rollingFriction = engine_force* s->get_step(); + } else + { + real_t defaultRollingFrictionImpulse = 0.f; + real_t maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse; + btVehicleWheelContactPoint contactPt(s,wheelInfo.m_raycastInfo.m_groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse); + rollingFriction = _calc_rolling_friction(contactPt); + } + } + + //switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break) + + + + + m_forwardImpulse[wheel] = real_t(0.); + wheelInfo.m_skidInfo= real_t(1.); + + if (wheelInfo.m_raycastInfo.m_isInContact) + { + wheelInfo.m_skidInfo= real_t(1.); + + real_t maximp = wheelInfo.m_wheelsSuspensionForce * s->get_step() * wheelInfo.m_frictionSlip; + real_t maximpSide = maximp; + + real_t maximpSquared = maximp * maximpSide; + + + m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep; + + real_t x = (m_forwardImpulse[wheel] ) * fwdFactor; + real_t y = (m_sideImpulse[wheel] ) * sideFactor; + + real_t impulseSquared = (x*x + y*y); + + if (impulseSquared > maximpSquared) + { + sliding = true; + + real_t factor = maximp / Math::sqrt(impulseSquared); + + wheelInfo.m_skidInfo *= factor; + } + } + + } + } + + + + + if (sliding) + { + for (int wheel = 0;wheel < wheels.size(); wheel++) + { + if (m_sideImpulse[wheel] != real_t(0.)) + { + if (wheels[wheel]->m_skidInfo< real_t(1.)) + { + m_forwardImpulse[wheel] *= wheels[wheel]->m_skidInfo; + m_sideImpulse[wheel] *= wheels[wheel]->m_skidInfo; + } + } + } + } + + // apply the impulses + { + for (int wheel = 0;wheel<wheels.size(); wheel++) + { + VehicleWheel& wheelInfo = *wheels[wheel]; + + Vector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - + s->get_transform().origin; + + if (m_forwardImpulse[wheel] != real_t(0.)) + { + s->apply_impulse(rel_pos,m_forwardWS[wheel]*(m_forwardImpulse[wheel])); + } + if (m_sideImpulse[wheel] != real_t(0.)) + { + PhysicsBody* groundObject = wheelInfo.m_raycastInfo.m_groundObject; + + Vector3 rel_pos2; + if (groundObject) { + rel_pos2=wheelInfo.m_raycastInfo.m_contactPointWS - groundObject->get_global_transform().origin; + } + + + Vector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel]; + +#if defined ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT. + Vector3 vChassisWorldUp = s->get_transform().basis.transposed()[1];//getRigidBody()->getCenterOfMassTransform().getBasis().getColumn(m_indexUpAxis); + rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f-wheelInfo.m_rollInfluence)); +#else + rel_pos[1] *= wheelInfo.m_rollInfluence; //? +#endif + s->apply_impulse(rel_pos,sideImp); + + //apply friction impulse on the ground + //todo + //groundObject->applyImpulse(-sideImp,rel_pos2); + } + } + } + + +} + + +void VehicleBody::_direct_state_changed(Object *p_state) { + + + PhysicsDirectBodyState *s = p_state->cast_to<PhysicsDirectBodyState>(); + + set_ignore_transform_notification(true); + set_global_transform(s->get_transform()); + set_ignore_transform_notification(false); + + + float step = s->get_step(); + + for(int i=0;i<wheels.size();i++) { + + _update_wheel(i,s); + } + + + for(int i=0;i<wheels.size();i++) { + + _ray_cast(i,s); + wheels[i]->set_transform(s->get_transform().inverse() * wheels[i]->m_worldTransform); + } + + _update_suspension(s); + + for(int i=0;i<wheels.size();i++) { + + //apply suspension force + VehicleWheel& wheel = *wheels[i]; + + real_t suspensionForce = wheel.m_wheelsSuspensionForce; + + if (suspensionForce > wheel.m_maxSuspensionForce) + { + suspensionForce = wheel.m_maxSuspensionForce; + } + Vector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step; + Vector3 relpos = wheel.m_raycastInfo.m_contactPointWS - s->get_transform().origin; + + s->apply_impulse(relpos,impulse); + //getRigidBody()->applyImpulse(impulse, relpos); + + } + + + _update_friction(s); + + + for (int i=0;i<wheels.size();i++) + { + VehicleWheel& wheel = *wheels[i]; + Vector3 relpos = wheel.m_raycastInfo.m_hardPointWS - s->get_transform().origin; + Vector3 vel = s->get_linear_velocity() + (s->get_angular_velocity()).cross(relpos);// * mPos); + + if (wheel.m_raycastInfo.m_isInContact) + { + const Transform& chassisWorldTransform = s->get_transform(); + + Vector3 fwd ( + chassisWorldTransform.basis[0][Vector3::AXIS_Z], + chassisWorldTransform.basis[1][Vector3::AXIS_Z], + chassisWorldTransform.basis[2][Vector3::AXIS_Z]); + + real_t proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS); + fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj; + + real_t proj2 = fwd.dot(vel); + + wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelRadius); + wheel.m_rotation += wheel.m_deltaRotation; + + } else + { + wheel.m_rotation += wheel.m_deltaRotation; + } + + wheel.m_deltaRotation *= real_t(0.99);//damping of rotation when not in contact + + } + +} + +void VehicleBody::set_mass(real_t p_mass) { + + mass=p_mass; + PhysicsServer::get_singleton()->body_set_param(get_rid(),PhysicsServer::BODY_PARAM_MASS,mass); +} + +real_t VehicleBody::get_mass() const{ + + return mass; +} + + +void VehicleBody::set_friction(real_t p_friction) { + + friction=p_friction; + PhysicsServer::get_singleton()->body_set_param(get_rid(),PhysicsServer::BODY_PARAM_FRICTION,friction); +} + +real_t VehicleBody::get_friction() const{ + + return friction; +} + +void VehicleBody::set_engine_force(float p_force) { + + engine_force=p_force; +} + +float VehicleBody::get_engine_force() const{ + + return engine_force; +} + +void VehicleBody::set_brake(float p_brake){ + + brake=p_brake; +} +float VehicleBody::get_brake() const{ + + return brake; +} + +void VehicleBody::set_steering(float p_steering){ + + m_steeringValue=p_steering; +} +float VehicleBody::get_steering() const{ + + return m_steeringValue; +} + + +void VehicleBody::_bind_methods(){ + + ObjectTypeDB::bind_method(_MD("set_mass","mass"),&VehicleBody::set_mass); + ObjectTypeDB::bind_method(_MD("get_mass"),&VehicleBody::get_mass); + + ObjectTypeDB::bind_method(_MD("set_friction","friction"),&VehicleBody::set_friction); + ObjectTypeDB::bind_method(_MD("get_friction"),&VehicleBody::get_friction); + + ObjectTypeDB::bind_method(_MD("set_engine_force","engine_force"),&VehicleBody::set_engine_force); + ObjectTypeDB::bind_method(_MD("get_engine_force"),&VehicleBody::get_engine_force); + + ObjectTypeDB::bind_method(_MD("set_brake","brake"),&VehicleBody::set_brake); + ObjectTypeDB::bind_method(_MD("get_brake"),&VehicleBody::get_brake); + + ObjectTypeDB::bind_method(_MD("set_steering","steering"),&VehicleBody::set_steering); + ObjectTypeDB::bind_method(_MD("get_steering"),&VehicleBody::get_steering); + + ObjectTypeDB::bind_method(_MD("_direct_state_changed"),&VehicleBody::_direct_state_changed); + + ADD_PROPERTY( PropertyInfo(Variant::REAL,"motion/engine_force",PROPERTY_HINT_RANGE,"0.01,1024.0,0.01"),_SCS("set_engine_force"),_SCS("get_engine_force")); + ADD_PROPERTY( PropertyInfo(Variant::REAL,"motion/brake",PROPERTY_HINT_RANGE,"0.01,1024.0,0.01"),_SCS("set_brake"),_SCS("get_brake")); + ADD_PROPERTY( PropertyInfo(Variant::REAL,"motion/steering",PROPERTY_HINT_RANGE,"0.01,1024.0,0.01"),_SCS("set_steering"),_SCS("get_steering")); + ADD_PROPERTY( PropertyInfo(Variant::REAL,"body/mass",PROPERTY_HINT_RANGE,"0.01,65536,0.01"),_SCS("set_mass"),_SCS("get_mass")); + ADD_PROPERTY( PropertyInfo(Variant::REAL,"body/friction",PROPERTY_HINT_RANGE,"0.01,1,0.01"),_SCS("set_friction"),_SCS("get_friction")); + + +} + + + +VehicleBody::VehicleBody() : PhysicsBody(PhysicsServer::BODY_MODE_RIGID) { + + + m_pitchControl=0; + m_currentVehicleSpeedKmHour = real_t(0.); + m_steeringValue = real_t(0.); + + engine_force=0; + brake=0; + + + + friction=1; + + ccd=false; + + exclude.insert(get_rid()); + PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(),this,"_direct_state_changed"); + + set_mass(40); +} + diff --git a/scene/3d/vehicle_body.h b/scene/3d/vehicle_body.h new file mode 100644 index 0000000000..285cca142d --- /dev/null +++ b/scene/3d/vehicle_body.h @@ -0,0 +1,185 @@ +#ifndef VEHICLE_BODY_H +#define VEHICLE_BODY_H + +#include "scene/3d/physics_body.h" + +class VehicleBody; + +class VehicleWheel : public Spatial { + + OBJ_TYPE(VehicleWheel,Spatial); + +friend class VehicleBody; + + + Transform m_worldTransform; + Transform local_xform; + bool engine_traction; + bool steers; + + + Vector3 m_chassisConnectionPointCS; //const + Vector3 m_wheelDirectionCS;//const + Vector3 m_wheelAxleCS; // const or modified by steering + + real_t m_suspensionRestLength; + real_t m_maxSuspensionTravelCm; + real_t m_wheelRadius; + + real_t m_suspensionStiffness; + real_t m_wheelsDampingCompression; + real_t m_wheelsDampingRelaxation; + real_t m_frictionSlip; + real_t m_maxSuspensionForce; + bool m_bIsFrontWheel; + + VehicleBody *body; + +// btVector3 m_wheelAxleCS; // const or modified by steering ? + + real_t m_steering; + real_t m_rotation; + real_t m_deltaRotation; + real_t m_rollInfluence; + //real_t m_engineForce; + real_t m_brake; + + real_t m_clippedInvContactDotSuspension; + real_t m_suspensionRelativeVelocity; + //calculated by suspension + real_t m_wheelsSuspensionForce; + real_t m_skidInfo; + + + struct RaycastInfo { + //set by raycaster + Vector3 m_contactNormalWS;//contactnormal + Vector3 m_contactPointWS;//raycast hitpoint + real_t m_suspensionLength; + Vector3 m_hardPointWS;//raycast starting point + Vector3 m_wheelDirectionWS; //direction in worldspace + Vector3 m_wheelAxleWS; // axle in worldspace + bool m_isInContact; + PhysicsBody* m_groundObject; //could be general void* ptr + } m_raycastInfo; + + void _update(PhysicsDirectBodyState *s); + +protected: + void _notification(int p_what); + static void _bind_methods(); + +public: + + void set_radius(float p_radius); + float get_radius() const; + + void set_suspension_rest_length(float p_length); + float get_suspension_rest_length() const; + + void set_suspension_travel(float p_length); + float get_suspension_travel() const; + + void set_suspension_stiffness(float p_value); + float get_suspension_stiffness() const; + + void set_suspension_max_force(float p_value); + float get_suspension_max_force() const; + + void set_damping_compression(float p_value); + float get_damping_compression() const; + + void set_damping_relaxation(float p_value); + float get_damping_relaxation() const; + + void set_friction_slip(float p_value); + float get_friction_slip() const; + + void set_use_as_traction(bool p_enable); + bool is_used_as_traction() const; + + void set_use_as_steering(bool p_enabled); + bool is_used_as_steering() const; + + VehicleWheel(); + +}; + + +class VehicleBody : public PhysicsBody { + + OBJ_TYPE(VehicleBody,PhysicsBody); + + real_t mass; + real_t friction; + + float engine_force; + float brake; + + Vector3 linear_velocity; + Vector3 angular_velocity; + bool ccd; + + real_t m_pitchControl; + real_t m_steeringValue; + real_t m_currentVehicleSpeedKmHour; + + Set<RID> exclude; + + Vector<Vector3> m_forwardWS; + Vector<Vector3> m_axle; + Vector<real_t> m_forwardImpulse; + Vector<real_t> m_sideImpulse; + + struct btVehicleWheelContactPoint { + PhysicsDirectBodyState *m_s; + PhysicsBody* m_body1; + Vector3 m_frictionPositionWorld; + Vector3 m_frictionDirectionWorld; + real_t m_jacDiagABInv; + real_t m_maxImpulse; + + + btVehicleWheelContactPoint(PhysicsDirectBodyState *s,PhysicsBody* body1,const Vector3& frictionPosWorld,const Vector3& frictionDirectionWorld, real_t maxImpulse); + }; + + void _resolve_single_bilateral(PhysicsDirectBodyState *s, const Vector3& pos1, PhysicsBody* body2, const Vector3& pos2, const Vector3& normal, real_t& impulse); + real_t _calc_rolling_friction(btVehicleWheelContactPoint& contactPoint); + + void _update_friction(PhysicsDirectBodyState *s); + void _update_suspension(PhysicsDirectBodyState *s); + real_t _ray_cast(int p_idx,PhysicsDirectBodyState *s); + void _update_wheel_transform(VehicleWheel& wheel ,PhysicsDirectBodyState *s); + void _update_wheel(int p_idx,PhysicsDirectBodyState *s); + + + +friend class VehicleWheel; + Vector<VehicleWheel*> wheels; + + static void _bind_methods(); + + void _direct_state_changed(Object *p_state); +public: + + + void set_mass(real_t p_mass); + real_t get_mass() const; + + void set_friction(real_t p_friction); + real_t get_friction() const; + + void set_engine_force(float p_engine_force); + float get_engine_force() const; + + void set_brake(float p_force); + float get_brake() const; + + void set_steering(float p_steering); + float get_steering() const; + + + VehicleBody(); +}; + +#endif // VEHICLE_BODY_H diff --git a/scene/3d/visual_instance.cpp b/scene/3d/visual_instance.cpp index 625da9b093..af535e139f 100644 --- a/scene/3d/visual_instance.cpp +++ b/scene/3d/visual_instance.cpp @@ -195,6 +195,7 @@ void GeometryInstance::_notification(int p_what) { _find_baked_light(); } + _update_visibility(); } else if (p_what==NOTIFICATION_EXIT_WORLD) { @@ -207,8 +208,13 @@ void GeometryInstance::_notification(int p_what) { _baked_light_changed(); } + + } if (p_what==NOTIFICATION_VISIBILITY_CHANGED) { + + _update_visibility(); } + } void GeometryInstance::_baked_light_changed() { @@ -241,6 +247,15 @@ void GeometryInstance::_find_baked_light() { _baked_light_changed(); } +void GeometryInstance::_update_visibility() { + + if (!is_inside_scene()) + return; + + _change_notify("geometry/visible"); + VS::get_singleton()->instance_geometry_set_flag(get_instance(),VS::INSTANCE_FLAG_VISIBLE,is_visible() && flags[FLAG_VISIBLE]); +} + void GeometryInstance::set_flag(Flags p_flag,bool p_value) { ERR_FAIL_INDEX(p_flag,FLAG_MAX); @@ -250,8 +265,7 @@ void GeometryInstance::set_flag(Flags p_flag,bool p_value) { flags[p_flag]=p_value; VS::get_singleton()->instance_geometry_set_flag(get_instance(),(VS::InstanceFlags)p_flag,p_value); if (p_flag==FLAG_VISIBLE) { - _change_notify("geometry/visible"); - emit_signal(SceneStringNames::get_singleton()->visibility_changed); + _update_visibility(); } if (p_flag==FLAG_USE_BAKED_LIGHT) { @@ -276,6 +290,18 @@ bool GeometryInstance::get_flag(Flags p_flag) const{ } +void GeometryInstance::set_baked_light_texture_id(int p_id) { + + baked_light_texture_id=p_id; + VS::get_singleton()->instance_geometry_set_baked_light_texture_index(get_instance(),baked_light_texture_id); + +} + +int GeometryInstance::get_baked_light_texture_id() const{ + + return baked_light_texture_id; +} + void GeometryInstance::_bind_methods() { @@ -291,6 +317,9 @@ void GeometryInstance::_bind_methods() { ObjectTypeDB::bind_method(_MD("set_draw_range_end","mode"), &GeometryInstance::set_draw_range_end); ObjectTypeDB::bind_method(_MD("get_draw_range_end"), &GeometryInstance::get_draw_range_end); + ObjectTypeDB::bind_method(_MD("set_baked_light_texture_id","id"), &GeometryInstance::set_baked_light_texture_id); + ObjectTypeDB::bind_method(_MD("get_baked_light_texture_id"), &GeometryInstance::get_baked_light_texture_id); + ObjectTypeDB::bind_method(_MD("_baked_light_changed"), &GeometryInstance::_baked_light_changed); ADD_PROPERTYI( PropertyInfo( Variant::BOOL, "geometry/visible"), _SCS("set_flag"), _SCS("get_flag"),FLAG_VISIBLE); @@ -304,8 +333,9 @@ void GeometryInstance::_bind_methods() { ADD_PROPERTYI( PropertyInfo( Variant::BOOL, "geometry/depth_scale"), _SCS("set_flag"), _SCS("get_flag"),FLAG_DEPH_SCALE); ADD_PROPERTYI( PropertyInfo( Variant::BOOL, "geometry/visible_in_all_rooms"), _SCS("set_flag"), _SCS("get_flag"),FLAG_VISIBLE_IN_ALL_ROOMS); ADD_PROPERTYI( PropertyInfo( Variant::BOOL, "geometry/use_baked_light"), _SCS("set_flag"), _SCS("get_flag"),FLAG_USE_BAKED_LIGHT); + ADD_PROPERTY( PropertyInfo( Variant::INT, "geometry/baked_light_tex_id"), _SCS("set_baked_light_texture_id"), _SCS("get_baked_light_texture_id")); - ADD_SIGNAL( MethodInfo("visibility_changed")); +// ADD_SIGNAL( MethodInfo("visibility_changed")); BIND_CONSTANT(FLAG_VISIBLE ); BIND_CONSTANT(FLAG_CAST_SHADOW ); @@ -329,6 +359,8 @@ GeometryInstance::GeometryInstance() { flags[FLAG_DEPH_SCALE]=false; flags[FLAG_VISIBLE_IN_ALL_ROOMS]=false; baked_light_instance=NULL; + baked_light_texture_id=0; + VS::get_singleton()->instance_geometry_set_baked_light_texture_index(get_instance(),0); } diff --git a/scene/3d/visual_instance.h b/scene/3d/visual_instance.h index 1cf96d5d9e..bbb49a2e78 100644 --- a/scene/3d/visual_instance.h +++ b/scene/3d/visual_instance.h @@ -106,8 +106,10 @@ private: float draw_end; void _find_baked_light(); BakedLightInstance *baked_light_instance; + int baked_light_texture_id; void _baked_light_changed(); + void _update_visibility(); protected: void _notification(int p_what); @@ -126,6 +128,9 @@ public: void set_material_override(const Ref<Material>& p_material); Ref<Material> get_material_override() const; + void set_baked_light_texture_id(int p_id); + int get_baked_light_texture_id() const; + GeometryInstance(); }; |