1/**************************************************************************/
2/* godot_body_3d.h */
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#ifndef GODOT_BODY_3D_H
32#define GODOT_BODY_3D_H
33
34#include "godot_area_3d.h"
35#include "godot_collision_object_3d.h"
36
37#include "core/templates/vset.h"
38
39class GodotConstraint3D;
40class GodotPhysicsDirectBodyState3D;
41
42class GodotBody3D : public GodotCollisionObject3D {
43 PhysicsServer3D::BodyMode mode = PhysicsServer3D::BODY_MODE_RIGID;
44
45 Vector3 linear_velocity;
46 Vector3 angular_velocity;
47
48 Vector3 prev_linear_velocity;
49 Vector3 prev_angular_velocity;
50
51 Vector3 constant_linear_velocity;
52 Vector3 constant_angular_velocity;
53
54 Vector3 biased_linear_velocity;
55 Vector3 biased_angular_velocity;
56 real_t mass = 1.0;
57 real_t bounce = 0.0;
58 real_t friction = 1.0;
59 Vector3 inertia;
60
61 PhysicsServer3D::BodyDampMode linear_damp_mode = PhysicsServer3D::BODY_DAMP_MODE_COMBINE;
62 PhysicsServer3D::BodyDampMode angular_damp_mode = PhysicsServer3D::BODY_DAMP_MODE_COMBINE;
63
64 real_t linear_damp = 0.0;
65 real_t angular_damp = 0.0;
66
67 real_t total_linear_damp = 0.0;
68 real_t total_angular_damp = 0.0;
69
70 real_t gravity_scale = 1.0;
71
72 uint16_t locked_axis = 0;
73
74 real_t _inv_mass = 1.0;
75 Vector3 _inv_inertia; // Relative to the principal axes of inertia
76
77 // Relative to the local frame of reference
78 Basis principal_inertia_axes_local;
79 Vector3 center_of_mass_local;
80
81 // In world orientation with local origin
82 Basis _inv_inertia_tensor;
83 Basis principal_inertia_axes;
84 Vector3 center_of_mass;
85
86 bool calculate_inertia = true;
87 bool calculate_center_of_mass = true;
88
89 Vector3 gravity;
90
91 real_t still_time = 0.0;
92
93 Vector3 applied_force;
94 Vector3 applied_torque;
95
96 Vector3 constant_force;
97 Vector3 constant_torque;
98
99 SelfList<GodotBody3D> active_list;
100 SelfList<GodotBody3D> mass_properties_update_list;
101 SelfList<GodotBody3D> direct_state_query_list;
102
103 VSet<RID> exceptions;
104 bool omit_force_integration = false;
105 bool active = true;
106
107 bool continuous_cd = false;
108 bool can_sleep = true;
109 bool first_time_kinematic = false;
110
111 void _mass_properties_changed();
112 virtual void _shapes_changed() override;
113 Transform3D new_transform;
114
115 HashMap<GodotConstraint3D *, int> constraint_map;
116
117 Vector<AreaCMP> areas;
118
119 struct Contact {
120 Vector3 local_pos;
121 Vector3 local_normal;
122 Vector3 local_velocity_at_pos;
123 real_t depth = 0.0;
124 int local_shape = 0;
125 Vector3 collider_pos;
126 int collider_shape = 0;
127 ObjectID collider_instance_id;
128 RID collider;
129 Vector3 collider_velocity_at_pos;
130 Vector3 impulse;
131 };
132
133 Vector<Contact> contacts; //no contacts by default
134 int contact_count = 0;
135
136 Callable body_state_callback;
137
138 struct ForceIntegrationCallbackData {
139 Callable callable;
140 Variant udata;
141 };
142
143 ForceIntegrationCallbackData *fi_callback_data = nullptr;
144
145 GodotPhysicsDirectBodyState3D *direct_state = nullptr;
146
147 uint64_t island_step = 0;
148
149 void _update_transform_dependent();
150
151 friend class GodotPhysicsDirectBodyState3D; // i give up, too many functions to expose
152
153public:
154 void set_state_sync_callback(const Callable &p_callable);
155 void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant());
156
157 GodotPhysicsDirectBodyState3D *get_direct_state();
158
159 _FORCE_INLINE_ void add_area(GodotArea3D *p_area) {
160 int index = areas.find(AreaCMP(p_area));
161 if (index > -1) {
162 areas.write[index].refCount += 1;
163 } else {
164 areas.ordered_insert(AreaCMP(p_area));
165 }
166 }
167
168 _FORCE_INLINE_ void remove_area(GodotArea3D *p_area) {
169 int index = areas.find(AreaCMP(p_area));
170 if (index > -1) {
171 areas.write[index].refCount -= 1;
172 if (areas[index].refCount < 1) {
173 areas.remove_at(index);
174 }
175 }
176 }
177
178 _FORCE_INLINE_ void set_max_contacts_reported(int p_size) {
179 contacts.resize(p_size);
180 contact_count = 0;
181 if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC && p_size) {
182 set_active(true);
183 }
184 }
185 _FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); }
186
187 _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.is_empty(); }
188 _FORCE_INLINE_ void add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_local_velocity_at_pos, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos, const Vector3 &p_impulse);
189
190 _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); }
191 _FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); }
192 _FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); }
193 _FORCE_INLINE_ const VSet<RID> &get_exceptions() const { return exceptions; }
194
195 _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
196 _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; }
197
198 _FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; }
199 _FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraint_map.erase(p_constraint); }
200 const HashMap<GodotConstraint3D *, int> &get_constraint_map() const { return constraint_map; }
201 _FORCE_INLINE_ void clear_constraint_map() { constraint_map.clear(); }
202
203 _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; }
204 _FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; }
205
206 _FORCE_INLINE_ Basis get_principal_inertia_axes() const { return principal_inertia_axes; }
207 _FORCE_INLINE_ Vector3 get_center_of_mass() const { return center_of_mass; }
208 _FORCE_INLINE_ Vector3 get_center_of_mass_local() const { return center_of_mass_local; }
209 _FORCE_INLINE_ Vector3 xform_local_to_principal(const Vector3 &p_pos) const { return principal_inertia_axes_local.xform(p_pos - center_of_mass_local); }
210
211 _FORCE_INLINE_ void set_linear_velocity(const Vector3 &p_velocity) { linear_velocity = p_velocity; }
212 _FORCE_INLINE_ Vector3 get_linear_velocity() const { return linear_velocity; }
213
214 _FORCE_INLINE_ void set_angular_velocity(const Vector3 &p_velocity) { angular_velocity = p_velocity; }
215 _FORCE_INLINE_ Vector3 get_angular_velocity() const { return angular_velocity; }
216
217 _FORCE_INLINE_ Vector3 get_prev_linear_velocity() const { return prev_linear_velocity; }
218 _FORCE_INLINE_ Vector3 get_prev_angular_velocity() const { return prev_angular_velocity; }
219
220 _FORCE_INLINE_ const Vector3 &get_biased_linear_velocity() const { return biased_linear_velocity; }
221 _FORCE_INLINE_ const Vector3 &get_biased_angular_velocity() const { return biased_angular_velocity; }
222
223 _FORCE_INLINE_ void apply_central_impulse(const Vector3 &p_impulse) {
224 linear_velocity += p_impulse * _inv_mass;
225 }
226
227 _FORCE_INLINE_ void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) {
228 linear_velocity += p_impulse * _inv_mass;
229 angular_velocity += _inv_inertia_tensor.xform((p_position - center_of_mass).cross(p_impulse));
230 }
231
232 _FORCE_INLINE_ void apply_torque_impulse(const Vector3 &p_impulse) {
233 angular_velocity += _inv_inertia_tensor.xform(p_impulse);
234 }
235
236 _FORCE_INLINE_ void apply_bias_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3(), real_t p_max_delta_av = -1.0) {
237 biased_linear_velocity += p_impulse * _inv_mass;
238 if (p_max_delta_av != 0.0) {
239 Vector3 delta_av = _inv_inertia_tensor.xform((p_position - center_of_mass).cross(p_impulse));
240 if (p_max_delta_av > 0 && delta_av.length() > p_max_delta_av) {
241 delta_av = delta_av.normalized() * p_max_delta_av;
242 }
243 biased_angular_velocity += delta_av;
244 }
245 }
246
247 _FORCE_INLINE_ void apply_bias_torque_impulse(const Vector3 &p_impulse) {
248 biased_angular_velocity += _inv_inertia_tensor.xform(p_impulse);
249 }
250
251 _FORCE_INLINE_ void apply_central_force(const Vector3 &p_force) {
252 applied_force += p_force;
253 }
254
255 _FORCE_INLINE_ void apply_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) {
256 applied_force += p_force;
257 applied_torque += (p_position - center_of_mass).cross(p_force);
258 }
259
260 _FORCE_INLINE_ void apply_torque(const Vector3 &p_torque) {
261 applied_torque += p_torque;
262 }
263
264 _FORCE_INLINE_ void add_constant_central_force(const Vector3 &p_force) {
265 constant_force += p_force;
266 }
267
268 _FORCE_INLINE_ void add_constant_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) {
269 constant_force += p_force;
270 constant_torque += (p_position - center_of_mass).cross(p_force);
271 }
272
273 _FORCE_INLINE_ void add_constant_torque(const Vector3 &p_torque) {
274 constant_torque += p_torque;
275 }
276
277 void set_constant_force(const Vector3 &p_force) { constant_force = p_force; }
278 Vector3 get_constant_force() const { return constant_force; }
279
280 void set_constant_torque(const Vector3 &p_torque) { constant_torque = p_torque; }
281 Vector3 get_constant_torque() const { return constant_torque; }
282
283 void set_active(bool p_active);
284 _FORCE_INLINE_ bool is_active() const { return active; }
285
286 _FORCE_INLINE_ void wakeup() {
287 if ((!get_space()) || mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) {
288 return;
289 }
290 set_active(true);
291 }
292
293 void set_param(PhysicsServer3D::BodyParameter p_param, const Variant &p_value);
294 Variant get_param(PhysicsServer3D::BodyParameter p_param) const;
295
296 void set_mode(PhysicsServer3D::BodyMode p_mode);
297 PhysicsServer3D::BodyMode get_mode() const;
298
299 void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant);
300 Variant get_state(PhysicsServer3D::BodyState p_state) const;
301
302 _FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd = p_enable; }
303 _FORCE_INLINE_ bool is_continuous_collision_detection_enabled() const { return continuous_cd; }
304
305 void set_space(GodotSpace3D *p_space) override;
306
307 void update_mass_properties();
308 void reset_mass_properties();
309
310 _FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; }
311 _FORCE_INLINE_ const Vector3 &get_inv_inertia() const { return _inv_inertia; }
312 _FORCE_INLINE_ const Basis &get_inv_inertia_tensor() const { return _inv_inertia_tensor; }
313 _FORCE_INLINE_ real_t get_friction() const { return friction; }
314 _FORCE_INLINE_ real_t get_bounce() const { return bounce; }
315
316 void set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool lock);
317 bool is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const;
318
319 void integrate_forces(real_t p_step);
320 void integrate_velocities(real_t p_step);
321
322 _FORCE_INLINE_ Vector3 get_velocity_in_local_point(const Vector3 &rel_pos) const {
323 return linear_velocity + angular_velocity.cross(rel_pos - center_of_mass);
324 }
325
326 _FORCE_INLINE_ real_t compute_impulse_denominator(const Vector3 &p_pos, const Vector3 &p_normal) const {
327 Vector3 r0 = p_pos - get_transform().origin - center_of_mass;
328
329 Vector3 c0 = (r0).cross(p_normal);
330
331 Vector3 vec = (_inv_inertia_tensor.xform_inv(c0)).cross(r0);
332
333 return _inv_mass + p_normal.dot(vec);
334 }
335
336 _FORCE_INLINE_ real_t compute_angular_impulse_denominator(const Vector3 &p_axis) const {
337 return p_axis.dot(_inv_inertia_tensor.xform_inv(p_axis));
338 }
339
340 //void simulate_motion(const Transform3D& p_xform,real_t p_step);
341 void call_queries();
342 void wakeup_neighbours();
343
344 bool sleep_test(real_t p_step);
345
346 GodotBody3D();
347 ~GodotBody3D();
348};
349
350//add contact inline
351
352void GodotBody3D::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_local_velocity_at_pos, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos, const Vector3 &p_impulse) {
353 int c_max = contacts.size();
354
355 if (c_max == 0) {
356 return;
357 }
358
359 Contact *c = contacts.ptrw();
360
361 int idx = -1;
362
363 if (contact_count < c_max) {
364 idx = contact_count++;
365 } else {
366 real_t least_depth = 1e20;
367 int least_deep = -1;
368 for (int i = 0; i < c_max; i++) {
369 if (i == 0 || c[i].depth < least_depth) {
370 least_deep = i;
371 least_depth = c[i].depth;
372 }
373 }
374
375 if (least_deep >= 0 && least_depth < p_depth) {
376 idx = least_deep;
377 }
378 if (idx == -1) {
379 return; //none least deepe than this
380 }
381 }
382
383 c[idx].local_pos = p_local_pos;
384 c[idx].local_normal = p_local_normal;
385 c[idx].local_velocity_at_pos = p_local_velocity_at_pos;
386 c[idx].depth = p_depth;
387 c[idx].local_shape = p_local_shape;
388 c[idx].collider_pos = p_collider_pos;
389 c[idx].collider_shape = p_collider_shape;
390 c[idx].collider_instance_id = p_collider_instance_id;
391 c[idx].collider = p_collider;
392 c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos;
393 c[idx].impulse = p_impulse;
394}
395
396#endif // GODOT_BODY_3D_H
397