1 /*************************************************************************/
2 /* body_2d_sw.h */
3 /*************************************************************************/
4 /* This file is part of: */
5 /* GODOT ENGINE */
6 /* https://godotengine.org */
7 /*************************************************************************/
8 /* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */
9 /* Copyright (c) 2014-2019 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 #ifndef BODY_2D_SW_H
31 #define BODY_2D_SW_H
32
33 #include "area_2d_sw.h"
34 #include "collision_object_2d_sw.h"
35 #include "vset.h"
36
37 class Constraint2DSW;
38
39 class Body2DSW : public CollisionObject2DSW {
40
41 Physics2DServer::BodyMode mode;
42
43 Vector2 biased_linear_velocity;
44 real_t biased_angular_velocity;
45
46 Vector2 linear_velocity;
47 real_t angular_velocity;
48
49 real_t linear_damp;
50 real_t angular_damp;
51 real_t gravity_scale;
52
53 real_t mass;
54 real_t bounce;
55 real_t friction;
56
57 real_t _inv_mass;
58 real_t _inv_inertia;
59 bool user_inertia;
60
61 Vector2 gravity;
62 real_t area_linear_damp;
63 real_t area_angular_damp;
64
65 real_t still_time;
66
67 Vector2 applied_force;
68 real_t applied_torque;
69
70 Vector2 one_way_collision_direction;
71 float one_way_collision_max_depth;
72
73 SelfList<Body2DSW> active_list;
74 SelfList<Body2DSW> inertia_update_list;
75 SelfList<Body2DSW> direct_state_query_list;
76
77 VSet<RID> exceptions;
78 Physics2DServer::CCDMode continuous_cd_mode;
79 bool omit_force_integration;
80 bool active;
81 bool can_sleep;
82 bool first_time_kinematic;
83 bool first_integration;
84 bool using_one_way_cache;
85 void _update_inertia();
86 virtual void _shapes_changed();
87 Matrix32 new_transform;
88
89 Map<Constraint2DSW *, int> constraint_map;
90
91 struct AreaCMP {
92
93 Area2DSW *area;
94 int refCount;
95 _FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); }
96 _FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); }
AreaCMPAreaCMP97 _FORCE_INLINE_ AreaCMP() {}
AreaCMPAreaCMP98 _FORCE_INLINE_ AreaCMP(Area2DSW *p_area) {
99 area = p_area;
100 refCount = 1;
101 }
102 };
103
104 Vector<AreaCMP> areas;
105
106 struct Contact {
107
108 Vector2 local_pos;
109 Vector2 local_normal;
110 float depth;
111 int local_shape;
112 Vector2 collider_pos;
113 int collider_shape;
114 ObjectID collider_instance_id;
115 RID collider;
116 Vector2 collider_velocity_at_pos;
117 };
118
119 Vector<Contact> contacts; //no contacts by default
120 int contact_count;
121
122 struct ForceIntegrationCallback {
123
124 ObjectID id;
125 StringName method;
126 Variant callback_udata;
127 };
128
129 ForceIntegrationCallback *fi_callback;
130
131 uint64_t island_step;
132 Body2DSW *island_next;
133 Body2DSW *island_list_next;
134
135 _FORCE_INLINE_ void _compute_area_gravity_and_dampenings(const Area2DSW *p_area);
136
137 friend class Physics2DDirectBodyStateSW; // i give up, too many functions to expose
138
139 protected:
140 virtual void _shape_index_removed(int p_index);
141
142 public:
143 void set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant());
144
add_area(Area2DSW * p_area)145 _FORCE_INLINE_ void add_area(Area2DSW *p_area) {
146 int index = areas.find(AreaCMP(p_area));
147 if (index > -1) {
148 areas[index].refCount += 1;
149 } else {
150 areas.ordered_insert(AreaCMP(p_area));
151 }
152 }
153
remove_area(Area2DSW * p_area)154 _FORCE_INLINE_ void remove_area(Area2DSW *p_area) {
155 int index = areas.find(AreaCMP(p_area));
156 if (index > -1) {
157 areas[index].refCount -= 1;
158 if (areas[index].refCount < 1)
159 areas.remove(index);
160 }
161 }
162
set_max_contacts_reported(int p_size)163 _FORCE_INLINE_ void set_max_contacts_reported(int p_size) {
164 contacts.resize(p_size);
165 contact_count = 0;
166 if (mode == Physics2DServer::BODY_MODE_KINEMATIC && p_size) set_active(true);
167 }
168
get_max_contacts_reported()169 _FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); }
170
can_report_contacts()171 _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.empty(); }
172 _FORCE_INLINE_ void add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, float p_depth, int p_local_shape, 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);
173
add_exception(const RID & p_exception)174 _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); }
remove_exception(const RID & p_exception)175 _FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); }
has_exception(const RID & p_exception)176 _FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); }
get_exceptions()177 _FORCE_INLINE_ const VSet<RID> &get_exceptions() const { return exceptions; }
178
get_island_step()179 _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
set_island_step(uint64_t p_step)180 _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; }
181
get_island_next()182 _FORCE_INLINE_ Body2DSW *get_island_next() const { return island_next; }
set_island_next(Body2DSW * p_next)183 _FORCE_INLINE_ void set_island_next(Body2DSW *p_next) { island_next = p_next; }
184
get_island_list_next()185 _FORCE_INLINE_ Body2DSW *get_island_list_next() const { return island_list_next; }
set_island_list_next(Body2DSW * p_next)186 _FORCE_INLINE_ void set_island_list_next(Body2DSW *p_next) { island_list_next = p_next; }
187
add_constraint(Constraint2DSW * p_constraint,int p_pos)188 _FORCE_INLINE_ void add_constraint(Constraint2DSW *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; }
remove_constraint(Constraint2DSW * p_constraint)189 _FORCE_INLINE_ void remove_constraint(Constraint2DSW *p_constraint) { constraint_map.erase(p_constraint); }
get_constraint_map()190 const Map<Constraint2DSW *, int> &get_constraint_map() const { return constraint_map; }
clear_constraint_map()191 _FORCE_INLINE_ void clear_constraint_map() { constraint_map.clear(); }
192
set_omit_force_integration(bool p_omit_force_integration)193 _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; }
get_omit_force_integration()194 _FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; }
195
set_linear_velocity(const Vector2 & p_velocity)196 _FORCE_INLINE_ void set_linear_velocity(const Vector2 &p_velocity) { linear_velocity = p_velocity; }
get_linear_velocity()197 _FORCE_INLINE_ Vector2 get_linear_velocity() const { return linear_velocity; }
198
set_angular_velocity(real_t p_velocity)199 _FORCE_INLINE_ void set_angular_velocity(real_t p_velocity) { angular_velocity = p_velocity; }
get_angular_velocity()200 _FORCE_INLINE_ real_t get_angular_velocity() const { return angular_velocity; }
201
set_biased_linear_velocity(const Vector2 & p_velocity)202 _FORCE_INLINE_ void set_biased_linear_velocity(const Vector2 &p_velocity) { biased_linear_velocity = p_velocity; }
get_biased_linear_velocity()203 _FORCE_INLINE_ Vector2 get_biased_linear_velocity() const { return biased_linear_velocity; }
204
set_biased_angular_velocity(real_t p_velocity)205 _FORCE_INLINE_ void set_biased_angular_velocity(real_t p_velocity) { biased_angular_velocity = p_velocity; }
get_biased_angular_velocity()206 _FORCE_INLINE_ real_t get_biased_angular_velocity() const { return biased_angular_velocity; }
207
apply_impulse(const Vector2 & p_offset,const Vector2 & p_impulse)208 _FORCE_INLINE_ void apply_impulse(const Vector2 &p_offset, const Vector2 &p_impulse) {
209
210 linear_velocity += p_impulse * _inv_mass;
211 angular_velocity += _inv_inertia * p_offset.cross(p_impulse);
212 }
213
apply_bias_impulse(const Vector2 & p_pos,const Vector2 & p_j)214 _FORCE_INLINE_ void apply_bias_impulse(const Vector2 &p_pos, const Vector2 &p_j) {
215
216 biased_linear_velocity += p_j * _inv_mass;
217 biased_angular_velocity += _inv_inertia * p_pos.cross(p_j);
218 }
219
220 void set_active(bool p_active);
is_active()221 _FORCE_INLINE_ bool is_active() const { return active; }
222
wakeup()223 _FORCE_INLINE_ void wakeup() {
224 if ((!get_space()) || mode == Physics2DServer::BODY_MODE_STATIC || mode == Physics2DServer::BODY_MODE_KINEMATIC)
225 return;
226 set_active(true);
227 }
228
229 void set_param(Physics2DServer::BodyParameter p_param, float);
230 float get_param(Physics2DServer::BodyParameter p_param) const;
231
232 void set_mode(Physics2DServer::BodyMode p_mode);
233 Physics2DServer::BodyMode get_mode() const;
234
235 void set_state(Physics2DServer::BodyState p_state, const Variant &p_variant);
236 Variant get_state(Physics2DServer::BodyState p_state) const;
237
set_applied_force(const Vector2 & p_force)238 void set_applied_force(const Vector2 &p_force) { applied_force = p_force; }
get_applied_force()239 Vector2 get_applied_force() const { return applied_force; }
240
set_applied_torque(real_t p_torque)241 void set_applied_torque(real_t p_torque) { applied_torque = p_torque; }
get_applied_torque()242 real_t get_applied_torque() const { return applied_torque; }
243
add_force(const Vector2 & p_force,const Vector2 & p_offset)244 _FORCE_INLINE_ void add_force(const Vector2 &p_force, const Vector2 &p_offset) {
245
246 applied_force += p_force;
247 applied_torque += p_offset.cross(p_force);
248 }
249
set_continuous_collision_detection_mode(Physics2DServer::CCDMode p_mode)250 _FORCE_INLINE_ void set_continuous_collision_detection_mode(Physics2DServer::CCDMode p_mode) { continuous_cd_mode = p_mode; }
get_continuous_collision_detection_mode()251 _FORCE_INLINE_ Physics2DServer::CCDMode get_continuous_collision_detection_mode() const { return continuous_cd_mode; }
252
set_one_way_collision_direction(const Vector2 & p_dir)253 void set_one_way_collision_direction(const Vector2 &p_dir) {
254 one_way_collision_direction = p_dir;
255 using_one_way_cache = one_way_collision_direction != Vector2();
256 }
get_one_way_collision_direction()257 Vector2 get_one_way_collision_direction() const { return one_way_collision_direction; }
258
set_one_way_collision_max_depth(float p_depth)259 void set_one_way_collision_max_depth(float p_depth) { one_way_collision_max_depth = p_depth; }
get_one_way_collision_max_depth()260 float get_one_way_collision_max_depth() const { return one_way_collision_max_depth; }
261
is_using_one_way_collision()262 _FORCE_INLINE_ bool is_using_one_way_collision() const { return using_one_way_cache; }
263
264 void set_space(Space2DSW *p_space);
265
266 void update_inertias();
267
get_inv_mass()268 _FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; }
get_inv_inertia()269 _FORCE_INLINE_ real_t get_inv_inertia() const { return _inv_inertia; }
get_friction()270 _FORCE_INLINE_ real_t get_friction() const { return friction; }
get_gravity()271 _FORCE_INLINE_ Vector2 get_gravity() const { return gravity; }
get_bounce()272 _FORCE_INLINE_ real_t get_bounce() const { return bounce; }
get_linear_damp()273 _FORCE_INLINE_ real_t get_linear_damp() const { return linear_damp; }
get_angular_damp()274 _FORCE_INLINE_ real_t get_angular_damp() const { return angular_damp; }
275
276 void integrate_forces(real_t p_step);
277 void integrate_velocities(real_t p_step);
278
get_motion()279 _FORCE_INLINE_ Vector2 get_motion() const {
280
281 if (mode > Physics2DServer::BODY_MODE_KINEMATIC) {
282 return new_transform.get_origin() - get_transform().get_origin();
283 } else if (mode == Physics2DServer::BODY_MODE_KINEMATIC) {
284 return get_transform().get_origin() - new_transform.get_origin(); //kinematic simulates forward
285 }
286 return Vector2();
287 }
288
289 void call_queries();
290 void wakeup_neighbours();
291
292 bool sleep_test(real_t p_step);
293
294 Body2DSW();
295 ~Body2DSW();
296 };
297
298 //add contact inline
299
add_contact(const Vector2 & p_local_pos,const Vector2 & p_local_normal,float p_depth,int p_local_shape,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)300 void Body2DSW::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, float p_depth, int p_local_shape, 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) {
301
302 int c_max = contacts.size();
303
304 if (c_max == 0)
305 return;
306
307 Contact *c = &contacts[0];
308
309 int idx = -1;
310
311 if (contact_count < c_max) {
312 idx = contact_count++;
313 } else {
314
315 float least_depth = 1e20;
316 int least_deep = -1;
317 for (int i = 0; i < c_max; i++) {
318
319 if (i == 0 || c[i].depth < least_depth) {
320 least_deep = i;
321 least_depth = c[i].depth;
322 }
323 }
324
325 if (least_deep >= 0 && least_depth < p_depth) {
326
327 idx = least_deep;
328 }
329 if (idx == -1)
330 return; //none least deepe than this
331 }
332
333 c[idx].local_pos = p_local_pos;
334 c[idx].local_normal = p_local_normal;
335 c[idx].depth = p_depth;
336 c[idx].local_shape = p_local_shape;
337 c[idx].collider_pos = p_collider_pos;
338 c[idx].collider_shape = p_collider_shape;
339 c[idx].collider_instance_id = p_collider_instance_id;
340 c[idx].collider = p_collider;
341 c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos;
342 }
343
344 class Physics2DDirectBodyStateSW : public Physics2DDirectBodyState {
345
346 OBJ_TYPE(Physics2DDirectBodyStateSW, Physics2DDirectBodyState);
347
348 public:
349 static Physics2DDirectBodyStateSW *singleton;
350 Body2DSW *body;
351 real_t step;
352
get_total_gravity()353 virtual Vector2 get_total_gravity() const { return body->gravity; } // get gravity vector working on this body space/area
get_total_angular_damp()354 virtual float get_total_angular_damp() const { return body->area_angular_damp; } // get density of this body space/area
get_total_linear_damp()355 virtual float get_total_linear_damp() const { return body->area_linear_damp; } // get density of this body space/area
356
get_inverse_mass()357 virtual float get_inverse_mass() const { return body->get_inv_mass(); } // get the mass
get_inverse_inertia()358 virtual real_t get_inverse_inertia() const { return body->get_inv_inertia(); } // get density of this body space
359
set_linear_velocity(const Vector2 & p_velocity)360 virtual void set_linear_velocity(const Vector2 &p_velocity) { body->set_linear_velocity(p_velocity); }
get_linear_velocity()361 virtual Vector2 get_linear_velocity() const { return body->get_linear_velocity(); }
362
set_angular_velocity(real_t p_velocity)363 virtual void set_angular_velocity(real_t p_velocity) { body->set_angular_velocity(p_velocity); }
get_angular_velocity()364 virtual real_t get_angular_velocity() const { return body->get_angular_velocity(); }
365
set_transform(const Matrix32 & p_transform)366 virtual void set_transform(const Matrix32 &p_transform) { body->set_state(Physics2DServer::BODY_STATE_TRANSFORM, p_transform); }
get_transform()367 virtual Matrix32 get_transform() const { return body->get_transform(); }
368
set_sleep_state(bool p_enable)369 virtual void set_sleep_state(bool p_enable) { body->set_active(!p_enable); }
is_sleeping()370 virtual bool is_sleeping() const { return !body->is_active(); }
371
get_contact_count()372 virtual int get_contact_count() const { return body->contact_count; }
373
get_contact_local_pos(int p_contact_idx)374 virtual Vector2 get_contact_local_pos(int p_contact_idx) const {
375 ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
376 return body->contacts[p_contact_idx].local_pos;
377 }
get_contact_local_normal(int p_contact_idx)378 virtual Vector2 get_contact_local_normal(int p_contact_idx) const {
379 ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
380 return body->contacts[p_contact_idx].local_normal;
381 }
get_contact_local_shape(int p_contact_idx)382 virtual int get_contact_local_shape(int p_contact_idx) const {
383 ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1);
384 return body->contacts[p_contact_idx].local_shape;
385 }
386
get_contact_collider(int p_contact_idx)387 virtual RID get_contact_collider(int p_contact_idx) const {
388 ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID());
389 return body->contacts[p_contact_idx].collider;
390 }
get_contact_collider_pos(int p_contact_idx)391 virtual Vector2 get_contact_collider_pos(int p_contact_idx) const {
392 ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
393 return body->contacts[p_contact_idx].collider_pos;
394 }
get_contact_collider_id(int p_contact_idx)395 virtual ObjectID get_contact_collider_id(int p_contact_idx) const {
396 ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0);
397 return body->contacts[p_contact_idx].collider_instance_id;
398 }
get_contact_collider_shape(int p_contact_idx)399 virtual int get_contact_collider_shape(int p_contact_idx) const {
400 ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0);
401 return body->contacts[p_contact_idx].collider_shape;
402 }
403 virtual Variant get_contact_collider_shape_metadata(int p_contact_idx) const;
404
get_contact_collider_velocity_at_pos(int p_contact_idx)405 virtual Vector2 get_contact_collider_velocity_at_pos(int p_contact_idx) const {
406 ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
407 return body->contacts[p_contact_idx].collider_velocity_at_pos;
408 }
409
410 virtual Physics2DDirectSpaceState *get_space_state();
411
get_step()412 virtual real_t get_step() const { return step; }
Physics2DDirectBodyStateSW()413 Physics2DDirectBodyStateSW() {
414 singleton = this;
415 body = NULL;
416 }
417 };
418
419 #endif // BODY_2D_SW_H
420