From 2e73be99d8d86d9dad7bcb99518a4d3cbb5c373c Mon Sep 17 00:00:00 2001 From: Juan Linietsky Date: Sat, 15 Jul 2017 01:23:10 -0300 Subject: Lots of work on Audio & Physics engine: -Added new 3D stream player node -Added ability for Area to capture sound from streams -Added small features in physics to be able to properly guess distance to areas for sound -Fixed 3D CollisionObject so shapes are added the same as in 2D, directly from children -Fixed KinematicBody API to make it the same as 2D. --- servers/audio/audio_filter_sw.cpp | 37 ++- servers/audio/audio_filter_sw.h | 25 +- servers/audio/audio_stream.cpp | 131 +++++++++++ servers/audio/audio_stream.h | 55 +++++ servers/physics/area_pair_sw.cpp | 22 +- servers/physics/body_pair_sw.cpp | 11 +- servers/physics/broad_phase_basic.cpp | 20 ++ servers/physics/broad_phase_basic.h | 1 + servers/physics/broad_phase_octree.cpp | 5 + servers/physics/broad_phase_octree.h | 1 + servers/physics/broad_phase_sw.h | 1 + servers/physics/collision_object_sw.h | 8 +- servers/physics/physics_server_sw.cpp | 34 ++- servers/physics/physics_server_sw.h | 9 +- servers/physics/shape_sw.cpp | 229 ++++++++++++++++++ servers/physics/shape_sw.h | 24 +- servers/physics/space_sw.cpp | 417 +++++++++++++++++++++++++++++++++ servers/physics/space_sw.h | 6 + servers/physics_2d/space_2d_sw.cpp | 156 ------------ servers/physics_server.h | 38 ++- servers/register_server_types.cpp | 1 + servers/visual/visual_server_scene.cpp | 18 ++ 22 files changed, 1037 insertions(+), 212 deletions(-) (limited to 'servers') diff --git a/servers/audio/audio_filter_sw.cpp b/servers/audio/audio_filter_sw.cpp index 1210312ac5..4bf1cebf12 100644 --- a/servers/audio/audio_filter_sw.cpp +++ b/servers/audio/audio_filter_sw.cpp @@ -242,28 +242,49 @@ AudioFilterSW::Processor::Processor() { set_filter(NULL); } -void AudioFilterSW::Processor::set_filter(AudioFilterSW *p_filter) { +void AudioFilterSW::Processor::set_filter(AudioFilterSW *p_filter, bool p_clear_history) { - ha1 = ha2 = hb1 = hb2 = 0; + if (p_clear_history) { + ha1 = ha2 = hb1 = hb2 = 0; + } filter = p_filter; } -void AudioFilterSW::Processor::update_coeffs() { +void AudioFilterSW::Processor::update_coeffs(int p_interp_buffer_len) { if (!filter) return; - filter->prepare_coefficients(&coeffs); + if (p_interp_buffer_len) { //interpolate + Coeffs old_coeffs = coeffs; + filter->prepare_coefficients(&coeffs); + incr_coeffs.a1 = (coeffs.a1 - old_coeffs.a1) / p_interp_buffer_len; + incr_coeffs.a2 = (coeffs.a2 - old_coeffs.a2) / p_interp_buffer_len; + incr_coeffs.b0 = (coeffs.b0 - old_coeffs.b0) / p_interp_buffer_len; + incr_coeffs.b1 = (coeffs.b1 - old_coeffs.b1) / p_interp_buffer_len; + incr_coeffs.b2 = (coeffs.b2 - old_coeffs.b2) / p_interp_buffer_len; + coeffs = old_coeffs; + } else { + filter->prepare_coefficients(&coeffs); + } } -void AudioFilterSW::Processor::process(float *p_samples, int p_amount, int p_stride) { +void AudioFilterSW::Processor::process(float *p_samples, int p_amount, int p_stride, bool p_interpolate) { if (!filter) return; - for (int i = 0; i < p_amount; i++) { + if (p_interpolate) { + for (int i = 0; i < p_amount; i++) { + + process_one_interp(*p_samples); + p_samples += p_stride; + } + } else { + for (int i = 0; i < p_amount; i++) { - process_one(*p_samples); - p_samples += p_stride; + process_one(*p_samples); + p_samples += p_stride; + } } } diff --git a/servers/audio/audio_filter_sw.h b/servers/audio/audio_filter_sw.h index e1dd5e5c0e..f5a07c4c8f 100644 --- a/servers/audio/audio_filter_sw.h +++ b/servers/audio/audio_filter_sw.h @@ -60,11 +60,14 @@ public: AudioFilterSW *filter; Coeffs coeffs; float ha1, ha2, hb1, hb2; //history + Coeffs incr_coeffs; + public: - void set_filter(AudioFilterSW *p_filter); - void process(float *p_samples, int p_amount, int p_stride = 1); - void update_coeffs(); + void set_filter(AudioFilterSW *p_filter, bool p_clear_history = true); + void process(float *p_samples, int p_amount, int p_stride = 1, bool p_interpolate = false); + void update_coeffs(int p_interp_buffer_len = 0); _ALWAYS_INLINE_ void process_one(float &p_sample); + _ALWAYS_INLINE_ void process_one_interp(float &p_sample); Processor(); }; @@ -104,4 +107,20 @@ void AudioFilterSW::Processor::process_one(float &p_val) { ha1 = p_val; } +void AudioFilterSW::Processor::process_one_interp(float &p_val) { + + float pre = p_val; + p_val = (p_val * coeffs.b0 + hb1 * coeffs.b1 + hb2 * coeffs.b2 + ha1 * coeffs.a1 + ha2 * coeffs.a2); + ha2 = ha1; + hb2 = hb1; + hb1 = pre; + ha1 = p_val; + + coeffs.b0 += incr_coeffs.b0; + coeffs.b1 += incr_coeffs.b1; + coeffs.b2 += incr_coeffs.b2; + coeffs.a1 += incr_coeffs.a1; + coeffs.a2 += incr_coeffs.a2; +} + #endif // AUDIO_FILTER_SW_H diff --git a/servers/audio/audio_stream.cpp b/servers/audio/audio_stream.cpp index 14a091e27c..aa498cccad 100644 --- a/servers/audio/audio_stream.cpp +++ b/servers/audio/audio_stream.cpp @@ -81,3 +81,134 @@ void AudioStreamPlaybackResampled::mix(AudioFrame *p_buffer, float p_rate_scale, } } } +//////////////////////////////// + +void AudioStreamRandomPitch::set_audio_stream(const Ref &p_audio_stream) { + + audio_stream = p_audio_stream; + if (audio_stream.is_valid()) { + for (Set::Element *E = playbacks.front(); E; E = E->next()) { + E->get()->playback = audio_stream->instance_playback(); + } + } +} + +Ref AudioStreamRandomPitch::get_audio_stream() const { + + return audio_stream; +} + +void AudioStreamRandomPitch::set_random_pitch(float p_pitch) { + + if (p_pitch < 1) + p_pitch = 1; + random_pitch = p_pitch; +} + +float AudioStreamRandomPitch::get_random_pitch() const { + return random_pitch; +} + +Ref AudioStreamRandomPitch::instance_playback() { + Ref playback; + playback.instance(); + if (audio_stream.is_valid()) + playback->playback = audio_stream->instance_playback(); + + playbacks.insert(playback.ptr()); + playback->random_pitch = Ref((AudioStreamRandomPitch *)this); + return playback; +} + +String AudioStreamRandomPitch::get_stream_name() const { + + if (audio_stream.is_valid()) { + return "Random: " + audio_stream->get_name(); + } + return "RandomPitch"; +} + +void AudioStreamRandomPitch::_bind_methods() { + + ClassDB::bind_method(D_METHOD("set_audio_stream", "stream"), &AudioStreamRandomPitch::set_audio_stream); + ClassDB::bind_method(D_METHOD("get_audio_stream"), &AudioStreamRandomPitch::get_audio_stream); + + ClassDB::bind_method(D_METHOD("set_random_pitch", "scale"), &AudioStreamRandomPitch::set_random_pitch); + ClassDB::bind_method(D_METHOD("get_random_pitch"), &AudioStreamRandomPitch::get_random_pitch); + + ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "audio_stream", PROPERTY_HINT_RESOURCE_TYPE, "AudioStream"), "set_audio_stream", "get_audio_stream"); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "random_pitch", PROPERTY_HINT_RANGE, "1,16,0.01"), "set_random_pitch", "get_random_pitch"); +} + +AudioStreamRandomPitch::AudioStreamRandomPitch() { + random_pitch = 1.1; +} + +void AudioStreamPlaybackRandomPitch::start(float p_from_pos) { + playing = playback; + float range_from = 1.0 / random_pitch->random_pitch; + float range_to = random_pitch->random_pitch; + + pitch_scale = range_from + Math::randf() * (range_to - range_from); + + if (playing.is_valid()) { + playing->start(p_from_pos); + } +} + +void AudioStreamPlaybackRandomPitch::stop() { + if (playing.is_valid()) { + playing->stop(); + ; + } +} +bool AudioStreamPlaybackRandomPitch::is_playing() const { + if (playing.is_valid()) { + return playing->is_playing(); + } + + return false; +} + +int AudioStreamPlaybackRandomPitch::get_loop_count() const { + if (playing.is_valid()) { + return playing->get_loop_count(); + } + + return 0; +} + +float AudioStreamPlaybackRandomPitch::get_pos() const { + if (playing.is_valid()) { + return playing->get_pos(); + } + + return 0; +} +void AudioStreamPlaybackRandomPitch::seek_pos(float p_time) { + if (playing.is_valid()) { + playing->seek_pos(p_time); + } +} + +void AudioStreamPlaybackRandomPitch::mix(AudioFrame *p_bufer, float p_rate_scale, int p_frames) { + if (playing.is_valid()) { + playing->mix(p_bufer, p_rate_scale * pitch_scale, p_frames); + } else { + for (int i = 0; i < p_frames; i++) { + p_bufer[i] = AudioFrame(0, 0); + } + } +} + +float AudioStreamPlaybackRandomPitch::get_length() const { + if (playing.is_valid()) { + return playing->get_length(); + } + + return 0; +} + +AudioStreamPlaybackRandomPitch::~AudioStreamPlaybackRandomPitch() { + random_pitch->playbacks.erase(this); +} diff --git a/servers/audio/audio_stream.h b/servers/audio/audio_stream.h index 1cf3cd294d..a35826be21 100644 --- a/servers/audio/audio_stream.h +++ b/servers/audio/audio_stream.h @@ -31,6 +31,7 @@ #define AUDIO_STREAM_H #include "resource.h" +#include "servers/audio/audio_filter_sw.h" #include "servers/audio_server.h" class AudioStreamPlayback : public Reference { @@ -88,4 +89,58 @@ public: virtual String get_stream_name() const = 0; }; +class AudioStreamPlaybackRandomPitch; + +class AudioStreamRandomPitch : public AudioStream { + + GDCLASS(AudioStreamRandomPitch, AudioStream) + friend class AudioStreamPlaybackRandomPitch; + + Set playbacks; + Ref audio_stream; + float random_pitch; + +protected: + static void _bind_methods(); + +public: + void set_audio_stream(const Ref &audio_stream); + Ref get_audio_stream() const; + + void set_random_pitch(float p_pitch); + float get_random_pitch() const; + + virtual Ref instance_playback(); + virtual String get_stream_name() const; + + AudioStreamRandomPitch(); +}; + +class AudioStreamPlaybackRandomPitch : public AudioStreamPlayback { + + GDCLASS(AudioStreamPlaybackRandomPitch, AudioStreamPlayback) + friend class AudioStreamRandomPitch; + + Ref random_pitch; + Ref playback; + Ref playing; + float pitch_scale; + +public: + virtual void start(float p_from_pos = 0.0); + virtual void stop(); + virtual bool is_playing() const; + + virtual int get_loop_count() const; //times it looped + + virtual float get_pos() const; + virtual void seek_pos(float p_time); + + virtual void mix(AudioFrame *p_bufer, float p_rate_scale, int p_frames); + + virtual float get_length() const; //if supported, otherwise return 0 + + ~AudioStreamPlaybackRandomPitch(); +}; + #endif // AUDIO_STREAM_H diff --git a/servers/physics/area_pair_sw.cpp b/servers/physics/area_pair_sw.cpp index 8ec001709d..5c418c473f 100644 --- a/servers/physics/area_pair_sw.cpp +++ b/servers/physics/area_pair_sw.cpp @@ -32,12 +32,13 @@ bool AreaPairSW::setup(real_t p_step) { - if (!area->test_collision_mask(body)) { - colliding = false; - return false; - } + bool result = false; - bool result = CollisionSolverSW::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), NULL, this); + if (area->is_shape_set_as_disabled(area_shape) || body->is_shape_set_as_disabled(body_shape)) { + result = false; + } else if (area->test_collision_mask(body) && CollisionSolverSW::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), NULL, this)) { + result = true; + } if (result != colliding) { @@ -95,14 +96,13 @@ AreaPairSW::~AreaPairSW() { bool Area2PairSW::setup(real_t p_step) { - if (!area_a->test_collision_mask(area_b)) { - colliding = false; - return false; + bool result = false; + if (area_a->is_shape_set_as_disabled(shape_a) || area_b->is_shape_set_as_disabled(shape_b)) { + result = false; + } else if (area_a->test_collision_mask(area_b) && CollisionSolverSW::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), NULL, this)) { + result = true; } - //bool result = area_a->test_collision_mask(area_b) && CollisionSolverSW::solve(area_a->get_shape(shape_a),area_a->get_transform() * area_a->get_shape_transform(shape_a),Vector2(),area_b->get_shape(shape_b),area_b->get_transform() * area_b->get_shape_transform(shape_b),Vector2(),NULL,this); - bool result = CollisionSolverSW::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), NULL, this); - if (result != colliding) { if (result) { diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp index d740d3c384..9ada1fbc50 100644 --- a/servers/physics/body_pair_sw.cpp +++ b/servers/physics/body_pair_sw.cpp @@ -214,6 +214,11 @@ bool BodyPairSW::setup(real_t p_step) { return false; } + if (A->is_shape_set_as_disabled(shape_A) || B->is_shape_set_as_disabled(shape_B)) { + collided = false; + return false; + } + offset_B = B->get_transform().get_origin() - A->get_transform().get_origin(); validate_contacts(); @@ -313,12 +318,6 @@ bool BodyPairSW::setup(real_t p_step) { B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crB); } - if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || (A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC)) { - c.active = false; - collided = false; - continue; - } - c.active = true; // Precompute normal mass, tangent mass, and bias. diff --git a/servers/physics/broad_phase_basic.cpp b/servers/physics/broad_phase_basic.cpp index 77d8538574..679b9a31fc 100644 --- a/servers/physics/broad_phase_basic.cpp +++ b/servers/physics/broad_phase_basic.cpp @@ -105,6 +105,26 @@ int BroadPhaseBasic::get_subindex(ID p_id) const { return E->get().subindex; } +int BroadPhaseBasic::cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { + + int rc = 0; + + for (Map::Element *E = element_map.front(); E; E = E->next()) { + + const Rect3 aabb = E->get().aabb; + if (aabb.has_point(p_point)) { + + p_results[rc] = E->get().owner; + p_result_indices[rc] = E->get().subindex; + rc++; + if (rc >= p_max_results) + break; + } + } + + return rc; +} + int BroadPhaseBasic::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { int rc = 0; diff --git a/servers/physics/broad_phase_basic.h b/servers/physics/broad_phase_basic.h index a285204a32..8dabf72f11 100644 --- a/servers/physics/broad_phase_basic.h +++ b/servers/physics/broad_phase_basic.h @@ -91,6 +91,7 @@ public: virtual bool is_static(ID p_id) const; virtual int get_subindex(ID p_id) const; + virtual int cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); virtual int cull_aabb(const Rect3 &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); diff --git a/servers/physics/broad_phase_octree.cpp b/servers/physics/broad_phase_octree.cpp index d18da1b238..2439fbeae9 100644 --- a/servers/physics/broad_phase_octree.cpp +++ b/servers/physics/broad_phase_octree.cpp @@ -66,6 +66,11 @@ int BroadPhaseOctree::get_subindex(ID p_id) const { return octree.get_subindex(p_id); } +int BroadPhaseOctree::cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { + + return octree.cull_point(p_point, p_results, p_max_results, p_result_indices); +} + int BroadPhaseOctree::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) { return octree.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices); diff --git a/servers/physics/broad_phase_octree.h b/servers/physics/broad_phase_octree.h index 086fb0a1a3..88d72a2bd8 100644 --- a/servers/physics/broad_phase_octree.h +++ b/servers/physics/broad_phase_octree.h @@ -56,6 +56,7 @@ public: virtual bool is_static(ID p_id) const; virtual int get_subindex(ID p_id) const; + virtual int cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); virtual int cull_aabb(const Rect3 &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL); diff --git a/servers/physics/broad_phase_sw.h b/servers/physics/broad_phase_sw.h index 8fe901c8ef..5564cf5077 100644 --- a/servers/physics/broad_phase_sw.h +++ b/servers/physics/broad_phase_sw.h @@ -57,6 +57,7 @@ public: virtual bool is_static(ID p_id) const = 0; virtual int get_subindex(ID p_id) const = 0; + virtual int cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0; virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0; virtual int cull_aabb(const Rect3 &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0; diff --git a/servers/physics/collision_object_sw.h b/servers/physics/collision_object_sw.h index 15082a0551..a56253e33d 100644 --- a/servers/physics/collision_object_sw.h +++ b/servers/physics/collision_object_sw.h @@ -64,9 +64,9 @@ private: Rect3 aabb_cache; //for rayqueries real_t area_cache; ShapeSW *shape; - bool trigger; + bool disabled; - Shape() { trigger = false; } + Shape() { disabled = false; } }; Vector shapes; @@ -131,8 +131,8 @@ public: _FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; } _FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; } - _FORCE_INLINE_ void set_shape_as_trigger(int p_idx, bool p_enable) { shapes[p_idx].trigger = p_enable; } - _FORCE_INLINE_ bool is_shape_set_as_trigger(int p_idx) const { return shapes[p_idx].trigger; } + _FORCE_INLINE_ void set_shape_as_disabled(int p_idx, bool p_enable) { shapes[p_idx].disabled = p_enable; } + _FORCE_INLINE_ bool is_shape_set_as_disabled(int p_idx) const { return shapes[p_idx].disabled; } _FORCE_INLINE_ void set_collision_layer(uint32_t p_layer) { collision_layer = p_layer; } _FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; } diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp index 733bd5b63b..101bd4b185 100644 --- a/servers/physics/physics_server_sw.cpp +++ b/servers/physics/physics_server_sw.cpp @@ -330,6 +330,14 @@ void PhysicsServerSW::area_clear_shapes(RID p_area) { area->remove_shape(0); } +void PhysicsServerSW::area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) { + + AreaSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + ERR_FAIL_INDEX(p_shape_idx, area->get_shape_count()); + area->set_shape_as_disabled(p_shape_idx, p_disabled); +} + void PhysicsServerSW::area_attach_object_instance_ID(RID p_area, ObjectID p_ID) { if (space_owner.owns(p_area)) { @@ -551,21 +559,12 @@ RID PhysicsServerSW::body_get_shape(RID p_body, int p_shape_idx) const { return shape->get_self(); } -void PhysicsServerSW::body_set_shape_as_trigger(RID p_body, int p_shape_idx, bool p_enable) { +void PhysicsServerSW::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) { BodySW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count()); - body->set_shape_as_trigger(p_shape_idx, p_enable); -} - -bool PhysicsServerSW::body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const { - - BodySW *body = body_owner.get(p_body); - ERR_FAIL_COND_V(!body, false); - ERR_FAIL_INDEX_V(p_shape_idx, body->get_shape_count(), false); - - return body->is_shape_set_as_trigger(p_shape_idx); + body->set_shape_as_disabled(p_shape_idx, p_disabled); } Transform PhysicsServerSW::body_get_shape_transform(RID p_body, int p_shape_idx) const { @@ -875,6 +874,16 @@ bool PhysicsServerSW::body_is_ray_pickable(RID p_body) const { return body->is_ray_pickable(); } +bool PhysicsServerSW::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, float p_margin, MotionResult *r_result) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body, false); + ERR_FAIL_COND_V(!body->get_space(), false); + ERR_FAIL_COND_V(body->get_space()->is_locked(), false); + + return body->get_space()->test_body_motion(body, p_from, p_motion, p_margin, r_result); +} + /* JOINT API */ RID PhysicsServerSW::joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) { @@ -1530,8 +1539,9 @@ void PhysicsServerSW::_shape_col_cbk(const Vector3 &p_point_A, const Vector3 &p_ } } +PhysicsServerSW *PhysicsServerSW::singleton = NULL; PhysicsServerSW::PhysicsServerSW() { - + singleton = this; BroadPhaseSW::create_func = BroadPhaseOctree::_create; island_count = 0; active_objects = 0; diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h index a0a1bcf963..591fe4af46 100644 --- a/servers/physics/physics_server_sw.h +++ b/servers/physics/physics_server_sw.h @@ -63,6 +63,8 @@ class PhysicsServerSW : public PhysicsServer { //void _clear_query(QuerySW *p_query); public: + static PhysicsServerSW *singleton; + struct CollCbkData { int max; @@ -117,6 +119,8 @@ public: virtual void area_remove_shape(RID p_area, int p_shape_idx); virtual void area_clear_shapes(RID p_area); + virtual void area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled); + virtual void area_attach_object_instance_ID(RID p_area, ObjectID p_ID); virtual ObjectID area_get_object_instance_ID(RID p_area) const; @@ -156,8 +160,7 @@ public: virtual RID body_get_shape(RID p_body, int p_shape_idx) const; virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const; - virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx, bool p_enable); - virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const; + virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled); virtual void body_remove_shape(RID p_body, int p_shape_idx); virtual void body_clear_shapes(RID p_body); @@ -214,6 +217,8 @@ public: virtual void body_set_ray_pickable(RID p_body, bool p_enable); virtual bool body_is_ray_pickable(RID p_body) const; + virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, float p_margin = 0.001, MotionResult *r_result = NULL); + /* JOINT API */ virtual RID joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B); diff --git a/servers/physics/shape_sw.cpp b/servers/physics/shape_sw.cpp index a5cea8aff7..b4004c8c94 100644 --- a/servers/physics/shape_sw.cpp +++ b/servers/physics/shape_sw.cpp @@ -117,6 +117,20 @@ bool PlaneShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_en return inters; } +bool PlaneShapeSW::intersect_point(const Vector3 &p_point) const { + + return plane.distance_to(p_point) < 0; +} + +Vector3 PlaneShapeSW::get_closest_point_to(const Vector3 &p_point) const { + + if (plane.is_point_over(p_point)) { + return plane.project(p_point); + } else { + return p_point; + } +} + Vector3 PlaneShapeSW::get_moment_of_inertia(real_t p_mass) const { return Vector3(); //wtf @@ -184,6 +198,21 @@ bool RayShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, return false; //simply not possible } +bool RayShapeSW::intersect_point(const Vector3 &p_point) const { + + return false; //simply not possible +} + +Vector3 RayShapeSW::get_closest_point_to(const Vector3 &p_point) const { + + Vector3 s[2] = { + Vector3(0, 0, 0), + Vector3(0, 0, length) + }; + + return Geometry::get_closest_point_to_segment(p_point, s); +} + Vector3 RayShapeSW::get_moment_of_inertia(real_t p_mass) const { return Vector3(); @@ -245,6 +274,20 @@ bool SphereShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_e return Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal); } +bool SphereShapeSW::intersect_point(const Vector3 &p_point) const { + + return p_point.length() < radius; +} + +Vector3 SphereShapeSW::get_closest_point_to(const Vector3 &p_point) const { + + Vector3 p = p_point; + float l = p.length(); + if (l < radius) + return p_point; + return (p / l) * radius; +} + Vector3 SphereShapeSW::get_moment_of_inertia(real_t p_mass) const { real_t s = 0.4 * p_mass * radius * radius; @@ -390,6 +433,62 @@ bool BoxShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, return aabb.intersects_segment(p_begin, p_end, &r_result, &r_normal); } +bool BoxShapeSW::intersect_point(const Vector3 &p_point) const { + + return (Math::abs(p_point.x) < half_extents.x && Math::abs(p_point.y) < half_extents.y && Math::abs(p_point.z) < half_extents.z); +} + +Vector3 BoxShapeSW::get_closest_point_to(const Vector3 &p_point) const { + + int outside = 0; + Vector3 min_point; + + for (int i = 0; i < 3; i++) { + + if (Math::abs(p_point[i]) > half_extents[i]) { + outside++; + if (outside == 1) { + //use plane if only one side matches + Vector3 n; + n[i] = SGN(p_point[i]); + + Plane p(n, half_extents[i]); + min_point = p.project(p_point); + } + } + } + + if (!outside) + return p_point; //it's inside, don't do anything else + + if (outside == 1) //if only above one plane, this plane clearly wins + return min_point; + + //check segments + float min_distance = 1e20; + Vector3 closest_vertex = half_extents * p_point.sign(); + Vector3 s[2] = { + closest_vertex, + closest_vertex + }; + + for (int i = 0; i < 3; i++) { + + s[1] = closest_vertex; + s[1][i] = -s[1][i]; //edge + + Vector3 closest_edge = Geometry::get_closest_point_to_segment(p_point, s); + + float d = p_point.distance_to(closest_edge); + if (d < min_distance) { + min_point = closest_edge; + min_distance = d; + } + } + + return min_point; +} + Vector3 BoxShapeSW::get_moment_of_inertia(real_t p_mass) const { real_t lx = half_extents.x; @@ -542,6 +641,32 @@ bool CapsuleShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_ return collision; } +bool CapsuleShapeSW::intersect_point(const Vector3 &p_point) const { + + if (Math::abs(p_point.z) < height * 0.5) { + return Vector3(p_point.x, p_point.y, 0).length() < radius; + } else { + Vector3 p = p_point; + p.z = Math::abs(p.z) - height * 0.5; + return p.length() < radius; + } +} + +Vector3 CapsuleShapeSW::get_closest_point_to(const Vector3 &p_point) const { + + Vector3 s[2] = { + Vector3(0, 0, -height * 0.5), + Vector3(0, 0, height * 0.5), + }; + + Vector3 p = Geometry::get_closest_point_to_segment(p_point, s); + + if (p.distance_to(p_point) < radius) + return p_point; + + return p + (p_point - p).normalized() * radius; +} + Vector3 CapsuleShapeSW::get_moment_of_inertia(real_t p_mass) const { // use crappy AABB approximation @@ -738,6 +863,81 @@ bool ConvexPolygonShapeSW::intersect_segment(const Vector3 &p_begin, const Vecto return col; } +bool ConvexPolygonShapeSW::intersect_point(const Vector3 &p_point) const { + + const Geometry::MeshData::Face *faces = mesh.faces.ptr(); + int fc = mesh.faces.size(); + + for (int i = 0; i < fc; i++) { + + if (faces[i].plane.distance_to(p_point) >= 0) + return false; + } + + return true; +} + +Vector3 ConvexPolygonShapeSW::get_closest_point_to(const Vector3 &p_point) const { + + const Geometry::MeshData::Face *faces = mesh.faces.ptr(); + int fc = mesh.faces.size(); + const Vector3 *vertices = mesh.vertices.ptr(); + + bool all_inside = true; + for (int i = 0; i < fc; i++) { + + if (!faces[i].plane.is_point_over(p_point)) + continue; + + all_inside = false; + bool is_inside = true; + int ic = faces[i].indices.size(); + const int *indices = faces[i].indices.ptr(); + + for (int j = 0; j < ic; j++) { + + Vector3 a = vertices[indices[j]]; + Vector3 b = vertices[indices[(j + 1) % ic]]; + Vector3 n = (a - b).cross(faces[i].plane.normal).normalized(); + if (Plane(a, n).is_point_over(p_point)) { + is_inside = false; + break; + } + } + + if (is_inside) { + return faces[i].plane.project(p_point); + } + } + + if (all_inside) { + return p_point; + } + + float min_distance = 1e20; + Vector3 min_point; + + //check edges + const Geometry::MeshData::Edge *edges = mesh.edges.ptr(); + int ec = mesh.edges.size(); + for (int i = 0; i < ec; i++) { + + Vector3 s[2] = { + vertices[edges[i].a], + vertices[edges[i].b] + }; + + Vector3 closest = Geometry::get_closest_point_to_segment(p_point, s); + float d = closest.distance_to(p_point); + if (d < min_distance) { + min_distance = d; + min_point = closest; + } + } + + return min_point; +} + Vector3 ConvexPolygonShapeSW::get_moment_of_inertia(real_t p_mass) const { // use crappy AABB approximation @@ -880,6 +1080,16 @@ bool FaceShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end return c; } +bool FaceShapeSW::intersect_point(const Vector3 &p_point) const { + + return false; //face is flat +} + +Vector3 FaceShapeSW::get_closest_point_to(const Vector3 &p_point) const { + + return Face3(vertex[0], vertex[1], vertex[2]).get_closest_point_to(p_point); +} + Vector3 FaceShapeSW::get_moment_of_inertia(real_t p_mass) const { return Vector3(); // Sorry, but i don't think anyone cares, FaceShape! @@ -1046,6 +1256,16 @@ bool ConcavePolygonShapeSW::intersect_segment(const Vector3 &p_begin, const Vect } } +bool ConcavePolygonShapeSW::intersect_point(const Vector3 &p_point) const { + + return false; //face is flat +} + +Vector3 ConcavePolygonShapeSW::get_closest_point_to(const Vector3 &p_point) const { + + return Vector3(); +} + void ConcavePolygonShapeSW::_cull(int p_idx, _CullParams *p_params) const { const BVH *bvh = &p_params->bvh[p_idx]; @@ -1471,6 +1691,15 @@ bool HeightMapShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 & return false; } +bool HeightMapShapeSW::intersect_point(const Vector3 &p_point) const { + return false; +} + +Vector3 HeightMapShapeSW::get_closest_point_to(const Vector3 &p_point) const { + + return Vector3(); +} + void HeightMapShapeSW::cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const { } diff --git a/servers/physics/shape_sw.h b/servers/physics/shape_sw.h index 808ff0a3a1..aa1975b655 100644 --- a/servers/physics/shape_sw.h +++ b/servers/physics/shape_sw.h @@ -87,8 +87,9 @@ public: virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const = 0; virtual Vector3 get_support(const Vector3 &p_normal) const; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const = 0; - + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const = 0; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const = 0; + virtual bool intersect_point(const Vector3 &p_point) const = 0; virtual Vector3 get_moment_of_inertia(real_t p_mass) const = 0; virtual void set_data(const Variant &p_data) = 0; @@ -134,7 +135,8 @@ public: virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; } virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - + virtual bool intersect_point(const Vector3 &p_point) const; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; virtual Vector3 get_moment_of_inertia(real_t p_mass) const; virtual void set_data(const Variant &p_data); @@ -159,6 +161,8 @@ public: virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; + virtual bool intersect_point(const Vector3 &p_point) const; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; virtual Vector3 get_moment_of_inertia(real_t p_mass) const; @@ -185,6 +189,8 @@ public: virtual Vector3 get_support(const Vector3 &p_normal) const; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; + virtual bool intersect_point(const Vector3 &p_point) const; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; virtual Vector3 get_moment_of_inertia(real_t p_mass) const; @@ -209,6 +215,8 @@ public: virtual Vector3 get_support(const Vector3 &p_normal) const; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; + virtual bool intersect_point(const Vector3 &p_point) const; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; virtual Vector3 get_moment_of_inertia(real_t p_mass) const; @@ -237,6 +245,8 @@ public: virtual Vector3 get_support(const Vector3 &p_normal) const; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; + virtual bool intersect_point(const Vector3 &p_point) const; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; virtual Vector3 get_moment_of_inertia(real_t p_mass) const; @@ -261,6 +271,8 @@ public: virtual Vector3 get_support(const Vector3 &p_normal) const; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; + virtual bool intersect_point(const Vector3 &p_point) const; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; virtual Vector3 get_moment_of_inertia(real_t p_mass) const; @@ -338,6 +350,8 @@ public: virtual Vector3 get_support(const Vector3 &p_normal) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; + virtual bool intersect_point(const Vector3 &p_point) const; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; virtual void cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const; @@ -372,7 +386,9 @@ public: virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3 &p_normal) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; + virtual bool intersect_point(const Vector3 &p_point) const; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; virtual void cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const; virtual Vector3 get_moment_of_inertia(real_t p_mass) const; @@ -397,6 +413,8 @@ struct FaceShapeSW : public ShapeSW { Vector3 get_support(const Vector3 &p_normal) const; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; + virtual bool intersect_point(const Vector3 &p_point) const; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; Vector3 get_moment_of_inertia(real_t p_mass) const; @@ -436,6 +454,8 @@ struct MotionShapeSW : public ShapeSW { } virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; } bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { return false; } + virtual bool intersect_point(const Vector3 &p_point) const { return false; } + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const { return p_point; } Vector3 get_moment_of_inertia(real_t p_mass) const { return Vector3(); } diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp index 2bf98cecfa..2d71fd6061 100644 --- a/servers/physics/space_sw.cpp +++ b/servers/physics/space_sw.cpp @@ -45,6 +45,50 @@ _FORCE_INLINE_ static bool _match_object_type_query(CollisionObjectSW *p_object, return (1 << body->get_mode()) & p_type_mask; } +int PhysicsDirectSpaceStateSW::intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask) { + + ERR_FAIL_COND_V(space->locked, false); + int amount = space->broadphase->cull_point(p_point, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + int cc = 0; + + //Transform ai = p_xform.affine_inverse(); + + for (int i = 0; i < amount; i++) { + + if (cc >= p_result_max) + break; + + if (!_match_object_type_query(space->intersection_query_results[i], p_collision_layer, p_object_type_mask)) + continue; + + //area can't be picked by ray (default) + + if (p_exclude.has(space->intersection_query_results[i]->get_self())) + continue; + + const CollisionObjectSW *col_obj = space->intersection_query_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; + + Transform inv_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + inv_xform.affine_invert(); + + if (!col_obj->get_shape(shape_idx)->intersect_point(inv_xform.xform(p_point))) + continue; + + r_results[cc].collider_id = col_obj->get_instance_id(); + if (r_results[cc].collider_id != 0) + r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id); + else + r_results[cc].collider = NULL; + r_results[cc].rid = col_obj->get_self(); + r_results[cc].shape = shape_idx; + + cc++; + } + + return cc; +} + bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask, bool p_pick_ray) { ERR_FAIL_COND_V(space->locked, false); @@ -428,6 +472,48 @@ bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform &p_shape_ return true; } +Vector3 PhysicsDirectSpaceStateSW::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const { + + CollisionObjectSW *obj = NULL; + obj = PhysicsServerSW::singleton->area_owner.getornull(p_object); + if (!obj) { + obj = PhysicsServerSW::singleton->body_owner.getornull(p_object); + } + ERR_FAIL_COND_V(!obj, Vector3()); + + ERR_FAIL_COND_V(obj->get_space() != space, Vector3()); + + float min_distance = 1e20; + Vector3 min_point; + + bool shapes_found = false; + + for (int i = 0; i < obj->get_shape_count(); i++) { + + if (obj->is_shape_set_as_disabled(i)) + continue; + + Transform shape_xform = obj->get_transform() * obj->get_shape_transform(i); + ShapeSW *shape = obj->get_shape(i); + + Vector3 point = shape->get_closest_point_to(shape_xform.affine_inverse().xform(p_point)); + point = shape_xform.xform(point); + + float dist = point.distance_to(p_point); + if (dist < min_distance) { + min_distance = dist; + min_point = point; + } + shapes_found = true; + } + + if (!shapes_found) { + return obj->get_transform().origin; //no shapes found, use distance to origin. + } else { + return min_point; + } +} + PhysicsDirectSpaceStateSW::PhysicsDirectSpaceStateSW() { space = NULL; @@ -435,6 +521,337 @@ PhysicsDirectSpaceStateSW::PhysicsDirectSpaceStateSW() { //////////////////////////////////////////////////////////////////////////////////////////////////////////// +int SpaceSW::_cull_aabb_for_body(BodySW *p_body, const Rect3 &p_aabb) { + + int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results); + + for (int i = 0; i < amount; i++) { + + bool keep = true; + + if (intersection_query_results[i] == p_body) + keep = false; + else if (intersection_query_results[i]->get_type() == CollisionObjectSW::TYPE_AREA) + keep = false; + else if ((static_cast(intersection_query_results[i])->test_collision_mask(p_body)) == 0) + keep = false; + else if (static_cast(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) + keep = false; + else if (static_cast(intersection_query_results[i])->is_shape_set_as_disabled(intersection_query_subindex_results[i])) + keep = false; + + if (!keep) { + + if (i < amount - 1) { + SWAP(intersection_query_results[i], intersection_query_results[amount - 1]); + SWAP(intersection_query_subindex_results[i], intersection_query_subindex_results[amount - 1]); + } + + amount--; + i--; + } + } + + return amount; +} + +bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer::MotionResult *r_result) { + + //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.. + + if (r_result) { + r_result->collider_id = 0; + r_result->collider_shape = 0; + } + Rect3 body_aabb; + + for (int i = 0; i < p_body->get_shape_count(); i++) { + + if (i == 0) + body_aabb = p_body->get_shape_aabb(i); + else + body_aabb = body_aabb.merge(p_body->get_shape_aabb(i)); + } + + // Undo the currently transform the physics server is aware of and apply the provided one + body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb)); + body_aabb = body_aabb.grow(p_margin); + + Transform body_transform = p_from; + + { + //STEP 1, FREE BODY IF STUCK + + const int max_results = 32; + int recover_attempts = 4; + Vector3 sr[max_results * 2]; + + do { + + PhysicsServerSW::CollCbkData cbk; + cbk.max = max_results; + cbk.amount = 0; + cbk.ptr = sr; + + CollisionSolverSW::CallbackResult cbkres = NULL; + + PhysicsServerSW::CollCbkData *cbkptr = NULL; + cbkptr = &cbk; + cbkres = PhysicsServerSW::_shape_col_cbk; + + bool collided = false; + + int amount = _cull_aabb_for_body(p_body, body_aabb); + + for (int j = 0; j < p_body->get_shape_count(); j++) { + if (p_body->is_shape_set_as_disabled(j)) + continue; + + Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); + ShapeSW *body_shape = p_body->get_shape(j); + for (int i = 0; i < amount; i++) { + + const CollisionObjectSW *col_obj = intersection_query_results[i]; + int shape_idx = intersection_query_subindex_results[i]; + + if (CollisionSolverSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, NULL, p_margin)) { + collided = cbk.amount > 0; + } + } + } + + if (!collided) { + break; + } + + Vector3 recover_motion; + + for (int i = 0; i < cbk.amount; i++) { + + Vector3 a = sr[i * 2 + 0]; + Vector3 b = sr[i * 2 + 1]; + +#if 0 + Vector3 rel = b-a; + real_t d = rel.length(); + if (d==0) + continue; + + Vector3 n = rel/d; + real_t traveled = n.dot(recover_motion); + a+=n*traveled; + + real_t d = a.distance_to(b); + if (dget_shape_count(); j++) { + + if (p_body->is_shape_set_as_disabled(j)) + continue; + + Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); + ShapeSW *body_shape = p_body->get_shape(j); + + Transform body_shape_xform_inv = body_shape_xform.affine_inverse(); + MotionShapeSW mshape; + mshape.shape = body_shape; + mshape.motion = body_shape_xform_inv.basis.xform(p_motion); + + bool stuck = false; + + real_t best_safe = 1; + real_t best_unsafe = 1; + + for (int i = 0; i < amount; i++) { + + const CollisionObjectSW *col_obj = intersection_query_results[i]; + int shape_idx = intersection_query_subindex_results[i]; + + //test initial overlap, does it collide if going all the way? + Vector3 point_A, point_B; + Vector3 sep_axis = p_motion.normalized(); + + Transform col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + //test initial overlap, does it collide if going all the way? + if (CollisionSolverSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { + //print_line("failed motion cast (no collision)"); + continue; + } + sep_axis = p_motion.normalized(); + + if (!CollisionSolverSW::solve_distance(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { + //print_line("failed motion cast (no collision)"); + stuck = true; + break; + } + + //just do kinematic solving + real_t low = 0; + real_t hi = 1; + Vector3 mnormal = p_motion.normalized(); + + for (int i = 0; i < 8; i++) { //steps should be customizable.. + + real_t ofs = (low + hi) * 0.5; + + Vector3 sep = mnormal; //important optimization for this to work fast enough + + mshape.motion = body_shape_xform_inv.basis.xform(p_motion * ofs); + + Vector3 lA, lB; + + bool collided = !CollisionSolverSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, motion_aabb, &sep); + + if (collided) { + + //print_line(itos(i)+": "+rtos(ofs)); + hi = ofs; + } else { + + point_A = lA; + point_B = lB; + low = ofs; + } + } + + if (low < best_safe) { + best_safe = low; + best_unsafe = hi; + } + } + + if (stuck) { + + safe = 0; + unsafe = 0; + best_shape = j; //sadly it's the best + break; + } + if (best_safe == 1.0) { + continue; + } + if (best_safe < safe) { + + safe = best_safe; + unsafe = best_unsafe; + best_shape = j; + } + } + } + + bool collided = false; + if (safe >= 1) { + //not collided + collided = false; + if (r_result) { + + r_result->motion = p_motion; + r_result->remainder = Vector3(); + r_result->motion += (body_transform.get_origin() - p_from.get_origin()); + } + + } else { + + //it collided, let's get the rest info in unsafe advance + Transform ugt = body_transform; + ugt.origin += p_motion * unsafe; + + _RestCallbackData rcd; + rcd.best_len = 0; + rcd.best_object = NULL; + rcd.best_shape = 0; + + Transform body_shape_xform = ugt * p_body->get_shape_transform(best_shape); + ShapeSW *body_shape = p_body->get_shape(best_shape); + + body_aabb.position += p_motion * unsafe; + + int amount = _cull_aabb_for_body(p_body, body_aabb); + + for (int i = 0; i < amount; i++) { + + const CollisionObjectSW *col_obj = intersection_query_results[i]; + int shape_idx = intersection_query_subindex_results[i]; + + rcd.object = col_obj; + rcd.shape = shape_idx; + bool sc = CollisionSolverSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, NULL, p_margin); + if (!sc) + continue; + } + + if (rcd.best_len != 0) { + + if (r_result) { + r_result->collider = rcd.best_object->get_self(); + r_result->collider_id = rcd.best_object->get_instance_id(); + r_result->collider_shape = rcd.best_shape; + r_result->collision_local_shape = best_shape; + r_result->collision_normal = rcd.best_normal; + r_result->collision_point = rcd.best_contact; + //r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape); + + const BodySW *body = static_cast(rcd.best_object); + //Vector3 rel_vec = r_result->collision_point - body->get_transform().get_origin(); + // r_result->collider_velocity = Vector3(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); + r_result->collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - rcd.best_contact); // * mPos); + + r_result->motion = safe * p_motion; + r_result->remainder = p_motion - safe * p_motion; + r_result->motion += (body_transform.get_origin() - p_from.get_origin()); + } + + collided = true; + } else { + if (r_result) { + + r_result->motion = p_motion; + r_result->remainder = Vector3(); + r_result->motion += (body_transform.get_origin() - p_from.get_origin()); + } + + collided = false; + } + } + + return collided; +} + void *SpaceSW::_broadphase_pair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_self) { CollisionObjectSW::Type type_A = A->get_type(); diff --git a/servers/physics/space_sw.h b/servers/physics/space_sw.h index b0e54f647c..6ef12dbeae 100644 --- a/servers/physics/space_sw.h +++ b/servers/physics/space_sw.h @@ -47,11 +47,13 @@ class PhysicsDirectSpaceStateSW : public PhysicsDirectSpaceState { public: SpaceSW *space; + virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set &p_exclude = Set(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION); virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set &p_exclude = Set(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, bool p_pick_ray = false); virtual int intersect_shape(const RID &p_shape, const Transform &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set &p_exclude = Set(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION); virtual bool cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set &p_exclude = Set(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, ShapeRestInfo *r_info = NULL); virtual bool collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set &p_exclude = Set(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION); virtual bool rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set &p_exclude = Set(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION); + virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const; PhysicsDirectSpaceStateSW(); }; @@ -120,6 +122,8 @@ private: friend class PhysicsDirectSpaceStateSW; + int _cull_aabb_for_body(BodySW *p_body, const Rect3 &p_aabb); + public: _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } _FORCE_INLINE_ RID get_self() const { return self; } @@ -192,6 +196,8 @@ public: void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; } uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; } + bool test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer::MotionResult *r_result); + SpaceSW(); ~SpaceSW(); }; diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index bcc58b3aac..c407a17bc6 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -809,162 +809,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co } return collided; - -#if 0 - //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_tree(),Vector2()); - Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(get_world_2d()->get_space()); - ERR_FAIL_COND_V(!dss,Vector2()); - const int max_shapes=32; - Vector2 sr[max_shapes*2]; - int res_shapes; - - Set exclude; - exclude.insert(get_rid()); - - - //recover first - int recover_attempts=4; - - bool collided=false; - uint32_t mask=0; - if (collide_static) - mask|=Physics2DDirectSpaceState::TYPE_MASK_STATIC_BODY; - if (collide_kinematic) - mask|=Physics2DDirectSpaceState::TYPE_MASK_KINEMATIC_BODY; - if (collide_rigid) - mask|=Physics2DDirectSpaceState::TYPE_MASK_RIGID_BODY; - if (collide_character) - mask|=Physics2DDirectSpaceState::TYPE_MASK_CHARACTER_BODY; - - //print_line("motion: "+p_motion+" margin: "+rtos(margin)); - - //print_line("margin: "+rtos(margin)); - do { - - //motion recover - for(int i=0;icollide_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),Vector2(),margin,sr,max_shapes,res_shapes,exclude,get_layer_mask(),mask)) - collided=true; - - } - - if (!collided) - break; - - Vector2 recover_motion; - - for(int i=0;icast_motion(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i), p_motion, 0,lsafe,lunsafe,exclude,get_layer_mask(),mask); - //print_line("shape: "+itos(i)+" travel:"+rtos(ltravel)); - if (!valid) { - - safe=0; - unsafe=0; - best_shape=i; //sadly it's the best - break; - } - if (lsafe==1.0) { - continue; - } - if (lsafe < safe) { - - safe=lsafe; - unsafe=lunsafe; - best_shape=i; - } - } - - - //print_line("best shape: "+itos(best_shape)+" motion "+p_motion); - - if (safe>=1) { - //not collided - colliding=false; - } else { - - //it collided, let's get the rest info in unsafe advance - Matrix32 ugt = get_global_transform(); - ugt.elements[2]+=p_motion*unsafe; - Physics2DDirectSpaceState::ShapeRestInfo rest_info; - bool c2 = dss->rest_info(get_shape(best_shape)->get_rid(), ugt*get_shape_transform(best_shape), Vector2(), margin,&rest_info,exclude,get_layer_mask(),mask); - if (!c2) { - //should not happen, but floating point precision is so weird.. - - colliding=false; - } else { - - - //print_line("Travel: "+rtos(travel)); - colliding=true; - collision=rest_info.point; - normal=rest_info.normal; - collider=rest_info.collider_id; - collider_vel=rest_info.linear_velocity; - collider_shape=rest_info.shape; - collider_metadata=rest_info.metadata; - } - - } - - Vector2 motion=p_motion*safe; - Matrix32 gt = get_global_transform(); - gt.elements[2]+=motion; - set_global_transform(gt); - - return p_motion-motion; - -#endif - return false; } void *Space2DSW::_broadphase_pair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_self) { diff --git a/servers/physics_server.h b/servers/physics_server.h index 21c65a74d0..0f07fca637 100644 --- a/servers/physics_server.h +++ b/servers/physics_server.h @@ -156,26 +156,28 @@ protected: static void _bind_methods(); public: - struct RayResult { + struct ShapeResult { - Vector3 position; - Vector3 normal; RID rid; ObjectID collider_id; Object *collider; int shape; }; - virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set &p_exclude = Set(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, bool p_pick_ray = false) = 0; + virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set &p_exclude = Set(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION) = 0; - struct ShapeResult { + struct RayResult { + Vector3 position; + Vector3 normal; RID rid; ObjectID collider_id; Object *collider; int shape; }; + virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set &p_exclude = Set(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, bool p_pick_ray = false) = 0; + virtual int intersect_shape(const RID &p_shape, const Transform &p_xform, float p_margin, ShapeResult *r_results, int p_result_max, const Set &p_exclude = Set(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION) = 0; struct ShapeRestInfo { @@ -194,6 +196,8 @@ public: virtual bool rest_info(RID p_shape, const Transform &p_shape_xform, float p_margin, ShapeRestInfo *r_info, const Set &p_exclude = Set(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION) = 0; + virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const = 0; + PhysicsDirectSpaceState(); }; @@ -322,6 +326,8 @@ public: virtual void area_remove_shape(RID p_area, int p_shape_idx) = 0; virtual void area_clear_shapes(RID p_area) = 0; + virtual void area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) = 0; + virtual void area_attach_object_instance_ID(RID p_area, ObjectID p_ID) = 0; virtual ObjectID area_get_object_instance_ID(RID p_area) const = 0; @@ -370,12 +376,11 @@ public: virtual RID body_get_shape(RID p_body, int p_shape_idx) const = 0; virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const = 0; - virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx, bool p_enable) = 0; - virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const = 0; - virtual void body_remove_shape(RID p_body, int p_shape_idx) = 0; virtual void body_clear_shapes(RID p_body) = 0; + virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) = 0; + virtual void body_attach_object_instance_ID(RID p_body, uint32_t p_ID) = 0; virtual uint32_t body_get_object_instance_ID(RID p_body) const = 0; @@ -458,6 +463,23 @@ public: virtual void body_set_ray_pickable(RID p_body, bool p_enable) = 0; virtual bool body_is_ray_pickable(RID p_body) const = 0; + struct MotionResult { + + Vector3 motion; + Vector3 remainder; + + Vector3 collision_point; + Vector3 collision_normal; + Vector3 collider_velocity; + int collision_local_shape; + ObjectID collider_id; + RID collider; + int collider_shape; + Variant collider_metadata; + }; + + virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, float p_margin = 0.001, MotionResult *r_result = NULL) = 0; + /* JOINT API */ enum JointType { diff --git a/servers/register_server_types.cpp b/servers/register_server_types.cpp index 79b994dd27..e97a6baeba 100644 --- a/servers/register_server_types.cpp +++ b/servers/register_server_types.cpp @@ -84,6 +84,7 @@ void register_server_types() { ClassDB::register_virtual_class(); ClassDB::register_virtual_class(); + ClassDB::register_class(); ClassDB::register_virtual_class(); ClassDB::register_class(); diff --git a/servers/visual/visual_server_scene.cpp b/servers/visual/visual_server_scene.cpp index fb1c66d0b9..5009d76d43 100644 --- a/servers/visual/visual_server_scene.cpp +++ b/servers/visual/visual_server_scene.cpp @@ -1482,6 +1482,8 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons // a pre pass will need to be needed to determine the actual z-near to be used + Plane near_plane(p_instance->transform.origin, -p_instance->transform.basis.get_axis(2)); + for (int j = 0; j < cull_count; j++) { float min, max; @@ -1494,6 +1496,8 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons } instance->transformed_aabb.project_range_in_plane(Plane(z_vec, 0), min, max); + instance->depth = near_plane.distance_to(instance->transform.origin); + instance->depth_layer = 0; if (max > z_max) z_max = max; } @@ -1539,6 +1543,7 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons planes[4] = p_instance->transform.xform(Plane(Vector3(0, -1, z).normalized(), radius)); int cull_count = p_scenario->octree.cull_convex(planes, instance_shadow_cull_result, MAX_INSTANCE_CULL, VS::INSTANCE_GEOMETRY_MASK); + Plane near_plane(p_instance->transform.origin, p_instance->transform.basis.get_axis(2) * z); for (int j = 0; j < cull_count; j++) { @@ -1547,6 +1552,9 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons cull_count--; SWAP(instance_shadow_cull_result[j], instance_shadow_cull_result[cull_count]); j--; + } else { + instance->depth = near_plane.distance_to(instance->transform.origin); + instance->depth_layer = 0; } } @@ -1587,6 +1595,7 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons int cull_count = p_scenario->octree.cull_convex(planes, instance_shadow_cull_result, MAX_INSTANCE_CULL, VS::INSTANCE_GEOMETRY_MASK); + Plane near_plane(xform.origin, -xform.basis.get_axis(2)); for (int j = 0; j < cull_count; j++) { Instance *instance = instance_shadow_cull_result[j]; @@ -1594,6 +1603,9 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons cull_count--; SWAP(instance_shadow_cull_result[j], instance_shadow_cull_result[cull_count]); j--; + } else { + instance->depth = near_plane.distance_to(instance->transform.origin); + instance->depth_layer = 0; } } @@ -1619,6 +1631,7 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons Vector planes = cm.get_projection_planes(p_instance->transform); int cull_count = p_scenario->octree.cull_convex(planes, instance_shadow_cull_result, MAX_INSTANCE_CULL, VS::INSTANCE_GEOMETRY_MASK); + Plane near_plane(p_instance->transform.origin, -p_instance->transform.basis.get_axis(2)); for (int j = 0; j < cull_count; j++) { Instance *instance = instance_shadow_cull_result[j]; @@ -1626,6 +1639,9 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons cull_count--; SWAP(instance_shadow_cull_result[j], instance_shadow_cull_result[cull_count]); j--; + } else { + instance->depth = near_plane.distance_to(instance->transform.origin); + instance->depth_layer = 0; } } @@ -3344,6 +3360,8 @@ void VisualServerScene::_update_dirty_instance(Instance *p_instance) { for (int i = 0; i < dp; i++) { RID mesh = VSG::storage->particles_get_draw_pass_mesh(p_instance->base, i); + if (!mesh.is_valid()) + continue; int sc = VSG::storage->mesh_get_surface_count(mesh); for (int j = 0; j < sc; j++) { -- cgit v1.2.3