seedlingattempt/godot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Move Godot Physics 2D into a module; add dummy 2D physics server

Browse Source 
If the module is enabled (default), 2D physics works as it did before.

If the module is disabled and no other 2D physics server is registered
(via a module or GDExtension), then we fall back to a dummy
implementation which effectively disables 2D physics functionality (and
a warning is printed).

The dummy 2D physics server can also be selected explicitly, in which
case no warning is printed.
This commit is contained in:
Ricardo Buring
2024-08-07 21:15:17 +02:00
parent 4254946de9
commit 7c4c4b9987
47 changed files with 497 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
servers/physics_2d/SCsub
View File

@ -1,5 +0,0 @@
#!/usr/bin/env python
Import("env")
env.add_source_files(env.servers_sources, "*.cpp")

314
servers/physics_2d/godot_area_2d.cpp
View File

@ -1,314 +0,0 @@
/**************************************************************************/
/* godot_area_2d.cpp */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#include "godot_area_2d.h"
#include "godot_body_2d.h"
#include "godot_space_2d.h"
GodotArea2D::BodyKey::BodyKey(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
rid = p_body->get_self();
instance_id = p_body->get_instance_id();
body_shape = p_body_shape;
area_shape = p_area_shape;
}
GodotArea2D::BodyKey::BodyKey(GodotArea2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
rid = p_body->get_self();
instance_id = p_body->get_instance_id();
body_shape = p_body_shape;
area_shape = p_area_shape;
}
void GodotArea2D::_shapes_changed() {
if (!moved_list.in_list() && get_space()) {
get_space()->area_add_to_moved_list(&moved_list);
}
}
void GodotArea2D::set_transform(const Transform2D &p_transform) {
if (!moved_list.in_list() && get_space()) {
get_space()->area_add_to_moved_list(&moved_list);
}
_set_transform(p_transform);
_set_inv_transform(p_transform.affine_inverse());
}
void GodotArea2D::set_space(GodotSpace2D *p_space) {
if (get_space()) {
if (monitor_query_list.in_list()) {
get_space()->area_remove_from_monitor_query_list(&monitor_query_list);
}
if (moved_list.in_list()) {
get_space()->area_remove_from_moved_list(&moved_list);
}
}
monitored_bodies.clear();
monitored_areas.clear();
_set_space(p_space);
}
void GodotArea2D::set_monitor_callback(const Callable &p_callback) {
_unregister_shapes();
monitor_callback = p_callback;
monitored_bodies.clear();
monitored_areas.clear();
_shape_changed();
if (!moved_list.in_list() && get_space()) {
get_space()->area_add_to_moved_list(&moved_list);
}
}
void GodotArea2D::set_area_monitor_callback(const Callable &p_callback) {
_unregister_shapes();
area_monitor_callback = p_callback;
monitored_bodies.clear();
monitored_areas.clear();
_shape_changed();
if (!moved_list.in_list() && get_space()) {
get_space()->area_add_to_moved_list(&moved_list);
}
}
void GodotArea2D::_set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode &r_mode, PhysicsServer2D::AreaSpaceOverrideMode p_new_mode) {
bool do_override = p_new_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED;
if (do_override == (r_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED)) {
return;
}
_unregister_shapes();
r_mode = p_new_mode;
_shape_changed();
}
void GodotArea2D::set_param(PhysicsServer2D::AreaParameter p_param, const Variant &p_value) {
switch (p_param) {
case PhysicsServer2D::AREA_PARAM_GRAVITY_OVERRIDE_MODE:
_set_space_override_mode(gravity_override_mode, (PhysicsServer2D::AreaSpaceOverrideMode)(int)p_value);
break;
case PhysicsServer2D::AREA_PARAM_GRAVITY:
gravity = p_value;
break;
case PhysicsServer2D::AREA_PARAM_GRAVITY_VECTOR:
gravity_vector = p_value;
break;
case PhysicsServer2D::AREA_PARAM_GRAVITY_IS_POINT:
gravity_is_point = p_value;
break;
case PhysicsServer2D::AREA_PARAM_GRAVITY_POINT_UNIT_DISTANCE:
gravity_point_unit_distance = p_value;
break;
case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE:
_set_space_override_mode(linear_damping_override_mode, (PhysicsServer2D::AreaSpaceOverrideMode)(int)p_value);
break;
case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP:
linear_damp = p_value;
break;
case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE:
_set_space_override_mode(angular_damping_override_mode, (PhysicsServer2D::AreaSpaceOverrideMode)(int)p_value);
break;
case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP:
angular_damp = p_value;
break;
case PhysicsServer2D::AREA_PARAM_PRIORITY:
priority = p_value;
break;
}
}
Variant GodotArea2D::get_param(PhysicsServer2D::AreaParameter p_param) const {
switch (p_param) {
case PhysicsServer2D::AREA_PARAM_GRAVITY_OVERRIDE_MODE:
return gravity_override_mode;
case PhysicsServer2D::AREA_PARAM_GRAVITY:
return gravity;
case PhysicsServer2D::AREA_PARAM_GRAVITY_VECTOR:
return gravity_vector;
case PhysicsServer2D::AREA_PARAM_GRAVITY_IS_POINT:
return gravity_is_point;
case PhysicsServer2D::AREA_PARAM_GRAVITY_POINT_UNIT_DISTANCE:
return gravity_point_unit_distance;
case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE:
return linear_damping_override_mode;
case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP:
return linear_damp;
case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE:
return angular_damping_override_mode;
case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP:
return angular_damp;
case PhysicsServer2D::AREA_PARAM_PRIORITY:
return priority;
}
return Variant();
}
void GodotArea2D::_queue_monitor_update() {
ERR_FAIL_NULL(get_space());
if (!monitor_query_list.in_list()) {
get_space()->area_add_to_monitor_query_list(&monitor_query_list);
}
}
void GodotArea2D::set_monitorable(bool p_monitorable) {
if (monitorable == p_monitorable) {
return;
}
monitorable = p_monitorable;
_set_static(!monitorable);
_shapes_changed();
}
void GodotArea2D::call_queries() {
if (!monitor_callback.is_null() && !monitored_bodies.is_empty()) {
if (monitor_callback.is_valid()) {
Variant res[5];
Variant *resptr[5];
for (int i = 0; i < 5; i++) {
resptr[i] = &res[i];
}
for (HashMap<BodyKey, BodyState, BodyKey>::Iterator E = monitored_bodies.begin(); E;) {
if (E->value.state == 0) { // Nothing happened
HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E;
++next;
monitored_bodies.remove(E);
E = next;
continue;
}
res[0] = E->value.state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED;
res[1] = E->key.rid;
res[2] = E->key.instance_id;
res[3] = E->key.body_shape;
res[4] = E->key.area_shape;
HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E;
++next;
monitored_bodies.remove(E);
E = next;
Callable::CallError ce;
Variant ret;
monitor_callback.callp((const Variant **)resptr, 5, ret, ce);
if (ce.error != Callable::CallError::CALL_OK) {
ERR_PRINT_ONCE("Error calling event callback method " + Variant::get_callable_error_text(monitor_callback, (const Variant **)resptr, 5, ce));
}
}
} else {
monitored_bodies.clear();
monitor_callback = Callable();
}
}
if (!area_monitor_callback.is_null() && !monitored_areas.is_empty()) {
if (area_monitor_callback.is_valid()) {
Variant res[5];
Variant *resptr[5];
for (int i = 0; i < 5; i++) {
resptr[i] = &res[i];
}
for (HashMap<BodyKey, BodyState, BodyKey>::Iterator E = monitored_areas.begin(); E;) {
if (E->value.state == 0) { // Nothing happened
HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E;
++next;
monitored_areas.remove(E);
E = next;
continue;
}
res[0] = E->value.state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED;
res[1] = E->key.rid;
res[2] = E->key.instance_id;
res[3] = E->key.body_shape;
res[4] = E->key.area_shape;
HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E;
++next;
monitored_areas.remove(E);
E = next;
Callable::CallError ce;
Variant ret;
area_monitor_callback.callp((const Variant **)resptr, 5, ret, ce);
if (ce.error != Callable::CallError::CALL_OK) {
ERR_PRINT_ONCE("Error calling event callback method " + Variant::get_callable_error_text(area_monitor_callback, (const Variant **)resptr, 5, ce));
}
}
} else {
monitored_areas.clear();
area_monitor_callback = Callable();
}
}
}
void GodotArea2D::compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const {
if (is_gravity_point()) {
const real_t gr_unit_dist = get_gravity_point_unit_distance();
Vector2 v = get_transform().xform(get_gravity_vector()) - p_position;
if (gr_unit_dist > 0) {
const real_t v_length_sq = v.length_squared();
if (v_length_sq > 0) {
const real_t gravity_strength = get_gravity() * gr_unit_dist * gr_unit_dist / v_length_sq;
r_gravity = v.normalized() * gravity_strength;
} else {
r_gravity = Vector2();
}
} else {
r_gravity = v.normalized() * get_gravity();
}
} else {
r_gravity = get_gravity_vector() * get_gravity();
}
}
GodotArea2D::GodotArea2D() :
GodotCollisionObject2D(TYPE_AREA),
monitor_query_list(this),
moved_list(this) {
_set_static(true); //areas are not active by default
}
GodotArea2D::~GodotArea2D() {
}

191
servers/physics_2d/godot_area_2d.h
View File

@ -1,191 +0,0 @@
/**************************************************************************/
/* godot_area_2d.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#ifndef GODOT_AREA_2D_H
#define GODOT_AREA_2D_H
#include "godot_collision_object_2d.h"
#include "core/templates/self_list.h"
#include "servers/physics_server_2d.h"
class GodotSpace2D;
class GodotBody2D;
class GodotConstraint2D;
class GodotArea2D : public GodotCollisionObject2D {
PhysicsServer2D::AreaSpaceOverrideMode gravity_override_mode = PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED;
PhysicsServer2D::AreaSpaceOverrideMode linear_damping_override_mode = PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED;
PhysicsServer2D::AreaSpaceOverrideMode angular_damping_override_mode = PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED;
real_t gravity = 9.80665;
Vector2 gravity_vector = Vector2(0, -1);
bool gravity_is_point = false;
real_t gravity_point_unit_distance = 0.0;
real_t linear_damp = 0.1;
real_t angular_damp = 1.0;
int priority = 0;
bool monitorable = false;
Callable monitor_callback;
Callable area_monitor_callback;
SelfList<GodotArea2D> monitor_query_list;
SelfList<GodotArea2D> moved_list;
struct BodyKey {
RID rid;
ObjectID instance_id;
uint32_t body_shape = 0;
uint32_t area_shape = 0;
static uint32_t hash(const BodyKey &p_key) {
uint32_t h = hash_one_uint64(p_key.rid.get_id());
h = hash_murmur3_one_64(p_key.instance_id, h);
h = hash_murmur3_one_32(p_key.area_shape, h);
return hash_fmix32(hash_murmur3_one_32(p_key.body_shape, h));
}
_FORCE_INLINE_ bool operator==(const BodyKey &p_key) const {
return rid == p_key.rid && instance_id == p_key.instance_id && body_shape == p_key.body_shape && area_shape == p_key.area_shape;
}
_FORCE_INLINE_ BodyKey() {}
BodyKey(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
BodyKey(GodotArea2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
};
struct BodyState {
int state = 0;
_FORCE_INLINE_ void inc() { state++; }
_FORCE_INLINE_ void dec() { state--; }
};
HashMap<BodyKey, BodyState, BodyKey> monitored_bodies;
HashMap<BodyKey, BodyState, BodyKey> monitored_areas;
HashSet<GodotConstraint2D *> constraints;
virtual void _shapes_changed() override;
void _queue_monitor_update();
void _set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode &r_mode, PhysicsServer2D::AreaSpaceOverrideMode p_new_mode);
public:
void set_monitor_callback(const Callable &p_callback);
_FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback.is_valid(); }
void set_area_monitor_callback(const Callable &p_callback);
_FORCE_INLINE_ bool has_area_monitor_callback() const { return area_monitor_callback.is_valid(); }
_FORCE_INLINE_ void add_body_to_query(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
_FORCE_INLINE_ void remove_body_from_query(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
_FORCE_INLINE_ void add_area_to_query(GodotArea2D *p_area, uint32_t p_area_shape, uint32_t p_self_shape);
_FORCE_INLINE_ void remove_area_from_query(GodotArea2D *p_area, uint32_t p_area_shape, uint32_t p_self_shape);
void set_param(PhysicsServer2D::AreaParameter p_param, const Variant &p_value);
Variant get_param(PhysicsServer2D::AreaParameter p_param) const;
_FORCE_INLINE_ void set_gravity(real_t p_gravity) { gravity = p_gravity; }
_FORCE_INLINE_ real_t get_gravity() const { return gravity; }
_FORCE_INLINE_ void set_gravity_vector(const Vector2 &p_gravity) { gravity_vector = p_gravity; }
_FORCE_INLINE_ Vector2 get_gravity_vector() const { return gravity_vector; }
_FORCE_INLINE_ void set_gravity_as_point(bool p_enable) { gravity_is_point = p_enable; }
_FORCE_INLINE_ bool is_gravity_point() const { return gravity_is_point; }
_FORCE_INLINE_ void set_gravity_point_unit_distance(real_t scale) { gravity_point_unit_distance = scale; }
_FORCE_INLINE_ real_t get_gravity_point_unit_distance() const { return gravity_point_unit_distance; }
_FORCE_INLINE_ void set_linear_damp(real_t p_linear_damp) { linear_damp = p_linear_damp; }
_FORCE_INLINE_ real_t get_linear_damp() const { return linear_damp; }
_FORCE_INLINE_ void set_angular_damp(real_t p_angular_damp) { angular_damp = p_angular_damp; }
_FORCE_INLINE_ real_t get_angular_damp() const { return angular_damp; }
_FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; }
_FORCE_INLINE_ int get_priority() const { return priority; }
_FORCE_INLINE_ void add_constraint(GodotConstraint2D *p_constraint) { constraints.insert(p_constraint); }
_FORCE_INLINE_ void remove_constraint(GodotConstraint2D *p_constraint) { constraints.erase(p_constraint); }
_FORCE_INLINE_ const HashSet<GodotConstraint2D *> &get_constraints() const { return constraints; }
_FORCE_INLINE_ void clear_constraints() { constraints.clear(); }
void set_monitorable(bool p_monitorable);
_FORCE_INLINE_ bool is_monitorable() const { return monitorable; }
void set_transform(const Transform2D &p_transform);
void set_space(GodotSpace2D *p_space) override;
void call_queries();
void compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const;
GodotArea2D();
~GodotArea2D();
};
void GodotArea2D::add_body_to_query(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
BodyKey bk(p_body, p_body_shape, p_area_shape);
monitored_bodies[bk].inc();
if (!monitor_query_list.in_list()) {
_queue_monitor_update();
}
}
void GodotArea2D::remove_body_from_query(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
BodyKey bk(p_body, p_body_shape, p_area_shape);
monitored_bodies[bk].dec();
if (get_space() && !monitor_query_list.in_list()) {
_queue_monitor_update();
}
}
void GodotArea2D::add_area_to_query(GodotArea2D *p_area, uint32_t p_area_shape, uint32_t p_self_shape) {
BodyKey bk(p_area, p_area_shape, p_self_shape);
monitored_areas[bk].inc();
if (!monitor_query_list.in_list()) {
_queue_monitor_update();
}
}
void GodotArea2D::remove_area_from_query(GodotArea2D *p_area, uint32_t p_area_shape, uint32_t p_self_shape) {
BodyKey bk(p_area, p_area_shape, p_self_shape);
monitored_areas[bk].dec();
if (get_space() && !monitor_query_list.in_list()) {
_queue_monitor_update();
}
}
#endif // GODOT_AREA_2D_H

203
servers/physics_2d/godot_area_pair_2d.cpp
View File

@ -1,203 +0,0 @@
/**************************************************************************/
/* godot_area_pair_2d.cpp */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#include "godot_area_pair_2d.h"
#include "godot_collision_solver_2d.h"
bool GodotAreaPair2D::setup(real_t p_step) {
bool result = false;
if (area->collides_with(body) && GodotCollisionSolver2D::solve(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), Vector2(), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), Vector2(), nullptr, this)) {
result = true;
}
process_collision = false;
has_space_override = false;
if (result != colliding) {
if ((int)area->get_param(PhysicsServer2D::AREA_PARAM_GRAVITY_OVERRIDE_MODE) != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
has_space_override = true;
} else if ((int)area->get_param(PhysicsServer2D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE) != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
has_space_override = true;
} else if ((int)area->get_param(PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE) != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
has_space_override = true;
}
process_collision = has_space_override;
if (area->has_monitor_callback()) {
process_collision = true;
}
colliding = result;
}
return process_collision;
}
bool GodotAreaPair2D::pre_solve(real_t p_step) {
if (!process_collision) {
return false;
}
if (colliding) {
if (has_space_override) {
body_has_attached_area = true;
body->add_area(area);
}
if (area->has_monitor_callback()) {
area->add_body_to_query(body, body_shape, area_shape);
}
} else {
if (has_space_override) {
body_has_attached_area = false;
body->remove_area(area);
}
if (area->has_monitor_callback()) {
area->remove_body_from_query(body, body_shape, area_shape);
}
}
return false; // Never do any post solving.
}
void GodotAreaPair2D::solve(real_t p_step) {
// Nothing to do.
}
GodotAreaPair2D::GodotAreaPair2D(GodotBody2D *p_body, int p_body_shape, GodotArea2D *p_area, int p_area_shape) {
body = p_body;
area = p_area;
body_shape = p_body_shape;
area_shape = p_area_shape;
body->add_constraint(this, 0);
area->add_constraint(this);
if (p_body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { //need to be active to process pair
p_body->set_active(true);
}
}
GodotAreaPair2D::~GodotAreaPair2D() {
if (colliding) {
if (body_has_attached_area) {
body_has_attached_area = false;
body->remove_area(area);
}
if (area->has_monitor_callback()) {
area->remove_body_from_query(body, body_shape, area_shape);
}
}
body->remove_constraint(this, 0);
area->remove_constraint(this);
}
//////////////////////////////////
bool GodotArea2Pair2D::setup(real_t p_step) {
bool result_a = area_a->collides_with(area_b);
bool result_b = area_b->collides_with(area_a);
if ((result_a || result_b) && !GodotCollisionSolver2D::solve(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), Vector2(), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), Vector2(), nullptr, this)) {
result_a = false;
result_b = false;
}
bool process_collision = false;
process_collision_a = false;
if (result_a != colliding_a) {
if (area_a->has_area_monitor_callback() && area_b_monitorable) {
process_collision_a = true;
process_collision = true;
}
colliding_a = result_a;
}
process_collision_b = false;
if (result_b != colliding_b) {
if (area_b->has_area_monitor_callback() && area_a_monitorable) {
process_collision_b = true;
process_collision = true;
}
colliding_b = result_b;
}
return process_collision;
}
bool GodotArea2Pair2D::pre_solve(real_t p_step) {
if (process_collision_a) {
if (colliding_a) {
area_a->add_area_to_query(area_b, shape_b, shape_a);
} else {
area_a->remove_area_from_query(area_b, shape_b, shape_a);
}
}
if (process_collision_b) {
if (colliding_b) {
area_b->add_area_to_query(area_a, shape_a, shape_b);
} else {
area_b->remove_area_from_query(area_a, shape_a, shape_b);
}
}
return false; // Never do any post solving.
}
void GodotArea2Pair2D::solve(real_t p_step) {
// Nothing to do.
}
GodotArea2Pair2D::GodotArea2Pair2D(GodotArea2D *p_area_a, int p_shape_a, GodotArea2D *p_area_b, int p_shape_b) {
area_a = p_area_a;
area_b = p_area_b;
shape_a = p_shape_a;
shape_b = p_shape_b;
area_a_monitorable = area_a->is_monitorable();
area_b_monitorable = area_b->is_monitorable();
area_a->add_constraint(this);
area_b->add_constraint(this);
}
GodotArea2Pair2D::~GodotArea2Pair2D() {
if (colliding_a) {
if (area_a->has_area_monitor_callback() && area_b_monitorable) {
area_a->remove_area_from_query(area_b, shape_b, shape_a);
}
}
if (colliding_b) {
if (area_b->has_area_monitor_callback() && area_a_monitorable) {
area_b->remove_area_from_query(area_a, shape_a, shape_b);
}
}
area_a->remove_constraint(this);
area_b->remove_constraint(this);
}

78
servers/physics_2d/godot_area_pair_2d.h
View File

@ -1,78 +0,0 @@
/**************************************************************************/
/* godot_area_pair_2d.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#ifndef GODOT_AREA_PAIR_2D_H
#define GODOT_AREA_PAIR_2D_H
#include "godot_area_2d.h"
#include "godot_body_2d.h"
#include "godot_constraint_2d.h"
class GodotAreaPair2D : public GodotConstraint2D {
GodotBody2D *body = nullptr;
GodotArea2D *area = nullptr;
int body_shape = 0;
int area_shape = 0;
bool colliding = false;
bool has_space_override = false;
bool process_collision = false;
bool body_has_attached_area = false;
public:
virtual bool setup(real_t p_step) override;
virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
GodotAreaPair2D(GodotBody2D *p_body, int p_body_shape, GodotArea2D *p_area, int p_area_shape);
~GodotAreaPair2D();
};
class GodotArea2Pair2D : public GodotConstraint2D {
GodotArea2D *area_a = nullptr;
GodotArea2D *area_b = nullptr;
int shape_a = 0;
int shape_b = 0;
bool colliding_a = false;
bool colliding_b = false;
bool process_collision_a = false;
bool process_collision_b = false;
bool area_a_monitorable;
bool area_b_monitorable;
public:
virtual bool setup(real_t p_step) override;
virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
GodotArea2Pair2D(GodotArea2D *p_area_a, int p_shape_a, GodotArea2D *p_area_b, int p_shape_b);
~GodotArea2Pair2D();
};
#endif // GODOT_AREA_PAIR_2D_H

762
servers/physics_2d/godot_body_2d.cpp
View File

@ -1,762 +0,0 @@
/**************************************************************************/
/* godot_body_2d.cpp */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#include "godot_body_2d.h"
#include "godot_area_2d.h"
#include "godot_body_direct_state_2d.h"
#include "godot_space_2d.h"
void GodotBody2D::_mass_properties_changed() {
if (get_space() && !mass_properties_update_list.in_list()) {
get_space()->body_add_to_mass_properties_update_list(&mass_properties_update_list);
}
}
void GodotBody2D::update_mass_properties() {
//update shapes and motions
switch (mode) {
case PhysicsServer2D::BODY_MODE_RIGID: {
real_t total_area = 0;
for (int i = 0; i < get_shape_count(); i++) {
if (is_shape_disabled(i)) {
continue;
}
total_area += get_shape_aabb(i).get_area();
}
if (calculate_center_of_mass) {
// We have to recompute the center of mass.
center_of_mass_local = Vector2();
if (total_area != 0.0) {
for (int i = 0; i < get_shape_count(); i++) {
if (is_shape_disabled(i)) {
continue;
}
real_t area = get_shape_aabb(i).get_area();
real_t mass_new = area * mass / total_area;
// NOTE: we assume that the shape origin is also its center of mass.
center_of_mass_local += mass_new * get_shape_transform(i).get_origin();
}
center_of_mass_local /= mass;
}
}
if (calculate_inertia) {
inertia = 0;
for (int i = 0; i < get_shape_count(); i++) {
if (is_shape_disabled(i)) {
continue;
}
const GodotShape2D *shape = get_shape(i);
real_t area = get_shape_aabb(i).get_area();
if (area == 0.0) {
continue;
}
real_t mass_new = area * mass / total_area;
Transform2D mtx = get_shape_transform(i);
Vector2 scale = mtx.get_scale();
Vector2 shape_origin = mtx.get_origin() - center_of_mass_local;
inertia += shape->get_moment_of_inertia(mass_new, scale) + mass_new * shape_origin.length_squared();
}
}
_inv_inertia = inertia > 0.0 ? (1.0 / inertia) : 0.0;
if (mass) {
_inv_mass = 1.0 / mass;
} else {
_inv_mass = 0;
}
} break;
case PhysicsServer2D::BODY_MODE_KINEMATIC:
case PhysicsServer2D::BODY_MODE_STATIC: {
_inv_inertia = 0;
_inv_mass = 0;
} break;
case PhysicsServer2D::BODY_MODE_RIGID_LINEAR: {
_inv_inertia = 0;
_inv_mass = 1.0 / mass;
} break;
}
_update_transform_dependent();
}
void GodotBody2D::reset_mass_properties() {
calculate_inertia = true;
calculate_center_of_mass = true;
_mass_properties_changed();
}
void GodotBody2D::set_active(bool p_active) {
if (active == p_active) {
return;
}
active = p_active;
if (active) {
if (mode == PhysicsServer2D::BODY_MODE_STATIC) {
// Static bodies can't be active.
active = false;
} else if (get_space()) {
get_space()->body_add_to_active_list(&active_list);
}
} else if (get_space()) {
get_space()->body_remove_from_active_list(&active_list);
}
}
void GodotBody2D::set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value) {
switch (p_param) {
case PhysicsServer2D::BODY_PARAM_BOUNCE: {
bounce = p_value;
} break;
case PhysicsServer2D::BODY_PARAM_FRICTION: {
friction = p_value;
} break;
case PhysicsServer2D::BODY_PARAM_MASS: {
real_t mass_value = p_value;
ERR_FAIL_COND(mass_value <= 0);
mass = mass_value;
if (mode >= PhysicsServer2D::BODY_MODE_RIGID) {
_mass_properties_changed();
}
} break;
case PhysicsServer2D::BODY_PARAM_INERTIA: {
real_t inertia_value = p_value;
if (inertia_value <= 0.0) {
calculate_inertia = true;
if (mode == PhysicsServer2D::BODY_MODE_RIGID) {
_mass_properties_changed();
}
} else {
calculate_inertia = false;
inertia = inertia_value;
if (mode == PhysicsServer2D::BODY_MODE_RIGID) {
_inv_inertia = 1.0 / inertia;
}
}
} break;
case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: {
calculate_center_of_mass = false;
center_of_mass_local = p_value;
_update_transform_dependent();
} break;
case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: {
if (Math::is_zero_approx(gravity_scale)) {
wakeup();
}
gravity_scale = p_value;
} break;
case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP_MODE: {
int mode_value = p_value;
linear_damp_mode = (PhysicsServer2D::BodyDampMode)mode_value;
} break;
case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP_MODE: {
int mode_value = p_value;
angular_damp_mode = (PhysicsServer2D::BodyDampMode)mode_value;
} break;
case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP: {
linear_damp = p_value;
} break;
case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP: {
angular_damp = p_value;
} break;
default: {
}
}
}
Variant GodotBody2D::get_param(PhysicsServer2D::BodyParameter p_param) const {
switch (p_param) {
case PhysicsServer2D::BODY_PARAM_BOUNCE: {
return bounce;
}
case PhysicsServer2D::BODY_PARAM_FRICTION: {
return friction;
}
case PhysicsServer2D::BODY_PARAM_MASS: {
return mass;
}
case PhysicsServer2D::BODY_PARAM_INERTIA: {
return inertia;
}
case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: {
return center_of_mass_local;
}
case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: {
return gravity_scale;
}
case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP_MODE: {
return linear_damp_mode;
}
case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP_MODE: {
return angular_damp_mode;
}
case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP: {
return linear_damp;
}
case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP: {
return angular_damp;
}
default: {
}
}
return 0;
}
void GodotBody2D::set_mode(PhysicsServer2D::BodyMode p_mode) {
PhysicsServer2D::BodyMode prev = mode;
mode = p_mode;
switch (p_mode) {
//CLEAR UP EVERYTHING IN CASE IT NOT WORKS!
case PhysicsServer2D::BODY_MODE_STATIC:
case PhysicsServer2D::BODY_MODE_KINEMATIC: {
_set_inv_transform(get_transform().affine_inverse());
_inv_mass = 0;
_inv_inertia = 0;
_set_static(p_mode == PhysicsServer2D::BODY_MODE_STATIC);
set_active(p_mode == PhysicsServer2D::BODY_MODE_KINEMATIC && contacts.size());
linear_velocity = Vector2();
angular_velocity = 0;
if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC && prev != mode) {
first_time_kinematic = true;
}
} break;
case PhysicsServer2D::BODY_MODE_RIGID: {
_inv_mass = mass > 0 ? (1.0 / mass) : 0;
if (!calculate_inertia) {
_inv_inertia = 1.0 / inertia;
}
_mass_properties_changed();
_set_static(false);
set_active(true);
} break;
case PhysicsServer2D::BODY_MODE_RIGID_LINEAR: {
_inv_mass = mass > 0 ? (1.0 / mass) : 0;
_inv_inertia = 0;
angular_velocity = 0;
_set_static(false);
set_active(true);
}
}
}
PhysicsServer2D::BodyMode GodotBody2D::get_mode() const {
return mode;
}
void GodotBody2D::_shapes_changed() {
_mass_properties_changed();
wakeup();
wakeup_neighbours();
}
void GodotBody2D::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_variant) {
switch (p_state) {
case PhysicsServer2D::BODY_STATE_TRANSFORM: {
if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) {
new_transform = p_variant;
//wakeup_neighbours();
set_active(true);
if (first_time_kinematic) {
_set_transform(p_variant);
_set_inv_transform(get_transform().affine_inverse());
first_time_kinematic = false;
}
} else if (mode == PhysicsServer2D::BODY_MODE_STATIC) {
_set_transform(p_variant);
_set_inv_transform(get_transform().affine_inverse());
wakeup_neighbours();
} else {
Transform2D t = p_variant;
t.orthonormalize();
new_transform = get_transform(); //used as old to compute motion
if (t == new_transform) {
break;
}
_set_transform(t);
_set_inv_transform(get_transform().inverse());
_update_transform_dependent();
}
wakeup();
} break;
case PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY: {
linear_velocity = p_variant;
constant_linear_velocity = linear_velocity;
wakeup();
} break;
case PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY: {
angular_velocity = p_variant;
constant_angular_velocity = angular_velocity;
wakeup();
} break;
case PhysicsServer2D::BODY_STATE_SLEEPING: {
if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) {
break;
}
bool do_sleep = p_variant;
if (do_sleep) {
linear_velocity = Vector2();
//biased_linear_velocity=Vector3();
angular_velocity = 0;
//biased_angular_velocity=Vector3();
set_active(false);
} else {
if (mode != PhysicsServer2D::BODY_MODE_STATIC) {
set_active(true);
}
}
} break;
case PhysicsServer2D::BODY_STATE_CAN_SLEEP: {
can_sleep = p_variant;
if (mode >= PhysicsServer2D::BODY_MODE_RIGID && !active && !can_sleep) {
set_active(true);
}
} break;
}
}
Variant GodotBody2D::get_state(PhysicsServer2D::BodyState p_state) const {
switch (p_state) {
case PhysicsServer2D::BODY_STATE_TRANSFORM: {
return get_transform();
}
case PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY: {
return linear_velocity;
}
case PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY: {
return angular_velocity;
}
case PhysicsServer2D::BODY_STATE_SLEEPING: {
return !is_active();
}
case PhysicsServer2D::BODY_STATE_CAN_SLEEP: {
return can_sleep;
}
}
return Variant();
}
void GodotBody2D::set_space(GodotSpace2D *p_space) {
if (get_space()) {
wakeup_neighbours();
if (mass_properties_update_list.in_list()) {
get_space()->body_remove_from_mass_properties_update_list(&mass_properties_update_list);
}
if (active_list.in_list()) {
get_space()->body_remove_from_active_list(&active_list);
}
if (direct_state_query_list.in_list()) {
get_space()->body_remove_from_state_query_list(&direct_state_query_list);
}
}
_set_space(p_space);
if (get_space()) {
_mass_properties_changed();
if (active && !active_list.in_list()) {
get_space()->body_add_to_active_list(&active_list);
}
}
}
void GodotBody2D::_update_transform_dependent() {
center_of_mass = get_transform().basis_xform(center_of_mass_local);
}
void GodotBody2D::integrate_forces(real_t p_step) {
if (mode == PhysicsServer2D::BODY_MODE_STATIC) {
return;
}
ERR_FAIL_NULL(get_space());
int ac = areas.size();
bool gravity_done = false;
bool linear_damp_done = false;
bool angular_damp_done = false;
bool stopped = false;
gravity = Vector2(0, 0);
total_linear_damp = 0.0;
total_angular_damp = 0.0;
// Combine gravity and damping from overlapping areas in priority order.
if (ac) {
areas.sort();
const AreaCMP *aa = &areas[0];
for (int i = ac - 1; i >= 0 && !stopped; i--) {
if (!gravity_done) {
PhysicsServer2D::AreaSpaceOverrideMode area_gravity_mode = (PhysicsServer2D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer2D::AREA_PARAM_GRAVITY_OVERRIDE_MODE);
if (area_gravity_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
Vector2 area_gravity;
aa[i].area->compute_gravity(get_transform().get_origin(), area_gravity);
switch (area_gravity_mode) {
case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE:
case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
gravity += area_gravity;
gravity_done = area_gravity_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
} break;
case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE:
case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
gravity = area_gravity;
gravity_done = area_gravity_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE;
} break;
default: {
}
}
}
}
if (!linear_damp_done) {
PhysicsServer2D::AreaSpaceOverrideMode area_linear_damp_mode = (PhysicsServer2D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer2D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE);
if (area_linear_damp_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
real_t area_linear_damp = aa[i].area->get_linear_damp();
switch (area_linear_damp_mode) {
case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE:
case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
total_linear_damp += area_linear_damp;
linear_damp_done = area_linear_damp_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
} break;
case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE:
case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
total_linear_damp = area_linear_damp;
linear_damp_done = area_linear_damp_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE;
} break;
default: {
}
}
}
}
if (!angular_damp_done) {
PhysicsServer2D::AreaSpaceOverrideMode area_angular_damp_mode = (PhysicsServer2D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE);
if (area_angular_damp_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
real_t area_angular_damp = aa[i].area->get_angular_damp();
switch (area_angular_damp_mode) {
case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE:
case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
total_angular_damp += area_angular_damp;
angular_damp_done = area_angular_damp_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
} break;
case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE:
case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
total_angular_damp = area_angular_damp;
angular_damp_done = area_angular_damp_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE;
} break;
default: {
}
}
}
}
stopped = gravity_done && linear_damp_done && angular_damp_done;
}
}
// Add default gravity and damping from space area.
if (!stopped) {
GodotArea2D *default_area = get_space()->get_default_area();
ERR_FAIL_NULL(default_area);
if (!gravity_done) {
Vector2 default_gravity;
default_area->compute_gravity(get_transform().get_origin(), default_gravity);
gravity += default_gravity;
}
if (!linear_damp_done) {
total_linear_damp += default_area->get_linear_damp();
}
if (!angular_damp_done) {
total_angular_damp += default_area->get_angular_damp();
}
}
// Override linear damping with body's value.
switch (linear_damp_mode) {
case PhysicsServer2D::BODY_DAMP_MODE_COMBINE: {
total_linear_damp += linear_damp;
} break;
case PhysicsServer2D::BODY_DAMP_MODE_REPLACE: {
total_linear_damp = linear_damp;
} break;
}
// Override angular damping with body's value.
switch (angular_damp_mode) {
case PhysicsServer2D::BODY_DAMP_MODE_COMBINE: {
total_angular_damp += angular_damp;
} break;
case PhysicsServer2D::BODY_DAMP_MODE_REPLACE: {
total_angular_damp = angular_damp;
} break;
}
gravity *= gravity_scale;
prev_linear_velocity = linear_velocity;
prev_angular_velocity = angular_velocity;
Vector2 motion;
bool do_motion = false;
if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) {
//compute motion, angular and etc. velocities from prev transform
motion = new_transform.get_origin() - get_transform().get_origin();
linear_velocity = constant_linear_velocity + motion / p_step;
real_t rot = new_transform.get_rotation() - get_transform().get_rotation();
angular_velocity = constant_angular_velocity + remainder(rot, 2.0 * Math_PI) / p_step;
do_motion = true;
} else {
if (!omit_force_integration) {
//overridden by direct state query
Vector2 force = gravity * mass + applied_force + constant_force;
real_t torque = applied_torque + constant_torque;
real_t damp = 1.0 - p_step * total_linear_damp;
if (damp < 0) { // reached zero in the given time
damp = 0;
}
real_t angular_damp_new = 1.0 - p_step * total_angular_damp;
if (angular_damp_new < 0) { // reached zero in the given time
angular_damp_new = 0;
}
linear_velocity *= damp;
angular_velocity *= angular_damp_new;
linear_velocity += _inv_mass * force * p_step;
angular_velocity += _inv_inertia * torque * p_step;
}
if (continuous_cd_mode != PhysicsServer2D::CCD_MODE_DISABLED) {
motion = linear_velocity * p_step;
do_motion = true;
}
}
applied_force = Vector2();
applied_torque = 0.0;
biased_angular_velocity = 0.0;
biased_linear_velocity = Vector2();
if (do_motion) { //shapes temporarily extend for raycast
_update_shapes_with_motion(motion);
}
contact_count = 0;
}
void GodotBody2D::integrate_velocities(real_t p_step) {
if (mode == PhysicsServer2D::BODY_MODE_STATIC) {
return;
}
ERR_FAIL_NULL(get_space());
if (fi_callback_data || body_state_callback.is_valid()) {
get_space()->body_add_to_state_query_list(&direct_state_query_list);
}
if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) {
_set_transform(new_transform, false);
_set_inv_transform(new_transform.affine_inverse());
if (contacts.size() == 0 && linear_velocity == Vector2() && angular_velocity == 0) {
set_active(false); //stopped moving, deactivate
}
return;
}
real_t total_angular_velocity = angular_velocity + biased_angular_velocity;
Vector2 total_linear_velocity = linear_velocity + biased_linear_velocity;
real_t angle_delta = total_angular_velocity * p_step;
real_t angle = get_transform().get_rotation() + angle_delta;
Vector2 pos = get_transform().get_origin() + total_linear_velocity * p_step;
if (center_of_mass.length_squared() > CMP_EPSILON2) {
// Calculate displacement due to center of mass offset.
pos += center_of_mass - center_of_mass.rotated(angle_delta);
}
_set_transform(Transform2D(angle, pos), continuous_cd_mode == PhysicsServer2D::CCD_MODE_DISABLED);
_set_inv_transform(get_transform().inverse());
if (continuous_cd_mode != PhysicsServer2D::CCD_MODE_DISABLED) {
new_transform = get_transform();
}
_update_transform_dependent();
}
void GodotBody2D::wakeup_neighbours() {
for (const Pair<GodotConstraint2D *, int> &E : constraint_list) {
const GodotConstraint2D *c = E.first;
GodotBody2D **n = c->get_body_ptr();
int bc = c->get_body_count();
for (int i = 0; i < bc; i++) {
if (i == E.second) {
continue;
}
GodotBody2D *b = n[i];
if (b->mode < PhysicsServer2D::BODY_MODE_RIGID) {
continue;
}
if (!b->is_active()) {
b->set_active(true);
}
}
}
}
void GodotBody2D::call_queries() {
Variant direct_state_variant = get_direct_state();
if (fi_callback_data) {
if (!fi_callback_data->callable.is_valid()) {
set_force_integration_callback(Callable());
} else {
const Variant *vp[2] = { &direct_state_variant, &fi_callback_data->udata };
Callable::CallError ce;
Variant rv;
if (fi_callback_data->udata.get_type() != Variant::NIL) {
fi_callback_data->callable.callp(vp, 2, rv, ce);
} else {
fi_callback_data->callable.callp(vp, 1, rv, ce);
}
}
}
if (body_state_callback.is_valid()) {
body_state_callback.call(direct_state_variant);
}
}
bool GodotBody2D::sleep_test(real_t p_step) {
if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) {
return true;
} else if (!can_sleep) {
return false;
}
ERR_FAIL_NULL_V(get_space(), true);
if (Math::abs(angular_velocity) < get_space()->get_body_angular_velocity_sleep_threshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_threshold() * get_space()->get_body_linear_velocity_sleep_threshold()) {
still_time += p_step;
return still_time > get_space()->get_body_time_to_sleep();
} else {
still_time = 0; //maybe this should be set to 0 on set_active?
return false;
}
}
void GodotBody2D::set_state_sync_callback(const Callable &p_callable) {
body_state_callback = p_callable;
}
void GodotBody2D::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) {
if (p_callable.is_valid()) {
if (!fi_callback_data) {
fi_callback_data = memnew(ForceIntegrationCallbackData);
}
fi_callback_data->callable = p_callable;
fi_callback_data->udata = p_udata;
} else if (fi_callback_data) {
memdelete(fi_callback_data);
fi_callback_data = nullptr;
}
}
GodotPhysicsDirectBodyState2D *GodotBody2D::get_direct_state() {
if (!direct_state) {
direct_state = memnew(GodotPhysicsDirectBodyState2D);
direct_state->body = this;
}
return direct_state;
}
GodotBody2D::GodotBody2D() :
GodotCollisionObject2D(TYPE_BODY),
active_list(this),
mass_properties_update_list(this),
direct_state_query_list(this) {
_set_static(false);
}
GodotBody2D::~GodotBody2D() {
if (fi_callback_data) {
memdelete(fi_callback_data);
}
if (direct_state) {
memdelete(direct_state);
}
}

389
servers/physics_2d/godot_body_2d.h
View File

@ -1,389 +0,0 @@
/**************************************************************************/
/* godot_body_2d.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#ifndef GODOT_BODY_2D_H
#define GODOT_BODY_2D_H
#include "godot_area_2d.h"
#include "godot_collision_object_2d.h"
#include "core/templates/list.h"
#include "core/templates/pair.h"
#include "core/templates/vset.h"
class GodotConstraint2D;
class GodotPhysicsDirectBodyState2D;
class GodotBody2D : public GodotCollisionObject2D {
PhysicsServer2D::BodyMode mode = PhysicsServer2D::BODY_MODE_RIGID;
Vector2 biased_linear_velocity;
real_t biased_angular_velocity = 0.0;
Vector2 linear_velocity;
real_t angular_velocity = 0.0;
Vector2 prev_linear_velocity;
real_t prev_angular_velocity = 0.0;
Vector2 constant_linear_velocity;
real_t constant_angular_velocity = 0.0;
PhysicsServer2D::BodyDampMode linear_damp_mode = PhysicsServer2D::BODY_DAMP_MODE_COMBINE;
PhysicsServer2D::BodyDampMode angular_damp_mode = PhysicsServer2D::BODY_DAMP_MODE_COMBINE;
real_t linear_damp = 0.0;
real_t angular_damp = 0.0;
real_t total_linear_damp = 0.0;
real_t total_angular_damp = 0.0;
real_t gravity_scale = 1.0;
real_t bounce = 0.0;
real_t friction = 1.0;
real_t mass = 1.0;
real_t _inv_mass = 1.0;
real_t inertia = 0.0;
real_t _inv_inertia = 0.0;
Vector2 center_of_mass_local;
Vector2 center_of_mass;
bool calculate_inertia = true;
bool calculate_center_of_mass = true;
Vector2 gravity;
real_t still_time = 0.0;
Vector2 applied_force;
real_t applied_torque = 0.0;
Vector2 constant_force;
real_t constant_torque = 0.0;
SelfList<GodotBody2D> active_list;
SelfList<GodotBody2D> mass_properties_update_list;
SelfList<GodotBody2D> direct_state_query_list;
VSet<RID> exceptions;
PhysicsServer2D::CCDMode continuous_cd_mode = PhysicsServer2D::CCD_MODE_DISABLED;
bool omit_force_integration = false;
bool active = true;
bool can_sleep = true;
bool first_time_kinematic = false;
void _mass_properties_changed();
virtual void _shapes_changed() override;
Transform2D new_transform;
List<Pair<GodotConstraint2D *, int>> constraint_list;
struct AreaCMP {
GodotArea2D *area = nullptr;
int refCount = 0;
_FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); }
_FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); }
_FORCE_INLINE_ AreaCMP() {}
_FORCE_INLINE_ AreaCMP(GodotArea2D *p_area) {
area = p_area;
refCount = 1;
}
};
Vector<AreaCMP> areas;
struct Contact {
Vector2 local_pos;
Vector2 local_normal;
Vector2 local_velocity_at_pos;
real_t depth = 0.0;
int local_shape = 0;
Vector2 collider_pos;
int collider_shape = 0;
ObjectID collider_instance_id;
RID collider;
Vector2 collider_velocity_at_pos;
Vector2 impulse;
};
Vector<Contact> contacts; //no contacts by default
int contact_count = 0;
Callable body_state_callback;
struct ForceIntegrationCallbackData {
Callable callable;
Variant udata;
};
ForceIntegrationCallbackData *fi_callback_data = nullptr;
GodotPhysicsDirectBodyState2D *direct_state = nullptr;
uint64_t island_step = 0;
void _update_transform_dependent();
friend class GodotPhysicsDirectBodyState2D; // i give up, too many functions to expose
public:
void set_state_sync_callback(const Callable &p_callable);
void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant());
GodotPhysicsDirectBodyState2D *get_direct_state();
_FORCE_INLINE_ void add_area(GodotArea2D *p_area) {
int index = areas.find(AreaCMP(p_area));
if (index > -1) {
areas.write[index].refCount += 1;
} else {
areas.ordered_insert(AreaCMP(p_area));
}
}
_FORCE_INLINE_ void remove_area(GodotArea2D *p_area) {
int index = areas.find(AreaCMP(p_area));
if (index > -1) {
areas.write[index].refCount -= 1;
if (areas[index].refCount < 1) {
areas.remove_at(index);
}
}
}
_FORCE_INLINE_ void set_max_contacts_reported(int p_size) {
contacts.resize(p_size);
contact_count = 0;
if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC && p_size) {
set_active(true);
}
}
_FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); }
_FORCE_INLINE_ bool can_report_contacts() const { return !contacts.is_empty(); }
_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);
_FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); }
_FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); }
_FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); }
_FORCE_INLINE_ const VSet<RID> &get_exceptions() const { return exceptions; }
_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; }
_FORCE_INLINE_ void add_constraint(GodotConstraint2D *p_constraint, int p_pos) { constraint_list.push_back({ p_constraint, p_pos }); }
_FORCE_INLINE_ void remove_constraint(GodotConstraint2D *p_constraint, int p_pos) { constraint_list.erase({ p_constraint, p_pos }); }
const List<Pair<GodotConstraint2D *, int>> &get_constraint_list() const { return constraint_list; }
_FORCE_INLINE_ void clear_constraint_list() { constraint_list.clear(); }
_FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; }
_FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; }
_FORCE_INLINE_ void set_linear_velocity(const Vector2 &p_velocity) { linear_velocity = p_velocity; }
_FORCE_INLINE_ Vector2 get_linear_velocity() const { return linear_velocity; }
_FORCE_INLINE_ void set_angular_velocity(real_t p_velocity) { angular_velocity = p_velocity; }
_FORCE_INLINE_ real_t get_angular_velocity() const { return angular_velocity; }
_FORCE_INLINE_ Vector2 get_prev_linear_velocity() const { return prev_linear_velocity; }
_FORCE_INLINE_ real_t get_prev_angular_velocity() const { return prev_angular_velocity; }
_FORCE_INLINE_ void set_biased_linear_velocity(const Vector2 &p_velocity) { biased_linear_velocity = p_velocity; }
_FORCE_INLINE_ Vector2 get_biased_linear_velocity() const { return biased_linear_velocity; }
_FORCE_INLINE_ void set_biased_angular_velocity(real_t p_velocity) { biased_angular_velocity = p_velocity; }
_FORCE_INLINE_ real_t get_biased_angular_velocity() const { return biased_angular_velocity; }
_FORCE_INLINE_ void apply_central_impulse(const Vector2 &p_impulse) {
linear_velocity += p_impulse * _inv_mass;
}
_FORCE_INLINE_ void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) {
linear_velocity += p_impulse * _inv_mass;
angular_velocity += _inv_inertia * (p_position - center_of_mass).cross(p_impulse);
}
_FORCE_INLINE_ void apply_torque_impulse(real_t p_torque) {
angular_velocity += _inv_inertia * p_torque;
}
_FORCE_INLINE_ void apply_bias_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2(), real_t p_max_delta_av = -1.0) {
biased_linear_velocity += p_impulse * _inv_mass;
if (p_max_delta_av != 0.0) {
real_t delta_av = _inv_inertia * (p_position - center_of_mass).cross(p_impulse);
if (p_max_delta_av > 0 && delta_av > p_max_delta_av) {
delta_av = p_max_delta_av;
}
biased_angular_velocity += delta_av;
}
}
_FORCE_INLINE_ void apply_central_force(const Vector2 &p_force) {
applied_force += p_force;
}
_FORCE_INLINE_ void apply_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) {
applied_force += p_force;
applied_torque += (p_position - center_of_mass).cross(p_force);
}
_FORCE_INLINE_ void apply_torque(real_t p_torque) {
applied_torque += p_torque;
}
_FORCE_INLINE_ void add_constant_central_force(const Vector2 &p_force) {
constant_force += p_force;
}
_FORCE_INLINE_ void add_constant_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) {
constant_force += p_force;
constant_torque += (p_position - center_of_mass).cross(p_force);
}
_FORCE_INLINE_ void add_constant_torque(real_t p_torque) {
constant_torque += p_torque;
}
void set_constant_force(const Vector2 &p_force) { constant_force = p_force; }
Vector2 get_constant_force() const { return constant_force; }
void set_constant_torque(real_t p_torque) { constant_torque = p_torque; }
real_t get_constant_torque() const { return constant_torque; }
void set_active(bool p_active);
_FORCE_INLINE_ bool is_active() const { return active; }
_FORCE_INLINE_ void wakeup() {
if ((!get_space()) || mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) {
return;
}
set_active(true);
}
void set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value);
Variant get_param(PhysicsServer2D::BodyParameter p_param) const;
void set_mode(PhysicsServer2D::BodyMode p_mode);
PhysicsServer2D::BodyMode get_mode() const;
void set_state(PhysicsServer2D::BodyState p_state, const Variant &p_variant);
Variant get_state(PhysicsServer2D::BodyState p_state) const;
_FORCE_INLINE_ void set_continuous_collision_detection_mode(PhysicsServer2D::CCDMode p_mode) { continuous_cd_mode = p_mode; }
_FORCE_INLINE_ PhysicsServer2D::CCDMode get_continuous_collision_detection_mode() const { return continuous_cd_mode; }
void set_space(GodotSpace2D *p_space) override;
void update_mass_properties();
void reset_mass_properties();
_FORCE_INLINE_ const Vector2 &get_center_of_mass() const { return center_of_mass; }
_FORCE_INLINE_ const Vector2 &get_center_of_mass_local() const { return center_of_mass_local; }
_FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; }
_FORCE_INLINE_ real_t get_inv_inertia() const { return _inv_inertia; }
_FORCE_INLINE_ real_t get_friction() const { return friction; }
_FORCE_INLINE_ real_t get_bounce() const { return bounce; }
void integrate_forces(real_t p_step);
void integrate_velocities(real_t p_step);
_FORCE_INLINE_ Vector2 get_velocity_in_local_point(const Vector2 &rel_pos) const {
return linear_velocity + Vector2(-angular_velocity * rel_pos.y, angular_velocity * rel_pos.x);
}
_FORCE_INLINE_ Vector2 get_motion() const {
if (mode > PhysicsServer2D::BODY_MODE_KINEMATIC) {
return new_transform.get_origin() - get_transform().get_origin();
} else if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) {
return get_transform().get_origin() - new_transform.get_origin(); //kinematic simulates forward
}
return Vector2();
}
void call_queries();
void wakeup_neighbours();
bool sleep_test(real_t p_step);
GodotBody2D();
~GodotBody2D();
};
//add contact inline
void 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) {
int c_max = contacts.size();
if (c_max == 0) {
return;
}
Contact *c = contacts.ptrw();
int idx = -1;
if (contact_count < c_max) {
idx = contact_count++;
} else {
real_t least_depth = 1e20;
int least_deep = -1;
for (int i = 0; i < c_max; i++) {
if (i == 0 || c[i].depth < least_depth) {
least_deep = i;
least_depth = c[i].depth;
}
}
if (least_deep >= 0 && least_depth < p_depth) {
idx = least_deep;
}
if (idx == -1) {
return; //none least deepe than this
}
}
c[idx].local_pos = p_local_pos;
c[idx].local_normal = p_local_normal;
c[idx].local_velocity_at_pos = p_local_velocity_at_pos;
c[idx].depth = p_depth;
c[idx].local_shape = p_local_shape;
c[idx].collider_pos = p_collider_pos;
c[idx].collider_shape = p_collider_shape;
c[idx].collider_instance_id = p_collider_instance_id;
c[idx].collider = p_collider;
c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos;
c[idx].impulse = p_impulse;
}
#endif // GODOT_BODY_2D_H

229
servers/physics_2d/godot_body_direct_state_2d.cpp
View File

@ -1,229 +0,0 @@
/**************************************************************************/
/* godot_body_direct_state_2d.cpp */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#include "godot_body_direct_state_2d.h"
#include "godot_body_2d.h"
#include "godot_physics_server_2d.h"
#include "godot_space_2d.h"
Vector2 GodotPhysicsDirectBodyState2D::get_total_gravity() const {
return body->gravity;
}
real_t GodotPhysicsDirectBodyState2D::get_total_angular_damp() const {
return body->total_angular_damp;
}
real_t GodotPhysicsDirectBodyState2D::get_total_linear_damp() const {
return body->total_linear_damp;
}
Vector2 GodotPhysicsDirectBodyState2D::get_center_of_mass() const {
return body->get_center_of_mass();
}
Vector2 GodotPhysicsDirectBodyState2D::get_center_of_mass_local() const {
return body->get_center_of_mass_local();
}
real_t GodotPhysicsDirectBodyState2D::get_inverse_mass() const {
return body->get_inv_mass();
}
real_t GodotPhysicsDirectBodyState2D::get_inverse_inertia() const {
return body->get_inv_inertia();
}
void GodotPhysicsDirectBodyState2D::set_linear_velocity(const Vector2 &p_velocity) {
body->wakeup();
body->set_linear_velocity(p_velocity);
}
Vector2 GodotPhysicsDirectBodyState2D::get_linear_velocity() const {
return body->get_linear_velocity();
}
void GodotPhysicsDirectBodyState2D::set_angular_velocity(real_t p_velocity) {
body->wakeup();
body->set_angular_velocity(p_velocity);
}
real_t GodotPhysicsDirectBodyState2D::get_angular_velocity() const {
return body->get_angular_velocity();
}
void GodotPhysicsDirectBodyState2D::set_transform(const Transform2D &p_transform) {
body->set_state(PhysicsServer2D::BODY_STATE_TRANSFORM, p_transform);
}
Transform2D GodotPhysicsDirectBodyState2D::get_transform() const {
return body->get_transform();
}
Vector2 GodotPhysicsDirectBodyState2D::get_velocity_at_local_position(const Vector2 &p_position) const {
return body->get_velocity_in_local_point(p_position);
}
void GodotPhysicsDirectBodyState2D::apply_central_impulse(const Vector2 &p_impulse) {
body->wakeup();
body->apply_central_impulse(p_impulse);
}
void GodotPhysicsDirectBodyState2D::apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position) {
body->wakeup();
body->apply_impulse(p_impulse, p_position);
}
void GodotPhysicsDirectBodyState2D::apply_torque_impulse(real_t p_torque) {
body->wakeup();
body->apply_torque_impulse(p_torque);
}
void GodotPhysicsDirectBodyState2D::apply_central_force(const Vector2 &p_force) {
body->wakeup();
body->apply_central_force(p_force);
}
void GodotPhysicsDirectBodyState2D::apply_force(const Vector2 &p_force, const Vector2 &p_position) {
body->wakeup();
body->apply_force(p_force, p_position);
}
void GodotPhysicsDirectBodyState2D::apply_torque(real_t p_torque) {
body->wakeup();
body->apply_torque(p_torque);
}
void GodotPhysicsDirectBodyState2D::add_constant_central_force(const Vector2 &p_force) {
body->wakeup();
body->add_constant_central_force(p_force);
}
void GodotPhysicsDirectBodyState2D::add_constant_force(const Vector2 &p_force, const Vector2 &p_position) {
body->wakeup();
body->add_constant_force(p_force, p_position);
}
void GodotPhysicsDirectBodyState2D::add_constant_torque(real_t p_torque) {
body->wakeup();
body->add_constant_torque(p_torque);
}
void GodotPhysicsDirectBodyState2D::set_constant_force(const Vector2 &p_force) {
if (!p_force.is_zero_approx()) {
body->wakeup();
}
body->set_constant_force(p_force);
}
Vector2 GodotPhysicsDirectBodyState2D::get_constant_force() const {
return body->get_constant_force();
}
void GodotPhysicsDirectBodyState2D::set_constant_torque(real_t p_torque) {
if (!Math::is_zero_approx(p_torque)) {
body->wakeup();
}
body->set_constant_torque(p_torque);
}
real_t GodotPhysicsDirectBodyState2D::get_constant_torque() const {
return body->get_constant_torque();
}
void GodotPhysicsDirectBodyState2D::set_sleep_state(bool p_enable) {
body->set_active(!p_enable);
}
bool GodotPhysicsDirectBodyState2D::is_sleeping() const {
return !body->is_active();
}
int GodotPhysicsDirectBodyState2D::get_contact_count() const {
return body->contact_count;
}
Vector2 GodotPhysicsDirectBodyState2D::get_contact_local_position(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
return body->contacts[p_contact_idx].local_pos;
}
Vector2 GodotPhysicsDirectBodyState2D::get_contact_local_normal(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
return body->contacts[p_contact_idx].local_normal;
}
int GodotPhysicsDirectBodyState2D::get_contact_local_shape(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1);
return body->contacts[p_contact_idx].local_shape;
}
Vector2 GodotPhysicsDirectBodyState2D::get_contact_local_velocity_at_position(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
return body->contacts[p_contact_idx].local_velocity_at_pos;
}
RID GodotPhysicsDirectBodyState2D::get_contact_collider(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID());
return body->contacts[p_contact_idx].collider;
}
Vector2 GodotPhysicsDirectBodyState2D::get_contact_collider_position(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
return body->contacts[p_contact_idx].collider_pos;
}
ObjectID GodotPhysicsDirectBodyState2D::get_contact_collider_id(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID());
return body->contacts[p_contact_idx].collider_instance_id;
}
int GodotPhysicsDirectBodyState2D::get_contact_collider_shape(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0);
return body->contacts[p_contact_idx].collider_shape;
}
Vector2 GodotPhysicsDirectBodyState2D::get_contact_collider_velocity_at_position(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
return body->contacts[p_contact_idx].collider_velocity_at_pos;
}
Vector2 GodotPhysicsDirectBodyState2D::get_contact_impulse(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
return body->contacts[p_contact_idx].impulse;
}
PhysicsDirectSpaceState2D *GodotPhysicsDirectBodyState2D::get_space_state() {
return body->get_space()->get_direct_state();
}
real_t GodotPhysicsDirectBodyState2D::get_step() const {
return body->get_space()->get_last_step();
}

104
servers/physics_2d/godot_body_direct_state_2d.h
View File

@ -1,104 +0,0 @@
/**************************************************************************/
/* godot_body_direct_state_2d.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#ifndef GODOT_BODY_DIRECT_STATE_2D_H
#define GODOT_BODY_DIRECT_STATE_2D_H
#include "servers/physics_server_2d.h"
class GodotBody2D;
class GodotPhysicsDirectBodyState2D : public PhysicsDirectBodyState2D {
GDCLASS(GodotPhysicsDirectBodyState2D, PhysicsDirectBodyState2D);
public:
GodotBody2D *body = nullptr;
virtual Vector2 get_total_gravity() const override;
virtual real_t get_total_angular_damp() const override;
virtual real_t get_total_linear_damp() const override;
virtual Vector2 get_center_of_mass() const override;
virtual Vector2 get_center_of_mass_local() const override;
virtual real_t get_inverse_mass() const override;
virtual real_t get_inverse_inertia() const override;
virtual void set_linear_velocity(const Vector2 &p_velocity) override;
virtual Vector2 get_linear_velocity() const override;
virtual void set_angular_velocity(real_t p_velocity) override;
virtual real_t get_angular_velocity() const override;
virtual void set_transform(const Transform2D &p_transform) override;
virtual Transform2D get_transform() const override;
virtual Vector2 get_velocity_at_local_position(const Vector2 &p_position) const override;
virtual void apply_central_impulse(const Vector2 &p_impulse) override;
virtual void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override;
virtual void apply_torque_impulse(real_t p_torque) override;
virtual void apply_central_force(const Vector2 &p_force) override;
virtual void apply_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override;
virtual void apply_torque(real_t p_torque) override;
virtual void add_constant_central_force(const Vector2 &p_force) override;
virtual void add_constant_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override;
virtual void add_constant_torque(real_t p_torque) override;
virtual void set_constant_force(const Vector2 &p_force) override;
virtual Vector2 get_constant_force() const override;
virtual void set_constant_torque(real_t p_torque) override;
virtual real_t get_constant_torque() const override;
virtual void set_sleep_state(bool p_enable) override;
virtual bool is_sleeping() const override;
virtual int get_contact_count() const override;
virtual Vector2 get_contact_local_position(int p_contact_idx) const override;
virtual Vector2 get_contact_local_normal(int p_contact_idx) const override;
virtual int get_contact_local_shape(int p_contact_idx) const override;
virtual Vector2 get_contact_local_velocity_at_position(int p_contact_idx) const override;
virtual RID get_contact_collider(int p_contact_idx) const override;
virtual Vector2 get_contact_collider_position(int p_contact_idx) const override;
virtual ObjectID get_contact_collider_id(int p_contact_idx) const override;
virtual int get_contact_collider_shape(int p_contact_idx) const override;
virtual Vector2 get_contact_collider_velocity_at_position(int p_contact_idx) const override;
virtual Vector2 get_contact_impulse(int p_contact_idx) const override;
virtual PhysicsDirectSpaceState2D *get_space_state() override;
virtual real_t get_step() const override;
};
#endif // GODOT_BODY_DIRECT_STATE_2D_H

608
servers/physics_2d/godot_body_pair_2d.cpp
View File

@ -1,608 +0,0 @@
/**************************************************************************/
/* godot_body_pair_2d.cpp */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#include "godot_body_pair_2d.h"
#include "godot_collision_solver_2d.h"
#include "godot_space_2d.h"
#define ACCUMULATE_IMPULSES
#define MIN_VELOCITY 0.001
#define MAX_BIAS_ROTATION (Math_PI / 8)
void GodotBodyPair2D::_add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self) {
GodotBodyPair2D *self = static_cast<GodotBodyPair2D *>(p_self);
self->_contact_added_callback(p_point_A, p_point_B);
}
void GodotBodyPair2D::_contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B) {
Vector2 local_A = A->get_inv_transform().basis_xform(p_point_A);
Vector2 local_B = B->get_inv_transform().basis_xform(p_point_B - offset_B);
int new_index = contact_count;
ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1));
Contact contact;
contact.local_A = local_A;
contact.local_B = local_B;
contact.normal = (p_point_A - p_point_B).normalized();
contact.used = true;
// Attempt to determine if the contact will be reused.
real_t recycle_radius_2 = space->get_contact_recycle_radius() * space->get_contact_recycle_radius();
for (int i = 0; i < contact_count; i++) {
Contact &c = contacts[i];
if (c.local_A.distance_squared_to(local_A) < (recycle_radius_2) &&
c.local_B.distance_squared_to(local_B) < (recycle_radius_2)) {
contact.acc_normal_impulse = c.acc_normal_impulse;
contact.acc_tangent_impulse = c.acc_tangent_impulse;
contact.acc_bias_impulse = c.acc_bias_impulse;
contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass;
c = contact;
return;
}
}
// Figure out if the contact amount must be reduced to fit the new contact.
if (new_index == MAX_CONTACTS) {
// Remove the contact with the minimum depth.
const Transform2D &transform_A = A->get_transform();
const Transform2D &transform_B = B->get_transform();
int least_deep = -1;
real_t min_depth;
// Start with depth for new contact.
{
Vector2 global_A = transform_A.basis_xform(contact.local_A);
Vector2 global_B = transform_B.basis_xform(contact.local_B) + offset_B;
Vector2 axis = global_A - global_B;
min_depth = axis.dot(contact.normal);
}
for (int i = 0; i < contact_count; i++) {
const Contact &c = contacts[i];
Vector2 global_A = transform_A.basis_xform(c.local_A);
Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
Vector2 axis = global_A - global_B;
real_t depth = axis.dot(c.normal);
if (depth < min_depth) {
min_depth = depth;
least_deep = i;
}
}
if (least_deep > -1) {
// Replace the least deep contact by the new one.
contacts[least_deep] = contact;
}
return;
}
contacts[new_index] = contact;
contact_count++;
}
void GodotBodyPair2D::_validate_contacts() {
// Make sure to erase contacts that are no longer valid.
real_t max_separation = space->get_contact_max_separation();
real_t max_separation2 = max_separation * max_separation;
const Transform2D &transform_A = A->get_transform();
const Transform2D &transform_B = B->get_transform();
for (int i = 0; i < contact_count; i++) {
Contact &c = contacts[i];
bool erase = false;
if (!c.used) {
// Was left behind in previous frame.
erase = true;
} else {
c.used = false;
Vector2 global_A = transform_A.basis_xform(c.local_A);
Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
Vector2 axis = global_A - global_B;
real_t depth = axis.dot(c.normal);
if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) {
erase = true;
}
}
if (erase) {
// Contact no longer needed, remove.
if ((i + 1) < contact_count) {
// Swap with the last one.
SWAP(contacts[i], contacts[contact_count - 1]);
}
i--;
contact_count--;
}
}
}
// _test_ccd prevents tunneling by slowing down a high velocity body that is about to collide so that next frame it will be at an appropriate location to collide (i.e. slight overlap)
// Warning: the way velocity is adjusted down to cause a collision means the momentum will be weaker than it should for a bounce!
// Process: only proceed if body A's motion is high relative to its size.
// cast forward along motion vector to see if A is going to enter/pass B's collider next frame, only proceed if it does.
// adjust the velocity of A down so that it will just slightly intersect the collider instead of blowing right past it.
bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B) {
Vector2 motion = p_A->get_linear_velocity() * p_step;
real_t mlen = motion.length();
if (mlen < CMP_EPSILON) {
return false;
}
Vector2 mnormal = motion / mlen;
real_t min = 0.0, max = 0.0;
p_A->get_shape(p_shape_A)->project_rangev(mnormal, p_xform_A, min, max);
// Did it move enough in this direction to even attempt raycast?
// Let's say it should move more than 1/3 the size of the object in that axis.
bool fast_object = mlen > (max - min) * 0.3;
if (!fast_object) {
return false;
}
// A is moving fast enough that tunneling might occur. See if it's really about to collide.
// Roughly predict body B's position in the next frame (ignoring collisions).
Transform2D predicted_xform_B = p_xform_B.translated(p_B->get_linear_velocity() * p_step);
// Cast a segment from support in motion normal, in the same direction of motion by motion length.
// Support point will the farthest forward collision point along the movement vector.
// i.e. the point that should hit B first if any collision does occur.
// convert mnormal into body A's local xform because get_support requires (and returns) local coordinates.
int a;
Vector2 s[2];
p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform_inv(mnormal).normalized(), s, a);
Vector2 from = p_xform_A.xform(s[0]);
// Back up 10% of the per-frame motion behind the support point and use that as the beginning of our cast.
// This should ensure the calculated new velocity will really cause a bit of overlap instead of just getting us very close.
Vector2 to = from + motion;
Transform2D from_inv = predicted_xform_B.affine_inverse();
// Back up 10% of the per-frame motion behind the support point and use that as the beginning of our cast.
// At high speeds, this may mean we're actually casting from well behind the body instead of inside it, which is odd. But it still works out.
Vector2 local_from = from_inv.xform(from - motion * 0.1);
Vector2 local_to = from_inv.xform(to);
Vector2 rpos, rnorm;
if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) {
// there was no hit. Since the segment is the length of per-frame motion, this means the bodies will not
// actually collide yet on next frame. We'll probably check again next frame once they're closer.
return false;
}
// Check one-way collision based on motion direction.
if (p_A->get_shape(p_shape_A)->allows_one_way_collision() && p_B->is_shape_set_as_one_way_collision(p_shape_B)) {
Vector2 direction = predicted_xform_B.columns[1].normalized();
if (direction.dot(mnormal) < CMP_EPSILON) {
collided = false;
oneway_disabled = true;
return false;
}
}
// Shorten the linear velocity so it does not hit, but gets close enough,
// next frame will hit softly or soft enough.
Vector2 hitpos = predicted_xform_B.xform(rpos);
real_t newlen = hitpos.distance_to(from) + (max - min) * 0.01; // adding 1% of body length to the distance between collision and support point should cause body A's support point to arrive just within B's collider next frame.
p_A->set_linear_velocity(mnormal * (newlen / p_step));
return true;
}
real_t combine_bounce(GodotBody2D *A, GodotBody2D *B) {
return CLAMP(A->get_bounce() + B->get_bounce(), 0, 1);
}
real_t combine_friction(GodotBody2D *A, GodotBody2D *B) {
return ABS(MIN(A->get_friction(), B->get_friction()));
}
bool GodotBodyPair2D::setup(real_t p_step) {
check_ccd = false;
if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) {
collided = false;
return false;
}
collide_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && A->collides_with(B);
collide_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && B->collides_with(A);
report_contacts_only = false;
if (!collide_A && !collide_B) {
if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) {
report_contacts_only = true;
} else {
collided = false;
return false;
}
}
//use local A coordinates to avoid numerical issues on collision detection
offset_B = B->get_transform().get_origin() - A->get_transform().get_origin();
_validate_contacts();
const Vector2 &offset_A = A->get_transform().get_origin();
Transform2D xform_Au = A->get_transform().untranslated();
Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A);
Transform2D xform_Bu = B->get_transform();
xform_Bu.columns[2] -= offset_A;
Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B);
GodotShape2D *shape_A_ptr = A->get_shape(shape_A);
GodotShape2D *shape_B_ptr = B->get_shape(shape_B);
Vector2 motion_A, motion_B;
if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_SHAPE) {
motion_A = A->get_motion();
}
if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_SHAPE) {
motion_B = B->get_motion();
}
bool prev_collided = collided;
collided = GodotCollisionSolver2D::solve(shape_A_ptr, xform_A, motion_A, shape_B_ptr, xform_B, motion_B, _add_contact, this, &sep_axis);
if (!collided) {
oneway_disabled = false;
if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) {
check_ccd = true;
return true;
}
if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_B) {
check_ccd = true;
return true;
}
return false;
}
if (oneway_disabled) {
return false;
}
if (!prev_collided) {
if (shape_B_ptr->allows_one_way_collision() && A->is_shape_set_as_one_way_collision(shape_A)) {
Vector2 direction = xform_A.columns[1].normalized();
bool valid = false;
for (int i = 0; i < contact_count; i++) {
Contact &c = contacts[i];
if (c.normal.dot(direction) > -CMP_EPSILON) { // Greater (normal inverted).
continue;
}
valid = true;
break;
}
if (!valid) {
collided = false;
oneway_disabled = true;
return false;
}
}
if (shape_A_ptr->allows_one_way_collision() && B->is_shape_set_as_one_way_collision(shape_B)) {
Vector2 direction = xform_B.columns[1].normalized();
bool valid = false;
for (int i = 0; i < contact_count; i++) {
Contact &c = contacts[i];
if (c.normal.dot(direction) < CMP_EPSILON) { // Less (normal ok).
continue;
}
valid = true;
break;
}
if (!valid) {
collided = false;
oneway_disabled = true;
return false;
}
}
}
return true;
}
bool GodotBodyPair2D::pre_solve(real_t p_step) {
if (oneway_disabled) {
return false;
}
if (!collided) {
if (check_ccd) {
const Vector2 &offset_A = A->get_transform().get_origin();
Transform2D xform_Au = A->get_transform().untranslated();
Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A);
Transform2D xform_Bu = B->get_transform();
xform_Bu.columns[2] -= offset_A;
Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B);
if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) {
_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B);
}
if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_B) {
_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A);
}
}
return false;
}
real_t max_penetration = space->get_contact_max_allowed_penetration();
real_t bias = space->get_contact_bias();
GodotShape2D *shape_A_ptr = A->get_shape(shape_A);
GodotShape2D *shape_B_ptr = B->get_shape(shape_B);
if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) {
if (shape_A_ptr->get_custom_bias() == 0) {
bias = shape_B_ptr->get_custom_bias();
} else if (shape_B_ptr->get_custom_bias() == 0) {
bias = shape_A_ptr->get_custom_bias();
} else {
bias = (shape_B_ptr->get_custom_bias() + shape_A_ptr->get_custom_bias()) * 0.5;
}
}
real_t inv_dt = 1.0 / p_step;
bool do_process = false;
const Vector2 &offset_A = A->get_transform().get_origin();
const Transform2D &transform_A = A->get_transform();
const Transform2D &transform_B = B->get_transform();
real_t inv_inertia_A = collide_A ? A->get_inv_inertia() : 0.0;
real_t inv_inertia_B = collide_B ? B->get_inv_inertia() : 0.0;
real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0;
real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0;
for (int i = 0; i < contact_count; i++) {
Contact &c = contacts[i];
c.active = false;
Vector2 global_A = transform_A.basis_xform(c.local_A);
Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
Vector2 axis = global_A - global_B;
real_t depth = axis.dot(c.normal);
if (depth <= 0.0) {
continue;
}
#ifdef DEBUG_ENABLED
if (space->is_debugging_contacts()) {
space->add_debug_contact(global_A + offset_A);
space->add_debug_contact(global_B + offset_A);
}
#endif
c.rA = global_A - A->get_center_of_mass();
c.rB = global_B - B->get_center_of_mass() - offset_B;
// Precompute normal mass, tangent mass, and bias.
real_t rnA = c.rA.dot(c.normal);
real_t rnB = c.rB.dot(c.normal);
real_t kNormal = inv_mass_A + inv_mass_B;
kNormal += inv_inertia_A * (c.rA.dot(c.rA) - rnA * rnA) + inv_inertia_B * (c.rB.dot(c.rB) - rnB * rnB);
c.mass_normal = 1.0f / kNormal;
Vector2 tangent = c.normal.orthogonal();
real_t rtA = c.rA.dot(tangent);
real_t rtB = c.rB.dot(tangent);
real_t kTangent = inv_mass_A + inv_mass_B;
kTangent += inv_inertia_A * (c.rA.dot(c.rA) - rtA * rtA) + inv_inertia_B * (c.rB.dot(c.rB) - rtB * rtB);
c.mass_tangent = 1.0f / kTangent;
c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
c.depth = depth;
Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent;
c.acc_impulse -= P;
if (A->can_report_contacts() || B->can_report_contacts()) {
Vector2 crB = Vector2(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x) + B->get_linear_velocity();
Vector2 crA = Vector2(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x) + A->get_linear_velocity();
if (A->can_report_contacts()) {
A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, crA, global_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB, c.acc_impulse);
}
if (B->can_report_contacts()) {
B->add_contact(global_B + offset_A, c.normal, depth, shape_B, crB, global_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA, c.acc_impulse);
}
}
if (report_contacts_only) {
collided = false;
continue;
}
#ifdef ACCUMULATE_IMPULSES
{
// Apply normal + friction impulse
if (collide_A) {
A->apply_impulse(-P, c.rA + A->get_center_of_mass());
}
if (collide_B) {
B->apply_impulse(P, c.rB + B->get_center_of_mass());
}
}
#endif
c.bounce = combine_bounce(A, B);
if (c.bounce) {
Vector2 crA(-A->get_prev_angular_velocity() * c.rA.y, A->get_prev_angular_velocity() * c.rA.x);
Vector2 crB(-B->get_prev_angular_velocity() * c.rB.y, B->get_prev_angular_velocity() * c.rB.x);
Vector2 dv = B->get_prev_linear_velocity() + crB - A->get_prev_linear_velocity() - crA;
c.bounce = c.bounce * dv.dot(c.normal);
}
c.active = true;
do_process = true;
}
return do_process;
}
void GodotBodyPair2D::solve(real_t p_step) {
if (!collided || oneway_disabled) {
return;
}
const real_t max_bias_av = MAX_BIAS_ROTATION / p_step;
real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0;
real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0;
for (int i = 0; i < contact_count; ++i) {
Contact &c = contacts[i];
if (!c.active) {
continue;
}
// Relative velocity at contact
Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x);
Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x);
Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA;
Vector2 crbA(-A->get_biased_angular_velocity() * c.rA.y, A->get_biased_angular_velocity() * c.rA.x);
Vector2 crbB(-B->get_biased_angular_velocity() * c.rB.y, B->get_biased_angular_velocity() * c.rB.x);
Vector2 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA;
real_t vn = dv.dot(c.normal);
real_t vbn = dbv.dot(c.normal);
Vector2 tangent = c.normal.orthogonal();
real_t vt = dv.dot(tangent);
real_t jbn = (c.bias - vbn) * c.mass_normal;
real_t jbnOld = c.acc_bias_impulse;
c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f);
Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld);
if (collide_A) {
A->apply_bias_impulse(-jb, c.rA + A->get_center_of_mass(), max_bias_av);
}
if (collide_B) {
B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), max_bias_av);
}
crbA = Vector2(-A->get_biased_angular_velocity() * c.rA.y, A->get_biased_angular_velocity() * c.rA.x);
crbB = Vector2(-B->get_biased_angular_velocity() * c.rB.y, B->get_biased_angular_velocity() * c.rB.x);
dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA;
vbn = dbv.dot(c.normal);
if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) {
real_t jbn_com = (-vbn + c.bias) / (inv_mass_A + inv_mass_B);
real_t jbnOld_com = c.acc_bias_impulse_center_of_mass;
c.acc_bias_impulse_center_of_mass = MAX(jbnOld_com + jbn_com, 0.0f);
Vector2 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com);
if (collide_A) {
A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f);
}
if (collide_B) {
B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f);
}
}
real_t jn = -(c.bounce + vn) * c.mass_normal;
real_t jnOld = c.acc_normal_impulse;
c.acc_normal_impulse = MAX(jnOld + jn, 0.0f);
real_t friction = combine_friction(A, B);
real_t jtMax = friction * c.acc_normal_impulse;
real_t jt = -vt * c.mass_tangent;
real_t jtOld = c.acc_tangent_impulse;
c.acc_tangent_impulse = CLAMP(jtOld + jt, -jtMax, jtMax);
Vector2 j = c.normal * (c.acc_normal_impulse - jnOld) + tangent * (c.acc_tangent_impulse - jtOld);
if (collide_A) {
A->apply_impulse(-j, c.rA + A->get_center_of_mass());
}
if (collide_B) {
B->apply_impulse(j, c.rB + B->get_center_of_mass());
}
c.acc_impulse -= j;
}
}
GodotBodyPair2D::GodotBodyPair2D(GodotBody2D *p_A, int p_shape_A, GodotBody2D *p_B, int p_shape_B) :
GodotConstraint2D(_arr, 2) {
A = p_A;
B = p_B;
shape_A = p_shape_A;
shape_B = p_shape_B;
space = A->get_space();
A->add_constraint(this, 0);
B->add_constraint(this, 1);
}
GodotBodyPair2D::~GodotBodyPair2D() {
A->remove_constraint(this, 0);
B->remove_constraint(this, 1);
}

101
servers/physics_2d/godot_body_pair_2d.h
View File

@ -1,101 +0,0 @@
/**************************************************************************/
/* godot_body_pair_2d.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#ifndef GODOT_BODY_PAIR_2D_H
#define GODOT_BODY_PAIR_2D_H
#include "godot_body_2d.h"
#include "godot_constraint_2d.h"
class GodotBodyPair2D : public GodotConstraint2D {
enum {
MAX_CONTACTS = 2
};
union {
struct {
GodotBody2D *A;
GodotBody2D *B;
};
GodotBody2D *_arr[2] = { nullptr, nullptr };
};
int shape_A = 0;
int shape_B = 0;
bool collide_A = false;
bool collide_B = false;
GodotSpace2D *space = nullptr;
struct Contact {
Vector2 position;
Vector2 normal;
Vector2 local_A, local_B;
Vector2 acc_impulse; // accumulated impulse
real_t acc_normal_impulse = 0.0; // accumulated normal impulse (Pn)
real_t acc_tangent_impulse = 0.0; // accumulated tangent impulse (Pt)
real_t acc_bias_impulse = 0.0; // accumulated normal impulse for position bias (Pnb)
real_t acc_bias_impulse_center_of_mass = 0.0; // accumulated normal impulse for position bias applied to com
real_t mass_normal, mass_tangent = 0.0;
real_t bias = 0.0;
real_t depth = 0.0;
bool active = false;
bool used = false;
Vector2 rA, rB;
real_t bounce = 0.0;
};
Vector2 offset_B; //use local A coordinates to avoid numerical issues on collision detection
Vector2 sep_axis;
Contact contacts[MAX_CONTACTS];
int contact_count = 0;
bool collided = false;
bool check_ccd = false;
bool oneway_disabled = false;
bool report_contacts_only = false;
bool _test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B);
void _validate_contacts();
static void _add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self);
_FORCE_INLINE_ void _contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B);
public:
virtual bool setup(real_t p_step) override;
virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
GodotBodyPair2D(GodotBody2D *p_A, int p_shape_A, GodotBody2D *p_B, int p_shape_B);
~GodotBodyPair2D();
};
#endif // GODOT_BODY_PAIR_2D_H

36
servers/physics_2d/godot_broad_phase_2d.cpp
View File

@ -1,36 +0,0 @@
/**************************************************************************/
/* godot_broad_phase_2d.cpp */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#include "godot_broad_phase_2d.h"
GodotBroadPhase2D::CreateFunction GodotBroadPhase2D::create_func = nullptr;
GodotBroadPhase2D::~GodotBroadPhase2D() {
}

71
servers/physics_2d/godot_broad_phase_2d.h
View File

@ -1,71 +0,0 @@
/**************************************************************************/
/* godot_broad_phase_2d.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#ifndef GODOT_BROAD_PHASE_2D_H
#define GODOT_BROAD_PHASE_2D_H
#include "core/math/math_funcs.h"
#include "core/math/rect2.h"
class GodotCollisionObject2D;
class GodotBroadPhase2D {
public:
typedef GodotBroadPhase2D *(*CreateFunction)();
static CreateFunction create_func;
typedef uint32_t ID;
typedef void *(*PairCallback)(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_userdata);
typedef void (*UnpairCallback)(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_data, void *p_userdata);
// 0 is an invalid ID
virtual ID create(GodotCollisionObject2D *p_object_, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false) = 0;
virtual void move(ID p_id, const Rect2 &p_aabb) = 0;
virtual void set_static(ID p_id, bool p_static) = 0;
virtual void remove(ID p_id) = 0;
virtual GodotCollisionObject2D *get_object(ID p_id) const = 0;
virtual bool is_static(ID p_id) const = 0;
virtual int get_subindex(ID p_id) const = 0;
virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0;
virtual int cull_aabb(const Rect2 &p_aabb, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0;
virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata) = 0;
virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) = 0;
virtual void update() = 0;
virtual ~GodotBroadPhase2D();
};
#endif // GODOT_BROAD_PHASE_2D_H

123
servers/physics_2d/godot_broad_phase_2d_bvh.cpp
View File

@ -1,123 +0,0 @@
/**************************************************************************/
/* godot_broad_phase_2d_bvh.cpp */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#include "godot_broad_phase_2d_bvh.h"
#include "godot_collision_object_2d.h"
GodotBroadPhase2D::ID GodotBroadPhase2DBVH::create(GodotCollisionObject2D *p_object, int p_subindex, const Rect2 &p_aabb, bool p_static) {
uint32_t tree_id = p_static ? TREE_STATIC : TREE_DYNAMIC;
uint32_t tree_collision_mask = p_static ? TREE_FLAG_DYNAMIC : (TREE_FLAG_STATIC | TREE_FLAG_DYNAMIC);
ID oid = bvh.create(p_object, true, tree_id, tree_collision_mask, p_aabb, p_subindex); // Pair everything, don't care?
return oid + 1;
}
void GodotBroadPhase2DBVH::move(ID p_id, const Rect2 &p_aabb) {
ERR_FAIL_COND(!p_id);
bvh.move(p_id - 1, p_aabb);
}
void GodotBroadPhase2DBVH::set_static(ID p_id, bool p_static) {
ERR_FAIL_COND(!p_id);
uint32_t tree_id = p_static ? TREE_STATIC : TREE_DYNAMIC;
uint32_t tree_collision_mask = p_static ? TREE_FLAG_DYNAMIC : (TREE_FLAG_STATIC | TREE_FLAG_DYNAMIC);
bvh.set_tree(p_id - 1, tree_id, tree_collision_mask, false);
}
void GodotBroadPhase2DBVH::remove(ID p_id) {
ERR_FAIL_COND(!p_id);
bvh.erase(p_id - 1);
}
GodotCollisionObject2D *GodotBroadPhase2DBVH::get_object(ID p_id) const {
ERR_FAIL_COND_V(!p_id, nullptr);
GodotCollisionObject2D *it = bvh.get(p_id - 1);
ERR_FAIL_NULL_V(it, nullptr);
return it;
}
bool GodotBroadPhase2DBVH::is_static(ID p_id) const {
ERR_FAIL_COND_V(!p_id, false);
uint32_t tree_id = bvh.get_tree_id(p_id - 1);
return tree_id == 0;
}
int GodotBroadPhase2DBVH::get_subindex(ID p_id) const {
ERR_FAIL_COND_V(!p_id, 0);
return bvh.get_subindex(p_id - 1);
}
int GodotBroadPhase2DBVH::cull_segment(const Vector2 &p_from, const Vector2 &p_to, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices) {
return bvh.cull_segment(p_from, p_to, p_results, p_max_results, nullptr, 0xFFFFFFFF, p_result_indices);
}
int GodotBroadPhase2DBVH::cull_aabb(const Rect2 &p_aabb, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices) {
return bvh.cull_aabb(p_aabb, p_results, p_max_results, nullptr, 0xFFFFFFFF, p_result_indices);
}
void *GodotBroadPhase2DBVH::_pair_callback(void *self, uint32_t p_A, GodotCollisionObject2D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject2D *p_object_B, int subindex_B) {
GodotBroadPhase2DBVH *bpo = static_cast<GodotBroadPhase2DBVH *>(self);
if (!bpo->pair_callback) {
return nullptr;
}
return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata);
}
void GodotBroadPhase2DBVH::_unpair_callback(void *self, uint32_t p_A, GodotCollisionObject2D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject2D *p_object_B, int subindex_B, void *pairdata) {
GodotBroadPhase2DBVH *bpo = static_cast<GodotBroadPhase2DBVH *>(self);
if (!bpo->unpair_callback) {
return;
}
bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata);
}
void GodotBroadPhase2DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
pair_callback = p_pair_callback;
pair_userdata = p_userdata;
}
void GodotBroadPhase2DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
unpair_callback = p_unpair_callback;
unpair_userdata = p_userdata;
}
void GodotBroadPhase2DBVH::update() {
bvh.update();
}
GodotBroadPhase2D *GodotBroadPhase2DBVH::_create() {
return memnew(GodotBroadPhase2DBVH);
}
GodotBroadPhase2DBVH::GodotBroadPhase2DBVH() {
bvh.set_pair_callback(_pair_callback, this);
bvh.set_unpair_callback(_unpair_callback, this);
}

101
servers/physics_2d/godot_broad_phase_2d_bvh.h
View File

@ -1,101 +0,0 @@
/**************************************************************************/
/* godot_broad_phase_2d_bvh.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#ifndef GODOT_BROAD_PHASE_2D_BVH_H
#define GODOT_BROAD_PHASE_2D_BVH_H
#include "godot_broad_phase_2d.h"
#include "core/math/bvh.h"
#include "core/math/rect2.h"
#include "core/math/vector2.h"
class GodotBroadPhase2DBVH : public GodotBroadPhase2D {
template <typename T>
class UserPairTestFunction {
public:
static bool user_pair_check(const T *p_a, const T *p_b) {
// return false if no collision, decided by masks etc
return p_a->interacts_with(p_b);
}
};
template <typename T>
class UserCullTestFunction {
public:
static bool user_cull_check(const T *p_a, const T *p_b) {
return true;
}
};
enum Tree {
TREE_STATIC = 0,
TREE_DYNAMIC = 1,
};
enum TreeFlag {
TREE_FLAG_STATIC = 1 << TREE_STATIC,
TREE_FLAG_DYNAMIC = 1 << TREE_DYNAMIC,
};
BVH_Manager<GodotCollisionObject2D, 2, true, 128, UserPairTestFunction<GodotCollisionObject2D>, UserCullTestFunction<GodotCollisionObject2D>, Rect2, Vector2> bvh;
static void *_pair_callback(void *, uint32_t, GodotCollisionObject2D *, int, uint32_t, GodotCollisionObject2D *, int);
static void _unpair_callback(void *, uint32_t, GodotCollisionObject2D *, int, uint32_t, GodotCollisionObject2D *, int, void *);
PairCallback pair_callback = nullptr;
void *pair_userdata = nullptr;
UnpairCallback unpair_callback = nullptr;
void *unpair_userdata = nullptr;
public:
// 0 is an invalid ID
virtual ID create(GodotCollisionObject2D *p_object, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false) override;
virtual void move(ID p_id, const Rect2 &p_aabb) override;
virtual void set_static(ID p_id, bool p_static) override;
virtual void remove(ID p_id) override;
virtual GodotCollisionObject2D *get_object(ID p_id) const override;
virtual bool is_static(ID p_id) const override;
virtual int get_subindex(ID p_id) const override;
virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr) override;
virtual int cull_aabb(const Rect2 &p_aabb, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr) override;
virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata) override;
virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) override;
virtual void update() override;
static GodotBroadPhase2D *_create();
GodotBroadPhase2DBVH();
};
#endif // GODOT_BROAD_PHASE_2D_BVH_H

244
servers/physics_2d/godot_collision_object_2d.cpp
View File

@ -1,244 +0,0 @@
/**************************************************************************/
/* godot_collision_object_2d.cpp */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#include "godot_collision_object_2d.h"
#include "godot_physics_server_2d.h"
#include "godot_space_2d.h"
void GodotCollisionObject2D::add_shape(GodotShape2D *p_shape, const Transform2D &p_transform, bool p_disabled) {
Shape s;
s.shape = p_shape;
s.xform = p_transform;
s.xform_inv = s.xform.affine_inverse();
s.bpid = 0; //needs update
s.disabled = p_disabled;
s.one_way_collision = false;
s.one_way_collision_margin = 0;
shapes.push_back(s);
p_shape->add_owner(this);
if (!pending_shape_update_list.in_list()) {
GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
}
}
void GodotCollisionObject2D::set_shape(int p_index, GodotShape2D *p_shape) {
ERR_FAIL_INDEX(p_index, shapes.size());
shapes[p_index].shape->remove_owner(this);
shapes.write[p_index].shape = p_shape;
p_shape->add_owner(this);
if (!pending_shape_update_list.in_list()) {
GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
}
}
void GodotCollisionObject2D::set_shape_transform(int p_index, const Transform2D &p_transform) {
ERR_FAIL_INDEX(p_index, shapes.size());
shapes.write[p_index].xform = p_transform;
shapes.write[p_index].xform_inv = p_transform.affine_inverse();
if (!pending_shape_update_list.in_list()) {
GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
}
}
void GodotCollisionObject2D::set_shape_disabled(int p_idx, bool p_disabled) {
ERR_FAIL_INDEX(p_idx, shapes.size());
GodotCollisionObject2D::Shape &shape = shapes.write[p_idx];
if (shape.disabled == p_disabled) {
return;
}
shape.disabled = p_disabled;
if (!space) {
return;
}
if (p_disabled && shape.bpid != 0) {
space->get_broadphase()->remove(shape.bpid);
shape.bpid = 0;
if (!pending_shape_update_list.in_list()) {
GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
}
} else if (!p_disabled && shape.bpid == 0) {
if (!pending_shape_update_list.in_list()) {
GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
}
}
}
void GodotCollisionObject2D::remove_shape(GodotShape2D *p_shape) {
//remove a shape, all the times it appears
for (int i = 0; i < shapes.size(); i++) {
if (shapes[i].shape == p_shape) {
remove_shape(i);
i--;
}
}
}
void GodotCollisionObject2D::remove_shape(int p_index) {
//remove anything from shape to be erased to end, so subindices don't change
ERR_FAIL_INDEX(p_index, shapes.size());
for (int i = p_index; i < shapes.size(); i++) {
if (shapes[i].bpid == 0) {
continue;
}
//should never get here with a null owner
space->get_broadphase()->remove(shapes[i].bpid);
shapes.write[i].bpid = 0;
}
shapes[p_index].shape->remove_owner(this);
shapes.remove_at(p_index);
if (!pending_shape_update_list.in_list()) {
GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
}
// _update_shapes();
// _shapes_changed();
}
void GodotCollisionObject2D::_set_static(bool p_static) {
if (_static == p_static) {
return;
}
_static = p_static;
if (!space) {
return;
}
for (int i = 0; i < get_shape_count(); i++) {
const Shape &s = shapes[i];
if (s.bpid > 0) {
space->get_broadphase()->set_static(s.bpid, _static);
}
}
}
void GodotCollisionObject2D::_unregister_shapes() {
for (int i = 0; i < shapes.size(); i++) {
Shape &s = shapes.write[i];
if (s.bpid > 0) {
space->get_broadphase()->remove(s.bpid);
s.bpid = 0;
}
}
}
void GodotCollisionObject2D::_update_shapes() {
if (!space) {
return;
}
for (int i = 0; i < shapes.size(); i++) {
Shape &s = shapes.write[i];
if (s.disabled) {
continue;
}
//not quite correct, should compute the next matrix..
Rect2 shape_aabb = s.shape->get_aabb();
Transform2D xform = transform * s.xform;
shape_aabb = xform.xform(shape_aabb);
shape_aabb.grow_by((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05);
s.aabb_cache = shape_aabb;
if (s.bpid == 0) {
s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static);
space->get_broadphase()->set_static(s.bpid, _static);
}
space->get_broadphase()->move(s.bpid, shape_aabb);
}
}
void GodotCollisionObject2D::_update_shapes_with_motion(const Vector2 &p_motion) {
if (!space) {
return;
}
for (int i = 0; i < shapes.size(); i++) {
Shape &s = shapes.write[i];
if (s.disabled) {
continue;
}
//not quite correct, should compute the next matrix..
Rect2 shape_aabb = s.shape->get_aabb();
Transform2D xform = transform * s.xform;
shape_aabb = xform.xform(shape_aabb);
shape_aabb = shape_aabb.merge(Rect2(shape_aabb.position + p_motion, shape_aabb.size)); //use motion
s.aabb_cache = shape_aabb;
if (s.bpid == 0) {
s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static);
space->get_broadphase()->set_static(s.bpid, _static);
}
space->get_broadphase()->move(s.bpid, shape_aabb);
}
}
void GodotCollisionObject2D::_set_space(GodotSpace2D *p_space) {
GodotSpace2D *old_space = space;
space = p_space;
if (old_space) {
old_space->remove_object(this);
for (int i = 0; i < shapes.size(); i++) {
Shape &s = shapes.write[i];
if (s.bpid) {
old_space->get_broadphase()->remove(s.bpid);
s.bpid = 0;
}
}
}
if (space) {
space->add_object(this);
_update_shapes();
}
}
void GodotCollisionObject2D::_shape_changed() {
_update_shapes();
_shapes_changed();
}
GodotCollisionObject2D::GodotCollisionObject2D(Type p_type) :
pending_shape_update_list(this) {
type = p_type;
}

198
servers/physics_2d/godot_collision_object_2d.h
View File

@ -1,198 +0,0 @@
/**************************************************************************/
/* godot_collision_object_2d.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#ifndef GODOT_COLLISION_OBJECT_2D_H
#define GODOT_COLLISION_OBJECT_2D_H
#include "godot_broad_phase_2d.h"
#include "godot_shape_2d.h"
#include "core/templates/self_list.h"
#include "servers/physics_server_2d.h"
class GodotSpace2D;
class GodotCollisionObject2D : public GodotShapeOwner2D {
public:
enum Type {
TYPE_AREA,
TYPE_BODY
};
private:
Type type;
RID self;
ObjectID instance_id;
ObjectID canvas_instance_id;
bool pickable = true;
struct Shape {
Transform2D xform;
Transform2D xform_inv;
GodotBroadPhase2D::ID bpid = 0;
Rect2 aabb_cache; //for rayqueries
GodotShape2D *shape = nullptr;
bool disabled = false;
bool one_way_collision = false;
real_t one_way_collision_margin = 0.0;
};
Vector<Shape> shapes;
GodotSpace2D *space = nullptr;
Transform2D transform;
Transform2D inv_transform;
uint32_t collision_mask = 1;
uint32_t collision_layer = 1;
real_t collision_priority = 1.0;
bool _static = true;
SelfList<GodotCollisionObject2D> pending_shape_update_list;
void _update_shapes();
protected:
void _update_shapes_with_motion(const Vector2 &p_motion);
void _unregister_shapes();
_FORCE_INLINE_ void _set_transform(const Transform2D &p_transform, bool p_update_shapes = true) {
transform = p_transform;
if (p_update_shapes) {
_update_shapes();
}
}
_FORCE_INLINE_ void _set_inv_transform(const Transform2D &p_transform) { inv_transform = p_transform; }
void _set_static(bool p_static);
virtual void _shapes_changed() = 0;
void _set_space(GodotSpace2D *p_space);
GodotCollisionObject2D(Type p_type);
public:
_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
_FORCE_INLINE_ RID get_self() const { return self; }
_FORCE_INLINE_ void set_instance_id(const ObjectID &p_instance_id) { instance_id = p_instance_id; }
_FORCE_INLINE_ ObjectID get_instance_id() const { return instance_id; }
_FORCE_INLINE_ void set_canvas_instance_id(const ObjectID &p_canvas_instance_id) { canvas_instance_id = p_canvas_instance_id; }
_FORCE_INLINE_ ObjectID get_canvas_instance_id() const { return canvas_instance_id; }
void _shape_changed() override;
_FORCE_INLINE_ Type get_type() const { return type; }
void add_shape(GodotShape2D *p_shape, const Transform2D &p_transform = Transform2D(), bool p_disabled = false);
void set_shape(int p_index, GodotShape2D *p_shape);
void set_shape_transform(int p_index, const Transform2D &p_transform);
_FORCE_INLINE_ int get_shape_count() const { return shapes.size(); }
_FORCE_INLINE_ GodotShape2D *get_shape(int p_index) const {
CRASH_BAD_INDEX(p_index, shapes.size());
return shapes[p_index].shape;
}
_FORCE_INLINE_ const Transform2D &get_shape_transform(int p_index) const {
CRASH_BAD_INDEX(p_index, shapes.size());
return shapes[p_index].xform;
}
_FORCE_INLINE_ const Transform2D &get_shape_inv_transform(int p_index) const {
CRASH_BAD_INDEX(p_index, shapes.size());
return shapes[p_index].xform_inv;
}
_FORCE_INLINE_ const Rect2 &get_shape_aabb(int p_index) const {
CRASH_BAD_INDEX(p_index, shapes.size());
return shapes[p_index].aabb_cache;
}
_FORCE_INLINE_ const Transform2D &get_transform() const { return transform; }
_FORCE_INLINE_ const Transform2D &get_inv_transform() const { return inv_transform; }
_FORCE_INLINE_ GodotSpace2D *get_space() const { return space; }
void set_shape_disabled(int p_idx, bool p_disabled);
_FORCE_INLINE_ bool is_shape_disabled(int p_idx) const {
ERR_FAIL_INDEX_V(p_idx, shapes.size(), false);
return shapes[p_idx].disabled;
}
_FORCE_INLINE_ void set_shape_as_one_way_collision(int p_idx, bool p_one_way_collision, real_t p_margin) {
CRASH_BAD_INDEX(p_idx, shapes.size());
shapes.write[p_idx].one_way_collision = p_one_way_collision;
shapes.write[p_idx].one_way_collision_margin = p_margin;
}
_FORCE_INLINE_ bool is_shape_set_as_one_way_collision(int p_idx) const {
CRASH_BAD_INDEX(p_idx, shapes.size());
return shapes[p_idx].one_way_collision;
}
_FORCE_INLINE_ real_t get_shape_one_way_collision_margin(int p_idx) const {
CRASH_BAD_INDEX(p_idx, shapes.size());
return shapes[p_idx].one_way_collision_margin;
}
void set_collision_mask(uint32_t p_mask) {
collision_mask = p_mask;
_shape_changed();
}
_FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; }
void set_collision_layer(uint32_t p_layer) {
collision_layer = p_layer;
_shape_changed();
}
_FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; }
_FORCE_INLINE_ void set_collision_priority(real_t p_priority) {
ERR_FAIL_COND_MSG(p_priority <= 0, "Priority must be greater than 0.");
collision_priority = p_priority;
_shape_changed();
}
_FORCE_INLINE_ real_t get_collision_priority() const { return collision_priority; }
void remove_shape(GodotShape2D *p_shape) override;
void remove_shape(int p_index);
virtual void set_space(GodotSpace2D *p_space) = 0;
_FORCE_INLINE_ bool is_static() const { return _static; }
void set_pickable(bool p_pickable) { pickable = p_pickable; }
_FORCE_INLINE_ bool is_pickable() const { return pickable; }
_FORCE_INLINE_ bool collides_with(GodotCollisionObject2D *p_other) const {
return p_other->collision_layer & collision_mask;
}
_FORCE_INLINE_ bool interacts_with(const GodotCollisionObject2D *p_other) const {
return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask;
}
virtual ~GodotCollisionObject2D() {}
};
#endif // GODOT_COLLISION_OBJECT_2D_H

274
servers/physics_2d/godot_collision_solver_2d.cpp
View File

@ -1,274 +0,0 @@
/**************************************************************************/
/* godot_collision_solver_2d.cpp */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#include "godot_collision_solver_2d.h"
#include "godot_collision_solver_2d_sat.h"
#define collision_solver sat_2d_calculate_penetration
//#define collision_solver gjk_epa_calculate_penetration
bool GodotCollisionSolver2D::solve_static_world_boundary(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) {
const GodotWorldBoundaryShape2D *world_boundary = static_cast<const GodotWorldBoundaryShape2D *>(p_shape_A);
if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) {
return false;
}
Vector2 n = p_transform_A.basis_xform(world_boundary->get_normal()).normalized();
Vector2 p = p_transform_A.xform(world_boundary->get_normal() * world_boundary->get_d());
real_t d = n.dot(p);
Vector2 supports[2];
int support_count;
p_shape_B->get_supports(p_transform_B.affine_inverse().basis_xform(-n).normalized(), supports, support_count);
bool found = false;
for (int i = 0; i < support_count; i++) {
supports[i] += p_margin * supports[i].normalized();
supports[i] = p_transform_B.xform(supports[i]);
supports[i] += p_motion_B;
real_t pd = n.dot(supports[i]);
if (pd >= d) {
continue;
}
found = true;
Vector2 support_A = supports[i] - n * (pd - d);
if (p_result_callback) {
if (p_swap_result) {
p_result_callback(supports[i], support_A, p_userdata);
} else {
p_result_callback(support_A, supports[i], p_userdata);
}
}
}
return found;
}
bool GodotCollisionSolver2D::solve_separation_ray(const GodotShape2D *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis, real_t p_margin) {
const GodotSeparationRayShape2D *ray = static_cast<const GodotSeparationRayShape2D *>(p_shape_A);
if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY) {
return false;
}
Vector2 from = p_transform_A.get_origin();
Vector2 to = from + p_transform_A[1] * (ray->get_length() + p_margin);
if (p_motion_A != Vector2()) {
//not the best but should be enough
Vector2 normal = (to - from).normalized();
to += normal * MAX(0.0, normal.dot(p_motion_A));
}
Vector2 support_A = to;
Transform2D invb = p_transform_B.affine_inverse();
from = invb.xform(from);
to = invb.xform(to);
Vector2 p, n;
if (!p_shape_B->intersect_segment(from, to, p, n)) {
if (r_sep_axis) {
*r_sep_axis = p_transform_A[1].normalized();
}
return false;
}
// Discard contacts when the ray is fully contained inside the shape.
if (n == Vector2()) {
if (r_sep_axis) {
*r_sep_axis = p_transform_A[1].normalized();
}
return false;
}
// Discard contacts in the wrong direction.
if (n.dot(from - to) < CMP_EPSILON) {
if (r_sep_axis) {
*r_sep_axis = p_transform_A[1].normalized();
}
return false;
}
Vector2 support_B = p_transform_B.xform(p);
if (ray->get_slide_on_slope()) {
Vector2 global_n = invb.basis_xform_inv(n).normalized();
support_B = support_A + (support_B - support_A).length() * global_n;
}
if (p_result_callback) {
if (p_swap_result) {
p_result_callback(support_B, support_A, p_userdata);
} else {
p_result_callback(support_A, support_B, p_userdata);
}
}
return true;
}
struct _ConcaveCollisionInfo2D {
const Transform2D *transform_A = nullptr;
const GodotShape2D *shape_A = nullptr;
const Transform2D *transform_B = nullptr;
Vector2 motion_A;
Vector2 motion_B;
real_t margin_A = 0.0;
real_t margin_B = 0.0;
GodotCollisionSolver2D::CallbackResult result_callback = nullptr;
void *userdata = nullptr;
bool swap_result = false;
bool collided = false;
int aabb_tests = 0;
int collisions = 0;
Vector2 *sep_axis = nullptr;
};
bool GodotCollisionSolver2D::concave_callback(void *p_userdata, GodotShape2D *p_convex) {
_ConcaveCollisionInfo2D &cinfo = *(static_cast<_ConcaveCollisionInfo2D *>(p_userdata));
cinfo.aabb_tests++;
bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex, *cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, cinfo.sep_axis, cinfo.margin_A, cinfo.margin_B);
if (!collided) {
return false;
}
cinfo.collided = true;
cinfo.collisions++;
// Stop at first collision if contacts are not needed.
return !cinfo.result_callback;
}
bool GodotCollisionSolver2D::solve_concave(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) {
const GodotConcaveShape2D *concave_B = static_cast<const GodotConcaveShape2D *>(p_shape_B);
_ConcaveCollisionInfo2D cinfo;
cinfo.transform_A = &p_transform_A;
cinfo.shape_A = p_shape_A;
cinfo.transform_B = &p_transform_B;
cinfo.motion_A = p_motion_A;
cinfo.result_callback = p_result_callback;
cinfo.userdata = p_userdata;
cinfo.swap_result = p_swap_result;
cinfo.collided = false;
cinfo.collisions = 0;
cinfo.sep_axis = r_sep_axis;
cinfo.margin_A = p_margin_A;
cinfo.margin_B = p_margin_B;
cinfo.aabb_tests = 0;
Transform2D rel_transform = p_transform_A;
rel_transform.columns[2] -= p_transform_B.get_origin();
// Quickly compute a local Rect2.
Rect2 local_aabb;
for (int i = 0; i < 2; i++) {
Vector2 axis(p_transform_B.columns[i]);
real_t axis_scale = 1.0 / axis.length();
axis *= axis_scale;
real_t smin = 0.0, smax = 0.0;
p_shape_A->project_rangev(axis, rel_transform, smin, smax);
smin *= axis_scale;
smax *= axis_scale;
local_aabb.position[i] = smin;
local_aabb.size[i] = smax - smin;
}
// In case of motion, expand the Rect2 in the motion direction.
if (p_motion_A != Vector2()) {
Rect2 moved_aabb = local_aabb;
moved_aabb.position += p_motion_A;
local_aabb = local_aabb.merge(moved_aabb);
}
concave_B->cull(local_aabb, concave_callback, &cinfo);
return cinfo.collided;
}
bool GodotCollisionSolver2D::solve(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) {
PhysicsServer2D::ShapeType type_A = p_shape_A->get_type();
PhysicsServer2D::ShapeType type_B = p_shape_B->get_type();
bool concave_A = p_shape_A->is_concave();
bool concave_B = p_shape_B->is_concave();
real_t margin_A = p_margin_A, margin_B = p_margin_B;
bool swap = false;
if (type_A > type_B) {
SWAP(type_A, type_B);
SWAP(concave_A, concave_B);
SWAP(margin_A, margin_B);
swap = true;
}
if (type_A == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) {
if (type_B == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) {
WARN_PRINT_ONCE("Collisions between world boundaries are not supported.");
return false;
}
if (swap) {
return solve_static_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_motion_A, p_result_callback, p_userdata, true, p_margin_A);
} else {
return solve_static_world_boundary(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, p_margin_B);
}
} else if (type_A == PhysicsServer2D::SHAPE_SEPARATION_RAY) {
if (type_B == PhysicsServer2D::SHAPE_SEPARATION_RAY) {
WARN_PRINT_ONCE("Collisions between two rays are not supported.");
return false; //no ray-ray
}
if (swap) {
return solve_separation_ray(p_shape_B, p_motion_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, r_sep_axis, p_margin_B);
} else {
return solve_separation_ray(p_shape_A, p_motion_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, r_sep_axis, p_margin_A);
}
} else if (concave_B) {
if (concave_A) {
WARN_PRINT_ONCE("Collisions between two concave shapes are not supported.");
return false;
}
if (!swap) {
return solve_concave(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, r_sep_axis, margin_A, margin_B);
} else {
return solve_concave(p_shape_B, p_transform_B, p_motion_B, p_shape_A, p_transform_A, p_motion_A, p_result_callback, p_userdata, true, r_sep_axis, margin_A, margin_B);
}
} else {
return collision_solver(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, r_sep_axis, margin_A, margin_B);
}
}

50
servers/physics_2d/godot_collision_solver_2d.h
View File

@ -1,50 +0,0 @@
/**************************************************************************/
/* godot_collision_solver_2d.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#ifndef GODOT_COLLISION_SOLVER_2D_H
#define GODOT_COLLISION_SOLVER_2D_H
#include "godot_shape_2d.h"
class GodotCollisionSolver2D {
public:
typedef void (*CallbackResult)(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata);
private:
static bool solve_static_world_boundary(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0);
static bool concave_callback(void *p_userdata, GodotShape2D *p_convex);
static bool solve_concave(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0);
static bool solve_separation_ray(const GodotShape2D *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis = nullptr, real_t p_margin = 0);
public:
static bool solve(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0);
};
#endif // GODOT_COLLISION_SOLVER_2D_H

1404
servers/physics_2d/godot_collision_solver_2d_sat.cpp
View File

File diff suppressed because it is too large Load Diff

38
servers/physics_2d/godot_collision_solver_2d_sat.h
View File

@ -1,38 +0,0 @@
/**************************************************************************/
/* godot_collision_solver_2d_sat.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#ifndef GODOT_COLLISION_SOLVER_2D_SAT_H
#define GODOT_COLLISION_SOLVER_2D_SAT_H
#include "godot_collision_solver_2d.h"
bool sat_2d_calculate_penetration(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, GodotCollisionSolver2D::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector2 *sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0);
#endif // GODOT_COLLISION_SOLVER_2D_SAT_H

70
servers/physics_2d/godot_constraint_2d.h
View File

@ -1,70 +0,0 @@
/**************************************************************************/
/* godot_constraint_2d.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#ifndef GODOT_CONSTRAINT_2D_H
#define GODOT_CONSTRAINT_2D_H
#include "godot_body_2d.h"
class GodotConstraint2D {
GodotBody2D **_body_ptr;
int _body_count;
uint64_t island_step = 0;
bool disabled_collisions_between_bodies = true;
RID self;
protected:
GodotConstraint2D(GodotBody2D **p_body_ptr = nullptr, int p_body_count = 0) {
_body_ptr = p_body_ptr;
_body_count = p_body_count;
}
public:
_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
_FORCE_INLINE_ RID get_self() const { return self; }
_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; }
_FORCE_INLINE_ GodotBody2D **get_body_ptr() const { return _body_ptr; }
_FORCE_INLINE_ int get_body_count() const { return _body_count; }
_FORCE_INLINE_ void disable_collisions_between_bodies(const bool p_disabled) { disabled_collisions_between_bodies = p_disabled; }
_FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; }
virtual bool setup(real_t p_step) = 0;
virtual bool pre_solve(real_t p_step) = 0;
virtual void solve(real_t p_step) = 0;
virtual ~GodotConstraint2D() {}
};
#endif // GODOT_CONSTRAINT_2D_H

595
servers/physics_2d/godot_joints_2d.cpp
View File

@ -1,595 +0,0 @@
/**************************************************************************/
/* godot_joints_2d.cpp */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#include "godot_joints_2d.h"
#include "godot_space_2d.h"
//based on chipmunk joint constraints
/* Copyright (c) 2007 Scott Lembcke
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
void GodotJoint2D::copy_settings_from(GodotJoint2D *p_joint) {
set_self(p_joint->get_self());
set_max_force(p_joint->get_max_force());
set_bias(p_joint->get_bias());
set_max_bias(p_joint->get_max_bias());
disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies());
}
static inline real_t k_scalar(GodotBody2D *a, GodotBody2D *b, const Vector2 &rA, const Vector2 &rB, const Vector2 &n) {
real_t value = 0.0;
{
value += a->get_inv_mass();
real_t rcn = (rA - a->get_center_of_mass()).cross(n);
value += a->get_inv_inertia() * rcn * rcn;
}
if (b) {
value += b->get_inv_mass();
real_t rcn = (rB - b->get_center_of_mass()).cross(n);
value += b->get_inv_inertia() * rcn * rcn;
}
return value;
}
static inline Vector2
relative_velocity(GodotBody2D *a, GodotBody2D *b, Vector2 rA, Vector2 rB) {
Vector2 sum = a->get_linear_velocity() - (rA - a->get_center_of_mass()).orthogonal() * a->get_angular_velocity();
if (b) {
return (b->get_linear_velocity() - (rB - b->get_center_of_mass()).orthogonal() * b->get_angular_velocity()) - sum;
} else {
return -sum;
}
}
static inline real_t
normal_relative_velocity(GodotBody2D *a, GodotBody2D *b, Vector2 rA, Vector2 rB, Vector2 n) {
return relative_velocity(a, b, rA, rB).dot(n);
}
bool GodotPinJoint2D::setup(real_t p_step) {
dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
if (!dynamic_A && !dynamic_B) {
return false;
}
GodotSpace2D *space = A->get_space();
ERR_FAIL_NULL_V(space, false);
rA = A->get_transform().basis_xform(anchor_A);
rB = B ? B->get_transform().basis_xform(anchor_B) : anchor_B;
real_t B_inv_mass = B ? B->get_inv_mass() : 0.0;
Transform2D K1;
K1[0].x = A->get_inv_mass() + B_inv_mass;
K1[1].x = 0.0f;
K1[0].y = 0.0f;
K1[1].y = A->get_inv_mass() + B_inv_mass;
Vector2 r1 = rA - A->get_center_of_mass();
Transform2D K2;
K2[0].x = A->get_inv_inertia() * r1.y * r1.y;
K2[1].x = -A->get_inv_inertia() * r1.x * r1.y;
K2[0].y = -A->get_inv_inertia() * r1.x * r1.y;
K2[1].y = A->get_inv_inertia() * r1.x * r1.x;
Transform2D K;
K[0] = K1[0] + K2[0];
K[1] = K1[1] + K2[1];
if (B) {
Vector2 r2 = rB - B->get_center_of_mass();
Transform2D K3;
K3[0].x = B->get_inv_inertia() * r2.y * r2.y;
K3[1].x = -B->get_inv_inertia() * r2.x * r2.y;
K3[0].y = -B->get_inv_inertia() * r2.x * r2.y;
K3[1].y = B->get_inv_inertia() * r2.x * r2.x;
K[0] += K3[0];
K[1] += K3[1];
}
K[0].x += softness;
K[1].y += softness;
M = K.affine_inverse();
Vector2 gA = rA + A->get_transform().get_origin();
Vector2 gB = B ? rB + B->get_transform().get_origin() : rB;
Vector2 delta = gB - gA;
bias = delta * -(get_bias() == 0 ? space->get_constraint_bias() : get_bias()) * (1.0 / p_step);
// Compute max impulse.
jn_max = get_max_force() * p_step;
return true;
}
inline Vector2 custom_cross(const Vector2 &p_vec, real_t p_other) {
return Vector2(p_other * p_vec.y, -p_other * p_vec.x);
}
bool GodotPinJoint2D::pre_solve(real_t p_step) {
// Apply accumulated impulse.
if (dynamic_A) {
A->apply_impulse(-P, rA);
}
if (B && dynamic_B) {
B->apply_impulse(P, rB);
}
// Angle limits joint pre_solve step taken from https://github.com/slembcke/Chipmunk2D/blob/d0239ef4599b3688a5a336373f7d0a68426414ba/src/cpRotaryLimitJoint.c
real_t i_sum_local = A->get_inv_inertia();
if (B) {
i_sum_local += B->get_inv_inertia();
}
i_sum = 1.0 / (i_sum_local);
if (angular_limit_enabled && B) {
Vector2 diff_vector = B->get_transform().get_origin() - A->get_transform().get_origin();
diff_vector = diff_vector.rotated(-initial_angle);
real_t dist = diff_vector.angle();
real_t pdist = 0.0;
if (dist > angular_limit_upper) {
pdist = dist - angular_limit_upper;
} else if (dist < angular_limit_lower) {
pdist = dist - angular_limit_lower;
}
real_t error_bias = Math::pow(1.0 - 0.15, 60.0);
// Calculate bias velocity.
bias_velocity = -CLAMP((-1.0 - Math::pow(error_bias, p_step)) * pdist / p_step, -get_max_bias(), get_max_bias());
// If the bias velocity is 0, the joint is not at a limit.
if (bias_velocity >= -CMP_EPSILON && bias_velocity <= CMP_EPSILON) {
j_acc = 0;
is_joint_at_limit = false;
} else {
is_joint_at_limit = true;
}
} else {
bias_velocity = 0.0;
}
return true;
}
void GodotPinJoint2D::solve(real_t p_step) {
// Compute relative velocity.
Vector2 vA = A->get_linear_velocity() - custom_cross(rA - A->get_center_of_mass(), A->get_angular_velocity());
Vector2 rel_vel;
if (B) {
rel_vel = B->get_linear_velocity() - custom_cross(rB - B->get_center_of_mass(), B->get_angular_velocity()) - vA;
} else {
rel_vel = -vA;
}
// Angle limits joint solve step taken from https://github.com/slembcke/Chipmunk2D/blob/d0239ef4599b3688a5a336373f7d0a68426414ba/src/cpRotaryLimitJoint.c
if ((angular_limit_enabled || motor_enabled) && B) {
// Compute relative rotational velocity.
real_t wr = B->get_angular_velocity() - A->get_angular_velocity();
// Motor solve part taken from https://github.com/slembcke/Chipmunk2D/blob/d0239ef4599b3688a5a336373f7d0a68426414ba/src/cpSimpleMotor.c
if (motor_enabled) {
wr -= motor_target_velocity;
}
real_t j_max = jn_max;
// Compute normal impulse.
real_t j = -(bias_velocity + wr) * i_sum;
real_t j_old = j_acc;
// Only enable the limits if we have to.
if (angular_limit_enabled && is_joint_at_limit) {
if (bias_velocity < 0.0) {
j_acc = CLAMP(j_old + j, 0.0, j_max);
} else {
j_acc = CLAMP(j_old + j, -j_max, 0.0);
}
} else {
j_acc = CLAMP(j_old + j, -j_max, j_max);
}
j = j_acc - j_old;
A->apply_torque_impulse(-j * A->get_inv_inertia());
B->apply_torque_impulse(j * B->get_inv_inertia());
}
Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness, softness) * P);
if (dynamic_A) {
A->apply_impulse(-impulse, rA);
}
if (B && dynamic_B) {
B->apply_impulse(impulse, rB);
}
P += impulse;
}
void GodotPinJoint2D::set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value) {
switch (p_param) {
case PhysicsServer2D::PIN_JOINT_SOFTNESS: {
softness = p_value;
} break;
case PhysicsServer2D::PIN_JOINT_LIMIT_UPPER: {
angular_limit_upper = p_value;
} break;
case PhysicsServer2D::PIN_JOINT_LIMIT_LOWER: {
angular_limit_lower = p_value;
} break;
case PhysicsServer2D::PIN_JOINT_MOTOR_TARGET_VELOCITY: {
motor_target_velocity = p_value;
} break;
}
}
real_t GodotPinJoint2D::get_param(PhysicsServer2D::PinJointParam p_param) const {
switch (p_param) {
case PhysicsServer2D::PIN_JOINT_SOFTNESS: {
return softness;
}
case PhysicsServer2D::PIN_JOINT_LIMIT_UPPER: {
return angular_limit_upper;
}
case PhysicsServer2D::PIN_JOINT_LIMIT_LOWER: {
return angular_limit_lower;
}
case PhysicsServer2D::PIN_JOINT_MOTOR_TARGET_VELOCITY: {
return motor_target_velocity;
}
}
ERR_FAIL_V(0);
}
void GodotPinJoint2D::set_flag(PhysicsServer2D::PinJointFlag p_flag, bool p_enabled) {
switch (p_flag) {
case PhysicsServer2D::PIN_JOINT_FLAG_ANGULAR_LIMIT_ENABLED: {
angular_limit_enabled = p_enabled;
} break;
case PhysicsServer2D::PIN_JOINT_FLAG_MOTOR_ENABLED: {
motor_enabled = p_enabled;
} break;
}
}
bool GodotPinJoint2D::get_flag(PhysicsServer2D::PinJointFlag p_flag) const {
switch (p_flag) {
case PhysicsServer2D::PIN_JOINT_FLAG_ANGULAR_LIMIT_ENABLED: {
return angular_limit_enabled;
}
case PhysicsServer2D::PIN_JOINT_FLAG_MOTOR_ENABLED: {
return motor_enabled;
}
}
ERR_FAIL_V(0);
}
GodotPinJoint2D::GodotPinJoint2D(const Vector2 &p_pos, GodotBody2D *p_body_a, GodotBody2D *p_body_b) :
GodotJoint2D(_arr, p_body_b ? 2 : 1) {
A = p_body_a;
B = p_body_b;
anchor_A = p_body_a->get_inv_transform().xform(p_pos);
anchor_B = p_body_b ? p_body_b->get_inv_transform().xform(p_pos) : p_pos;
p_body_a->add_constraint(this, 0);
if (p_body_b) {
p_body_b->add_constraint(this, 1);
initial_angle = A->get_transform().get_origin().angle_to_point(B->get_transform().get_origin());
}
}
//////////////////////////////////////////////
//////////////////////////////////////////////
//////////////////////////////////////////////
static inline void
k_tensor(GodotBody2D *a, GodotBody2D *b, Vector2 r1, Vector2 r2, Vector2 *k1, Vector2 *k2) {
// calculate mass matrix
// If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross...
real_t k11, k12, k21, k22;
real_t m_sum = a->get_inv_mass() + b->get_inv_mass();
// start with I*m_sum
k11 = m_sum;
k12 = 0.0f;
k21 = 0.0f;
k22 = m_sum;
r1 -= a->get_center_of_mass();
r2 -= b->get_center_of_mass();
// add the influence from r1
real_t a_i_inv = a->get_inv_inertia();
real_t r1xsq = r1.x * r1.x * a_i_inv;
real_t r1ysq = r1.y * r1.y * a_i_inv;
real_t r1nxy = -r1.x * r1.y * a_i_inv;
k11 += r1ysq;
k12 += r1nxy;
k21 += r1nxy;
k22 += r1xsq;
// add the influnce from r2
real_t b_i_inv = b->get_inv_inertia();
real_t r2xsq = r2.x * r2.x * b_i_inv;
real_t r2ysq = r2.y * r2.y * b_i_inv;
real_t r2nxy = -r2.x * r2.y * b_i_inv;
k11 += r2ysq;
k12 += r2nxy;
k21 += r2nxy;
k22 += r2xsq;
// invert
real_t determinant = k11 * k22 - k12 * k21;
ERR_FAIL_COND(determinant == 0.0);
real_t det_inv = 1.0f / determinant;
*k1 = Vector2(k22 * det_inv, -k12 * det_inv);
*k2 = Vector2(-k21 * det_inv, k11 * det_inv);
}
static _FORCE_INLINE_ Vector2
mult_k(const Vector2 &vr, const Vector2 &k1, const Vector2 &k2) {
return Vector2(vr.dot(k1), vr.dot(k2));
}
bool GodotGrooveJoint2D::setup(real_t p_step) {
dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
if (!dynamic_A && !dynamic_B) {
return false;
}
GodotSpace2D *space = A->get_space();
ERR_FAIL_NULL_V(space, false);
// calculate endpoints in worldspace
Vector2 ta = A->get_transform().xform(A_groove_1);
Vector2 tb = A->get_transform().xform(A_groove_2);
// calculate axis
Vector2 n = -(tb - ta).orthogonal().normalized();
real_t d = ta.dot(n);
xf_normal = n;
rB = B->get_transform().basis_xform(B_anchor);
// calculate tangential distance along the axis of rB
real_t td = (B->get_transform().get_origin() + rB).cross(n);
// calculate clamping factor and rB
if (td <= ta.cross(n)) {
clamp = 1.0f;
rA = ta - A->get_transform().get_origin();
} else if (td >= tb.cross(n)) {
clamp = -1.0f;
rA = tb - A->get_transform().get_origin();
} else {
clamp = 0.0f;
//joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p);
rA = ((-n.orthogonal() * -td) + n * d) - A->get_transform().get_origin();
}
// Calculate mass tensor
k_tensor(A, B, rA, rB, &k1, &k2);
// compute max impulse
jn_max = get_max_force() * p_step;
// calculate bias velocity
//cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
//joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias);
Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA);
real_t _b = get_bias();
gbias = (delta * -(_b == 0 ? space->get_constraint_bias() : _b) * (1.0 / p_step)).limit_length(get_max_bias());
correct = true;
return true;
}
bool GodotGrooveJoint2D::pre_solve(real_t p_step) {
// Apply accumulated impulse.
if (dynamic_A) {
A->apply_impulse(-jn_acc, rA);
}
if (dynamic_B) {
B->apply_impulse(jn_acc, rB);
}
return true;
}
void GodotGrooveJoint2D::solve(real_t p_step) {
// compute impulse
Vector2 vr = relative_velocity(A, B, rA, rB);
Vector2 j = mult_k(gbias - vr, k1, k2);
Vector2 jOld = jn_acc;
j += jOld;
jn_acc = (((clamp * j.cross(xf_normal)) > 0) ? j : j.project(xf_normal)).limit_length(jn_max);
j = jn_acc - jOld;
if (dynamic_A) {
A->apply_impulse(-j, rA);
}
if (dynamic_B) {
B->apply_impulse(j, rB);
}
}
GodotGrooveJoint2D::GodotGrooveJoint2D(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, GodotBody2D *p_body_a, GodotBody2D *p_body_b) :
GodotJoint2D(_arr, 2) {
A = p_body_a;
B = p_body_b;
A_groove_1 = A->get_inv_transform().xform(p_a_groove1);
A_groove_2 = A->get_inv_transform().xform(p_a_groove2);
B_anchor = B->get_inv_transform().xform(p_b_anchor);
A_groove_normal = -(A_groove_2 - A_groove_1).normalized().orthogonal();
A->add_constraint(this, 0);
B->add_constraint(this, 1);
}
//////////////////////////////////////////////
//////////////////////////////////////////////
//////////////////////////////////////////////
bool GodotDampedSpringJoint2D::setup(real_t p_step) {
dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
if (!dynamic_A && !dynamic_B) {
return false;
}
rA = A->get_transform().basis_xform(anchor_A);
rB = B->get_transform().basis_xform(anchor_B);
Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA);
real_t dist = delta.length();
if (dist) {
n = delta / dist;
} else {
n = Vector2();
}
real_t k = k_scalar(A, B, rA, rB, n);
n_mass = 1.0f / k;
target_vrn = 0.0f;
v_coef = 1.0f - Math::exp(-damping * (p_step)*k);
// Calculate spring force.
real_t f_spring = (rest_length - dist) * stiffness;
j = n * f_spring * (p_step);
return true;
}
bool GodotDampedSpringJoint2D::pre_solve(real_t p_step) {
// Apply spring force.
if (dynamic_A) {
A->apply_impulse(-j, rA);
}
if (dynamic_B) {
B->apply_impulse(j, rB);
}
return true;
}
void GodotDampedSpringJoint2D::solve(real_t p_step) {
// compute relative velocity
real_t vrn = normal_relative_velocity(A, B, rA, rB, n) - target_vrn;
// compute velocity loss from drag
// not 100% certain this is derived correctly, though it makes sense
real_t v_damp = -vrn * v_coef;
target_vrn = vrn + v_damp;
Vector2 j_new = n * v_damp * n_mass;
if (dynamic_A) {
A->apply_impulse(-j_new, rA);
}
if (dynamic_B) {
B->apply_impulse(j_new, rB);
}
}
void GodotDampedSpringJoint2D::set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value) {
switch (p_param) {
case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: {
rest_length = p_value;
} break;
case PhysicsServer2D::DAMPED_SPRING_DAMPING: {
damping = p_value;
} break;
case PhysicsServer2D::DAMPED_SPRING_STIFFNESS: {
stiffness = p_value;
} break;
}
}
real_t GodotDampedSpringJoint2D::get_param(PhysicsServer2D::DampedSpringParam p_param) const {
switch (p_param) {
case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: {
return rest_length;
} break;
case PhysicsServer2D::DAMPED_SPRING_DAMPING: {
return damping;
} break;
case PhysicsServer2D::DAMPED_SPRING_STIFFNESS: {
return stiffness;
} break;
}
ERR_FAIL_V(0);
}
GodotDampedSpringJoint2D::GodotDampedSpringJoint2D(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, GodotBody2D *p_body_a, GodotBody2D *p_body_b) :
GodotJoint2D(_arr, 2) {
A = p_body_a;
B = p_body_b;
anchor_A = A->get_inv_transform().xform(p_anchor_a);
anchor_B = B->get_inv_transform().xform(p_anchor_b);
rest_length = p_anchor_a.distance_to(p_anchor_b);
A->add_constraint(this, 0);
B->add_constraint(this, 1);
}

192
servers/physics_2d/godot_joints_2d.h
View File

@ -1,192 +0,0 @@
/**************************************************************************/
/* godot_joints_2d.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#ifndef GODOT_JOINTS_2D_H
#define GODOT_JOINTS_2D_H
#include "godot_body_2d.h"
#include "godot_constraint_2d.h"
class GodotJoint2D : public GodotConstraint2D {
real_t bias = 0;
real_t max_bias = 3.40282e+38;
real_t max_force = 3.40282e+38;
protected:
bool dynamic_A = false;
bool dynamic_B = false;
public:
_FORCE_INLINE_ void set_max_force(real_t p_force) { max_force = p_force; }
_FORCE_INLINE_ real_t get_max_force() const { return max_force; }
_FORCE_INLINE_ void set_bias(real_t p_bias) { bias = p_bias; }
_FORCE_INLINE_ real_t get_bias() const { return bias; }
_FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias = p_bias; }
_FORCE_INLINE_ real_t get_max_bias() const { return max_bias; }
virtual bool setup(real_t p_step) override { return false; }
virtual bool pre_solve(real_t p_step) override { return false; }
virtual void solve(real_t p_step) override {}
void copy_settings_from(GodotJoint2D *p_joint);
virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_MAX; }
GodotJoint2D(GodotBody2D **p_body_ptr = nullptr, int p_body_count = 0) :
GodotConstraint2D(p_body_ptr, p_body_count) {}
virtual ~GodotJoint2D() {
for (int i = 0; i < get_body_count(); i++) {
GodotBody2D *body = get_body_ptr()[i];
if (body) {
body->remove_constraint(this, i);
}
}
};
};
class GodotPinJoint2D : public GodotJoint2D {
union {
struct {
GodotBody2D *A;
GodotBody2D *B;
};
GodotBody2D *_arr[2] = { nullptr, nullptr };
};
Transform2D M;
Vector2 rA, rB;
Vector2 anchor_A;
Vector2 anchor_B;
Vector2 bias;
real_t initial_angle = 0.0;
real_t bias_velocity = 0.0;
real_t jn_max = 0.0;
real_t j_acc = 0.0;
real_t i_sum = 0.0;
Vector2 P;
real_t softness = 0.0;
real_t angular_limit_lower = 0.0;
real_t angular_limit_upper = 0.0;
real_t motor_target_velocity = 0.0;
bool is_joint_at_limit = false;
bool motor_enabled = false;
bool angular_limit_enabled = false;
public:
virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_PIN; }
virtual bool setup(real_t p_step) override;
virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
void set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value);
real_t get_param(PhysicsServer2D::PinJointParam p_param) const;
void set_flag(PhysicsServer2D::PinJointFlag p_flag, bool p_enabled);
bool get_flag(PhysicsServer2D::PinJointFlag p_flag) const;
GodotPinJoint2D(const Vector2 &p_pos, GodotBody2D *p_body_a, GodotBody2D *p_body_b = nullptr);
};
class GodotGrooveJoint2D : public GodotJoint2D {
union {
struct {
GodotBody2D *A;
GodotBody2D *B;
};
GodotBody2D *_arr[2] = { nullptr, nullptr };
};
Vector2 A_groove_1;
Vector2 A_groove_2;
Vector2 A_groove_normal;
Vector2 B_anchor;
Vector2 jn_acc;
Vector2 gbias;
real_t jn_max = 0.0;
real_t clamp = 0.0;
Vector2 xf_normal;
Vector2 rA, rB;
Vector2 k1, k2;
bool correct = false;
public:
virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_GROOVE; }
virtual bool setup(real_t p_step) override;
virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
GodotGrooveJoint2D(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, GodotBody2D *p_body_a, GodotBody2D *p_body_b);
};
class GodotDampedSpringJoint2D : public GodotJoint2D {
union {
struct {
GodotBody2D *A;
GodotBody2D *B;
};
GodotBody2D *_arr[2] = { nullptr, nullptr };
};
Vector2 anchor_A;
Vector2 anchor_B;
real_t rest_length = 0.0;
real_t damping = 1.5;
real_t stiffness = 20.0;
Vector2 rA, rB;
Vector2 n;
Vector2 j;
real_t n_mass = 0.0;
real_t target_vrn = 0.0;
real_t v_coef = 0.0;
public:
virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_DAMPED_SPRING; }
virtual bool setup(real_t p_step) override;
virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
void set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value);
real_t get_param(PhysicsServer2D::DampedSpringParam p_param) const;
GodotDampedSpringJoint2D(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, GodotBody2D *p_body_a, GodotBody2D *p_body_b);
};
#endif // GODOT_JOINTS_2D_H

1400
servers/physics_2d/godot_physics_server_2d.cpp
View File

File diff suppressed because it is too large Load Diff

307
servers/physics_2d/godot_physics_server_2d.h
View File

@ -1,307 +0,0 @@
/**************************************************************************/
/* godot_physics_server_2d.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#ifndef GODOT_PHYSICS_SERVER_2D_H
#define GODOT_PHYSICS_SERVER_2D_H
#include "godot_joints_2d.h"
#include "godot_shape_2d.h"
#include "godot_space_2d.h"
#include "godot_step_2d.h"
#include "core/templates/rid_owner.h"
#include "servers/physics_server_2d.h"
class GodotPhysicsServer2D : public PhysicsServer2D {
GDCLASS(GodotPhysicsServer2D, PhysicsServer2D);
friend class GodotPhysicsDirectSpaceState2D;
friend class GodotPhysicsDirectBodyState2D;
bool active = true;
bool doing_sync = false;
int island_count = 0;
int active_objects = 0;
int collision_pairs = 0;
bool using_threads = false;
bool flushing_queries = false;
GodotStep2D *stepper = nullptr;
HashSet<const GodotSpace2D *> active_spaces;
mutable RID_PtrOwner<GodotShape2D, true> shape_owner;
mutable RID_PtrOwner<GodotSpace2D, true> space_owner;
mutable RID_PtrOwner<GodotArea2D, true> area_owner;
mutable RID_PtrOwner<GodotBody2D, true> body_owner;
mutable RID_PtrOwner<GodotJoint2D, true> joint_owner;
static GodotPhysicsServer2D *godot_singleton;
friend class GodotCollisionObject2D;
SelfList<GodotCollisionObject2D>::List pending_shape_update_list;
void _update_shapes();
RID _shape_create(ShapeType p_shape);
public:
struct CollCbkData {
Vector2 valid_dir;
real_t valid_depth = 0.0;
int max = 0;
int amount = 0;
int passed = 0;
int invalid_by_dir = 0;
Vector2 *ptr = nullptr;
};
virtual RID world_boundary_shape_create() override;
virtual RID separation_ray_shape_create() override;
virtual RID segment_shape_create() override;
virtual RID circle_shape_create() override;
virtual RID rectangle_shape_create() override;
virtual RID capsule_shape_create() override;
virtual RID convex_polygon_shape_create() override;
virtual RID concave_polygon_shape_create() override;
static void _shape_col_cbk(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata);
virtual void shape_set_data(RID p_shape, const Variant &p_data) override;
virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias) override;
virtual ShapeType shape_get_type(RID p_shape) const override;
virtual Variant shape_get_data(RID p_shape) const override;
virtual real_t shape_get_custom_solver_bias(RID p_shape) const override;
virtual bool shape_collide(RID p_shape_A, const Transform2D &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Transform2D &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) override;
/* SPACE API */
virtual RID space_create() override;
virtual void space_set_active(RID p_space, bool p_active) override;
virtual bool space_is_active(RID p_space) const override;
virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) override;
virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const override;
virtual void space_set_debug_contacts(RID p_space, int p_max_contacts) override;
virtual Vector<Vector2> space_get_contacts(RID p_space) const override;
virtual int space_get_contact_count(RID p_space) const override;
// this function only works on physics process, errors and returns null otherwise
virtual PhysicsDirectSpaceState2D *space_get_direct_state(RID p_space) override;
/* AREA API */
virtual RID area_create() override;
virtual void area_set_space(RID p_area, RID p_space) override;
virtual RID area_get_space(RID p_area) const override;
virtual void area_add_shape(RID p_area, RID p_shape, const Transform2D &p_transform = Transform2D(), bool p_disabled = false) override;
virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape) override;
virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform2D &p_transform) override;
virtual int area_get_shape_count(RID p_area) const override;
virtual RID area_get_shape(RID p_area, int p_shape_idx) const override;
virtual Transform2D area_get_shape_transform(RID p_area, int p_shape_idx) const override;
virtual void area_set_shape_disabled(RID p_area, int p_shape, bool p_disabled) override;
virtual void area_remove_shape(RID p_area, int p_shape_idx) override;
virtual void area_clear_shapes(RID p_area) override;
virtual void area_attach_object_instance_id(RID p_area, ObjectID p_id) override;
virtual ObjectID area_get_object_instance_id(RID p_area) const override;
virtual void area_attach_canvas_instance_id(RID p_area, ObjectID p_id) override;
virtual ObjectID area_get_canvas_instance_id(RID p_area) const override;
virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) override;
virtual void area_set_transform(RID p_area, const Transform2D &p_transform) override;
virtual Variant area_get_param(RID p_area, AreaParameter p_param) const override;
virtual Transform2D area_get_transform(RID p_area) const override;
virtual void area_set_monitorable(RID p_area, bool p_monitorable) override;
virtual void area_set_collision_layer(RID p_area, uint32_t p_layer) override;
virtual uint32_t area_get_collision_layer(RID p_area) const override;
virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) override;
virtual uint32_t area_get_collision_mask(RID p_area) const override;
virtual void area_set_monitor_callback(RID p_area, const Callable &p_callback) override;
virtual void area_set_area_monitor_callback(RID p_area, const Callable &p_callback) override;
virtual void area_set_pickable(RID p_area, bool p_pickable) override;
/* BODY API */
// create a body of a given type
virtual RID body_create() override;
virtual void body_set_space(RID p_body, RID p_space) override;
virtual RID body_get_space(RID p_body) const override;
virtual void body_set_mode(RID p_body, BodyMode p_mode) override;
virtual BodyMode body_get_mode(RID p_body) const override;
virtual void body_add_shape(RID p_body, RID p_shape, const Transform2D &p_transform = Transform2D(), bool p_disabled = false) override;
virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape) override;
virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform2D &p_transform) override;
virtual int body_get_shape_count(RID p_body) const override;
virtual RID body_get_shape(RID p_body, int p_shape_idx) const override;
virtual Transform2D body_get_shape_transform(RID p_body, int p_shape_idx) const override;
virtual void body_remove_shape(RID p_body, int p_shape_idx) override;
virtual void body_clear_shapes(RID p_body) override;
virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) override;
virtual void body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, real_t p_margin) override;
virtual void body_attach_object_instance_id(RID p_body, ObjectID p_id) override;
virtual ObjectID body_get_object_instance_id(RID p_body) const override;
virtual void body_attach_canvas_instance_id(RID p_body, ObjectID p_id) override;
virtual ObjectID body_get_canvas_instance_id(RID p_body) const override;
virtual void body_set_continuous_collision_detection_mode(RID p_body, CCDMode p_mode) override;
virtual CCDMode body_get_continuous_collision_detection_mode(RID p_body) const override;
virtual void body_set_collision_layer(RID p_body, uint32_t p_layer) override;
virtual uint32_t body_get_collision_layer(RID p_body) const override;
virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) override;
virtual uint32_t body_get_collision_mask(RID p_body) const override;
virtual void body_set_collision_priority(RID p_body, real_t p_priority) override;
virtual real_t body_get_collision_priority(RID p_body) const override;
virtual void body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) override;
virtual Variant body_get_param(RID p_body, BodyParameter p_param) const override;
virtual void body_reset_mass_properties(RID p_body) override;
virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override;
virtual Variant body_get_state(RID p_body, BodyState p_state) const override;
virtual void body_apply_central_impulse(RID p_body, const Vector2 &p_impulse) override;
virtual void body_apply_torque_impulse(RID p_body, real_t p_torque) override;
virtual void body_apply_impulse(RID p_body, const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override;
virtual void body_apply_central_force(RID p_body, const Vector2 &p_force) override;
virtual void body_apply_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position = Vector2()) override;
virtual void body_apply_torque(RID p_body, real_t p_torque) override;
virtual void body_add_constant_central_force(RID p_body, const Vector2 &p_force) override;
virtual void body_add_constant_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position = Vector2()) override;
virtual void body_add_constant_torque(RID p_body, real_t p_torque) override;
virtual void body_set_constant_force(RID p_body, const Vector2 &p_force) override;
virtual Vector2 body_get_constant_force(RID p_body) const override;
virtual void body_set_constant_torque(RID p_body, real_t p_torque) override;
virtual real_t body_get_constant_torque(RID p_body) const override;
virtual void body_set_axis_velocity(RID p_body, const Vector2 &p_axis_velocity) override;
virtual void body_add_collision_exception(RID p_body, RID p_body_b) override;
virtual void body_remove_collision_exception(RID p_body, RID p_body_b) override;
virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) override;
virtual void body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) override;
virtual real_t body_get_contacts_reported_depth_threshold(RID p_body) const override;
virtual void body_set_omit_force_integration(RID p_body, bool p_omit) override;
virtual bool body_is_omitting_force_integration(RID p_body) const override;
virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) override;
virtual int body_get_max_contacts_reported(RID p_body) const override;
virtual void body_set_state_sync_callback(RID p_body, const Callable &p_callable) override;
virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override;
virtual bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override;
virtual void body_set_pickable(RID p_body, bool p_pickable) override;
virtual bool body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result = nullptr) override;
// this function only works on physics process, errors and returns null otherwise
virtual PhysicsDirectBodyState2D *body_get_direct_state(RID p_body) override;
/* JOINT API */
virtual RID joint_create() override;
virtual void joint_clear(RID p_joint) override;
virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value) override;
virtual real_t joint_get_param(RID p_joint, JointParam p_param) const override;
virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disabled) override;
virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const override;
virtual void joint_make_pin(RID p_joint, const Vector2 &p_anchor, RID p_body_a, RID p_body_b = RID()) override;
virtual void joint_make_groove(RID p_joint, const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) override;
virtual void joint_make_damped_spring(RID p_joint, const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b = RID()) override;
virtual void pin_joint_set_flag(RID p_joint, PinJointFlag p_flag, bool p_enabled) override;
virtual bool pin_joint_get_flag(RID p_joint, PinJointFlag p_flag) const override;
virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) override;
virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const override;
virtual void damped_spring_joint_set_param(RID p_joint, DampedSpringParam p_param, real_t p_value) override;
virtual real_t damped_spring_joint_get_param(RID p_joint, DampedSpringParam p_param) const override;
virtual JointType joint_get_type(RID p_joint) const override;
/* MISC */
virtual void free(RID p_rid) override;
virtual void set_active(bool p_active) override;
virtual void init() override;
virtual void step(real_t p_step) override;
virtual void sync() override;
virtual void flush_queries() override;
virtual void end_sync() override;
virtual void finish() override;
virtual bool is_flushing_queries() const override { return flushing_queries; }
int get_process_info(ProcessInfo p_info) override;
GodotPhysicsServer2D(bool p_using_threads = false);
~GodotPhysicsServer2D() {}
};
#endif // GODOT_PHYSICS_SERVER_2D_H

985
servers/physics_2d/godot_shape_2d.cpp
View File

@ -1,985 +0,0 @@
/**************************************************************************/
/* godot_shape_2d.cpp */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#include "godot_shape_2d.h"
#include "core/math/geometry_2d.h"
#include "core/templates/sort_array.h"
void GodotShape2D::configure(const Rect2 &p_aabb) {
aabb = p_aabb;
configured = true;
for (const KeyValue<GodotShapeOwner2D *, int> &E : owners) {
GodotShapeOwner2D *co = const_cast<GodotShapeOwner2D *>(E.key);
co->_shape_changed();
}
}
Vector2 GodotShape2D::get_support(const Vector2 &p_normal) const {
Vector2 res[2];
int amnt;
get_supports(p_normal, res, amnt);
return res[0];
}
void GodotShape2D::add_owner(GodotShapeOwner2D *p_owner) {
HashMap<GodotShapeOwner2D *, int>::Iterator E = owners.find(p_owner);
if (E) {
E->value++;
} else {
owners[p_owner] = 1;
}
}
void GodotShape2D::remove_owner(GodotShapeOwner2D *p_owner) {
HashMap<GodotShapeOwner2D *, int>::Iterator E = owners.find(p_owner);
ERR_FAIL_COND(!E);
E->value--;
if (E->value == 0) {
owners.remove(E);
}
}
bool GodotShape2D::is_owner(GodotShapeOwner2D *p_owner) const {
return owners.has(p_owner);
}
const HashMap<GodotShapeOwner2D *, int> &GodotShape2D::get_owners() const {
return owners;
}
GodotShape2D::~GodotShape2D() {
ERR_FAIL_COND(owners.size());
}
/*********************************************************/
/*********************************************************/
/*********************************************************/
void GodotWorldBoundaryShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
r_amount = 0;
}
bool GodotWorldBoundaryShape2D::contains_point(const Vector2 &p_point) const {
return normal.dot(p_point) < d;
}
bool GodotWorldBoundaryShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
Vector2 segment = p_begin - p_end;
real_t den = normal.dot(segment);
//printf("den is %i\n",den);
if (Math::abs(den) <= CMP_EPSILON) {
return false;
}
real_t dist = (normal.dot(p_begin) - d) / den;
//printf("dist is %i\n",dist);
if (dist < -CMP_EPSILON || dist > (1.0 + CMP_EPSILON)) {
return false;
}
r_point = p_begin + segment * -dist;
r_normal = normal;
return true;
}
real_t GodotWorldBoundaryShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
return 0;
}
void GodotWorldBoundaryShape2D::set_data(const Variant &p_data) {
ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY);
Array arr = p_data;
ERR_FAIL_COND(arr.size() != 2);
normal = arr[0];
d = arr[1];
configure(Rect2(Vector2(-1e15, -1e15), Vector2(1e15 * 2, 1e15 * 2)));
}
Variant GodotWorldBoundaryShape2D::get_data() const {
Array arr;
arr.resize(2);
arr[0] = normal;
arr[1] = d;
return arr;
}
/*********************************************************/
/*********************************************************/
/*********************************************************/
void GodotSeparationRayShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
r_amount = 1;
if (p_normal.y > 0) {
*r_supports = Vector2(0, length);
} else {
*r_supports = Vector2();
}
}
bool GodotSeparationRayShape2D::contains_point(const Vector2 &p_point) const {
return false;
}
bool GodotSeparationRayShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
return false; //rays can't be intersected
}
real_t GodotSeparationRayShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
return 0; //rays are mass-less
}
void GodotSeparationRayShape2D::set_data(const Variant &p_data) {
Dictionary d = p_data;
length = d["length"];
slide_on_slope = d["slide_on_slope"];
configure(Rect2(0, 0, 0.001, length));
}
Variant GodotSeparationRayShape2D::get_data() const {
Dictionary d;
d["length"] = length;
d["slide_on_slope"] = slide_on_slope;
return d;
}
/*********************************************************/
/*********************************************************/
/*********************************************************/
void GodotSegmentShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
if (Math::abs(p_normal.dot(n)) > segment_is_valid_support_threshold) {
r_supports[0] = a;
r_supports[1] = b;
r_amount = 2;
return;
}
real_t dp = p_normal.dot(b - a);
if (dp > 0) {
*r_supports = b;
} else {
*r_supports = a;
}
r_amount = 1;
}
bool GodotSegmentShape2D::contains_point(const Vector2 &p_point) const {
return false;
}
bool GodotSegmentShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
if (!Geometry2D::segment_intersects_segment(p_begin, p_end, a, b, &r_point)) {
return false;
}
if (n.dot(p_begin) > n.dot(a)) {
r_normal = n;
} else {
r_normal = -n;
}
return true;
}
real_t GodotSegmentShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
return p_mass * ((a * p_scale).distance_squared_to(b * p_scale)) / 12;
}
void GodotSegmentShape2D::set_data(const Variant &p_data) {
ERR_FAIL_COND(p_data.get_type() != Variant::RECT2);
Rect2 r = p_data;
a = r.position;
b = r.size;
n = (b - a).orthogonal();
Rect2 aabb_new;
aabb_new.position = a;
aabb_new.expand_to(b);
if (aabb_new.size.x == 0) {
aabb_new.size.x = 0.001;
}
if (aabb_new.size.y == 0) {
aabb_new.size.y = 0.001;
}
configure(aabb_new);
}
Variant GodotSegmentShape2D::get_data() const {
Rect2 r;
r.position = a;
r.size = b;
return r;
}
/*********************************************************/
/*********************************************************/
/*********************************************************/
void GodotCircleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
r_amount = 1;
*r_supports = p_normal * radius;
}
bool GodotCircleShape2D::contains_point(const Vector2 &p_point) const {
return p_point.length_squared() < radius * radius;
}
bool GodotCircleShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
Vector2 line_vec = p_end - p_begin;
real_t a, b, c;
a = line_vec.dot(line_vec);
b = 2 * p_begin.dot(line_vec);
c = p_begin.dot(p_begin) - radius * radius;
real_t sqrtterm = b * b - 4 * a * c;
if (sqrtterm < 0) {
return false;
}
sqrtterm = Math::sqrt(sqrtterm);
real_t res = (-b - sqrtterm) / (2 * a);
if (res < 0 || res > 1 + CMP_EPSILON) {
return false;
}
r_point = p_begin + line_vec * res;
r_normal = r_point.normalized();
return true;
}
real_t GodotCircleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
real_t a = radius * p_scale.x;
real_t b = radius * p_scale.y;
return p_mass * (a * a + b * b) / 4;
}
void GodotCircleShape2D::set_data(const Variant &p_data) {
ERR_FAIL_COND(!p_data.is_num());
radius = p_data;
configure(Rect2(-radius, -radius, radius * 2, radius * 2));
}
Variant GodotCircleShape2D::get_data() const {
return radius;
}
/*********************************************************/
/*********************************************************/
/*********************************************************/
void GodotRectangleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
for (int i = 0; i < 2; i++) {
Vector2 ag;
ag[i] = 1.0;
real_t dp = ag.dot(p_normal);
if (Math::abs(dp) <= segment_is_valid_support_threshold) {
continue;
}
real_t sgn = dp > 0 ? 1.0 : -1.0;
r_amount = 2;
r_supports[0][i] = half_extents[i] * sgn;
r_supports[0][i ^ 1] = half_extents[i ^ 1];
r_supports[1][i] = half_extents[i] * sgn;
r_supports[1][i ^ 1] = -half_extents[i ^ 1];
return;
}
/* USE POINT */
r_amount = 1;
r_supports[0] = Vector2(
(p_normal.x < 0) ? -half_extents.x : half_extents.x,
(p_normal.y < 0) ? -half_extents.y : half_extents.y);
}
bool GodotRectangleShape2D::contains_point(const Vector2 &p_point) const {
real_t x = p_point.x;
real_t y = p_point.y;
real_t edge_x = half_extents.x;
real_t edge_y = half_extents.y;
return (x >= -edge_x) && (x < edge_x) && (y >= -edge_y) && (y < edge_y);
}
bool GodotRectangleShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
return get_aabb().intersects_segment(p_begin, p_end, &r_point, &r_normal);
}
real_t GodotRectangleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
Vector2 he2 = half_extents * 2 * p_scale;
return p_mass * he2.dot(he2) / 12.0;
}
void GodotRectangleShape2D::set_data(const Variant &p_data) {
ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR2);
half_extents = p_data;
configure(Rect2(-half_extents, half_extents * 2.0));
}
Variant GodotRectangleShape2D::get_data() const {
return half_extents;
}
/*********************************************************/
/*********************************************************/
/*********************************************************/
void GodotCapsuleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
Vector2 n = p_normal;
real_t h = height * 0.5 - radius; // half-height of the rectangle part
if (h > 0 && Math::abs(n.x) > segment_is_valid_support_threshold) {
// make it flat
n.y = 0.0;
n.x = SIGN(n.x) * radius;
r_amount = 2;
r_supports[0] = n;
r_supports[0].y += h;
r_supports[1] = n;
r_supports[1].y -= h;
} else {
n *= radius;
n.y += (n.y > 0) ? h : -h;
r_amount = 1;
*r_supports = n;
}
}
bool GodotCapsuleShape2D::contains_point(const Vector2 &p_point) const {
Vector2 p = p_point;
p.y = Math::abs(p.y);
p.y -= height * 0.5 - radius;
if (p.y < 0) {
p.y = 0;
}
return p.length_squared() < radius * radius;
}
bool GodotCapsuleShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
real_t d = 1e10;
Vector2 n = (p_end - p_begin).normalized();
bool collided = false;
//try spheres
for (int i = 0; i < 2; i++) {
Vector2 begin = p_begin;
Vector2 end = p_end;
real_t ofs = (i == 0) ? -height * 0.5 + radius : height * 0.5 - radius;
begin.y += ofs;
end.y += ofs;
Vector2 line_vec = end - begin;
real_t a, b, c;
a = line_vec.dot(line_vec);
b = 2 * begin.dot(line_vec);
c = begin.dot(begin) - radius * radius;
real_t sqrtterm = b * b - 4 * a * c;
if (sqrtterm < 0) {
continue;
}
sqrtterm = Math::sqrt(sqrtterm);
real_t res = (-b - sqrtterm) / (2 * a);
if (res < 0 || res > 1 + CMP_EPSILON) {
continue;
}
Vector2 point = begin + line_vec * res;
Vector2 pointf(point.x, point.y - ofs);
real_t pd = n.dot(pointf);
if (pd < d) {
r_point = pointf;
r_normal = point.normalized();
d = pd;
collided = true;
}
}
Vector2 rpos, rnorm;
if (Rect2(Point2(-radius, -height * 0.5 + radius), Size2(radius * 2.0, height - radius * 2)).intersects_segment(p_begin, p_end, &rpos, &rnorm)) {
real_t pd = n.dot(rpos);
if (pd < d) {
r_point = rpos;
r_normal = rnorm;
d = pd;
collided = true;
}
}
//return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);
return collided; //todo
}
real_t GodotCapsuleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
Vector2 he2 = Vector2(radius * 2, height) * p_scale;
return p_mass * he2.dot(he2) / 12.0;
}
void GodotCapsuleShape2D::set_data(const Variant &p_data) {
ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY && p_data.get_type() != Variant::VECTOR2);
if (p_data.get_type() == Variant::ARRAY) {
Array arr = p_data;
ERR_FAIL_COND(arr.size() != 2);
height = arr[0];
radius = arr[1];
} else {
Point2 p = p_data;
radius = p.x;
height = p.y;
}
Point2 he(radius, height * 0.5);
configure(Rect2(-he, he * 2));
}
Variant GodotCapsuleShape2D::get_data() const {
return Point2(height, radius);
}
/*********************************************************/
/*********************************************************/
/*********************************************************/
void GodotConvexPolygonShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
int support_idx = -1;
real_t d = -1e10;
r_amount = 0;
for (int i = 0; i < point_count; i++) {
//test point
real_t ld = p_normal.dot(points[i].pos);
if (ld > d) {
support_idx = i;
d = ld;
}
//test segment
if (points[i].normal.dot(p_normal) > segment_is_valid_support_threshold) {
r_amount = 2;
r_supports[0] = points[i].pos;
r_supports[1] = points[(i + 1) % point_count].pos;
return;
}
}
ERR_FAIL_COND_MSG(support_idx == -1, "Convex polygon shape support not found.");
r_amount = 1;
r_supports[0] = points[support_idx].pos;
}
bool GodotConvexPolygonShape2D::contains_point(const Vector2 &p_point) const {
bool out = false;
bool in = false;
for (int i = 0; i < point_count; i++) {
real_t d = points[i].normal.dot(p_point) - points[i].normal.dot(points[i].pos);
if (d > 0) {
out = true;
} else {
in = true;
}
}
return in != out;
}
bool GodotConvexPolygonShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
Vector2 n = (p_end - p_begin).normalized();
real_t d = 1e10;
bool inters = false;
for (int i = 0; i < point_count; i++) {
Vector2 res;
if (!Geometry2D::segment_intersects_segment(p_begin, p_end, points[i].pos, points[(i + 1) % point_count].pos, &res)) {
continue;
}
real_t nd = n.dot(res);
if (nd < d) {
d = nd;
r_point = res;
r_normal = points[i].normal;
inters = true;
}
}
return inters;
}
real_t GodotConvexPolygonShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
ERR_FAIL_COND_V_MSG(point_count == 0, 0, "Convex polygon shape has no points.");
Rect2 aabb_new;
aabb_new.position = points[0].pos * p_scale;
for (int i = 0; i < point_count; i++) {
aabb_new.expand_to(points[i].pos * p_scale);
}
return p_mass * aabb_new.size.dot(aabb_new.size) / 12.0;
}
void GodotConvexPolygonShape2D::set_data(const Variant &p_data) {
#ifdef REAL_T_IS_DOUBLE
ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY);
#else
ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY);
#endif
if (points) {
memdelete_arr(points);
}
points = nullptr;
point_count = 0;
if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) {
Vector<Vector2> arr = p_data;
ERR_FAIL_COND(arr.is_empty());
point_count = arr.size();
points = memnew_arr(Point, point_count);
const Vector2 *r = arr.ptr();
for (int i = 0; i < point_count; i++) {
points[i].pos = r[i];
}
for (int i = 0; i < point_count; i++) {
Vector2 p = points[i].pos;
Vector2 pn = points[(i + 1) % point_count].pos;
points[i].normal = (pn - p).orthogonal().normalized();
}
} else {
Vector<real_t> dvr = p_data;
point_count = dvr.size() / 4;
ERR_FAIL_COND(point_count == 0);
points = memnew_arr(Point, point_count);
const real_t *r = dvr.ptr();
for (int i = 0; i < point_count; i++) {
int idx = i << 2;
points[i].pos.x = r[idx + 0];
points[i].pos.y = r[idx + 1];
points[i].normal.x = r[idx + 2];
points[i].normal.y = r[idx + 3];
}
}
ERR_FAIL_COND(point_count == 0);
Rect2 aabb_new;
aabb_new.position = points[0].pos;
for (int i = 1; i < point_count; i++) {
aabb_new.expand_to(points[i].pos);
}
configure(aabb_new);
}
Variant GodotConvexPolygonShape2D::get_data() const {
Vector<Vector2> dvr;
dvr.resize(point_count);
for (int i = 0; i < point_count; i++) {
dvr.set(i, points[i].pos);
}
return dvr;
}
GodotConvexPolygonShape2D::~GodotConvexPolygonShape2D() {
if (points) {
memdelete_arr(points);
}
}
//////////////////////////////////////////////////
void GodotConcavePolygonShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
real_t d = -1e10;
int idx = -1;
for (int i = 0; i < points.size(); i++) {
real_t ld = p_normal.dot(points[i]);
if (ld > d) {
d = ld;
idx = i;
}
}
r_amount = 1;
ERR_FAIL_COND(idx == -1);
*r_supports = points[idx];
}
bool GodotConcavePolygonShape2D::contains_point(const Vector2 &p_point) const {
return false; //sorry
}
bool GodotConcavePolygonShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
if (segments.size() == 0 || points.size() == 0) {
return false;
}
uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth);
enum {
TEST_AABB_BIT = 0,
VISIT_LEFT_BIT = 1,
VISIT_RIGHT_BIT = 2,
VISIT_DONE_BIT = 3,
VISITED_BIT_SHIFT = 29,
NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1,
VISITED_BIT_MASK = ~NODE_IDX_MASK,
};
Vector2 n = (p_end - p_begin).normalized();
real_t d = 1e10;
bool inters = false;
/*
for(int i=0;i<bvh_depth;i++)
stack[i]=0;
*/
int level = 0;
const Segment *segmentptr = &segments[0];
const Vector2 *pointptr = &points[0];
const BVH *bvhptr = &bvh[0];
stack[0] = 0;
while (true) {
uint32_t node = stack[level] & NODE_IDX_MASK;
const BVH &bvh2 = bvhptr[node];
bool done = false;
switch (stack[level] >> VISITED_BIT_SHIFT) {
case TEST_AABB_BIT: {
bool valid = bvh2.aabb.intersects_segment(p_begin, p_end);
if (!valid) {
stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
} else {
if (bvh2.left < 0) {
const Segment &s = segmentptr[bvh2.right];
Vector2 a = pointptr[s.points[0]];
Vector2 b = pointptr[s.points[1]];
Vector2 res;
if (Geometry2D::segment_intersects_segment(p_begin, p_end, a, b, &res)) {
real_t nd = n.dot(res);
if (nd < d) {
d = nd;
r_point = res;
r_normal = (b - a).orthogonal().normalized();
inters = true;
}
}
stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
} else {
stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node;
}
}
}
continue;
case VISIT_LEFT_BIT: {
stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node;
stack[level + 1] = bvh2.left | TEST_AABB_BIT;
level++;
}
continue;
case VISIT_RIGHT_BIT: {
stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
stack[level + 1] = bvh2.right | TEST_AABB_BIT;
level++;
}
continue;
case VISIT_DONE_BIT: {
if (level == 0) {
done = true;
break;
} else {
level--;
}
}
continue;
}
if (done) {
break;
}
}
if (inters) {
if (n.dot(r_normal) > 0) {
r_normal = -r_normal;
}
}
return inters;
}
int GodotConcavePolygonShape2D::_generate_bvh(BVH *p_bvh, int p_len, int p_depth) {
if (p_len == 1) {
bvh_depth = MAX(p_depth, bvh_depth);
bvh.push_back(*p_bvh);
return bvh.size() - 1;
}
//else sort best
Rect2 global_aabb = p_bvh[0].aabb;
for (int i = 1; i < p_len; i++) {
global_aabb = global_aabb.merge(p_bvh[i].aabb);
}
if (global_aabb.size.x > global_aabb.size.y) {
SortArray<BVH, BVH_CompareX> sort;
sort.sort(p_bvh, p_len);
} else {
SortArray<BVH, BVH_CompareY> sort;
sort.sort(p_bvh, p_len);
}
int median = p_len / 2;
BVH node;
node.aabb = global_aabb;
int node_idx = bvh.size();
bvh.push_back(node);
int l = _generate_bvh(p_bvh, median, p_depth + 1);
int r = _generate_bvh(&p_bvh[median], p_len - median, p_depth + 1);
bvh.write[node_idx].left = l;
bvh.write[node_idx].right = r;
return node_idx;
}
void GodotConcavePolygonShape2D::set_data(const Variant &p_data) {
#ifdef REAL_T_IS_DOUBLE
ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY);
#else
ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY);
#endif
Rect2 aabb_new;
if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) {
Vector<Vector2> p2arr = p_data;
int len = p2arr.size();
ERR_FAIL_COND(len % 2);
segments.clear();
points.clear();
bvh.clear();
bvh_depth = 1;
if (len == 0) {
configure(aabb_new);
return;
}
const Vector2 *arr = p2arr.ptr();
HashMap<Point2, int> pointmap;
for (int i = 0; i < len; i += 2) {
Point2 p1 = arr[i];
Point2 p2 = arr[i + 1];
int idx_p1, idx_p2;
if (pointmap.has(p1)) {
idx_p1 = pointmap[p1];
} else {
idx_p1 = pointmap.size();
pointmap[p1] = idx_p1;
}
if (pointmap.has(p2)) {
idx_p2 = pointmap[p2];
} else {
idx_p2 = pointmap.size();
pointmap[p2] = idx_p2;
}
Segment s;
s.points[0] = idx_p1;
s.points[1] = idx_p2;
segments.push_back(s);
}
points.resize(pointmap.size());
aabb_new.position = pointmap.begin()->key;
for (const KeyValue<Point2, int> &E : pointmap) {
aabb_new.expand_to(E.key);
points.write[E.value] = E.key;
}
Vector<BVH> main_vbh;
main_vbh.resize(segments.size());
for (int i = 0; i < main_vbh.size(); i++) {
main_vbh.write[i].aabb.position = points[segments[i].points[0]];
main_vbh.write[i].aabb.expand_to(points[segments[i].points[1]]);
main_vbh.write[i].left = -1;
main_vbh.write[i].right = i;
}
_generate_bvh(main_vbh.ptrw(), main_vbh.size(), 1);
} else {
//dictionary with arrays
}
configure(aabb_new);
}
Variant GodotConcavePolygonShape2D::get_data() const {
Vector<Vector2> rsegments;
int len = segments.size();
rsegments.resize(len * 2);
Vector2 *w = rsegments.ptrw();
for (int i = 0; i < len; i++) {
w[(i << 1) + 0] = points[segments[i].points[0]];
w[(i << 1) + 1] = points[segments[i].points[1]];
}
return rsegments;
}
void GodotConcavePolygonShape2D::cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const {
uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth);
enum {
TEST_AABB_BIT = 0,
VISIT_LEFT_BIT = 1,
VISIT_RIGHT_BIT = 2,
VISIT_DONE_BIT = 3,
VISITED_BIT_SHIFT = 29,
NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1,
VISITED_BIT_MASK = ~NODE_IDX_MASK,
};
/*
for(int i=0;i<bvh_depth;i++)
stack[i]=0;
*/
if (segments.size() == 0 || points.size() == 0 || bvh.size() == 0) {
return;
}
int level = 0;
const Segment *segmentptr = &segments[0];
const Vector2 *pointptr = &points[0];
const BVH *bvhptr = &bvh[0];
stack[0] = 0;
while (true) {
uint32_t node = stack[level] & NODE_IDX_MASK;
const BVH &bvh2 = bvhptr[node];
switch (stack[level] >> VISITED_BIT_SHIFT) {
case TEST_AABB_BIT: {
bool valid = p_local_aabb.intersects(bvh2.aabb);
if (!valid) {
stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
} else {
if (bvh2.left < 0) {
const Segment &s = segmentptr[bvh2.right];
Vector2 a = pointptr[s.points[0]];
Vector2 b = pointptr[s.points[1]];
GodotSegmentShape2D ss(a, b, (b - a).orthogonal().normalized());
if (p_callback(p_userdata, &ss)) {
return;
}
stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
} else {
stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node;
}
}
}
continue;
case VISIT_LEFT_BIT: {
stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node;
stack[level + 1] = bvh2.left | TEST_AABB_BIT;
level++;
}
continue;
case VISIT_RIGHT_BIT: {
stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
stack[level + 1] = bvh2.right | TEST_AABB_BIT;
level++;
}
continue;
case VISIT_DONE_BIT: {
if (level == 0) {
return;
} else {
level--;
}
}
continue;
}
}
}

539
servers/physics_2d/godot_shape_2d.h
View File

@ -1,539 +0,0 @@
/**************************************************************************/
/* godot_shape_2d.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#ifndef GODOT_SHAPE_2D_H
#define GODOT_SHAPE_2D_H
#include "servers/physics_server_2d.h"
class GodotShape2D;
class GodotShapeOwner2D {
public:
virtual void _shape_changed() = 0;
virtual void remove_shape(GodotShape2D *p_shape) = 0;
virtual ~GodotShapeOwner2D() {}
};
class GodotShape2D {
RID self;
Rect2 aabb;
bool configured = false;
real_t custom_bias = 0.0;
HashMap<GodotShapeOwner2D *, int> owners;
protected:
const double segment_is_valid_support_threshold = 0.99998;
const double segment_is_valid_support_threshold_lower =
Math::sqrt(1.0 - segment_is_valid_support_threshold * segment_is_valid_support_threshold);
void configure(const Rect2 &p_aabb);
public:
_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
_FORCE_INLINE_ RID get_self() const { return self; }
virtual PhysicsServer2D::ShapeType get_type() const = 0;
_FORCE_INLINE_ Rect2 get_aabb() const { return aabb; }
_FORCE_INLINE_ bool is_configured() const { return configured; }
virtual bool allows_one_way_collision() const { return true; }
virtual bool is_concave() const { return false; }
virtual bool contains_point(const Vector2 &p_point) const = 0;
virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const = 0;
virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const = 0;
virtual Vector2 get_support(const Vector2 &p_normal) const;
virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const = 0;
virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const = 0;
virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const = 0;
virtual void set_data(const Variant &p_data) = 0;
virtual Variant get_data() const = 0;
_FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias = p_bias; }
_FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; }
void add_owner(GodotShapeOwner2D *p_owner);
void remove_owner(GodotShapeOwner2D *p_owner);
bool is_owner(GodotShapeOwner2D *p_owner) const;
const HashMap<GodotShapeOwner2D *, int> &get_owners() const;
_FORCE_INLINE_ void get_supports_transformed_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_xform, Vector2 *r_supports, int &r_amount) const {
get_supports(p_xform.basis_xform_inv(p_normal).normalized(), r_supports, r_amount);
for (int i = 0; i < r_amount; i++) {
r_supports[i] = p_xform.xform(r_supports[i]);
}
if (r_amount == 1) {
if (Math::abs(p_normal.dot(p_cast.normalized())) < segment_is_valid_support_threshold_lower) {
//make line because they are parallel
r_amount = 2;
r_supports[1] = r_supports[0] + p_cast;
} else if (p_cast.dot(p_normal) > 0) {
//normal points towards cast, add cast
r_supports[0] += p_cast;
}
} else {
if (Math::abs(p_normal.dot(p_cast.normalized())) < segment_is_valid_support_threshold_lower) {
//optimize line and make it larger because they are parallel
if ((r_supports[1] - r_supports[0]).dot(p_cast) > 0) {
//larger towards 1
r_supports[1] += p_cast;
} else {
//larger towards 0
r_supports[0] += p_cast;
}
} else if (p_cast.dot(p_normal) > 0) {
//normal points towards cast, add cast
r_supports[0] += p_cast;
r_supports[1] += p_cast;
}
}
}
GodotShape2D() {}
virtual ~GodotShape2D();
};
//let the optimizer do the magic
#define DEFAULT_PROJECT_RANGE_CAST \
virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { \
project_range_cast(p_cast, p_normal, p_transform, r_min, r_max); \
} \
_FORCE_INLINE_ void project_range_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { \
real_t mina, maxa; \
real_t minb, maxb; \
Transform2D ofsb = p_transform; \
ofsb.columns[2] += p_cast; \
project_range(p_normal, p_transform, mina, maxa); \
project_range(p_normal, ofsb, minb, maxb); \
r_min = MIN(mina, minb); \
r_max = MAX(maxa, maxb); \
}
class GodotWorldBoundaryShape2D : public GodotShape2D {
Vector2 normal;
real_t d = 0.0;
public:
_FORCE_INLINE_ Vector2 get_normal() const { return normal; }
_FORCE_INLINE_ real_t get_d() const { return d; }
virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_WORLD_BOUNDARY; }
virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); }
virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override;
virtual bool contains_point(const Vector2 &p_point) const override;
virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override;
virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override;
virtual void set_data(const Variant &p_data) override;
virtual Variant get_data() const override;
_FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const {
//real large
r_min = -1e10;
r_max = 1e10;
}
virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override {
project_range_cast(p_cast, p_normal, p_transform, r_min, r_max);
}
_FORCE_INLINE_ void project_range_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const {
//real large
r_min = -1e10;
r_max = 1e10;
}
};
class GodotSeparationRayShape2D : public GodotShape2D {
real_t length = 0.0;
bool slide_on_slope = false;
public:
_FORCE_INLINE_ real_t get_length() const { return length; }
_FORCE_INLINE_ bool get_slide_on_slope() const { return slide_on_slope; }
virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_SEPARATION_RAY; }
virtual bool allows_one_way_collision() const override { return false; }
virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); }
virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override;
virtual bool contains_point(const Vector2 &p_point) const override;
virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override;
virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override;
virtual void set_data(const Variant &p_data) override;
virtual Variant get_data() const override;
_FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const {
//real large
r_max = p_normal.dot(p_transform.get_origin());
r_min = p_normal.dot(p_transform.xform(Vector2(0, length)));
if (r_max < r_min) {
SWAP(r_max, r_min);
}
}
DEFAULT_PROJECT_RANGE_CAST
_FORCE_INLINE_ GodotSeparationRayShape2D() {}
_FORCE_INLINE_ GodotSeparationRayShape2D(real_t p_length) { length = p_length; }
};
class GodotSegmentShape2D : public GodotShape2D {
Vector2 a;
Vector2 b;
Vector2 n;
public:
_FORCE_INLINE_ const Vector2 &get_a() const { return a; }
_FORCE_INLINE_ const Vector2 &get_b() const { return b; }
_FORCE_INLINE_ const Vector2 &get_normal() const { return n; }
virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_SEGMENT; }
_FORCE_INLINE_ Vector2 get_xformed_normal(const Transform2D &p_xform) const {
return (p_xform.xform(b) - p_xform.xform(a)).normalized().orthogonal();
}
virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); }
virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override;
virtual bool contains_point(const Vector2 &p_point) const override;
virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override;
virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override;
virtual void set_data(const Variant &p_data) override;
virtual Variant get_data() const override;
_FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const {
//real large
r_max = p_normal.dot(p_transform.xform(a));
r_min = p_normal.dot(p_transform.xform(b));
if (r_max < r_min) {
SWAP(r_max, r_min);
}
}
DEFAULT_PROJECT_RANGE_CAST
_FORCE_INLINE_ GodotSegmentShape2D() {}
_FORCE_INLINE_ GodotSegmentShape2D(const Vector2 &p_a, const Vector2 &p_b, const Vector2 &p_n) {
a = p_a;
b = p_b;
n = p_n;
}
};
class GodotCircleShape2D : public GodotShape2D {
real_t radius;
public:
_FORCE_INLINE_ const real_t &get_radius() const { return radius; }
virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CIRCLE; }
virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); }
virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override;
virtual bool contains_point(const Vector2 &p_point) const override;
virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override;
virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override;
virtual void set_data(const Variant &p_data) override;
virtual Variant get_data() const override;
_FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const {
//real large
real_t d = p_normal.dot(p_transform.get_origin());
// figure out scale at point
Vector2 local_normal = p_transform.basis_xform_inv(p_normal);
real_t scale = local_normal.length();
r_min = d - (radius)*scale;
r_max = d + (radius)*scale;
}
DEFAULT_PROJECT_RANGE_CAST
};
class GodotRectangleShape2D : public GodotShape2D {
Vector2 half_extents;
public:
_FORCE_INLINE_ const Vector2 &get_half_extents() const { return half_extents; }
virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_RECTANGLE; }
virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); }
virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override;
virtual bool contains_point(const Vector2 &p_point) const override;
virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override;
virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override;
virtual void set_data(const Variant &p_data) override;
virtual Variant get_data() const override;
_FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const {
// no matter the angle, the box is mirrored anyway
r_max = -1e20;
r_min = 1e20;
for (int i = 0; i < 4; i++) {
real_t d = p_normal.dot(p_transform.xform(Vector2(((i & 1) * 2 - 1) * half_extents.x, ((i >> 1) * 2 - 1) * half_extents.y)));
if (d > r_max) {
r_max = d;
}
if (d < r_min) {
r_min = d;
}
}
}
_FORCE_INLINE_ Vector2 get_circle_axis(const Transform2D &p_xform, const Transform2D &p_xform_inv, const Vector2 &p_circle) const {
Vector2 local_v = p_xform_inv.xform(p_circle);
Vector2 he(
(local_v.x < 0) ? -half_extents.x : half_extents.x,
(local_v.y < 0) ? -half_extents.y : half_extents.y);
return (p_xform.xform(he) - p_circle).normalized();
}
_FORCE_INLINE_ Vector2 get_box_axis(const Transform2D &p_xform, const Transform2D &p_xform_inv, const GodotRectangleShape2D *p_B, const Transform2D &p_B_xform, const Transform2D &p_B_xform_inv) const {
Vector2 a, b;
{
Vector2 local_v = p_xform_inv.xform(p_B_xform.get_origin());
Vector2 he(
(local_v.x < 0) ? -half_extents.x : half_extents.x,
(local_v.y < 0) ? -half_extents.y : half_extents.y);
a = p_xform.xform(he);
}
{
Vector2 local_v = p_B_xform_inv.xform(p_xform.get_origin());
Vector2 he(
(local_v.x < 0) ? -p_B->half_extents.x : p_B->half_extents.x,
(local_v.y < 0) ? -p_B->half_extents.y : p_B->half_extents.y);
b = p_B_xform.xform(he);
}
return (a - b).normalized();
}
DEFAULT_PROJECT_RANGE_CAST
};
class GodotCapsuleShape2D : public GodotShape2D {
real_t radius = 0.0;
real_t height = 0.0;
public:
_FORCE_INLINE_ const real_t &get_radius() const { return radius; }
_FORCE_INLINE_ const real_t &get_height() const { return height; }
virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CAPSULE; }
virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); }
virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override;
virtual bool contains_point(const Vector2 &p_point) const override;
virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override;
virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override;
virtual void set_data(const Variant &p_data) override;
virtual Variant get_data() const override;
_FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const {
// no matter the angle, the box is mirrored anyway
Vector2 n = p_transform.basis_xform_inv(p_normal).normalized();
real_t h = height * 0.5 - radius;
n *= radius;
n.y += (n.y > 0) ? h : -h;
r_max = p_normal.dot(p_transform.xform(n));
r_min = p_normal.dot(p_transform.xform(-n));
if (r_max < r_min) {
SWAP(r_max, r_min);
}
//ERR_FAIL_COND( r_max < r_min );
}
DEFAULT_PROJECT_RANGE_CAST
};
class GodotConvexPolygonShape2D : public GodotShape2D {
struct Point {
Vector2 pos;
Vector2 normal; //normal to next segment
};
Point *points = nullptr;
int point_count = 0;
public:
_FORCE_INLINE_ int get_point_count() const { return point_count; }
_FORCE_INLINE_ const Vector2 &get_point(int p_idx) const { return points[p_idx].pos; }
_FORCE_INLINE_ const Vector2 &get_segment_normal(int p_idx) const { return points[p_idx].normal; }
_FORCE_INLINE_ Vector2 get_xformed_segment_normal(const Transform2D &p_xform, int p_idx) const {
Vector2 a = points[p_idx].pos;
p_idx++;
Vector2 b = points[p_idx == point_count ? 0 : p_idx].pos;
return (p_xform.xform(b) - p_xform.xform(a)).normalized().orthogonal();
}
virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CONVEX_POLYGON; }
virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); }
virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override;
virtual bool contains_point(const Vector2 &p_point) const override;
virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override;
virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override;
virtual void set_data(const Variant &p_data) override;
virtual Variant get_data() const override;
_FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const {
if (!points || point_count <= 0) {
r_min = r_max = 0;
return;
}
r_min = r_max = p_normal.dot(p_transform.xform(points[0].pos));
for (int i = 1; i < point_count; i++) {
real_t d = p_normal.dot(p_transform.xform(points[i].pos));
if (d > r_max) {
r_max = d;
}
if (d < r_min) {
r_min = d;
}
}
}
DEFAULT_PROJECT_RANGE_CAST
GodotConvexPolygonShape2D() {}
~GodotConvexPolygonShape2D();
};
class GodotConcaveShape2D : public GodotShape2D {
public:
virtual bool is_concave() const override { return true; }
// Returns true to stop the query.
typedef bool (*QueryCallback)(void *p_userdata, GodotShape2D *p_convex);
virtual void cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0;
};
class GodotConcavePolygonShape2D : public GodotConcaveShape2D {
struct Segment {
int points[2] = {};
};
Vector<Segment> segments;
Vector<Point2> points;
struct BVH {
Rect2 aabb;
int left = 0, right = 0;
};
Vector<BVH> bvh;
int bvh_depth = 0;
struct BVH_CompareX {
_FORCE_INLINE_ bool operator()(const BVH &a, const BVH &b) const {
return (a.aabb.position.x + a.aabb.size.x * 0.5) < (b.aabb.position.x + b.aabb.size.x * 0.5);
}
};
struct BVH_CompareY {
_FORCE_INLINE_ bool operator()(const BVH &a, const BVH &b) const {
return (a.aabb.position.y + a.aabb.size.y * 0.5) < (b.aabb.position.y + b.aabb.size.y * 0.5);
}
};
int _generate_bvh(BVH *p_bvh, int p_len, int p_depth);
public:
virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CONCAVE_POLYGON; }
virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override {
r_min = 0;
r_max = 0;
ERR_FAIL_MSG("Unsupported call to project_rangev in GodotConcavePolygonShape2D");
}
void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const {
r_min = 0;
r_max = 0;
ERR_FAIL_MSG("Unsupported call to project_range in GodotConcavePolygonShape2D");
}
virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override;
virtual bool contains_point(const Vector2 &p_point) const override;
virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override;
virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override { return 0; }
virtual void set_data(const Variant &p_data) override;
virtual Variant get_data() const override;
virtual void cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override;
DEFAULT_PROJECT_RANGE_CAST
};
#undef DEFAULT_PROJECT_RANGE_CAST
#endif // GODOT_SHAPE_2D_H

1240
servers/physics_2d/godot_space_2d.cpp
View File

File diff suppressed because it is too large Load Diff

214
servers/physics_2d/godot_space_2d.h
View File

@ -1,214 +0,0 @@
/**************************************************************************/
/* godot_space_2d.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#ifndef GODOT_SPACE_2D_H
#define GODOT_SPACE_2D_H
#include "godot_area_2d.h"
#include "godot_area_pair_2d.h"
#include "godot_body_2d.h"
#include "godot_body_pair_2d.h"
#include "godot_broad_phase_2d.h"
#include "godot_collision_object_2d.h"
#include "core/config/project_settings.h"
#include "core/templates/hash_map.h"
#include "core/typedefs.h"
class GodotPhysicsDirectSpaceState2D : public PhysicsDirectSpaceState2D {
GDCLASS(GodotPhysicsDirectSpaceState2D, PhysicsDirectSpaceState2D);
public:
GodotSpace2D *space = nullptr;
virtual int intersect_point(const PointParameters &p_parameters, ShapeResult *r_results, int p_result_max) override;
virtual bool intersect_ray(const RayParameters &p_parameters, RayResult &r_result) override;
virtual int intersect_shape(const ShapeParameters &p_parameters, ShapeResult *r_results, int p_result_max) override;
virtual bool cast_motion(const ShapeParameters &p_parameters, real_t &p_closest_safe, real_t &p_closest_unsafe) override;
virtual bool collide_shape(const ShapeParameters &p_parameters, Vector2 *r_results, int p_result_max, int &r_result_count) override;
virtual bool rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) override;
GodotPhysicsDirectSpaceState2D() {}
};
class GodotSpace2D {
public:
enum ElapsedTime {
ELAPSED_TIME_INTEGRATE_FORCES,
ELAPSED_TIME_GENERATE_ISLANDS,
ELAPSED_TIME_SETUP_CONSTRAINTS,
ELAPSED_TIME_SOLVE_CONSTRAINTS,
ELAPSED_TIME_INTEGRATE_VELOCITIES,
ELAPSED_TIME_MAX
};
private:
struct ExcludedShapeSW {
GodotShape2D *local_shape = nullptr;
const GodotCollisionObject2D *against_object = nullptr;
int against_shape_index = 0;
};
uint64_t elapsed_time[ELAPSED_TIME_MAX] = {};
GodotPhysicsDirectSpaceState2D *direct_access = nullptr;
RID self;
GodotBroadPhase2D *broadphase = nullptr;
SelfList<GodotBody2D>::List active_list;
SelfList<GodotBody2D>::List mass_properties_update_list;
SelfList<GodotBody2D>::List state_query_list;
SelfList<GodotArea2D>::List monitor_query_list;
SelfList<GodotArea2D>::List area_moved_list;
static void *_broadphase_pair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_self);
static void _broadphase_unpair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_data, void *p_self);
HashSet<GodotCollisionObject2D *> objects;
GodotArea2D *area = nullptr;
int solver_iterations = 0;
real_t contact_recycle_radius = 0.0;
real_t contact_max_separation = 0.0;
real_t contact_max_allowed_penetration = 0.0;
real_t contact_bias = 0.0;
real_t constraint_bias = 0.0;
enum {
INTERSECTION_QUERY_MAX = 2048
};
GodotCollisionObject2D *intersection_query_results[INTERSECTION_QUERY_MAX];
int intersection_query_subindex_results[INTERSECTION_QUERY_MAX];
real_t body_linear_velocity_sleep_threshold = 0.0;
real_t body_angular_velocity_sleep_threshold = 0.0;
real_t body_time_to_sleep = 0.0;
bool locked = false;
real_t last_step = 0.001;
int island_count = 0;
int active_objects = 0;
int collision_pairs = 0;
int _cull_aabb_for_body(GodotBody2D *p_body, const Rect2 &p_aabb);
Vector<Vector2> contact_debug;
int contact_debug_count = 0;
friend class GodotPhysicsDirectSpaceState2D;
public:
_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
_FORCE_INLINE_ RID get_self() const { return self; }
void set_default_area(GodotArea2D *p_area) { area = p_area; }
GodotArea2D *get_default_area() const { return area; }
const SelfList<GodotBody2D>::List &get_active_body_list() const;
void body_add_to_active_list(SelfList<GodotBody2D> *p_body);
void body_remove_from_active_list(SelfList<GodotBody2D> *p_body);
void body_add_to_mass_properties_update_list(SelfList<GodotBody2D> *p_body);
void body_remove_from_mass_properties_update_list(SelfList<GodotBody2D> *p_body);
void area_add_to_moved_list(SelfList<GodotArea2D> *p_area);
void area_remove_from_moved_list(SelfList<GodotArea2D> *p_area);
const SelfList<GodotArea2D>::List &get_moved_area_list() const;
void body_add_to_state_query_list(SelfList<GodotBody2D> *p_body);
void body_remove_from_state_query_list(SelfList<GodotBody2D> *p_body);
void area_add_to_monitor_query_list(SelfList<GodotArea2D> *p_area);
void area_remove_from_monitor_query_list(SelfList<GodotArea2D> *p_area);
GodotBroadPhase2D *get_broadphase();
void add_object(GodotCollisionObject2D *p_object);
void remove_object(GodotCollisionObject2D *p_object);
const HashSet<GodotCollisionObject2D *> &get_objects() const;
_FORCE_INLINE_ int get_solver_iterations() const { return solver_iterations; }
_FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; }
_FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; }
_FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; }
_FORCE_INLINE_ real_t get_contact_bias() const { return contact_bias; }
_FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; }
_FORCE_INLINE_ real_t get_body_linear_velocity_sleep_threshold() const { return body_linear_velocity_sleep_threshold; }
_FORCE_INLINE_ real_t get_body_angular_velocity_sleep_threshold() const { return body_angular_velocity_sleep_threshold; }
_FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; }
void update();
void setup();
void call_queries();
bool is_locked() const;
void lock();
void unlock();
real_t get_last_step() const { return last_step; }
void set_last_step(real_t p_step) { last_step = p_step; }
void set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_value);
real_t get_param(PhysicsServer2D::SpaceParameter p_param) const;
void set_island_count(int p_island_count) { island_count = p_island_count; }
int get_island_count() const { return island_count; }
void set_active_objects(int p_active_objects) { active_objects = p_active_objects; }
int get_active_objects() const { return active_objects; }
int get_collision_pairs() const { return collision_pairs; }
bool test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::MotionParameters &p_parameters, PhysicsServer2D::MotionResult *r_result);
void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); }
_FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.is_empty(); }
_FORCE_INLINE_ void add_debug_contact(const Vector2 &p_contact) {
if (contact_debug_count < contact_debug.size()) {
contact_debug.write[contact_debug_count++] = p_contact;
}
}
_FORCE_INLINE_ Vector<Vector2> get_debug_contacts() { return contact_debug; }
_FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; }
GodotPhysicsDirectSpaceState2D *get_direct_state();
void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; }
uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; }
GodotSpace2D();
~GodotSpace2D();
};
#endif // GODOT_SPACE_2D_H

306
servers/physics_2d/godot_step_2d.cpp
View File

@ -1,306 +0,0 @@
/**************************************************************************/
/* godot_step_2d.cpp */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#include "godot_step_2d.h"
#include "core/object/worker_thread_pool.h"
#include "core/os/os.h"
#define BODY_ISLAND_COUNT_RESERVE 128
#define BODY_ISLAND_SIZE_RESERVE 512
#define ISLAND_COUNT_RESERVE 128
#define ISLAND_SIZE_RESERVE 512
#define CONSTRAINT_COUNT_RESERVE 1024
void GodotStep2D::_populate_island(GodotBody2D *p_body, LocalVector<GodotBody2D *> &p_body_island, LocalVector<GodotConstraint2D *> &p_constraint_island) {
p_body->set_island_step(_step);
if (p_body->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) {
// Only rigid bodies are tested for activation.
p_body_island.push_back(p_body);
}
for (const Pair<GodotConstraint2D *, int> &E : p_body->get_constraint_list()) {
GodotConstraint2D *constraint = const_cast<GodotConstraint2D *>(E.first);
if (constraint->get_island_step() == _step) {
continue; // Already processed.
}
constraint->set_island_step(_step);
p_constraint_island.push_back(constraint);
all_constraints.push_back(constraint);
for (int i = 0; i < constraint->get_body_count(); i++) {
if (i == E.second) {
continue;
}
GodotBody2D *other_body = constraint->get_body_ptr()[i];
if (other_body->get_island_step() == _step) {
continue; // Already processed.
}
if (other_body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC) {
continue; // Static bodies don't connect islands.
}
_populate_island(other_body, p_body_island, p_constraint_island);
}
}
}
void GodotStep2D::_setup_constraint(uint32_t p_constraint_index, void *p_userdata) {
GodotConstraint2D *constraint = all_constraints[p_constraint_index];
constraint->setup(delta);
}
void GodotStep2D::_pre_solve_island(LocalVector<GodotConstraint2D *> &p_constraint_island) const {
uint32_t constraint_count = p_constraint_island.size();
uint32_t valid_constraint_count = 0;
for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
GodotConstraint2D *constraint = p_constraint_island[constraint_index];
if (p_constraint_island[constraint_index]->pre_solve(delta)) {
// Keep this constraint for solving.
p_constraint_island[valid_constraint_count++] = constraint;
}
}
p_constraint_island.resize(valid_constraint_count);
}
void GodotStep2D::_solve_island(uint32_t p_island_index, void *p_userdata) const {
const LocalVector<GodotConstraint2D *> &constraint_island = constraint_islands[p_island_index];
for (int i = 0; i < iterations; i++) {
uint32_t constraint_count = constraint_island.size();
for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
constraint_island[constraint_index]->solve(delta);
}
}
}
void GodotStep2D::_check_suspend(LocalVector<GodotBody2D *> &p_body_island) const {
bool can_sleep = true;
uint32_t body_count = p_body_island.size();
for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
GodotBody2D *body = p_body_island[body_index];
if (!body->sleep_test(delta)) {
can_sleep = false;
}
}
// Put all to sleep or wake up everyone.
for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
GodotBody2D *body = p_body_island[body_index];
bool active = body->is_active();
if (active == can_sleep) {
body->set_active(!can_sleep);
}
}
}
void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta) {
p_space->lock(); // can't access space during this
p_space->setup(); //update inertias, etc
p_space->set_last_step(p_delta);
iterations = p_space->get_solver_iterations();
delta = p_delta;
const SelfList<GodotBody2D>::List *body_list = &p_space->get_active_body_list();
/* INTEGRATE FORCES */
uint64_t profile_begtime = OS::get_singleton()->get_ticks_usec();
uint64_t profile_endtime = 0;
int active_count = 0;
const SelfList<GodotBody2D> *b = body_list->first();
while (b) {
b->self()->integrate_forces(p_delta);
b = b->next();
active_count++;
}
p_space->set_active_objects(active_count);
// Update the broadphase to register collision pairs.
p_space->update();
{ //profile
profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime);
profile_begtime = profile_endtime;
}
/* GENERATE CONSTRAINT ISLANDS FOR MOVING AREAS */
uint32_t island_count = 0;
const SelfList<GodotArea2D>::List &aml = p_space->get_moved_area_list();
while (aml.first()) {
for (GodotConstraint2D *E : aml.first()->self()->get_constraints()) {
GodotConstraint2D *constraint = E;
if (constraint->get_island_step() == _step) {
continue;
}
constraint->set_island_step(_step);
// Each constraint can be on a separate island for areas as there's no solving phase.
++island_count;
if (constraint_islands.size() < island_count) {
constraint_islands.resize(island_count);
}
LocalVector<GodotConstraint2D *> &constraint_island = constraint_islands[island_count - 1];
constraint_island.clear();
all_constraints.push_back(constraint);
constraint_island.push_back(constraint);
}
p_space->area_remove_from_moved_list((SelfList<GodotArea2D> *)aml.first()); //faster to remove here
}
/* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */
b = body_list->first();
uint32_t body_island_count = 0;
while (b) {
GodotBody2D *body = b->self();
if (body->get_island_step() != _step) {
++body_island_count;
if (body_islands.size() < body_island_count) {
body_islands.resize(body_island_count);
}
LocalVector<GodotBody2D *> &body_island = body_islands[body_island_count - 1];
body_island.clear();
body_island.reserve(BODY_ISLAND_SIZE_RESERVE);
++island_count;
if (constraint_islands.size() < island_count) {
constraint_islands.resize(island_count);
}
LocalVector<GodotConstraint2D *> &constraint_island = constraint_islands[island_count - 1];
constraint_island.clear();
constraint_island.reserve(ISLAND_SIZE_RESERVE);
_populate_island(body, body_island, constraint_island);
if (body_island.is_empty()) {
--body_island_count;
}
if (constraint_island.is_empty()) {
--island_count;
}
}
b = b->next();
}
p_space->set_island_count((int)island_count);
{ //profile
profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime);
profile_begtime = profile_endtime;
}
/* SETUP CONSTRAINTS / PROCESS COLLISIONS */
uint32_t total_constraint_count = all_constraints.size();
WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep2D::_setup_constraint, nullptr, total_constraint_count, -1, true, SNAME("Physics2DConstraintSetup"));
WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
{ //profile
profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime);
profile_begtime = profile_endtime;
}
/* PRE-SOLVE CONSTRAINT ISLANDS */
// Warning: This doesn't run on threads, because it involves thread-unsafe processing.
for (uint32_t island_index = 0; island_index < island_count; ++island_index) {
_pre_solve_island(constraint_islands[island_index]);
}
/* SOLVE CONSTRAINT ISLANDS */
// Warning: _solve_island modifies the constraint islands for optimization purpose,
// their content is not reliable after these calls and shouldn't be used anymore.
group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep2D::_solve_island, nullptr, island_count, -1, true, SNAME("Physics2DConstraintSolveIslands"));
WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
{ //profile
profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime);
profile_begtime = profile_endtime;
}
/* INTEGRATE VELOCITIES */
b = body_list->first();
while (b) {
const SelfList<GodotBody2D> *n = b->next();
b->self()->integrate_velocities(p_delta);
b = n; // in case it shuts itself down
}
/* SLEEP / WAKE UP ISLANDS */
for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) {
_check_suspend(body_islands[island_index]);
}
{ //profile
profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime);
//profile_begtime=profile_endtime;
}
all_constraints.clear();
p_space->unlock();
_step++;
}
GodotStep2D::GodotStep2D() {
body_islands.reserve(BODY_ISLAND_COUNT_RESERVE);
constraint_islands.reserve(ISLAND_COUNT_RESERVE);
all_constraints.reserve(CONSTRAINT_COUNT_RESERVE);
}
GodotStep2D::~GodotStep2D() {
}

60
servers/physics_2d/godot_step_2d.h
View File

@ -1,60 +0,0 @@
/**************************************************************************/
/* godot_step_2d.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#ifndef GODOT_STEP_2D_H
#define GODOT_STEP_2D_H
#include "godot_space_2d.h"
#include "core/templates/local_vector.h"
class GodotStep2D {
uint64_t _step = 1;
int iterations = 0;
real_t delta = 0.0;
LocalVector<LocalVector<GodotBody2D *>> body_islands;
LocalVector<LocalVector<GodotConstraint2D *>> constraint_islands;
LocalVector<GodotConstraint2D *> all_constraints;
void _populate_island(GodotBody2D *p_body, LocalVector<GodotBody2D *> &p_body_island, LocalVector<GodotConstraint2D *> &p_constraint_island);
void _setup_constraint(uint32_t p_constraint_index, void *p_userdata = nullptr);
void _pre_solve_island(LocalVector<GodotConstraint2D *> &p_constraint_island) const;
void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr) const;
void _check_suspend(LocalVector<GodotBody2D *> &p_body_island) const;
public:
void step(GodotSpace2D *p_space, real_t p_delta);
GodotStep2D();
~GodotStep2D();
};
#endif // GODOT_STEP_2D_H