1/**************************************************************************/
2/* velocity_tracker_3d.cpp */
3/**************************************************************************/
4/* This file is part of: */
5/* GODOT ENGINE */
6/* https://godotengine.org */
7/**************************************************************************/
8/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
9/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
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 "velocity_tracker_3d.h"
32
33void VelocityTracker3D::set_track_physics_step(bool p_track_physics_step) {
34 physics_step = p_track_physics_step;
35}
36
37bool VelocityTracker3D::is_tracking_physics_step() const {
38 return physics_step;
39}
40
41void VelocityTracker3D::update_position(const Vector3 &p_position) {
42 PositionHistory ph;
43 ph.position = p_position;
44 if (physics_step) {
45 ph.frame = Engine::get_singleton()->get_physics_frames();
46 } else {
47 ph.frame = Engine::get_singleton()->get_frame_ticks();
48 }
49
50 if (position_history_len == 0 || position_history[0].frame != ph.frame) { //in same frame, use latest
51 position_history_len = MIN(position_history.size(), position_history_len + 1);
52 for (int i = position_history_len - 1; i > 0; i--) {
53 position_history.write[i] = position_history[i - 1];
54 }
55 }
56
57 position_history.write[0] = ph;
58}
59
60Vector3 VelocityTracker3D::get_tracked_linear_velocity() const {
61 Vector3 linear_velocity;
62
63 double max_time = 1 / 5.0; //maximum time to interpolate a velocity
64
65 Vector3 distance_accum;
66 double time_accum = 0.0;
67 double base_time = 0.0;
68
69 if (position_history_len) {
70 if (physics_step) {
71 uint64_t base = Engine::get_singleton()->get_physics_frames();
72 base_time = double(base - position_history[0].frame) / Engine::get_singleton()->get_physics_ticks_per_second();
73 } else {
74 uint64_t base = Engine::get_singleton()->get_frame_ticks();
75 base_time = double(base - position_history[0].frame) / 1000000.0;
76 }
77 }
78
79 for (int i = 0; i < position_history_len - 1; i++) {
80 double delta = 0.0;
81 uint64_t diff = position_history[i].frame - position_history[i + 1].frame;
82 Vector3 distance = position_history[i].position - position_history[i + 1].position;
83
84 if (physics_step) {
85 delta = double(diff) / Engine::get_singleton()->get_physics_ticks_per_second();
86 } else {
87 delta = double(diff) / 1000000.0;
88 }
89
90 if (base_time + time_accum + delta > max_time) {
91 break;
92 }
93
94 distance_accum += distance;
95 time_accum += delta;
96 }
97
98 if (time_accum) {
99 linear_velocity = distance_accum / time_accum;
100 }
101
102 return linear_velocity;
103}
104
105void VelocityTracker3D::reset(const Vector3 &p_new_pos) {
106 PositionHistory ph;
107 ph.position = p_new_pos;
108 if (physics_step) {
109 ph.frame = Engine::get_singleton()->get_physics_frames();
110 } else {
111 ph.frame = Engine::get_singleton()->get_frame_ticks();
112 }
113
114 position_history.write[0] = ph;
115 position_history_len = 1;
116}
117
118void VelocityTracker3D::_bind_methods() {
119 ClassDB::bind_method(D_METHOD("set_track_physics_step", "enable"), &VelocityTracker3D::set_track_physics_step);
120 ClassDB::bind_method(D_METHOD("is_tracking_physics_step"), &VelocityTracker3D::is_tracking_physics_step);
121 ClassDB::bind_method(D_METHOD("update_position", "position"), &VelocityTracker3D::update_position);
122 ClassDB::bind_method(D_METHOD("get_tracked_linear_velocity"), &VelocityTracker3D::get_tracked_linear_velocity);
123 ClassDB::bind_method(D_METHOD("reset", "position"), &VelocityTracker3D::reset);
124
125 ADD_PROPERTY(PropertyInfo(Variant::BOOL, "track_physics_step"), "set_track_physics_step", "is_tracking_physics_step");
126}
127
128VelocityTracker3D::VelocityTracker3D() {
129 position_history.resize(4); // should be configurable
130}
131