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