1 /*************************************************************************/
2 /* spatial_velocity_tracker.cpp */
3 /*************************************************************************/
4 /* This file is part of: */
5 /* GODOT ENGINE */
6 /* https://godotengine.org */
7 /*************************************************************************/
8 /* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
9 /* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */
10 /* */
11 /* Permission is hereby granted, free of charge, to any person obtaining */
12 /* a copy of this software and associated documentation files (the */
13 /* "Software"), to deal in the Software without restriction, including */
14 /* without limitation the rights to use, copy, modify, merge, publish, */
15 /* distribute, sublicense, and/or sell copies of the Software, and to */
16 /* permit persons to whom the Software is furnished to do so, subject to */
17 /* the following conditions: */
18 /* */
19 /* The above copyright notice and this permission notice shall be */
20 /* included in all copies or substantial portions of the Software. */
21 /* */
22 /* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
23 /* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
24 /* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
25 /* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
26 /* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
27 /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
28 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
29 /*************************************************************************/
30
31 #include "spatial_velocity_tracker.h"
32 #include "core/engine.h"
33
set_track_physics_step(bool p_track_physics_step)34 void SpatialVelocityTracker::set_track_physics_step(bool p_track_physics_step) {
35
36 physics_step = p_track_physics_step;
37 }
38
is_tracking_physics_step() const39 bool SpatialVelocityTracker::is_tracking_physics_step() const {
40
41 return physics_step;
42 }
update_position(const Vector3 & p_position)43 void SpatialVelocityTracker::update_position(const Vector3 &p_position) {
44
45 PositionHistory ph;
46 ph.position = p_position;
47 if (physics_step) {
48 ph.frame = Engine::get_singleton()->get_physics_frames();
49 } else {
50 ph.frame = Engine::get_singleton()->get_idle_frame_ticks();
51 }
52
53 if (position_history_len == 0 || position_history[0].frame != ph.frame) { //in same frame, use latest
54 position_history_len = MIN(position_history.size(), position_history_len + 1);
55 for (int i = position_history_len - 1; i > 0; i--) {
56 position_history.write[i] = position_history[i - 1];
57 }
58 }
59
60 position_history.write[0] = ph;
61 }
get_tracked_linear_velocity() const62 Vector3 SpatialVelocityTracker::get_tracked_linear_velocity() const {
63
64 Vector3 linear_velocity;
65
66 float max_time = 1 / 5.0; //maximum time to interpolate a velocity
67
68 Vector3 distance_accum;
69 float time_accum = 0.0;
70 float base_time = 0.0;
71
72 if (position_history_len) {
73 if (physics_step) {
74 uint64_t base = Engine::get_singleton()->get_physics_frames();
75 base_time = float(base - position_history[0].frame) / Engine::get_singleton()->get_iterations_per_second();
76 } else {
77 uint64_t base = Engine::get_singleton()->get_idle_frame_ticks();
78 base_time = double(base - position_history[0].frame) / 1000000.0;
79 }
80 }
81
82 for (int i = 0; i < position_history_len - 1; i++) {
83 float delta = 0.0;
84 uint64_t diff = position_history[i].frame - position_history[i + 1].frame;
85 Vector3 distance = position_history[i].position - position_history[i + 1].position;
86
87 if (physics_step) {
88 delta = float(diff) / Engine::get_singleton()->get_iterations_per_second();
89 } else {
90 delta = double(diff) / 1000000.0;
91 }
92
93 if (base_time + time_accum + delta > max_time)
94 break;
95
96 distance_accum += distance;
97 time_accum += delta;
98 }
99
100 if (time_accum) {
101 linear_velocity = distance_accum / time_accum;
102 }
103
104 return linear_velocity;
105 }
106
reset(const Vector3 & p_new_pos)107 void SpatialVelocityTracker::reset(const Vector3 &p_new_pos) {
108
109 PositionHistory ph;
110 ph.position = p_new_pos;
111 if (physics_step) {
112 ph.frame = Engine::get_singleton()->get_physics_frames();
113 } else {
114 ph.frame = Engine::get_singleton()->get_idle_frame_ticks();
115 }
116
117 position_history.write[0] = ph;
118 position_history_len = 1;
119 }
120
_bind_methods()121 void SpatialVelocityTracker::_bind_methods() {
122
123 ClassDB::bind_method(D_METHOD("set_track_physics_step", "enable"), &SpatialVelocityTracker::set_track_physics_step);
124 ClassDB::bind_method(D_METHOD("is_tracking_physics_step"), &SpatialVelocityTracker::is_tracking_physics_step);
125 ClassDB::bind_method(D_METHOD("update_position", "position"), &SpatialVelocityTracker::update_position);
126 ClassDB::bind_method(D_METHOD("get_tracked_linear_velocity"), &SpatialVelocityTracker::get_tracked_linear_velocity);
127 ClassDB::bind_method(D_METHOD("reset", "position"), &SpatialVelocityTracker::reset);
128
129 ADD_PROPERTY(PropertyInfo(Variant::BOOL, "track_physics_step"), "set_track_physics_step", "is_tracking_physics_step");
130 }
131
SpatialVelocityTracker()132 SpatialVelocityTracker::SpatialVelocityTracker() {
133 position_history.resize(4); // should be configurable
134 position_history_len = 0;
135 physics_step = false;
136 }
137